Add Nimble in libs directory

238 files changed, 112202 insertions, 0 deletions

diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_att.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_att.h
new file mode 100644
index 00000000..391a992a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_att.h
@@ -0,0 +1,194 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_ATT_
+#define H_BLE_ATT_
+
+/**
+ * @brief Bluetooth Attribute Protocol (ATT)
+ * @defgroup bt_att Bluetooth Attribute Protocol (ATT)
+ * @ingroup bt_host
+ * @{
+ */
+
+#include "os/queue.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+#define BLE_ATT_UUID_PRIMARY_SERVICE        0x2800
+#define BLE_ATT_UUID_SECONDARY_SERVICE      0x2801
+#define BLE_ATT_UUID_INCLUDE                0x2802
+#define BLE_ATT_UUID_CHARACTERISTIC         0x2803
+
+#define BLE_ATT_ERR_INVALID_HANDLE          0x01
+#define BLE_ATT_ERR_READ_NOT_PERMITTED      0x02
+#define BLE_ATT_ERR_WRITE_NOT_PERMITTED     0x03
+#define BLE_ATT_ERR_INVALID_PDU             0x04
+#define BLE_ATT_ERR_INSUFFICIENT_AUTHEN     0x05
+#define BLE_ATT_ERR_REQ_NOT_SUPPORTED       0x06
+#define BLE_ATT_ERR_INVALID_OFFSET          0x07
+#define BLE_ATT_ERR_INSUFFICIENT_AUTHOR     0x08
+#define BLE_ATT_ERR_PREPARE_QUEUE_FULL      0x09
+#define BLE_ATT_ERR_ATTR_NOT_FOUND          0x0a
+#define BLE_ATT_ERR_ATTR_NOT_LONG           0x0b
+#define BLE_ATT_ERR_INSUFFICIENT_KEY_SZ     0x0c
+#define BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN  0x0d
+#define BLE_ATT_ERR_UNLIKELY                0x0e
+#define BLE_ATT_ERR_INSUFFICIENT_ENC        0x0f
+#define BLE_ATT_ERR_UNSUPPORTED_GROUP       0x10
+#define BLE_ATT_ERR_INSUFFICIENT_RES        0x11
+
+#define BLE_ATT_OP_ERROR_RSP                0x01
+#define BLE_ATT_OP_MTU_REQ                  0x02
+#define BLE_ATT_OP_MTU_RSP                  0x03
+#define BLE_ATT_OP_FIND_INFO_REQ            0x04
+#define BLE_ATT_OP_FIND_INFO_RSP            0x05
+#define BLE_ATT_OP_FIND_TYPE_VALUE_REQ      0x06
+#define BLE_ATT_OP_FIND_TYPE_VALUE_RSP      0x07
+#define BLE_ATT_OP_READ_TYPE_REQ            0x08
+#define BLE_ATT_OP_READ_TYPE_RSP            0x09
+#define BLE_ATT_OP_READ_REQ                 0x0a
+#define BLE_ATT_OP_READ_RSP                 0x0b
+#define BLE_ATT_OP_READ_BLOB_REQ            0x0c
+#define BLE_ATT_OP_READ_BLOB_RSP            0x0d
+#define BLE_ATT_OP_READ_MULT_REQ            0x0e
+#define BLE_ATT_OP_READ_MULT_RSP            0x0f
+#define BLE_ATT_OP_READ_GROUP_TYPE_REQ      0x10
+#define BLE_ATT_OP_READ_GROUP_TYPE_RSP      0x11
+#define BLE_ATT_OP_WRITE_REQ                0x12
+#define BLE_ATT_OP_WRITE_RSP                0x13
+#define BLE_ATT_OP_PREP_WRITE_REQ           0x16
+#define BLE_ATT_OP_PREP_WRITE_RSP           0x17
+#define BLE_ATT_OP_EXEC_WRITE_REQ           0x18
+#define BLE_ATT_OP_EXEC_WRITE_RSP           0x19
+#define BLE_ATT_OP_NOTIFY_REQ               0x1b
+#define BLE_ATT_OP_INDICATE_REQ             0x1d
+#define BLE_ATT_OP_INDICATE_RSP             0x1e
+#define BLE_ATT_OP_WRITE_CMD                0x52
+
+#define BLE_ATT_ATTR_MAX_LEN                512
+
+#define BLE_ATT_F_READ                      0x01
+#define BLE_ATT_F_WRITE                     0x02
+#define BLE_ATT_F_READ_ENC                  0x04
+#define BLE_ATT_F_READ_AUTHEN               0x08
+#define BLE_ATT_F_READ_AUTHOR               0x10
+#define BLE_ATT_F_WRITE_ENC                 0x20
+#define BLE_ATT_F_WRITE_AUTHEN              0x40
+#define BLE_ATT_F_WRITE_AUTHOR              0x80
+
+#define HA_FLAG_PERM_RW                     (BLE_ATT_F_READ | BLE_ATT_F_WRITE)
+
+#define BLE_ATT_ACCESS_OP_READ              1
+#define BLE_ATT_ACCESS_OP_WRITE             2
+
+/** Default ATT MTU. Also the minimum. */
+#define BLE_ATT_MTU_DFLT                    23
+
+/**
+ * An ATT MTU of 527 allows the largest ATT command (signed write) to contain a
+ * 512-byte attribute value.
+ */
+#define BLE_ATT_MTU_MAX                     527
+
+/**
+ * Reads a locally registered attribute.  If the specified attribute handle
+ * corresponds to a GATT characteristic value or descriptor, the read is
+ * performed by calling the registered GATT access callback.
+ *
+ * @param attr_handle           The 16-bit handle of the attribute to read.
+ * @param out_om                On success, this is made to point to a
+ *                                  newly-allocated mbuf containing the
+ *                                  attribute data read.
+ *
+ * @return                      0 on success;
+ *                              NimBLE host ATT return code if the attribute
+ *                                  access callback reports failure;
+ *                              NimBLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om);
+
+/**
+ * Writes a locally registered attribute.  This function consumes the supplied
+ * mbuf regardless of the outcome.  If the specified attribute handle
+ * corresponds to a GATT characteristic value or descriptor, the write is
+ * performed by calling the registered GATT access callback.
+ *
+ * @param attr_handle           The 16-bit handle of the attribute to write.
+ * @param om                    The value to write to the attribute.
+ *
+ * @return                      0 on success;
+ *                              NimBLE host ATT return code if the attribute
+ *                                  access callback reports failure;
+ *                              NimBLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om);
+
+/**
+ * Retrieves the ATT MTU of the specified connection.  If an MTU exchange for
+ * this connection has occurred, the MTU is the lower of the two peers'
+ * preferred values.  Otherwise, the MTU is the default value of 23.
+ *
+ * @param conn_handle           The handle of the connection to query.
+ *
+ * @return                      The specified connection's ATT MTU, or 0 if
+ *                                  there is no such connection.
+ */
+uint16_t ble_att_mtu(uint16_t conn_handle);
+
+/**
+ * Retrieves the preferred ATT MTU.  This is the value indicated by the device
+ * during an ATT MTU exchange.
+ *
+ * @return                      The preferred ATT MTU.
+ */
+uint16_t ble_att_preferred_mtu(void);
+
+/**
+ * Sets the preferred ATT MTU; the device will indicate this value in all
+ * subsequent ATT MTU exchanges.  The ATT MTU of a connection is equal to the
+ * lower of the two peers' preferred MTU values.  The ATT MTU is what dictates
+ * the maximum size of any message sent during a GATT procedure.
+ *
+ * The specified MTU must be within the following range: [23, BLE_ATT_MTU_MAX].
+ * 23 is a minimum imposed by the Bluetooth specification; BLE_ATT_MTU_MAX is a
+ * NimBLE compile-time setting.
+ *
+ * @param mtu                   The preferred ATT MTU.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the specified value is not
+ *                                  within the allowed range.
+ */
+int ble_att_set_preferred_mtu(uint16_t mtu);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_eddystone.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_eddystone.h
new file mode 100644
index 00000000..76b7e2b0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_eddystone.h
@@ -0,0 +1,117 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_EDDYSTONE_
+#define H_BLE_EDDYSTONE_
+
+/**
+ * @brief Eddystone - BLE beacon from Google
+ * @defgroup bt_eddystone Eddystone - BLE beacon from Google
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_adv_fields;
+
+#define BLE_EDDYSTONE_MAX_UUIDS16           3
+#define BLE_EDDYSTONE_URL_MAX_LEN           17
+
+#define BLE_EDDYSTONE_URL_SCHEME_HTTP_WWW   0
+#define BLE_EDDYSTONE_URL_SCHEME_HTTPS_WWW  1
+#define BLE_EDDYSTONE_URL_SCHEME_HTTP       2
+#define BLE_EDDYSTONE_URL_SCHEME_HTTPS      3
+
+#define BLE_EDDYSTONE_URL_SUFFIX_COM_SLASH  0x00
+#define BLE_EDDYSTONE_URL_SUFFIX_ORG_SLASH  0x01
+#define BLE_EDDYSTONE_URL_SUFFIX_EDU_SLASH  0x02
+#define BLE_EDDYSTONE_URL_SUFFIX_NET_SLASH  0x03
+#define BLE_EDDYSTONE_URL_SUFFIX_INFO_SLASH 0x04
+#define BLE_EDDYSTONE_URL_SUFFIX_BIZ_SLASH  0x05
+#define BLE_EDDYSTONE_URL_SUFFIX_GOV_SLASH  0x06
+#define BLE_EDDYSTONE_URL_SUFFIX_COM        0x07
+#define BLE_EDDYSTONE_URL_SUFFIX_ORG        0x08
+#define BLE_EDDYSTONE_URL_SUFFIX_EDU        0x09
+#define BLE_EDDYSTONE_URL_SUFFIX_NET        0x0a
+#define BLE_EDDYSTONE_URL_SUFFIX_INFO       0x0b
+#define BLE_EDDYSTONE_URL_SUFFIX_BIZ        0x0c
+#define BLE_EDDYSTONE_URL_SUFFIX_GOV        0x0d
+#define BLE_EDDYSTONE_URL_SUFFIX_NONE       0xff
+
+/**
+ * Configures the device to advertise Eddystone UID beacons.
+ *
+ * @param adv_fields            The base advertisement fields to transform into
+ *                                  an eddystone beacon.  All configured fields
+ *                                  are preserved; you probably want to clear
+ *                                  this struct before calling this function.
+ * @param uid                   The 16-byte UID to advertise.
+ * @param measured_power        The Measured Power (RSSI value at 0 Meter).
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              BLE_HS_EMSGSIZE if the specified data is too
+ *                                  large to fit in an advertisement;
+ *                              Other nonzero on failure.
+ */
+int ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields,
+                                   void *uid, int8_t measured_power);
+
+/**
+ * Configures the device to advertise Eddystone URL beacons.
+ *
+ * @param adv_fields            The base advertisement fields to transform into
+ *                                  an eddystone beacon.  All configured fields
+ *                                  are preserved; you probably want to clear
+ *                                  this struct before calling this function.
+ * @param url_scheme            The prefix of the URL; one of the
+ *                                  BLE_EDDYSTONE_URL_SCHEME values.
+ * @param url_body              The middle of the URL.  Don't include the
+ *                                  suffix if there is a suitable suffix code.
+ * @param url_body_len          The string length of the url_body argument.
+ * @param url_suffix            The suffix of the URL; one of the
+ *                                  BLE_EDDYSTONE_URL_SUFFIX values; use
+ *                                  BLE_EDDYSTONE_URL_SUFFIX_NONE if the suffix
+ *                                  is embedded in the body argument.
+ * @param measured_power        The Measured Power (RSSI value at 0 Meter).
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              BLE_HS_EMSGSIZE if the specified data is too
+ *                                  large to fit in an advertisement;
+ *                              Other nonzero on failure.
+ */
+int ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields,
+                                   uint8_t url_scheme, char *url_body,
+                                   uint8_t url_body_len, uint8_t suffix,
+                                   int8_t measured_power);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_gap.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_gap.h
new file mode 100644
index 00000000..b58f350f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_gap.h
@@ -0,0 +1,2052 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_GAP_
+#define H_BLE_GAP_
+
+/**
+ * @brief Bluetooth Host Generic Access Profile (GAP)
+ * @defgroup bt_host_gap Bluetooth Host Generic Access Profile (GAP)
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+#include "host/ble_hs.h"
+#include "host/ble_hs_adv.h"
+#include "syscfg/syscfg.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct hci_le_conn_complete;
+struct hci_conn_update;
+
+/** 30 ms. */
+#define BLE_GAP_ADV_FAST_INTERVAL1_MIN      (30 * 1000 / BLE_HCI_ADV_ITVL)
+
+/** 60 ms. */
+#define BLE_GAP_ADV_FAST_INTERVAL1_MAX      (60 * 1000 / BLE_HCI_ADV_ITVL)
+
+/** 100 ms. */
+#define BLE_GAP_ADV_FAST_INTERVAL2_MIN      (100 * 1000 / BLE_HCI_ADV_ITVL)
+
+/** 150 ms. */
+#define BLE_GAP_ADV_FAST_INTERVAL2_MAX      (150 * 1000 / BLE_HCI_ADV_ITVL)
+
+/** 30 ms; active scanning. */
+#define BLE_GAP_SCAN_FAST_INTERVAL_MIN      (30 * 1000 / BLE_HCI_ADV_ITVL)
+
+/** 60 ms; active scanning. */
+#define BLE_GAP_SCAN_FAST_INTERVAL_MAX      (60 * 1000 / BLE_HCI_ADV_ITVL)
+
+/** 11.25 ms; limited discovery interval. */
+#define BLE_GAP_LIM_DISC_SCAN_INT           (11.25 * 1000 / BLE_HCI_SCAN_ITVL)
+
+/** 11.25 ms; limited discovery window (not from the spec). */
+#define BLE_GAP_LIM_DISC_SCAN_WINDOW        (11.25 * 1000 / BLE_HCI_SCAN_ITVL)
+
+/** 30 ms; active scanning. */
+#define BLE_GAP_SCAN_FAST_WINDOW            (30 * 1000 / BLE_HCI_SCAN_ITVL)
+
+/* 30.72 seconds; active scanning. */
+#define BLE_GAP_SCAN_FAST_PERIOD            (30.72 * 1000)
+
+/** 1.28 seconds; background scanning. */
+#define BLE_GAP_SCAN_SLOW_INTERVAL1         (1280 * 1000 / BLE_HCI_SCAN_ITVL)
+
+/** 11.25 ms; background scanning. */
+#define BLE_GAP_SCAN_SLOW_WINDOW1           (11.25 * 1000 / BLE_HCI_SCAN_ITVL)
+
+/** 10.24 seconds. */
+#define BLE_GAP_DISC_DUR_DFLT               (10.24 * 1000)
+
+/** 30 seconds (not from the spec). */
+#define BLE_GAP_CONN_DUR_DFLT               (30 * 1000)
+
+/** 1 second. */
+#define BLE_GAP_CONN_PAUSE_CENTRAL          (1 * 1000)
+
+/** 5 seconds. */
+#define BLE_GAP_CONN_PAUSE_PERIPHERAL       (5 * 1000)
+
+/* 30 ms. */
+#define BLE_GAP_INITIAL_CONN_ITVL_MIN       (30 * 1000 / BLE_HCI_CONN_ITVL)
+
+/* 50 ms. */
+#define BLE_GAP_INITIAL_CONN_ITVL_MAX       (50 * 1000 / BLE_HCI_CONN_ITVL)
+
+/** Default channels mask: all three channels are used. */
+#define BLE_GAP_ADV_DFLT_CHANNEL_MAP        0x07
+
+#define BLE_GAP_INITIAL_CONN_LATENCY        0
+#define BLE_GAP_INITIAL_SUPERVISION_TIMEOUT 0x0100
+#define BLE_GAP_INITIAL_CONN_MIN_CE_LEN     0x0010
+#define BLE_GAP_INITIAL_CONN_MAX_CE_LEN     0x0300
+
+#define BLE_GAP_ROLE_MASTER                 0
+#define BLE_GAP_ROLE_SLAVE                  1
+
+#define BLE_GAP_EVENT_CONNECT               0
+#define BLE_GAP_EVENT_DISCONNECT            1
+/* Reserved                                 2 */
+#define BLE_GAP_EVENT_CONN_UPDATE           3
+#define BLE_GAP_EVENT_CONN_UPDATE_REQ       4
+#define BLE_GAP_EVENT_L2CAP_UPDATE_REQ      5
+#define BLE_GAP_EVENT_TERM_FAILURE          6
+#define BLE_GAP_EVENT_DISC                  7
+#define BLE_GAP_EVENT_DISC_COMPLETE         8
+#define BLE_GAP_EVENT_ADV_COMPLETE          9
+#define BLE_GAP_EVENT_ENC_CHANGE            10
+#define BLE_GAP_EVENT_PASSKEY_ACTION        11
+#define BLE_GAP_EVENT_NOTIFY_RX             12
+#define BLE_GAP_EVENT_NOTIFY_TX             13
+#define BLE_GAP_EVENT_SUBSCRIBE             14
+#define BLE_GAP_EVENT_MTU                   15
+#define BLE_GAP_EVENT_IDENTITY_RESOLVED     16
+#define BLE_GAP_EVENT_REPEAT_PAIRING        17
+#define BLE_GAP_EVENT_PHY_UPDATE_COMPLETE   18
+#define BLE_GAP_EVENT_EXT_DISC              19
+#define BLE_GAP_EVENT_PERIODIC_SYNC         20
+#define BLE_GAP_EVENT_PERIODIC_REPORT       21
+#define BLE_GAP_EVENT_PERIODIC_SYNC_LOST    22
+#define BLE_GAP_EVENT_SCAN_REQ_RCVD         23
+#define BLE_GAP_EVENT_PERIODIC_TRANSFER     24
+
+/*** Reason codes for the subscribe GAP event. */
+
+/** Peer's CCCD subscription state changed due to a descriptor write. */
+#define BLE_GAP_SUBSCRIBE_REASON_WRITE      1
+
+/** Peer's CCCD subscription state cleared due to connection termination. */
+#define BLE_GAP_SUBSCRIBE_REASON_TERM       2
+
+/**
+ * Peer's CCCD subscription state changed due to restore from persistence
+ * (bonding restored).
+ */
+#define BLE_GAP_SUBSCRIBE_REASON_RESTORE    3
+
+#define BLE_GAP_REPEAT_PAIRING_RETRY        1
+#define BLE_GAP_REPEAT_PAIRING_IGNORE       2
+
+/** Connection security state */
+struct ble_gap_sec_state {
+    /** If connection is encrypted */
+    unsigned encrypted:1;
+
+    /** If connection is authenticated */
+    unsigned authenticated:1;
+
+    /** If connection is bonded (security information is stored)  */
+    unsigned bonded:1;
+
+    /** Size of a key used for encryption */
+    unsigned key_size:5;
+};
+
+/** Advertising parameters */
+struct ble_gap_adv_params {
+    /** Advertising mode. Can be one of following constants:
+     *  - BLE_GAP_CONN_MODE_NON (non-connectable; 3.C.9.3.2).
+     *  - BLE_GAP_CONN_MODE_DIR (directed-connectable; 3.C.9.3.3).
+     *  - BLE_GAP_CONN_MODE_UND (undirected-connectable; 3.C.9.3.4).
+     */
+    uint8_t conn_mode;
+    /** Discoverable mode. Can be one of following constants:
+     *  - BLE_GAP_DISC_MODE_NON  (non-discoverable; 3.C.9.2.2).
+     *  - BLE_GAP_DISC_MODE_LTD (limited-discoverable; 3.C.9.2.3).
+     *  - BLE_GAP_DISC_MODE_GEN (general-discoverable; 3.C.9.2.4).
+     */
+    uint8_t disc_mode;
+
+    /** Minimum advertising interval, if 0 stack use sane defaults */
+    uint16_t itvl_min;
+    /** Maximum advertising interval, if 0 stack use sane defaults */
+    uint16_t itvl_max;
+    /** Advertising channel map , if 0 stack use sane defaults */
+    uint8_t channel_map;
+
+    /** Advertising  Filter policy */
+    uint8_t filter_policy;
+
+    /** If do High Duty cycle for Directed Advertising */
+    uint8_t high_duty_cycle:1;
+};
+
+/** @brief Connection descriptor */
+struct ble_gap_conn_desc {
+    /** Connection security state */
+    struct ble_gap_sec_state sec_state;
+
+    /** Local identity address */
+    ble_addr_t our_id_addr;
+
+    /** Peer identity address */
+    ble_addr_t peer_id_addr;
+
+    /** Local over-the-air address */
+    ble_addr_t our_ota_addr;
+
+    /** Peer over-the-air address */
+    ble_addr_t peer_ota_addr;
+
+    /** Connection handle */
+    uint16_t conn_handle;
+
+    /** Connection interval */
+    uint16_t conn_itvl;
+
+    /** Connection latency */
+    uint16_t conn_latency;
+
+    /** Connection supervision timeout */
+    uint16_t supervision_timeout;
+
+    /** Connection Role
+     * Possible values BLE_GAP_ROLE_SLAVE or BLE_GAP_ROLE_MASTER
+     */
+    uint8_t role;
+
+    /** Master clock accuracy */
+    uint8_t master_clock_accuracy;
+};
+
+/** @brief Connection parameters  */
+struct ble_gap_conn_params {
+    /** Scan interval in 0.625ms units */
+    uint16_t scan_itvl;
+
+    /** Scan window in 0.625ms units */
+    uint16_t scan_window;
+
+    /** Minimum value for connection interval in 1.25ms units */
+    uint16_t itvl_min;
+
+    /** Maximum value for connection interval in 1.25ms units */
+    uint16_t itvl_max;
+
+    /** Connection latency */
+    uint16_t latency;
+
+    /** Supervision timeout in 10ms units */
+    uint16_t supervision_timeout;
+
+    /** Minimum length of connection event in 0.625ms units */
+    uint16_t min_ce_len;
+
+    /** Maximum length of connection event in 0.625ms units */
+    uint16_t max_ce_len;
+};
+
+/** @brief Extended discovery parameters */
+struct ble_gap_ext_disc_params {
+    /** Scan interval in 0.625ms units */
+    uint16_t itvl;
+
+    /** Scan window in 0.625ms units */
+    uint16_t window;
+
+    /** If passive scan should be used */
+    uint8_t passive:1;
+};
+
+/** @brief Discovery parameters */
+struct ble_gap_disc_params {
+    /** Scan interval in 0.625ms units */
+    uint16_t itvl;
+
+    /** Scan window in 0.625ms units */
+    uint16_t window;
+
+    /** Scan filter policy */
+    uint8_t filter_policy;
+
+    /** If limited discovery procedure should be used */
+    uint8_t limited:1;
+
+    /** If passive scan should be used */
+    uint8_t passive:1;
+
+    /** If enable duplicates filtering */
+    uint8_t filter_duplicates:1;
+};
+
+/** @brief Connection parameters update parameters */
+struct ble_gap_upd_params {
+    /** Minimum value for connection interval in 1.25ms units */
+    uint16_t itvl_min;
+
+    /** Maximum value for connection interval in 1.25ms units */
+    uint16_t itvl_max;
+
+    /** Connection latency */
+    uint16_t latency;
+
+    /** Supervision timeout in 10ms units */
+    uint16_t supervision_timeout;
+
+    /** Minimum length of connection event in 0.625ms units */
+    uint16_t min_ce_len;
+
+    /** Maximum length of connection event in 0.625ms units */
+    uint16_t max_ce_len;
+};
+
+/** @brief Passkey query */
+struct ble_gap_passkey_params {
+    /** Passkey action, can be one of following constants:
+     *  - BLE_SM_IOACT_NONE
+     *  - BLE_SM_IOACT_OOB
+     *  - BLE_SM_IOACT_INPUT
+     *  - BLE_SM_IOACT_DISP
+     *  - BLE_SM_IOACT_NUMCMP
+     */
+    uint8_t action;
+
+    /** Passkey to compare, valid for BLE_SM_IOACT_NUMCMP action */
+    uint32_t numcmp;
+};
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+
+#define BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE   0x00
+#define BLE_GAP_EXT_ADV_DATA_STATUS_INCOMPLETE 0x01
+#define BLE_GAP_EXT_ADV_DATA_STATUS_TRUNCATED  0x02
+
+/** @brief Extended advertising report */
+struct ble_gap_ext_disc_desc {
+    /** Report properties bitmask
+     *  - BLE_HCI_ADV_CONN_MASK
+     *  - BLE_HCI_ADV_SCAN_MASK
+     *  - BLE_HCI_ADV_DIRECT_MASK
+     *  - BLE_HCI_ADV_SCAN_RSP_MASK
+     *  - BLE_HCI_ADV_LEGACY_MASK
+     *  */
+    uint8_t props;
+
+    /** Advertising data status, can be one of following constants:
+     *  - BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE
+     *  - BLE_GAP_EXT_ADV_DATA_STATUS_INCOMPLETE
+     *  - BLE_GAP_EXT_ADV_DATA_STATUS_TRUNCATED
+     */
+    uint8_t data_status;
+
+    /** Legacy advertising PDU type. Valid if BLE_HCI_ADV_LEGACY_MASK props is
+     * set. Can be one of following constants:
+     *  - BLE_HCI_ADV_RPT_EVTYPE_ADV_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_DIR_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP
+     */
+    uint8_t legacy_event_type;
+
+    /** Advertiser address */
+    ble_addr_t addr;
+
+    /** Received signal strength indication in dBm (127 if unavailable) */
+    int8_t rssi;
+
+    /** Advertiser transmit power in dBm (127 if unavailable) */
+    int8_t tx_power;
+
+    /** Advertising Set ID */
+    uint8_t sid;
+
+    /** Primary advertising PHY, can be one of following constants:
+     *  - BLE_HCI_LE_PHY_1M
+     *  - BLE_HCI_LE_PHY_CODED
+     */
+    uint8_t prim_phy;
+
+    /** Secondary advertising PHY, can be one of following constants:
+     *  - BLE_HCI_LE_PHY_1M
+     *  - LE_HCI_LE_PHY_2M
+     *  - BLE_HCI_LE_PHY_CODED
+     */
+    uint8_t sec_phy;
+
+    /** Periodic advertising interval. 0 if no periodic advertising. */
+    uint16_t periodic_adv_itvl;
+
+    /** Advertising Data length */
+    uint8_t length_data;
+
+    /** Advertising data */
+    const uint8_t *data;
+
+    /** Directed advertising address.  Valid if BLE_HCI_ADV_DIRECT_MASK props is
+     * set (BLE_ADDR_ANY otherwise).
+     */
+    ble_addr_t direct_addr;
+};
+#endif
+
+/** @brief Advertising report */
+struct ble_gap_disc_desc {
+    /** Advertising PDU type. Can be one of following constants:
+     *  - BLE_HCI_ADV_RPT_EVTYPE_ADV_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_DIR_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND
+     *  - BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP
+     */
+    uint8_t event_type;
+
+    /** Advertising Data length */
+    uint8_t length_data;
+
+    /** Advertiser address */
+    ble_addr_t addr;
+
+    /** Received signal strength indication in dBm (127 if unavailable) */
+    int8_t rssi;
+
+    /** Advertising data */
+    const uint8_t *data;
+
+    /** Directed advertising address.  Valid for BLE_HCI_ADV_RPT_EVTYPE_DIR_IND
+     * event type (BLE_ADDR_ANY otherwise).
+     */
+    ble_addr_t direct_addr;
+};
+
+struct ble_gap_repeat_pairing {
+    /** The handle of the relevant connection. */
+    uint16_t conn_handle;
+
+    /** Properties of the existing bond. */
+    uint8_t cur_key_size;
+    uint8_t cur_authenticated:1;
+    uint8_t cur_sc:1;
+
+    /**
+     * Properties of the imminent secure link if the pairing procedure is
+     * allowed to continue.
+     */
+    uint8_t new_key_size;
+    uint8_t new_authenticated:1;
+    uint8_t new_sc:1;
+    uint8_t new_bonding:1;
+};
+
+/**
+ * Represents a GAP-related event.  When such an event occurs, the host
+ * notifies the application by passing an instance of this structure to an
+ * application-specified callback.
+ */
+struct ble_gap_event {
+    /**
+     * Indicates the type of GAP event that occurred.  This is one of the
+     * BLE_GAP_EVENT codes.
+     */
+    uint8_t type;
+
+    /**
+     * A discriminated union containing additional details concerning the GAP
+     * event.  The 'type' field indicates which member of the union is valid.
+     */
+    union {
+        /**
+         * Represents a connection attempt.  Valid for the following event
+         * types:
+         *     o BLE_GAP_EVENT_CONNECT
+         */
+        struct {
+            /**
+             * The status of the connection attempt;
+             *     o 0: the connection was successfully established.
+             *     o BLE host error code: the connection attempt failed for
+             *       the specified reason.
+             */
+            int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } connect;
+
+        /**
+         * Represents a terminated connection.  Valid for the following event
+         * types:
+         *     o BLE_GAP_EVENT_DISCONNECT
+         */
+        struct {
+            /**
+             * A BLE host return code indicating the reason for the
+             * disconnect.
+             */
+            int reason;
+
+            /** Information about the connection prior to termination. */
+            struct ble_gap_conn_desc conn;
+        } disconnect;
+
+        /**
+         * Represents an advertising report received during a discovery
+         * procedure.  Valid for the following event types:
+         *     o BLE_GAP_EVENT_DISC
+         */
+        struct ble_gap_disc_desc disc;
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+        /**
+         * Represents an extended advertising report received during a discovery
+         * procedure.  Valid for the following event types:
+         *     o BLE_GAP_EVENT_EXT_DISC
+         */
+        struct ble_gap_ext_disc_desc ext_disc;
+#endif
+
+        /**
+         * Represents a completed discovery procedure.  Valid for the following
+         * event types:
+         *     o BLE_GAP_EVENT_DISC_COMPLETE
+         */
+        struct {
+            /**
+             * The reason the discovery procedure stopped.  Typical reason
+             * codes are:
+             *     o 0: Duration expired.
+             *     o BLE_HS_EPREEMPTED: Host aborted procedure to configure a
+             *       peer's identity.
+             */
+            int reason;
+        } disc_complete;
+
+        /**
+         * Represents a completed advertise procedure.  Valid for the following
+         * event types:
+         *     o BLE_GAP_EVENT_ADV_COMPLETE
+         */
+        struct {
+            /**
+             * The reason the advertise procedure stopped.  Typical reason
+             * codes are:
+             *     o 0: Terminated due to connection.
+             *     o BLE_HS_ETIMEOUT: Duration expired.
+             *     o BLE_HS_EPREEMPTED: Host aborted procedure to configure a
+             *       peer's identity.
+             */
+            int reason;
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+            /** Advertising instance */
+            uint8_t instance;
+            /** The handle of the relevant connection - valid if reason=0 */
+            uint16_t conn_handle;
+            /**
+             * Number of completed extended advertising events
+             *
+             * This field is only valid if non-zero max_events was passed to
+             * ble_gap_ext_adv_start() and advertising completed due to duration
+             * timeout or max events transmitted.
+             * */
+            uint8_t num_ext_adv_events;
+#endif
+        } adv_complete;
+
+        /**
+         * Represents an attempt to update a connection's parameters.  If the
+         * attempt was successful, the connection's descriptor reflects the
+         * updated parameters.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_CONN_UPDATE
+         */
+        struct {
+            /**
+             * The result of the connection update attempt;
+             *     o 0: the connection was successfully updated.
+             *     o BLE host error code: the connection update attempt failed
+             *       for the specified reason.
+             */
+            int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } conn_update;
+
+        /**
+         * Represents a peer's request to update the connection parameters.
+         * This event is generated when a peer performs any of the following
+         * procedures:
+         *     o L2CAP Connection Parameter Update Procedure
+         *     o Link-Layer Connection Parameters Request Procedure
+         *
+         * To reject the request, return a non-zero HCI error code.  The value
+         * returned is the reject reason given to the controller.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_L2CAP_UPDATE_REQ
+         *     o BLE_GAP_EVENT_CONN_UPDATE_REQ
+         */
+        struct {
+            /**
+             * Indicates the connection parameters that the peer would like to
+             * use.
+             */
+            const struct ble_gap_upd_params *peer_params;
+
+            /**
+             * Indicates the connection parameters that the local device would
+             * like to use.  The application callback should fill this in.  By
+             * default, this struct contains the requested parameters (i.e.,
+             * it is a copy of 'peer_params').
+             */
+            struct ble_gap_upd_params *self_params;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } conn_update_req;
+
+        /**
+         * Represents a failed attempt to terminate an established connection.
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_TERM_FAILURE
+         */
+        struct {
+            /**
+             * A BLE host return code indicating the reason for the failure.
+             */
+            int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } term_failure;
+
+        /**
+         * Represents an attempt to change the encrypted state of a
+         * connection.  If the attempt was successful, the connection
+         * descriptor reflects the updated encrypted state.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_ENC_CHANGE
+         */
+        struct {
+            /**
+             * Indicates the result of the encryption state change attempt;
+             *     o 0: the encrypted state was successfully updated;
+             *     o BLE host error code: the encryption state change attempt
+             *       failed for the specified reason.
+             */
+            int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } enc_change;
+
+        /**
+         * Represents a passkey query needed to complete a pairing procedure.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_PASSKEY_ACTION
+         */
+        struct {
+            /** Contains details about the passkey query. */
+            struct ble_gap_passkey_params params;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } passkey;
+
+        /**
+         * Represents a received ATT notification or indication.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_NOTIFY_RX
+         */
+        struct {
+            /**
+             * The contents of the notification or indication.  If the
+             * application wishes to retain this mbuf for later use, it must
+             * set this pointer to NULL to prevent the stack from freeing it.
+             */
+            struct os_mbuf *om;
+
+            /** The handle of the relevant ATT attribute. */
+            uint16_t attr_handle;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /**
+             * Whether the received command is a notification or an
+             * indication;
+             *     o 0: Notification;
+             *     o 1: Indication.
+             */
+            uint8_t indication:1;
+        } notify_rx;
+
+        /**
+         * Represents a transmitted ATT notification or indication, or a
+         * completed indication transaction.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_NOTIFY_TX
+         */
+        struct {
+            /**
+             * The status of the notification or indication transaction;
+             *     o 0:                 Command successfully sent;
+             *     o BLE_HS_EDONE:      Confirmation (indication ack) received;
+             *     o BLE_HS_ETIMEOUT:   Confirmation (indication ack) never
+             *                              received;
+             *     o Other return code: Error.
+             */
+            int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /** The handle of the relevant characteristic value. */
+            uint16_t attr_handle;
+
+            /**
+             * Whether the transmitted command is a notification or an
+             * indication;
+             *     o 0: Notification;
+             *     o 1: Indication.
+             */
+            uint8_t indication:1;
+        } notify_tx;
+
+        /**
+         * Represents a state change in a peer's subscription status.  In this
+         * comment, the term "update" is used to refer to either a notification
+         * or an indication.  This event is triggered by any of the following
+         * occurrences:
+         *     o Peer enables or disables updates via a CCCD write.
+         *     o Connection is about to be terminated and the peer is
+         *       subscribed to updates.
+         *     o Peer is now subscribed to updates after its state was restored
+         *       from persistence.  This happens when bonding is restored.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_SUBSCRIBE
+         */
+        struct {
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /** The value handle of the relevant characteristic. */
+            uint16_t attr_handle;
+
+            /** One of the BLE_GAP_SUBSCRIBE_REASON codes. */
+            uint8_t reason;
+
+            /** Whether the peer was previously subscribed to notifications. */
+            uint8_t prev_notify:1;
+
+            /** Whether the peer is currently subscribed to notifications. */
+            uint8_t cur_notify:1;
+
+            /** Whether the peer was previously subscribed to indications. */
+            uint8_t prev_indicate:1;
+
+            /** Whether the peer is currently subscribed to indications. */
+            uint8_t cur_indicate:1;
+        } subscribe;
+
+        /**
+         * Represents a change in an L2CAP channel's MTU.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_MTU
+         */
+        struct {
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /**
+             * Indicates the channel whose MTU has been updated; either
+             * BLE_L2CAP_CID_ATT or the ID of a connection-oriented channel.
+             */
+            uint16_t channel_id;
+
+            /* The channel's new MTU. */
+            uint16_t value;
+        } mtu;
+
+        /**
+         * Represents a change in peer's identity. This is issued after
+         * successful pairing when Identity Address Information was received.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_IDENTITY_RESOLVED
+         */
+        struct {
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } identity_resolved;
+
+        /**
+         * Represents a peer's attempt to pair despite a bond already existing.
+         * The application has two options for handling this event type:
+         *     o Retry: Return BLE_GAP_REPEAT_PAIRING_RETRY after deleting the
+         *              conflicting bond.  The stack will verify the bond has
+         *              been deleted and continue the pairing procedure.  If
+         *              the bond is still present, this event will be reported
+         *              again.
+         *     o Ignore: Return BLE_GAP_REPEAT_PAIRING_IGNORE.  The stack will
+         *               silently ignore the pairing request.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_REPEAT_PAIRING
+         */
+        struct ble_gap_repeat_pairing repeat_pairing;
+
+        /**
+         * Represents a change of PHY. This is issue after successful
+         * change on PHY.
+         */
+        struct {
+            int status;
+            uint16_t conn_handle;
+
+            /**
+             * Indicates enabled TX/RX PHY. Possible values:
+             *     o BLE_GAP_LE_PHY_1M
+             *     o BLE_GAP_LE_PHY_2M
+             *     o BLE_GAP_LE_PHY_CODED
+             */
+            uint8_t tx_phy;
+            uint8_t rx_phy;
+        } phy_updated;
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+        /**
+         * Represents a periodic advertising sync established during discovery
+         * procedure. Valid for the following event types:
+         *     o BLE_GAP_EVENT_PERIODIC_SYNC
+         */
+        struct {
+            /** BLE_ERR_SUCCESS on success or error code on failure. Other
+             * fields are valid only for success
+             */
+            uint8_t status;
+            /** Periodic sync handle */
+            uint16_t sync_handle;
+
+            /** Advertising Set ID */
+            uint8_t sid;
+
+            /** Advertiser address */
+            ble_addr_t adv_addr;
+
+            /** Advertising PHY, can be one of following constants:
+             *  - BLE_HCI_LE_PHY_1M
+             *  - LE_HCI_LE_PHY_2M
+             *  - BLE_HCI_LE_PHY_CODED
+            */
+            uint8_t adv_phy;
+
+            /** Periodic advertising interval */
+            uint16_t per_adv_ival;
+
+            /** Advertiser clock accuracy */
+            uint8_t adv_clk_accuracy;
+        } periodic_sync;
+
+        /**
+         * Represents a periodic advertising report received on established
+         * sync. Valid for the following event types:
+         *     o BLE_GAP_EVENT_PERIODIC_REPORT
+         */
+        struct {
+            /** Periodic sync handle */
+            uint16_t sync_handle;
+
+            /** Advertiser transmit power in dBm (127 if unavailable) */
+            int8_t tx_power;
+
+            /** Received signal strength indication in dBm (127 if unavailable) */
+            int8_t rssi;
+
+            /** Advertising data status, can be one of following constants:
+             *  - BLE_HCI_PERIODIC_DATA_STATUS_COMPLETE
+             *  - BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE
+             *  - BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED
+             */
+            uint8_t data_status;
+
+            /** Advertising Data length */
+            uint8_t data_length;
+
+            /** Advertising data */
+            const uint8_t *data;
+        } periodic_report;
+
+        /**
+         * Represents a periodic advertising sync lost of established sync.
+         * Sync lost reason can be BLE_HS_ETIMEOUT (sync timeout) or
+         * BLE_HS_EDONE (sync terminated locally).
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_PERIODIC_SYNC_LOST
+         */
+        struct {
+            /** Periodic sync handle */
+            uint16_t sync_handle;
+
+            /** Reason for sync lost, can be BLE_HS_ETIMEOUT for timeout or
+             * BLE_HS_EDONE for locally terminated sync
+             */
+            int reason;
+        } periodic_sync_lost;
+#endif
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+        /**
+         * Represents a scan request for an extended advertising instance where
+         * scan request notifications were enabled.
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_SCAN_REQ_RCVD
+         */
+        struct {
+            /** Extended advertising instance */
+            uint8_t instance;
+            /** Address of scanner */
+            ble_addr_t scan_addr;
+        } scan_req_rcvd;
+#endif
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+        /**
+         * Represents a periodic advertising sync transfer received. Valid for
+         * the following event types:
+         *     o BLE_GAP_EVENT_PERIODIC_TRANSFER
+         */
+        struct {
+            /** BLE_ERR_SUCCESS on success or error code on failure. Sync handle
+             * is valid only for success.
+             */
+            uint8_t status;
+
+            /** Periodic sync handle */
+            uint16_t sync_handle;
+
+            /** Connection handle */
+            uint16_t conn_handle;
+
+            /** Service Data */
+            uint16_t service_data;
+
+            /** Advertising Set ID */
+            uint8_t sid;
+
+            /** Advertiser address */
+            ble_addr_t adv_addr;
+
+            /** Advertising PHY, can be one of following constants:
+             *  - BLE_HCI_LE_PHY_1M
+             *  - LE_HCI_LE_PHY_2M
+             *  - BLE_HCI_LE_PHY_CODED
+            */
+            uint8_t adv_phy;
+
+            /** Periodic advertising interval */
+            uint16_t per_adv_itvl;
+
+            /** Advertiser clock accuracy */
+            uint8_t adv_clk_accuracy;
+        } periodic_transfer;
+#endif
+    };
+};
+
+typedef int ble_gap_event_fn(struct ble_gap_event *event, void *arg);
+
+#define BLE_GAP_CONN_MODE_NON               0
+#define BLE_GAP_CONN_MODE_DIR               1
+#define BLE_GAP_CONN_MODE_UND               2
+
+#define BLE_GAP_DISC_MODE_NON               0
+#define BLE_GAP_DISC_MODE_LTD               1
+#define BLE_GAP_DISC_MODE_GEN               2
+
+/**
+ * Searches for a connection with the specified handle.  If a matching
+ * connection is found, the supplied connection descriptor is filled
+ * correspondingly.
+ *
+ * @param handle    The connection handle to search for.
+ * @param out_desc  On success, this is populated with information relating to
+ *                  the matching connection.  Pass NULL if you don't need this
+ *                  information.
+ *
+ * @return          0 on success, BLE_HS_ENOTCONN if no matching connection was
+ *                  found.
+ */
+int ble_gap_conn_find(uint16_t handle, struct ble_gap_conn_desc *out_desc);
+
+/**
+ * Searches for a connection with a peer with the specified address.
+ * If a matching connection is found, the supplied connection descriptor
+ * is filled correspondingly.
+ *
+ * @param addr      The ble address of a connected peer device to search for.
+ * @param out_desc  On success, this is populated with information relating to
+ *                  the matching connection.  Pass NULL if you don't need this
+ *                  information.
+ *
+ * @return          0 on success, BLE_HS_ENOTCONN if no matching connection was
+ *                  found.
+ */
+int ble_gap_conn_find_by_addr(const ble_addr_t *addr,
+                              struct ble_gap_conn_desc *out_desc);
+
+/**
+ * Configures a connection to use the specified GAP event callback.  A
+ * connection's GAP event callback is first specified when the connection is
+ * created, either via advertising or initiation.  This function replaces the
+ * callback that was last configured.
+ *
+ * @param conn_handle   The handle of the connection to configure.
+ * @param cb            The callback to associate with the connection.
+ * @param cb_arg        An optional argument that the callback receives.
+ *
+ * @return              0 on success, BLE_HS_ENOTCONN if there is no connection
+ *                      with the specified handle.
+ */
+int ble_gap_set_event_cb(uint16_t conn_handle,
+                         ble_gap_event_fn *cb, void *cb_arg);
+
+/** @brief Start advertising
+ *
+ * This function configures and start advertising procedure.
+ *
+ * @param own_addr_type The type of address the stack should use for itself.
+ *                      Valid values are:
+ *                         - BLE_OWN_ADDR_PUBLIC
+ *                         - BLE_OWN_ADDR_RANDOM
+ *                         - BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ *                         - BLE_OWN_ADDR_RPA_RANDOM_DEFAULT
+ * @param direct_addr   The peer's address for directed advertising. This
+ *                      parameter shall be non-NULL if directed advertising is
+ *                      being used.
+ * @param duration_ms   The duration of the advertisement procedure. On
+ *                      expiration, the procedure ends and a
+ *                      BLE_GAP_EVENT_ADV_COMPLETE event is reported. Units are
+ *                      milliseconds. Specify BLE_HS_FOREVER for no expiration.
+ * @param adv_params    Additional arguments specifying the particulars of the
+ *                      advertising procedure.
+ * @param cb            The callback to associate with this advertising
+ *                      procedure.  If advertising ends, the event is reported
+ *                      through this callback.  If advertising results in a
+ *                      connection, the connection inherits this callback as its
+ *                      event-reporting mechanism.
+ * @param cb_arg        The optional argument to pass to the callback function.
+ *
+ * @return              0 on success, error code on failure.
+ */
+int ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr,
+                      int32_t duration_ms,
+                      const struct ble_gap_adv_params *adv_params,
+                      ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Stops the currently-active advertising procedure.  A success return
+ * code indicates that advertising has been fully aborted and a new advertising
+ * procedure can be initiated immediately.
+ *
+ * NOTE: If the caller is running in the same task as the NimBLE host, or if it
+ * is running in a higher priority task than that of the host, care must be
+ * taken when restarting advertising.  Under these conditions, the following is
+ * *not* a reliable method to restart advertising:
+ *     ble_gap_adv_stop()
+ *     ble_gap_adv_start()
+ *
+ * Instead, the call to `ble_gap_adv_start()` must be made in a separate event
+ * context.  That is, `ble_gap_adv_start()` must be called asynchronously by
+ * enqueueing an event on the current task's event queue.  See
+ * https://github.com/apache/mynewt-nimble/pull/211 for more information.
+ *
+ * @return  0 on success, BLE_HS_EALREADY if there is no active advertising
+ *          procedure, other error code on failure.
+ */
+int ble_gap_adv_stop(void);
+
+/**
+ * Indicates whether an advertisement procedure is currently in progress.
+ *
+ * @return 0 if no advertisement procedure in progress, 1 otherwise.
+ */
+int ble_gap_adv_active(void);
+
+/**
+ * Configures the data to include in subsequent advertisements.
+ *
+ * @param data      Buffer containing the advertising data.
+ * @param data_len  The size of the advertising data, in bytes.
+ *
+ * @return          0 on succes,  BLE_HS_EBUSY if advertising is in progress,
+ *                  other error code on failure.
+ */
+int ble_gap_adv_set_data(const uint8_t *data, int data_len);
+
+/**
+ * Configures the data to include in subsequent scan responses.
+ *
+ * @param data      Buffer containing the scan response data.
+ * @param data_len  The size of the response data, in bytes.
+ *
+ * @return          0 on succes,  BLE_HS_EBUSY if advertising is in progress,
+ *                  other error code on failure.
+ */
+int ble_gap_adv_rsp_set_data(const uint8_t *data, int data_len);
+
+/**
+ * Configures the fields to include in subsequent advertisements.  This is a
+ * convenience wrapper for ble_gap_adv_set_data().
+ *
+ * @param adv_fields    Specifies the advertisement data.
+ *
+ * @return              0 on success,
+ *                      BLE_HS_EBUSY if advertising is in progress,
+ *                      BLE_HS_EMSGSIZE if the specified data is too large to
+ *                      fit in an advertisement,
+ *                      other error code on failure.
+ */
+int ble_gap_adv_set_fields(const struct ble_hs_adv_fields *rsp_fields);
+
+/**
+ * Configures the fields to include in subsequent scan responses.  This is a
+ * convenience wrapper for ble_gap_adv_rsp_set_data().
+ *
+ * @param adv_fields   Specifies the scan response data.
+ *
+ * @return              0 on success,
+ *                      BLE_HS_EBUSY if advertising is in progress,
+ *                      BLE_HS_EMSGSIZE if the specified data is too large to
+ *                      fit in a scan response,
+ *                      other error code on failure.
+ */
+int ble_gap_adv_rsp_set_fields(const struct ble_hs_adv_fields *rsp_fields);
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+/** @brief Extended advertising parameters  */
+struct ble_gap_ext_adv_params {
+    /** If perform connectable advertising */
+    unsigned int connectable:1;
+
+    /** If perform scannable advertising */
+    unsigned int scannable:1;
+
+    /** If perform directed advertising */
+    unsigned int directed:1;
+
+    /** If perform high-duty directed advertising */
+    unsigned int high_duty_directed:1;
+
+    /** If use legacy PDUs for advertising */
+    unsigned int legacy_pdu:1;
+
+    /** If perform anonymous advertising */
+    unsigned int anonymous:1;
+
+    /** If include TX power in advertising PDU */
+    unsigned int include_tx_power:1;
+
+    /** If enable scan request notification  */
+    unsigned int scan_req_notif:1;
+
+    /** Minimum advertising interval in 0.625ms units, if 0 stack use sane
+     *  defaults
+     */
+    uint32_t itvl_min;
+
+    /** Maximum advertising interval in 0.625ms units, if 0 stack use sane
+     *  defaults
+     */
+    uint32_t itvl_max;
+
+    /** Advertising channel map , if 0 stack use sane defaults */
+    uint8_t channel_map;
+
+    /** Own address type to be used by advertising instance */
+    uint8_t own_addr_type;
+
+    /** Peer address for directed advertising, valid only if directed is set */
+    ble_addr_t peer;
+
+    /** Advertising  Filter policy */
+    uint8_t filter_policy;
+
+    /** Primary advertising PHY to use , can be one of following constants:
+     *  - BLE_HCI_LE_PHY_1M
+     *  - BLE_HCI_LE_PHY_CODED
+     */
+    uint8_t primary_phy;
+
+    /** Secondary advertising PHY to use, can be one of following constants:
+     *  - BLE_HCI_LE_PHY_1M
+     *  - LE_HCI_LE_PHY_2M
+     *  - BLE_HCI_LE_PHY_CODED
+     */
+    uint8_t secondary_phy;
+
+    /** Preferred advertiser transmit power */
+    int8_t tx_power;
+
+    /** Advertising Set ID */
+    uint8_t sid;
+};
+
+/**
+ * Configure extended advertising instance
+ *
+ * @param instance            Instance ID
+ * @param params              Additional arguments specifying the particulars
+ *                            of the advertising.
+ * @param selected_tx_power   Selected advertising transmit power will be
+ *                            stored in that param if non-NULL.
+ * @param cb                  The callback to associate with this advertising
+ *                            procedure. Advertising complete event is reported
+ *                            through this callback
+ * @param cb_arg              The optional argument to pass to the callback
+ *                            function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gap_ext_adv_configure(uint8_t instance,
+                              const struct ble_gap_ext_adv_params *params,
+                              int8_t *selected_tx_power,
+                              ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Set random address for configured advertising instance.
+ *
+ * @param instance            Instance ID
+ * @param addr                Random address to be set
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gap_ext_adv_set_addr(uint8_t instance, const ble_addr_t *addr);
+
+/**
+ * Start advertising instance.
+ *
+ * @param instance            Instance ID
+ * @param duration            The duration of the advertisement procedure. On
+ *                            expiration, the procedure ends and
+ *                            a BLE_GAP_EVENT_ADV_COMPLETE event is reported.
+ *                            Units are 10 milliseconds. Specify 0 for no
+ *                            expiration.
+ * @params max_events         Number of advertising events that should be sent
+ *                            before advertising ends and
+ *                            a BLE_GAP_EVENT_ADV_COMPLETE event is reported.
+ *                            Specify 0 for no limit.
+ *
+ * @return              0 on success, error code on failure.
+ */
+int ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events);
+
+/**
+ * Stops advertising procedure for specified instance.
+ *
+ * @param instance            Instance ID
+ *
+ * @return  0 on success, BLE_HS_EALREADY if there is no active advertising
+ *          procedure for instance, other error code on failure.
+ */
+int ble_gap_ext_adv_stop(uint8_t instance);
+
+/**
+ * Configures the data to include in advertisements packets for specified
+ * advertising instance.
+ *
+ * @param instance            Instance ID
+ * @param data                Chain containing the advertising data.
+ *
+ * @return          0 on success or error code on failure.
+ */
+int ble_gap_ext_adv_set_data(uint8_t instance, struct os_mbuf *data);
+
+/**
+ * Configures the data to include in subsequent scan responses for specified
+ * advertisign instance.
+ *
+ * @param instance            Instance ID
+ * @param data                Chain containing the scan response data.
+ *
+ * @return          0 on success or error code on failure.
+ */
+
+int ble_gap_ext_adv_rsp_set_data(uint8_t instance, struct os_mbuf *data);
+
+/**
+ * Remove existing advertising instance.
+ *
+ * @param instance            Instance ID
+ *
+ * @return              0 on success,
+ *                      BLE_HS_EBUSY if advertising is in progress,
+ *                      other error code on failure.
+ */
+int ble_gap_ext_adv_remove(uint8_t instance);
+
+/**
+ * Clear all existing advertising instances
+ * @return              0 on success,
+ *                      BLE_HS_EBUSY if advertising is in progress,
+ *                      other error code on failure.
+ */
+int ble_gap_ext_adv_clear(void);
+#endif
+
+/* Periodic Advertising */
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+
+/** @brief Periodic advertising parameters  */
+struct ble_gap_periodic_adv_params {
+    /** If include TX power in advertising PDU */
+    unsigned int include_tx_power:1;
+
+    /** Minimum advertising interval in 0.625ms units, if 0 stack use sane
+     *  defaults
+     */
+    uint16_t itvl_min;
+
+    /** Maximum advertising interval in 0.625ms units, if 0 stack use sane
+     *  defaults
+     */
+    uint16_t itvl_max;
+};
+
+/** @brief Periodic sync parameters  */
+struct ble_gap_periodic_sync_params {
+    /** The maximum number of periodic advertising events that controller can
+     * skip after a successful receive.
+     * */
+    uint16_t skip;
+
+    /** Synchronization timeout for the periodic advertising train in 10ms units
+     */
+    uint16_t sync_timeout;
+
+    /** If reports should be initially disabled when sync is created */
+    unsigned int reports_disabled:1;
+};
+
+/**
+ * Configure periodic advertising for specified advertising instance
+ *
+ * This is allowed only for instances configured as non-announymous,
+ * non-connectable and non-scannable.
+ *
+ * @param instance            Instance ID
+ * @param params              Additional arguments specifying the particulars
+ *                            of periodic advertising.
+ *
+ * @return                    0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_configure(uint8_t instance,
+                                   const struct ble_gap_periodic_adv_params *params);
+
+/**
+ * Start periodic advertising for specified advertising instance.
+ *
+ * @param instance            Instance ID
+ *
+ * @return              0 on success, error code on failure.
+ */
+int ble_gap_periodic_adv_start(uint8_t instance);
+
+/**
+ * Stop periodic advertising for specified advertising instance.
+ *
+ * @param instance            Instance ID
+ *
+ * @return              0 on success, error code on failure.
+ */
+int ble_gap_periodic_adv_stop(uint8_t instance);
+
+/**
+ * Configures the data to include in periodic advertisements for specified
+ * advertising instance.
+ *
+ * @param instance            Instance ID
+ * @param data                Chain containing the periodic advertising data.
+ *
+ * @return          0 on success or error code on failure.
+ */
+int ble_gap_periodic_adv_set_data(uint8_t instance, struct os_mbuf *data);
+
+/**
+ * Performs the Synchronization procedure with periodic advertiser.
+ *
+ * @param addr               Peer address to synchronize with. If NULL than
+ *                           peers from periodic list are used.
+ * @param adv_sid            Advertiser Set ID
+ * @param params             Additional arguments specifying the particulars
+ *                           of the synchronization procedure.
+ * @param cb                 The callback to associate with this synchrnization
+ *                           procedure. BLE_GAP_EVENT_PERIODIC_REPORT events
+ *                           are reported only by this callback.
+ * @param cb_arg             The optional argument to pass to the callback
+ *                           function.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_create(const ble_addr_t *addr, uint8_t adv_sid,
+                                     const struct ble_gap_periodic_sync_params *params,
+                                     ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Cancel pending synchronization procedure.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_create_cancel(void);
+
+/**
+ * Terminate synchronization procedure.
+ *
+ * @param sync_handle        Handle identifying synchronization to terminate.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_terminate(uint16_t sync_handle);
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+/**
+ * Disable or enable periodic reports for specified sync.
+ *
+ * @param sync_handle        Handle identifying synchronization.
+ * @param enable             If reports should be enabled.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_reporting(uint16_t sync_handle, bool enable);
+
+/**
+ * Initialize sync transfer procedure for specified handles.
+ *
+ * This allows to transfer periodic sync to which host is synchronized.
+ *
+ * @param sync_handle        Handle identifying synchronization.
+ * @param conn_handle        Handle identifying connection.
+ * @param service_data       Sync transfer service data
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_transfer(uint16_t sync_handle,
+                                       uint16_t conn_handle,
+                                       uint16_t service_data);
+
+/**
+ * Initialize set info transfer procedure for specified handles.
+ *
+ * This allows to transfer periodic sync which is being advertised by host.
+ *
+ * @param instance           Advertising instance with periodic adv enabled.
+ * @param conn_handle        Handle identifying connection.
+ * @param service_data       Sync transfer service data
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_set_info(uint8_t instance,
+                                       uint16_t conn_handle,
+                                       uint16_t service_data);
+
+/**
+ * Enables or disables sync transfer reception on specified connection.
+ * When sync transfer arrives, BLE_GAP_EVENT_PERIODIC_TRANSFER is sent to the user.
+ * After that, sync transfer reception on that connection is terminated and user needs
+ * to call this API again when expect to receive next sync transfers.
+ *
+ * Note: If ACL connection gets disconnected before sync transfer arrived, user will
+ * not receive BLE_GAP_EVENT_PERIODIC_TRANSFER. Instead, sync transfer reception
+ * is terminated by the host automatically.
+ *
+ * @param conn_handle        Handle identifying connection.
+ * @param params             Parameters for enabled sync transfer reception.
+ *                           Specify NULL to disable reception.
+ * @param cb                 The callback to associate with this synchronization
+ *                           procedure. BLE_GAP_EVENT_PERIODIC_REPORT events
+ *                           are reported only by this callback.
+ * @param cb_arg             The optional argument to pass to the callback
+ *                           function.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_periodic_adv_sync_receive(uint16_t conn_handle,
+                                      const struct ble_gap_periodic_sync_params *params,
+                                      ble_gap_event_fn *cb, void *cb_arg);
+#endif
+
+/**
+ * Add peer device to periodic synchronization list.
+ *
+ * @param addr               Peer address to add to list.
+ * @param adv_sid            Advertiser Set ID
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_add_dev_to_periodic_adv_list(const ble_addr_t *peer_addr,
+                                         uint8_t adv_sid);
+
+/**
+ * Remove peer device from periodic synchronization list.
+ *
+ * @param addr               Peer address to remove from list.
+ * @param adv_sid            Advertiser Set ID
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_rem_dev_from_periodic_adv_list(const ble_addr_t *peer_addr,
+                                           uint8_t adv_sid);
+
+/**
+ * Clear periodic synchrnization list.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_clear_periodic_adv_list(void);
+
+/**
+ * Get periodic synchronization list size.
+ *
+ * @param per_adv_list_size  On success list size is stored here.
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_read_periodic_adv_list_size(uint8_t *per_adv_list_size);
+#endif
+
+
+/**
+ * Performs the Limited or General Discovery Procedures.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself when sending scan requests.  Valid
+ *                                  values are:
+ *                                      - BLE_ADDR_TYPE_PUBLIC
+ *                                      - BLE_ADDR_TYPE_RANDOM
+ *                                      - BLE_ADDR_TYPE_RPA_PUB_DEFAULT
+ *                                      - BLE_ADDR_TYPE_RPA_RND_DEFAULT
+ *                                  This parameter is ignored unless active
+ *                                  scanning is being used.
+ * @param duration_ms           The duration of the discovery procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are milliseconds.  Specify
+ *                                  BLE_HS_FOREVER for no expiration. Specify
+ *                                  0 to use stack defaults.
+ * @param disc_params           Additional arguments specifying the particulars
+ *                                  of the discovery procedure.
+ * @param cb                    The callback to associate with this discovery
+ *                                  procedure.  Advertising reports and
+ *                                  discovery termination events are reported
+ *                                  through this callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms,
+                 const struct ble_gap_disc_params *disc_params,
+                 ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Performs the Limited or General Extended Discovery Procedures.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                              itself when sending scan requests.  Valid
+ *                              values are:
+ *                                      - BLE_ADDR_TYPE_PUBLIC
+ *                                      - BLE_ADDR_TYPE_RANDOM
+ *                                      - BLE_ADDR_TYPE_RPA_PUB_DEFAULT
+ *                                      - BLE_ADDR_TYPE_RPA_RND_DEFAULT
+ *                              This parameter is ignored unless active
+ *                              scanning is being used.
+ * @param duration              The duration of the discovery procedure.
+ *                                  On expiration, if period is set to 0, the
+ *                                  procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are 10 milliseconds.
+ *                                  Specify 0 for no expiration.
+ * @param period                Time interval from when the Controller started
+ *                              its last Scan Duration until it begins the
+ *                              subsequent Scan Duration. Specify 0 to scan
+ *                              continuously. Units are 1.28 second.
+ * @param limited               If limited discovery procedure should be used.
+ * @param uncoded_params        Additional arguments specifying the particulars
+ *                              of the discovery procedure for uncoded PHY.
+ *                              If NULL is provided no scan is performed for
+ *                              this PHY.
+  * @param coded_params         Additional arguments specifying the particulars
+ *                              of the discovery procedure for coded PHY.
+ *                              If NULL is provided no scan is performed for
+ *                              this PHY.
+ * @param cb                    The callback to associate with this discovery
+ *                              procedure.  Advertising reports and discovery
+ *                              termination events are reported through this
+ *                              callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                              function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period,
+                     uint8_t filter_duplicates, uint8_t filter_policy,
+                     uint8_t limited,
+                     const struct ble_gap_ext_disc_params *uncoded_params,
+                     const struct ble_gap_ext_disc_params *coded_params,
+                     ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Cancels the discovery procedure currently in progress.  A success return
+ * code indicates that scanning has been fully aborted; a new discovery or
+ * connect procedure can be initiated immediately.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if there is no discovery
+ *                                  procedure to cancel;
+ *                              Other nonzero on unexpected error.
+ */
+int ble_gap_disc_cancel(void);
+
+/**
+ * Indicates whether a discovery procedure is currently in progress.
+ *
+ * @return                      0: No discovery procedure in progress;
+ *                              1: Discovery procedure in progress.
+ */
+int ble_gap_disc_active(void);
+
+/**
+ * Initiates a connect procedure.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself during connection establishment.
+ *                                      - BLE_OWN_ADDR_PUBLIC
+ *                                      - BLE_OWN_ADDR_RANDOM
+ *                                      - BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ *                                      - BLE_OWN_ADDR_RPA_RANDOM_DEFAULT
+ * @param peer_addr             The address of the peer to connect to.
+ *                                  If this parameter is NULL, the white list
+ *                                  is used.
+ * @param duration_ms           The duration of the discovery procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are milliseconds.
+ * @param conn_params           Additional arguments specifying the particulars
+ *                                  of the connect procedure.  Specify null for
+ *                                  default values.
+ * @param cb                    The callback to associate with this connect
+ *                                  procedure.  When the connect procedure
+ *                                  completes, the result is reported through
+ *                                  this callback.  If the connect procedure
+ *                                  succeeds, the connection inherits this
+ *                                  callback as its event-reporting mechanism.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if a connection attempt is
+ *                                  already in progress;
+ *                              BLE_HS_EBUSY if initiating a connection is not
+ *                                  possible because scanning is in progress;
+ *                              BLE_HS_EDONE if the specified peer is already
+ *                                  connected;
+ *                              Other nonzero on error.
+ */
+int ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                    int32_t duration_ms,
+                    const struct ble_gap_conn_params *params,
+                    ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Initiates an extended connect procedure.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself during connection establishment.
+ *                                      - BLE_OWN_ADDR_PUBLIC
+ *                                      - BLE_OWN_ADDR_RANDOM
+ *                                      - BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ *                                      - BLE_OWN_ADDR_RPA_RANDOM_DEFAULT
+ * @param peer_addr             The address of the peer to connect to.
+ *                                  If this parameter is NULL, the white list
+ *                                  is used.
+ * @param duration_ms           The duration of the discovery procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are milliseconds.
+ * @param phy_mask              Define on which PHYs connection attempt should
+ *                                  be done
+ * @param phy_1m_conn_params     Additional arguments specifying the
+ *                                  particulars of the connect procedure. When
+ *                                  BLE_GAP_LE_PHY_1M_MASK is set in phy_mask
+ *                                  this parameter can be specify to null for
+ *                                  default values.
+ * @param phy_2m_conn_params     Additional arguments specifying the
+ *                                  particulars of the connect procedure. When
+ *                                  BLE_GAP_LE_PHY_2M_MASK is set in phy_mask
+ *                                  this parameter can be specify to null for
+ *                                  default values.
+ * @param phy_coded_conn_params  Additional arguments specifying the
+ *                                  particulars of the connect procedure. When
+ *                                  BLE_GAP_LE_PHY_CODED_MASK is set in
+ *                                  phy_mask this parameter can be specify to
+ *                                  null for default values.
+ * @param cb                    The callback to associate with this connect
+ *                                  procedure.  When the connect procedure
+ *                                  completes, the result is reported through
+ *                                  this callback.  If the connect procedure
+ *                                  succeeds, the connection inherits this
+ *                                  callback as its event-reporting mechanism.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if a connection attempt is
+ *                                  already in progress;
+ *                              BLE_HS_EBUSY if initiating a connection is not
+ *                                  possible because scanning is in progress;
+ *                              BLE_HS_EDONE if the specified peer is already
+ *                                  connected;
+ *                              Other nonzero on error.
+ */
+int ble_gap_ext_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                        int32_t duration_ms, uint8_t phy_mask,
+                        const struct ble_gap_conn_params *phy_1m_conn_params,
+                        const struct ble_gap_conn_params *phy_2m_conn_params,
+                        const struct ble_gap_conn_params *phy_coded_conn_params,
+                        ble_gap_event_fn *cb, void *cb_arg);
+
+/**
+ * Aborts a connect procedure in progress.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if there is no active connect
+ *                                  procedure.
+ *                              Other nonzero on error.
+ */
+int ble_gap_conn_cancel(void);
+
+/**
+ * Indicates whether a connect procedure is currently in progress.
+ *
+ * @return                      0: No connect procedure in progress;
+ *                              1: Connect procedure in progress.
+ */
+int ble_gap_conn_active(void);
+
+/**
+ * Terminates an established connection.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                                  terminate.
+ * @param hci_reason            The HCI error code to indicate as the reason
+ *                                  for termination.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if there is no connection with
+ *                                  the specified handle;
+ *                              Other nonzero on failure.
+ */
+int ble_gap_terminate(uint16_t conn_handle, uint8_t hci_reason);
+
+/**
+ * Overwrites the controller's white list with the specified contents.
+ *
+ * @param addrs                 The entries to write to the white list.
+ * @param white_list_count      The number of entries in the white list.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count);
+
+/**
+ * Initiates a connection parameter update procedure.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                                  update.
+ * @param params                The connection parameters to attempt to update
+ *                                  to.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if the there is no connection
+ *                                  with the specified handle;
+ *                              BLE_HS_EALREADY if a connection update
+ *                                  procedure for this connection is already in
+ *                                  progress;
+ *                              BLE_HS_EINVAL if requested parameters are
+ *                                  invalid;
+ *                              Other nonzero on error.
+ */
+int ble_gap_update_params(uint16_t conn_handle,
+                          const struct ble_gap_upd_params *params);
+
+/**
+ * Initiates the GAP security procedure.
+ *
+ * Depending on connection role and stored security information this function
+ * will start appropriate security procedure (pairing or encryption).
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                              secure.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if the there is no connection
+ *                                  with the specified handle;
+ *                              BLE_HS_EALREADY if an security procedure for
+ *                                  this connection is already in progress;
+ *                              Other nonzero on error.
+ */
+int ble_gap_security_initiate(uint16_t conn_handle);
+
+/**
+ * Initiates the GAP pairing procedure as a master. This is for testing only and
+ * should not be used by application. Use ble_gap_security_initiate() instead.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                              start pairing on.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if the there is no connection
+ *                                  with the specified handle;
+ *                              BLE_HS_EALREADY if an pairing procedure for
+ *                                  this connection is already in progress;
+ *                              Other nonzero on error.
+ */
+int ble_gap_pair_initiate(uint16_t conn_handle);
+
+/**
+ * Initiates the GAP encryption procedure as a master. This is for testing only
+ * and should not be used by application. Use ble_gap_security_initiate()
+ * instead.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                              start encryption.
+ * @param key_size              Encryption key size
+ * @param ltk                   Long Term Key to be used for encryption.
+ * @param udiv                  Encryption Diversifier for LTK
+ * @param rand_val              Random Value for EDIV and LTK
+ * @param auth                  If LTK provided is authenticated.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if the there is no connection
+ *                                  with the specified handle;
+ *                              BLE_HS_EALREADY if an encryption procedure for
+ *                                  this connection is already in progress;
+ *                              Other nonzero on error.
+ */
+int ble_gap_encryption_initiate(uint16_t conn_handle, uint8_t key_size,
+                                const uint8_t *ltk, uint16_t ediv,
+                                uint64_t rand_val, int auth);
+
+/**
+ * Retrieves the most-recently measured RSSI for the specified connection.  A
+ * connection's RSSI is updated whenever a data channel PDU is received.
+ *
+ * @param conn_handle           Specifies the connection to query.
+ * @param out_rssi              On success, the retrieved RSSI is written here.
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_gap_conn_rssi(uint16_t conn_handle, int8_t *out_rssi);
+
+/**
+ * Unpairs a device with the specified address. The keys related to that peer
+ * device are removed from storage and peer address is removed from the resolve
+ * list from the controller. If a peer is connected, the connection is terminated.
+ *
+ * @param peer_addr             Address of the device to be unpaired
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_gap_unpair(const ble_addr_t *peer_addr);
+
+/**
+ * Unpairs the oldest bonded peer device. The keys related to that peer
+ * device are removed from storage and peer address is removed from the resolve
+ * list from the controller. If a peer is connected, the connection is terminated.
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_gap_unpair_oldest_peer(void);
+
+/**
+ * Similar to `ble_gap_unpair_oldest_peer()`, except it makes sure that the
+ * peer received in input parameters is not deleted.
+ *
+ * @param peer_addr             Address of the peer (not to be deleted)
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_gap_unpair_oldest_except(const ble_addr_t *peer_addr);
+
+#define BLE_GAP_PRIVATE_MODE_NETWORK        0
+#define BLE_GAP_PRIVATE_MODE_DEVICE         1
+
+/**
+ * Set privacy mode for specified peer device
+ *
+ * @param peer_addr          Peer device address
+ * @param priv_mode          Privacy mode to be used. Can be one of following
+ *                           constants:
+ *                             - BLE_GAP_PRIVATE_MODE_NETWORK
+ *                             - BLE_GAP_PRIVATE_MODE_DEVICE
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode);
+
+#define BLE_GAP_LE_PHY_1M                   1
+#define BLE_GAP_LE_PHY_2M                   2
+#define BLE_GAP_LE_PHY_CODED                3
+/**
+ * Read PHYs used for specified connection.
+ *
+ * On success output parameters are filled with information about used PHY type.
+ *
+ * @param conn_handle       Connection handle
+ * @param tx_phy            TX PHY used. Can be one of following constants:
+ *                            - BLE_GAP_LE_PHY_1M
+ *                            - BLE_GAP_LE_PHY_2M
+ *                            - BLE_GAP_LE_PHY_CODED
+ * @param rx_phy            RX PHY used. Can be one of following constants:
+ *                            - BLE_GAP_LE_PHY_1M
+ *                            - BLE_GAP_LE_PHY_2M
+ *                            - BLE_GAP_LE_PHY_CODED
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_read_le_phy(uint16_t conn_handle, uint8_t *tx_phy, uint8_t *rx_phy);
+
+#define BLE_GAP_LE_PHY_1M_MASK              0x01
+#define BLE_GAP_LE_PHY_2M_MASK              0x02
+#define BLE_GAP_LE_PHY_CODED_MASK           0x04
+#define BLE_GAP_LE_PHY_ANY_MASK             0x0F
+/**
+ * Set preferred default PHYs to be used for connections.
+ *
+ * @params tx_phys_mask     Preferred TX PHY. Can be mask of following
+ *                          constants:
+ *                          - BLE_GAP_LE_PHY_1M_MASK
+ *                          - BLE_GAP_LE_PHY_2M_MASK
+ *                          - BLE_GAP_LE_PHY_CODED_MASK
+ *                          - BLE_GAP_LE_PHY_ANY_MASK
+ * @params rx_phys_mask     Preferred RX PHY. Can be mask of following
+ *                          constants:
+ *                          - BLE_GAP_LE_PHY_1M_MASK
+ *                          - BLE_GAP_LE_PHY_2M_MASK
+ *                          - BLE_GAP_LE_PHY_CODED_MASK
+ *                          - BLE_GAP_LE_PHY_ANY_MASK
+
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_set_prefered_default_le_phy(uint8_t tx_phys_mask,
+                                        uint8_t rx_phys_mask);
+
+#define BLE_GAP_LE_PHY_CODED_ANY            0
+#define BLE_GAP_LE_PHY_CODED_S2             1
+#define BLE_GAP_LE_PHY_CODED_S8             2
+/**
+ * Set preferred PHYs to be used for connection.
+ *
+ * @param conn_handle       Connection handle
+ * @params tx_phys_mask     Preferred TX PHY. Can be mask of following
+ *                          constants:
+ *                          - BLE_GAP_LE_PHY_1M_MASK
+ *                          - BLE_GAP_LE_PHY_2M_MASK
+ *                          - BLE_GAP_LE_PHY_CODED_MASK
+ *                          - BLE_GAP_LE_PHY_ANY_MASK
+ * @params rx_phys_mask     Preferred RX PHY. Can be mask of following
+ *                          constants:
+ *                          - BLE_GAP_LE_PHY_1M_MASK
+ *                          - BLE_GAP_LE_PHY_2M_MASK
+ *                          - BLE_GAP_LE_PHY_CODED_MASK
+ *                          - BLE_GAP_LE_PHY_ANY_MASK
+ * @param phy_opts          Additional PHY options. Valid values are:
+ *                          - BLE_GAP_LE_PHY_CODED_ANY
+ *                          - BLE_GAP_LE_PHY_CODED_S2
+ *                          - BLE_GAP_LE_PHY_CODED_S8
+ *
+ * @return                   0 on success; nonzero on failure.
+ */
+int ble_gap_set_prefered_le_phy(uint16_t conn_handle, uint8_t tx_phys_mask,
+                                uint8_t rx_phys_mask, uint16_t phy_opts);
+
+/**
+ * Event listener structure
+ *
+ * This should be used as an opaque structure and not modified manually.
+ */
+struct ble_gap_event_listener {
+    ble_gap_event_fn *fn;
+    void *arg;
+    SLIST_ENTRY(ble_gap_event_listener) link;
+};
+
+/**
+ * Registers listener for GAP events
+ *
+ * On success listener structure will be initialized automatically and does not
+ * need to be initialized prior to calling this function. To change callback
+ * and/or argument unregister listener first and register it again.
+ *
+ * @param listener      Listener structure
+ * @param fn            Callback function
+ * @param arg           Callback argument
+ *
+ * @return              0 on success
+ *                      BLE_HS_EINVAL if no callback is specified
+ *                      BLE_HS_EALREADY if listener is already registered
+ */
+int ble_gap_event_listener_register(struct ble_gap_event_listener *listener,
+                                    ble_gap_event_fn *fn, void *arg);
+
+/**
+ * Unregisters listener for GAP events
+ *
+ * @param listener      Listener structure
+ *
+ * @return              0 on success
+ *                      BLE_HS_ENOENT if listener was not registered
+ */
+int ble_gap_event_listener_unregister(struct ble_gap_event_listener *listener);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_gatt.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_gatt.h
new file mode 100644
index 00000000..b06344fc
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_gatt.h
@@ -0,0 +1,896 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_GATT_
+#define H_BLE_GATT_
+
+/**
+ * @brief Bluetooth Generic Attribute Profile (GATT)
+ * @defgroup bt_gatt Bluetooth Generic Attribute Profile (GATT)
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+#include "host/ble_att.h"
+#include "host/ble_uuid.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_conn;
+struct ble_att_error_rsp;
+struct ble_hs_cfg;
+
+#define BLE_GATT_REGISTER_OP_SVC                        1
+#define BLE_GATT_REGISTER_OP_CHR                        2
+#define BLE_GATT_REGISTER_OP_DSC                        3
+
+#define BLE_GATT_SVC_UUID16                             0x1801
+#define BLE_GATT_DSC_CLT_CFG_UUID16                     0x2902
+
+#define BLE_GATT_CHR_PROP_BROADCAST                     0x01
+#define BLE_GATT_CHR_PROP_READ                          0x02
+#define BLE_GATT_CHR_PROP_WRITE_NO_RSP                  0x04
+#define BLE_GATT_CHR_PROP_WRITE                         0x08
+#define BLE_GATT_CHR_PROP_NOTIFY                        0x10
+#define BLE_GATT_CHR_PROP_INDICATE                      0x20
+#define BLE_GATT_CHR_PROP_AUTH_SIGN_WRITE               0x40
+#define BLE_GATT_CHR_PROP_EXTENDED                      0x80
+
+#define BLE_GATT_ACCESS_OP_READ_CHR                     0
+#define BLE_GATT_ACCESS_OP_WRITE_CHR                    1
+#define BLE_GATT_ACCESS_OP_READ_DSC                     2
+#define BLE_GATT_ACCESS_OP_WRITE_DSC                    3
+
+#define BLE_GATT_CHR_F_BROADCAST                        0x0001
+#define BLE_GATT_CHR_F_READ                             0x0002
+#define BLE_GATT_CHR_F_WRITE_NO_RSP                     0x0004
+#define BLE_GATT_CHR_F_WRITE                            0x0008
+#define BLE_GATT_CHR_F_NOTIFY                           0x0010
+#define BLE_GATT_CHR_F_INDICATE                         0x0020
+#define BLE_GATT_CHR_F_AUTH_SIGN_WRITE                  0x0040
+#define BLE_GATT_CHR_F_RELIABLE_WRITE                   0x0080
+#define BLE_GATT_CHR_F_AUX_WRITE                        0x0100
+#define BLE_GATT_CHR_F_READ_ENC                         0x0200
+#define BLE_GATT_CHR_F_READ_AUTHEN                      0x0400
+#define BLE_GATT_CHR_F_READ_AUTHOR                      0x0800
+#define BLE_GATT_CHR_F_WRITE_ENC                        0x1000
+#define BLE_GATT_CHR_F_WRITE_AUTHEN                     0x2000
+#define BLE_GATT_CHR_F_WRITE_AUTHOR                     0x4000
+
+#define BLE_GATT_SVC_TYPE_END                           0
+#define BLE_GATT_SVC_TYPE_PRIMARY                       1
+#define BLE_GATT_SVC_TYPE_SECONDARY                     2
+
+/*** @client. */
+struct ble_gatt_error {
+    uint16_t status;
+    uint16_t att_handle;
+};
+
+struct ble_gatt_svc {
+    uint16_t start_handle;
+    uint16_t end_handle;
+    ble_uuid_any_t uuid;
+};
+
+struct ble_gatt_attr {
+    uint16_t handle;
+    uint16_t offset;
+    struct os_mbuf *om;
+};
+
+struct ble_gatt_chr {
+    uint16_t def_handle;
+    uint16_t val_handle;
+    uint8_t properties;
+    ble_uuid_any_t uuid;
+};
+
+struct ble_gatt_dsc {
+    uint16_t handle;
+    ble_uuid_any_t uuid;
+};
+
+typedef int ble_gatt_mtu_fn(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            uint16_t mtu, void *arg);
+typedef int ble_gatt_disc_svc_fn(uint16_t conn_handle,
+                                 const struct ble_gatt_error *error,
+                                 const struct ble_gatt_svc *service,
+                                 void *arg);
+
+/**
+ * The host will free the attribute mbuf automatically after the callback is
+ * executed.  The application can take ownership of the mbuf and prevent it
+ * from being freed by assigning NULL to attr->om.
+ */
+typedef int ble_gatt_attr_fn(uint16_t conn_handle,
+                             const struct ble_gatt_error *error,
+                             struct ble_gatt_attr *attr,
+                             void *arg);
+
+/**
+ * The host will free the attribute mbufs automatically after the callback is
+ * executed.  The application can take ownership of the mbufs and prevent them
+ * from being freed by assigning NULL to each attribute's om field.
+ */
+typedef int ble_gatt_reliable_attr_fn(uint16_t conn_handle,
+                                      const struct ble_gatt_error *error,
+                                      struct ble_gatt_attr *attrs,
+                                      uint8_t num_attrs, void *arg);
+
+typedef int ble_gatt_chr_fn(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            const struct ble_gatt_chr *chr, void *arg);
+
+typedef int ble_gatt_dsc_fn(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            uint16_t chr_val_handle,
+                            const struct ble_gatt_dsc *dsc,
+                            void *arg);
+
+/**
+ * Initiates GATT procedure: Exchange MTU.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_exchange_mtu(uint16_t conn_handle,
+                           ble_gatt_mtu_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Discover All Primary Services.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ */
+int ble_gattc_disc_all_svcs(uint16_t conn_handle,
+                            ble_gatt_disc_svc_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Discover Primary Service by Service UUID.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param service_uuid128       The 128-bit UUID of the service to discover.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, const ble_uuid_t *uuid,
+                               ble_gatt_disc_svc_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Find Included Services.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param start_handle          The handle to begin the search at (generally
+ *                                  the service definition handle).
+ * @param end_handle            The handle to end the search at (generally the
+ *                                  last handle in the service).
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_find_inc_svcs(uint16_t conn_handle, uint16_t start_handle,
+                            uint16_t end_handle,
+                            ble_gatt_disc_svc_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Discover All Characteristics of a Service.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param start_handle          The handle to begin the search at (generally
+ *                                  the service definition handle).
+ * @param end_handle            The handle to end the search at (generally the
+ *                                  last handle in the service).
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle,
+                            uint16_t end_handle, ble_gatt_chr_fn *cb,
+                            void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Discover Characteristics by UUID.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param start_handle          The handle to begin the search at (generally
+ *                                  the service definition handle).
+ * @param end_handle            The handle to end the search at (generally the
+ *                                  last handle in the service).
+ * @param chr_uuid128           The 128-bit UUID of the characteristic to
+ *                                  discover.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle,
+                               uint16_t end_handle, const ble_uuid_t *uuid,
+                               ble_gatt_chr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Discover All Characteristic Descriptors.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The handle of the characteristic value
+ *                                  attribute.
+ * @param chr_end_handle        The last handle in the characteristic
+ *                                  definition.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t start_handle,
+                            uint16_t end_handle,
+                            ble_gatt_dsc_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Read Characteristic Value.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to read.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_read(uint16_t conn_handle, uint16_t attr_handle,
+                   ble_gatt_attr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Read Using Characteristic UUID.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param start_handle          The first handle to search (generally the
+ *                                  handle of the service definition).
+ * @param end_handle            The last handle to search (generally the
+ *                                  last handle in the service definition).
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle,
+                           uint16_t end_handle, const ble_uuid_t *uuid,
+                           ble_gatt_attr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Read Long Characteristic Values.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param handle                The handle of the characteristic value to read.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_read_long(uint16_t conn_handle, uint16_t handle, uint16_t offset,
+                        ble_gatt_attr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Read Multiple Characteristic Values.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param handles               An array of 16-bit attribute handles to read.
+ * @param num_handles           The number of entries in the "handles" array.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles,
+                        uint8_t num_handles, ble_gatt_attr_fn *cb,
+                        void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Write Without Response.  This function consumes
+ * the supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param txom                  The value to write to the characteristic.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
+                           struct os_mbuf *om);
+
+/**
+ * Initiates GATT procedure: Write Without Response.  This function consumes
+ * the supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param value                 The value to write to the characteristic.
+ * @param value_len             The number of bytes to write.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_write_no_rsp_flat(uint16_t conn_handle, uint16_t attr_handle,
+                                const void *data, uint16_t data_len);
+
+/**
+ * Initiates GATT procedure: Write Characteristic Value.  This function
+ * consumes the supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param txom                  The value to write to the characteristic.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle,
+                    struct os_mbuf *om,
+                    ble_gatt_attr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Write Characteristic Value (flat buffer version).
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param value                 The value to write to the characteristic.
+ * @param value_len             The number of bytes to write.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_write_flat(uint16_t conn_handle, uint16_t attr_handle,
+                         const void *data, uint16_t data_len,
+                         ble_gatt_attr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Write Long Characteristic Values.  This function
+ * consumes the supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param txom                  The value to write to the characteristic.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle,
+                         uint16_t offset, struct os_mbuf *om,
+                         ble_gatt_attr_fn *cb, void *cb_arg);
+
+/**
+ * Initiates GATT procedure: Reliable Writes.  This function consumes the
+ * supplied mbufs regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attrs                 An array of attribute descriptors; specifies
+ *                                  which characteristics to write to and what
+ *                                  data to write to them.  The mbuf pointer in
+ *                                  each attribute is set to NULL by this
+ *                                  function.
+ * @param num_attrs             The number of characteristics to write; equal
+ *                                  to the number of elements in the 'attrs'
+ *                                  array.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ */
+int ble_gattc_write_reliable(uint16_t conn_handle,
+                             struct ble_gatt_attr *attrs,
+                             int num_attrs, ble_gatt_reliable_attr_fn *cb,
+                             void *cb_arg);
+
+/**
+ * Sends a "free-form" characteristic notification.  This function consumes the
+ * supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The attribute handle to indicate in the
+ *                                  outgoing notification.
+ * @param txom                  The value to write to the characteristic.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_notify_custom(uint16_t conn_handle, uint16_t att_handle,
+                            struct os_mbuf *om);
+
+/**
+ * Sends a characteristic notification.  The content of the message is read
+ * from the specified characteristic.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The value attribute handle of the
+ *                                  characteristic to include in the outgoing
+ *                                  notification.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle);
+
+/**
+ * Sends a "free-form" characteristic indication.  The provided mbuf contains
+ * the indication payload.  This function consumes the supplied mbuf regardless
+ * of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The value attribute handle of the
+ *                                  characteristic to include in the outgoing
+ *                                  indication.
+ * @param txom                  The data to include in the indication.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle,
+                              struct os_mbuf *txom);
+
+/**
+ * Sends a characteristic indication.  The content of the message is read from
+ * the specified characteristic.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The value attribute handle of the
+ *                                  characteristic to include in the outgoing
+ *                                  indication.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle);
+
+int ble_gattc_init(void);
+
+/*** @server. */
+
+struct ble_gatt_access_ctxt;
+typedef int ble_gatt_access_fn(uint16_t conn_handle, uint16_t attr_handle,
+                               struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+typedef uint16_t ble_gatt_chr_flags;
+
+struct ble_gatt_chr_def {
+    /**
+     * Pointer to characteristic UUID; use BLE_UUIDxx_DECLARE macros to declare
+     * proper UUID; NULL if there are no more characteristics in the service.
+     */
+    const ble_uuid_t *uuid;
+
+    /**
+     * Callback that gets executed when this characteristic is read or
+     * written.
+     */
+    ble_gatt_access_fn *access_cb;
+
+    /** Optional argument for callback. */
+    void *arg;
+
+    /**
+     * Array of this characteristic's descriptors.  NULL if no descriptors.
+     * Do not include CCCD; it gets added automatically if this
+     * characteristic's notify or indicate flag is set.
+     */
+    struct ble_gatt_dsc_def *descriptors;
+
+    /** Specifies the set of permitted operations for this characteristic. */
+    ble_gatt_chr_flags flags;
+
+    /** Specifies minimum required key size to access this characteristic. */
+    uint8_t min_key_size;
+
+    /**
+     * At registration time, this is filled in with the characteristic's value
+     * attribute handle.
+     */
+    uint16_t *val_handle;
+};
+
+struct ble_gatt_svc_def {
+    /**
+     * One of the following:
+     *     o BLE_GATT_SVC_TYPE_PRIMARY - primary service
+     *     o BLE_GATT_SVC_TYPE_SECONDARY - secondary service
+     *     o 0 - No more services in this array.
+     */
+    uint8_t type;
+
+    /**
+     * Pointer to service UUID; use BLE_UUIDxx_DECLARE macros to declare
+     * proper UUID; NULL if there are no more characteristics in the service.
+     */
+    const ble_uuid_t *uuid;
+
+    /**
+     * Array of pointers to other service definitions.  These services are
+     * reported as "included services" during service discovery.  Terminate the
+     * array with NULL.
+     */
+    const struct ble_gatt_svc_def **includes;
+
+    /**
+     * Array of characteristic definitions corresponding to characteristics
+     * belonging to this service.
+     */
+    const struct ble_gatt_chr_def *characteristics;
+};
+
+struct ble_gatt_dsc_def {
+    /**
+     * Pointer to descriptor UUID; use BLE_UUIDxx_DECLARE macros to declare
+     * proper UUID; NULL if there are no more characteristics in the service.
+     */
+    const ble_uuid_t *uuid;
+
+    /** Specifies the set of permitted operations for this descriptor. */
+    uint8_t att_flags;
+
+    /** Specifies minimum required key size to access this descriptor. */
+    uint8_t min_key_size;
+
+    /** Callback that gets executed when the descriptor is read or written. */
+    ble_gatt_access_fn *access_cb;
+
+    /** Optional argument for callback. */
+    void *arg;
+};
+
+/**
+ * Context for an access to a GATT characteristic or descriptor.  When a client
+ * reads or writes a locally registered characteristic or descriptor, an
+ * instance of this struct gets passed to the application callback.
+ */
+struct ble_gatt_access_ctxt {
+    /**
+     * Indicates the gatt operation being performed.  This is equal to one of
+     * the following values:
+     *     o  BLE_GATT_ACCESS_OP_READ_CHR
+     *     o  BLE_GATT_ACCESS_OP_WRITE_CHR
+     *     o  BLE_GATT_ACCESS_OP_READ_DSC
+     *     o  BLE_GATT_ACCESS_OP_WRITE_DSC
+     */
+    uint8_t op;
+
+    /**
+     * A container for the GATT access data.
+     *     o For reads: The application populates this with the value of the
+     *       characteristic or descriptor being read.
+     *     o For writes: This is already populated with the value being written
+     *       by the peer.  If the application wishes to retain this mbuf for
+     *       later use, the access callback must set this pointer to NULL to
+     *       prevent the stack from freeing it.
+     */
+    struct os_mbuf *om;
+
+    /**
+     * The GATT operation being performed dictates which field in this union is
+     * valid.  If a characteristic is being accessed, the chr field is valid.
+     * Otherwise a descriptor is being accessed, in which case the dsc field
+     * is valid.
+     */
+    union {
+        /**
+         * The characteristic definition corresponding to the characteristic
+         * being accessed.  This is what the app registered at startup.
+         */
+        const struct ble_gatt_chr_def *chr;
+
+        /**
+         * The descriptor definition corresponding to the descriptor being
+         * accessed.  This is what the app registered at startup.
+         */
+        const struct ble_gatt_dsc_def *dsc;
+    };
+};
+
+/**
+ * Context passed to the registration callback; represents the GATT service,
+ * characteristic, or descriptor being registered.
+ */
+struct ble_gatt_register_ctxt {
+    /**
+     * Indicates the gatt registration operation just performed.  This is
+     * equal to one of the following values:
+     *     o BLE_GATT_REGISTER_OP_SVC
+     *     o BLE_GATT_REGISTER_OP_CHR
+     *     o BLE_GATT_REGISTER_OP_DSC
+     */
+    uint8_t op;
+
+    /**
+     * The value of the op field determines which field in this union is valid.
+     */
+    union {
+        /** Service; valid if op == BLE_GATT_REGISTER_OP_SVC. */
+        struct {
+            /** The ATT handle of the service definition attribute. */
+            uint16_t handle;
+
+            /**
+             * The service definition representing the service being
+             * registered.
+             */
+            const struct ble_gatt_svc_def *svc_def;
+        } svc;
+
+        /** Characteristic; valid if op == BLE_GATT_REGISTER_OP_CHR. */
+        struct {
+            /** The ATT handle of the characteristic definition attribute. */
+            uint16_t def_handle;
+
+            /** The ATT handle of the characteristic value attribute. */
+            uint16_t val_handle;
+
+            /**
+             * The characteristic definition representing the characteristic
+             * being registered.
+             */
+            const struct ble_gatt_chr_def *chr_def;
+
+            /**
+             * The service definition corresponding to the characteristic's
+             * parent service.
+             */
+            const struct ble_gatt_svc_def *svc_def;
+        } chr;
+
+        /** Descriptor; valid if op == BLE_GATT_REGISTER_OP_DSC. */
+        struct {
+            /** The ATT handle of the descriptor definition attribute. */
+            uint16_t handle;
+
+            /**
+             * The descriptor definition corresponding to the descriptor being
+             * registered.
+             */
+            const struct ble_gatt_dsc_def *dsc_def;
+
+            /**
+             * The characteristic definition corresponding to the descriptor's
+             * parent characteristic.
+             */
+            const struct ble_gatt_chr_def *chr_def;
+
+            /**
+             * The service definition corresponding to the descriptor's
+             * grandparent service
+             */
+            const struct ble_gatt_svc_def *svc_def;
+        } dsc;
+    };
+};
+
+typedef void ble_gatt_register_fn(struct ble_gatt_register_ctxt *ctxt,
+                                  void *arg);
+
+/**
+ * Queues a set of service definitions for registration.  All services queued
+ * in this manner get registered when ble_gatts_start() is called.
+ *
+ * @param svcs                  An array of service definitions to queue for
+ *                                  registration.  This array must be
+ *                                  terminated with an entry whose 'type'
+ *                                  equals 0.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOMEM on heap exhaustion.
+ */
+int ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs);
+
+/**
+ * Set visibility of local GATT service. Invisible services are not removed
+ * from database but are not discoverable by peer devices. Service Changed
+ * should be handled by application when needed by calling
+ * ble_svc_gatt_changed().
+ *
+ * @param handle                Handle of service
+ * @param visible               non-zero if service should be visible
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if service wasn't found.
+ */
+int ble_gatts_svc_set_visibility(uint16_t handle, int visible);
+
+/**
+ * Adjusts a host configuration object's settings to accommodate the specified
+ * service definition array.  This function adds the counts to the appropriate
+ * fields in the supplied configuration object without clearing them first, so
+ * it can be called repeatedly with different inputs to calculate totals.  Be
+ * sure to zero the GATT server settings prior to the first call to this
+ * function.
+ *
+ * @param defs                  The service array containing the resource
+ *                                  definitions to be counted.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the svcs array contains an
+ *                                  invalid resource definition.
+ */
+int ble_gatts_count_cfg(const struct ble_gatt_svc_def *defs);
+
+/**
+ * Send notification (or indication) to any connected devices that have
+ * subscribed for notification (or indication) for specified characteristic.
+ *
+ * @param chr_val_handle        Characteristic value handle
+ */
+void ble_gatts_chr_updated(uint16_t chr_val_handle);
+
+/**
+ * Retrieves the attribute handle associated with a local GATT service.
+ *
+ * @param uuid                  The UUID of the service to look up.
+ * @param out_handle            On success, populated with the handle of the
+ *                                  service attribute.  Pass null if you don't
+ *                                  need this value.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if the specified service could
+ *                                  not be found.
+ */
+int ble_gatts_find_svc(const ble_uuid_t *uuid, uint16_t *out_handle);
+
+/**
+ * Retrieves the pair of attribute handles associated with a local GATT
+ * characteristic.
+ *
+ * @param svc_uuid              The UUID of the parent service.
+ * @param chr_uuid              The UUID of the characteristic to look up.
+ * @param out_def_handle        On success, populated with the handle
+ *                                  of the characteristic definition attribute.
+ *                                  Pass null if you don't need this value.
+ * @param out_val_handle        On success, populated with the handle
+ *                                  of the characteristic value attribute.
+ *                                  Pass null if you don't need this value.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if the specified service or
+ *                                  characteristic could not be found.
+ */
+int ble_gatts_find_chr(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
+                       uint16_t *out_def_handle, uint16_t *out_val_handle);
+
+/**
+ * Retrieves the attribute handle associated with a local GATT descriptor.
+ *
+ * @param svc_uuid              The UUID of the grandparent service.
+ * @param chr_uuid              The UUID of the parent characteristic.
+ * @param dsc_uuid              The UUID of the descriptor ro look up.
+ * @param out_handle            On success, populated with the handle
+ *                                  of the descriptor attribute.  Pass null if
+ *                                  you don't need this value.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if the specified service,
+ *                                  characteristic, or descriptor could not be
+ *                                  found.
+ */
+int ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
+                       const ble_uuid_t *dsc_uuid, uint16_t *out_dsc_handle);
+
+typedef void (*ble_gatt_svc_foreach_fn)(const struct ble_gatt_svc_def *svc,
+                                        uint16_t handle,
+                                        uint16_t end_group_handle,
+                                        void *arg);
+
+/**
+ * Prints dump of local GATT database. This is useful to log local state of
+ * database in human readable form.
+ */
+void ble_gatts_show_local(void);
+
+/**
+ * Resets the GATT server to its initial state.  On success, this function
+ * removes all supported services, characteristics, and descriptors.  This
+ * function requires that:
+ *     o No peers are connected, and
+ *     o No GAP operations are active (advertise, discover, or connect).
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if the GATT server could not be
+ *                                  reset due to existing connections or active
+ *                                  GAP procedures.
+ */
+int ble_gatts_reset(void);
+
+/**
+ * Makes all registered services available to peers.  This function gets called
+ * automatically by the NimBLE host on startup; manual calls are only necessary
+ * for replacing the set of supported services with a new one.  This function
+ * requires that:
+ *     o No peers are connected, and
+ *     o No GAP operations are active (advertise, discover, or connect).
+ *
+ * @return                      0 on success;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_gatts_start(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs.h
new file mode 100644
index 00000000..6c2acfd4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs.h
@@ -0,0 +1,386 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_
+#define H_BLE_HS_
+
+/**
+ * @brief Bluetooth Host
+ * @defgroup bt_host Bluetooth Host
+ * @{
+ */
+
+#include <inttypes.h>
+#include "nimble/hci_common.h"
+#include "host/ble_att.h"
+#include "host/ble_eddystone.h"
+#include "host/ble_gap.h"
+#include "host/ble_gatt.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_hs_id.h"
+#include "host/ble_hs_hci.h"
+#include "host/ble_hs_log.h"
+#include "host/ble_hs_mbuf.h"
+#include "host/ble_hs_stop.h"
+#include "host/ble_ibeacon.h"
+#include "host/ble_l2cap.h"
+#include "host/ble_sm.h"
+#include "host/ble_store.h"
+#include "host/ble_uuid.h"
+#include "nimble/nimble_npl.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_HS_FOREVER              INT32_MAX
+
+/** Connection handle not present */
+#define BLE_HS_CONN_HANDLE_NONE     0xffff
+
+/**
+ * @brief Bluetooth Host Error Code
+ * @defgroup bt_host_err Bluetooth Host Error Code
+ *
+ * Defines error codes returned by Bluetooth host. If error comes from specific
+ * component (eg L2CAP or Security Manager) it is shifted by base allowing to
+ * identify component.
+ * @{
+ */
+
+#define BLE_HS_EAGAIN               1
+#define BLE_HS_EALREADY             2
+#define BLE_HS_EINVAL               3
+#define BLE_HS_EMSGSIZE             4
+#define BLE_HS_ENOENT               5
+#define BLE_HS_ENOMEM               6
+#define BLE_HS_ENOTCONN             7
+#define BLE_HS_ENOTSUP              8
+#define BLE_HS_EAPP                 9
+#define BLE_HS_EBADDATA             10
+#define BLE_HS_EOS                  11
+#define BLE_HS_ECONTROLLER          12
+#define BLE_HS_ETIMEOUT             13
+#define BLE_HS_EDONE                14
+#define BLE_HS_EBUSY                15
+#define BLE_HS_EREJECT              16
+#define BLE_HS_EUNKNOWN             17
+#define BLE_HS_EROLE                18
+#define BLE_HS_ETIMEOUT_HCI         19
+#define BLE_HS_ENOMEM_EVT           20
+#define BLE_HS_ENOADDR              21
+#define BLE_HS_ENOTSYNCED           22
+#define BLE_HS_EAUTHEN              23
+#define BLE_HS_EAUTHOR              24
+#define BLE_HS_EENCRYPT             25
+#define BLE_HS_EENCRYPT_KEY_SZ      26
+#define BLE_HS_ESTORE_CAP           27
+#define BLE_HS_ESTORE_FAIL          28
+#define BLE_HS_EPREEMPTED           29
+#define BLE_HS_EDISABLED            30
+#define BLE_HS_ESTALLED             31
+
+/** Error base for ATT errors */
+#define BLE_HS_ERR_ATT_BASE         0x100
+
+/** Converts error to ATT base */
+#define BLE_HS_ATT_ERR(x)           ((x) ? BLE_HS_ERR_ATT_BASE + (x) : 0)
+
+/** Error base for HCI errors */
+#define BLE_HS_ERR_HCI_BASE         0x200
+
+/** Converts error to HCI base */
+#define BLE_HS_HCI_ERR(x)           ((x) ? BLE_HS_ERR_HCI_BASE + (x) : 0)
+
+/** Error base for L2CAP errors */
+#define BLE_HS_ERR_L2C_BASE         0x300
+
+/** Converts error to L2CAP base */
+#define BLE_HS_L2C_ERR(x)           ((x) ? BLE_HS_ERR_L2C_BASE + (x) : 0)
+
+/** Error base for local Security Manager errors */
+#define BLE_HS_ERR_SM_US_BASE       0x400
+
+/** Converts error to local Security Manager base */
+#define BLE_HS_SM_US_ERR(x)         ((x) ? BLE_HS_ERR_SM_US_BASE + (x) : 0)
+
+/** Error base for remote (peer) Security Manager errors */
+#define BLE_HS_ERR_SM_PEER_BASE     0x500
+
+/** Converts error to remote (peer) Security Manager base */
+#define BLE_HS_SM_PEER_ERR(x)       ((x) ? BLE_HS_ERR_SM_PEER_BASE + (x) : 0)
+
+/** Error base for hardware errors */
+#define BLE_HS_ERR_HW_BASE          0x600
+
+/** Converts error to hardware error  base */
+#define BLE_HS_HW_ERR(x)            (BLE_HS_ERR_HW_BASE + (x))
+
+/**
+ * @}
+ */
+
+/**
+ * @brief Bluetooth Host Configuration
+ * @defgroup bt_host_conf Bluetooth Host Configuration
+ *
+ * @{
+ */
+
+/**
+ * @brief Local Input-Output capabilities of device
+ * @defgroup bt_host_io_local Local Input-Output capabilities of device
+ *
+ * @{
+ */
+
+/** DisplayOnly IO capability */
+#define BLE_HS_IO_DISPLAY_ONLY              0x00
+
+/** DisplayYesNo IO capability */
+#define BLE_HS_IO_DISPLAY_YESNO             0x01
+
+/** KeyboardOnly IO capability */
+#define BLE_HS_IO_KEYBOARD_ONLY             0x02
+
+/** NoInputNoOutput IO capability */
+#define BLE_HS_IO_NO_INPUT_OUTPUT           0x03
+
+/** KeyboardDisplay Only IO capability */
+#define BLE_HS_IO_KEYBOARD_DISPLAY          0x04
+
+/**
+ * @}
+ */
+
+/** @brief Stack reset callback
+ *
+ * @param reason Reason code for reset
+ */
+typedef void ble_hs_reset_fn(int reason);
+
+
+/** @brief Stack sync callback */
+typedef void ble_hs_sync_fn(void);
+
+/** @brief Bluetooth Host main configuration structure
+ *
+ * Those can be used by application to configure stack.
+ *
+ * The only reason Security Manager (sm_ members) is configurable at runtime is
+ * to simplify security testing. Defaults for those are configured by selecting
+ * proper options in application's syscfg.
+ */
+struct ble_hs_cfg {
+    /**
+     * An optional callback that gets executed upon registration of each GATT
+     * resource (service, characteristic, or descriptor).
+     */
+    ble_gatt_register_fn *gatts_register_cb;
+
+    /**
+     * An optional argument that gets passed to the GATT registration
+     * callback.
+     */
+    void *gatts_register_arg;
+
+    /** Security Manager Local Input Output Capabilities */
+    uint8_t sm_io_cap;
+
+    /** @brief Security Manager OOB flag
+     *
+     * If set proper flag in Pairing Request/Response will be set.
+     */
+    unsigned sm_oob_data_flag:1;
+
+    /** @brief Security Manager Bond flag
+     *
+     * If set proper flag in Pairing Request/Response will be set. This results
+     * in storing keys distributed during bonding.
+     */
+    unsigned sm_bonding:1;
+
+    /** @brief Security Manager MITM flag
+     *
+     * If set proper flag in Pairing Request/Response will be set. This results
+     * in requiring Man-In-The-Middle protection when pairing.
+     */
+    unsigned sm_mitm:1;
+
+    /** @brief Security Manager Secure Connections flag
+     *
+     * If set proper flag in Pairing Request/Response will be set. This results
+     * in using LE Secure Connections for pairing if also supported by remote
+     * device. Fallback to legacy pairing if not supported by remote.
+     */
+    unsigned sm_sc:1;
+
+    /** @brief Security Manager Key Press Notification flag
+     *
+     * Currently unsupported and should not be set.
+     */
+    unsigned sm_keypress:1;
+
+    /** @brief Security Manager Local Key Distribution Mask */
+    uint8_t sm_our_key_dist;
+
+    /** @brief Security Manager Remote Key Distribution Mask */
+    uint8_t sm_their_key_dist;
+
+    /** @brief Stack reset callback
+     *
+     * This callback is executed when the host resets itself and the controller
+     * due to fatal error.
+     */
+    ble_hs_reset_fn *reset_cb;
+
+    /** @brief Stack sync callback
+     *
+     * This callback is executed when the host and controller become synced.
+     * This happens at startup and after a reset.
+     */
+    ble_hs_sync_fn *sync_cb;
+
+    /* XXX: These need to go away. Instead, the nimble host package should
+     * require the host-store API (not yet implemented)..
+     */
+    /** Storage Read callback handles read of security material */
+    ble_store_read_fn *store_read_cb;
+
+    /** Storage Write callback handles write of security material */
+    ble_store_write_fn *store_write_cb;
+
+    /** Storage Delete callback handles deletion of security material */
+    ble_store_delete_fn *store_delete_cb;
+
+    /** @brief Storage Status callback.
+     *
+     * This callback gets executed when a persistence operation cannot be
+     * performed or a persistence failure is imminent. For example, if is
+     * insufficient storage capacity for a record to be persisted, this
+     * function gets called to give the application the opportunity to make
+     * room.
+     */
+    ble_store_status_fn *store_status_cb;
+
+    /** An optional argument that gets passed to the storage status callback. */
+    void *store_status_arg;
+};
+
+extern struct ble_hs_cfg ble_hs_cfg;
+
+/**
+ * @}
+ */
+
+/**
+ * @brief Indicates whether the host is enabled.  The host is enabled if it is
+ * starting or fully started.  It is disabled if it is stopping or stopped.
+ *
+ * @return 1 if the host is enabled;
+ *         0 if the host is disabled.
+ */
+int ble_hs_is_enabled(void);
+
+/**
+ * Indicates whether the host has synchronized with the controller.
+ * Synchronization must occur before any host procedures can be performed.
+ *
+ * @return 1 if the host and controller are in sync;
+ *         0 if the host and controller are out of sync.
+ */
+int ble_hs_synced(void);
+
+/**
+ * Synchronizes the host with the controller by sending a sequence of HCI
+ * commands.  This function must be called before any other host functionality
+ * is used, but it must be called after both the host and controller are
+ * initialized.  Typically, the host-parent-task calls this function at the top
+ * of its task routine.  This function must only be called in the host parent
+ * task.  A safe alternative for starting the stack from any task is to call
+ * `ble_hs_sched_start()`.
+ *
+ * If the host fails to synchronize with the controller (if the controller is
+ * not fully booted, for example), the host will attempt to resynchronize every
+ * 100 ms. For this reason, an error return code is not necessarily fatal.
+ *
+ * @return 0 on success; nonzero on error.
+ */
+int ble_hs_start(void);
+
+/**
+ * Enqueues a host start event to the default event queue.  The actual host
+ * startup is performed in the host parent task, but using the default queue
+ * here ensures the event won't run until the end of main() when this is
+ * called during system initialization.  This allows the application to
+ * configure the host package in the meantime.
+ *
+ * If auto-start is disabled, the application should use this function to start
+ * the BLE stack.  This function can be called at any time as long as the host
+ * is stopped.  When the host successfully starts, the application is notified
+ * via the ble_hs_cfg.sync_cb callback.
+ */
+void ble_hs_sched_start(void);
+
+/**
+ * Causes the host to reset the NimBLE stack as soon as possible.  The
+ * application is notified when the reset occurs via the host reset callback.
+ *
+ * @param reason The host error code that gets passed to the reset callback.
+ */
+void ble_hs_sched_reset(int reason);
+
+/**
+ * Designates the specified event queue for NimBLE host work. By default, the
+ * host uses the default event queue and runs in the main task. This function
+ * is useful if you want the host to run in a different task.
+ *
+ * @param evq The event queue to use for host work.
+ */
+void ble_hs_evq_set(struct ble_npl_eventq *evq);
+
+/**
+ * Initializes the NimBLE host. This function must be called before the OS is
+ * started. The NimBLE stack requires an application task to function.  One
+ * application task in particular is designated as the "host parent task". In
+ * addition to application-specific work, the host parent task does work for
+ * NimBLE by processing events generated by the host.
+ */
+void ble_hs_init(void);
+
+/**
+ * @brief Called when the system is shutting down.  Stops the BLE host.
+ *
+ * @param reason                The reason for the shutdown.  One of the
+ *                                  HAL_RESET_[...] codes or an
+ *                                  implementation-defined value.
+ *
+ * @return                      SYSDOWN_IN_PROGRESS. 
+ */
+int ble_hs_shutdown(int reason);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_adv.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_adv.h
new file mode 100644
index 00000000..e3b6ea70
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_adv.h
@@ -0,0 +1,177 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_ADV_
+#define H_BLE_HS_ADV_
+
+#include <inttypes.h>
+#include "host/ble_uuid.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_HS_ADV_MAX_SZ           BLE_HCI_MAX_ADV_DATA_LEN
+
+/** Max field payload size (account for 2-byte header). */
+#define BLE_HS_ADV_MAX_FIELD_SZ     (BLE_HS_ADV_MAX_SZ - 2)
+
+struct ble_hs_adv_field {
+    uint8_t length;
+    uint8_t type;
+    uint8_t value[0];
+};
+
+typedef int (* ble_hs_adv_parse_func_t) (const struct ble_hs_adv_field *,
+                                         void *);
+
+struct ble_hs_adv_fields {
+    /*** 0x01 - Flags. */
+    uint8_t flags;
+
+    /*** 0x02,0x03 - 16-bit service class UUIDs. */
+    const ble_uuid16_t *uuids16;
+    uint8_t num_uuids16;
+    unsigned uuids16_is_complete:1;
+
+    /*** 0x04,0x05 - 32-bit service class UUIDs. */
+    const ble_uuid32_t *uuids32;
+    uint8_t num_uuids32;
+    unsigned uuids32_is_complete:1;
+
+    /*** 0x06,0x07 - 128-bit service class UUIDs. */
+    const ble_uuid128_t *uuids128;
+    uint8_t num_uuids128;
+    unsigned uuids128_is_complete:1;
+
+    /*** 0x08,0x09 - Local name. */
+    const uint8_t *name;
+    uint8_t name_len;
+    unsigned name_is_complete:1;
+
+    /*** 0x0a - Tx power level. */
+    int8_t tx_pwr_lvl;
+    unsigned tx_pwr_lvl_is_present:1;
+
+    /*** 0x0d - Slave connection interval range. */
+    const uint8_t *slave_itvl_range;
+
+    /*** 0x16 - Service data - 16-bit UUID. */
+    const uint8_t *svc_data_uuid16;
+    uint8_t svc_data_uuid16_len;
+
+    /*** 0x17 - Public target address. */
+    const uint8_t *public_tgt_addr;
+    uint8_t num_public_tgt_addrs;
+
+    /*** 0x19 - Appearance. */
+    uint16_t appearance;
+    unsigned appearance_is_present:1;
+
+    /*** 0x1a - Advertising interval. */
+    uint16_t adv_itvl;
+    unsigned adv_itvl_is_present:1;
+
+    /*** 0x20 - Service data - 32-bit UUID. */
+    const uint8_t *svc_data_uuid32;
+    uint8_t svc_data_uuid32_len;
+
+    /*** 0x21 - Service data - 128-bit UUID. */
+    const uint8_t *svc_data_uuid128;
+    uint8_t svc_data_uuid128_len;
+
+    /*** 0x24 - URI. */
+    const uint8_t *uri;
+    uint8_t uri_len;
+
+    /*** 0xff - Manufacturer specific data. */
+    const uint8_t *mfg_data;
+    uint8_t mfg_data_len;
+};
+
+#define BLE_HS_ADV_TYPE_FLAGS                   0x01
+#define BLE_HS_ADV_TYPE_INCOMP_UUIDS16          0x02
+#define BLE_HS_ADV_TYPE_COMP_UUIDS16            0x03
+#define BLE_HS_ADV_TYPE_INCOMP_UUIDS32          0x04
+#define BLE_HS_ADV_TYPE_COMP_UUIDS32            0x05
+#define BLE_HS_ADV_TYPE_INCOMP_UUIDS128         0x06
+#define BLE_HS_ADV_TYPE_COMP_UUIDS128           0x07
+#define BLE_HS_ADV_TYPE_INCOMP_NAME             0x08
+#define BLE_HS_ADV_TYPE_COMP_NAME               0x09
+#define BLE_HS_ADV_TYPE_TX_PWR_LVL              0x0a
+#define BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE        0x12
+#define BLE_HS_ADV_TYPE_SOL_UUIDS16             0x14
+#define BLE_HS_ADV_TYPE_SOL_UUIDS128            0x15
+#define BLE_HS_ADV_TYPE_SVC_DATA_UUID16         0x16
+#define BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR         0x17
+#define BLE_HS_ADV_TYPE_RANDOM_TGT_ADDR         0x18
+#define BLE_HS_ADV_TYPE_APPEARANCE              0x19
+#define BLE_HS_ADV_TYPE_ADV_ITVL                0x1a
+#define BLE_HS_ADV_TYPE_SVC_DATA_UUID32         0x20
+#define BLE_HS_ADV_TYPE_SVC_DATA_UUID128        0x21
+#define BLE_HS_ADV_TYPE_URI                     0x24
+#define BLE_HS_ADV_TYPE_MESH_PROV               0x29
+#define BLE_HS_ADV_TYPE_MESH_MESSAGE            0x2a
+#define BLE_HS_ADV_TYPE_MESH_BEACON             0x2b
+#define BLE_HS_ADV_TYPE_MFG_DATA                0xff
+
+#define BLE_HS_ADV_FLAGS_LEN                    1
+#define BLE_HS_ADV_F_DISC_LTD                   0x01
+#define BLE_HS_ADV_F_DISC_GEN                   0x02
+#define BLE_HS_ADV_F_BREDR_UNSUP                0x04
+
+#define BLE_HS_ADV_TX_PWR_LVL_LEN               1
+
+/**
+ * Set the tx_pwr_lvl field to this if you want the stack to fill in the tx
+ * power level field.
+ */
+#define BLE_HS_ADV_TX_PWR_LVL_AUTO              (-128)
+
+#define BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN         4
+
+#define BLE_HS_ADV_SVC_DATA_UUID16_MIN_LEN      2
+
+#define BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN    6
+
+#define BLE_HS_ADV_APPEARANCE_LEN               2
+
+#define BLE_HS_ADV_ADV_ITVL_LEN                 2
+
+#define BLE_HS_ADV_SVC_DATA_UUID32_MIN_LEN      4
+
+#define BLE_HS_ADV_SVC_DATA_UUID128_MIN_LEN     16
+
+int ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields,
+                               struct os_mbuf *om);
+
+int ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                          uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
+
+int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields,
+                            const uint8_t *src, uint8_t src_len);
+
+int ble_hs_adv_parse(const uint8_t *data, uint8_t length,
+                     ble_hs_adv_parse_func_t func, void *user_data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_hci.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_hci.h
new file mode 100644
index 00000000..e10b8e62
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_hci.h
@@ -0,0 +1,98 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_HCI_
+#define H_BLE_HS_HCI_
+
+/**
+ * @brief Bluetooth Host HCI utils
+ * @defgroup bt_host_hci Bluetooth Host HCI utils
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Queries the controller for the channel map used with the specified
+ * connection.  The channel map is represented as an array of five bytes, with
+ * each bit corresponding to an individual channel.  The array is interpreted
+ * as little-endian, such that:
+ *     map[0] & 0x01 --> Channel 0.
+ *     map[0] & 0x02 --> Channel 1.
+ *     ...
+ *     map[1] & 0x01 --> Channel 8.
+ *
+ * As there are 37 channels, only the first 37 bits get written.
+ *
+ * If a bit is 1, the corresponding channel is used.  Otherwise, the channel is
+ * unused.
+ *
+ * @param conn_handle           The handle of the connection whose channel map
+ *                                  is being read.
+ * @param out_chan_map          On success, the retrieved channel map gets
+ *                                  written here.  This buffer must have a size
+ *                                  >= 5 bytes.
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_hs_hci_read_chan_map(uint16_t conn_handle, uint8_t *out_chan_map);
+
+/**
+ * Instructs the controller to use the specified channel map.  The channel map
+ * is represented as an array of five bytes, with each bit corresponding to an
+ * individual channel.  The array is interpreted as little-endian, such that:
+ *     map[0] & 0x01 --> Channel 0.
+ *     map[0] & 0x02 --> Channel 1.
+ *     ...
+ *     map[1] & 0x01 --> Channel 8.
+ *
+ * As there are 37 channels, only the first 37 bits should be written are used.
+ *
+ * If a bit is 1, the corresponding channel can be used.  Otherwise, the
+ * channel should not be used.
+ *
+ * @param chan_map              The channel map to configure.  This buffer
+ *                                  should have a size of 5 bytes.
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int ble_hs_hci_set_chan_class(const uint8_t *chan_map);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_id.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_id.h
new file mode 100644
index 00000000..c96bd20f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_id.h
@@ -0,0 +1,132 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_ID_
+#define H_BLE_HS_ID_
+
+/**
+ * @brief Bluetooth Host Identity
+ * @defgroup bt_host_id Bluetooth Host Identity
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+#include "nimble/ble.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Generates a new random address.  This function does not configure the device
+ * with the new address; the caller can use the address in subsequent
+ * operations.
+ *
+ * @param nrpa                  The type of random address to generate:
+ *                                  0: static
+ *                                  1: non-resolvable private
+ * @param out_addr              On success, the generated address gets written
+ *                                  here.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int ble_hs_id_gen_rnd(int nrpa, ble_addr_t *out_addr);
+
+/**
+ * Sets the device's random address.  The address type (static vs.
+ * non-resolvable private) is inferred from the most-significant byte of the
+ * address.  The address is specified in host byte order (little-endian!).
+ *
+ * @param rnd_addr              The random address to set.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the specified address is not a
+ *                                  valid static random or non-resolvable
+ *                                  private address.
+ *                              Other nonzero on error.
+ */
+int ble_hs_id_set_rnd(const uint8_t *rnd_addr);
+
+/**
+ * Retrieves one of the device's identity addresses.  The device can have two
+ * identity addresses: one public and one random.  The id_addr_type argument
+ * specifies which of these two addresses to retrieve.
+ *
+ * @param id_addr_type          The type of identity address to retrieve.
+ *                                  Valid values are:
+ *                                      o BLE_ADDR_PUBLIC
+ *                                      o BLE_ADDR_RANDOM
+ * @param out_id_addr           On success, the requested identity address is
+ *                                  copied into this buffer.  The buffer must
+ *                                  be at least six bytes in size.  Pass NULL
+ *                                  if you do not require this information.
+ * @param out_is_nrpa           On success, the pointed-to value indicates
+ *                                  whether the retrieved address is a
+ *                                  non-resolvable private address.  Pass NULL
+ *                                  if you do not require this information.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if an invalid address type was
+ *                                  specified;
+ *                              BLE_HS_ENOADDR if the device does not have an
+ *                                  identity address of the requested type;
+ *                              Other BLE host core code on error.
+ */
+int ble_hs_id_copy_addr(uint8_t id_addr_type, uint8_t *out_id_addr,
+                        int *out_is_nrpa);
+
+/**
+ * Determines the best address type to use for automatic address type
+ * resolution.  Calculation of the best address type is done as follows:
+ *
+ * if privacy requested:
+ *     if we have a random static address:
+ *          --> RPA with static random ID
+ *     else
+ *          --> RPA with public ID
+ *     end
+ * else
+ *     if we have a random static address:
+ *          --> random static address
+ *     else
+ *          --> public address
+ *     end
+ * end
+ *
+ * @param privacy               (0/1) Whether to use a private address.
+ * @param out_addr_type         On success, the "own addr type" code gets
+ *                                  written here.
+ *
+ * @return                      0 if an address type was successfully inferred.
+ *                              BLE_HS_ENOADDR if the device does not have a
+ *                                  suitable address.
+ *                              Other BLE host core code on error.
+ */
+int ble_hs_id_infer_auto(int privacy, uint8_t *out_addr_type);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_log.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_log.h
new file mode 100644
index 00000000..8d0a4596
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_log.h
@@ -0,0 +1,52 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_LOG_
+#define H_BLE_HS_LOG_
+
+#include "modlog/modlog.h"
+
+/* Only include the logcfg header if this version of newt can generate it. */
+#if MYNEWT_VAL(NEWT_FEATURE_LOGCFG)
+#include "logcfg/logcfg.h"
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+#define BLE_HS_LOG(lvl, ...) \
+    BLE_HS_LOG_ ## lvl(__VA_ARGS__)
+
+#define BLE_HS_LOG_ADDR(lvl, addr)                      \
+    BLE_HS_LOG_ ## lvl("%02x:%02x:%02x:%02x:%02x:%02x", \
+                       (addr)[5], (addr)[4], (addr)[3], \
+                       (addr)[2], (addr)[1], (addr)[0])
+
+
+void ble_hs_log_mbuf(const struct os_mbuf *om);
+void ble_hs_log_flat_buf(const void *data, int len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_mbuf.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_mbuf.h
new file mode 100644
index 00000000..a3c2c029
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_mbuf.h
@@ -0,0 +1,82 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_MBUF_
+#define H_BLE_HS_MBUF_
+
+/**
+ * @brief Bluetooth Host chained memory buffer (mbuf)
+ * @defgroup bt_host_mbuf Bluetooth Host chained memory buffer (mbuf)
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+/**
+ * Allocates an mbuf suitable for an ATT command packet.  The resulting packet
+ * has sufficient leading space for:
+ *  - ACL data header
+ *  - L2CAP B-frame header
+ *  - Largest ATT command base (prepare write request / response).
+ *
+ * @return An empty mbuf on success, NULL on error.
+ */
+struct os_mbuf *ble_hs_mbuf_att_pkt(void);
+
+/**
+ * Allocates an mbuf and fills it with the contents of the specified flat
+ * buffer.
+ *
+ * @param buf The flat buffer to copy from.
+ * @param len The length of the flat buffer.
+ *
+ * @return A newly-allocated mbuf on success, NULL on error.
+ */
+struct os_mbuf *ble_hs_mbuf_from_flat(const void *buf, uint16_t len);
+
+/**
+ * Copies the contents of an mbuf into the specified flat buffer.  If the flat
+ * buffer is too small to contain the mbuf's contents, it is filled to capacity
+ * and BLE_HS_EMSGSIZE is returned.
+ *
+ * @param om            The mbuf to copy from.
+ * @param flat          The destination flat buffer.
+ * @param max_len       The size of the flat buffer.
+ * @param out_copy_len  The number of bytes actually copied gets written here.
+ *
+ * @return               0 on success or BLE host core return code on error.
+ */
+int ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len,
+                        uint16_t *out_copy_len);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_stop.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_stop.h
new file mode 100644
index 00000000..d16c9c27
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_hs_stop.h
@@ -0,0 +1,70 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_STOP_
+#define H_BLE_HS_STOP_
+
+/** @typedef ble_hs_stop_fn
+ * @brief Callback function; reports the result of a host stop procedure.
+ *
+ * @param status                The result of the host stop procedure.  One of
+ *                                  the HAL_RESET_[...] codes or an
+ *                                  implementation-defined value.
+ * @param arg                   Optional argument specified when the stop
+ *                                  procedure was initiated.
+ *
+ */
+typedef void ble_hs_stop_fn(int status, void *arg);
+
+/**
+ * @brief Used to report the result of a stop procedure.
+ *
+ * This should be used as an opaque structure and not modified manually.
+ */
+struct ble_hs_stop_listener {
+    ble_hs_stop_fn *fn;
+    void *arg;
+    SLIST_ENTRY(ble_hs_stop_listener) link;
+};
+
+/**
+ * @brief Stops the BLE host.
+ *
+ * Aborts all active GAP procedures and terminates all open connections.
+ * Connection termination is performed asynchronously, so this function's
+ * result is reported via the provided listener.
+ *
+ * @param listener              A listener to populate.  This object's initial
+ *                                  value doesn't matter, but its lifetime must
+ *                                  extend until the stop procedure completes.
+ * @param fn                    The callback to execute when the stop procedure
+ *                                  completes.
+ * @param arg                   Optional argument to pass to the callback.
+ *
+ * @return                      0: Stop procedure successfully initiated.
+ *                              BLE_HS_EBUSY: Stop procedure already in
+ *                                  progress; the provided callback gets called
+ *                                  when the procedure completes.
+ *                              BLE_HS_EALREADY: Host already stopped; the
+ *                                  provided callback does *not* get called.
+ */
+int ble_hs_stop(struct ble_hs_stop_listener *listener, 
+                ble_hs_stop_fn *fn, void *arg);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_ibeacon.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_ibeacon.h
new file mode 100644
index 00000000..fff7c57a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_ibeacon.h
@@ -0,0 +1,34 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_IBEACON_
+#define H_BLE_IBEACON_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_ibeacon_set_adv_data(void *uuid128, uint16_t major,
+                             uint16_t minor, int8_t measured_power);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_l2cap.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_l2cap.h
new file mode 100644
index 00000000..aef9682c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_l2cap.h
@@ -0,0 +1,266 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_L2CAP_
+#define H_BLE_L2CAP_
+
+#include "nimble/nimble_opt.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_l2cap_sig_update_req;
+struct ble_hs_conn;
+
+#define BLE_L2CAP_CID_ATT           4
+#define BLE_L2CAP_CID_SIG           5
+#define BLE_L2CAP_CID_SM            6
+
+#define BLE_L2CAP_SIG_OP_REJECT                 0x01
+#define BLE_L2CAP_SIG_OP_CONNECT_REQ            0x02
+#define BLE_L2CAP_SIG_OP_CONNECT_RSP            0x03
+#define BLE_L2CAP_SIG_OP_CONFIG_REQ             0x04
+#define BLE_L2CAP_SIG_OP_CONFIG_RSP             0x05
+#define BLE_L2CAP_SIG_OP_DISCONN_REQ            0x06
+#define BLE_L2CAP_SIG_OP_DISCONN_RSP            0x07
+#define BLE_L2CAP_SIG_OP_ECHO_REQ               0x08
+#define BLE_L2CAP_SIG_OP_ECHO_RSP               0x09
+#define BLE_L2CAP_SIG_OP_INFO_REQ               0x0a
+#define BLE_L2CAP_SIG_OP_INFO_RSP               0x0b
+#define BLE_L2CAP_SIG_OP_CREATE_CHAN_REQ        0x0c
+#define BLE_L2CAP_SIG_OP_CREATE_CHAN_RSP        0x0d
+#define BLE_L2CAP_SIG_OP_MOVE_CHAN_REQ          0x0e
+#define BLE_L2CAP_SIG_OP_MOVE_CHAN_RSP          0x0f
+#define BLE_L2CAP_SIG_OP_MOVE_CHAN_CONF_REQ     0x10
+#define BLE_L2CAP_SIG_OP_MOVE_CHAN_CONF_RSP     0x11
+#define BLE_L2CAP_SIG_OP_UPDATE_REQ             0x12
+#define BLE_L2CAP_SIG_OP_UPDATE_RSP             0x13
+#define BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ  0x14
+#define BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_RSP  0x15
+#define BLE_L2CAP_SIG_OP_FLOW_CTRL_CREDIT       0x16
+#define BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ     0x17
+#define BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP     0x18
+#define BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_REQ    0x19
+#define BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_RSP    0x1A
+#define BLE_L2CAP_SIG_OP_MAX                    0x1B
+
+#define BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD    0x0000
+#define BLE_L2CAP_SIG_ERR_MTU_EXCEEDED          0x0001
+#define BLE_L2CAP_SIG_ERR_INVALID_CID           0x0002
+
+#define BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS        0x0000
+#define BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM            0x0002
+#define BLE_L2CAP_COC_ERR_NO_RESOURCES              0x0004
+#define BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN       0x0005
+#define BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR       0x0006
+#define BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ       0x0007
+#define BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC          0x0008
+#define BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID        0x0009
+#define BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED   0x000A
+#define BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS   0x000B
+#define BLE_L2CAP_COC_ERR_INVALID_PARAMETERS        0x000C
+
+#define BLE_L2CAP_ERR_RECONFIG_SUCCEED                       0x0000
+#define BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED     0x0001
+#define BLE_L2CAP_ERR_RECONFIG_REDUCTION_MPS_NOT_ALLOWED     0x0002
+#define BLE_L2CAP_ERR_RECONFIG_INVALID_DCID                  0x0003
+#define BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM              0x0004
+
+#define BLE_L2CAP_EVENT_COC_CONNECTED                 0
+#define BLE_L2CAP_EVENT_COC_DISCONNECTED              1
+#define BLE_L2CAP_EVENT_COC_ACCEPT                    2
+#define BLE_L2CAP_EVENT_COC_DATA_RECEIVED             3
+#define BLE_L2CAP_EVENT_COC_TX_UNSTALLED              4
+#define BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED        5
+#define BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED         6
+
+typedef void ble_l2cap_sig_update_fn(uint16_t conn_handle, int status,
+                                     void *arg);
+
+struct ble_l2cap_sig_update_params {
+    uint16_t itvl_min;
+    uint16_t itvl_max;
+    uint16_t slave_latency;
+    uint16_t timeout_multiplier;
+};
+
+int ble_l2cap_sig_update(uint16_t conn_handle,
+                         struct ble_l2cap_sig_update_params *params,
+                         ble_l2cap_sig_update_fn *cb, void *cb_arg);
+
+struct ble_l2cap_chan;
+
+/**
+ * Represents a L2CAP-related event.
+ * When such an event occurs, the host notifies the application by passing an
+ * instance of this structure to an application-specified callback.
+ */
+struct ble_l2cap_event {
+    /**
+     * Indicates the type of L2CAP event that occurred.  This is one of the
+     * BLE_L2CAP_EVENT codes.
+     */
+    uint8_t type;
+
+    /**
+     * A discriminated union containing additional details concerning the L2CAP
+     * event.  The 'type' field indicates which member of the union is valid.
+     */
+    union {
+        /**
+         * Represents a connection attempt. Valid for the following event
+         * types:
+         *     o BLE_L2CAP_EVENT_COC_CONNECTED */
+        struct {
+            /**
+             * The status of the connection attempt;
+             *     o 0: the connection was successfully established.
+             *     o BLE host error code: the connection attempt failed for
+             *       the specified reason.
+             */
+            int status;
+
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+        } connect;
+
+        /**
+         * Represents a terminated connection. Valid for the following event
+         * types:
+         *     o BLE_L2CAP_EVENT_COC_DISCONNECTED
+         */
+        struct {
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** Information about the L2CAP connection prior to termination. */
+            struct ble_l2cap_chan *chan;
+        } disconnect;
+
+        /**
+         * Represents connection accept. Valid for the following event
+         * types:
+         *     o BLE_L2CAP_EVENT_COC_ACCEPT
+         */
+        struct {
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** MTU supported by peer device on the channel */
+            uint16_t peer_sdu_size;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+        } accept;
+
+        /**
+         * Represents received data. Valid for the following event
+         * types:
+         *     o BLE_L2CAP_EVENT_COC_DATA_RECEIVED
+         */
+        struct {
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+
+            /** The mbuf with received SDU. */
+            struct os_mbuf *sdu_rx;
+        } receive;
+
+        /**
+         * Represents tx_unstalled data. Valid for the following event
+         * types:
+         *     o BLE_L2CAP_EVENT_COC_TX_UNSTALLED
+         */
+        struct {
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+
+            /**
+             * The status of the send attempt which was stalled due to
+             * lack of credits; This can be non zero only if there
+             * is an issue with memory allocation for following SDU fragments.
+             * In such a case last SDU has been partially sent to peer device
+             * and it is up to application to decide how to handle it.
+             */
+            int status;
+        } tx_unstalled;
+
+        /**
+         * Represents reconfiguration done. Valid for the following event
+         * types:
+         *      o BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED
+         *      o BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED
+         */
+        struct {
+            /**
+             * The status of the reconfiguration attempt;
+             *     o 0: the reconfiguration was successfully done.
+             *     o BLE host error code: the reconfiguration attempt failed for
+             *       the specified reason.
+             */
+            int status;
+
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+        } reconfigured;
+    };
+};
+
+struct ble_l2cap_chan_info {
+    uint16_t scid;
+    uint16_t dcid;
+    uint16_t our_l2cap_mtu;
+    uint16_t peer_l2cap_mtu;
+    uint16_t psm;
+    uint16_t our_coc_mtu;
+    uint16_t peer_coc_mtu;
+};
+
+typedef int ble_l2cap_event_fn(struct ble_l2cap_event *event, void *arg);
+
+
+uint16_t ble_l2cap_get_conn_handle(struct ble_l2cap_chan *chan);
+int ble_l2cap_create_server(uint16_t psm, uint16_t mtu,
+                            ble_l2cap_event_fn *cb, void *cb_arg);
+
+int ble_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
+                      struct os_mbuf *sdu_rx,
+                      ble_l2cap_event_fn *cb, void *cb_arg);
+int ble_l2cap_disconnect(struct ble_l2cap_chan *chan);
+int ble_l2cap_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx);
+int ble_l2cap_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx);
+int ble_l2cap_get_chan_info(struct ble_l2cap_chan *chan, struct ble_l2cap_chan_info *chan_info);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_monitor.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_monitor.h
new file mode 100644
index 00000000..61722f7d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_monitor.h
@@ -0,0 +1,40 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_MONITOR_
+#define H_BLE_MONITOR_
+
+#include <syscfg/syscfg.h>
+
+#undef BLE_MONITOR
+#define BLE_MONITOR (MYNEWT_VAL(BLE_MONITOR_UART) || MYNEWT_VAL(BLE_MONITOR_RTT))
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_monitor_log(int level, const char *fmt, ...);
+
+int ble_monitor_out(int c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_sm.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_sm.h
new file mode 100644
index 00000000..ceebb856
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_sm.h
@@ -0,0 +1,124 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SM_
+#define H_BLE_SM_
+
+#include <inttypes.h>
+#include "syscfg/syscfg.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_SM_ERR_PASSKEY                      0x01
+#define BLE_SM_ERR_OOB                          0x02
+#define BLE_SM_ERR_AUTHREQ                      0x03
+#define BLE_SM_ERR_CONFIRM_MISMATCH             0x04
+#define BLE_SM_ERR_PAIR_NOT_SUPP                0x05
+#define BLE_SM_ERR_ENC_KEY_SZ                   0x06
+#define BLE_SM_ERR_CMD_NOT_SUPP                 0x07
+#define BLE_SM_ERR_UNSPECIFIED                  0x08
+#define BLE_SM_ERR_REPEATED                     0x09
+#define BLE_SM_ERR_INVAL                        0x0a
+#define BLE_SM_ERR_DHKEY                        0x0b
+#define BLE_SM_ERR_NUMCMP                       0x0c
+#define BLE_SM_ERR_ALREADY                      0x0d
+#define BLE_SM_ERR_CROSS_TRANS                  0x0e
+#define BLE_SM_ERR_MAX_PLUS_1                   0x0f
+
+#define BLE_SM_PAIR_ALG_JW                      0
+#define BLE_SM_PAIR_ALG_PASSKEY                 1
+#define BLE_SM_PAIR_ALG_OOB                     2
+#define BLE_SM_PAIR_ALG_NUMCMP                  3
+
+#define BLE_SM_PAIR_KEY_DIST_ENC                0x01
+#define BLE_SM_PAIR_KEY_DIST_ID                 0x02
+#define BLE_SM_PAIR_KEY_DIST_SIGN               0x04
+#define BLE_SM_PAIR_KEY_DIST_LINK               0x08
+#define BLE_SM_PAIR_KEY_DIST_RESERVED           0xf0
+
+#define BLE_SM_IO_CAP_DISP_ONLY                 0x00
+#define BLE_SM_IO_CAP_DISP_YES_NO               0x01
+#define BLE_SM_IO_CAP_KEYBOARD_ONLY             0x02
+#define BLE_SM_IO_CAP_NO_IO                     0x03
+#define BLE_SM_IO_CAP_KEYBOARD_DISP             0x04
+#define BLE_SM_IO_CAP_RESERVED                  0x05
+
+#define BLE_SM_PAIR_OOB_NO                      0x00
+#define BLE_SM_PAIR_OOB_YES                     0x01
+#define BLE_SM_PAIR_OOB_RESERVED                0x02
+
+#define BLE_SM_PAIR_AUTHREQ_BOND                0x01
+#define BLE_SM_PAIR_AUTHREQ_MITM                0x04
+#define BLE_SM_PAIR_AUTHREQ_SC                  0x08
+#define BLE_SM_PAIR_AUTHREQ_KEYPRESS            0x10
+#define BLE_SM_PAIR_AUTHREQ_RESERVED            0xe2
+
+#define BLE_SM_PAIR_KEY_SZ_MIN                  7
+#define BLE_SM_PAIR_KEY_SZ_MAX                  16
+
+/*
+ * The security manager asks the application to perform a key generation
+ * action.  The application passes the passkey back to SM via
+ * ble_sm_inject_io().
+ */
+#define BLE_SM_IOACT_NONE                       0
+#define BLE_SM_IOACT_OOB                        1
+#define BLE_SM_IOACT_INPUT                      2
+#define BLE_SM_IOACT_DISP                       3
+#define BLE_SM_IOACT_NUMCMP                     4
+#define BLE_SM_IOACT_OOB_SC                     5
+#define BLE_SM_IOACT_MAX_PLUS_ONE               6
+
+struct ble_sm_sc_oob_data {
+    /** Random Number. */
+    uint8_t r[16];
+
+    /** Confirm Value. */
+    uint8_t c[16];
+};
+
+struct ble_sm_io {
+    uint8_t action;
+    union {
+        uint32_t passkey;
+        uint8_t  oob[16];
+        uint8_t  numcmp_accept;
+        struct {
+            struct ble_sm_sc_oob_data *local;
+            struct ble_sm_sc_oob_data *remote;
+        } oob_sc_data;
+    };
+};
+
+int ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data);
+
+#if NIMBLE_BLE_SM
+int ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey);
+#else
+#define ble_sm_inject_io(conn_handle, pkey) \
+    ((void)(conn_handle), BLE_HS_ENOTSUP)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_store.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_store.h
new file mode 100644
index 00000000..30a5666c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_store.h
@@ -0,0 +1,303 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_STORE_
+#define H_BLE_STORE_
+
+#include <inttypes.h>
+#include "nimble/ble.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_STORE_OBJ_TYPE_OUR_SEC      1
+#define BLE_STORE_OBJ_TYPE_PEER_SEC     2
+#define BLE_STORE_OBJ_TYPE_CCCD         3
+
+/** Failed to persist record; insufficient storage capacity. */
+#define BLE_STORE_EVENT_OVERFLOW        1
+
+/** About to execute a procedure that may fail due to overflow. */
+#define BLE_STORE_EVENT_FULL            2
+
+/**
+ * Used as a key for lookups of security material.  This struct corresponds to
+ * the following store object types:
+ *     o BLE_STORE_OBJ_TYPE_OUR_SEC
+ *     o BLE_STORE_OBJ_TYPE_PEER_SEC
+ */
+struct ble_store_key_sec {
+    /**
+     * Key by peer identity address;
+     * peer_addr=BLE_ADDR_NONE means don't key off peer.
+     */
+    ble_addr_t peer_addr;
+
+    /** Key by ediv; ediv_rand_present=0 means don't key off ediv. */
+    uint16_t ediv;
+
+    /** Key by rand_num; ediv_rand_present=0 means don't key off rand_num. */
+    uint64_t rand_num;
+
+    unsigned ediv_rand_present:1;
+
+    /** Number of results to skip; 0 means retrieve the first match. */
+    uint8_t idx;
+};
+
+/**
+ * Represents stored security material.  This struct corresponds to the
+ * following store object types:
+ *     o BLE_STORE_OBJ_TYPE_OUR_SEC
+ *     o BLE_STORE_OBJ_TYPE_PEER_SEC
+ */
+struct ble_store_value_sec {
+    ble_addr_t peer_addr;
+
+    uint8_t key_size;
+    uint16_t ediv;
+    uint64_t rand_num;
+    uint8_t ltk[16];
+    uint8_t ltk_present:1;
+
+    uint8_t irk[16];
+    uint8_t irk_present:1;
+
+    uint8_t csrk[16];
+    uint8_t csrk_present:1;
+
+    unsigned authenticated:1;
+    uint8_t sc:1;
+};
+
+/**
+ * Used as a key for lookups of stored client characteristic configuration
+ * descriptors (CCCDs).  This struct corresponds to the BLE_STORE_OBJ_TYPE_CCCD
+ * store object type.
+ */
+struct ble_store_key_cccd {
+    /**
+     * Key by peer identity address;
+     * peer_addr=BLE_ADDR_NONE means don't key off peer.
+     */
+    ble_addr_t peer_addr;
+
+    /**
+     * Key by characteristic value handle;
+     * chr_val_handle=0 means don't key off characteristic handle.
+     */
+    uint16_t chr_val_handle;
+
+    /** Number of results to skip; 0 means retrieve the first match. */
+    uint8_t idx;
+};
+
+/**
+ * Represents a stored client characteristic configuration descriptor (CCCD).
+ * This struct corresponds to the BLE_STORE_OBJ_TYPE_CCCD store object type.
+ */
+struct ble_store_value_cccd {
+    ble_addr_t peer_addr;
+    uint16_t chr_val_handle;
+    uint16_t flags;
+    unsigned value_changed:1;
+};
+
+/**
+ * Used as a key for store lookups.  This union must be accompanied by an
+ * object type code to indicate which field is valid.
+ */
+union ble_store_key {
+    struct ble_store_key_sec sec;
+    struct ble_store_key_cccd cccd;
+};
+
+/**
+ * Represents stored data.  This union must be accompanied by an object type
+ * code to indicate which field is valid.
+ */
+union ble_store_value {
+    struct ble_store_value_sec sec;
+    struct ble_store_value_cccd cccd;
+};
+
+struct ble_store_status_event {
+    /**
+     * The type of event being reported; one of the BLE_STORE_EVENT_TYPE_[...]
+     * codes.
+     */
+    int event_code;
+
+    /**
+     * Additional data related to the event; the valid field is inferred from
+     * the obj_type,event_code pair.
+     */
+    union {
+        /**
+         * Represents a write that failed due to storage exhaustion.  Valid for
+         * the following event types:
+         *     o BLE_STORE_EVENT_OVERFLOW
+         */
+        struct {
+            /** The type of object that failed to be written. */
+            int obj_type;
+
+            /** The object that failed to be written. */
+            const union ble_store_value *value;
+        } overflow;
+
+        /**
+         * Represents the possibility that a scheduled write will fail due to
+         * storage exhaustion.  Valid for the following event types:
+         *     o BLE_STORE_EVENT_FULL
+         */
+        struct {
+            /** The type of object that may fail to be written. */
+            int obj_type;
+
+            /** The handle of the connection which prompted the write. */
+            uint16_t conn_handle;
+        } full;
+    };
+};
+
+/**
+ * Searches the store for an object matching the specified criteria.  If a
+ * match is found, it is read from the store and the dst parameter is populated
+ * with the retrieved object.
+ *
+ * @param obj_type              The type of object to search for; one of the
+ *                                  BLE_STORE_OBJ_TYPE_[...] codes.
+ * @param key                   Specifies properties of the object to search
+ *                                  for.  An object is retrieved if it matches
+ *                                  these criteria.
+ * @param dst                   On success, this is populated with the
+ *                                  retrieved object.
+ *
+ * @return                      0 if an object was successfully retreived;
+ *                              BLE_HS_ENOENT if no matching object was found;
+ *                              Other nonzero on error.
+ */
+typedef int ble_store_read_fn(int obj_type, const union ble_store_key *key,
+                              union ble_store_value *dst);
+
+/**
+ * Writes the specified object to the store.  If an object with the same
+ * identity is already in the store, it is replaced.  If the store lacks
+ * sufficient capacity to write the object, this function may remove previously
+ * stored values to make room.
+ *
+ * @param obj_type              The type of object being written; one of the
+ *                                  BLE_STORE_OBJ_TYPE_[...] codes.
+ * @param val                   The object to persist.
+ *
+ * @return                      0 if the object was successfully written;
+ *                              Other nonzero on error.
+ */
+typedef int ble_store_write_fn(int obj_type, const union ble_store_value *val);
+
+/**
+ * Searches the store for the first object matching the specified criteria.  If
+ * a match is found, it is deleted from the store.
+ *
+ * @param obj_type              The type of object to delete; one of the
+ *                                  BLE_STORE_OBJ_TYPE_[...] codes.
+ * @param key                   Specifies properties of the object to search
+ *                                  for.  An object is deleted if it matches
+ *                                  these criteria.
+ * @return                      0 if an object was successfully retrieved;
+ *                              BLE_HS_ENOENT if no matching object was found;
+ *                              Other nonzero on error.
+ */
+typedef int ble_store_delete_fn(int obj_type, const union ble_store_key *key);
+
+/**
+ * Indicates an inability to perform a store operation.  This callback should
+ * do one of two things:
+ *     o Address the problem and return 0, indicating that the store operation
+ *       should proceed.
+ *     o Return nonzero to indicate that the store operation should be aborted.
+ *
+ * @param event                 Describes the store event being reported.
+ * @param arg                   Optional user argument.
+ *
+ * @return                      0 if the store operation should proceed;
+ *                              nonzero if the store operation should be
+ *                                  aborted.
+ */
+typedef int ble_store_status_fn(struct ble_store_status_event *event,
+                                void *arg);
+
+int ble_store_read(int obj_type, const union ble_store_key *key,
+                   union ble_store_value *val);
+int ble_store_write(int obj_type, const union ble_store_value *val);
+int ble_store_delete(int obj_type, const union ble_store_key *key);
+int ble_store_overflow_event(int obj_type, const union ble_store_value *value);
+int ble_store_full_event(int obj_type, uint16_t conn_handle);
+
+int ble_store_read_our_sec(const struct ble_store_key_sec *key_sec,
+                           struct ble_store_value_sec *value_sec);
+int ble_store_write_our_sec(const struct ble_store_value_sec *value_sec);
+int ble_store_delete_our_sec(const struct ble_store_key_sec *key_sec);
+int ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec,
+                            struct ble_store_value_sec *value_sec);
+int ble_store_write_peer_sec(const struct ble_store_value_sec *value_sec);
+int ble_store_delete_peer_sec(const struct ble_store_key_sec *key_sec);
+
+int ble_store_read_cccd(const struct ble_store_key_cccd *key,
+                        struct ble_store_value_cccd *out_value);
+int ble_store_write_cccd(const struct ble_store_value_cccd *value);
+int ble_store_delete_cccd(const struct ble_store_key_cccd *key);
+
+void ble_store_key_from_value_sec(struct ble_store_key_sec *out_key,
+                                  const struct ble_store_value_sec *value);
+void ble_store_key_from_value_cccd(struct ble_store_key_cccd *out_key,
+                                   const struct ble_store_value_cccd *value);
+
+void ble_store_key_from_value(int obj_type,
+                              union ble_store_key *out_key,
+                              const union ble_store_value *value);
+
+typedef int ble_store_iterator_fn(int obj_type,
+                                  union ble_store_value *val,
+                                  void *cookie);
+
+int ble_store_iterate(int obj_type,
+                      ble_store_iterator_fn *callback,
+                      void *cookie);
+
+int ble_store_clear(void);
+
+/*** Utility functions. */
+
+int ble_store_util_bonded_peers(ble_addr_t *out_peer_id_addrs,
+                                int *out_num_peers,
+                                int max_peers);
+int ble_store_util_delete_all(int type, const union ble_store_key *key);
+int ble_store_util_delete_peer(const ble_addr_t *peer_id_addr);
+int ble_store_util_delete_oldest_peer(void);
+int ble_store_util_count(int type, int *out_count);
+int ble_store_util_status_rr(struct ble_store_status_event *event, void *arg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/include/host/ble_uuid.h b/src/libs/mynewt-nimble/nimble/host/include/host/ble_uuid.h
new file mode 100644
index 00000000..d3576c59
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/include/host/ble_uuid.h
@@ -0,0 +1,182 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_UUID_
+#define H_BLE_UUID_
+
+/**
+ * @brief Bluetooth UUID
+ * @defgroup bt_uuid Bluetooth UUID
+ * @ingroup bt_host
+ * @{
+ */
+
+#include <inttypes.h>
+#include <stddef.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+/** Type of UUID */
+enum {
+    /** 16-bit UUID (BT SIG assigned) */
+    BLE_UUID_TYPE_16 = 16,
+
+    /** 32-bit UUID (BT SIG assigned) */
+    BLE_UUID_TYPE_32 = 32,
+
+    /** 128-bit UUID */
+    BLE_UUID_TYPE_128 = 128,
+};
+
+/** Generic UUID type, to be used only as a pointer */
+typedef struct {
+    /** Type of the UUID */
+    uint8_t type;
+} ble_uuid_t;
+
+/** 16-bit UUID */
+typedef struct {
+    ble_uuid_t u;
+    uint16_t value;
+} ble_uuid16_t;
+
+/** 32-bit UUID */
+typedef struct {
+    ble_uuid_t u;
+    uint32_t value;
+} ble_uuid32_t;
+
+/** 128-bit UUID */
+typedef struct {
+    ble_uuid_t u;
+    uint8_t value[16];
+} ble_uuid128_t;
+
+/** Universal UUID type, to be used for any-UUID static allocation */
+typedef union {
+    ble_uuid_t u;
+    ble_uuid16_t u16;
+    ble_uuid32_t u32;
+    ble_uuid128_t u128;
+} ble_uuid_any_t;
+
+#define BLE_UUID16_INIT(uuid16)         \
+    {                                   \
+        .u.type = BLE_UUID_TYPE_16,     \
+        .value = (uuid16),              \
+    }
+
+#define BLE_UUID32_INIT(uuid32)         \
+    {                                   \
+        .u.type = BLE_UUID_TYPE_32,     \
+        .value = (uuid32),              \
+    }
+
+#define BLE_UUID128_INIT(uuid128...)    \
+    {                                   \
+        .u.type = BLE_UUID_TYPE_128,    \
+        .value = { uuid128 },           \
+    }
+
+#define BLE_UUID16_DECLARE(uuid16) \
+    ((ble_uuid_t *) (&(ble_uuid16_t) BLE_UUID16_INIT(uuid16)))
+
+#define BLE_UUID32_DECLARE(uuid32) \
+    ((ble_uuid_t *) (&(ble_uuid32_t) BLE_UUID32_INIT(uuid32)))
+
+#define BLE_UUID128_DECLARE(uuid128...) \
+    ((ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT(uuid128)))
+
+#define BLE_UUID16(u) \
+    ((ble_uuid16_t *) (u))
+
+#define BLE_UUID32(u) \
+    ((ble_uuid32_t *) (u))
+
+#define BLE_UUID128(u) \
+    ((ble_uuid128_t *) (u))
+
+/** Size of buffer needed to store UUID as a string.
+ *  Includes trailing \0.
+ */
+#define BLE_UUID_STR_LEN (37)
+
+/** @brief Constructs a UUID object from a byte array.
+ *
+ * @param uuid  On success, this gets populated with the constructed UUID.
+ * @param buf   The source buffer to parse.
+ * @param len   The size of the buffer, in bytes.
+ *
+ * @return      0 on success, BLE_HS_EINVAL if the source buffer does not contain
+ *              a valid UUID.
+ */
+int ble_uuid_init_from_buf(ble_uuid_any_t *uuid, const void *buf, size_t len);
+
+/** @brief Compares two Bluetooth UUIDs.
+ *
+ * @param uuid1  The first UUID to compare.
+ * @param uuid2  The second UUID to compare.
+ *
+ * @return       0 if the two UUIDs are equal, nonzero if the UUIDs differ.
+ */
+int ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2);
+
+/** @brief Copy Bluetooth UUID
+ *
+ * @param dst    Destination UUID.
+ * @param src    Source UUID.
+ */
+void ble_uuid_copy(ble_uuid_any_t *dst, const ble_uuid_t *src);
+
+/** @brief Converts the specified UUID to its string representation.
+ *
+ * Example string representations:
+ *     o 16-bit:  0x1234
+ *     o 32-bit:  0x12345678
+ *     o 128-bit: 12345678-1234-1234-1234-123456789abc
+ *
+ * @param uuid   The source UUID to convert.
+ * @param dst    The destination buffer.
+ *
+ * @return       A pointer to the supplied destination buffer.
+ */
+char *ble_uuid_to_str(const ble_uuid_t *uuid, char *dst);
+
+/** @brief Converts the specified 16-bit UUID to a uint16_t.
+ *
+ * @param uuid   The source UUID to convert.
+ *
+ * @return       The converted integer on success, NULL if the specified UUID is
+ *               not 16 bits.
+ */
+uint16_t ble_uuid_u16(const ble_uuid_t *uuid);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* _BLE_HOST_UUID_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/access.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/access.h
new file mode 100644
index 00000000..1f99f412
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/access.h
@@ -0,0 +1,656 @@
+/** @file
+ *  @brief Bluetooth Mesh Access Layer APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_ACCESS_H
+#define __BT_MESH_ACCESS_H
+
+/**
+ * @brief Bluetooth Mesh Access Layer
+ * @defgroup bt_mesh_access Bluetooth Mesh Access Layer
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BT_MESH_ADDR_UNASSIGNED   0x0000
+#define BT_MESH_ADDR_ALL_NODES    0xffff
+#define BT_MESH_ADDR_PROXIES      0xfffc
+#define BT_MESH_ADDR_FRIENDS      0xfffd
+#define BT_MESH_ADDR_RELAYS       0xfffe
+
+#define BT_MESH_KEY_UNUSED        0xffff
+#define BT_MESH_KEY_DEV           0xfffe
+#define BT_MESH_KEY_DEV_LOCAL     BT_MESH_KEY_DEV
+#define BT_MESH_KEY_DEV_REMOTE    0xfffd
+#define BT_MESH_KEY_DEV_ANY       0xfffc
+
+#define BT_MESH_IS_DEV_KEY(key) (key == BT_MESH_KEY_DEV_LOCAL || \
+				 key == BT_MESH_KEY_DEV_REMOTE)
+
+/** Helper to define a mesh element within an array.
+ *
+ *  In case the element has no SIG or Vendor models the helper
+ *  macro BT_MESH_MODEL_NONE can be given instead.
+ *
+ *  @param _loc       Location Descriptor.
+ *  @param _mods      Array of models.
+ *  @param _vnd_mods  Array of vendor models.
+ */
+#define BT_MESH_ELEM(_loc, _mods, _vnd_mods)        \
+{                                                   \
+	.loc              = (_loc),                 \
+	.model_count      = ARRAY_SIZE(_mods),      \
+	.models           = (_mods),                \
+	.vnd_model_count  = ARRAY_SIZE(_vnd_mods),  \
+	.vnd_models       = (_vnd_mods),            \
+}
+
+/** Abstraction that describes a Mesh Element */
+struct bt_mesh_elem {
+	/* Unicast Address. Set at runtime during provisioning. */
+	u16_t addr;
+
+	/* Location Descriptor (GATT Bluetooth Namespace Descriptors) */
+	const u16_t loc;
+
+	const u8_t model_count;
+	const u8_t vnd_model_count;
+
+	struct bt_mesh_model * const models;
+	struct bt_mesh_model * const vnd_models;
+};
+
+/* Foundation Models */
+#define BT_MESH_MODEL_ID_CFG_SRV                   0x0000
+#define BT_MESH_MODEL_ID_CFG_CLI                   0x0001
+#define BT_MESH_MODEL_ID_HEALTH_SRV                0x0002
+#define BT_MESH_MODEL_ID_HEALTH_CLI                0x0003
+
+/* Models from the Mesh Model Specification */
+#define BT_MESH_MODEL_ID_GEN_ONOFF_SRV             0x1000
+#define BT_MESH_MODEL_ID_GEN_ONOFF_CLI             0x1001
+#define BT_MESH_MODEL_ID_GEN_LEVEL_SRV             0x1002
+#define BT_MESH_MODEL_ID_GEN_LEVEL_CLI             0x1003
+#define BT_MESH_MODEL_ID_GEN_DEF_TRANS_TIME_SRV    0x1004
+#define BT_MESH_MODEL_ID_GEN_DEF_TRANS_TIME_CLI    0x1005
+#define BT_MESH_MODEL_ID_GEN_POWER_ONOFF_SRV       0x1006
+#define BT_MESH_MODEL_ID_GEN_POWER_ONOFF_SETUP_SRV 0x1007
+#define BT_MESH_MODEL_ID_GEN_POWER_ONOFF_CLI       0x1008
+#define BT_MESH_MODEL_ID_GEN_POWER_LEVEL_SRV       0x1009
+#define BT_MESH_MODEL_ID_GEN_POWER_LEVEL_SETUP_SRV 0x100a
+#define BT_MESH_MODEL_ID_GEN_POWER_LEVEL_CLI       0x100b
+#define BT_MESH_MODEL_ID_GEN_BATTERY_SRV           0x100c
+#define BT_MESH_MODEL_ID_GEN_BATTERY_CLI           0x100d
+#define BT_MESH_MODEL_ID_GEN_LOCATION_SRV          0x100e
+#define BT_MESH_MODEL_ID_GEN_LOCATION_SETUPSRV     0x100f
+#define BT_MESH_MODEL_ID_GEN_LOCATION_CLI          0x1010
+#define BT_MESH_MODEL_ID_GEN_ADMIN_PROP_SRV        0x1011
+#define BT_MESH_MODEL_ID_GEN_MANUFACTURER_PROP_SRV 0x1012
+#define BT_MESH_MODEL_ID_GEN_USER_PROP_SRV         0x1013
+#define BT_MESH_MODEL_ID_GEN_CLIENT_PROP_SRV       0x1014
+#define BT_MESH_MODEL_ID_GEN_PROP_CLI              0x1015
+#define BT_MESH_MODEL_ID_SENSOR_SRV                0x1100
+#define BT_MESH_MODEL_ID_SENSOR_SETUP_SRV          0x1101
+#define BT_MESH_MODEL_ID_SENSOR_CLI                0x1102
+#define BT_MESH_MODEL_ID_TIME_SRV                  0x1200
+#define BT_MESH_MODEL_ID_TIME_SETUP_SRV            0x1201
+#define BT_MESH_MODEL_ID_TIME_CLI                  0x1202
+#define BT_MESH_MODEL_ID_SCENE_SRV                 0x1203
+#define BT_MESH_MODEL_ID_SCENE_SETUP_SRV           0x1204
+#define BT_MESH_MODEL_ID_SCENE_CLI                 0x1205
+#define BT_MESH_MODEL_ID_SCHEDULER_SRV             0x1206
+#define BT_MESH_MODEL_ID_SCHEDULER_SETUP_SRV       0x1207
+#define BT_MESH_MODEL_ID_SCHEDULER_CLI             0x1208
+#define BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_SRV       0x1300
+#define BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_SETUP_SRV 0x1301
+#define BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_CLI       0x1302
+#define BT_MESH_MODEL_ID_LIGHT_CTL_SRV             0x1303
+#define BT_MESH_MODEL_ID_LIGHT_CTL_SETUP_SRV       0x1304
+#define BT_MESH_MODEL_ID_LIGHT_CTL_CLI             0x1305
+#define BT_MESH_MODEL_ID_LIGHT_CTL_TEMP_SRV        0x1306
+#define BT_MESH_MODEL_ID_LIGHT_HSL_SRV             0x1307
+#define BT_MESH_MODEL_ID_LIGHT_HSL_SETUP_SRV       0x1308
+#define BT_MESH_MODEL_ID_LIGHT_HSL_CLI             0x1309
+#define BT_MESH_MODEL_ID_LIGHT_HSL_HUE_SRV         0x130a
+#define BT_MESH_MODEL_ID_LIGHT_HSL_SAT_SRV         0x130b
+#define BT_MESH_MODEL_ID_LIGHT_XYL_SRV             0x130c
+#define BT_MESH_MODEL_ID_LIGHT_XYL_SETUP_SRV       0x130d
+#define BT_MESH_MODEL_ID_LIGHT_XYL_CLI             0x130e
+#define BT_MESH_MODEL_ID_LIGHT_LC_SRV              0x130f
+#define BT_MESH_MODEL_ID_LIGHT_LC_SETUPSRV         0x1310
+#define BT_MESH_MODEL_ID_LIGHT_LC_CLI              0x1311
+
+/** Message sending context. */
+struct bt_mesh_msg_ctx {
+	/** NetKey Index of the subnet to send the message on. */
+	u16_t net_idx;
+
+	/** AppKey Index to encrypt the message with. */
+	u16_t app_idx;
+
+	/** Remote address. */
+	u16_t addr;
+
+	/** Destination address of a received message. Not used for sending. */
+	u16_t recv_dst;
+
+	/** RSSI of received packet. Not used for sending. */
+	s8_t  recv_rssi;
+
+	/** Received TTL value. Not used for sending. */
+	u8_t  recv_ttl;
+
+	/** Force sending reliably by using segment acknowledgement */
+	bool  send_rel;
+
+	/** TTL, or BT_MESH_TTL_DEFAULT for default TTL. */
+	u8_t  send_ttl;
+};
+
+struct bt_mesh_model_op {
+	/* OpCode encoded using the BT_MESH_MODEL_OP_* macros */
+	const u32_t  opcode;
+
+	/* Minimum required message length */
+	const size_t min_len;
+
+	/* Message handler for the opcode */
+	void (*const func)(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf);
+};
+
+#define BT_MESH_MODEL_OP_1(b0) (b0)
+#define BT_MESH_MODEL_OP_2(b0, b1) (((b0) << 8) | (b1))
+#define BT_MESH_MODEL_OP_3(b0, cid) ((((b0) << 16) | 0xc00000) | (cid))
+
+#define BT_MESH_MODEL_OP_END { 0, 0, NULL }
+#define BT_MESH_MODEL_NO_OPS ((struct bt_mesh_model_op []) \
+			      { BT_MESH_MODEL_OP_END })
+
+/** Helper to define an empty model array */
+#define BT_MESH_MODEL_NONE ((struct bt_mesh_model []){})
+
+/** Length of a short Mesh MIC. */
+#define BT_MESH_MIC_SHORT 4
+/** Length of a long Mesh MIC. */
+#define BT_MESH_MIC_LONG 8
+
+/** @def BT_MESH_MODEL_OP_LEN
+ *
+ * @brief Helper to determine the length of an opcode.
+ *
+ * @param _op Opcode.
+ */
+#define BT_MESH_MODEL_OP_LEN(_op) ((_op) <= 0xff ? 1 : (_op) <= 0xffff ? 2 : 3)
+
+/** @def BT_MESH_MODEL_BUF_LEN
+ *
+ * @brief Helper for model message buffer length.
+ *
+ * Returns the length of a Mesh model message buffer, including the opcode
+ * length and a short MIC.
+ *
+ * @param _op Opcode of the message.
+ * @param _payload_len Length of the model payload.
+ */
+#define BT_MESH_MODEL_BUF_LEN(_op, _payload_len)                               \
+	(BT_MESH_MODEL_OP_LEN(_op) + (_payload_len) + BT_MESH_MIC_SHORT)
+
+/** @def BT_MESH_MODEL_BUF_LEN_LONG_MIC
+ *
+ * @brief Helper for model message buffer length.
+ *
+ * Returns the length of a Mesh model message buffer, including the opcode
+ * length and a long MIC.
+ *
+ * @param _op Opcode of the message.
+ * @param _payload_len Length of the model payload.
+ */
+#define BT_MESH_MODEL_BUF_LEN_LONG_MIC(_op, _payload_len)                      \
+	(BT_MESH_MODEL_OP_LEN(_op) + (_payload_len) + BT_MESH_MIC_LONG)
+
+/** @def BT_MESH_MODEL_BUF_DEFINE
+ *
+ * @brief Define a Mesh model message buffer using @ref NET_BUF_SIMPLE.
+ *
+ * @param _op Opcode of the message.
+ * @param _payload_len Length of the model message payload.
+ */
+#define BT_MESH_MODEL_BUF(_op, _payload_len)                      \
+	NET_BUF_SIMPLE(BT_MESH_MODEL_BUF_LEN(_op, (_payload_len)))
+
+/** @def BT_MESH_MODEL_CB
+ *
+ * @brief Composition data SIG model entry with callback functions.
+ *
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ * @param _cb Callback structure, or NULL to keep no callbacks.
+ */
+#define BT_MESH_MODEL_CB(_id, _op, _pub, _user_data, _cb)                    \
+{                                                                            \
+	.id = (_id),                                                         \
+	.op = _op,                                                           \
+	.keys = { [0 ... (CONFIG_BT_MESH_MODEL_KEY_COUNT - 1)] =             \
+			BT_MESH_KEY_UNUSED },                                \
+	.pub = _pub,                                                         \
+	.groups = { [0 ... (CONFIG_BT_MESH_MODEL_GROUP_COUNT - 1)] =         \
+			BT_MESH_ADDR_UNASSIGNED },                           \
+	.user_data = _user_data,                                             \
+	.cb = _cb,                                                           \
+}
+
+/** @def BT_MESH_MODEL_VND_CB
+ *
+ * @brief Composition data vendor model entry with callback functions.
+ *
+ * @param _company Company ID.
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ * @param _cb Callback structure, or NULL to keep no callbacks.
+ */
+#define BT_MESH_MODEL_VND_CB(_company, _id, _op, _pub, _user_data, _cb)      \
+{                                                                            \
+	.vnd.company = (_company),                                           \
+	.vnd.id = (_id),                                                     \
+	.op = _op,                                                           \
+	.pub = _pub,                                                         \
+	.keys = { [0 ... (CONFIG_BT_MESH_MODEL_KEY_COUNT - 1)] =             \
+			BT_MESH_KEY_UNUSED },                                \
+	.groups = { [0 ... (CONFIG_BT_MESH_MODEL_GROUP_COUNT - 1)] =         \
+			BT_MESH_ADDR_UNASSIGNED },                           \
+	.user_data = _user_data,                                             \
+	.cb = _cb,                                                           \
+}
+
+
+/** @def BT_MESH_MODEL
+ *
+ * @brief Composition data SIG model entry.
+ *
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ */
+#define BT_MESH_MODEL(_id, _op, _pub, _user_data)                              \
+	BT_MESH_MODEL_CB(_id, _op, _pub, _user_data, NULL)
+
+/** @def BT_MESH_MODEL_VND
+ *
+ * @brief Composition data vendor model entry.
+ *
+ * @param _company Company ID.
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ */
+#define BT_MESH_MODEL_VND(_company, _id, _op, _pub, _user_data)                \
+	BT_MESH_MODEL_VND_CB(_company, _id, _op, _pub, _user_data, NULL)
+
+/** @def BT_MESH_TRANSMIT
+ *
+ *  @brief Encode transmission count & interval steps.
+ *
+ *  @param count   Number of retransmissions (first transmission is excluded).
+ *  @param int_ms  Interval steps in milliseconds. Must be greater than 0,
+ *                 less than or equal to 320, and a multiple of 10.
+ *
+ *  @return Mesh transmit value that can be used e.g. for the default
+ *          values of the configuration model data.
+ */
+#define BT_MESH_TRANSMIT(count, int_ms) ((count) | (((int_ms / 10) - 1) << 3))
+
+/** @def BT_MESH_TRANSMIT_COUNT
+ *
+ *  @brief Decode transmit count from a transmit value.
+ *
+ *  @param transmit Encoded transmit count & interval value.
+ *
+ *  @return Transmission count (actual transmissions is N + 1).
+ */
+#define BT_MESH_TRANSMIT_COUNT(transmit) (((transmit) & (u8_t)BIT_MASK(3)))
+
+/** @def BT_MESH_TRANSMIT_INT
+ *
+ *  @brief Decode transmit interval from a transmit value.
+ *
+ *  @param transmit Encoded transmit count & interval value.
+ *
+ *  @return Transmission interval in milliseconds.
+ */
+#define BT_MESH_TRANSMIT_INT(transmit) ((((transmit) >> 3) + 1) * 10)
+
+/** @def BT_MESH_PUB_TRANSMIT
+ *
+ *  @brief Encode Publish Retransmit count & interval steps.
+ *
+ *  @param count   Number of retransmissions (first transmission is excluded).
+ *  @param int_ms  Interval steps in milliseconds. Must be greater than 0
+ *                 and a multiple of 50.
+ *
+ *  @return Mesh transmit value that can be used e.g. for the default
+ *          values of the configuration model data.
+ */
+#define BT_MESH_PUB_TRANSMIT(count, int_ms) BT_MESH_TRANSMIT(count,           \
+							     (int_ms) / 5)
+
+/** @def BT_MESH_PUB_TRANSMIT_COUNT
+ *
+ *  @brief Decode Pubhlish Retransmit count from a given value.
+ *
+ *  @param transmit Encoded Publish Retransmit count & interval value.
+ *
+ *  @return Retransmission count (actual transmissions is N + 1).
+ */
+#define BT_MESH_PUB_TRANSMIT_COUNT(transmit) BT_MESH_TRANSMIT_COUNT(transmit)
+
+/** @def BT_MESH_PUB_TRANSMIT_INT
+ *
+ *  @brief Decode Publish Retransmit interval from a given value.
+ *
+ *  @param transmit Encoded Publish Retransmit count & interval value.
+ *
+ *  @return Transmission interval in milliseconds.
+ */
+#define BT_MESH_PUB_TRANSMIT_INT(transmit) ((((transmit) >> 3) + 1) * 50)
+
+/** Model publication context. */
+struct bt_mesh_model_pub {
+	/** The model the context belongs to. Initialized by the stack. */
+	struct bt_mesh_model *mod;
+
+	u16_t addr;         /**< Publish Address. */
+	u16_t key;          /**< Publish AppKey Index. */
+
+	u8_t  ttl;          /**< Publish Time to Live. */
+	u8_t  retransmit;   /**< Retransmit Count & Interval Steps. */
+	u8_t  period;       /**< Publish Period. */
+	u8_t  period_div:4, /**< Divisor for the Period. */
+	      cred:1,       /**< Friendship Credentials Flag. */
+	      fast_period:1,/**< Use FastPeriodDivisor */
+	      count:3;      /**< Retransmissions left. */
+
+	u32_t period_start; /**< Start of the current period. */
+
+	/** @brief Publication buffer, containing the publication message.
+	 *
+	 *  The application is expected to initialize this with
+	 *  a valid net_buf_simple pointer, with the help of e.g.
+	 *  the NET_BUF_SIMPLE() macro. The publication buffer must
+	 *  contain a valid publication message before calling the
+	 *  bt_mesh_model_publish() API or after the publication's
+	 *  @ref bt_mesh_model_pub.update callback has been called
+	 *  and returned success. The buffer must be created outside
+	 *  of function context, i.e. it must not be on the stack.
+	 *  This is most conveniently acheived by creating it inline
+	 *  when declaring the publication context:
+	 *
+	 *      static struct bt_mesh_model_pub my_pub = {
+	 *              .msg = NET_BUF_SIMPLE(size),
+	 *      };
+	 */
+	struct os_mbuf *msg;
+
+	/** @brief Callback for updating the publication buffer.
+	 *
+	 *  When set to NULL, the model is assumed not to support
+	 *  periodic publishing. When set to non-NULL the callback
+	 *  will be called periodically and is expected to update
+	 *  @ref bt_mesh_model_pub.msg with a valid publication
+	 *  message.
+	 *
+	 *  @param mod The Model the Publication Context belogs to.
+	 *
+	 *  @return Zero on success or (negative) error code otherwise.
+	 */
+	int (*update)(struct bt_mesh_model *mod);
+
+	/** Publish Period Timer. Only for stack-internal use. */
+	struct k_delayed_work timer;
+};
+
+/** Model callback functions. */
+struct bt_mesh_model_cb {
+	/** @brief Set value handler of user data tied to the model.
+	 *
+	 * @sa settings_handler::h_set
+	 *
+	 * @param model Model to set the persistent data of.
+	 * @param val Data from the backend.
+	 *
+	 * @return 0 on success, error otherwise.
+	 */
+	int (*const settings_set)(struct bt_mesh_model *model, char *val);
+
+	/** @brief Callback called when all settings have been loaded.
+	 *
+	 * This handler gets called after the settings have been loaded in
+	 * full.
+	 *
+	 * @sa settings_handler::h_commit
+	 *
+	 * @param model Model this callback belongs to.
+	 *
+	 * @return 0 on success, error otherwise.
+	 */
+	int (*const settings_commit)(struct bt_mesh_model *model);
+
+	/** @brief Model init callback.
+	 *
+	 * Called on every model instance during mesh initialization.
+	 *
+	 * @param model Model to be initialized.
+	 *
+	 * @return 0 on success, error otherwise.
+	 */
+	int (*const init)(struct bt_mesh_model *model);
+
+	/** @brief Model reset callback.
+	 *
+	 * Called when the mesh node is reset. All model data is deleted on
+	 * reset, and the model should clear its state.
+	 *
+	 * @param model Model this callback belongs to.
+	 */
+	void (*const reset)(struct bt_mesh_model *model);
+};
+
+/** Abstraction that describes a Mesh Model instance */
+struct bt_mesh_model {
+	union {
+		const u16_t id;
+		struct {
+			u16_t company;
+			u16_t id;
+		} vnd;
+	};
+
+	/* Internal information, mainly for persistent storage */
+	u8_t  elem_idx;   /* Belongs to Nth element */
+	u8_t  mod_idx;    /* Is the Nth model in the element */
+	u16_t flags;      /* Model flags for internal bookkeeping */
+
+	/* Model Publication */
+	struct bt_mesh_model_pub * const pub;
+
+	/* AppKey List */
+	u16_t keys[CONFIG_BT_MESH_MODEL_KEY_COUNT];
+
+	/* Subscription List (group or virtual addresses) */
+	u16_t groups[CONFIG_BT_MESH_MODEL_GROUP_COUNT];
+
+	const struct bt_mesh_model_op * const op;
+
+	/* Model callback structure. */
+	const struct bt_mesh_model_cb * const cb;
+
+#if MYNEWT_VAL(BLE_MESH_MODEL_EXTENSIONS)
+	/* Pointer to the next model in a model extension tree. */
+	struct bt_mesh_model *next;
+	/* Pointer to the first model this model extends. */
+	struct bt_mesh_model *extends;
+#endif
+	/* Model-specific user data */
+	void *user_data;
+};
+
+struct bt_mesh_send_cb {
+	void (*start)(u16_t duration, int err, void *cb_data);
+	void (*end)(int err, void *cb_data);
+};
+
+void bt_mesh_model_msg_init(struct os_mbuf *msg, u32_t opcode);
+
+/** Special TTL value to request using configured default TTL */
+#define BT_MESH_TTL_DEFAULT 0xff
+
+/** Maximum allowed TTL value */
+#define BT_MESH_TTL_MAX     0x7f
+
+/**
+ * @brief Send an Access Layer message.
+ *
+ * @param model     Mesh (client) Model that the message belongs to.
+ * @param ctx       Message context, includes keys, TTL, etc.
+ * @param msg       Access Layer payload (the actual message to be sent).
+ * @param cb        Optional "message sent" callback.
+ * @param cb_data   User data to be passed to the callback.
+ *
+ * @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_model_send(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *msg,
+		       const struct bt_mesh_send_cb *cb,
+		       void *cb_data);
+
+/**
+ * @brief Send a model publication message.
+ *
+ * Before calling this function, the user needs to ensure that the model
+ * publication message (@ref bt_mesh_model_pub.msg) contains a valid
+ * message to be sent. Note that this API is only to be used for
+ * non-period publishing. For periodic publishing the app only needs
+ * to make sure that @ref bt_mesh_model_pub.msg contains a valid message
+ * whenever the @ref bt_mesh_model_pub.update callback is called.
+ *
+ * @param model  Mesh (client) Model that's publishing the message.
+ *
+ * @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_model_publish(struct bt_mesh_model *model);
+
+/**
+ * @brief Get the element that a model belongs to.
+ *
+ * @param mod  Mesh model.
+ *
+ * @return Pointer to the element that the given model belongs to.
+ */
+struct bt_mesh_elem *bt_mesh_model_elem(struct bt_mesh_model *mod);
+
+/** @brief Find a SIG model.
+ *
+ * @param elem Element to search for the model in.
+ * @param id Model ID of the model.
+ *
+ * @return A pointer to the Mesh model matching the given parameters, or NULL
+ * if no SIG model with the given ID exists in the given element.
+ */
+struct bt_mesh_model *bt_mesh_model_find(const struct bt_mesh_elem *elem,
+					 u16_t id);
+
+/** @brief Find a vendor model.
+ *
+ * @param elem Element to search for the model in.
+ * @param company Company ID of the model.
+ * @param id Model ID of the model.
+ *
+ * @return A pointer to the Mesh model matching the given parameters, or NULL
+ * if no vendor model with the given ID exists in the given element.
+ */
+struct bt_mesh_model *bt_mesh_model_find_vnd(const struct bt_mesh_elem *elem,
+					     u16_t company, u16_t id);
+
+/** @brief Get whether the model is in the primary element of the device.
+ *
+ * @param mod Mesh model.
+ *
+ * @return true if the model is on the primary element, false otherwise.
+ */
+static inline bool bt_mesh_model_in_primary(const struct bt_mesh_model *mod)
+{
+	return (mod->elem_idx == 0);
+}
+
+/** @brief Immediately store the model's user data in persistent storage.
+ *
+ * @param mod Mesh model.
+ * @param vnd This is a vendor model.
+ * @param data Model data to store, or NULL to delete any model data.
+ * @param data_len Length of the model data.
+ *
+ * @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_model_data_store(struct bt_mesh_model *mod, bool vnd,
+			     const void *data, size_t data_len);
+
+/** @brief Let a model extend another.
+ *
+ * Mesh models may be extended to reuse their functionality, forming a more
+ * complex model. A Mesh model may extend any number of models, in any element.
+ * The extensions may also be nested, ie a model that extends another may itself
+ * be extended. Extensions may not be cyclical, and a model can only be extended
+ * by one other model.
+ *
+ * A set of models that extend each other form a model extension tree.
+ *
+ * All models in an extension tree share one subscription list per element. The
+ * access layer will utilize the combined subscription list of all models in an
+ * extension tree and element, giving the models extended subscription list
+ * capacity.
+ *
+ * @param[in] mod Mesh model.
+ * @param[in] base_mod The model being extended.
+ *
+ * @retval 0 Successfully extended the base_mod model.
+ * @retval -EALREADY The base_mod model is already extended.
+ */
+int bt_mesh_model_extend(struct bt_mesh_model *mod,
+			 struct bt_mesh_model *base_mod);
+
+/** Node Composition */
+struct bt_mesh_comp {
+	u16_t cid;
+	u16_t pid;
+	u16_t vid;
+
+	size_t elem_count;
+	struct bt_mesh_elem *elem;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_ACCESS_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/cfg_cli.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/cfg_cli.h
new file mode 100644
index 00000000..7dc237be
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/cfg_cli.h
@@ -0,0 +1,234 @@
+/** @file
+ *  @brief Bluetooth Mesh Configuration Client Model APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_CFG_CLI_H
+#define __BT_MESH_CFG_CLI_H
+
+/**
+ * @brief Bluetooth Mesh
+ * @defgroup bt_mesh_cfg_cli Bluetooth Mesh Configuration Client Model
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Mesh Configuration Client Model Context */
+struct bt_mesh_cfg_cli {
+	struct bt_mesh_model *model;
+
+	struct k_sem          op_sync;
+	u32_t                 op_pending;
+	void                 *op_param;
+};
+
+extern const struct bt_mesh_model_op bt_mesh_cfg_cli_op[];
+extern const struct bt_mesh_model_cb bt_mesh_cfg_cli_cb;
+
+#define BT_MESH_MODEL_CFG_CLI(cli_data)                                        \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_CFG_CLI, bt_mesh_cfg_cli_op, NULL,   \
+			 cli_data, &bt_mesh_cfg_cli_cb)
+
+int bt_mesh_cfg_comp_data_get(u16_t net_idx, u16_t addr, u8_t page,
+			      u8_t *status, struct os_mbuf *comp);
+
+int bt_mesh_cfg_beacon_get(u16_t net_idx, u16_t addr, u8_t *status);
+
+int bt_mesh_cfg_beacon_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *status);
+
+int bt_mesh_cfg_ttl_get(u16_t net_idx, u16_t addr, u8_t *ttl);
+
+int bt_mesh_cfg_ttl_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *ttl);
+
+int bt_mesh_cfg_friend_get(u16_t net_idx, u16_t addr, u8_t *status);
+
+int bt_mesh_cfg_friend_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *status);
+
+int bt_mesh_cfg_gatt_proxy_get(u16_t net_idx, u16_t addr, u8_t *status);
+
+int bt_mesh_cfg_gatt_proxy_set(u16_t net_idx, u16_t addr, u8_t val,
+			       u8_t *status);
+
+int bt_mesh_cfg_relay_get(u16_t net_idx, u16_t addr, u8_t *status,
+			  u8_t *transmit);
+
+int bt_mesh_cfg_relay_set(u16_t net_idx, u16_t addr, u8_t new_relay,
+			  u8_t new_transmit, u8_t *status, u8_t *transmit);
+
+int bt_mesh_cfg_net_key_add(u16_t net_idx, u16_t addr, u16_t key_net_idx,
+			    const u8_t net_key[16], u8_t *status);
+
+int bt_mesh_cfg_app_key_add(u16_t net_idx, u16_t addr, u16_t key_net_idx,
+			    u16_t key_app_idx, const u8_t app_key[16],
+			    u8_t *status);
+
+int bt_mesh_cfg_mod_app_bind(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			     u16_t mod_app_idx, u16_t mod_id, u8_t *status);
+
+int bt_mesh_cfg_mod_app_bind_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				 u16_t mod_app_idx, u16_t mod_id, u16_t cid,
+				 u8_t *status);
+
+/** @def BT_MESH_PUB_PERIOD_100MS
+ *
+ *  @brief Helper macro to encode model publication period in units of 100ms
+ *
+ *  @param steps Number of 100ms steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_100MS(steps)  ((steps) & BIT_MASK(6))
+
+/** @def BT_MESH_PUB_PERIOD_SEC
+ *
+ *  @brief Helper macro to encode model publication period in units of 1 second
+ *
+ *  @param steps Number of 1 second steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_SEC(steps)   (((steps) & BIT_MASK(6)) | (1 << 6))
+
+/** @def BT_MESH_PUB_PERIOD_10SEC
+ *
+ *  @brief Helper macro to encode model publication period in units of 10
+ *  seconds
+ *
+ *  @param steps Number of 10 second steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_10SEC(steps) (((steps) & BIT_MASK(6)) | (2 << 6))
+
+/** @def BT_MESH_PUB_PERIOD_10MIN
+ *
+ *  @brief Helper macro to encode model publication period in units of 10
+ *  minutes
+ *
+ *  @param steps Number of 10 minute steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_10MIN(steps) (((steps) & BIT_MASK(6)) | (3 << 6))
+
+struct bt_mesh_cfg_mod_pub {
+	u16_t  addr;
+	u16_t  app_idx;
+	bool   cred_flag;
+	u8_t   ttl;
+	u8_t   period;
+	u8_t   transmit;
+};
+
+int bt_mesh_cfg_mod_pub_get(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
+			    u8_t *status);
+
+int bt_mesh_cfg_mod_pub_get_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				u16_t mod_id, u16_t cid,
+				struct bt_mesh_cfg_mod_pub *pub, u8_t *status);
+
+int bt_mesh_cfg_mod_pub_set(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
+			    u8_t *status);
+
+int bt_mesh_cfg_mod_pub_set_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				u16_t mod_id, u16_t cid,
+				struct bt_mesh_cfg_mod_pub *pub, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_add(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t sub_addr, u16_t mod_id, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_add_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				 u16_t sub_addr, u16_t mod_id, u16_t cid,
+				 u8_t *status);
+
+int bt_mesh_cfg_mod_sub_del(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t sub_addr, u16_t mod_id, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_del_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				 u16_t sub_addr, u16_t mod_id, u16_t cid,
+				 u8_t *status);
+
+int bt_mesh_cfg_mod_sub_overwrite(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				  u16_t sub_addr, u16_t mod_id, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_overwrite_vnd(u16_t net_idx, u16_t addr,
+				      u16_t elem_addr, u16_t sub_addr,
+				      u16_t mod_id, u16_t cid, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_va_add(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			       const u8_t label[16], u16_t mod_id,
+			       u16_t *virt_addr, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_va_add_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				   const u8_t label[16], u16_t mod_id,
+				   u16_t cid, u16_t *virt_addr, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_va_del(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			       const u8_t label[16], u16_t mod_id,
+			       u16_t *virt_addr, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_va_del_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				   const u8_t label[16], u16_t mod_id,
+				   u16_t cid, u16_t *virt_addr, u8_t *status);
+
+int bt_mesh_cfg_mod_sub_va_overwrite(u16_t net_idx, u16_t addr,
+				     u16_t elem_addr, const u8_t label[16],
+				     u16_t mod_id, u16_t *virt_addr,
+				     u8_t *status);
+
+int bt_mesh_cfg_mod_sub_va_overwrite_vnd(u16_t net_idx, u16_t addr,
+					 u16_t elem_addr, const u8_t label[16],
+					 u16_t mod_id, u16_t cid,
+					 u16_t *virt_addr, u8_t *status);
+
+struct bt_mesh_cfg_hb_sub {
+	u16_t src;
+	u16_t dst;
+	u8_t  period;
+	u8_t  count;
+	u8_t  min;
+	u8_t  max;
+};
+
+int bt_mesh_cfg_hb_sub_set(u16_t net_idx, u16_t addr,
+			   struct bt_mesh_cfg_hb_sub *sub, u8_t *status);
+
+int bt_mesh_cfg_hb_sub_get(u16_t net_idx, u16_t addr,
+			   struct bt_mesh_cfg_hb_sub *sub, u8_t *status);
+
+struct bt_mesh_cfg_hb_pub {
+	u16_t dst;
+	u8_t  count;
+	u8_t  period;
+	u8_t  ttl;
+	u16_t feat;
+	u16_t net_idx;
+};
+
+int bt_mesh_cfg_hb_pub_set(u16_t net_idx, u16_t addr,
+			   const struct bt_mesh_cfg_hb_pub *pub, u8_t *status);
+
+int bt_mesh_cfg_hb_pub_get(u16_t net_idx, u16_t addr,
+			   struct bt_mesh_cfg_hb_pub *pub, u8_t *status);
+
+s32_t bt_mesh_cfg_cli_timeout_get(void);
+void bt_mesh_cfg_cli_timeout_set(s32_t timeout);
+
+#ifdef __cplusplus
+}
+#endif
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_CFG_CLI_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/cfg_srv.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/cfg_srv.h
new file mode 100644
index 00000000..14d8a295
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/cfg_srv.h
@@ -0,0 +1,78 @@
+/** @file
+ *  @brief Bluetooth Mesh Configuration Server Model APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_CFG_SRV_H
+#define __BT_MESH_CFG_SRV_H
+
+/**
+ * @brief Bluetooth Mesh
+ * @defgroup bt_mesh_cfg_srv Bluetooth Mesh Configuration Server Model
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Mesh Configuration Server Model Context */
+struct bt_mesh_cfg_srv {
+	struct bt_mesh_model *model;
+
+	u8_t net_transmit;         /* Network Transmit state */
+	u8_t relay;                /* Relay Mode state */
+	u8_t relay_retransmit;     /* Relay Retransmit state */
+	u8_t beacon;               /* Secure Network Beacon state */
+	u8_t gatt_proxy;           /* GATT Proxy state */
+	u8_t frnd;                 /* Friend state */
+	u8_t default_ttl;          /* Default TTL */
+
+	/* Heartbeat Publication */
+	struct bt_mesh_hb_pub {
+		struct k_delayed_work timer;
+
+		u16_t dst;
+		u16_t count;
+		u8_t  period;
+		u8_t  ttl;
+		u16_t feat;
+		u16_t net_idx;
+	} hb_pub;
+
+	/* Heartbeat Subscription */
+	struct bt_mesh_hb_sub {
+		s64_t  expiry;
+
+		u16_t src;
+		u16_t dst;
+		u16_t count;
+		u8_t  min_hops;
+		u8_t  max_hops;
+
+		/* Optional subscription tracking function */
+		void (*func)(u8_t hops, u16_t feat);
+	} hb_sub;
+};
+
+extern const struct bt_mesh_model_op bt_mesh_cfg_srv_op[];
+extern const struct bt_mesh_model_cb bt_mesh_cfg_srv_cb;
+
+#define BT_MESH_MODEL_CFG_SRV(srv_data)                                        \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_CFG_SRV, bt_mesh_cfg_srv_op, NULL,   \
+			 srv_data, &bt_mesh_cfg_srv_cb)
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_CFG_SRV_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/glue.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/glue.h
new file mode 100644
index 00000000..e37fcfbc
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/glue.h
@@ -0,0 +1,502 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef _MESH_GLUE_
+#define _MESH_GLUE_
+
+#include <assert.h>
+#include <errno.h>
+
+#include "syscfg/syscfg.h"
+#include "logcfg/logcfg.h"
+#include "modlog/modlog.h"
+#include "nimble/nimble_npl.h"
+
+#include "os/os_mbuf.h"
+#include "os/queue.h"
+
+#include "nimble/ble.h"
+#include "host/ble_hs.h"
+#include "host/ble_uuid.h"
+#include "../src/ble_sm_priv.h"
+#include "../src/ble_hs_hci_priv.h"
+
+#include "tinycrypt/aes.h"
+#include "tinycrypt/constants.h"
+#include "tinycrypt/utils.h"
+#include "tinycrypt/cmac_mode.h"
+#include "tinycrypt/ecc_dh.h"
+
+#if MYNEWT_VAL(BLE_MESH_SETTINGS)
+#include "config/config.h"
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define u8_t    uint8_t
+#define s8_t    int8_t
+#define u16_t   uint16_t
+#define s16_t   int16_t
+#define u32_t   uint32_t
+#define u64_t   uint64_t
+#define s64_t   int64_t
+#define s32_t   int32_t
+
+/** @brief Helper to declare elements of bt_data arrays
+ *
+ *  This macro is mainly for creating an array of struct bt_data
+ *  elements which is then passed to bt_le_adv_start().
+ *
+ *  @param _type Type of advertising data field
+ *  @param _data Pointer to the data field payload
+ *  @param _data_len Number of bytes behind the _data pointer
+ */
+#define BT_DATA(_type, _data, _data_len) \
+    { \
+        .type = (_type), \
+        .data_len = (_data_len), \
+        .data = (const u8_t *)(_data), \
+    }
+
+/** @brief Helper to declare elements of bt_data arrays
+ *
+ *  This macro is mainly for creating an array of struct bt_data
+ *  elements which is then passed to bt_le_adv_start().
+ *
+ *  @param _type Type of advertising data field
+ *  @param _bytes Variable number of single-byte parameters
+ */
+#define BT_DATA_BYTES(_type, _bytes...) \
+    BT_DATA(_type, ((u8_t []) { _bytes }), \
+        sizeof((u8_t []) { _bytes }))
+
+/* EIR/AD data type definitions */
+#define BT_DATA_FLAGS                   0x01 /* AD flags */
+#define BT_DATA_UUID16_SOME             0x02 /* 16-bit UUID, more available */
+#define BT_DATA_UUID16_ALL              0x03 /* 16-bit UUID, all listed */
+#define BT_DATA_UUID32_SOME             0x04 /* 32-bit UUID, more available */
+#define BT_DATA_UUID32_ALL              0x05 /* 32-bit UUID, all listed */
+#define BT_DATA_UUID128_SOME            0x06 /* 128-bit UUID, more available */
+#define BT_DATA_UUID128_ALL             0x07 /* 128-bit UUID, all listed */
+#define BT_DATA_NAME_SHORTENED          0x08 /* Shortened name */
+#define BT_DATA_NAME_COMPLETE           0x09 /* Complete name */
+#define BT_DATA_TX_POWER                0x0a /* Tx Power */
+#define BT_DATA_SOLICIT16               0x14 /* Solicit UUIDs, 16-bit */
+#define BT_DATA_SOLICIT128              0x15 /* Solicit UUIDs, 128-bit */
+#define BT_DATA_SVC_DATA16              0x16 /* Service data, 16-bit UUID */
+#define BT_DATA_GAP_APPEARANCE          0x19 /* GAP appearance */
+#define BT_DATA_SOLICIT32               0x1f /* Solicit UUIDs, 32-bit */
+#define BT_DATA_SVC_DATA32              0x20 /* Service data, 32-bit UUID */
+#define BT_DATA_SVC_DATA128             0x21 /* Service data, 128-bit UUID */
+#define BT_DATA_URI                     0x24 /* URI */
+#define BT_DATA_MESH_PROV               0x29 /* Mesh Provisioning PDU */
+#define BT_DATA_MESH_MESSAGE            0x2a /* Mesh Networking PDU */
+#define BT_DATA_MESH_BEACON             0x2b /* Mesh Beacon */
+
+#define BT_DATA_MANUFACTURER_DATA       0xff /* Manufacturer Specific Data */
+
+#define BT_LE_AD_LIMITED                0x01 /* Limited Discoverable */
+#define BT_LE_AD_GENERAL                0x02 /* General Discoverable */
+#define BT_LE_AD_NO_BREDR               0x04 /* BR/EDR not supported */
+
+#define sys_put_be16(a,b) put_be16(b, a)
+#define sys_put_le16(a,b) put_le16(b, a)
+#define sys_put_be32(a,b) put_be32(b, a)
+#define sys_get_be16(a) get_be16(a)
+#define sys_get_le16(a) get_le16(a)
+#define sys_get_be32(a) get_be32(a)
+#define sys_cpu_to_be16(a) htobe16(a)
+#define sys_cpu_to_be32(a) htobe32(a)
+#define sys_be32_to_cpu(a) be32toh(a)
+#define sys_be16_to_cpu(a) be16toh(a)
+#define sys_le16_to_cpu(a) le16toh(a)
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+#endif
+
+#define CODE_UNREACHABLE __builtin_unreachable()
+#define __ASSERT(code, str) \
+    do {                          \
+        if (!(code)) BT_ERR(str); \
+        assert(code);             \
+    } while (0);
+
+#define __ASSERT_NO_MSG(test) __ASSERT(test, "")
+
+/* Mesh is designed to not use mbuf chains */
+#if BT_DBG_ENABLED
+#define ASSERT_NOT_CHAIN(om) assert(SLIST_NEXT(om, om_next) == NULL)
+#else
+#define ASSERT_NOT_CHAIN(om) (void)(om)
+#endif
+
+#define __packed    __attribute__((__packed__))
+
+#define MSEC_PER_SEC   (1000)
+#define K_MSEC(ms)     (ms)
+#define K_SECONDS(s)   K_MSEC((s) * MSEC_PER_SEC)
+#define K_MINUTES(m)   K_SECONDS((m) * 60)
+#define K_HOURS(h)     K_MINUTES((h) * 60)
+
+#ifndef BIT
+#define BIT(n)  (1UL << (n))
+#endif
+
+#define BIT_MASK(n) (BIT(n) - 1)
+
+#define BT_GAP_ADV_FAST_INT_MIN_1               0x0030  /* 30 ms    */
+#define BT_GAP_ADV_FAST_INT_MAX_1               0x0060  /* 60 ms    */
+#define BT_GAP_ADV_FAST_INT_MIN_2               0x00a0  /* 100 ms   */
+#define BT_GAP_ADV_FAST_INT_MAX_2               0x00f0  /* 150 ms   */
+#define BT_GAP_ADV_SLOW_INT_MIN                 0x0640  /* 1 s      */
+#define BT_GAP_ADV_SLOW_INT_MAX                 0x0780  /* 1.2 s    */
+
+#ifndef MESH_LOG_MODULE
+#define MESH_LOG_MODULE BLE_MESH_LOG
+#endif
+
+#define CAT(a, ...) PRIMITIVE_CAT(a, __VA_ARGS__)
+#define PRIMITIVE_CAT(a, ...) a ## __VA_ARGS__
+
+#define BLE_MESH_LOG(lvl, ...) CAT(MESH_LOG_MODULE, CAT(_, lvl))(__VA_ARGS__)
+
+#define BT_DBG(fmt, ...)    BLE_MESH_LOG(DEBUG, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_INFO(fmt, ...)   BLE_MESH_LOG(INFO, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_WARN(fmt, ...)   BLE_MESH_LOG(WARN, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_ERR(fmt, ...)    BLE_MESH_LOG(ERROR, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_GATT_ERR(_att_err)   (-(_att_err))
+
+typedef ble_addr_t bt_addr_le_t;
+
+#define k_fifo_init(queue) ble_npl_eventq_init(queue)
+#define net_buf_simple_tailroom(buf) OS_MBUF_TRAILINGSPACE(buf)
+#define net_buf_tailroom(buf) net_buf_simple_tailroom(buf)
+#define net_buf_headroom(buf) ((buf)->om_data - &(buf)->om_databuf[buf->om_pkthdr_len])
+#define net_buf_simple_headroom(buf) net_buf_headroom(buf)
+#define net_buf_simple_tail(buf) ((buf)->om_data + (buf)->om_len)
+
+struct net_buf_simple_state {
+    /** Offset of the data pointer from the beginning of the storage */
+    u16_t offset;
+    /** Length of data */
+    u16_t len;
+};
+
+static inline struct os_mbuf * NET_BUF_SIMPLE(uint16_t size)
+{
+    struct os_mbuf *buf;
+
+    buf = os_msys_get(size, 0);
+    assert(buf);
+
+    return buf;
+}
+
+#define K_NO_WAIT (0)
+#define K_FOREVER (-1)
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+#define BT_MESH_ADV_INST     (MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES))
+
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+/* Note that BLE_MULTI_ADV_INSTANCES contains number of additional instances.
+ * Instance 0 is always there
+ */
+#if MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES) < 1
+#error "Mesh needs at least BLE_MULTI_ADV_INSTANCES set to 1"
+#endif
+#define BT_MESH_ADV_GATT_INST     (MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES) - 1)
+#endif /* BLE_MESH_PROXY */
+#endif /* BLE_EXT_ADV */
+
+/* This is by purpose */
+static inline void net_buf_simple_init(struct os_mbuf *buf,
+                                       size_t reserve_head)
+{
+    /* This is called in Zephyr after init.
+     * Note in Mynewt case we don't care abour reserved head*/
+    buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + reserve_head;
+    buf->om_len = 0;
+}
+
+void net_buf_put(struct ble_npl_eventq *fifo, struct os_mbuf *buf);
+void * net_buf_ref(struct os_mbuf *om);
+void net_buf_unref(struct os_mbuf *om);
+uint16_t net_buf_simple_pull_le16(struct os_mbuf *om);
+uint16_t net_buf_simple_pull_be16(struct os_mbuf *om);
+uint32_t net_buf_simple_pull_be32(struct os_mbuf *om);
+uint32_t net_buf_simple_pull_le32(struct os_mbuf *om);
+uint8_t net_buf_simple_pull_u8(struct os_mbuf *om);
+void net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val);
+void net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val);
+void net_buf_simple_add_le32(struct os_mbuf *om, uint32_t val);
+void net_buf_add_zeros(struct os_mbuf *om, uint8_t len);
+void net_buf_simple_push_le16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_push_be16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_push_u8(struct os_mbuf *om, uint8_t val);
+void *net_buf_simple_pull(struct os_mbuf *om, uint8_t len);
+void *net_buf_simple_pull_mem(struct os_mbuf *om, uint8_t len);
+void *net_buf_simple_add(struct os_mbuf *om, uint8_t len);
+bool k_fifo_is_empty(struct ble_npl_eventq *q);
+void *net_buf_get(struct ble_npl_eventq *fifo,s32_t t);
+uint8_t *net_buf_simple_push(struct os_mbuf *om, uint8_t len);
+void net_buf_reserve(struct os_mbuf *om, size_t reserve);
+
+#define net_buf_add_mem(a,b,c) os_mbuf_append(a,b,c)
+#define net_buf_simple_add_mem(a,b,c) os_mbuf_append(a,b,c)
+#define net_buf_add_u8(a,b) net_buf_simple_add_u8(a,b)
+#define net_buf_add(a,b) net_buf_simple_add(a,b)
+
+#define net_buf_clone(a, b) os_mbuf_dup(a)
+#define net_buf_add_be32(a, b) net_buf_simple_add_be32(a, b)
+#define net_buf_add_be16(a, b) net_buf_simple_add_be16(a, b)
+#define net_buf_pull(a, b) net_buf_simple_pull(a, b)
+#define net_buf_pull_mem(a, b) net_buf_simple_pull_mem(a, b)
+#define net_buf_pull_u8(a) net_buf_simple_pull_u8(a)
+#define net_buf_pull_be16(a) net_buf_simple_pull_be16(a)
+#define net_buf_skip(a, b) net_buf_simple_pull_mem(a, b)
+
+#define BT_GATT_CCC_NOTIFY BLE_GATT_CHR_PROP_NOTIFY
+
+/** Description of different data types that can be encoded into
+  * advertising data. Used to form arrays that are passed to the
+  * bt_le_adv_start() function.
+  */
+struct bt_data {
+    u8_t type;
+    u8_t data_len;
+    const u8_t *data;
+};
+
+struct bt_pub_key_cb {
+    /** @brief Callback type for Public Key generation.
+     *
+     *  Used to notify of the local public key or that the local key is not
+     *  available (either because of a failure to read it or because it is
+     *  being regenerated).
+     *
+     *  @param key The local public key, or NULL in case of no key.
+     */
+    void (*func)(const u8_t key[64]);
+
+    struct bt_pub_key_cb *_next;
+};
+
+typedef void (*bt_dh_key_cb_t)(const u8_t key[32]);
+int bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb);
+int bt_pub_key_gen(struct bt_pub_key_cb *new_cb);
+uint8_t *bt_pub_key_get(void);
+int bt_rand(void *buf, size_t len);
+const char * bt_hex(const void *buf, size_t len);
+int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data);
+void bt_mesh_register_gatt(void);
+int bt_le_adv_start(const struct ble_gap_adv_params *param,
+                    const struct bt_data *ad, size_t ad_len,
+                    const struct bt_data *sd, size_t sd_len);
+int bt_le_adv_stop(bool proxy);
+
+struct k_delayed_work {
+    struct ble_npl_callout work;
+};
+
+void k_work_init(struct ble_npl_callout *work, ble_npl_event_fn handler);
+void k_delayed_work_init(struct k_delayed_work *w, ble_npl_event_fn *f);
+void k_delayed_work_cancel(struct k_delayed_work *w);
+void k_delayed_work_submit(struct k_delayed_work *w, uint32_t ms);
+int64_t k_uptime_get(void);
+u32_t k_uptime_get_32(void);
+void k_sleep(int32_t duration);
+void k_work_submit(struct ble_npl_callout *w);
+void k_work_add_arg(struct ble_npl_callout *w, void *arg);
+void k_delayed_work_add_arg(struct k_delayed_work *w, void *arg);
+uint32_t k_delayed_work_remaining_get(struct k_delayed_work *w);
+
+static inline void net_buf_simple_save(struct os_mbuf *buf,
+                       struct net_buf_simple_state *state)
+{
+    state->offset = net_buf_simple_headroom(buf);
+    state->len = buf->om_len;
+}
+
+static inline void net_buf_simple_restore(struct os_mbuf *buf,
+                                          struct net_buf_simple_state *state)
+{
+      buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + state->offset;
+      buf->om_len = state->len;
+}
+
+static inline void sys_memcpy_swap(void *dst, const void *src, size_t length)
+{
+    __ASSERT(((src < dst && (src + length) <= dst) ||
+          (src > dst && (dst + length) <= src)),
+         "Source and destination buffers must not overlap");
+
+    src += length - 1;
+
+    for (; length > 0; length--) {
+        *((u8_t *)dst++) = *((u8_t *)src--);
+    }
+}
+
+#define popcount(x) __builtin_popcount(x)
+
+static inline unsigned int find_lsb_set(u32_t op)
+{
+    return __builtin_ffs(op);
+}
+
+static inline unsigned int find_msb_set(u32_t op)
+{
+    if (!op)
+        return 0;
+
+    return 32 - __builtin_clz(op);
+}
+
+#define CONFIG_BT_MESH_FRIEND               BLE_MESH_FRIEND
+#define CONFIG_BT_MESH_GATT_PROXY           BLE_MESH_GATT_PROXY
+#define CONFIG_BT_MESH_IV_UPDATE_TEST       BLE_MESH_IV_UPDATE_TEST
+#define CONFIG_BT_MESH_LOW_POWER            BLE_MESH_LOW_POWER
+#define CONFIG_BT_MESH_LPN_AUTO             BLE_MESH_LPN_AUTO
+#define CONFIG_BT_MESH_LPN_ESTABLISHMENT    BLE_MESH_LPN_ESTABLISHMENT
+#define CONFIG_BT_MESH_PB_ADV               BLE_MESH_PB_ADV
+#define CONFIG_BT_MESH_PB_GATT              BLE_MESH_PB_GATT
+#define CONFIG_BT_MESH_PROV                 BLE_MESH_PROV
+#define CONFIG_BT_MESH_PROXY                BLE_MESH_PROXY
+#define CONFIG_BT_TESTING                   BLE_MESH_TESTING
+#define CONFIG_BT_SETTINGS                  BLE_MESH_SETTINGS
+#define CONFIG_SETTINGS                     BLE_MESH_SETTINGS
+#define CONFIG_BT_MESH_PROVISIONER          BLE_MESH_PROVISIONER
+
+/* Above flags are used with IS_ENABLED macro */
+#define IS_ENABLED(config) MYNEWT_VAL(config)
+
+#define CONFIG_BT_MESH_LPN_GROUPS           MYNEWT_VAL(BLE_MESH_LPN_GROUPS)
+#define CONFIG_BT_MESH_ADV_BUF_COUNT        MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT)
+#define CONFIG_BT_MESH_FRIEND_QUEUE_SIZE    MYNEWT_VAL(BLE_MESH_FRIEND_QUEUE_SIZE)
+#define CONFIG_BT_MESH_FRIEND_RECV_WIN      MYNEWT_VAL(BLE_MESH_FRIEND_RECV_WIN)
+#define CONFIG_BT_MESH_LPN_POLL_TIMEOUT     MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT)
+#define CONFIG_BT_MESH_MODEL_GROUP_COUNT    MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT)
+#define CONFIG_BT_MESH_MODEL_KEY_COUNT      MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT)
+#define CONFIG_BT_MESH_NODE_ID_TIMEOUT      MYNEWT_VAL(BLE_MESH_NODE_ID_TIMEOUT)
+#define CONFIG_BT_MAX_CONN                  MYNEWT_VAL(BLE_MAX_CONNECTIONS)
+#define CONFIG_BT_MESH_SEQ_STORE_RATE       MYNEWT_VAL(BLE_MESH_SEQ_STORE_RATE)
+#define CONFIG_BT_MESH_RPL_STORE_TIMEOUT    MYNEWT_VAL(BLE_MESH_RPL_STORE_TIMEOUT)
+#define CONFIG_BT_MESH_APP_KEY_COUNT        MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT)
+#define CONFIG_BT_MESH_SUBNET_COUNT         MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)
+#define CONFIG_BT_MESH_STORE_TIMEOUT        MYNEWT_VAL(BLE_MESH_STORE_TIMEOUT)
+#define CONFIG_BT_MESH_IVU_DIVIDER          MYNEWT_VAL(BLE_MESH_IVU_DIVIDER)
+#define CONFIG_BT_DEVICE_NAME               MYNEWT_VAL(BLE_MESH_DEVICE_NAME)
+#define CONFIG_BT_MESH_TX_SEG_MAX           MYNEWT_VAL(BLE_MESH_TX_SEG_MAX)
+#define CONFIG_BT_MESH_LABEL_COUNT          MYNEWT_VAL(BLE_MESH_LABEL_COUNT)
+#define CONFIG_BT_MESH_NODE_COUNT           MYNEWT_VAL(BLE_MESH_NODE_COUNT)
+
+#define printk console_printf
+
+#define CONTAINER_OF(ptr, type, field) \
+	((type *)(((char *)(ptr)) - offsetof(type, field)))
+
+
+#define k_sem ble_npl_sem
+
+static inline void k_sem_init(struct k_sem *sem, unsigned int initial_count,
+			      unsigned int limit)
+{
+	ble_npl_sem_init(sem, initial_count);
+}
+
+static inline int k_sem_take(struct k_sem *sem, s32_t timeout)
+{
+	uint32_t ticks;
+
+	ble_npl_time_ms_to_ticks(timeout, &ticks);
+	return - ble_npl_sem_pend(sem,  ticks);
+}
+
+static inline void k_sem_give(struct k_sem *sem)
+{
+	ble_npl_sem_release(sem);
+}
+
+/* Helpers to access the storage array, since we don't have access to its
+ * type at this point anymore.
+ */
+
+#define BUF_SIZE(pool) (pool->omp_pool->mp_block_size)
+
+static inline int net_buf_id(struct os_mbuf *buf)
+{
+	struct os_mbuf_pool *pool = buf->om_omp;
+	u8_t *pool_start = (u8_t *)pool->omp_pool->mp_membuf_addr;
+	u8_t *buf_ptr = (u8_t *)buf;
+
+	return (buf_ptr - pool_start) / BUF_SIZE(pool);
+}
+
+/* XXX: We should not use os_mbuf_pkthdr chains to represent a list of
+ * packets, this is a hack. For now this is not an issue, because mesh
+ * does not use os_mbuf chains. We should change this in the future.
+ */
+STAILQ_HEAD(net_buf_slist_t, os_mbuf_pkthdr);
+
+void net_buf_slist_init(struct net_buf_slist_t *list);
+bool net_buf_slist_is_empty(struct net_buf_slist_t *list);
+struct os_mbuf *net_buf_slist_peek_head(struct net_buf_slist_t *list);
+struct os_mbuf *net_buf_slist_peek_next(struct os_mbuf *buf);
+struct os_mbuf *net_buf_slist_get(struct net_buf_slist_t *list);
+void net_buf_slist_put(struct net_buf_slist_t *list, struct os_mbuf *buf);
+void net_buf_slist_remove(struct net_buf_slist_t *list, struct os_mbuf *prev,
+			  struct os_mbuf *cur);
+void net_buf_slist_merge_slist(struct net_buf_slist_t *list,
+			       struct net_buf_slist_t *list_to_append);
+#define NET_BUF_SLIST_FOR_EACH_NODE(head, var) STAILQ_FOREACH(var, head, omp_next)
+
+#if MYNEWT_VAL(BLE_MESH_SETTINGS)
+
+#define settings_load conf_load
+int settings_bytes_from_str(char *val_str, void *vp, int *len);
+char *settings_str_from_bytes(const void *vp, int vp_len,
+			      char *buf, int buf_len);
+
+#define snprintk snprintf
+#define BT_SETTINGS_SIZE(in_size) ((((((in_size) - 1) / 3) * 4) + 4) + 1)
+#define settings_save_one conf_save_one
+
+#else
+
+static inline int
+settings_load(void)
+{
+	return 0;
+}
+
+#endif /* MYNEWT_VAL(MYNEWT_VAL_BLE_MESH_SETTINGS) */
+
+#define BUILD_ASSERT(cond) _Static_assert(cond, "")
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MESH_GLUE_ */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/health_cli.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/health_cli.h
new file mode 100644
index 00000000..8ab8d6d5
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/health_cli.h
@@ -0,0 +1,81 @@
+/** @file
+ *  @brief Bluetooth Mesh Health Client Model APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_HEALTH_CLI_H
+#define __BT_MESH_HEALTH_CLI_H
+
+/**
+ * @brief Bluetooth Mesh
+ * @defgroup bt_mesh_health_cli Bluetooth Mesh Health Client Model
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Mesh Health Client Model Context */
+struct bt_mesh_health_cli {
+	struct bt_mesh_model *model;
+
+	void (*current_status)(struct bt_mesh_health_cli *cli, u16_t addr,
+			       u8_t test_id, u16_t cid, u8_t *faults,
+			       size_t fault_count);
+
+	struct k_sem          op_sync;
+	u32_t                 op_pending;
+	void                 *op_param;
+};
+
+extern const struct bt_mesh_model_op bt_mesh_health_cli_op[];
+extern const struct bt_mesh_model_cb bt_mesh_health_cli_cb;
+
+#define BT_MESH_MODEL_HEALTH_CLI(cli_data)                                     \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_HEALTH_CLI, bt_mesh_health_cli_op,   \
+			 NULL, cli_data, &bt_mesh_health_cli_cb)
+
+int bt_mesh_health_cli_set(struct bt_mesh_model *model);
+
+int bt_mesh_health_fault_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			     u16_t cid, u8_t *test_id, u8_t *faults,
+			     size_t *fault_count);
+
+int bt_mesh_health_fault_clear(u16_t net_idx, u16_t addr, u16_t app_idx,
+			       u16_t cid, u8_t *test_id, u8_t *faults,
+			       size_t *fault_count);
+
+int bt_mesh_health_fault_test(u16_t net_idx, u16_t addr, u16_t app_idx,
+			      u16_t cid, u8_t test_id, u8_t *faults,
+			      size_t *fault_count);
+
+int bt_mesh_health_period_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			      u8_t *divisor);
+
+int bt_mesh_health_period_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+			      u8_t divisor, u8_t *updated_divisor);
+
+int bt_mesh_health_attention_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+				 u8_t *attention);
+
+int bt_mesh_health_attention_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+				 u8_t attention, u8_t *updated_attention);
+
+s32_t bt_mesh_health_cli_timeout_get(void);
+void bt_mesh_health_cli_timeout_set(s32_t timeout);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_HEALTH_CLI_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/health_srv.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/health_srv.h
new file mode 100644
index 00000000..83982376
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/health_srv.h
@@ -0,0 +1,100 @@
+/** @file
+ *  @brief Bluetooth Mesh Health Server Model APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_HEALTH_SRV_H
+#define __BT_MESH_HEALTH_SRV_H
+
+/**
+ * @brief Mesh Bluetooth Mesh Health Server Model
+ * @defgroup bt_mesh_health_srv
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct bt_mesh_health_srv_cb {
+	/* Fetch current faults */
+	int (*fault_get_cur)(struct bt_mesh_model *model, u8_t *test_id,
+			     u16_t *company_id, u8_t *faults,
+			     u8_t *fault_count);
+
+	/* Fetch registered faults */
+	int (*fault_get_reg)(struct bt_mesh_model *model, u16_t company_id,
+			     u8_t *test_id, u8_t *faults,
+			     u8_t *fault_count);
+
+	/* Clear registered faults */
+	int (*fault_clear)(struct bt_mesh_model *model, u16_t company_id);
+
+	/* Run a specific test */
+	int (*fault_test)(struct bt_mesh_model *model, u8_t test_id,
+			  u16_t company_id);
+
+	/* Attention on */
+	void (*attn_on)(struct bt_mesh_model *model);
+
+	/* Attention off */
+	void (*attn_off)(struct bt_mesh_model *model);
+};
+
+/** @def BT_MESH_HEALTH_FAULT_MSG
+ *
+ *  A helper to define a health fault message.
+ *
+ *  @param max_faults Maximum number of faults the element can have.
+ *
+ *  @return a New net_buf_simple of the needed size.
+ */
+#define BT_MESH_HEALTH_FAULT_MSG(max_faults) \
+	NET_BUF_SIMPLE(1 + 3 + (max_faults))
+
+/** Mesh Health Server Model Context */
+struct bt_mesh_health_srv {
+	struct bt_mesh_model *model;
+
+	/* Optional callback struct */
+	const struct bt_mesh_health_srv_cb *cb;
+
+	/* Attention Timer state */
+	struct k_delayed_work attn_timer;
+};
+
+int bt_mesh_fault_update(struct bt_mesh_elem *elem);
+
+extern const struct bt_mesh_model_op bt_mesh_health_srv_op[];
+extern const struct bt_mesh_model_cb bt_mesh_health_srv_cb;
+
+/** @def BT_MESH_MODEL_HEALTH_SRV
+ *
+ *  Define a new health server model. Note that this API needs to be
+ *  repeated for each element that the application wants to have a
+ *  health server model on. Each instance also needs a unique
+ *  bt_mesh_health_srv and bt_mesh_model_pub context.
+ *
+ *  @param srv Pointer to a unique struct bt_mesh_health_srv.
+ *  @param pub Pointer to a unique struct bt_mesh_model_pub.
+ *
+ *  @return New mesh model instance.
+ */
+#define BT_MESH_MODEL_HEALTH_SRV(srv, pub)                                     \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_HEALTH_SRV, bt_mesh_health_srv_op,   \
+			 pub, srv, &bt_mesh_health_srv_cb)
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_HEALTH_SRV_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/main.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/main.h
new file mode 100644
index 00000000..4a5bedba
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/main.h
@@ -0,0 +1,441 @@
+/** @file
+ *  @brief Bluetooth Mesh Profile APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_MAIN_H
+#define __BT_MESH_MAIN_H
+
+/**
+ * @brief Bluetooth Mesh Provisioning
+ * @defgroup bt_mesh_prov Bluetooth Mesh Provisioning
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+	BT_MESH_NO_OUTPUT       = 0,
+	BT_MESH_BLINK           = BIT(0),
+	BT_MESH_BEEP            = BIT(1),
+	BT_MESH_VIBRATE         = BIT(2),
+	BT_MESH_DISPLAY_NUMBER  = BIT(3),
+	BT_MESH_DISPLAY_STRING  = BIT(4),
+} bt_mesh_output_action_t;
+
+typedef enum {
+	BT_MESH_NO_INPUT      = 0,
+	BT_MESH_PUSH          = BIT(0),
+	BT_MESH_TWIST         = BIT(1),
+	BT_MESH_ENTER_NUMBER  = BIT(2),
+	BT_MESH_ENTER_STRING  = BIT(3),
+} bt_mesh_input_action_t;
+
+typedef enum {
+	BT_MESH_PROV_ADV   = BIT(0),
+	BT_MESH_PROV_GATT  = BIT(1),
+} bt_mesh_prov_bearer_t;
+
+typedef enum {
+	BT_MESH_PROV_OOB_OTHER     = BIT(0),
+	BT_MESH_PROV_OOB_URI       = BIT(1),
+	BT_MESH_PROV_OOB_2D_CODE   = BIT(2),
+	BT_MESH_PROV_OOB_BAR_CODE  = BIT(3),
+	BT_MESH_PROV_OOB_NFC       = BIT(4),
+	BT_MESH_PROV_OOB_NUMBER    = BIT(5),
+	BT_MESH_PROV_OOB_STRING    = BIT(6),
+	/* 7 - 10 are reserved */
+	BT_MESH_PROV_OOB_ON_BOX    = BIT(11),
+	BT_MESH_PROV_OOB_IN_BOX    = BIT(12),
+	BT_MESH_PROV_OOB_ON_PAPER  = BIT(13),
+	BT_MESH_PROV_OOB_IN_MANUAL = BIT(14),
+	BT_MESH_PROV_OOB_ON_DEV    = BIT(15),
+} bt_mesh_prov_oob_info_t;
+
+/** Provisioning properties & capabilities. */
+struct bt_mesh_prov {
+	/** The UUID that's used when advertising as unprovisioned */
+	const u8_t *uuid;
+
+	/** Optional URI. This will be advertised separately from the
+	 *  unprovisioned beacon, however the unprovisioned beacon will
+	 *  contain a hash of it so the two can be associated by the
+	 *  provisioner.
+	 */
+	const char *uri;
+
+	/** Out of Band information field. */
+	bt_mesh_prov_oob_info_t oob_info;
+
+	/** Static OOB value */
+	const u8_t *static_val;
+	/** Static OOB value length */
+	u8_t        static_val_len;
+
+	/** Maximum size of Output OOB supported */
+	u8_t        output_size;
+	/** Supported Output OOB Actions */
+	u16_t       output_actions;
+
+	/* Maximum size of Input OOB supported */
+	u8_t        input_size;
+	/** Supported Input OOB Actions */
+	u16_t       input_actions;
+
+	/** @brief Output of a number is requested.
+	 *
+	 *  This callback notifies the application that it should
+	 *  output the given number using the given action.
+	 *
+	 *  @param act Action for outputting the number.
+	 *  @param num Number to be outputted.
+	 *
+	 *  @return Zero on success or negative error code otherwise
+	 */
+	int         (*output_number)(bt_mesh_output_action_t act, u32_t num);
+
+	/** @brief Output of a string is requested.
+	 *
+	 *  This callback notifies the application that it should
+	 *  display the given string to the user.
+	 *
+	 *  @param str String to be displayed.
+	 *
+	 *  @return Zero on success or negative error code otherwise
+	 */
+	int         (*output_string)(const char *str);
+
+	/** @brief Input is requested.
+	 *
+	 *  This callback notifies the application that it should
+	 *  request input from the user using the given action. The
+	 *  requested input will either be a string or a number, and
+	 *  the application needs to consequently call the
+	 *  bt_mesh_input_string() or bt_mesh_input_number() functions
+	 *  once the data has been acquired from the user.
+	 *
+	 *  @param act Action for inputting data.
+	 *  @param num Maximum size of the inputted data.
+	 *
+	 *  @return Zero on success or negative error code otherwise
+	 */
+	int         (*input)(bt_mesh_input_action_t act, u8_t size);
+
+	/** @brief The other device finished their OOB input.
+	 *
+	 * This callback notifies the application that it should stop
+	 * displaying its output OOB value, as the other party finished their
+	 * OOB input.
+	 */
+	void 	    (*input_complete)(void);
+
+	/** @brief Unprovisioned beacon has been received.
+	 *
+	 * This callback notifies the application that an unprovisioned
+	 * beacon has been received.
+	 *
+	 * @param uuid UUID
+	 * @param oob_info OOB Information
+	 * @param uri_hash Pointer to URI Hash value. NULL if no hash was
+	 *                 present in the beacon.
+	 */
+	void        (*unprovisioned_beacon)(u8_t uuid[16],
+					    bt_mesh_prov_oob_info_t oob_info,
+					    u32_t *uri_hash);
+
+	/** @brief Provisioning link has been opened.
+	 *
+	 *  This callback notifies the application that a provisioning
+	 *  link has been opened on the given provisioning bearer.
+	 *
+	 *  @param bearer Provisioning bearer.
+	 */
+	void        (*link_open)(bt_mesh_prov_bearer_t bearer);
+
+	/** @brief Provisioning link has been closed.
+	 *
+	 *  This callback notifies the application that a provisioning
+	 *  link has been closed on the given provisioning bearer.
+	 *
+	 *  @param bearer Provisioning bearer.
+	 */
+	void        (*link_close)(bt_mesh_prov_bearer_t bearer);
+
+	/** @brief Provisioning is complete.
+	 *
+	 *  This callback notifies the application that provisioning has
+	 *  been successfully completed, and that the local node has been
+	 *  assigned the specified NetKeyIndex and primary element address.
+	 *
+	 *  @param net_idx NetKeyIndex given during provisioning.
+	 *  @param addr Primary element address.
+	 */
+	void        (*complete)(u16_t net_idx, u16_t addr);
+
+	/** @brief A new node has been added to the provisioning database.
+	 *
+	 *  This callback notifies the application that provisioning has
+	 *  been successfully completed, and that a node has been assigned
+	 *  the specified NetKeyIndex and primary element address.
+	 *
+	 *  @param net_idx NetKeyIndex given during provisioning.
+	 *  @param addr Primary element address.
+	 *  @param num_elem Number of elements that this node has.
+	 */
+	void        (*node_added)(u16_t net_idx, u16_t addr, u8_t num_elem);
+
+	/** @brief Node has been reset.
+	 *
+	 *  This callback notifies the application that the local node
+	 *  has been reset and needs to be reprovisioned. The node will
+	 *  not automatically advertise as unprovisioned, rather the
+	 *  bt_mesh_prov_enable() API needs to be called to enable
+	 *  unprovisioned advertising on one or more provisioning bearers.
+	 */
+	void        (*reset)(void);
+};
+
+/** @brief Provide provisioning input OOB string.
+ *
+ *  This is intended to be called after the bt_mesh_prov input callback
+ *  has been called with BT_MESH_ENTER_STRING as the action.
+ *
+ *  @param str String.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_input_string(const char *str);
+
+/** @brief Provide provisioning input OOB number.
+ *
+ *  This is intended to be called after the bt_mesh_prov input callback
+ *  has been called with BT_MESH_ENTER_NUMBER as the action.
+ *
+ *  @param num Number.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_input_number(u32_t num);
+
+/** @brief Enable specific provisioning bearers
+ *
+ *  Enable one or more provisioning bearers.
+ *
+ *  @param bearers Bit-wise or of provisioning bearers.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_prov_enable(bt_mesh_prov_bearer_t bearers);
+
+/** @brief Disable specific provisioning bearers
+ *
+ *  Disable one or more provisioning bearers.
+ *
+ *  @param bearers Bit-wise or of provisioning bearers.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_prov_disable(bt_mesh_prov_bearer_t bearers);
+
+/**
+ * @}
+ */
+
+/**
+ * @brief Bluetooth Mesh
+ * @defgroup bt_mesh Bluetooth Mesh
+ * @ingroup bluetooth
+ * @{
+ */
+
+/* Primary Network Key index */
+#define BT_MESH_NET_PRIMARY                 0x000
+
+#define BT_MESH_RELAY_DISABLED              0x00
+#define BT_MESH_RELAY_ENABLED               0x01
+#define BT_MESH_RELAY_NOT_SUPPORTED         0x02
+
+#define BT_MESH_BEACON_DISABLED             0x00
+#define BT_MESH_BEACON_ENABLED              0x01
+
+#define BT_MESH_GATT_PROXY_DISABLED         0x00
+#define BT_MESH_GATT_PROXY_ENABLED          0x01
+#define BT_MESH_GATT_PROXY_NOT_SUPPORTED    0x02
+
+#define BT_MESH_FRIEND_DISABLED             0x00
+#define BT_MESH_FRIEND_ENABLED              0x01
+#define BT_MESH_FRIEND_NOT_SUPPORTED        0x02
+
+#define BT_MESH_NODE_IDENTITY_STOPPED       0x00
+#define BT_MESH_NODE_IDENTITY_RUNNING       0x01
+#define BT_MESH_NODE_IDENTITY_NOT_SUPPORTED 0x02
+
+/* Features */
+#define BT_MESH_FEAT_RELAY                  BIT(0)
+#define BT_MESH_FEAT_PROXY                  BIT(1)
+#define BT_MESH_FEAT_FRIEND                 BIT(2)
+#define BT_MESH_FEAT_LOW_POWER              BIT(3)
+#define BT_MESH_FEAT_SUPPORTED              (BT_MESH_FEAT_RELAY |   \
+					     BT_MESH_FEAT_PROXY |   \
+					     BT_MESH_FEAT_FRIEND |  \
+					     BT_MESH_FEAT_LOW_POWER)
+
+/** @brief Initialize Mesh support
+ *
+ *  After calling this API, the node will not automatically advertise as
+ *  unprovisioned, rather the bt_mesh_prov_enable() API needs to be called
+ *  to enable unprovisioned advertising on one or more provisioning bearers.
+ *
+ *  @param own_addr_type Node address type
+ *  @param prov Node provisioning information.
+ *  @param comp Node Composition.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_init(u8_t own_addr_type,
+		 const struct bt_mesh_prov *prov,
+		 const struct bt_mesh_comp *comp);
+
+/** @brief Reset the state of the local Mesh node.
+ *
+ *  Resets the state of the node, which means that it needs to be
+ *  reprovisioned to become an active node in a Mesh network again.
+ *
+ *  After calling this API, the node will not automatically advertise as
+ *  unprovisioned, rather the bt_mesh_prov_enable() API needs to be called
+ *  to enable unprovisioned advertising on one or more provisioning bearers.
+ *
+ */
+void bt_mesh_reset(void);
+
+/** @brief Suspend the Mesh network temporarily.
+ *
+ *  This API can be used for power saving purposes, but the user should be
+ *  aware that leaving the local node suspended for a long period of time
+ *  may cause it to become permanently disconnected from the Mesh network.
+ *  If at all possible, the Friendship feature should be used instead, to
+ *  make the node into a Low Power Node.
+ *
+ *  @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_suspend(void);
+
+/** @brief Resume a suspended Mesh network.
+ *
+ *  This API resumes the local node, after it has been suspended using the
+ *  bt_mesh_suspend() API.
+ *
+ *  @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_resume(void);
+
+/** @brief Provision the local Mesh Node.
+ *
+ *  This API should normally not be used directly by the application. The
+ *  only exception is for testing purposes where manual provisioning is
+ *  desired without an actual external provisioner.
+ *
+ *  @param net_key  Network Key
+ *  @param net_idx  Network Key Index
+ *  @param flags    Provisioning Flags
+ *  @param iv_index IV Index
+ *  @param addr     Primary element address
+ *  @param dev_key  Device Key
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_provision(const u8_t net_key[16], u16_t net_idx,
+		      u8_t flags, u32_t iv_index, u16_t addr,
+		      const u8_t dev_key[16]);
+
+/** @brief Provision a Mesh Node using PB-ADV
+ *
+ * @param uuid    UUID
+ * @param net_idx Network Key Index
+ * @param addr    Address to assign to remote device. If addr is 0, the lowest
+ *                available address will be chosen.
+ * @param attention_duration The attention duration to be send to remote device
+ *
+ * @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_provision_adv(const u8_t uuid[16], u16_t net_idx, u16_t addr,
+			  u8_t attention_duration);
+
+/** @brief Check if the local node has been provisioned.
+ *
+ *  This API can be used to check if the local node has been provisioned
+ *  or not. It can e.g. be helpful to determine if there was a stored
+ *  network in flash, i.e. if the network was restored after calling
+ *  settings_load().
+ *
+ *  @return True if the node is provisioned. False otherwise.
+ */
+bool bt_mesh_is_provisioned(void);
+
+/** @brief Toggle the IV Update test mode
+ *
+ *  This API is only available if the IV Update test mode has been enabled
+ *  in Kconfig. It is needed for passing most of the IV Update qualification
+ *  test cases.
+ *
+ *  @param enable true to enable IV Update test mode, false to disable it.
+ */
+void bt_mesh_iv_update_test(bool enable);
+
+/** @brief Toggle the IV Update state
+ *
+ *  This API is only available if the IV Update test mode has been enabled
+ *  in Kconfig. It is needed for passing most of the IV Update qualification
+ *  test cases.
+ *
+ *  @return true if IV Update In Progress state was entered, false otherwise.
+ */
+bool bt_mesh_iv_update(void);
+
+/** @brief Toggle the Low Power feature of the local device
+ *
+ *  Enables or disables the Low Power feature of the local device. This is
+ *  exposed as a run-time feature, since the device might want to change
+ *  this e.g. based on being plugged into a stable power source or running
+ *  from a battery power source.
+ *
+ *  @param enable  true to enable LPN functionality, false to disable it.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_lpn_set(bool enable);
+
+/** @brief Send out a Friend Poll message.
+ *
+ *  Send a Friend Poll message to the Friend of this node. If there is no
+ *  established Friendship the function will return an error.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_lpn_poll(void);
+
+/** @brief Register a callback for Friendship changes.
+ *
+ *  Registers a callback that will be called whenever Friendship gets
+ *  established or is lost.
+ *
+ *  @param cb Function to call when the Friendship status changes.
+ */
+void bt_mesh_lpn_set_cb(void (*cb)(u16_t friend_addr, bool established));
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_MAIN_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/mesh.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/mesh.h
new file mode 100644
index 00000000..9ba63ef0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/mesh.h
@@ -0,0 +1,26 @@
+/** @file
+ *  @brief Bluetooth Mesh Profile APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_H
+#define __BT_MESH_H
+
+#include <stddef.h>
+#include "syscfg/syscfg.h"
+#include "os/os_mbuf.h"
+
+#include "glue.h"
+#include "access.h"
+#include "main.h"
+#include "cfg_srv.h"
+#include "health_srv.h"
+#include "cfg_cli.h"
+#include "health_cli.h"
+#include "proxy.h"
+
+#endif /* __BT_MESH_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/model_cli.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/model_cli.h
new file mode 100644
index 00000000..f2e77a47
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/model_cli.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __MODEL_CLI_H__
+#define __MODEL_CLI_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct bt_mesh_gen_model_cli {
+    struct bt_mesh_model *model;
+
+    struct k_sem          op_sync;
+    u32_t                 op_pending;
+    void                 *op_param;
+};
+
+extern const struct bt_mesh_model_op gen_onoff_cli_op[];
+extern const struct bt_mesh_model_cb bt_mesh_gen_onoff_cli_cb;
+
+#define BT_MESH_MODEL_GEN_ONOFF_CLI(cli_data, pub)                             \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_GEN_ONOFF_CLI, gen_onoff_cli_op, pub,\
+			 cli_data, &bt_mesh_gen_onoff_cli_cb)
+
+extern const struct bt_mesh_model_op gen_level_cli_op[];
+extern const struct bt_mesh_model_cb bt_mesh_gen_level_cli_cb;
+
+#define BT_MESH_MODEL_GEN_LEVEL_CLI(cli_data, pub)                             \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_GEN_LEVEL_CLI, gen_level_cli_op, pub,\
+			 cli_data, &bt_mesh_gen_level_cli_cb)
+
+int bt_mesh_gen_onoff_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  u8_t *state);
+int bt_mesh_gen_onoff_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  u8_t val, u8_t *state);
+int bt_mesh_gen_level_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  s16_t *level);
+int bt_mesh_gen_level_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  s16_t val, s16_t *state);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MODEL_CLI_H__ */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/model_srv.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/model_srv.h
new file mode 100644
index 00000000..e498ad34
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/model_srv.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __MODEL_SRV_H__
+#define __MODEL_SRV_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct bt_mesh_gen_onoff_srv {
+    struct bt_mesh_model *model;
+
+    int (*get)(struct bt_mesh_model *model, u8_t *state);
+    int (*set)(struct bt_mesh_model *model, u8_t state);
+};
+
+extern const struct bt_mesh_model_op gen_onoff_srv_op[];
+extern const struct bt_mesh_model_cb gen_onoff_srv_cb;
+
+#define BT_MESH_MODEL_GEN_ONOFF_SRV(srv, pub)		\
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_GEN_ONOFF_SRV,	\
+			 gen_onoff_srv_op, pub, srv, &gen_onoff_srv_cb)
+
+struct bt_mesh_gen_level_srv {
+    struct bt_mesh_model *model;
+
+    int (*get)(struct bt_mesh_model *model, s16_t *level);
+    int (*set)(struct bt_mesh_model *model, s16_t level);
+};
+
+extern const struct bt_mesh_model_op gen_level_srv_op[];
+extern const struct bt_mesh_model_cb gen_level_srv_cb;
+
+#define BT_MESH_MODEL_GEN_LEVEL_SRV(srv, pub)		\
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_GEN_LEVEL_SRV,	\
+			 gen_level_srv_op, pub, srv, &gen_level_srv_cb)
+
+struct bt_mesh_light_lightness_srv {
+    struct bt_mesh_model *model;
+
+    int (*get)(struct bt_mesh_model *model, s16_t *level);
+    int (*set)(struct bt_mesh_model *model, s16_t level);
+};
+
+extern const struct bt_mesh_model_op light_lightness_srv_op[];
+extern const struct bt_mesh_model_cb light_lightness_srv_cb;
+
+#define BT_MESH_MODEL_LIGHT_LIGHTNESS_SRV(srv, pub)		\
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_SRV,	\
+			 light_lightness_srv_op, pub, srv, &light_lightness_srv_cb)
+
+void bt_mesh_set_gen_onoff_srv_cb(int (*get)(struct bt_mesh_model *model, u8_t *state),
+				  int (*set)(struct bt_mesh_model *model, u8_t state));
+void bt_mesh_set_gen_level_srv_cb(int (*get)(struct bt_mesh_model *model, s16_t *level),
+				  int (*set)(struct bt_mesh_model *model, s16_t level));
+void bt_mesh_set_light_lightness_srv_cb(int (*get)(struct bt_mesh_model *model, s16_t *level),
+					int (*set)(struct bt_mesh_model *model, s16_t level));
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MODEL_SRV_H__ */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/porting.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/porting.h
new file mode 100644
index 00000000..1667a8a0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/porting.h
@@ -0,0 +1,27 @@
+/** @file
+ *  @brief Bluetooth Mesh Porting APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_PORTING_H
+#define __BT_MESH_PORTING_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void mesh_adv_thread(void *args);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_PORTING_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/proxy.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/proxy.h
new file mode 100644
index 00000000..63bbfa3e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/proxy.h
@@ -0,0 +1,43 @@
+/** @file
+ *  @brief Bluetooth Mesh Proxy APIs.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_PROXY_H
+#define __BT_MESH_PROXY_H
+
+/**
+ * @brief Bluetooth Mesh Proxy
+ * @defgroup bt_mesh_proxy Bluetooth Mesh Proxy
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Enable advertising with Node Identity.
+ *
+ * This API requires that GATT Proxy support has been enabled. Once called
+ * each subnet will start advertising using Node Identity for the next
+ * 60 seconds.
+ *
+ * @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_proxy_identity_enable(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* __BT_MESH_PROXY_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/slist.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/slist.h
new file mode 100644
index 00000000..8a858f83
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/slist.h
@@ -0,0 +1,468 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+/**
+ * @file
+ *
+ * @brief Single-linked list implementation
+ *
+ * Single-linked list implementation using inline macros/functions.
+ * This API is not thread safe, and thus if a list is used across threads,
+ * calls to functions must be protected with synchronization primitives.
+ */
+
+#ifndef __SLIST_H__
+#define __SLIST_H__
+
+#include <stddef.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+struct _snode {
+    struct _snode *next;
+};
+
+typedef struct _snode sys_snode_t;
+
+struct _slist {
+    sys_snode_t *head;
+    sys_snode_t *tail;
+};
+
+typedef struct _slist sys_slist_t;
+
+/**
+ * @brief Provide the primitive to iterate on a list
+ * Note: the loop is unsafe and thus __sn should not be removed
+ *
+ * User _MUST_ add the loop statement curly braces enclosing its own code:
+ *
+ *     SYS_SLIST_FOR_EACH_NODE(l, n) {
+ *         <user code>
+ *     }
+ *
+ * This and other SYS_SLIST_*() macros are not thread safe.
+ *
+ * @param __sl A pointer on a sys_slist_t to iterate on
+ * @param __sn A sys_snode_t pointer to peek each node of the list
+ */
+#define SYS_SLIST_FOR_EACH_NODE(__sl, __sn)             \
+    for (__sn = sys_slist_peek_head(__sl); __sn;            \
+         __sn = sys_slist_peek_next(__sn))
+
+/**
+ * @brief Provide the primitive to iterate on a list, from a node in the list
+ * Note: the loop is unsafe and thus __sn should not be removed
+ *
+ * User _MUST_ add the loop statement curly braces enclosing its own code:
+ *
+ *     SYS_SLIST_ITERATE_FROM_NODE(l, n) {
+ *         <user code>
+ *     }
+ *
+ * Like SYS_SLIST_FOR_EACH_NODE(), but __dn already contains a node in the list
+ * where to start searching for the next entry from. If NULL, it starts from
+ * the head.
+ *
+ * This and other SYS_SLIST_*() macros are not thread safe.
+ *
+ * @param __sl A pointer on a sys_slist_t to iterate on
+ * @param __sn A sys_snode_t pointer to peek each node of the list
+ *             it contains the starting node, or NULL to start from the head
+ */
+#define SYS_SLIST_ITERATE_FROM_NODE(__sl, __sn)             \
+    for (__sn = __sn ? sys_slist_peek_next_no_check(__sn)       \
+             : sys_slist_peek_head(__sl);           \
+         __sn;                          \
+         __sn = sys_slist_peek_next(__sn))
+
+/**
+ * @brief Provide the primitive to safely iterate on a list
+ * Note: __sn can be removed, it will not break the loop.
+ *
+ * User _MUST_ add the loop statement curly braces enclosing its own code:
+ *
+ *     SYS_SLIST_FOR_EACH_NODE_SAFE(l, n, s) {
+ *         <user code>
+ *     }
+ *
+ * This and other SYS_SLIST_*() macros are not thread safe.
+ *
+ * @param __sl A pointer on a sys_slist_t to iterate on
+ * @param __sn A sys_snode_t pointer to peek each node of the list
+ * @param __sns A sys_snode_t pointer for the loop to run safely
+ */
+#define SYS_SLIST_FOR_EACH_NODE_SAFE(__sl, __sn, __sns)         \
+    for (__sn = sys_slist_peek_head(__sl),              \
+             __sns = sys_slist_peek_next(__sn);         \
+         __sn; __sn = __sns,                    \
+             __sns = sys_slist_peek_next(__sn))
+
+/*
+ * @brief Provide the primitive to resolve the container of a list node
+ * Note: it is safe to use with NULL pointer nodes
+ *
+ * @param __ln A pointer on a sys_node_t to get its container
+ * @param __cn Container struct type pointer
+ * @param __n The field name of sys_node_t within the container struct
+ */
+#define SYS_SLIST_CONTAINER(__ln, __cn, __n) \
+    ((__ln) ? CONTAINER_OF((__ln), __typeof__(*(__cn)), __n) : NULL)
+/*
+ * @brief Provide the primitive to peek container of the list head
+ *
+ * @param __sl A pointer on a sys_slist_t to peek
+ * @param __cn Container struct type pointer
+ * @param __n The field name of sys_node_t within the container struct
+ */
+#define SYS_SLIST_PEEK_HEAD_CONTAINER(__sl, __cn, __n) \
+    SYS_SLIST_CONTAINER(sys_slist_peek_head(__sl), __cn, __n)
+
+/*
+ * @brief Provide the primitive to peek container of the list tail
+ *
+ * @param __sl A pointer on a sys_slist_t to peek
+ * @param __cn Container struct type pointer
+ * @param __n The field name of sys_node_t within the container struct
+ */
+#define SYS_SLIST_PEEK_TAIL_CONTAINER(__sl, __cn, __n) \
+    SYS_SLIST_CONTAINER(sys_slist_peek_tail(__sl), __cn, __n)
+
+/*
+ * @brief Provide the primitive to peek the next container
+ *
+ * @param __cn Container struct type pointer
+ * @param __n The field name of sys_node_t within the container struct
+ */
+
+#define SYS_SLIST_PEEK_NEXT_CONTAINER(__cn, __n) \
+    ((__cn) ? SYS_SLIST_CONTAINER(sys_slist_peek_next(&((__cn)->__n)), \
+                      __cn, __n) : NULL)
+
+/**
+ * @brief Provide the primitive to iterate on a list under a container
+ * Note: the loop is unsafe and thus __cn should not be detached
+ *
+ * User _MUST_ add the loop statement curly braces enclosing its own code:
+ *
+ *     SYS_SLIST_FOR_EACH_CONTAINER(l, c, n) {
+ *         <user code>
+ *     }
+ *
+ * @param __sl A pointer on a sys_slist_t to iterate on
+ * @param __cn A pointer to peek each entry of the list
+ * @param __n The field name of sys_node_t within the container struct
+ */
+#define SYS_SLIST_FOR_EACH_CONTAINER(__sl, __cn, __n)           \
+    for (__cn = SYS_SLIST_PEEK_HEAD_CONTAINER(__sl, __cn, __n); __cn; \
+         __cn = SYS_SLIST_PEEK_NEXT_CONTAINER(__cn, __n))
+
+/**
+ * @brief Provide the primitive to safely iterate on a list under a container
+ * Note: __cn can be detached, it will not break the loop.
+ *
+ * User _MUST_ add the loop statement curly braces enclosing its own code:
+ *
+ *     SYS_SLIST_FOR_EACH_NODE_SAFE(l, c, cn, n) {
+ *         <user code>
+ *     }
+ *
+ * @param __sl A pointer on a sys_slist_t to iterate on
+ * @param __cn A pointer to peek each entry of the list
+ * @param __cns A pointer for the loop to run safely
+ * @param __n The field name of sys_node_t within the container struct
+ */
+#define SYS_SLIST_FOR_EACH_CONTAINER_SAFE(__sl, __cn, __cns, __n)   \
+    for (__cn = SYS_SLIST_PEEK_HEAD_CONTAINER(__sl, __cn, __n), \
+         __cns = SYS_SLIST_PEEK_NEXT_CONTAINER(__cn, __n); __cn;    \
+         __cn = __cns, __cns = SYS_SLIST_PEEK_NEXT_CONTAINER(__cn, __n))
+
+/**
+ * @brief Initialize a list
+ *
+ * @param list A pointer on the list to initialize
+ */
+static inline void sys_slist_init(sys_slist_t *list)
+{
+    list->head = NULL;
+    list->tail = NULL;
+}
+
+#define SYS_SLIST_STATIC_INIT(ptr_to_list) {NULL, NULL}
+
+/**
+ * @brief Test if the given list is empty
+ *
+ * @param list A pointer on the list to test
+ *
+ * @return a boolean, true if it's empty, false otherwise
+ */
+static inline bool sys_slist_is_empty(sys_slist_t *list)
+{
+    return (!list->head);
+}
+
+/**
+ * @brief Peek the first node from the list
+ *
+ * @param list A point on the list to peek the first node from
+ *
+ * @return A pointer on the first node of the list (or NULL if none)
+ */
+static inline sys_snode_t *sys_slist_peek_head(sys_slist_t *list)
+{
+    return list->head;
+}
+
+/**
+ * @brief Peek the last node from the list
+ *
+ * @param list A point on the list to peek the last node from
+ *
+ * @return A pointer on the last node of the list (or NULL if none)
+ */
+static inline sys_snode_t *sys_slist_peek_tail(sys_slist_t *list)
+{
+    return list->tail;
+}
+
+/**
+ * @brief Peek the next node from current node, node is not NULL
+ *
+ * Faster then sys_slist_peek_next() if node is known not to be NULL.
+ *
+ * @param node A pointer on the node where to peek the next node
+ *
+ * @return a pointer on the next node (or NULL if none)
+ */
+static inline sys_snode_t *sys_slist_peek_next_no_check(sys_snode_t *node)
+{
+    return node->next;
+}
+
+/**
+ * @brief Peek the next node from current node
+ *
+ * @param node A pointer on the node where to peek the next node
+ *
+ * @return a pointer on the next node (or NULL if none)
+ */
+static inline sys_snode_t *sys_slist_peek_next(sys_snode_t *node)
+{
+    return node ? sys_slist_peek_next_no_check(node) : NULL;
+}
+
+/**
+ * @brief Prepend a node to the given list
+ *
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param node A pointer on the node to prepend
+ */
+static inline void sys_slist_prepend(sys_slist_t *list,
+                     sys_snode_t *node)
+{
+    node->next = list->head;
+    list->head = node;
+
+    if (!list->tail) {
+        list->tail = list->head;
+    }
+}
+
+/**
+ * @brief Append a node to the given list
+ *
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param node A pointer on the node to append
+ */
+static inline void sys_slist_append(sys_slist_t *list,
+                    sys_snode_t *node)
+{
+    node->next = NULL;
+
+    if (!list->tail) {
+        list->tail = node;
+        list->head = node;
+    } else {
+        list->tail->next = node;
+        list->tail = node;
+    }
+}
+
+/**
+ * @brief Append a list to the given list
+ *
+ * Append a singly-linked, NULL-terminated list consisting of nodes containing
+ * the pointer to the next node as the first element of a node, to @a list.
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param head A pointer to the first element of the list to append
+ * @param tail A pointer to the last element of the list to append
+ */
+static inline void sys_slist_append_list(sys_slist_t *list,
+                     void *head, void *tail)
+{
+    if (!list->tail) {
+        list->head = (sys_snode_t *)head;
+        list->tail = (sys_snode_t *)tail;
+    } else {
+        list->tail->next = (sys_snode_t *)head;
+        list->tail = (sys_snode_t *)tail;
+    }
+}
+
+/**
+ * @brief merge two slists, appending the second one to the first
+ *
+ * When the operation is completed, the appending list is empty.
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param list_to_append A pointer to the list to append.
+ */
+static inline void sys_slist_merge_slist(sys_slist_t *list,
+                     sys_slist_t *list_to_append)
+{
+    sys_slist_append_list(list, list_to_append->head,
+                    list_to_append->tail);
+    sys_slist_init(list_to_append);
+}
+
+/**
+ * @brief Insert a node to the given list
+ *
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param prev A pointer on the previous node
+ * @param node A pointer on the node to insert
+ */
+static inline void sys_slist_insert(sys_slist_t *list,
+                    sys_snode_t *prev,
+                    sys_snode_t *node)
+{
+    if (!prev) {
+        sys_slist_prepend(list, node);
+    } else if (!prev->next) {
+        sys_slist_append(list, node);
+    } else {
+        node->next = prev->next;
+        prev->next = node;
+    }
+}
+
+/**
+ * @brief Fetch and remove the first node of the given list
+ *
+ * List must be known to be non-empty.
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ *
+ * @return A pointer to the first node of the list
+ */
+static inline sys_snode_t *sys_slist_get_not_empty(sys_slist_t *list)
+{
+    sys_snode_t *node = list->head;
+
+    list->head = node->next;
+    if (list->tail == node) {
+        list->tail = list->head;
+    }
+
+    return node;
+}
+
+/**
+ * @brief Fetch and remove the first node of the given list
+ *
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ *
+ * @return A pointer to the first node of the list (or NULL if empty)
+ */
+static inline sys_snode_t *sys_slist_get(sys_slist_t *list)
+{
+    return sys_slist_is_empty(list) ? NULL : sys_slist_get_not_empty(list);
+}
+
+/**
+ * @brief Remove a node
+ *
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param prev_node A pointer on the previous node
+ *        (can be NULL, which means the node is the list's head)
+ * @param node A pointer on the node to remove
+ */
+static inline void sys_slist_remove(sys_slist_t *list,
+                    sys_snode_t *prev_node,
+                    sys_snode_t *node)
+{
+    if (!prev_node) {
+        list->head = node->next;
+
+        /* Was node also the tail? */
+        if (list->tail == node) {
+            list->tail = list->head;
+        }
+    } else {
+        prev_node->next = node->next;
+
+        /* Was node the tail? */
+        if (list->tail == node) {
+            list->tail = prev_node;
+        }
+    }
+
+    node->next = NULL;
+}
+
+/**
+ * @brief Find and remove a node from a list
+ *
+ * This and other sys_slist_*() functions are not thread safe.
+ *
+ * @param list A pointer on the list to affect
+ * @param node A pointer on the node to remove from the list
+ *
+ * @return true if node was removed
+ */
+static inline bool sys_slist_find_and_remove(sys_slist_t *list,
+                         sys_snode_t *node)
+{
+    sys_snode_t *prev = NULL;
+    sys_snode_t *test;
+
+    SYS_SLIST_FOR_EACH_NODE(list, test) {
+        if (test == node) {
+            sys_slist_remove(list, prev, node);
+            return true;
+        }
+
+        prev = test;
+    }
+
+    return false;
+}
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __SLIST_H__ */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/testing.h b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/testing.h
new file mode 100644
index 00000000..4c2b2a61
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/include/mesh/testing.h
@@ -0,0 +1,105 @@
+/**
+ * @file testing.h
+ * @brief Internal API for Bluetooth testing.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __BT_TESTING_H
+#define __BT_TESTING_H
+
+#include "slist.h"
+#include "glue.h"
+#include "access.h"
+
+/**
+ * @brief Bluetooth testing
+ * @defgroup bt_test_cb Bluetooth testing callbacks
+ * @ingroup bluetooth
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Bluetooth Testing callbacks structure.
+ *
+ *  Callback structure to be used for Bluetooth testing purposes.
+ *  Allows access to Bluetooth stack internals, not exposed by public API.
+ */
+struct bt_test_cb {
+	void (*mesh_net_recv)(u8_t ttl, u8_t ctl, u16_t src, u16_t dst,
+			      const void *payload, size_t payload_len);
+	void (*mesh_model_bound)(u16_t addr, struct bt_mesh_model *model,
+				 u16_t key_idx);
+	void (*mesh_model_unbound)(u16_t addr, struct bt_mesh_model *model,
+				   u16_t key_idx);
+	void (*mesh_prov_invalid_bearer)(u8_t opcode);
+	void (*mesh_trans_incomp_timer_exp)(void);
+
+	sys_snode_t node;
+};
+
+/** Register callbacks for Bluetooth testing purposes
+ *
+ *  @param cb bt_test_cb callback structure
+ */
+void bt_test_cb_register(struct bt_test_cb *cb);
+
+/** Unregister callbacks for Bluetooth testing purposes
+ *
+ *  @param cb bt_test_cb callback structure
+ */
+void bt_test_cb_unregister(struct bt_test_cb *cb);
+
+/** Send Friend Subscription List Add message.
+ *
+ *  Used by Low Power node to send the group address for which messages are to
+ *  be stored by Friend node.
+ *
+ *  @param group Group address
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_test_mesh_lpn_group_add(u16_t group);
+
+/** Send Friend Subscription List Remove message.
+ *
+ *  Used by Low Power node to remove the group addresses from Friend node
+ *  subscription list. Messages sent to those addresses will not be stored
+ *  by Friend node.
+ *
+ *  @param groups Group addresses
+ *  @param groups_count Group addresses count
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_test_mesh_lpn_group_remove(u16_t *groups, size_t groups_count);
+
+/** Clear replay protection list cache.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_test_mesh_rpl_clear(void);
+
+u8_t mod_bind(struct bt_mesh_model *model, u16_t key_idx);
+u8_t mod_unbind(struct bt_mesh_model *model, u16_t key_idx, bool store);
+int cmd_mesh_init(int argc, char *argv[]);
+
+int bt_test_shell_init(void);
+int bt_test_bind_app_key_to_model(struct bt_mesh_model *model, u16_t key_idx, u16_t id);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_TESTING_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/pkg.yml b/src/libs/mynewt-nimble/nimble/host/mesh/pkg.yml
new file mode 100644
index 00000000..44cc0c73
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/pkg.yml
@@ -0,0 +1,49 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/mesh
+pkg.description: Bluetooth Mesh
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - mesh
+
+pkg.deps:
+    - "@apache-mynewt-core/kernel/os"
+    - "@apache-mynewt-core/util/mem"
+    - "@apache-mynewt-core/crypto/tinycrypt"
+    - nimble
+    - nimble/host
+
+pkg.deps.BLE_MESH_SHELL:
+    - "@apache-mynewt-core/sys/shell"
+
+pkg.deps.BLE_MESH_SETTINGS:
+    - "@apache-mynewt-core/encoding/base64"
+    - "@apache-mynewt-core/sys/config"
+
+pkg.req_apis:
+    - log
+    - stats
+
+pkg.init:
+    bt_mesh_register_gatt: 'MYNEWT_VAL(BLE_MESH_SYSINIT_STAGE)'
+    ble_mesh_shell_init: 'MYNEWT_VAL(BLE_MESH_SYSINIT_STAGE_SHELL)'
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/access.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/access.c
new file mode 100644
index 00000000..ff8e9999
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/access.c
@@ -0,0 +1,856 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_ACCESS_LOG
+
+#include <errno.h>
+#include <os/os_mbuf.h>
+
+#include "mesh/mesh.h"
+
+#include "mesh_priv.h"
+#include "adv.h"
+#include "net.h"
+#include "lpn.h"
+#include "transport.h"
+#include "access.h"
+#include "foundation.h"
+#if MYNEWT_VAL(BLE_MESH_SHELL_MODELS)
+#include "mesh/model_cli.h"
+#endif
+
+static const struct bt_mesh_comp *dev_comp;
+static u16_t dev_primary_addr;
+
+void bt_mesh_model_foreach(void (*func)(struct bt_mesh_model *mod,
+					struct bt_mesh_elem *elem,
+					bool vnd, bool primary,
+					void *user_data),
+			   void *user_data)
+{
+	int i, j;
+
+	for (i = 0; i < dev_comp->elem_count; i++) {
+		struct bt_mesh_elem *elem = &dev_comp->elem[i];
+
+		for (j = 0; j < elem->model_count; j++) {
+			struct bt_mesh_model *model = &elem->models[j];
+
+			func(model, elem, false, i == 0, user_data);
+		}
+
+		for (j = 0; j < elem->vnd_model_count; j++) {
+			struct bt_mesh_model *model = &elem->vnd_models[j];
+
+			func(model, elem, true, i == 0, user_data);
+		}
+	}
+}
+
+s32_t bt_mesh_model_pub_period_get(struct bt_mesh_model *mod)
+{
+	int period;
+
+	if (!mod->pub) {
+		return 0;
+	}
+
+	switch (mod->pub->period >> 6) {
+	case 0x00:
+		/* 1 step is 100 ms */
+		period = K_MSEC((mod->pub->period & BIT_MASK(6)) * 100);
+		break;
+	case 0x01:
+		/* 1 step is 1 second */
+		period = K_SECONDS(mod->pub->period & BIT_MASK(6));
+		break;
+	case 0x02:
+		/* 1 step is 10 seconds */
+		period = K_SECONDS((mod->pub->period & BIT_MASK(6)) * 10);
+		break;
+	case 0x03:
+		/* 1 step is 10 minutes */
+		period = K_MINUTES((mod->pub->period & BIT_MASK(6)) * 10);
+		break;
+	default:
+		CODE_UNREACHABLE;
+	}
+
+	if (mod->pub->fast_period) {
+		return period >> mod->pub->period_div;
+	} else {
+		return period;
+	}
+}
+
+static s32_t next_period(struct bt_mesh_model *mod)
+{
+	struct bt_mesh_model_pub *pub = mod->pub;
+	u32_t elapsed, period;
+
+	period = bt_mesh_model_pub_period_get(mod);
+	if (!period) {
+		return 0;
+	}
+
+	elapsed = k_uptime_get_32() - pub->period_start;
+
+	BT_DBG("Publishing took %ums", (unsigned) elapsed);
+
+	if (elapsed > period) {
+		BT_WARN("Publication sending took longer than the period");
+		/* Return smallest positive number since 0 means disabled */
+		return K_MSEC(1);
+	}
+
+	return period - elapsed;
+}
+
+static void publish_sent(int err, void *user_data)
+{
+	struct bt_mesh_model *mod = user_data;
+	s32_t delay;
+
+	BT_DBG("err %d", err);
+
+	if (mod->pub->count) {
+		delay = BT_MESH_PUB_TRANSMIT_INT(mod->pub->retransmit);
+	} else {
+		delay = next_period(mod);
+	}
+
+	if (delay) {
+		BT_DBG("Publishing next time in %dms", (int) delay);
+		k_delayed_work_submit(&mod->pub->timer, delay);
+	}
+}
+
+static void publish_start(u16_t duration, int err, void *user_data)
+{
+	struct bt_mesh_model *mod = user_data;
+	struct bt_mesh_model_pub *pub = mod->pub;
+
+	if (err) {
+		BT_ERR("Failed to publish: err %d", err);
+		return;
+	}
+
+	/* Initialize the timestamp for the beginning of a new period */
+	if (pub->count == BT_MESH_PUB_TRANSMIT_COUNT(pub->retransmit)) {
+		pub->period_start = k_uptime_get_32();
+	}
+}
+
+static const struct bt_mesh_send_cb pub_sent_cb = {
+	.start = publish_start,
+	.end = publish_sent,
+};
+
+static int publish_retransmit(struct bt_mesh_model *mod)
+{
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	struct bt_mesh_model_pub *pub = mod->pub;
+	struct bt_mesh_app_key *key;
+	struct bt_mesh_msg_ctx ctx = {
+		.addr = pub->addr,
+		.send_ttl = pub->ttl,
+	};
+	struct bt_mesh_net_tx tx = {
+		.ctx = &ctx,
+		.src = bt_mesh_model_elem(mod)->addr,
+		.xmit = bt_mesh_net_transmit_get(),
+		.friend_cred = pub->cred,
+	};
+	int err;
+
+	key = bt_mesh_app_key_find(pub->key);
+	if (!key) {
+		err = -EADDRNOTAVAIL;
+		goto done;
+	}
+
+	tx.sub = bt_mesh_subnet_get(key->net_idx);
+
+	ctx.net_idx = key->net_idx;
+	ctx.app_idx = key->app_idx;
+
+	net_buf_simple_init(sdu, 0);
+	net_buf_simple_add_mem(sdu, pub->msg->om_data, pub->msg->om_len);
+
+	pub->count--;
+
+	err = bt_mesh_trans_send(&tx, sdu, &pub_sent_cb, mod);
+
+done:
+	os_mbuf_free_chain(sdu);
+	return err;
+}
+
+static void mod_publish(struct ble_npl_event *work)
+{
+	struct bt_mesh_model_pub *pub = ble_npl_event_get_arg(work);
+	s32_t period_ms;
+	int err;
+
+	BT_DBG("");
+
+	period_ms = bt_mesh_model_pub_period_get(pub->mod);
+	BT_DBG("period %u ms", (unsigned) period_ms);
+
+	if (pub->count) {
+		err = publish_retransmit(pub->mod);
+		if (err) {
+			BT_ERR("Failed to retransmit (err %d)", err);
+
+			pub->count = 0;
+
+			/* Continue with normal publication */
+			if (period_ms) {
+				k_delayed_work_submit(&pub->timer, period_ms);
+			}
+		}
+
+		return;
+	}
+
+	if (!period_ms) {
+		return;
+	}
+
+	__ASSERT_NO_MSG(pub->update != NULL);
+
+	err = pub->update(pub->mod);
+	if (err) {
+		BT_ERR("Failed to update publication message");
+		return;
+	}
+
+	err = bt_mesh_model_publish(pub->mod);
+	if (err) {
+		BT_ERR("Publishing failed (err %d)", err);
+	}
+}
+
+struct bt_mesh_elem *bt_mesh_model_elem(struct bt_mesh_model *mod)
+{
+	return &dev_comp->elem[mod->elem_idx];
+}
+
+struct bt_mesh_model *bt_mesh_model_get(bool vnd, u8_t elem_idx, u8_t mod_idx)
+{
+	struct bt_mesh_elem *elem;
+
+	if (elem_idx >= dev_comp->elem_count) {
+		BT_ERR("Invalid element index %u", elem_idx);
+		return NULL;
+	}
+
+	elem = &dev_comp->elem[elem_idx];
+
+	if (vnd) {
+		if (mod_idx >= elem->vnd_model_count) {
+			BT_ERR("Invalid vendor model index %u", mod_idx);
+			return NULL;
+		}
+
+		return &elem->vnd_models[mod_idx];
+	} else {
+		if (mod_idx >= elem->model_count) {
+			BT_ERR("Invalid SIG model index %u", mod_idx);
+			return NULL;
+		}
+
+		return &elem->models[mod_idx];
+	}
+}
+
+static void mod_init(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
+		     bool vnd, bool primary, void *user_data)
+{
+	int i;
+
+	if (mod->pub) {
+		mod->pub->mod = mod;
+		k_delayed_work_init(&mod->pub->timer, mod_publish);
+		k_delayed_work_add_arg(&mod->pub->timer, mod->pub);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mod->keys); i++) {
+		mod->keys[i] = BT_MESH_KEY_UNUSED;
+	}
+
+	mod->elem_idx = elem - dev_comp->elem;
+	if (vnd) {
+		mod->mod_idx = mod - elem->vnd_models;
+	} else {
+		mod->mod_idx = mod - elem->models;
+	}
+
+	if (mod->cb && mod->cb->init) {
+		mod->cb->init(mod);
+	}
+}
+
+int bt_mesh_comp_register(const struct bt_mesh_comp *comp)
+{
+	/* There must be at least one element */
+	if (!comp->elem_count) {
+		return -EINVAL;
+	}
+
+	dev_comp = comp;
+
+	bt_mesh_model_foreach(mod_init, NULL);
+
+	return 0;
+}
+
+void bt_mesh_comp_provision(u16_t addr)
+{
+	int i;
+
+	dev_primary_addr = addr;
+
+	BT_DBG("addr 0x%04x elem_count %zu", addr, dev_comp->elem_count);
+
+	for (i = 0; i < dev_comp->elem_count; i++) {
+		struct bt_mesh_elem *elem = &dev_comp->elem[i];
+
+		elem->addr = addr++;
+
+		BT_DBG("addr 0x%04x mod_count %u vnd_mod_count %u",
+		       elem->addr, elem->model_count, elem->vnd_model_count);
+	}
+}
+
+void bt_mesh_comp_unprovision(void)
+{
+	BT_DBG("");
+
+	dev_primary_addr = BT_MESH_ADDR_UNASSIGNED;
+
+	bt_mesh_model_foreach(mod_init, NULL);
+}
+
+u16_t bt_mesh_primary_addr(void)
+{
+	return dev_primary_addr;
+}
+
+static u16_t *model_group_get(struct bt_mesh_model *mod, u16_t addr)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
+		if (mod->groups[i] == addr) {
+			return &mod->groups[i];
+		}
+	}
+
+	return NULL;
+}
+
+struct find_group_visitor_ctx {
+	u16_t *entry;
+	struct bt_mesh_model *mod;
+	u16_t addr;
+};
+
+static enum bt_mesh_walk find_group_mod_visitor(struct bt_mesh_model *mod,
+						u32_t depth, void *user_data)
+{
+	struct find_group_visitor_ctx *ctx = user_data;
+
+	if (mod->elem_idx != ctx->mod->elem_idx) {
+		return BT_MESH_WALK_CONTINUE;
+	}
+
+	ctx->entry = model_group_get(mod, ctx->addr);
+	if (ctx->entry) {
+		ctx->mod = mod;
+		return BT_MESH_WALK_STOP;
+	}
+
+	return BT_MESH_WALK_CONTINUE;
+}
+
+u16_t *bt_mesh_model_find_group(struct bt_mesh_model **mod, u16_t addr)
+{
+	struct find_group_visitor_ctx ctx = {
+		.mod = *mod,
+		.entry = NULL,
+		.addr = addr,
+	};
+
+	bt_mesh_model_tree_walk(bt_mesh_model_root(*mod),
+				find_group_mod_visitor, &ctx);
+
+	*mod = ctx.mod;
+	return ctx.entry;
+}
+
+static struct bt_mesh_model *bt_mesh_elem_find_group(struct bt_mesh_elem *elem,
+						     u16_t group_addr)
+{
+	struct bt_mesh_model *model;
+	u16_t *match;
+	int i;
+
+	for (i = 0; i < elem->model_count; i++) {
+		model = &elem->models[i];
+
+		match = model_group_get(model, group_addr);
+		if (match) {
+			return model;
+		}
+	}
+
+	for (i = 0; i < elem->vnd_model_count; i++) {
+		model = &elem->vnd_models[i];
+
+		match = model_group_get(model, group_addr);
+		if (match) {
+			return model;
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_mesh_elem *bt_mesh_elem_find(u16_t addr)
+{
+	u16_t index;
+
+	if (BT_MESH_ADDR_IS_UNICAST(addr)) {
+		index = (addr - dev_comp->elem[0].addr);
+		if (index < dev_comp->elem_count) {
+			return &dev_comp->elem[index];
+		} else {
+			return NULL;
+		}
+	}
+
+	for (index = 0; index < dev_comp->elem_count; index++) {
+		struct bt_mesh_elem *elem = &dev_comp->elem[index];
+
+		if (bt_mesh_elem_find_group(elem, addr)) {
+			return elem;
+		}
+	}
+
+	return NULL;
+}
+
+u8_t bt_mesh_elem_count(void)
+{
+	return dev_comp->elem_count;
+}
+
+static bool model_has_key(struct bt_mesh_model *mod, u16_t key)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mod->keys); i++) {
+		if (mod->keys[i] == key ||
+		    (mod->keys[i] == BT_MESH_KEY_DEV_ANY &&
+		     BT_MESH_IS_DEV_KEY(key))) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static bool model_has_dst(struct bt_mesh_model *mod, u16_t dst)
+{
+	if (BT_MESH_ADDR_IS_UNICAST(dst)) {
+		return (dev_comp->elem[mod->elem_idx].addr == dst);
+	} else if (BT_MESH_ADDR_IS_GROUP(dst) || BT_MESH_ADDR_IS_VIRTUAL(dst)) {
+		return bt_mesh_model_find_group(&mod, dst);
+	}
+
+	return (mod->elem_idx == 0 && bt_mesh_fixed_group_match(dst));
+}
+
+static const struct bt_mesh_model_op *find_op(struct bt_mesh_model *models,
+					      u8_t model_count, u32_t opcode,
+					      struct bt_mesh_model **model)
+{
+	u8_t i;
+
+	for (i = 0; i < model_count; i++) {
+		const struct bt_mesh_model_op *op;
+
+		*model = &models[i];
+
+		for (op = (*model)->op; op->func; op++) {
+			if (op->opcode == opcode) {
+				return op;
+			}
+		}
+	}
+
+	*model = NULL;
+	return NULL;
+}
+
+static int get_opcode(struct os_mbuf *buf, u32_t *opcode)
+{
+	switch (buf->om_data[0] >> 6) {
+	case 0x00:
+	case 0x01:
+		if (buf->om_data[0] == 0x7f) {
+			BT_ERR("Ignoring RFU OpCode");
+			return -EINVAL;
+		}
+
+		*opcode = net_buf_simple_pull_u8(buf);
+		return 0;
+	case 0x02:
+		if (buf->om_len < 2) {
+			BT_ERR("Too short payload for 2-octet OpCode");
+			return -EINVAL;
+		}
+
+		*opcode = net_buf_simple_pull_be16(buf);
+		return 0;
+	case 0x03:
+		if (buf->om_len < 3) {
+			BT_ERR("Too short payload for 3-octet OpCode");
+			return -EINVAL;
+		}
+
+		*opcode = net_buf_simple_pull_u8(buf) << 16;
+		*opcode |= net_buf_simple_pull_le16(buf);
+		return 0;
+	}
+
+	CODE_UNREACHABLE;
+}
+
+bool bt_mesh_fixed_group_match(u16_t addr)
+{
+	/* Check for fixed group addresses */
+	switch (addr) {
+	case BT_MESH_ADDR_ALL_NODES:
+		return true;
+	case BT_MESH_ADDR_PROXIES:
+		return (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED);
+	case BT_MESH_ADDR_FRIENDS:
+		return (bt_mesh_friend_get() == BT_MESH_FRIEND_ENABLED);
+	case BT_MESH_ADDR_RELAYS:
+		return (bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED);
+	default:
+		return false;
+	}
+}
+
+void bt_mesh_model_recv(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
+{
+	struct bt_mesh_model *models, *model;
+	const struct bt_mesh_model_op *op;
+	u32_t opcode;
+	u8_t count;
+	int i;
+
+	BT_DBG("app_idx 0x%04x src 0x%04x dst 0x%04x", rx->ctx.app_idx,
+	       rx->ctx.addr, rx->ctx.recv_dst);
+	BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	if (get_opcode(buf, &opcode) < 0) {
+		BT_WARN("Unable to decode OpCode");
+		return;
+	}
+
+	BT_DBG("OpCode 0x%08x", (unsigned) opcode);
+
+	for (i = 0; i < dev_comp->elem_count; i++) {
+		struct bt_mesh_elem *elem = &dev_comp->elem[i];
+		struct net_buf_simple_state state;
+
+		/* SIG models cannot contain 3-byte (vendor) OpCodes, and
+		 * vendor models cannot contain SIG (1- or 2-byte) OpCodes, so
+		 * we only need to do the lookup in one of the model lists.
+		 */
+		if (BT_MESH_MODEL_OP_LEN(opcode) < 3) {
+			models = elem->models;
+			count = elem->model_count;
+		} else {
+			models = elem->vnd_models;
+			count = elem->vnd_model_count;
+		}
+
+		op = find_op(models, count, opcode, &model);
+		if (!op) {
+			BT_DBG("No OpCode 0x%08x for elem %d", opcode, i);
+			continue;
+		}
+
+		if (!model_has_key(model, rx->ctx.app_idx)) {
+				continue;
+			}
+
+		if (!model_has_dst(model, rx->ctx.recv_dst)) {
+			continue;
+		}
+
+		if (buf->om_len < op->min_len) {
+			BT_ERR("Too short message for OpCode 0x%08x", opcode);
+			continue;
+		}
+
+		/* The callback will likely parse the buffer, so
+		 * store the parsing state in case multiple models
+		 * receive the message.
+		 */
+		net_buf_simple_save(buf, &state);
+		op->func(model, &rx->ctx, buf);
+		net_buf_simple_restore(buf, &state);
+	}
+}
+
+void bt_mesh_model_msg_init(struct os_mbuf *msg, u32_t opcode)
+{
+	net_buf_simple_init(msg, 0);
+
+	switch (BT_MESH_MODEL_OP_LEN(opcode)) {
+	case 1:
+		net_buf_simple_add_u8(msg, opcode);
+		break;
+	case 2:
+		net_buf_simple_add_be16(msg, opcode);
+		break;
+	case 3:
+		net_buf_simple_add_u8(msg, ((opcode >> 16) & 0xff));
+		net_buf_simple_add_le16(msg, opcode & 0xffff);
+		break;
+	default:
+		BT_WARN("Unknown opcode format");
+		break;
+	}
+}
+
+static int model_send(struct bt_mesh_model *model,
+		      struct bt_mesh_net_tx *tx, bool implicit_bind,
+		      struct os_mbuf *msg,
+		      const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x dst 0x%04x", tx->ctx->net_idx,
+	       tx->ctx->app_idx, tx->ctx->addr);
+	BT_DBG("len %u: %s", msg->om_len, bt_hex(msg->om_data, msg->om_len));
+
+	if (!bt_mesh_is_provisioned()) {
+		BT_ERR("Local node is not yet provisioned");
+		return -EAGAIN;
+	}
+
+	if (net_buf_simple_tailroom(msg) < 4) {
+		BT_ERR("Not enough tailroom for TransMIC");
+		return -EINVAL;
+	}
+
+	if (msg->om_len > BT_MESH_TX_SDU_MAX - 4) {
+		BT_ERR("Too big message");
+		return -EMSGSIZE;
+	}
+
+	if (!implicit_bind && !model_has_key(model, tx->ctx->app_idx)) {
+		BT_ERR("Model not bound to AppKey 0x%04x", tx->ctx->app_idx);
+		return -EINVAL;
+	}
+
+	return bt_mesh_trans_send(tx, msg, cb, cb_data);
+}
+
+int bt_mesh_model_send(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *msg,
+		       const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	struct bt_mesh_net_tx tx = {
+		.sub = bt_mesh_subnet_get(ctx->net_idx),
+		.ctx = ctx,
+		.src = bt_mesh_model_elem(model)->addr,
+		.xmit = bt_mesh_net_transmit_get(),
+		.friend_cred = 0,
+	};
+
+	return model_send(model, &tx, false, msg, cb, cb_data);
+}
+
+int bt_mesh_model_publish(struct bt_mesh_model *model)
+{
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	struct bt_mesh_model_pub *pub = model->pub;
+	struct bt_mesh_app_key *key;
+	struct bt_mesh_msg_ctx ctx = {
+	};
+	struct bt_mesh_net_tx tx = {
+		.ctx = &ctx,
+		.src = bt_mesh_model_elem(model)->addr,
+		.xmit = bt_mesh_net_transmit_get(),
+	};
+	int err;
+
+	BT_DBG("");
+
+	if (!pub) {
+		err = -ENOTSUP;
+		goto done;
+	}
+
+	if (pub->addr == BT_MESH_ADDR_UNASSIGNED) {
+		err = -EADDRNOTAVAIL;
+		goto done;
+	}
+
+	key = bt_mesh_app_key_find(pub->key);
+	if (!key) {
+		err = -EADDRNOTAVAIL;
+		goto done;
+	}
+
+	if (pub->msg->om_len + 4 > BT_MESH_TX_SDU_MAX) {
+		BT_ERR("Message does not fit maximum SDU size");
+		err = -EMSGSIZE;
+		goto done;
+	}
+
+	if (pub->count) {
+		BT_WARN("Clearing publish retransmit timer");
+		k_delayed_work_cancel(&pub->timer);
+	}
+
+	net_buf_simple_init(sdu, 0);
+	net_buf_simple_add_mem(sdu, pub->msg->om_data, pub->msg->om_len);
+
+	ctx.addr = pub->addr;
+	ctx.send_ttl = pub->ttl;
+	ctx.net_idx = key->net_idx;
+	ctx.app_idx = key->app_idx;
+
+	tx.friend_cred = pub->cred;
+	tx.sub = bt_mesh_subnet_get(ctx.net_idx),
+
+	pub->count = BT_MESH_PUB_TRANSMIT_COUNT(pub->retransmit);
+
+	BT_DBG("Publish Retransmit Count %u Interval %ums", pub->count,
+	       BT_MESH_PUB_TRANSMIT_INT(pub->retransmit));
+
+	err = model_send(model, &tx, true, sdu, &pub_sent_cb, model);
+	if (err) {
+		/* Don't try retransmissions for this publish attempt */
+		pub->count = 0;
+		/* Make sure the publish timer gets reset */
+		publish_sent(err, model);
+	}
+
+done:
+	os_mbuf_free_chain(sdu);
+	return err;
+}
+
+struct bt_mesh_model *bt_mesh_model_find_vnd(const struct bt_mesh_elem *elem,
+					     u16_t company, u16_t id)
+{
+	u8_t i;
+
+	for (i = 0; i < elem->vnd_model_count; i++) {
+		if (elem->vnd_models[i].vnd.company == company &&
+		    elem->vnd_models[i].vnd.id == id) {
+			return &elem->vnd_models[i];
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_mesh_model *bt_mesh_model_find(const struct bt_mesh_elem *elem,
+					 u16_t id)
+{
+	u8_t i;
+
+	for (i = 0; i < elem->model_count; i++) {
+		if (elem->models[i].id == id) {
+			return &elem->models[i];
+		}
+	}
+
+	return NULL;
+}
+
+const struct bt_mesh_comp *bt_mesh_comp_get(void)
+{
+	return dev_comp;
+}
+
+struct bt_mesh_model *bt_mesh_model_root(struct bt_mesh_model *mod)
+{
+#ifdef CONFIG_BT_MESH_MODEL_EXTENSIONS
+	while (mod->next) {
+		mod = mod->next;
+	}
+#endif
+	return mod;
+}
+
+void bt_mesh_model_tree_walk(struct bt_mesh_model *root,
+			     enum bt_mesh_walk (*cb)(struct bt_mesh_model *mod,
+						     u32_t depth,
+						     void *user_data),
+			     void *user_data)
+{
+	struct bt_mesh_model *m = root;
+	u32_t depth = 0;
+
+	do {
+		if (cb(m, depth, user_data) == BT_MESH_WALK_STOP) {
+			return;
+		}
+#if MYNEWT_VAL(BLE_MESH_MODEL_EXTENSIONS)
+		if (m->extends) {
+			m = m->extends;
+			depth++;
+		} else if (m->flags & BT_MESH_MOD_NEXT_IS_PARENT) {
+			m = m->next->next;
+			depth--;
+		} else {
+			m = m->next;
+		}
+#endif
+	} while (m && m != root);
+}
+
+#if MYNEWT_VAL(BLE_MESH_MODEL_EXTENSIONS)
+int bt_mesh_model_extend(struct bt_mesh_model *mod,
+			 struct bt_mesh_model *base_mod)
+{
+	/* Form a cyclical LCRS tree:
+	 * The extends-pointer points to the first child, and the next-pointer
+	 * points to the next sibling. The last sibling is marked by the
+	 * BT_MESH_MOD_NEXT_IS_PARENT flag, and its next-pointer points back to
+	 * the parent. This way, the whole tree is accessible from any node.
+	 *
+	 * We add children (extend them) by inserting them as the first child.
+	 */
+	if (base_mod->next) {
+		return -EALREADY;
+	}
+
+	if (mod->extends) {
+		base_mod->next = mod->extends;
+	} else {
+		base_mod->next = mod;
+		base_mod->flags |= BT_MESH_MOD_NEXT_IS_PARENT;
+	}
+
+	mod->extends = base_mod;
+	return 0;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/access.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/access.h
new file mode 100644
index 00000000..48514983
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/access.h
@@ -0,0 +1,67 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __ACCESS_H__
+#define __ACCESS_H__
+
+#include "mesh/mesh.h"
+
+/* bt_mesh_model.flags */
+enum {
+	BT_MESH_MOD_BIND_PENDING = BIT(0),
+	BT_MESH_MOD_SUB_PENDING = BIT(1),
+	BT_MESH_MOD_PUB_PENDING = BIT(2),
+	BT_MESH_MOD_DATA_PRESENT = BIT(3),
+	BT_MESH_MOD_NEXT_IS_PARENT = BIT(4),
+};
+
+/* Tree walk return codes */
+enum bt_mesh_walk {
+	BT_MESH_WALK_STOP,
+	BT_MESH_WALK_CONTINUE,
+};
+
+void bt_mesh_elem_register(struct bt_mesh_elem *elem, u8_t count);
+
+u8_t bt_mesh_elem_count(void);
+
+/* Find local element based on unicast or group address */
+struct bt_mesh_elem *bt_mesh_elem_find(u16_t addr);
+
+struct bt_mesh_model *bt_mesh_model_root(struct bt_mesh_model *mod);
+void bt_mesh_model_tree_walk(struct bt_mesh_model *root,
+			     enum bt_mesh_walk (*cb)(struct bt_mesh_model *mod,
+						     u32_t depth,
+						     void *user_data),
+			     void *user_data);
+
+u16_t *bt_mesh_model_find_group(struct bt_mesh_model **mod, u16_t addr);
+
+bool bt_mesh_fixed_group_match(u16_t addr);
+
+void bt_mesh_model_foreach(void (*func)(struct bt_mesh_model *mod,
+					struct bt_mesh_elem *elem,
+					bool vnd, bool primary,
+					void *user_data),
+			   void *user_data);
+
+s32_t bt_mesh_model_pub_period_get(struct bt_mesh_model *mod);
+
+void bt_mesh_comp_provision(u16_t addr);
+void bt_mesh_comp_unprovision(void);
+
+u16_t bt_mesh_primary_addr(void);
+
+const struct bt_mesh_comp *bt_mesh_comp_get(void);
+
+struct bt_mesh_model *bt_mesh_model_get(bool vnd, u8_t elem_idx, u8_t mod_idx);
+
+void bt_mesh_model_recv(struct bt_mesh_net_rx *rx, struct os_mbuf *buf);
+
+int bt_mesh_comp_register(const struct bt_mesh_comp *comp);
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/adv.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/adv.c
new file mode 100644
index 00000000..4bd51cc1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/adv.c
@@ -0,0 +1,439 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2018 Nordic Semiconductor ASA
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_ADV_LOG
+
+#include "mesh/mesh.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_gap.h"
+#include "nimble/hci_common.h"
+#include "mesh/porting.h"
+
+#include "adv.h"
+#include "net.h"
+#include "foundation.h"
+#include "beacon.h"
+#include "prov.h"
+#include "proxy.h"
+
+/* Convert from ms to 0.625ms units */
+#define ADV_SCAN_UNIT(_ms) ((_ms) * 8 / 5)
+
+/* Window and Interval are equal for continuous scanning */
+#define MESH_SCAN_INTERVAL_MS 30
+#define MESH_SCAN_WINDOW_MS   30
+#define MESH_SCAN_INTERVAL    ADV_SCAN_UNIT(MESH_SCAN_INTERVAL_MS)
+#define MESH_SCAN_WINDOW      ADV_SCAN_UNIT(MESH_SCAN_WINDOW_MS)
+
+/* Pre-5.0 controllers enforce a minimum interval of 100ms
+ * whereas 5.0+ controllers can go down to 20ms.
+ */
+#define ADV_INT_DEFAULT_MS 100
+#define ADV_INT_FAST_MS    20
+
+static s32_t adv_int_min =  ADV_INT_DEFAULT_MS;
+
+/* TinyCrypt PRNG consumes a lot of stack space, so we need to have
+ * an increased call stack whenever it's used.
+ */
+#if MYNEWT
+#define ADV_STACK_SIZE 768
+OS_TASK_STACK_DEFINE(g_blemesh_stack, ADV_STACK_SIZE);
+struct os_task adv_task;
+#endif
+
+static struct ble_npl_eventq adv_queue;
+extern u8_t g_mesh_addr_type;
+static int adv_initialized = false;
+
+static os_membuf_t adv_buf_mem[OS_MEMPOOL_SIZE(
+		MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT),
+		BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE)];
+
+struct os_mbuf_pool adv_os_mbuf_pool;
+static struct os_mempool adv_buf_mempool;
+
+static const u8_t adv_type[] = {
+	[BT_MESH_ADV_PROV]   = BLE_HS_ADV_TYPE_MESH_PROV,
+	[BT_MESH_ADV_DATA]   = BLE_HS_ADV_TYPE_MESH_MESSAGE,
+	[BT_MESH_ADV_BEACON] = BLE_HS_ADV_TYPE_MESH_BEACON,
+	[BT_MESH_ADV_URI]    = BLE_HS_ADV_TYPE_URI,
+};
+
+
+static struct bt_mesh_adv adv_pool[CONFIG_BT_MESH_ADV_BUF_COUNT];
+
+static struct bt_mesh_adv *adv_alloc(int id)
+{
+	return &adv_pool[id];
+}
+
+static inline void adv_send_start(u16_t duration, int err,
+				  const struct bt_mesh_send_cb *cb,
+				  void *cb_data)
+{
+	if (cb && cb->start) {
+		cb->start(duration, err, cb_data);
+	}
+}
+
+static inline void adv_send_end(int err, const struct bt_mesh_send_cb *cb,
+				void *cb_data)
+{
+	if (cb && cb->end) {
+		cb->end(err, cb_data);
+	}
+}
+
+static inline void adv_send(struct os_mbuf *buf)
+{
+	const struct bt_mesh_send_cb *cb = BT_MESH_ADV(buf)->cb;
+	void *cb_data = BT_MESH_ADV(buf)->cb_data;
+	struct ble_gap_adv_params param = { 0 };
+	u16_t duration, adv_int;
+	struct bt_data ad;
+	int err;
+
+	adv_int = max(adv_int_min,
+		      BT_MESH_TRANSMIT_INT(BT_MESH_ADV(buf)->xmit));
+#if MYNEWT_VAL(BLE_CONTROLLER)
+	duration = ((BT_MESH_TRANSMIT_COUNT(BT_MESH_ADV(buf)->xmit) + 1) *
+				(adv_int + 10));
+#else
+	duration = (MESH_SCAN_WINDOW_MS +
+		    ((BT_MESH_TRANSMIT_COUNT(BT_MESH_ADV(buf)->xmit) + 1) *
+		     (adv_int + 10)));
+#endif
+
+	BT_DBG("type %u om_len %u: %s", BT_MESH_ADV(buf)->type,
+	       buf->om_len, bt_hex(buf->om_data, buf->om_len));
+	BT_DBG("count %u interval %ums duration %ums",
+	       BT_MESH_TRANSMIT_COUNT(BT_MESH_ADV(buf)->xmit) + 1, adv_int,
+	       duration);
+
+	ad.type = adv_type[BT_MESH_ADV(buf)->type];
+	ad.data_len = buf->om_len;
+	ad.data = buf->om_data;
+
+	param.itvl_min = ADV_SCAN_UNIT(adv_int);
+	param.itvl_max = param.itvl_min;
+	param.conn_mode = BLE_GAP_CONN_MODE_NON;
+
+	err = bt_le_adv_start(&param, &ad, 1, NULL, 0);
+	net_buf_unref(buf);
+	adv_send_start(duration, err, cb, cb_data);
+	if (err) {
+		BT_ERR("Advertising failed: err %d", err);
+		return;
+	}
+
+	BT_DBG("Advertising started. Sleeping %u ms", duration);
+
+	k_sleep(K_MSEC(duration));
+
+	err = bt_le_adv_stop(false);
+	adv_send_end(err, cb, cb_data);
+	if (err) {
+		BT_ERR("Stopping advertising failed: err %d", err);
+		return;
+	}
+
+	BT_DBG("Advertising stopped");
+}
+
+void
+mesh_adv_thread(void *args)
+{
+	static struct ble_npl_event *ev;
+	struct os_mbuf *buf;
+#if (MYNEWT_VAL(BLE_MESH_PROXY))
+	s32_t timeout;
+#endif
+
+	BT_DBG("started");
+
+	while (1) {
+#if (MYNEWT_VAL(BLE_MESH_PROXY))
+		ev = ble_npl_eventq_get(&adv_queue, 0);
+		while (!ev) {
+			timeout = bt_mesh_proxy_adv_start();
+			BT_DBG("Proxy Advertising up to %d ms", (int) timeout);
+
+			// FIXME: should we redefine K_SECONDS macro instead in glue?
+			if (timeout != K_FOREVER) {
+				timeout = ble_npl_time_ms_to_ticks32(timeout);
+			}
+
+			ev = ble_npl_eventq_get(&adv_queue, timeout);
+			bt_mesh_proxy_adv_stop();
+		}
+#else
+		ev = ble_npl_eventq_get(&adv_queue, BLE_NPL_TIME_FOREVER);
+#endif
+
+		if (!ev || !ble_npl_event_get_arg(ev)) {
+			continue;
+		}
+
+		buf = ble_npl_event_get_arg(ev);
+
+		/* busy == 0 means this was canceled */
+		if (BT_MESH_ADV(buf)->busy) {
+			BT_MESH_ADV(buf)->busy = 0;
+			adv_send(buf);
+		} else {
+			net_buf_unref(buf);
+		}
+
+		/* os_sched(NULL); */
+	}
+}
+
+void bt_mesh_adv_update(void)
+{
+	static struct ble_npl_event ev = { };
+
+	BT_DBG("");
+
+	ble_npl_eventq_put(&adv_queue, &ev);
+}
+
+struct os_mbuf *bt_mesh_adv_create_from_pool(struct os_mbuf_pool *pool,
+					     bt_mesh_adv_alloc_t get_id,
+					     enum bt_mesh_adv_type type,
+					     u8_t xmit, s32_t timeout)
+{
+	struct bt_mesh_adv *adv;
+	struct os_mbuf *buf;
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_SUSPENDED)) {
+		BT_WARN("Refusing to allocate buffer while suspended");
+		return NULL;
+	}
+
+	buf = os_mbuf_get_pkthdr(pool, BT_MESH_ADV_USER_DATA_SIZE);
+	if (!buf) {
+		return NULL;
+	}
+
+	adv = get_id(net_buf_id(buf));
+	BT_MESH_ADV(buf) = adv;
+
+	memset(adv, 0, sizeof(*adv));
+
+	adv->type         = type;
+	adv->xmit         = xmit;
+
+	adv->ref_cnt = 1;
+	ble_npl_event_set_arg(&adv->ev, buf);
+
+	return buf;
+}
+
+struct os_mbuf *bt_mesh_adv_create(enum bt_mesh_adv_type type, u8_t xmit,
+				   s32_t timeout)
+{
+	return bt_mesh_adv_create_from_pool(&adv_os_mbuf_pool, adv_alloc, type,
+					    xmit, timeout);
+}
+
+void bt_mesh_adv_send(struct os_mbuf *buf, const struct bt_mesh_send_cb *cb,
+		      void *cb_data)
+{
+	BT_DBG("buf %p, type 0x%02x len %u: %s", buf, BT_MESH_ADV(buf)->type, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	BT_MESH_ADV(buf)->cb = cb;
+	BT_MESH_ADV(buf)->cb_data = cb_data;
+	BT_MESH_ADV(buf)->busy = 1;
+
+	net_buf_put(&adv_queue, net_buf_ref(buf));
+}
+
+static void bt_mesh_scan_cb(const bt_addr_le_t *addr, s8_t rssi,
+			    u8_t adv_type, struct os_mbuf *buf)
+{
+	if (adv_type != BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) {
+		return;
+	}
+
+#if BT_MESH_EXTENDED_DEBUG
+	BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+#endif
+
+	while (buf->om_len > 1) {
+		struct net_buf_simple_state state;
+		u8_t len, type;
+
+		len = net_buf_simple_pull_u8(buf);
+		/* Check for early termination */
+		if (len == 0) {
+			return;
+		}
+
+		if (len > buf->om_len) {
+			BT_WARN("AD malformed");
+			return;
+		}
+
+		net_buf_simple_save(buf, &state);
+
+		type = net_buf_simple_pull_u8(buf);
+
+		switch (type) {
+		case BLE_HS_ADV_TYPE_MESH_MESSAGE:
+			bt_mesh_net_recv(buf, rssi, BT_MESH_NET_IF_ADV);
+			break;
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+		case BLE_HS_ADV_TYPE_MESH_PROV:
+			bt_mesh_pb_adv_recv(buf);
+			break;
+#endif
+		case BLE_HS_ADV_TYPE_MESH_BEACON:
+			bt_mesh_beacon_recv(buf);
+			break;
+		default:
+			break;
+		}
+
+		net_buf_simple_restore(buf, &state);
+		net_buf_simple_pull(buf, len);
+	}
+}
+
+void bt_mesh_adv_init(void)
+{
+	int rc;
+
+	/* Advertising should only be initialized once. Calling
+	 * os_task init the second time will result in an assert. */
+	if (adv_initialized) {
+		return;
+	}
+
+	rc = os_mempool_init(&adv_buf_mempool, MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT),
+			     BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE,
+			     adv_buf_mem, "adv_buf_pool");
+	assert(rc == 0);
+
+	rc = os_mbuf_pool_init(&adv_os_mbuf_pool, &adv_buf_mempool,
+			       BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE,
+			       MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT));
+	assert(rc == 0);
+
+	ble_npl_eventq_init(&adv_queue);
+
+#if MYNEWT
+	os_task_init(&adv_task, "mesh_adv", mesh_adv_thread, NULL,
+	             MYNEWT_VAL(BLE_MESH_ADV_TASK_PRIO), OS_WAIT_FOREVER,
+	             g_blemesh_stack, ADV_STACK_SIZE);
+#endif
+
+	/* For BT5 controllers we can have fast advertising interval */
+	if (ble_hs_hci_get_hci_version() >= BLE_HCI_VER_BCS_5_0) {
+	    adv_int_min = ADV_INT_FAST_MS;
+	}
+
+	adv_initialized = true;
+}
+
+int
+ble_adv_gap_mesh_cb(struct ble_gap_event *event, void *arg)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV)
+	struct ble_gap_ext_disc_desc *ext_desc;
+#endif
+	struct ble_gap_disc_desc *desc;
+	struct os_mbuf *buf = NULL;
+
+#if BT_MESH_EXTENDED_DEBUG
+	BT_DBG("event->type %d", event->type);
+#endif
+
+	switch (event->type) {
+#if MYNEWT_VAL(BLE_EXT_ADV)
+	case BLE_GAP_EVENT_EXT_DISC:
+		ext_desc = &event->ext_disc;
+		buf = os_mbuf_get_pkthdr(&adv_os_mbuf_pool, 0);
+		if (!buf || os_mbuf_append(buf, ext_desc->data, ext_desc->length_data)) {
+			BT_ERR("Could not append data");
+			goto done;
+		}
+		bt_mesh_scan_cb(&ext_desc->addr, ext_desc->rssi,
+				ext_desc->legacy_event_type, buf);
+		break;
+#endif
+	case BLE_GAP_EVENT_DISC:
+		desc = &event->disc;
+		buf = os_mbuf_get_pkthdr(&adv_os_mbuf_pool, 0);
+		if (!buf || os_mbuf_append(buf, desc->data, desc->length_data)) {
+			BT_ERR("Could not append data");
+			goto done;
+		}
+
+		bt_mesh_scan_cb(&desc->addr, desc->rssi, desc->event_type, buf);
+		break;
+	default:
+		break;
+	}
+
+done:
+	if (buf) {
+		os_mbuf_free_chain(buf);
+	}
+
+	return 0;
+}
+
+int bt_mesh_scan_enable(void)
+{
+	int err;
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+	struct ble_gap_ext_disc_params uncoded_params =
+		{ .itvl = MESH_SCAN_INTERVAL, .window = MESH_SCAN_WINDOW,
+		.passive = 1 };
+
+	BT_DBG("");
+
+	err =  ble_gap_ext_disc(g_mesh_addr_type, 0, 0, 0, 0, 0,
+				&uncoded_params, NULL, NULL, NULL);
+#else
+	struct ble_gap_disc_params scan_param =
+		{ .passive = 1, .filter_duplicates = 0, .itvl =
+		  MESH_SCAN_INTERVAL, .window = MESH_SCAN_WINDOW };
+
+	BT_DBG("");
+
+	err =  ble_gap_disc(g_mesh_addr_type, BLE_HS_FOREVER, &scan_param,
+			    NULL, NULL);
+#endif
+	if (err && err != BLE_HS_EALREADY) {
+		BT_ERR("starting scan failed (err %d)", err);
+		return err;
+	}
+
+	return 0;
+}
+
+int bt_mesh_scan_disable(void)
+{
+	int err;
+
+	BT_DBG("");
+
+	err = ble_gap_disc_cancel();
+	if (err && err != BLE_HS_EALREADY) {
+		BT_ERR("stopping scan failed (err %d)", err);
+		return err;
+	}
+
+	return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/adv.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/adv.h
new file mode 100644
index 00000000..4d0f7d8b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/adv.h
@@ -0,0 +1,79 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __ADV_H__
+#define __ADV_H__
+
+/* Maximum advertising data payload for a single data type */
+#include "mesh/mesh.h"
+
+#define BT_MESH_ADV(om) (*(struct bt_mesh_adv **) OS_MBUF_USRHDR(om))
+
+#define BT_MESH_ADV_DATA_SIZE 31
+
+/* The user data is a pointer (4 bytes) to struct bt_mesh_adv */
+#define BT_MESH_ADV_USER_DATA_SIZE (sizeof(struct bt_mesh_adv *))
+
+#define BT_MESH_MBUF_HEADER_SIZE (sizeof(struct os_mbuf_pkthdr) + \
+                                    BT_MESH_ADV_USER_DATA_SIZE +\
+				    sizeof(struct os_mbuf))
+
+enum bt_mesh_adv_type
+{
+	BT_MESH_ADV_PROV,
+	BT_MESH_ADV_DATA,
+	BT_MESH_ADV_BEACON,
+	BT_MESH_ADV_URI,
+};
+
+typedef void (*bt_mesh_adv_func_t)(struct os_mbuf *buf, u16_t duration,
+				   int err, void *user_data);
+
+struct bt_mesh_adv {
+	const struct bt_mesh_send_cb *cb;
+	void *cb_data;
+
+	u8_t      type:2,
+		  busy:1;
+	u8_t      xmit;
+
+	/* For transport layer segment sending */
+	struct {
+		u8_t attempts;
+	} seg;
+
+	u8_t flags;
+
+	int ref_cnt;
+	struct ble_npl_event ev;
+};
+
+typedef struct bt_mesh_adv *(*bt_mesh_adv_alloc_t)(int id);
+
+/* xmit_count: Number of retransmissions, i.e. 0 == 1 transmission */
+struct os_mbuf *bt_mesh_adv_create(enum bt_mesh_adv_type type, u8_t xmit,
+				   s32_t timeout);
+
+struct os_mbuf *bt_mesh_adv_create_from_pool(struct os_mbuf_pool *pool,
+					     bt_mesh_adv_alloc_t get_id,
+					     enum bt_mesh_adv_type type,
+					     u8_t xmit, s32_t timeout);
+
+void bt_mesh_adv_send(struct os_mbuf *buf, const struct bt_mesh_send_cb *cb,
+		      void *cb_data);
+
+void bt_mesh_adv_update(void);
+
+void bt_mesh_adv_init(void);
+
+int bt_mesh_scan_enable(void);
+
+int bt_mesh_scan_disable(void);
+
+int ble_adv_gap_mesh_cb(struct ble_gap_event *event, void *arg);
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/atomic.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/atomic.h
new file mode 100644
index 00000000..2c731794
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/atomic.h
@@ -0,0 +1,409 @@
+/* atomic operations */
+
+/*
+ * Copyright (c) 1997-2015, Wind River Systems, Inc.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __ATOMIC_H__
+#define __ATOMIC_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+typedef int atomic_t;
+typedef atomic_t atomic_val_t;
+
+/**
+ * @defgroup atomic_apis Atomic Services APIs
+ * @ingroup kernel_apis
+ * @{
+ */
+
+/**
+ * @brief Atomic compare-and-set.
+ *
+ * This routine performs an atomic compare-and-set on @a target. If the current
+ * value of @a target equals @a old_value, @a target is set to @a new_value.
+ * If the current value of @a target does not equal @a old_value, @a target
+ * is left unchanged.
+ *
+ * @param target Address of atomic variable.
+ * @param old_value Original value to compare against.
+ * @param new_value New value to store.
+ * @return 1 if @a new_value is written, 0 otherwise.
+ */
+static inline int atomic_cas(atomic_t *target, atomic_val_t old_value,
+        atomic_val_t new_value)
+{
+    return __atomic_compare_exchange_n(target, &old_value, new_value,
+            0, __ATOMIC_SEQ_CST,
+            __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic addition.
+ *
+ * This routine performs an atomic addition on @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to add.
+ *
+ * @return Previous value of @a target.
+ */
+static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_add(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic subtraction.
+ *
+ * This routine performs an atomic subtraction on @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to subtract.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_sub(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic increment.
+ *
+ * This routine performs an atomic increment by 1 on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_inc(atomic_t *target)
+{
+    return atomic_add(target, 1);
+}
+
+/**
+ *
+ * @brief Atomic decrement.
+ *
+ * This routine performs an atomic decrement by 1 on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_dec(atomic_t *target)
+{
+    return atomic_sub(target, 1);
+}
+
+/**
+ *
+ * @brief Atomic get.
+ *
+ * This routine performs an atomic read on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Value of @a target.
+ */
+
+static inline atomic_val_t atomic_get(const atomic_t *target)
+{
+    return __atomic_load_n(target, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic get-and-set.
+ *
+ * This routine atomically sets @a target to @a value and returns
+ * the previous value of @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to write to @a target.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_set(atomic_t *target, atomic_val_t value)
+{
+    /* This builtin, as described by Intel, is not a traditional
+     * test-and-set operation, but rather an atomic exchange operation. It
+     * writes value into *ptr, and returns the previous contents of *ptr.
+     */
+    return __atomic_exchange_n(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic clear.
+ *
+ * This routine atomically sets @a target to zero and returns its previous
+ * value. (Hence, it is equivalent to atomic_set(target, 0).)
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_clear(atomic_t *target)
+{
+    return atomic_set(target, 0);
+}
+
+/**
+ *
+ * @brief Atomic bitwise inclusive OR.
+ *
+ * This routine atomically sets @a target to the bitwise inclusive OR of
+ * @a target and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to OR.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_or(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise exclusive OR (XOR).
+ *
+ * This routine atomically sets @a target to the bitwise exclusive OR (XOR) of
+ * @a target and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to XOR
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_xor(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise AND.
+ *
+ * This routine atomically sets @a target to the bitwise AND of @a target
+ * and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to AND.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_and(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise NAND.
+ *
+ * This routine atomically sets @a target to the bitwise NAND of @a target
+ * and @a value. (This operation is equivalent to target = ~(target & value).)
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to NAND.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_nand(target, value, __ATOMIC_SEQ_CST);
+}
+
+    /**
+     * @brief Initialize an atomic variable.
+     *
+     * This macro can be used to initialize an atomic variable. For example,
+     * @code atomic_t my_var = ATOMIC_INIT(75); @endcode
+     *
+     * @param i Value to assign to atomic variable.
+     */
+#define ATOMIC_INIT(i) (i)
+
+    /**
+     * @cond INTERNAL_HIDDEN
+     */
+
+#define ATOMIC_BITS (sizeof(atomic_val_t) * 8)
+#define ATOMIC_MASK(bit) (1 << ((bit) & (ATOMIC_BITS - 1)))
+#define ATOMIC_ELEM(addr, bit) ((addr) + ((bit) / ATOMIC_BITS))
+
+    /**
+     * INTERNAL_HIDDEN @endcond
+     */
+
+    /**
+     * @brief Define an array of atomic variables.
+     *
+     * This macro defines an array of atomic variables containing at least
+     * @a num_bits bits.
+     *
+     * @note
+     * If used from file scope, the bits of the array are initialized to zero;
+     * if used from within a function, the bits are left uninitialized.
+     *
+     * @param name Name of array of atomic variables.
+     * @param num_bits Number of bits needed.
+     */
+#define ATOMIC_DEFINE(name, num_bits) \
+	atomic_t name[1 + ((num_bits) - 1) / ATOMIC_BITS]
+
+    /**
+     * @brief Atomically test a bit.
+     *
+     * This routine tests whether bit number @a bit of @a target is set or not.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_bit(const atomic_t *target, int bit)
+    {
+        atomic_val_t val = atomic_get(ATOMIC_ELEM(target, bit));
+
+        return (1 & (val >> (bit & (ATOMIC_BITS - 1))));
+    }
+
+    /**
+     * @brief Atomically test and clear a bit.
+     *
+     * Atomically clear bit number @a bit of @a target and return its old value.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_and_clear_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+        atomic_val_t old;
+
+        old = atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+
+        return (old & mask) != 0;
+    }
+
+    /**
+     * @brief Atomically set a bit.
+     *
+     * Atomically set bit number @a bit of @a target and return its old value.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_and_set_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+        atomic_val_t old;
+
+        old = atomic_or(ATOMIC_ELEM(target, bit), mask);
+
+        return (old & mask) != 0;
+    }
+
+    /**
+     * @brief Atomically clear a bit.
+     *
+     * Atomically clear bit number @a bit of @a target.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return N/A
+     */
+    static inline void
+    atomic_clear_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+
+        atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+    }
+
+    /**
+     * @brief Atomically set a bit.
+     *
+     * Atomically set bit number @a bit of @a target.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return N/A
+     */
+    static inline void
+    atomic_set_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+
+        atomic_or(ATOMIC_ELEM(target, bit), mask);
+    }
+
+/**
+* @brief Atomically set a bit to a given value.
+*
+* Atomically set bit number @a bit of @a target to value @a val.
+* The target may be a single atomic variable or an array of them.
+*
+* @param target Address of atomic variable or array.
+* @param bit Bit number (starting from 0).
+* @param val true for 1, false for 0.
+*
+* @return N/A
+*/
+static inline void atomic_set_bit_to(atomic_t *target, int bit, bool val)
+{
+    atomic_val_t mask = ATOMIC_MASK(bit);
+
+    if (val) {
+        (void)atomic_or(ATOMIC_ELEM(target, bit), mask);
+    } else {
+        (void)atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+    }
+}
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ATOMIC_H__ */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/beacon.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/beacon.c
new file mode 100644
index 00000000..cd540aa8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/beacon.c
@@ -0,0 +1,441 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_BEACON_LOG
+
+#include <errno.h>
+#include <assert.h>
+#include "os/os_mbuf.h"
+#include "mesh/mesh.h"
+
+#include "adv.h"
+#include "mesh_priv.h"
+#include "net.h"
+#include "prov.h"
+#include "crypto.h"
+#include "beacon.h"
+#include "foundation.h"
+
+#define UNPROVISIONED_INTERVAL    (K_SECONDS(5))
+#define PROVISIONED_INTERVAL      (K_SECONDS(10))
+
+#define BEACON_TYPE_UNPROVISIONED  0x00
+#define BEACON_TYPE_SECURE         0x01
+
+/* 3 transmissions, 20ms interval */
+#define UNPROV_XMIT                BT_MESH_TRANSMIT(2, 20)
+
+/* 1 transmission, 20ms interval */
+#define PROV_XMIT                  BT_MESH_TRANSMIT(0, 20)
+
+static struct k_delayed_work beacon_timer;
+
+static struct bt_mesh_subnet *cache_check(u8_t data[21])
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (!memcmp(sub->beacon_cache, data, 21)) {
+			return sub;
+		}
+	}
+
+	return NULL;
+}
+
+static void cache_add(u8_t data[21], struct bt_mesh_subnet *sub)
+{
+	memcpy(sub->beacon_cache, data, 21);
+}
+
+static void beacon_complete(int err, void *user_data)
+{
+	struct bt_mesh_subnet *sub = user_data;
+
+	BT_DBG("err %d", err);
+
+	sub->beacon_sent = k_uptime_get_32();
+}
+
+void bt_mesh_beacon_create(struct bt_mesh_subnet *sub,
+			   struct os_mbuf *buf)
+{
+	u8_t flags = bt_mesh_net_flags(sub);
+	struct bt_mesh_subnet_keys *keys;
+
+	net_buf_simple_add_u8(buf, BEACON_TYPE_SECURE);
+
+	if (sub->kr_flag) {
+		keys = &sub->keys[1];
+	} else {
+		keys = &sub->keys[0];
+	}
+
+	net_buf_simple_add_u8(buf, flags);
+
+	/* Network ID */
+	net_buf_simple_add_mem(buf, keys->net_id, 8);
+
+	/* IV Index */
+	net_buf_simple_add_be32(buf, bt_mesh.iv_index);
+
+	net_buf_simple_add_mem(buf, sub->auth, 8);
+
+	BT_DBG("net_idx 0x%04x flags 0x%02x NetID %s", sub->net_idx,
+	       flags, bt_hex(keys->net_id, 8));
+	BT_DBG("IV Index 0x%08x Auth %s", (unsigned) bt_mesh.iv_index,
+	       bt_hex(sub->auth, 8));
+}
+
+/* If the interval has passed or is within 5 seconds from now send a beacon */
+#define BEACON_THRESHOLD(sub) (K_SECONDS(10 * ((sub)->beacons_last + 1)) - \
+			       K_SECONDS(5))
+
+static int secure_beacon_send(void)
+{
+	static const struct bt_mesh_send_cb send_cb = {
+		.end = beacon_complete,
+	};
+	u32_t now = k_uptime_get_32();
+	int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+		struct os_mbuf *buf;
+		u32_t time_diff;
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		time_diff = now - sub->beacon_sent;
+		if (time_diff < K_SECONDS(600) &&
+		    time_diff < BEACON_THRESHOLD(sub)) {
+			continue;
+		}
+
+		buf = bt_mesh_adv_create(BT_MESH_ADV_BEACON, PROV_XMIT,
+					 K_NO_WAIT);
+		if (!buf) {
+			BT_ERR("Unable to allocate beacon buffer");
+			return -ENOBUFS;
+		}
+
+		bt_mesh_beacon_create(sub, buf);
+
+		bt_mesh_adv_send(buf, &send_cb, sub);
+		net_buf_unref(buf);
+	}
+
+	return 0;
+}
+
+static int unprovisioned_beacon_send(void)
+{
+	const struct bt_mesh_prov *prov;
+	u8_t uri_hash[16] = { 0 };
+	struct os_mbuf *buf;
+	u16_t oob_info;
+
+	BT_DBG("unprovisioned_beacon_send");
+
+	buf = bt_mesh_adv_create(BT_MESH_ADV_BEACON, UNPROV_XMIT, K_NO_WAIT);
+	if (!buf) {
+		BT_ERR("Unable to allocate beacon buffer");
+		return -ENOBUFS;
+	}
+
+	prov = bt_mesh_prov_get();
+
+	net_buf_add_u8(buf, BEACON_TYPE_UNPROVISIONED);
+	net_buf_add_mem(buf, prov->uuid, 16);
+
+	if (prov->uri && bt_mesh_s1(prov->uri, uri_hash) == 0) {
+		oob_info = prov->oob_info | BT_MESH_PROV_OOB_URI;
+	} else {
+		oob_info = prov->oob_info;
+	}
+
+	net_buf_add_be16(buf, oob_info);
+	net_buf_add_mem(buf, uri_hash, 4);
+
+	bt_mesh_adv_send(buf, NULL, NULL);
+	net_buf_unref(buf);
+
+	if (prov->uri) {
+		size_t len;
+
+		buf = bt_mesh_adv_create(BT_MESH_ADV_URI, UNPROV_XMIT,
+					 K_NO_WAIT);
+		if (!buf) {
+			BT_ERR("Unable to allocate URI buffer");
+			return -ENOBUFS;
+		}
+
+		len = strlen(prov->uri);
+		if (net_buf_tailroom(buf) < len) {
+			BT_WARN("Too long URI to fit advertising data");
+		} else {
+			net_buf_add_mem(buf, prov->uri, len);
+			bt_mesh_adv_send(buf, NULL, NULL);
+		}
+
+		net_buf_unref(buf);
+	}
+
+	return 0;
+}
+
+static void unprovisioned_beacon_recv(struct os_mbuf *buf)
+{
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+	const struct bt_mesh_prov *prov;
+	u8_t *uuid;
+	u16_t oob_info;
+	u32_t uri_hash_val;
+	u32_t *uri_hash = NULL;
+
+	if (buf->om_len != 18 && buf->om_len != 22) {
+		BT_ERR("Invalid unprovisioned beacon length (%u)", buf->om_len);
+		return;
+	}
+
+	uuid = net_buf_simple_pull_mem(buf, 16);
+	oob_info = net_buf_simple_pull_be16(buf);
+
+	if (buf->om_len == 4) {
+		uri_hash_val = net_buf_simple_pull_be32(buf);
+		uri_hash = &uri_hash_val;
+	}
+
+	BT_DBG("uuid %s", bt_hex(uuid, 16));
+
+	prov = bt_mesh_prov_get();
+
+	if (prov->unprovisioned_beacon) {
+		prov->unprovisioned_beacon(uuid,
+					   (bt_mesh_prov_oob_info_t)oob_info,
+					   uri_hash);
+	}
+#endif
+}
+
+static void update_beacon_observation(void)
+{
+	static bool first_half;
+	int i;
+
+	/* Observation period is 20 seconds, whereas the beacon timer
+	 * runs every 10 seconds. We process what's happened during the
+	 * window only after the seconnd half.
+	 */
+	first_half = !first_half;
+	if (first_half) {
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		sub->beacons_last = sub->beacons_cur;
+		sub->beacons_cur = 0;
+	}
+}
+
+static void beacon_send(struct ble_npl_event *work)
+{
+	/* Don't send anything if we have an active provisioning link */
+	if ((MYNEWT_VAL(BLE_MESH_PROV)) && bt_prov_active()) {
+		k_delayed_work_submit(&beacon_timer, UNPROVISIONED_INTERVAL);
+		return;
+	}
+
+	BT_DBG("");
+
+	if (bt_mesh_is_provisioned()) {
+		update_beacon_observation();
+		secure_beacon_send();
+
+		/* Only resubmit if beaconing is still enabled */
+		if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED ||
+		    atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_INITIATOR)) {
+			k_delayed_work_submit(&beacon_timer,
+					      PROVISIONED_INTERVAL);
+		}
+	} else if (IS_ENABLED(CONFIG_BT_MESH_PB_ADV)) {
+		unprovisioned_beacon_send();
+		k_delayed_work_submit(&beacon_timer, UNPROVISIONED_INTERVAL);
+	}
+}
+
+static void secure_beacon_recv(struct os_mbuf *buf)
+{
+	u8_t *data, *net_id, *auth;
+	struct bt_mesh_subnet *sub;
+	u32_t iv_index;
+	bool new_key, kr_change, iv_change;
+	u8_t flags;
+
+	if (buf->om_len < 21) {
+		BT_ERR("Too short secure beacon (len %u)", buf->om_len);
+		return;
+	}
+
+	sub = cache_check(buf->om_data);
+	if (sub) {
+		/* We've seen this beacon before - just update the stats */
+		goto update_stats;
+	}
+
+	/* So we can add to the cache if auth matches */
+	data = buf->om_data;
+
+	flags = net_buf_simple_pull_u8(buf);
+	net_id = net_buf_simple_pull_mem(buf, 8);
+	iv_index = net_buf_simple_pull_be32(buf);
+	auth = buf->om_data;
+
+	BT_DBG("flags 0x%02x id %s iv_index 0x%08x",
+	       flags, bt_hex(net_id, 8), (unsigned) iv_index);
+
+	sub = bt_mesh_subnet_find(net_id, flags, iv_index, auth, &new_key);
+	if (!sub) {
+		BT_DBG("No subnet that matched beacon");
+		return;
+	}
+
+	if (sub->kr_phase == BT_MESH_KR_PHASE_2 && !new_key) {
+		BT_WARN("Ignoring Phase 2 KR Update secured using old key");
+		return;
+	}
+
+	cache_add(data, sub);
+
+	/* If we have NetKey0 accept initiation only from it */
+	if (bt_mesh_subnet_get(BT_MESH_KEY_PRIMARY) &&
+	    sub->net_idx != BT_MESH_KEY_PRIMARY) {
+		BT_WARN("Ignoring secure beacon on non-primary subnet");
+		goto update_stats;
+	}
+
+	BT_DBG("net_idx 0x%04x iv_index 0x%08x, current iv_index 0x%08x",
+	       sub->net_idx, (unsigned) iv_index, (unsigned) bt_mesh.iv_index);
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_INITIATOR) &&
+	    (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS) ==
+	     BT_MESH_IV_UPDATE(flags))) {
+		bt_mesh_beacon_ivu_initiator(false);
+	}
+
+	iv_change = bt_mesh_net_iv_update(iv_index, BT_MESH_IV_UPDATE(flags));
+
+	kr_change = bt_mesh_kr_update(sub, BT_MESH_KEY_REFRESH(flags), new_key);
+	if (kr_change) {
+		bt_mesh_net_beacon_update(sub);
+	}
+
+	if (iv_change) {
+		/* Update all subnets */
+		bt_mesh_net_sec_update(NULL);
+	} else if (kr_change) {
+		/* Key Refresh without IV Update only impacts one subnet */
+		bt_mesh_net_sec_update(sub);
+	}
+
+update_stats:
+	if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED &&
+	    sub->beacons_cur < 0xff) {
+		sub->beacons_cur++;
+	}
+}
+
+void bt_mesh_beacon_recv(struct os_mbuf *buf)
+{
+	u8_t type;
+
+	BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	if (buf->om_len < 1) {
+		BT_ERR("Too short beacon");
+		return;
+	}
+
+	type = net_buf_simple_pull_u8(buf);
+	switch (type) {
+	case BEACON_TYPE_UNPROVISIONED:
+		unprovisioned_beacon_recv(buf);
+		break;
+	case BEACON_TYPE_SECURE:
+		secure_beacon_recv(buf);
+		break;
+	default:
+		BT_WARN("Unknown beacon type 0x%02x", type);
+		break;
+	}
+}
+
+void bt_mesh_beacon_init(void)
+{
+	k_delayed_work_init(&beacon_timer, beacon_send);
+}
+
+void bt_mesh_beacon_ivu_initiator(bool enable)
+{
+	atomic_set_bit_to(bt_mesh.flags, BT_MESH_IVU_INITIATOR, enable);
+
+	if (enable) {
+		k_work_submit(&beacon_timer.work);
+	} else if (bt_mesh_beacon_get() == BT_MESH_BEACON_DISABLED) {
+		k_delayed_work_cancel(&beacon_timer);
+	}
+}
+
+void bt_mesh_beacon_enable(void)
+{
+	int i;
+
+	if (!bt_mesh_is_provisioned()) {
+		k_work_submit(&beacon_timer.work);
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		sub->beacons_last = 0;
+		sub->beacons_cur = 0;
+
+		bt_mesh_net_beacon_update(sub);
+	}
+
+	k_work_submit(&beacon_timer.work);
+}
+
+void bt_mesh_beacon_disable(void)
+{
+	if (!atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_INITIATOR)) {
+		k_delayed_work_cancel(&beacon_timer);
+	}
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/beacon.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/beacon.h
new file mode 100644
index 00000000..ac4bfed8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/beacon.h
@@ -0,0 +1,26 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __BEACON_H__
+#define __BEACON_H__
+
+#include "os/os_mbuf.h"
+
+void bt_mesh_beacon_enable(void);
+void bt_mesh_beacon_disable(void);
+
+void bt_mesh_beacon_ivu_initiator(bool enable);
+
+void bt_mesh_beacon_recv(struct os_mbuf *buf);
+
+void bt_mesh_beacon_create(struct bt_mesh_subnet *sub,
+			   struct os_mbuf *buf);
+
+void bt_mesh_beacon_init(void);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/cfg_cli.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/cfg_cli.c
new file mode 100644
index 00000000..2c2f6c3f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/cfg_cli.c
@@ -0,0 +1,1498 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_MODEL_LOG
+#if MYNEWT_VAL(BLE_MESH_CFG_CLI)
+
+#include "mesh/mesh.h"
+
+#include <string.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include "net.h"
+#include "foundation.h"
+
+#define CID_NVAL 0xffff
+
+/* 2 byte dummy opcode for getting compile time buffer sizes. */
+#define DUMMY_2_BYTE_OP	BT_MESH_MODEL_OP_2(0xff, 0xff)
+
+struct comp_data {
+	u8_t *status;
+	struct os_mbuf *comp;
+};
+
+static s32_t msg_timeout = K_SECONDS(5);
+
+static struct bt_mesh_cfg_cli *cli;
+
+static void comp_data_status(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx,
+			     struct os_mbuf *buf)
+{
+	struct comp_data *param;
+	size_t to_copy;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_DEV_COMP_DATA_STATUS) {
+		BT_WARN("Unexpected Composition Data Status");
+		return;
+	}
+
+	param = cli->op_param;
+
+	*(param->status) = net_buf_simple_pull_u8(buf);
+	to_copy  = min(net_buf_simple_tailroom(param->comp), buf->om_len);
+	net_buf_simple_add_mem(param->comp, buf->om_data, to_copy);
+
+	k_sem_give(&cli->op_sync);
+}
+
+static void state_status_u8(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    struct os_mbuf*buf,
+			    u32_t expect_status)
+{
+	u8_t *status;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != expect_status) {
+		BT_WARN("Unexpected Status (0x%08x != 0x%08x)",
+			(unsigned) cli->op_pending, (unsigned) expect_status);
+		return;
+	}
+
+	status = cli->op_param;
+	*status = net_buf_simple_pull_u8(buf);
+
+	k_sem_give(&cli->op_sync);
+}
+
+static void beacon_status(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	state_status_u8(model, ctx, buf, OP_BEACON_STATUS);
+}
+
+static void ttl_status(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf*buf)
+{
+	state_status_u8(model, ctx, buf, OP_DEFAULT_TTL_STATUS);
+}
+
+static void friend_status(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf*buf)
+{
+	state_status_u8(model, ctx, buf, OP_FRIEND_STATUS);
+}
+
+static void gatt_proxy_status(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf*buf)
+{
+	state_status_u8(model, ctx, buf, OP_GATT_PROXY_STATUS);
+}
+
+struct relay_param {
+	u8_t *status;
+	u8_t *transmit;
+};
+
+static void relay_status(struct bt_mesh_model *model,
+			 struct bt_mesh_msg_ctx *ctx,
+			 struct os_mbuf*buf)
+{
+	struct relay_param *param;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_RELAY_STATUS) {
+		BT_WARN("Unexpected Relay Status message");
+		return;
+	}
+
+	param = cli->op_param;
+	*param->status = net_buf_simple_pull_u8(buf);
+	*param->transmit = net_buf_simple_pull_u8(buf);
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct net_key_param {
+	u8_t *status;
+	u16_t net_idx;
+};
+
+static void net_key_status(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct net_key_param *param;
+	u16_t net_idx, app_idx;
+	u8_t status;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_NET_KEY_STATUS) {
+		BT_WARN("Unexpected Net Key Status message");
+		return;
+	}
+
+	status = net_buf_simple_pull_u8(buf);
+	key_idx_unpack(buf, &net_idx, &app_idx);
+
+	param = cli->op_param;
+	if (param->net_idx != net_idx) {
+		BT_WARN("Net Key Status key index does not match");
+		return;
+	}
+
+	if (param->status) {
+		*param->status = status;
+	}
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct app_key_param {
+	u8_t *status;
+	u16_t net_idx;
+	u16_t app_idx;
+};
+
+static void app_key_status(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf*buf)
+{
+	struct app_key_param *param;
+	u16_t net_idx, app_idx;
+	u8_t status;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_APP_KEY_STATUS) {
+		BT_WARN("Unexpected App Key Status message");
+		return;
+	}
+
+	status = net_buf_simple_pull_u8(buf);
+	key_idx_unpack(buf, &net_idx, &app_idx);
+
+	param = cli->op_param;
+	if (param->net_idx != net_idx || param->app_idx != app_idx) {
+		BT_WARN("App Key Status key indices did not match");
+		return;
+	}
+
+	if (param->status) {
+		*param->status = status;
+	}
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct mod_app_param {
+	u8_t *status;
+	u16_t elem_addr;
+	u16_t mod_app_idx;
+	u16_t mod_id;
+	u16_t cid;
+};
+
+static void mod_app_status(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf*buf)
+{
+	u16_t elem_addr, mod_app_idx, mod_id, cid;
+	struct mod_app_param *param;
+	u8_t status;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_MOD_APP_STATUS) {
+		BT_WARN("Unexpected Model App Status message");
+		return;
+	}
+
+	status = net_buf_simple_pull_u8(buf);
+	elem_addr = net_buf_simple_pull_le16(buf);
+	mod_app_idx = net_buf_simple_pull_le16(buf);
+
+	if (buf->om_len >= 4) {
+		cid = net_buf_simple_pull_le16(buf);
+	} else {
+		cid = CID_NVAL;
+	}
+
+	mod_id = net_buf_simple_pull_le16(buf);
+
+	param = cli->op_param;
+	if (param->elem_addr != elem_addr ||
+	    param->mod_app_idx != mod_app_idx || param->mod_id != mod_id ||
+	    param->cid != cid) {
+		BT_WARN("Model App Status parameters did not match");
+		return;
+	}
+
+	if (param->status) {
+		*param->status = status;
+	}
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct mod_pub_param {
+	u16_t                       mod_id;
+	u16_t                       cid;
+	u16_t                       elem_addr;
+	u8_t                       *status;
+	struct bt_mesh_cfg_mod_pub *pub;
+};
+
+static void mod_pub_status(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf*buf)
+{
+	u16_t mod_id, cid, elem_addr;
+	struct mod_pub_param *param;
+	u8_t status;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_MOD_PUB_STATUS) {
+		BT_WARN("Unexpected Model Pub Status message");
+		return;
+	}
+
+	param = cli->op_param;
+	if (param->cid != CID_NVAL) {
+		if (buf->om_len < 14) {
+			BT_WARN("Unexpected Mod Pub Status with SIG Model");
+			return;
+		}
+
+		cid = sys_get_le16(&buf->om_data[10]);
+		mod_id = sys_get_le16(&buf->om_data[12]);
+	} else {
+		if (buf->om_len > 12) {
+			BT_WARN("Unexpected Mod Pub Status with Vendor Model");
+			return;
+		}
+
+		cid = CID_NVAL;
+		mod_id = sys_get_le16(&buf->om_data[10]);
+	}
+
+	if (mod_id != param->mod_id || cid != param->cid) {
+		BT_WARN("Mod Pub Model ID or Company ID mismatch");
+		return;
+	}
+
+	status = net_buf_simple_pull_u8(buf);
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (elem_addr != param->elem_addr) {
+		BT_WARN("Model Pub Status for unexpected element (0x%04x)",
+			elem_addr);
+		return;
+	}
+
+	if (param->status) {
+		*param->status = status;
+	}
+
+	if (param->pub) {
+		param->pub->addr = net_buf_simple_pull_le16(buf);
+		param->pub->app_idx = net_buf_simple_pull_le16(buf);
+		param->pub->cred_flag = (param->pub->app_idx & BIT(12));
+		param->pub->app_idx &= BIT_MASK(12);
+		param->pub->ttl = net_buf_simple_pull_u8(buf);
+		param->pub->period = net_buf_simple_pull_u8(buf);
+		param->pub->transmit = net_buf_simple_pull_u8(buf);
+	}
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct mod_sub_param {
+	u8_t *status;
+	u16_t elem_addr;
+	u16_t *sub_addr;
+	u16_t *expect_sub;
+	u16_t mod_id;
+	u16_t cid;
+};
+
+static void mod_sub_status(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf*buf)
+{
+	u16_t elem_addr, sub_addr, mod_id, cid;
+	struct mod_sub_param *param;
+	u8_t status;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_MOD_SUB_STATUS) {
+		BT_WARN("Unexpected Model Subscription Status message");
+		return;
+	}
+
+	status = net_buf_simple_pull_u8(buf);
+	elem_addr = net_buf_simple_pull_le16(buf);
+	sub_addr = net_buf_simple_pull_le16(buf);
+
+	if (buf->om_len >= 4) {
+		cid = net_buf_simple_pull_le16(buf);
+	} else {
+		cid = CID_NVAL;
+	}
+
+	mod_id = net_buf_simple_pull_le16(buf);
+
+	param = cli->op_param;
+	if (param->elem_addr != elem_addr || param->mod_id != mod_id ||
+	    (param->expect_sub && *param->expect_sub != sub_addr) ||
+	    param->cid != cid) {
+		BT_WARN("Model Subscription Status parameters did not match");
+		return;
+	}
+
+	if (param->sub_addr) {
+		*param->sub_addr = sub_addr;
+	}
+
+	if (param->status) {
+		*param->status = status;
+	}
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct hb_sub_param {
+	u8_t *status;
+	struct bt_mesh_cfg_hb_sub *sub;
+};
+
+static void hb_sub_status(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf*buf)
+{
+	struct hb_sub_param *param;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_HEARTBEAT_SUB_STATUS) {
+		BT_WARN("Unexpected Heartbeat Subscription Status message");
+		return;
+	}
+
+	param = cli->op_param;
+
+	*param->status = net_buf_simple_pull_u8(buf);
+
+	param->sub->src = net_buf_simple_pull_le16(buf);
+	param->sub->dst = net_buf_simple_pull_le16(buf);
+	param->sub->period = net_buf_simple_pull_u8(buf);
+	param->sub->count = net_buf_simple_pull_u8(buf);
+	param->sub->min = net_buf_simple_pull_u8(buf);
+	param->sub->max = net_buf_simple_pull_u8(buf);
+
+	k_sem_give(&cli->op_sync);
+}
+
+struct hb_pub_param {
+	u8_t *status;
+	struct bt_mesh_cfg_hb_pub *pub;
+};
+
+static void hb_pub_status(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	struct hb_pub_param *param;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_HEARTBEAT_PUB_STATUS) {
+		BT_WARN("Unexpected Heartbeat Publication Status message");
+		return;
+	}
+
+	param = cli->op_param;
+
+	*param->status = net_buf_simple_pull_u8(buf);
+
+	if (param->pub) {
+		param->pub->dst = net_buf_simple_pull_le16(buf);
+		param->pub->count = net_buf_simple_pull_u8(buf);
+		param->pub->period = net_buf_simple_pull_u8(buf);
+		param->pub->ttl = net_buf_simple_pull_u8(buf);
+		param->pub->feat = net_buf_simple_pull_u8(buf);
+		param->pub->net_idx = net_buf_simple_pull_u8(buf);
+	}
+
+	k_sem_give(&cli->op_sync);
+}
+
+const struct bt_mesh_model_op bt_mesh_cfg_cli_op[] = {
+	{ OP_DEV_COMP_DATA_STATUS,   15,  comp_data_status },
+	{ OP_BEACON_STATUS,          1,   beacon_status },
+	{ OP_DEFAULT_TTL_STATUS,     1,   ttl_status },
+	{ OP_FRIEND_STATUS,          1,   friend_status },
+	{ OP_GATT_PROXY_STATUS,      1,   gatt_proxy_status },
+	{ OP_RELAY_STATUS,           2,   relay_status },
+	{ OP_NET_KEY_STATUS,         3,   net_key_status },
+	{ OP_APP_KEY_STATUS,         4,   app_key_status },
+	{ OP_MOD_APP_STATUS,         7,   mod_app_status },
+	{ OP_MOD_PUB_STATUS,         12,  mod_pub_status },
+	{ OP_MOD_SUB_STATUS,         7,   mod_sub_status },
+	{ OP_HEARTBEAT_SUB_STATUS,   9,   hb_sub_status },
+	{ OP_HEARTBEAT_PUB_STATUS,   10,  hb_pub_status },
+	BT_MESH_MODEL_OP_END,
+};
+
+static int cfg_cli_init(struct bt_mesh_model *model)
+{
+	BT_DBG("");
+
+	if (!bt_mesh_model_in_primary(model)) {
+		BT_ERR("Configuration Client only allowed in primary element");
+		return -EINVAL;
+	}
+
+	if (!model->user_data) {
+		BT_ERR("No Configuration Client context provided");
+		return -EINVAL;
+	}
+
+	cli = model->user_data;
+	cli->model = model;
+
+	/*
+	 * Configuration Model security is device-key based and both the local
+	 * and remote keys are allowed to access this model.
+	 */
+	model->keys[0] = BT_MESH_KEY_DEV_ANY;
+
+	k_sem_init(&cli->op_sync, 0, 1);
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb bt_mesh_cfg_cli_cb = {
+	.init = cfg_cli_init,
+};
+
+static int cli_prepare(void *param, u32_t op)
+{
+	if (!cli) {
+		BT_ERR("No available Configuration Client context!");
+		return -EINVAL;
+	}
+
+	if (cli->op_pending) {
+		BT_WARN("Another synchronous operation pending");
+		return -EBUSY;
+	}
+
+	cli->op_param = param;
+	cli->op_pending = op;
+
+	return 0;
+}
+
+static void cli_reset(void)
+{
+	cli->op_pending = 0;
+	cli->op_param = NULL;
+}
+
+static int cli_wait(void)
+{
+	int err;
+
+	err = k_sem_take(&cli->op_sync, msg_timeout);
+
+	cli_reset();
+
+	return err;
+}
+
+int bt_mesh_cfg_comp_data_get(u16_t net_idx, u16_t addr, u8_t page,
+			      u8_t *status, struct os_mbuf *comp)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_DEV_COMP_DATA_GET, 1);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct comp_data param = {
+		.status = status,
+		.comp = comp,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_DEV_COMP_DATA_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_DEV_COMP_DATA_GET);
+	net_buf_simple_add_u8(msg, page);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+static int get_state_u8(u16_t net_idx, u16_t addr, u32_t op, u32_t rsp,
+			u8_t *val)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(DUMMY_2_BYTE_OP, 0);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	int err;
+
+	err = cli_prepare(val, rsp);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, op);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+static int set_state_u8(u16_t net_idx, u16_t addr, u32_t op, u32_t rsp,
+			u8_t new_val, u8_t *val)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(DUMMY_2_BYTE_OP, 1);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	int err;
+
+	err = cli_prepare(val, rsp);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, op);
+	net_buf_simple_add_u8(msg, new_val);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_beacon_get(u16_t net_idx, u16_t addr, u8_t *status)
+{
+	return get_state_u8(net_idx, addr, OP_BEACON_GET, OP_BEACON_STATUS,
+			    status);
+}
+
+int bt_mesh_cfg_beacon_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *status)
+{
+	return set_state_u8(net_idx, addr, OP_BEACON_SET, OP_BEACON_STATUS,
+			    val, status);
+}
+
+int bt_mesh_cfg_ttl_get(u16_t net_idx, u16_t addr, u8_t *ttl)
+{
+	return get_state_u8(net_idx, addr, OP_DEFAULT_TTL_GET,
+			    OP_DEFAULT_TTL_STATUS, ttl);
+}
+
+int bt_mesh_cfg_ttl_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *ttl)
+{
+	return set_state_u8(net_idx, addr, OP_DEFAULT_TTL_SET,
+			    OP_DEFAULT_TTL_STATUS, val, ttl);
+}
+
+int bt_mesh_cfg_friend_get(u16_t net_idx, u16_t addr, u8_t *status)
+{
+	return get_state_u8(net_idx, addr, OP_FRIEND_GET,
+			    OP_FRIEND_STATUS, status);
+}
+
+int bt_mesh_cfg_friend_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *status)
+{
+	return set_state_u8(net_idx, addr, OP_FRIEND_SET, OP_FRIEND_STATUS,
+			    val, status);
+}
+
+int bt_mesh_cfg_gatt_proxy_get(u16_t net_idx, u16_t addr, u8_t *status)
+{
+	return get_state_u8(net_idx, addr, OP_GATT_PROXY_GET,
+			    OP_GATT_PROXY_STATUS, status);
+}
+
+int bt_mesh_cfg_gatt_proxy_set(u16_t net_idx, u16_t addr, u8_t val,
+			       u8_t *status)
+{
+	return set_state_u8(net_idx, addr, OP_GATT_PROXY_SET,
+			    OP_GATT_PROXY_STATUS, val, status);
+}
+
+int bt_mesh_cfg_relay_get(u16_t net_idx, u16_t addr, u8_t *status,
+			  u8_t *transmit)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_RELAY_GET, 0);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct relay_param param = {
+		.status = status,
+		.transmit = transmit,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_RELAY_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_RELAY_GET);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_relay_set(u16_t net_idx, u16_t addr, u8_t new_relay,
+			  u8_t new_transmit, u8_t *status, u8_t *transmit)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_RELAY_SET, 2);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct relay_param param = {
+		.status = status,
+		.transmit = transmit,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_RELAY_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_RELAY_SET);
+	net_buf_simple_add_u8(msg, new_relay);
+	net_buf_simple_add_u8(msg, new_transmit);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_net_key_add(u16_t net_idx, u16_t addr, u16_t key_net_idx,
+			    const u8_t net_key[16], u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NET_KEY_ADD, 18);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct net_key_param param = {
+		.status = status,
+		.net_idx = key_net_idx,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_NET_KEY_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_NET_KEY_ADD);
+	net_buf_simple_add_le16(msg, key_net_idx);
+	net_buf_simple_add_mem(msg, net_key, 16);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_app_key_add(u16_t net_idx, u16_t addr, u16_t key_net_idx,
+			    u16_t key_app_idx, const u8_t app_key[16],
+			    u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_APP_KEY_ADD, 19);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct app_key_param param = {
+		.status = status,
+		.net_idx = key_net_idx,
+		.app_idx = key_app_idx,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_APP_KEY_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_APP_KEY_ADD);
+	key_idx_pack(msg, key_net_idx, key_app_idx);
+	net_buf_simple_add_mem(msg, app_key, 16);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+static int mod_app_bind(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			u16_t mod_app_idx, u16_t mod_id, u16_t cid,
+			u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_APP_BIND, 8);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct mod_app_param param = {
+		.status = status,
+		.elem_addr = elem_addr,
+		.mod_app_idx = mod_app_idx,
+		.mod_id = mod_id,
+		.cid = cid,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_MOD_APP_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_MOD_APP_BIND);
+	net_buf_simple_add_le16(msg, elem_addr);
+	net_buf_simple_add_le16(msg, mod_app_idx);
+
+	if (cid != CID_NVAL) {
+		net_buf_simple_add_le16(msg, cid);
+	}
+
+	net_buf_simple_add_le16(msg, mod_id);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_mod_app_bind(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			     u16_t mod_app_idx, u16_t mod_id, u8_t *status)
+{
+	return mod_app_bind(net_idx, addr, elem_addr, mod_app_idx, mod_id,
+			    CID_NVAL, status);
+}
+
+int bt_mesh_cfg_mod_app_bind_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				 u16_t mod_app_idx, u16_t mod_id, u16_t cid,
+				 u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_app_bind(net_idx, addr, elem_addr, mod_app_idx, mod_id, cid,
+			    status);
+}
+
+static int mod_sub(u32_t op, u16_t net_idx, u16_t addr, u16_t elem_addr,
+		   u16_t sub_addr, u16_t mod_id, u16_t cid, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(DUMMY_2_BYTE_OP, 8);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct mod_sub_param param = {
+		.status = status,
+		.elem_addr = elem_addr,
+		.expect_sub = &sub_addr,
+		.mod_id = mod_id,
+		.cid = cid,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_MOD_SUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, op);
+	net_buf_simple_add_le16(msg, elem_addr);
+	net_buf_simple_add_le16(msg, sub_addr);
+
+	if (cid != CID_NVAL) {
+		net_buf_simple_add_le16(msg, cid);
+	}
+
+	net_buf_simple_add_le16(msg, mod_id);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_mod_sub_add(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t sub_addr, u16_t mod_id, u8_t *status)
+{
+	return mod_sub(OP_MOD_SUB_ADD, net_idx, addr, elem_addr, sub_addr,
+		       mod_id, CID_NVAL, status);
+}
+
+int bt_mesh_cfg_mod_sub_add_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				 u16_t sub_addr, u16_t mod_id, u16_t cid,
+				 u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_sub(OP_MOD_SUB_ADD, net_idx, addr, elem_addr, sub_addr,
+		       mod_id, cid, status);
+}
+
+int bt_mesh_cfg_mod_sub_del(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t sub_addr, u16_t mod_id, u8_t *status)
+{
+	return mod_sub(OP_MOD_SUB_DEL, net_idx, addr, elem_addr, sub_addr,
+		       mod_id, CID_NVAL, status);
+}
+
+int bt_mesh_cfg_mod_sub_del_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				 u16_t sub_addr, u16_t mod_id, u16_t cid,
+				 u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_sub(OP_MOD_SUB_DEL, net_idx, addr, elem_addr, sub_addr,
+		       mod_id, cid, status);
+}
+
+int bt_mesh_cfg_mod_sub_overwrite(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				  u16_t sub_addr, u16_t mod_id, u8_t *status)
+{
+	return mod_sub(OP_MOD_SUB_OVERWRITE, net_idx, addr, elem_addr,
+		       sub_addr, mod_id, CID_NVAL, status);
+}
+
+int bt_mesh_cfg_mod_sub_overwrite_vnd(u16_t net_idx, u16_t addr,
+				      u16_t elem_addr, u16_t sub_addr,
+				      u16_t mod_id, u16_t cid, u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_sub(OP_MOD_SUB_OVERWRITE, net_idx, addr, elem_addr,
+		       sub_addr, mod_id, cid, status);
+}
+
+static int mod_sub_va(u32_t op, u16_t net_idx, u16_t addr, u16_t elem_addr,
+		      const u8_t label[16], u16_t mod_id, u16_t cid,
+		      u16_t *virt_addr, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(DUMMY_2_BYTE_OP, 22);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct mod_sub_param param = {
+		.status = status,
+		.elem_addr = elem_addr,
+		.sub_addr = virt_addr,
+		.mod_id = mod_id,
+		.cid = cid,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_MOD_SUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	BT_DBG("net_idx 0x%04x addr 0x%04x elem_addr 0x%04x label %s",
+	       net_idx, addr, elem_addr, label);
+	BT_DBG("mod_id 0x%04x cid 0x%04x", mod_id, cid);
+
+	bt_mesh_model_msg_init(msg, op);
+	net_buf_simple_add_le16(msg, elem_addr);
+	net_buf_simple_add_mem(msg, label, 16);
+
+	if (cid != CID_NVAL) {
+		net_buf_simple_add_le16(msg, cid);
+	}
+
+	net_buf_simple_add_le16(msg, mod_id);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_mod_sub_va_add(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			       const u8_t label[16], u16_t mod_id,
+			       u16_t *virt_addr, u8_t *status)
+{
+	return mod_sub_va(OP_MOD_SUB_VA_ADD, net_idx, addr, elem_addr, label,
+			  mod_id, CID_NVAL, virt_addr, status);
+}
+
+int bt_mesh_cfg_mod_sub_va_add_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				   const u8_t label[16], u16_t mod_id,
+				   u16_t cid, u16_t *virt_addr, u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_sub_va(OP_MOD_SUB_VA_ADD, net_idx, addr, elem_addr, label,
+			  mod_id, cid, virt_addr, status);
+}
+
+int bt_mesh_cfg_mod_sub_va_del(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			       const u8_t label[16], u16_t mod_id,
+			       u16_t *virt_addr, u8_t *status)
+{
+	return mod_sub_va(OP_MOD_SUB_VA_DEL, net_idx, addr, elem_addr, label,
+			  mod_id, CID_NVAL, virt_addr, status);
+}
+
+int bt_mesh_cfg_mod_sub_va_del_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				   const u8_t label[16], u16_t mod_id,
+				   u16_t cid, u16_t *virt_addr, u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_sub_va(OP_MOD_SUB_VA_DEL, net_idx, addr, elem_addr, label,
+			  mod_id, cid, virt_addr, status);
+}
+
+int bt_mesh_cfg_mod_sub_va_overwrite(u16_t net_idx, u16_t addr,
+				     u16_t elem_addr, const u8_t label[16],
+				     u16_t mod_id, u16_t *virt_addr,
+				     u8_t *status)
+{
+	return mod_sub_va(OP_MOD_SUB_VA_OVERWRITE, net_idx, addr, elem_addr,
+			  label, mod_id, CID_NVAL, virt_addr, status);
+}
+
+int bt_mesh_cfg_mod_sub_va_overwrite_vnd(u16_t net_idx, u16_t addr,
+					 u16_t elem_addr, const u8_t label[16],
+					 u16_t mod_id, u16_t cid,
+					 u16_t *virt_addr, u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_sub_va(OP_MOD_SUB_VA_OVERWRITE, net_idx, addr, elem_addr,
+			  label, mod_id, cid, virt_addr, status);
+}
+
+static int mod_pub_get(u16_t net_idx, u16_t addr, u16_t elem_addr,
+		       u16_t mod_id, u16_t cid,
+		       struct bt_mesh_cfg_mod_pub *pub, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_PUB_GET, 6);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct mod_pub_param param = {
+		.mod_id = mod_id,
+		.cid = cid,
+		.elem_addr = elem_addr,
+		.status = status,
+		.pub = pub,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_MOD_PUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_MOD_PUB_GET);
+
+	net_buf_simple_add_le16(msg, elem_addr);
+
+	if (cid != CID_NVAL) {
+		net_buf_simple_add_le16(msg, cid);
+	}
+
+	net_buf_simple_add_le16(msg, mod_id);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_mod_pub_get(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
+			    u8_t *status)
+{
+	return mod_pub_get(net_idx, addr, elem_addr, mod_id, CID_NVAL,
+			   pub, status);
+}
+
+int bt_mesh_cfg_mod_pub_get_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				u16_t mod_id, u16_t cid,
+				struct bt_mesh_cfg_mod_pub *pub, u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_pub_get(net_idx, addr, elem_addr, mod_id, cid, pub, status);
+}
+
+static int mod_pub_set(u16_t net_idx, u16_t addr, u16_t elem_addr,
+		       u16_t mod_id, u16_t cid,
+		       struct bt_mesh_cfg_mod_pub *pub, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_PUB_SET, 13);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct mod_pub_param param = {
+		.mod_id = mod_id,
+		.cid = cid,
+		.elem_addr = elem_addr,
+		.status = status,
+		.pub = pub,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_MOD_PUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_MOD_PUB_SET);
+
+	net_buf_simple_add_le16(msg, elem_addr);
+	net_buf_simple_add_le16(msg, pub->addr);
+	net_buf_simple_add_le16(msg, (pub->app_idx | (pub->cred_flag << 12)));
+	net_buf_simple_add_u8(msg, pub->ttl);
+	net_buf_simple_add_u8(msg, pub->period);
+	net_buf_simple_add_u8(msg, pub->transmit);
+
+	if (cid != CID_NVAL) {
+		net_buf_simple_add_le16(msg, cid);
+	}
+
+	net_buf_simple_add_le16(msg, mod_id);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_mod_pub_set(u16_t net_idx, u16_t addr, u16_t elem_addr,
+			    u16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
+			    u8_t *status)
+{
+	return mod_pub_set(net_idx, addr, elem_addr, mod_id, CID_NVAL,
+			   pub, status);
+}
+
+int bt_mesh_cfg_mod_pub_set_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
+				u16_t mod_id, u16_t cid,
+				struct bt_mesh_cfg_mod_pub *pub, u8_t *status)
+{
+	if (cid == CID_NVAL) {
+		return -EINVAL;
+	}
+
+	return mod_pub_set(net_idx, addr, elem_addr, mod_id, cid, pub, status);
+}
+
+int bt_mesh_cfg_hb_sub_set(u16_t net_idx, u16_t addr,
+			   struct bt_mesh_cfg_hb_sub *sub, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEARTBEAT_SUB_SET, 5);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct hb_sub_param param = {
+		.status = status,
+		.sub = sub,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEARTBEAT_SUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEARTBEAT_SUB_SET);
+	net_buf_simple_add_le16(msg, sub->src);
+	net_buf_simple_add_le16(msg, sub->dst);
+	net_buf_simple_add_u8(msg, sub->period);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_hb_sub_get(u16_t net_idx, u16_t addr,
+			   struct bt_mesh_cfg_hb_sub *sub, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEARTBEAT_SUB_GET, 0);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct hb_sub_param param = {
+		.status = status,
+		.sub = sub,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEARTBEAT_SUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEARTBEAT_SUB_GET);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_hb_pub_set(u16_t net_idx, u16_t addr,
+			   const struct bt_mesh_cfg_hb_pub *pub, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEARTBEAT_PUB_SET, 9);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV_REMOTE,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct hb_pub_param param = {
+		.status = status,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEARTBEAT_PUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEARTBEAT_PUB_SET);
+	net_buf_simple_add_le16(msg, pub->dst);
+	net_buf_simple_add_u8(msg, pub->count);
+	net_buf_simple_add_u8(msg, pub->period);
+	net_buf_simple_add_u8(msg, pub->ttl);
+	net_buf_simple_add_le16(msg, pub->feat);
+	net_buf_simple_add_le16(msg, pub->net_idx);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_cfg_hb_pub_get(u16_t net_idx, u16_t addr,
+			   struct bt_mesh_cfg_hb_pub *pub, u8_t *status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEARTBEAT_PUB_GET, 0);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = BT_MESH_KEY_DEV,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct hb_pub_param param = {
+		.status = status,
+		.pub = pub,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEARTBEAT_PUB_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEARTBEAT_PUB_GET);
+
+	err = bt_mesh_model_send(cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!status) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+s32_t bt_mesh_cfg_cli_timeout_get(void)
+{
+	return msg_timeout;
+}
+
+void bt_mesh_cfg_cli_timeout_set(s32_t timeout)
+{
+	msg_timeout = timeout;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/cfg_srv.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/cfg_srv.c
new file mode 100644
index 00000000..57aac90a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/cfg_srv.c
@@ -0,0 +1,3619 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_MODEL_LOG
+
+#include <string.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include "mesh/mesh.h"
+
+#include "mesh_priv.h"
+#include "adv.h"
+#include "net.h"
+#include "lpn.h"
+#include "transport.h"
+#include "crypto.h"
+#include "access.h"
+#include "beacon.h"
+#include "proxy.h"
+#include "foundation.h"
+#include "friend.h"
+#include "testing.h"
+#include "settings.h"
+
+#define DEFAULT_TTL 7
+
+static struct bt_mesh_cfg_srv *conf;
+
+static struct label labels[CONFIG_BT_MESH_LABEL_COUNT];
+
+static int comp_add_elem(struct os_mbuf *buf, struct bt_mesh_elem *elem,
+			 bool primary)
+{
+	struct bt_mesh_model *mod;
+	int i;
+
+	if (net_buf_simple_tailroom(buf) <
+	    4 + (elem->model_count * 2) + (elem->vnd_model_count * 2)) {
+		BT_ERR("Too large device composition");
+		return -E2BIG;
+	}
+
+	net_buf_simple_add_le16(buf, elem->loc);
+
+	net_buf_simple_add_u8(buf, elem->model_count);
+	net_buf_simple_add_u8(buf, elem->vnd_model_count);
+
+	for (i = 0; i < elem->model_count; i++) {
+		mod = &elem->models[i];
+		net_buf_simple_add_le16(buf, mod->id);
+	}
+
+	for (i = 0; i < elem->vnd_model_count; i++) {
+		mod = &elem->vnd_models[i];
+		net_buf_simple_add_le16(buf, mod->vnd.company);
+		net_buf_simple_add_le16(buf, mod->vnd.id);
+	}
+
+	return 0;
+}
+
+static int comp_get_page_0(struct os_mbuf *buf)
+{
+	u16_t feat = 0;
+	const struct bt_mesh_comp *comp;
+	int i;
+
+	comp = bt_mesh_comp_get();
+
+	if ((MYNEWT_VAL(BLE_MESH_RELAY))) {
+		feat |= BT_MESH_FEAT_RELAY;
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
+		feat |= BT_MESH_FEAT_PROXY;
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_FRIEND))) {
+		feat |= BT_MESH_FEAT_FRIEND;
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
+		feat |= BT_MESH_FEAT_LOW_POWER;
+	}
+
+	net_buf_simple_add_le16(buf, comp->cid);
+	net_buf_simple_add_le16(buf, comp->pid);
+	net_buf_simple_add_le16(buf, comp->vid);
+	net_buf_simple_add_le16(buf, MYNEWT_VAL(BLE_MESH_CRPL));
+	net_buf_simple_add_le16(buf, feat);
+
+	for (i = 0; i < comp->elem_count; i++) {
+		int err;
+
+		err = comp_add_elem(buf, &comp->elem[i], i == 0);
+		if (err) {
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static void dev_comp_data_get(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	u8_t page;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	page = net_buf_simple_pull_u8(buf);
+	if (page != 0U) {
+		BT_DBG("Composition page %u not available", page);
+		page = 0U;
+	}
+
+	bt_mesh_model_msg_init(sdu, OP_DEV_COMP_DATA_STATUS);
+
+	net_buf_simple_add_u8(sdu, page);
+	if (comp_get_page_0(sdu) < 0) {
+		BT_ERR("Unable to get composition page 0");
+		goto done;
+	}
+
+	if (bt_mesh_model_send(model, ctx, sdu, NULL, NULL)) {
+		BT_ERR("Unable to send Device Composition Status response");
+	}
+
+done:
+	os_mbuf_free_chain(sdu);
+}
+
+static struct bt_mesh_model *get_model(struct bt_mesh_elem *elem,
+				       struct os_mbuf *buf, bool *vnd)
+{
+	if (buf->om_len < 4) {
+		u16_t id;
+
+		id = net_buf_simple_pull_le16(buf);
+
+		BT_DBG("ID 0x%04x addr 0x%04x", id, elem->addr);
+
+		*vnd = false;
+
+		return bt_mesh_model_find(elem, id);
+	} else {
+		u16_t company, id;
+
+		company = net_buf_simple_pull_le16(buf);
+		id = net_buf_simple_pull_le16(buf);
+
+		BT_DBG("Company 0x%04x ID 0x%04x addr 0x%04x", company, id,
+		       elem->addr);
+
+		*vnd = true;
+
+		return bt_mesh_model_find_vnd(elem, company, id);
+	}
+}
+
+static bool app_key_is_valid(u16_t app_idx)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+
+		if (key->net_idx != BT_MESH_KEY_UNUSED &&
+		    key->app_idx == app_idx) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static u8_t _mod_pub_set(struct bt_mesh_model *model, u16_t pub_addr,
+			 u16_t app_idx, u8_t cred_flag, u8_t ttl, u8_t period,
+			 u8_t retransmit, bool store)
+{
+	if (!model->pub) {
+		return STATUS_NVAL_PUB_PARAM;
+	}
+
+	if (!(MYNEWT_VAL(BLE_MESH_LOW_POWER)) && cred_flag) {
+		return STATUS_FEAT_NOT_SUPP;
+	}
+
+	if (!model->pub->update && period) {
+		return STATUS_NVAL_PUB_PARAM;
+	}
+
+	if (pub_addr == BT_MESH_ADDR_UNASSIGNED) {
+		if (model->pub->addr == BT_MESH_ADDR_UNASSIGNED) {
+			return STATUS_SUCCESS;
+		}
+
+		model->pub->addr = BT_MESH_ADDR_UNASSIGNED;
+		model->pub->key = 0;
+		model->pub->cred = 0;
+		model->pub->ttl = 0;
+		model->pub->period = 0;
+		model->pub->retransmit = 0;
+		model->pub->count = 0;
+
+		if (model->pub->update) {
+			k_delayed_work_cancel(&model->pub->timer);
+		}
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+			bt_mesh_store_mod_pub(model);
+		}
+
+		return STATUS_SUCCESS;
+	}
+
+	if (!bt_mesh_app_key_find(app_idx)) {
+		return STATUS_INVALID_APPKEY;
+	}
+
+	model->pub->addr = pub_addr;
+	model->pub->key = app_idx;
+	model->pub->cred = cred_flag;
+	model->pub->ttl = ttl;
+	model->pub->period = period;
+	model->pub->retransmit = retransmit;
+
+	if (model->pub->update) {
+		s32_t period_ms;
+
+		period_ms = bt_mesh_model_pub_period_get(model);
+		BT_DBG("period %u ms", (unsigned) period_ms);
+
+		if (period_ms) {
+			k_delayed_work_submit(&model->pub->timer, period_ms);
+		} else {
+			k_delayed_work_cancel(&model->pub->timer);
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+		bt_mesh_store_mod_pub(model);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+u8_t mod_bind(struct bt_mesh_model *model, u16_t key_idx)
+{
+	int i;
+
+	BT_DBG("model %p key_idx 0x%03x", model, key_idx);
+
+	if (!app_key_is_valid(key_idx)) {
+		return STATUS_INVALID_APPKEY;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(model->keys); i++) {
+		/* Treat existing binding as success */
+		if (model->keys[i] == key_idx) {
+			return STATUS_SUCCESS;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(model->keys); i++) {
+		if (model->keys[i] == BT_MESH_KEY_UNUSED) {
+			model->keys[i] = key_idx;
+
+			if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+				bt_mesh_store_mod_bind(model);
+			}
+
+			return STATUS_SUCCESS;
+		}
+	}
+
+	return STATUS_INSUFF_RESOURCES;
+}
+
+u8_t mod_unbind(struct bt_mesh_model *model, u16_t key_idx, bool store)
+{
+	int i;
+
+	BT_DBG("model %p key_idx 0x%03x store %u", model, key_idx, store);
+
+	if (!app_key_is_valid(key_idx)) {
+		return STATUS_INVALID_APPKEY;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(model->keys); i++) {
+		if (model->keys[i] != key_idx) {
+			continue;
+		}
+
+		model->keys[i] = BT_MESH_KEY_UNUSED;
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+			bt_mesh_store_mod_bind(model);
+		}
+
+		if (model->pub && model->pub->key == key_idx) {
+			_mod_pub_set(model, BT_MESH_ADDR_UNASSIGNED,
+				     0, 0, 0, 0, 0, store);
+		}
+	}
+
+	return STATUS_SUCCESS;
+}
+
+struct bt_mesh_app_key *bt_mesh_app_key_alloc(u16_t app_idx)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+
+		if (key->net_idx == BT_MESH_KEY_UNUSED) {
+			return key;
+		}
+	}
+
+	return NULL;
+}
+
+static u8_t app_key_set(u16_t net_idx, u16_t app_idx, const u8_t val[16],
+			bool update)
+{
+	struct bt_mesh_app_keys *keys;
+	struct bt_mesh_app_key *key;
+	struct bt_mesh_subnet *sub;
+
+	BT_DBG("net_idx 0x%04x app_idx %04x update %u val %s",
+	       net_idx, app_idx, update, bt_hex(val, 16));
+
+	sub = bt_mesh_subnet_get(net_idx);
+	if (!sub) {
+		return STATUS_INVALID_NETKEY;
+	}
+
+	key = bt_mesh_app_key_find(app_idx);
+	if (update) {
+		if (!key) {
+			return STATUS_INVALID_APPKEY;
+		}
+
+		if (key->net_idx != net_idx) {
+			return STATUS_INVALID_BINDING;
+		}
+
+		keys = &key->keys[1];
+
+		/* The AppKey Update message shall generate an error when node
+		 * is in normal operation, Phase 2, or Phase 3 or in Phase 1
+		 * when the AppKey Update message on a valid AppKeyIndex when
+		 * the AppKey value is different.
+		 */
+		if (sub->kr_phase != BT_MESH_KR_PHASE_1) {
+			return STATUS_CANNOT_UPDATE;
+		}
+
+		if (key->updated) {
+			if (memcmp(keys->val, val, 16)) {
+				return STATUS_CANNOT_UPDATE;
+			} else {
+				return STATUS_SUCCESS;
+			}
+		}
+
+		key->updated = true;
+	} else {
+		if (key) {
+			if (key->net_idx == net_idx &&
+			    !memcmp(key->keys[0].val, val, 16)) {
+				return STATUS_SUCCESS;
+			}
+
+			if (key->net_idx == net_idx) {
+				return STATUS_IDX_ALREADY_STORED;
+			} else {
+				return STATUS_INVALID_NETKEY;
+			}
+		}
+
+		key = bt_mesh_app_key_alloc(app_idx);
+		if (!key) {
+			return STATUS_INSUFF_RESOURCES;
+		}
+
+		keys = &key->keys[0];
+	}
+
+	if (bt_mesh_app_id(val, &keys->id)) {
+		if (update) {
+			key->updated = false;
+		}
+
+		return STATUS_STORAGE_FAIL;
+	}
+
+	BT_DBG("app_idx 0x%04x AID 0x%02x", app_idx, keys->id);
+
+	key->net_idx = net_idx;
+	key->app_idx = app_idx;
+	memcpy(keys->val, val, 16);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		BT_DBG("Storing AppKey persistently");
+		bt_mesh_store_app_key(key);
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static void app_key_add(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_APP_KEY_STATUS, 4);
+	u16_t key_net_idx, key_app_idx;
+	u8_t status;
+
+	key_idx_unpack(buf, &key_net_idx, &key_app_idx);
+
+	BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx);
+
+	bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS);
+
+	status = app_key_set(key_net_idx, key_app_idx, buf->om_data, false);
+	BT_DBG("status 0x%02x", status);
+	net_buf_simple_add_u8(msg, status);
+
+	key_idx_pack(msg, key_net_idx, key_app_idx);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send App Key Status response");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void app_key_update(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_APP_KEY_STATUS, 4);
+	u16_t key_net_idx, key_app_idx;
+	u8_t status;
+
+	key_idx_unpack(buf, &key_net_idx, &key_app_idx);
+
+	BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx);
+
+	bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS);
+
+	status = app_key_set(key_net_idx, key_app_idx, buf->om_data, true);
+	BT_DBG("status 0x%02x", status);
+	net_buf_simple_add_u8(msg, status);
+
+	key_idx_pack(msg, key_net_idx, key_app_idx);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send App Key Status response");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+struct unbind_data {
+	u16_t app_idx;
+	bool store;
+};
+
+static void _mod_unbind(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
+			bool vnd, bool primary, void *user_data)
+{
+	struct unbind_data *data = user_data;
+
+	mod_unbind(mod, data->app_idx, data->store);
+}
+
+void bt_mesh_app_key_del(struct bt_mesh_app_key *key, bool store)
+{
+	struct unbind_data data = { .app_idx = key->app_idx, .store = store };
+
+	BT_DBG("AppIdx 0x%03x store %u", key->app_idx, store);
+
+	bt_mesh_model_foreach(_mod_unbind, &data);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+		bt_mesh_clear_app_key(key);
+	}
+
+	key->net_idx = BT_MESH_KEY_UNUSED;
+	memset(key->keys, 0, sizeof(key->keys));
+}
+
+static void app_key_del(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_APP_KEY_STATUS, 4);
+	u16_t key_net_idx, key_app_idx;
+	struct bt_mesh_app_key *key;
+	u8_t status;
+
+	key_idx_unpack(buf, &key_net_idx, &key_app_idx);
+
+	BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx);
+
+	if (!bt_mesh_subnet_get(key_net_idx)) {
+		status = STATUS_INVALID_NETKEY;
+		goto send_status;
+	}
+
+	key = bt_mesh_app_key_find(key_app_idx);
+	if (!key) {
+		/* Treat as success since the client might have missed a
+		 * previous response and is resending the request.
+		 */
+		status = STATUS_SUCCESS;
+		goto send_status;
+	}
+
+	if (key->net_idx != key_net_idx) {
+		status = STATUS_INVALID_BINDING;
+		goto send_status;
+	}
+
+	bt_mesh_app_key_del(key, true);
+	status = STATUS_SUCCESS;
+
+send_status:
+	bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+
+	key_idx_pack(msg, key_net_idx, key_app_idx);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send App Key Status response");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+/* Index list length: 3 bytes for every pair and 2 bytes for an odd idx */
+#define IDX_LEN(num) (((num) / 2) * 3 + ((num) % 2) * 2)
+
+static void app_key_get(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct os_mbuf *msg =
+		BT_MESH_MODEL_BUF(OP_APP_KEY_LIST,
+				  3 + IDX_LEN(CONFIG_BT_MESH_APP_KEY_COUNT));
+	u16_t get_idx, i, prev;
+	u8_t status;
+
+	get_idx = net_buf_simple_pull_le16(buf);
+	if (get_idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", get_idx);
+		goto done;
+	}
+
+	BT_DBG("idx 0x%04x", get_idx);
+
+	bt_mesh_model_msg_init(msg, OP_APP_KEY_LIST);
+
+	if (!bt_mesh_subnet_get(get_idx)) {
+		status = STATUS_INVALID_NETKEY;
+	} else {
+		status = STATUS_SUCCESS;
+	}
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, get_idx);
+
+	if (status != STATUS_SUCCESS) {
+		goto send_status;
+	}
+
+	prev = BT_MESH_KEY_UNUSED;
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+
+		if (key->net_idx != get_idx) {
+			continue;
+		}
+
+		if (prev == BT_MESH_KEY_UNUSED) {
+			prev = key->app_idx;
+			continue;
+		}
+
+		key_idx_pack(msg, prev, key->app_idx);
+		prev = BT_MESH_KEY_UNUSED;
+	}
+
+	if (prev != BT_MESH_KEY_UNUSED) {
+		net_buf_simple_add_le16(msg, prev);
+	}
+
+send_status:
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send AppKey List");
+	}
+
+done:
+    os_mbuf_free_chain(msg);
+}
+
+static void beacon_get(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_BEACON_STATUS, 1);
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	bt_mesh_model_msg_init(msg, OP_BEACON_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_beacon_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Config Beacon Status response");
+	}
+	os_mbuf_free_chain(msg);
+}
+
+static void beacon_set(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_BEACON_STATUS, 1);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (!cfg) {
+		BT_WARN("No Configuration Server context available");
+	} else if (buf->om_data[0] == 0x00 || buf->om_data[0] == 0x01) {
+		if (buf->om_data[0] != cfg->beacon) {
+			cfg->beacon = buf->om_data[0];
+
+			if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+				bt_mesh_store_cfg();
+			}
+
+			if (cfg->beacon) {
+				bt_mesh_beacon_enable();
+			} else {
+				bt_mesh_beacon_disable();
+			}
+		}
+	} else {
+		BT_WARN("Invalid Config Beacon value 0x%02x", buf->om_data[0]);
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_BEACON_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_beacon_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Config Beacon Status response");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+
+}
+
+static void default_ttl_get(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_DEFAULT_TTL_STATUS, 1);
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	bt_mesh_model_msg_init(msg, OP_DEFAULT_TTL_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_default_ttl_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Default TTL Status response");
+	}
+
+	os_mbuf_free_chain(msg);
+
+}
+
+static void default_ttl_set(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_DEFAULT_TTL_STATUS, 1);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (!cfg) {
+		BT_WARN("No Configuration Server context available");
+	} else if (buf->om_data[0] <= BT_MESH_TTL_MAX && buf->om_data[0] != 0x01) {
+		if (cfg->default_ttl != buf->om_data[0]) {
+			cfg->default_ttl = buf->om_data[0];
+
+			if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+				bt_mesh_store_cfg();
+			}
+		}
+	} else {
+		BT_WARN("Prohibited Default TTL value 0x%02x", buf->om_data[0]);
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_DEFAULT_TTL_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_default_ttl_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Default TTL Status response");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+}
+
+static void send_gatt_proxy_status(struct bt_mesh_model *model,
+				   struct bt_mesh_msg_ctx *ctx)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_GATT_PROXY_STATUS, 1);
+
+	bt_mesh_model_msg_init(msg, OP_GATT_PROXY_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_gatt_proxy_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send GATT Proxy Status");
+	}
+
+	os_mbuf_free_chain(msg);
+
+}
+
+static void gatt_proxy_get(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	send_gatt_proxy_status(model, ctx);
+}
+
+static void gatt_proxy_set(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (buf->om_data[0] != 0x00 && buf->om_data[0] != 0x01) {
+		BT_WARN("Invalid GATT Proxy value 0x%02x", buf->om_data[0]);
+		return;
+	}
+
+	if (!(MYNEWT_VAL(BLE_MESH_GATT_PROXY)) ||
+	    bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_NOT_SUPPORTED) {
+		goto send_status;
+	}
+
+	if (!cfg) {
+		BT_WARN("No Configuration Server context available");
+		goto send_status;
+	}
+
+	BT_DBG("GATT Proxy 0x%02x -> 0x%02x", cfg->gatt_proxy, buf->om_data[0]);
+
+	if (cfg->gatt_proxy == buf->om_data[0]) {
+		goto send_status;
+	}
+
+	cfg->gatt_proxy = buf->om_data[0];
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_cfg();
+	}
+
+	bt_mesh_adv_update();
+
+	if (cfg->hb_pub.feat & BT_MESH_FEAT_PROXY) {
+		bt_mesh_heartbeat_send();
+	}
+
+send_status:
+	send_gatt_proxy_status(model, ctx);
+}
+
+static void net_transmit_get(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx,
+			     struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NET_TRANSMIT_STATUS, 1);
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	bt_mesh_model_msg_init(msg, OP_NET_TRANSMIT_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_net_transmit_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Config Network Transmit Status");
+	}
+
+	os_mbuf_free_chain(msg);
+
+}
+
+static void net_transmit_set(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx,
+			     struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NET_TRANSMIT_STATUS, 1);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	BT_DBG("Transmit 0x%02x (count %u interval %ums)", buf->om_data[0],
+	       BT_MESH_TRANSMIT_COUNT(buf->om_data[0]),
+	       BT_MESH_TRANSMIT_INT(buf->om_data[0]));
+
+	if (!cfg) {
+		BT_WARN("No Configuration Server context available");
+	} else {
+		cfg->net_transmit = buf->om_data[0];
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+			bt_mesh_store_cfg();
+		}
+	}
+
+	bt_mesh_model_msg_init(msg, OP_NET_TRANSMIT_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_net_transmit_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Network Transmit Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void relay_get(struct bt_mesh_model *model,
+		      struct bt_mesh_msg_ctx *ctx,
+		      struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_RELAY_STATUS, 2);
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	bt_mesh_model_msg_init(msg, OP_RELAY_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_relay_get());
+	net_buf_simple_add_u8(msg, bt_mesh_relay_retransmit_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Config Relay Status response");
+	}
+
+	os_mbuf_free_chain(msg);
+
+}
+
+static void relay_set(struct bt_mesh_model *model,
+		      struct bt_mesh_msg_ctx *ctx,
+		      struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_RELAY_STATUS, 2);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (!cfg) {
+		BT_WARN("No Configuration Server context available");
+	} else if (buf->om_data[0] == 0x00 || buf->om_data[0] == 0x01) {
+		bool change;
+
+		if (cfg->relay == BT_MESH_RELAY_NOT_SUPPORTED) {
+			change = false;
+		} else {
+			change = (cfg->relay != buf->om_data[0]);
+			cfg->relay = buf->om_data[0];
+			cfg->relay_retransmit = buf->om_data[1];
+
+			if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+				bt_mesh_store_cfg();
+			}
+		}
+
+		BT_DBG("Relay 0x%02x (%s) xmit 0x%02x (count %u interval %u)",
+		       cfg->relay, change ? "changed" : "not changed",
+		       cfg->relay_retransmit,
+		       BT_MESH_TRANSMIT_COUNT(cfg->relay_retransmit),
+		       BT_MESH_TRANSMIT_INT(cfg->relay_retransmit));
+
+		if ((cfg->hb_pub.feat & BT_MESH_FEAT_RELAY) && change) {
+			bt_mesh_heartbeat_send();
+		}
+	} else {
+		BT_WARN("Invalid Relay value 0x%02x", buf->om_data[0]);
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_RELAY_STATUS);
+	net_buf_simple_add_u8(msg, bt_mesh_relay_get());
+	net_buf_simple_add_u8(msg, bt_mesh_relay_retransmit_get());
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Relay Status response");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+
+}
+
+static void send_mod_pub_status(struct bt_mesh_model *cfg_mod,
+				struct bt_mesh_msg_ctx *ctx,
+				u16_t elem_addr, u16_t pub_addr,
+				bool vnd, struct bt_mesh_model *mod,
+				u8_t status, u8_t *mod_id)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_PUB_STATUS, 14);
+
+	bt_mesh_model_msg_init(msg, OP_MOD_PUB_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, elem_addr);
+
+	if (status != STATUS_SUCCESS) {
+		memset(net_buf_simple_add(msg, 7), 0, 7);
+	} else {
+		u16_t idx_cred;
+
+		net_buf_simple_add_le16(msg, pub_addr);
+
+		idx_cred = mod->pub->key | (u16_t)mod->pub->cred << 12;
+		net_buf_simple_add_le16(msg, idx_cred);
+		net_buf_simple_add_u8(msg, mod->pub->ttl);
+		net_buf_simple_add_u8(msg, mod->pub->period);
+		net_buf_simple_add_u8(msg, mod->pub->retransmit);
+	}
+
+	if (vnd) {
+		memcpy(net_buf_simple_add(msg, 4), mod_id, 4);
+	} else {
+		memcpy(net_buf_simple_add(msg, 2), mod_id, 2);
+	}
+
+	if (bt_mesh_model_send(cfg_mod, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Model Publication Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void mod_pub_get(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	u16_t elem_addr, pub_addr = 0;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id, status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	mod_id = buf->om_data;
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	if (!mod->pub) {
+		status = STATUS_NVAL_PUB_PARAM;
+		goto send_status;
+	}
+
+	pub_addr = mod->pub->addr;
+	status = STATUS_SUCCESS;
+
+send_status:
+	send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
+			    status, mod_id);
+}
+
+static void mod_pub_set(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	u8_t retransmit, status, pub_ttl, pub_period, cred_flag;
+	u16_t elem_addr, pub_addr, pub_app_idx;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	pub_addr = net_buf_simple_pull_le16(buf);
+	pub_app_idx = net_buf_simple_pull_le16(buf);
+	cred_flag = ((pub_app_idx >> 12) & BIT_MASK(1));
+	pub_app_idx &= BIT_MASK(12);
+
+	pub_ttl = net_buf_simple_pull_u8(buf);
+	if (pub_ttl > BT_MESH_TTL_MAX && pub_ttl != BT_MESH_TTL_DEFAULT) {
+		BT_ERR("Invalid TTL value 0x%02x", pub_ttl);
+		return;
+	}
+
+	pub_period = net_buf_simple_pull_u8(buf);
+	retransmit = net_buf_simple_pull_u8(buf);
+	mod_id = buf->om_data;
+
+	BT_DBG("elem_addr 0x%04x pub_addr 0x%04x cred_flag %u",
+	       elem_addr, pub_addr, cred_flag);
+	BT_DBG("pub_app_idx 0x%03x, pub_ttl %u pub_period 0x%02x",
+	       pub_app_idx, pub_ttl, pub_period);
+	BT_DBG("retransmit 0x%02x (count %u interval %ums)", retransmit,
+	       BT_MESH_PUB_TRANSMIT_COUNT(retransmit),
+	       BT_MESH_PUB_TRANSMIT_INT(retransmit));
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = _mod_pub_set(mod, pub_addr, pub_app_idx, cred_flag, pub_ttl,
+			      pub_period, retransmit, true);
+
+send_status:
+	send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
+			    status, mod_id);
+}
+
+struct label *get_label(u16_t index)
+{
+	if (index >= ARRAY_SIZE(labels)) {
+		return NULL;
+	}
+
+	return &labels[index];
+}
+
+#if CONFIG_BT_MESH_LABEL_COUNT > 0
+static inline void va_store(struct label *store)
+{
+	atomic_set_bit(store->flags, BT_MESH_VA_CHANGED);
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_label();
+	}
+}
+
+static struct label *va_find(const u8_t *label_uuid,
+				struct label **free_slot)
+{
+	struct label *match = NULL;
+	int i;
+
+	if (free_slot != NULL) {
+		*free_slot = NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(labels); i++) {
+		if (labels[i].ref == 0) {
+			if (free_slot != NULL) {
+				*free_slot = &labels[i];
+			}
+			continue;
+		}
+
+		if (!memcmp(labels[i].uuid, label_uuid, 16)) {
+			match = &labels[i];
+		}
+	}
+
+	return match;
+}
+
+static u8_t va_add(u8_t *label_uuid, u16_t *addr)
+{
+	struct label *update, *free_slot = NULL;
+
+	update = va_find(label_uuid, &free_slot);
+	if (update) {
+		update->ref++;
+		va_store(update);
+		return 0;
+	}
+
+	if (!free_slot) {
+		return STATUS_INSUFF_RESOURCES;
+	}
+
+	if (bt_mesh_virtual_addr(label_uuid, addr) < 0) {
+		return STATUS_UNSPECIFIED;
+	}
+
+	free_slot->ref = 1;
+	free_slot->addr = *addr;
+	memcpy(free_slot->uuid, label_uuid, 16);
+	va_store(free_slot);
+
+	return STATUS_SUCCESS;
+}
+
+static u8_t va_del(u8_t *label_uuid, u16_t *addr)
+{
+	struct label *update;
+
+	update = va_find(label_uuid, NULL);
+	if (update) {
+		update->ref--;
+
+		if (addr) {
+			*addr = update->addr;
+		}
+
+		va_store(update);
+	}
+
+	if (addr) {
+		*addr = BT_MESH_ADDR_UNASSIGNED;
+	}
+
+	return STATUS_CANNOT_REMOVE;
+}
+
+static size_t mod_sub_list_clear(struct bt_mesh_model *mod)
+{
+	u8_t *label_uuid;
+	size_t clear_count;
+	int i;
+
+	/* Unref stored labels related to this model */
+	for (i = 0, clear_count = 0; i < ARRAY_SIZE(mod->groups); i++) {
+		if (!BT_MESH_ADDR_IS_VIRTUAL(mod->groups[i])) {
+			if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) {
+				mod->groups[i] = BT_MESH_ADDR_UNASSIGNED;
+				clear_count++;
+			}
+
+			continue;
+		}
+
+		label_uuid = bt_mesh_label_uuid_get(mod->groups[i]);
+
+		mod->groups[i] = BT_MESH_ADDR_UNASSIGNED;
+		clear_count++;
+
+		if (label_uuid) {
+			va_del(label_uuid, NULL);
+		} else {
+			BT_ERR("Label UUID not found");
+		}
+	}
+
+	return clear_count;
+}
+
+static void mod_pub_va_set(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	u8_t retransmit, status, pub_ttl, pub_period, cred_flag;
+	u16_t elem_addr, pub_addr, pub_app_idx;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *label_uuid;
+	u8_t *mod_id;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	label_uuid = net_buf_simple_pull_mem(buf, 16);
+	pub_app_idx = net_buf_simple_pull_le16(buf);
+	cred_flag = ((pub_app_idx >> 12) & BIT_MASK(1));
+	pub_app_idx &= BIT_MASK(12);
+	pub_ttl = net_buf_simple_pull_u8(buf);
+	if (pub_ttl > BT_MESH_TTL_MAX && pub_ttl != BT_MESH_TTL_DEFAULT) {
+		BT_ERR("Invalid TTL value 0x%02x", pub_ttl);
+		return;
+	}
+
+	pub_period = net_buf_simple_pull_u8(buf);
+	retransmit = net_buf_simple_pull_u8(buf);
+	mod_id = buf->om_data;
+
+	BT_DBG("elem_addr 0x%04x cred_flag %u", elem_addr, cred_flag);
+	BT_DBG("pub_app_idx 0x%03x, pub_ttl %u pub_period 0x%02x",
+	       pub_app_idx, pub_ttl, pub_period);
+	BT_DBG("retransmit 0x%02x (count %u interval %ums)", retransmit,
+	       BT_MESH_PUB_TRANSMIT_COUNT(retransmit),
+	       BT_MESH_PUB_TRANSMIT_INT(retransmit));
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		pub_addr = 0;
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		pub_addr = 0;
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = va_add(label_uuid, &pub_addr);
+	if (status == STATUS_SUCCESS) {
+		status = _mod_pub_set(mod, pub_addr, pub_app_idx, cred_flag,
+				      pub_ttl, pub_period, retransmit, true);
+	}
+
+send_status:
+	send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
+			    status, mod_id);
+}
+#else
+static size_t mod_sub_list_clear(struct bt_mesh_model *mod)
+{
+	size_t clear_count;
+	int i;
+
+	/* Unref stored labels related to this model */
+	for (i = 0, clear_count = 0; i < ARRAY_SIZE(mod->groups); i++) {
+		if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) {
+			mod->groups[i] = BT_MESH_ADDR_UNASSIGNED;
+			clear_count++;
+		}
+	}
+
+	return clear_count;
+}
+
+static void mod_pub_va_set(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	u8_t *mod_id, status;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u16_t elem_addr, pub_addr = 0;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	net_buf_simple_pull(buf, 16);
+	mod_id = net_buf_simple_pull(buf, 4);
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	if (!mod->pub) {
+		status = STATUS_NVAL_PUB_PARAM;
+		goto send_status;
+	}
+
+	pub_addr = mod->pub->addr;
+	status = STATUS_INSUFF_RESOURCES;
+
+send_status:
+	send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod,
+			    status, mod_id);
+}
+#endif /* MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 */
+
+static void send_mod_sub_status(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx, u8_t status,
+				u16_t elem_addr, u16_t sub_addr, u8_t *mod_id,
+				bool vnd)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_SUB_STATUS, 9);
+
+	BT_DBG("status 0x%02x elem_addr 0x%04x sub_addr 0x%04x", status,
+	       elem_addr, sub_addr);
+
+	bt_mesh_model_msg_init(msg, OP_MOD_SUB_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, elem_addr);
+	net_buf_simple_add_le16(msg, sub_addr);
+
+	if (vnd) {
+		memcpy(net_buf_simple_add(msg, 4), mod_id, 4);
+	} else {
+		memcpy(net_buf_simple_add(msg, 2), mod_id, 2);
+	}
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Model Subscription Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void mod_sub_add(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	u16_t elem_addr, sub_addr;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id;
+	u8_t status;
+	u16_t *entry;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	sub_addr = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("elem_addr 0x%04x, sub_addr 0x%04x", elem_addr, sub_addr);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) {
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	if (bt_mesh_model_find_group(&mod, sub_addr)) {
+		/* Tried to add existing subscription */
+		BT_DBG("found existing subscription");
+		status = STATUS_SUCCESS;
+		goto send_status;
+	}
+
+	entry = bt_mesh_model_find_group(&mod, BT_MESH_ADDR_UNASSIGNED);
+	if (!entry) {
+		status = STATUS_INSUFF_RESOURCES;
+		goto send_status;
+	}
+
+	*entry = sub_addr;
+	status = STATUS_SUCCESS;
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_mod_sub(mod);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+		bt_mesh_lpn_group_add(sub_addr);
+	}
+
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
+			    mod_id, vnd);
+}
+
+static void mod_sub_del(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	u16_t elem_addr, sub_addr;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id;
+	u16_t *match;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	sub_addr = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("elem_addr 0x%04x sub_addr 0x%04x", elem_addr, sub_addr);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) {
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	/* An attempt to remove a non-existing address shall be treated
+	 * as a success.
+	 */
+	status = STATUS_SUCCESS;
+
+	if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
+		bt_mesh_lpn_group_del(&sub_addr, 1);
+	}
+
+	match = bt_mesh_model_find_group(&mod, sub_addr);
+	if (match) {
+		*match = BT_MESH_ADDR_UNASSIGNED;
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+			bt_mesh_store_mod_sub(mod);
+		}
+	}
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
+			    mod_id, vnd);
+}
+
+static enum bt_mesh_walk mod_sub_clear_visitor(struct bt_mesh_model *mod,
+					       u32_t depth, void *user_data)
+{
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+		bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups));
+	}
+
+	mod_sub_list_clear(mod);
+
+	return BT_MESH_WALK_CONTINUE;
+}
+
+static void mod_sub_overwrite(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	u16_t elem_addr, sub_addr;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	sub_addr = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("elem_addr 0x%04x sub_addr 0x%04x", elem_addr, sub_addr);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) {
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	if (ARRAY_SIZE(mod->groups) > 0) {
+		bt_mesh_model_tree_walk(bt_mesh_model_root(mod),
+					mod_sub_clear_visitor, NULL);
+
+		mod->groups[0] = sub_addr;
+		status = STATUS_SUCCESS;
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+			bt_mesh_store_mod_sub(mod);
+		}
+
+		if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+			bt_mesh_lpn_group_add(sub_addr);
+		}
+	} else {
+		status = STATUS_INSUFF_RESOURCES;
+	}
+
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
+			    mod_id, vnd);
+}
+
+static void mod_sub_del_all(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    struct os_mbuf *buf)
+{
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u16_t elem_addr;
+	u8_t *mod_id;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	bt_mesh_model_tree_walk(bt_mesh_model_root(mod), mod_sub_clear_visitor,
+				NULL);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_mod_sub(mod);
+	}
+
+	status = STATUS_SUCCESS;
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr,
+			    BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
+}
+
+struct mod_sub_list_ctx {
+	u16_t elem_idx;
+	struct os_mbuf *msg;
+};
+
+static enum bt_mesh_walk mod_sub_list_visitor(struct bt_mesh_model *mod,
+					      u32_t depth, void *ctx)
+{
+	struct mod_sub_list_ctx *visit = ctx;
+	int count = 0;
+	int i;
+
+	if (mod->elem_idx != visit->elem_idx) {
+		return BT_MESH_WALK_CONTINUE;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mod->groups); i++) {
+		if (mod->groups[i] == BT_MESH_ADDR_UNASSIGNED) {
+			continue;
+		}
+
+		if (net_buf_simple_tailroom(visit->msg) <
+		    2 + BT_MESH_MIC_SHORT) {
+			BT_WARN("No room for all groups");
+			return BT_MESH_WALK_STOP;
+		}
+
+		net_buf_simple_add_le16(visit->msg, mod->groups[i]);
+		count++;
+	}
+
+	BT_DBG("sublist: model %u:%x: %u groups", mod->elem_idx, mod->id,
+	       count);
+
+	return BT_MESH_WALK_CONTINUE;
+}
+
+static void mod_sub_get(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	struct mod_sub_list_ctx visit_ctx;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u16_t addr, id;
+
+	addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(addr)) {
+		BT_WARN("Prohibited element address");
+		goto done;
+	}
+
+	id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("addr 0x%04x id 0x%04x", addr, id);
+
+	bt_mesh_model_msg_init(msg, OP_MOD_SUB_LIST);
+
+	elem = bt_mesh_elem_find(addr);
+	if (!elem) {
+		net_buf_simple_add_u8(msg, STATUS_INVALID_ADDRESS);
+		net_buf_simple_add_le16(msg, addr);
+		net_buf_simple_add_le16(msg, id);
+		goto send_list;
+	}
+
+	mod = bt_mesh_model_find(elem, id);
+	if (!mod) {
+		net_buf_simple_add_u8(msg, STATUS_INVALID_MODEL);
+		net_buf_simple_add_le16(msg, addr);
+		net_buf_simple_add_le16(msg, id);
+		goto send_list;
+	}
+
+	net_buf_simple_add_u8(msg, STATUS_SUCCESS);
+
+	net_buf_simple_add_le16(msg, addr);
+	net_buf_simple_add_le16(msg, id);
+
+	visit_ctx.msg = msg;
+	visit_ctx.elem_idx = mod->elem_idx;
+	bt_mesh_model_tree_walk(bt_mesh_model_root(mod), mod_sub_list_visitor,
+				&visit_ctx);
+
+send_list:
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Model Subscription List");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+
+}
+
+static void mod_sub_get_vnd(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u16_t company, addr, id;
+
+	addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(addr)) {
+		BT_WARN("Prohibited element address");
+		goto done;
+	}
+
+	company = net_buf_simple_pull_le16(buf);
+	id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("addr 0x%04x company 0x%04x id 0x%04x", addr, company, id);
+
+	bt_mesh_model_msg_init(msg, OP_MOD_SUB_LIST_VND);
+
+	elem = bt_mesh_elem_find(addr);
+	if (!elem) {
+		net_buf_simple_add_u8(msg, STATUS_INVALID_ADDRESS);
+		net_buf_simple_add_le16(msg, addr);
+		net_buf_simple_add_le16(msg, company);
+		net_buf_simple_add_le16(msg, id);
+		goto send_list;
+	}
+
+	mod = bt_mesh_model_find_vnd(elem, company, id);
+	if (!mod) {
+		net_buf_simple_add_u8(msg, STATUS_INVALID_MODEL);
+		net_buf_simple_add_le16(msg, addr);
+		net_buf_simple_add_le16(msg, company);
+		net_buf_simple_add_le16(msg, id);
+		goto send_list;
+	}
+
+	net_buf_simple_add_u8(msg, STATUS_SUCCESS);
+
+	net_buf_simple_add_le16(msg, addr);
+	net_buf_simple_add_le16(msg, company);
+	net_buf_simple_add_le16(msg, id);
+
+	bt_mesh_model_tree_walk(bt_mesh_model_root(mod), mod_sub_list_visitor,
+				msg);
+
+send_list:
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Vendor Model Subscription List");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+
+}
+
+#if MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0
+static void mod_sub_va_add(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	u16_t elem_addr, sub_addr;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *label_uuid;
+	u8_t *mod_id;
+	u16_t *entry;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	label_uuid = net_buf_simple_pull_mem(buf, 16);
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	mod_id = buf->om_data;
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		sub_addr = BT_MESH_ADDR_UNASSIGNED;
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		sub_addr = BT_MESH_ADDR_UNASSIGNED;
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = va_add(label_uuid, &sub_addr);
+	if (status != STATUS_SUCCESS) {
+		goto send_status;
+	}
+
+	if (bt_mesh_model_find_group(&mod, sub_addr)) {
+		/* Tried to add existing subscription */
+		status = STATUS_SUCCESS;
+		goto send_status;
+	}
+
+
+	entry = bt_mesh_model_find_group(&mod, BT_MESH_ADDR_UNASSIGNED);
+	if (!entry) {
+		status = STATUS_INSUFF_RESOURCES;
+		goto send_status;
+		}
+
+	*entry = sub_addr;
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+		bt_mesh_lpn_group_add(sub_addr);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_mod_sub(mod);
+	}
+
+	status = STATUS_SUCCESS;
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
+			    mod_id, vnd);
+}
+
+static void mod_sub_va_del(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	u16_t elem_addr, sub_addr;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *label_uuid;
+	u8_t *mod_id;
+	u16_t *match;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	label_uuid = net_buf_simple_pull_mem(buf, 16);
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		sub_addr = BT_MESH_ADDR_UNASSIGNED;
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		sub_addr = BT_MESH_ADDR_UNASSIGNED;
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = va_del(label_uuid, &sub_addr);
+	if (sub_addr == BT_MESH_ADDR_UNASSIGNED) {
+		goto send_status;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+		bt_mesh_lpn_group_del(&sub_addr, 1);
+	}
+
+	match = bt_mesh_model_find_group(&mod, sub_addr);
+	if (match) {
+		*match = BT_MESH_ADDR_UNASSIGNED;
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+			bt_mesh_store_mod_sub(mod);
+		}
+
+		status = STATUS_SUCCESS;
+	} else {
+		status = STATUS_CANNOT_REMOVE;
+	}
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
+			    mod_id, vnd);
+}
+
+static void mod_sub_va_overwrite(struct bt_mesh_model *model,
+				 struct bt_mesh_msg_ctx *ctx,
+				 struct os_mbuf *buf)
+{
+	u16_t elem_addr, sub_addr = BT_MESH_ADDR_UNASSIGNED;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *label_uuid;
+	u8_t *mod_id;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	label_uuid = net_buf_simple_pull_mem(buf, 16);
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	if (ARRAY_SIZE(mod->groups) > 0) {
+		bt_mesh_model_tree_walk(bt_mesh_model_root(mod),
+					mod_sub_clear_visitor, NULL);
+
+		status = va_add(label_uuid, &sub_addr);
+		if (status == STATUS_SUCCESS) {
+			mod->groups[0] = sub_addr;
+
+			if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+				bt_mesh_store_mod_sub(mod);
+			}
+
+			if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+				bt_mesh_lpn_group_add(sub_addr);
+			}
+		}
+	} else {
+		status = STATUS_INSUFF_RESOURCES;
+	}
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr, sub_addr,
+			    mod_id, vnd);
+}
+#else
+static void mod_sub_va_add(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u16_t elem_addr;
+	u8_t *mod_id;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	net_buf_simple_pull(buf, 16);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = STATUS_INSUFF_RESOURCES;
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr,
+			    BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
+}
+
+static void mod_sub_va_del(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct bt_mesh_elem *elem;
+	u16_t elem_addr;
+	u8_t *mod_id;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	net_buf_simple_pull(buf, 16);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	if (!get_model(elem, buf, &vnd)) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = STATUS_INSUFF_RESOURCES;
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr,
+			    BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
+}
+
+static void mod_sub_va_overwrite(struct bt_mesh_model *model,
+				 struct bt_mesh_msg_ctx *ctx,
+				 struct os_mbuf *buf)
+{
+	struct bt_mesh_elem *elem;
+	u16_t elem_addr;
+	u8_t *mod_id;
+	u8_t status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		return;
+	}
+
+	net_buf_simple_pull(buf, 18);
+
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	if (!get_model(elem, buf, &vnd)) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = STATUS_INSUFF_RESOURCES;
+
+send_status:
+	send_mod_sub_status(model, ctx, status, elem_addr,
+			    BT_MESH_ADDR_UNASSIGNED, mod_id, vnd);
+}
+#endif /* MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 */
+
+static void send_net_key_status(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx,
+				u16_t idx, u8_t status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NET_KEY_STATUS, 3);
+
+	bt_mesh_model_msg_init(msg, OP_NET_KEY_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, idx);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send NetKey Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void net_key_add(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct bt_mesh_subnet *sub;
+	u16_t idx;
+	int err;
+
+	idx = net_buf_simple_pull_le16(buf);
+	if (idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
+		return;
+	}
+
+	BT_DBG("idx 0x%04x", idx);
+
+	sub = bt_mesh_subnet_get(idx);
+	if (!sub) {
+		int i;
+
+		for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+			if (bt_mesh.sub[i].net_idx == BT_MESH_KEY_UNUSED) {
+				sub = &bt_mesh.sub[i];
+				break;
+			}
+		}
+
+		if (!sub) {
+			send_net_key_status(model, ctx, idx,
+					    STATUS_INSUFF_RESOURCES);
+			return;
+		}
+	}
+
+	/* Check for already existing subnet */
+	if (sub->net_idx == idx) {
+		u8_t status;
+
+		if (memcmp(buf->om_data, sub->keys[0].net, 16)) {
+			status = STATUS_IDX_ALREADY_STORED;
+		} else {
+			status = STATUS_SUCCESS;
+		}
+
+		send_net_key_status(model, ctx, idx, status);
+		return;
+	}
+
+	err = bt_mesh_net_keys_create(&sub->keys[0], buf->om_data);
+	if (err) {
+		send_net_key_status(model, ctx, idx, STATUS_UNSPECIFIED);
+		return;
+	}
+
+	sub->net_idx = idx;
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		BT_DBG("Storing NetKey persistently");
+		bt_mesh_store_subnet(sub);
+	}
+
+	/* Make sure we have valid beacon data to be sent */
+	bt_mesh_net_beacon_update(sub);
+
+	if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
+		sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED;
+		bt_mesh_proxy_beacon_send(sub);
+		bt_mesh_adv_update();
+	} else {
+		sub->node_id = BT_MESH_NODE_IDENTITY_NOT_SUPPORTED;
+	}
+
+	send_net_key_status(model, ctx, idx, STATUS_SUCCESS);
+}
+
+static void net_key_update(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct bt_mesh_subnet *sub;
+	u16_t idx;
+	int err;
+
+	idx = net_buf_simple_pull_le16(buf);
+	if (idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
+		return;
+	}
+
+	BT_DBG("idx 0x%04x", idx);
+
+	sub = bt_mesh_subnet_get(idx);
+	if (!sub) {
+		send_net_key_status(model, ctx, idx, STATUS_INVALID_NETKEY);
+		return;
+	}
+
+	/* The node shall successfully process a NetKey Update message on a
+	 * valid NetKeyIndex when the NetKey value is different and the Key
+	 * Refresh procedure has not been started, or when the NetKey value is
+	 * the same in Phase 1. The NetKey Update message shall generate an
+	 * error when the node is in Phase 2, or Phase 3.
+	 */
+	switch (sub->kr_phase) {
+	case BT_MESH_KR_NORMAL:
+		if (!memcmp(buf->om_data, sub->keys[0].net, 16)) {
+			return;
+		}
+		break;
+	case BT_MESH_KR_PHASE_1:
+		if (!memcmp(buf->om_data, sub->keys[1].net, 16)) {
+			send_net_key_status(model, ctx, idx, STATUS_SUCCESS);
+			return;
+		}
+		/* fall through */
+	case BT_MESH_KR_PHASE_2:
+	case BT_MESH_KR_PHASE_3:
+		send_net_key_status(model, ctx, idx, STATUS_CANNOT_UPDATE);
+		return;
+	}
+
+	err = bt_mesh_net_keys_create(&sub->keys[1], buf->om_data);
+	if (!err && ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) ||
+		     (MYNEWT_VAL(BLE_MESH_FRIEND)))) {
+		err = friend_cred_update(sub);
+	}
+
+	if (err) {
+		send_net_key_status(model, ctx, idx, STATUS_UNSPECIFIED);
+		return;
+	}
+
+	sub->kr_phase = BT_MESH_KR_PHASE_1;
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		BT_DBG("Storing NetKey persistently");
+		bt_mesh_store_subnet(sub);
+	}
+
+	bt_mesh_net_beacon_update(sub);
+
+	send_net_key_status(model, ctx, idx, STATUS_SUCCESS);
+}
+
+static void hb_pub_disable(struct bt_mesh_cfg_srv *cfg)
+{
+	BT_DBG("");
+
+	cfg->hb_pub.dst = BT_MESH_ADDR_UNASSIGNED;
+	cfg->hb_pub.count = 0;
+	cfg->hb_pub.ttl = 0;
+	cfg->hb_pub.period = 0;
+
+	k_delayed_work_cancel(&cfg->hb_pub.timer);
+}
+
+static void net_key_del(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct bt_mesh_subnet *sub;
+	u16_t del_idx;
+	u8_t status;
+
+	del_idx = net_buf_simple_pull_le16(buf);
+	if (del_idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", del_idx);
+		return;
+	}
+
+	BT_DBG("idx 0x%04x", del_idx);
+
+	sub = bt_mesh_subnet_get(del_idx);
+	if (!sub) {
+		/* This could be a retry of a previous attempt that had its
+		 * response lost, so pretend that it was a success.
+		 */
+		status = STATUS_SUCCESS;
+		goto send_status;
+	}
+
+	/* The key that the message was encrypted with cannot be removed.
+	 * The NetKey List must contain a minimum of one NetKey.
+	 */
+	if (ctx->net_idx == del_idx) {
+		status = STATUS_CANNOT_REMOVE;
+		goto send_status;
+	}
+
+	bt_mesh_subnet_del(sub, true);
+	status = STATUS_SUCCESS;
+
+send_status:
+	send_net_key_status(model, ctx, del_idx, status);
+}
+
+static void net_key_get(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct os_mbuf *msg =
+		BT_MESH_MODEL_BUF(OP_NET_KEY_LIST,
+				  IDX_LEN(CONFIG_BT_MESH_SUBNET_COUNT));
+	u16_t prev, i;
+
+	bt_mesh_model_msg_init(msg, OP_NET_KEY_LIST);
+
+	prev = BT_MESH_KEY_UNUSED;
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (prev == BT_MESH_KEY_UNUSED) {
+			prev = sub->net_idx;
+			continue;
+		}
+
+		key_idx_pack(msg, prev, sub->net_idx);
+		prev = BT_MESH_KEY_UNUSED;
+	}
+
+	if (prev != BT_MESH_KEY_UNUSED) {
+		net_buf_simple_add_le16(msg, prev);
+	}
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send NetKey List");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void node_identity_get(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NODE_IDENTITY_STATUS, 4);
+	struct bt_mesh_subnet *sub;
+	u8_t node_id;
+	u16_t idx;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	idx = net_buf_simple_pull_le16(buf);
+	if (idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_NODE_IDENTITY_STATUS);
+
+	sub = bt_mesh_subnet_get(idx);
+	if (!sub) {
+		net_buf_simple_add_u8(msg, STATUS_INVALID_NETKEY);
+		node_id = 0x00;
+	} else {
+		net_buf_simple_add_u8(msg, STATUS_SUCCESS);
+		node_id = sub->node_id;
+	}
+
+	net_buf_simple_add_le16(msg, idx);
+	net_buf_simple_add_u8(msg, node_id);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Node Identity Status");
+	}
+
+done:
+    os_mbuf_free_chain(msg);
+}
+
+static void node_identity_set(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NODE_IDENTITY_STATUS, 4);
+	struct bt_mesh_subnet *sub;
+	u8_t node_id;
+	u16_t idx;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	idx = net_buf_simple_pull_le16(buf);
+	if (idx > 0xfff) {
+		BT_WARN("Invalid NetKeyIndex 0x%04x", idx);
+		goto done;
+	}
+
+	node_id = net_buf_simple_pull_u8(buf);
+	if (node_id != 0x00 && node_id != 0x01) {
+		BT_WARN("Invalid Node ID value 0x%02x", node_id);
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_NODE_IDENTITY_STATUS);
+
+	sub = bt_mesh_subnet_get(idx);
+	if (!sub) {
+		net_buf_simple_add_u8(msg, STATUS_INVALID_NETKEY);
+		net_buf_simple_add_le16(msg, idx);
+		net_buf_simple_add_u8(msg, node_id);
+	} else  {
+		net_buf_simple_add_u8(msg, STATUS_SUCCESS);
+		net_buf_simple_add_le16(msg, idx);
+
+		if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) {
+			if (node_id) {
+				bt_mesh_proxy_identity_start(sub);
+			} else {
+				bt_mesh_proxy_identity_stop(sub);
+			}
+			bt_mesh_adv_update();
+		}
+
+		net_buf_simple_add_u8(msg, sub->node_id);
+	}
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Node Identity Status");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+
+}
+
+static void create_mod_app_status(struct os_mbuf *msg,
+				  struct bt_mesh_model *mod, bool vnd,
+				  u16_t elem_addr, u16_t app_idx,
+				  u8_t status, u8_t *mod_id)
+{
+	bt_mesh_model_msg_init(msg, OP_MOD_APP_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, elem_addr);
+	net_buf_simple_add_le16(msg, app_idx);
+
+	if (vnd) {
+		memcpy(net_buf_simple_add(msg, 4), mod_id, 4);
+	} else {
+		memcpy(net_buf_simple_add(msg, 2), mod_id, 2);
+	}
+}
+
+static void mod_app_bind(struct bt_mesh_model *model,
+			 struct bt_mesh_msg_ctx *ctx,
+			 struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_APP_STATUS, 9);
+	u16_t elem_addr, key_app_idx;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id, status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		goto done;
+	}
+
+	key_app_idx = net_buf_simple_pull_le16(buf);
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	/* Configuration Server only allows device key based access */
+	if (model == mod) {
+		BT_ERR("Client tried to bind AppKey to Configuration Model");
+		status = STATUS_CANNOT_BIND;
+		goto send_status;
+	}
+
+	status = mod_bind(mod, key_app_idx);
+
+	if (IS_ENABLED(CONFIG_BT_TESTING) && status == STATUS_SUCCESS) {
+		bt_test_mesh_model_bound(ctx->addr, mod, key_app_idx);
+	}
+
+send_status:
+	BT_DBG("status 0x%02x", status);
+	create_mod_app_status(msg, mod, vnd, elem_addr, key_app_idx, status,
+			      mod_id);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Model App Bind Status response");
+	}
+
+done:
+    os_mbuf_free_chain(msg);
+
+}
+
+static void mod_app_unbind(struct bt_mesh_model *model,
+			   struct bt_mesh_msg_ctx *ctx,
+			   struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_MOD_APP_STATUS, 9);
+	u16_t elem_addr, key_app_idx;
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id, status;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		goto done;
+	}
+
+	key_app_idx = net_buf_simple_pull_le16(buf);
+	mod_id = buf->om_data;
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_status;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_status;
+	}
+
+	status = mod_unbind(mod, key_app_idx, true);
+
+	if (IS_ENABLED(CONFIG_BT_TESTING) && status == STATUS_SUCCESS) {
+		bt_test_mesh_model_unbound(ctx->addr, mod, key_app_idx);
+	}
+
+send_status:
+	BT_DBG("status 0x%02x", status);
+	create_mod_app_status(msg, mod, vnd, elem_addr, key_app_idx, status,
+			      mod_id);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Model App Unbind Status response");
+	}
+
+done:
+    os_mbuf_free_chain(msg);
+}
+
+#define KEY_LIST_LEN (MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT) * 2)
+
+static void mod_app_get(struct bt_mesh_model *model,
+			struct bt_mesh_msg_ctx *ctx,
+			struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(max(BT_MESH_MODEL_BUF_LEN(OP_VND_MOD_APP_LIST,
+								       9 + KEY_LIST_LEN),
+						 BT_MESH_MODEL_BUF_LEN(OP_SIG_MOD_APP_LIST,
+								       9 + KEY_LIST_LEN)));
+
+	struct bt_mesh_model *mod;
+	struct bt_mesh_elem *elem;
+	u8_t *mod_id, status;
+	u16_t elem_addr;
+	bool vnd;
+
+	elem_addr = net_buf_simple_pull_le16(buf);
+	if (!BT_MESH_ADDR_IS_UNICAST(elem_addr)) {
+		BT_WARN("Prohibited element address");
+		goto done;
+	}
+
+	mod_id = buf->om_data;
+
+	BT_DBG("elem_addr 0x%04x", elem_addr);
+
+	elem = bt_mesh_elem_find(elem_addr);
+	if (!elem) {
+		mod = NULL;
+		vnd = (buf->om_len == 4);
+		status = STATUS_INVALID_ADDRESS;
+		goto send_list;
+	}
+
+	mod = get_model(elem, buf, &vnd);
+	if (!mod) {
+		status = STATUS_INVALID_MODEL;
+		goto send_list;
+	}
+
+	status = STATUS_SUCCESS;
+
+send_list:
+	if (vnd) {
+		bt_mesh_model_msg_init(msg, OP_VND_MOD_APP_LIST);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_SIG_MOD_APP_LIST);
+	}
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, elem_addr);
+
+	if (vnd) {
+		net_buf_simple_add_mem(msg, mod_id, 4);
+	} else {
+		net_buf_simple_add_mem(msg, mod_id, 2);
+	}
+
+	if (mod) {
+		int i;
+
+		for (i = 0; i < ARRAY_SIZE(mod->keys); i++) {
+			if (mod->keys[i] != BT_MESH_KEY_UNUSED) {
+				net_buf_simple_add_le16(msg, mod->keys[i]);
+			}
+		}
+	}
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Model Application List message");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+}
+
+static void node_reset(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_NODE_RESET_STATUS, 0);
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+
+	bt_mesh_model_msg_init(msg, OP_NODE_RESET_STATUS);
+
+	/* Send the response first since we wont have any keys left to
+	 * send it later.
+	 */
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Node Reset Status");
+	}
+
+	bt_mesh_reset();
+    os_mbuf_free_chain(msg);
+}
+
+static void send_friend_status(struct bt_mesh_model *model,
+			       struct bt_mesh_msg_ctx *ctx)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_FRIEND_STATUS, 1);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	bt_mesh_model_msg_init(msg, OP_FRIEND_STATUS);
+	net_buf_simple_add_u8(msg, cfg->frnd);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Friend Status");
+	}
+    os_mbuf_free_chain(msg);
+}
+
+static void friend_get(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *buf)
+{
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	send_friend_status(model, ctx);
+}
+
+static void friend_set(struct bt_mesh_model *model,
+		       struct bt_mesh_msg_ctx *ctx,
+		       struct os_mbuf *buf)
+{
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (buf->om_data[0] != 0x00 && buf->om_data[0] != 0x01) {
+		BT_WARN("Invalid Friend value 0x%02x", buf->om_data[0]);
+		return;
+	}
+
+	if (!cfg) {
+		BT_WARN("No Configuration Server context available");
+		goto send_status;
+	}
+
+	BT_DBG("Friend 0x%02x -> 0x%02x", cfg->frnd, buf->om_data[0]);
+
+	if (cfg->frnd == buf->om_data[0]) {
+		goto send_status;
+	}
+
+	if (MYNEWT_VAL(BLE_MESH_FRIEND)) {
+		cfg->frnd = buf->om_data[0];
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+			bt_mesh_store_cfg();
+		}
+
+		if (cfg->frnd == BT_MESH_FRIEND_DISABLED) {
+			bt_mesh_friend_clear_net_idx(BT_MESH_KEY_ANY);
+		}
+	}
+
+	if (cfg->hb_pub.feat & BT_MESH_FEAT_FRIEND) {
+		bt_mesh_heartbeat_send();
+	}
+
+send_status:
+	send_friend_status(model, ctx);
+}
+
+static void lpn_timeout_get(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    struct os_mbuf *buf)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_LPN_TIMEOUT_STATUS, 5);
+	struct bt_mesh_friend *frnd;
+	u16_t lpn_addr;
+	s32_t timeout;
+
+	lpn_addr = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x lpn_addr 0x%02x",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, lpn_addr);
+
+	/* check if it's the address of the Low Power Node? */
+	if (!BT_MESH_ADDR_IS_UNICAST(lpn_addr)) {
+		BT_WARN("Invalid LPNAddress; ignoring msg");
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_LPN_TIMEOUT_STATUS);
+	net_buf_simple_add_le16(msg, lpn_addr);
+
+	if (!IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		timeout = 0;
+		goto send_rsp;
+	}
+
+	frnd = bt_mesh_friend_find(BT_MESH_KEY_ANY, lpn_addr, true, true);
+	if (!frnd) {
+		timeout = 0;
+		goto send_rsp;
+	}
+
+	timeout = k_delayed_work_remaining_get(&frnd->timer) / 100;
+
+send_rsp:
+	net_buf_simple_add_u8(msg, timeout);
+	net_buf_simple_add_u8(msg, timeout >> 8);
+	net_buf_simple_add_u8(msg, timeout >> 16);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send LPN PollTimeout Status");
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+}
+
+static void send_krp_status(struct bt_mesh_model *model,
+			    struct bt_mesh_msg_ctx *ctx,
+			    u16_t idx, u8_t phase, u8_t status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_KRP_STATUS, 4);
+
+	bt_mesh_model_msg_init(msg, OP_KRP_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+	net_buf_simple_add_le16(msg, idx);
+	net_buf_simple_add_u8(msg, phase);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Key Refresh State Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void krp_get(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
+		    struct os_mbuf *buf)
+{
+	struct bt_mesh_subnet *sub;
+	u16_t idx;
+
+	idx = net_buf_simple_pull_le16(buf);
+	if (idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
+		return;
+	}
+
+	BT_DBG("idx 0x%04x", idx);
+
+	sub = bt_mesh_subnet_get(idx);
+	if (!sub) {
+		send_krp_status(model, ctx, idx, 0x00, STATUS_INVALID_NETKEY);
+	} else {
+		send_krp_status(model, ctx, idx, sub->kr_phase,
+				STATUS_SUCCESS);
+	}
+}
+
+static void krp_set(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
+		    struct os_mbuf *buf)
+{
+	struct bt_mesh_subnet *sub;
+	u8_t phase;
+	u16_t idx;
+
+	idx = net_buf_simple_pull_le16(buf);
+	phase = net_buf_simple_pull_u8(buf);
+
+	if (idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
+		return;
+	}
+
+	BT_DBG("idx 0x%04x transition 0x%02x", idx, phase);
+
+	sub = bt_mesh_subnet_get(idx);
+	if (!sub) {
+		send_krp_status(model, ctx, idx, 0x00, STATUS_INVALID_NETKEY);
+		return;
+	}
+
+	BT_DBG("%u -> %u", sub->kr_phase, phase);
+
+	if (phase < BT_MESH_KR_PHASE_2 || phase > BT_MESH_KR_PHASE_3 ||
+	    (sub->kr_phase == BT_MESH_KR_NORMAL &&
+	     phase == BT_MESH_KR_PHASE_2)) {
+		BT_WARN("Prohibited transition %u -> %u", sub->kr_phase, phase);
+		return;
+	}
+
+	if (sub->kr_phase == BT_MESH_KR_PHASE_1 &&
+	    phase == BT_MESH_KR_PHASE_2) {
+		sub->kr_phase = BT_MESH_KR_PHASE_2;
+		sub->kr_flag = 1;
+		bt_mesh_net_beacon_update(sub);
+	} else if ((sub->kr_phase == BT_MESH_KR_PHASE_1 ||
+		    sub->kr_phase == BT_MESH_KR_PHASE_2) &&
+		   phase == BT_MESH_KR_PHASE_3) {
+		bt_mesh_net_revoke_keys(sub);
+		if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) ||
+		    (MYNEWT_VAL(BLE_MESH_FRIEND))) {
+			friend_cred_refresh(ctx->net_idx);
+		}
+		sub->kr_phase = BT_MESH_KR_NORMAL;
+		sub->kr_flag = 0;
+		bt_mesh_net_beacon_update(sub);
+	}
+
+	send_krp_status(model, ctx, idx, sub->kr_phase, STATUS_SUCCESS);
+}
+
+static u8_t hb_log(u16_t val)
+{
+	if (!val) {
+		return 0x00;
+	} else if (val == 0xffff) {
+		return 0xff;
+	} else {
+		return 32 - __builtin_clz(val);
+	}
+}
+
+static u8_t hb_pub_count_log(u16_t val)
+{
+	if (!val) {
+		return 0x00;
+	} else if (val == 0x01) {
+		return 0x01;
+	} else if (val == 0xffff) {
+		return 0xff;
+	} else {
+		return 32 - __builtin_clz(val - 1) + 1;
+	}
+}
+
+static u16_t hb_pwr2(u8_t val, u8_t sub)
+{
+	if (!val) {
+		return 0x0000;
+	} else if (val == 0xff || val == 0x11) {
+		return 0xffff;
+	} else {
+		return (1 << (val - sub));
+	}
+}
+
+struct hb_pub_param {
+	u16_t dst;
+	u8_t  count_log;
+	u8_t  period_log;
+	u8_t  ttl;
+	u16_t feat;
+	u16_t net_idx;
+} __packed;
+
+static void hb_pub_send_status(struct bt_mesh_model *model,
+			       struct bt_mesh_msg_ctx *ctx, u8_t status,
+			       struct hb_pub_param *orig_msg)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEARTBEAT_PUB_STATUS, 10);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("src 0x%04x status 0x%02x", ctx->addr, status);
+
+	bt_mesh_model_msg_init(msg, OP_HEARTBEAT_PUB_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+
+	if (orig_msg) {
+		memcpy(net_buf_simple_add(msg, sizeof(*orig_msg)), orig_msg,
+		       sizeof(*orig_msg));
+		goto send;
+	}
+
+	net_buf_simple_add_le16(msg, cfg->hb_pub.dst);
+	net_buf_simple_add_u8(msg, hb_pub_count_log(cfg->hb_pub.count));
+	net_buf_simple_add_u8(msg, cfg->hb_pub.period);
+	net_buf_simple_add_u8(msg, cfg->hb_pub.ttl);
+	net_buf_simple_add_le16(msg, cfg->hb_pub.feat);
+	net_buf_simple_add_le16(msg, cfg->hb_pub.net_idx);
+
+send:
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Heartbeat Publication Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void heartbeat_pub_get(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	BT_DBG("src 0x%04x", ctx->addr);
+
+	hb_pub_send_status(model, ctx, STATUS_SUCCESS, NULL);
+}
+
+static void heartbeat_pub_set(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	struct hb_pub_param *param = (void *)buf->om_data;
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+	u16_t dst, feat, idx;
+	u8_t status;
+
+	BT_DBG("src 0x%04x", ctx->addr);
+
+	dst = sys_le16_to_cpu(param->dst);
+	/* All other address types but virtual are valid */
+	if (BT_MESH_ADDR_IS_VIRTUAL(dst)) {
+		status = STATUS_INVALID_ADDRESS;
+		goto failed;
+	}
+
+	if (param->count_log > 0x11 && param->count_log != 0xff) {
+		status = STATUS_CANNOT_SET;
+		goto failed;
+	}
+
+	if (param->period_log > 0x10) {
+		status = STATUS_CANNOT_SET;
+		goto failed;
+	}
+
+	if (param->ttl > BT_MESH_TTL_MAX && param->ttl != BT_MESH_TTL_DEFAULT) {
+		BT_ERR("Invalid TTL value 0x%02x", param->ttl);
+		return;
+	}
+
+	feat = sys_le16_to_cpu(param->feat);
+
+	idx = sys_le16_to_cpu(param->net_idx);
+	if (idx > 0xfff) {
+		BT_ERR("Invalid NetKeyIndex 0x%04x", idx);
+		return;
+	}
+
+	if (!bt_mesh_subnet_get(idx)) {
+		status = STATUS_INVALID_NETKEY;
+		goto failed;
+	}
+
+	cfg->hb_pub.dst = dst;
+	cfg->hb_pub.period = param->period_log;
+	cfg->hb_pub.feat = feat & BT_MESH_FEAT_SUPPORTED;
+	cfg->hb_pub.net_idx = idx;
+
+	if (dst == BT_MESH_ADDR_UNASSIGNED) {
+		hb_pub_disable(cfg);
+	} else {
+		/* 2^(n-1) */
+		cfg->hb_pub.count = hb_pwr2(param->count_log, 1);
+		cfg->hb_pub.ttl = param->ttl;
+
+		BT_DBG("period %u ms", hb_pwr2(param->period_log, 1) * 1000);
+
+		/* The first Heartbeat message shall be published as soon
+		 * as possible after the Heartbeat Publication Period state
+		 * has been configured for periodic publishing.
+		 */
+		if (param->period_log && param->count_log) {
+			k_work_submit(&cfg->hb_pub.timer.work);
+		} else {
+			k_delayed_work_cancel(&cfg->hb_pub.timer);
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_hb_pub();
+	}
+
+	hb_pub_send_status(model, ctx, STATUS_SUCCESS, NULL);
+
+	return;
+
+failed:
+	hb_pub_send_status(model, ctx, status, param);
+}
+
+static void hb_sub_send_status(struct bt_mesh_model *model,
+			       struct bt_mesh_msg_ctx *ctx, u8_t status)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEARTBEAT_SUB_STATUS, 9);
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+	u16_t period;
+	s64_t uptime;
+
+	BT_DBG("src 0x%04x status 0x%02x", ctx->addr, status);
+
+	uptime = k_uptime_get();
+	if (uptime > cfg->hb_sub.expiry) {
+		period = 0;
+	} else {
+		period = (cfg->hb_sub.expiry - uptime) / 1000;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEARTBEAT_SUB_STATUS);
+
+	net_buf_simple_add_u8(msg, status);
+
+	net_buf_simple_add_le16(msg, cfg->hb_sub.src);
+	net_buf_simple_add_le16(msg, cfg->hb_sub.dst);
+
+	net_buf_simple_add_u8(msg, hb_log(period));
+	net_buf_simple_add_u8(msg, hb_log(cfg->hb_sub.count));
+	net_buf_simple_add_u8(msg, cfg->hb_sub.min_hops);
+	net_buf_simple_add_u8(msg, cfg->hb_sub.max_hops);
+
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Heartbeat Subscription Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void heartbeat_sub_get(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	BT_DBG("src 0x%04x", ctx->addr);
+
+	hb_sub_send_status(model, ctx, STATUS_SUCCESS);
+}
+
+static void heartbeat_sub_set(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+	u16_t sub_src, sub_dst;
+	u8_t sub_period;
+	s32_t period_ms;
+
+	BT_DBG("src 0x%04x", ctx->addr);
+
+	sub_src = net_buf_simple_pull_le16(buf);
+	sub_dst = net_buf_simple_pull_le16(buf);
+	sub_period = net_buf_simple_pull_u8(buf);
+
+	BT_DBG("sub_src 0x%04x sub_dst 0x%04x period 0x%02x",
+	       sub_src, sub_dst, sub_period);
+
+	if (sub_src != BT_MESH_ADDR_UNASSIGNED &&
+	    !BT_MESH_ADDR_IS_UNICAST(sub_src)) {
+		BT_WARN("Prohibited source address");
+		return;
+	}
+
+	if (BT_MESH_ADDR_IS_VIRTUAL(sub_dst) || BT_MESH_ADDR_IS_RFU(sub_dst) ||
+	    (BT_MESH_ADDR_IS_UNICAST(sub_dst) &&
+	     sub_dst != bt_mesh_primary_addr())) {
+		BT_WARN("Prohibited destination address");
+		return;
+	}
+
+	if (sub_period > 0x11) {
+		BT_WARN("Prohibited subscription period 0x%02x", sub_period);
+		return;
+	}
+
+	if (sub_src == BT_MESH_ADDR_UNASSIGNED ||
+	    sub_dst == BT_MESH_ADDR_UNASSIGNED ||
+	    sub_period == 0x00) {
+		/* Only an explicit address change to unassigned should
+		 * trigger clearing of the values according to
+		 * MESH/NODE/CFG/HBS/BV-02-C.
+		 */
+		if (sub_src == BT_MESH_ADDR_UNASSIGNED ||
+		    sub_dst == BT_MESH_ADDR_UNASSIGNED) {
+			cfg->hb_sub.src = BT_MESH_ADDR_UNASSIGNED;
+			cfg->hb_sub.dst = BT_MESH_ADDR_UNASSIGNED;
+			cfg->hb_sub.min_hops = BT_MESH_TTL_MAX;
+			cfg->hb_sub.max_hops = 0;
+			cfg->hb_sub.count = 0;
+		}
+
+		period_ms = 0;
+	} else {
+		cfg->hb_sub.src = sub_src;
+		cfg->hb_sub.dst = sub_dst;
+		cfg->hb_sub.min_hops = BT_MESH_TTL_MAX;
+		cfg->hb_sub.max_hops = 0;
+		cfg->hb_sub.count = 0;
+		period_ms = hb_pwr2(sub_period, 1) * 1000;
+	}
+
+	/* Let the transport layer know it needs to handle this address */
+	bt_mesh_set_hb_sub_dst(cfg->hb_sub.dst);
+
+	BT_DBG("period_ms %u", (unsigned) period_ms);
+
+	if (period_ms) {
+		cfg->hb_sub.expiry = k_uptime_get() + period_ms;
+	} else {
+		cfg->hb_sub.expiry = 0;
+	}
+
+	hb_sub_send_status(model, ctx, STATUS_SUCCESS);
+
+	/* MESH/NODE/CFG/HBS/BV-01-C expects the MinHops to be 0x7f after
+	 * disabling subscription, but 0x00 for subsequent Get requests.
+	 */
+	if (!period_ms) {
+		cfg->hb_sub.min_hops = 0;
+	}
+}
+
+const struct bt_mesh_model_op bt_mesh_cfg_srv_op[] = {
+	{ OP_DEV_COMP_DATA_GET,        1,   dev_comp_data_get },
+	{ OP_APP_KEY_ADD,              19,  app_key_add },
+	{ OP_APP_KEY_UPDATE,           19,  app_key_update },
+	{ OP_APP_KEY_DEL,              3,   app_key_del },
+	{ OP_APP_KEY_GET,              2,   app_key_get },
+	{ OP_BEACON_GET,               0,   beacon_get },
+	{ OP_BEACON_SET,               1,   beacon_set },
+	{ OP_DEFAULT_TTL_GET,          0,   default_ttl_get },
+	{ OP_DEFAULT_TTL_SET,          1,   default_ttl_set },
+	{ OP_GATT_PROXY_GET,           0,   gatt_proxy_get },
+	{ OP_GATT_PROXY_SET,           1,   gatt_proxy_set },
+	{ OP_NET_TRANSMIT_GET,         0,   net_transmit_get },
+	{ OP_NET_TRANSMIT_SET,         1,   net_transmit_set },
+	{ OP_RELAY_GET,                0,   relay_get },
+	{ OP_RELAY_SET,                2,   relay_set },
+	{ OP_MOD_PUB_GET,              4,   mod_pub_get },
+	{ OP_MOD_PUB_SET,              11,  mod_pub_set },
+	{ OP_MOD_PUB_VA_SET,           24,  mod_pub_va_set },
+	{ OP_MOD_SUB_ADD,              6,   mod_sub_add },
+	{ OP_MOD_SUB_VA_ADD,           20,  mod_sub_va_add },
+	{ OP_MOD_SUB_DEL,              6,   mod_sub_del },
+	{ OP_MOD_SUB_VA_DEL,           20,  mod_sub_va_del },
+	{ OP_MOD_SUB_OVERWRITE,        6,   mod_sub_overwrite },
+	{ OP_MOD_SUB_VA_OVERWRITE,     20,  mod_sub_va_overwrite },
+	{ OP_MOD_SUB_DEL_ALL,          4,   mod_sub_del_all },
+	{ OP_MOD_SUB_GET,              4,   mod_sub_get },
+	{ OP_MOD_SUB_GET_VND,          6,   mod_sub_get_vnd },
+	{ OP_NET_KEY_ADD,              18,  net_key_add },
+	{ OP_NET_KEY_UPDATE,           18,  net_key_update },
+	{ OP_NET_KEY_DEL,              2,   net_key_del },
+	{ OP_NET_KEY_GET,              0,   net_key_get },
+	{ OP_NODE_IDENTITY_GET,        2,   node_identity_get },
+	{ OP_NODE_IDENTITY_SET,        3,   node_identity_set },
+	{ OP_MOD_APP_BIND,             6,   mod_app_bind },
+	{ OP_MOD_APP_UNBIND,           6,   mod_app_unbind },
+	{ OP_SIG_MOD_APP_GET,          4,   mod_app_get },
+	{ OP_VND_MOD_APP_GET,          6,   mod_app_get },
+	{ OP_NODE_RESET,               0,   node_reset },
+	{ OP_FRIEND_GET,               0,   friend_get },
+	{ OP_FRIEND_SET,               1,   friend_set },
+	{ OP_LPN_TIMEOUT_GET,          2,   lpn_timeout_get },
+	{ OP_KRP_GET,                  2,   krp_get },
+	{ OP_KRP_SET,                  3,   krp_set },
+	{ OP_HEARTBEAT_PUB_GET,        0,   heartbeat_pub_get },
+	{ OP_HEARTBEAT_PUB_SET,        9,   heartbeat_pub_set },
+	{ OP_HEARTBEAT_SUB_GET,        0,   heartbeat_sub_get },
+	{ OP_HEARTBEAT_SUB_SET,        5,   heartbeat_sub_set },
+	BT_MESH_MODEL_OP_END,
+};
+
+static void hb_publish(struct ble_npl_event *work)
+{
+	struct bt_mesh_cfg_srv *cfg = ble_npl_event_get_arg(work);
+	struct bt_mesh_subnet *sub;
+	u16_t period_ms;
+
+	BT_DBG("hb_pub.count: %u", cfg->hb_pub.count);
+
+	sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx);
+	if (!sub) {
+		BT_ERR("No matching subnet for idx 0x%02x",
+		       cfg->hb_pub.net_idx);
+		cfg->hb_pub.dst = BT_MESH_ADDR_UNASSIGNED;
+		return;
+	}
+
+	if (cfg->hb_pub.count == 0) {
+		return;
+	}
+
+	period_ms = hb_pwr2(cfg->hb_pub.period, 1) * 1000;
+	if (period_ms && cfg->hb_pub.count > 1) {
+		k_delayed_work_submit(&cfg->hb_pub.timer, period_ms);
+	}
+
+	bt_mesh_heartbeat_send();
+
+	if (cfg->hb_pub.count != 0xffff) {
+		cfg->hb_pub.count--;
+	}
+}
+
+static bool conf_is_valid(struct bt_mesh_cfg_srv *cfg)
+{
+	if (cfg->relay > 0x02) {
+		return false;
+	}
+
+	if (cfg->beacon > 0x01) {
+		return false;
+	}
+
+	if (cfg->default_ttl > BT_MESH_TTL_MAX) {
+		return false;
+	}
+
+	return true;
+}
+
+static int cfg_srv_init(struct bt_mesh_model *model)
+{
+	struct bt_mesh_cfg_srv *cfg = model->user_data;
+
+	BT_DBG("");
+
+	if (!bt_mesh_model_in_primary(model)) {
+		BT_ERR("Configuration Server only allowed in primary element");
+		return -EINVAL;
+	}
+
+	if (!cfg) {
+		BT_ERR("No Configuration Server context provided");
+		return -EINVAL;
+	}
+
+	if (!conf_is_valid(cfg)) {
+		BT_ERR("Invalid values in configuration");
+		return -EINVAL;
+	}
+
+	/*
+	 * Configuration Model security is device-key based and only the local
+	 * device-key is allowed to access this model.
+	 */
+	model->keys[0] = BT_MESH_KEY_DEV_LOCAL;
+
+	if (!(MYNEWT_VAL(BLE_MESH_RELAY))) {
+		cfg->relay = BT_MESH_RELAY_NOT_SUPPORTED;
+	}
+
+	if (!(MYNEWT_VAL(BLE_MESH_FRIEND))) {
+		cfg->frnd = BT_MESH_FRIEND_NOT_SUPPORTED;
+	}
+
+	if (!(MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
+		cfg->gatt_proxy = BT_MESH_GATT_PROXY_NOT_SUPPORTED;
+	}
+
+	k_delayed_work_init(&cfg->hb_pub.timer, hb_publish);
+	k_delayed_work_add_arg(&cfg->hb_pub.timer, cfg);
+	cfg->hb_pub.net_idx = BT_MESH_KEY_UNUSED;
+	cfg->hb_sub.expiry = 0;
+
+	cfg->model = model;
+
+	conf = cfg;
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb bt_mesh_cfg_srv_cb = {
+	.init = cfg_srv_init,
+};
+
+static void mod_reset(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
+		      bool vnd, bool primary, void *user_data)
+{
+	size_t clear_count;
+
+	/* Clear model state that isn't otherwise cleared. E.g. AppKey
+	 * binding and model publication is cleared as a consequence
+	 * of removing all app keys, however model subscription and user data
+	 * clearing must be taken care of here.
+	 */
+
+	clear_count = mod_sub_list_clear(mod);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		if (clear_count) {
+			bt_mesh_store_mod_sub(mod);
+		}
+
+		bt_mesh_model_data_store(mod, vnd, NULL, 0);
+	}
+
+	if (mod->cb && mod->cb->reset) {
+		mod->cb->reset(mod);
+	}
+}
+
+void bt_mesh_cfg_reset(void)
+{
+	struct bt_mesh_cfg_srv *cfg = conf;
+	int i;
+
+	BT_DBG("");
+
+	if (!cfg) {
+		return;
+	}
+
+	bt_mesh_set_hb_sub_dst(BT_MESH_ADDR_UNASSIGNED);
+
+	cfg->hb_sub.src = BT_MESH_ADDR_UNASSIGNED;
+	cfg->hb_sub.dst = BT_MESH_ADDR_UNASSIGNED;
+	cfg->hb_sub.expiry = 0;
+
+	/* Delete all net keys, which also takes care of all app keys which
+	 * are associated with each net key.
+	 */
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx != BT_MESH_KEY_UNUSED) {
+			bt_mesh_subnet_del(sub, true);
+		}
+	}
+
+	bt_mesh_model_foreach(mod_reset, NULL);
+
+	memset(labels, 0, sizeof(labels));
+}
+
+void bt_mesh_heartbeat(u16_t src, u16_t dst, u8_t hops, u16_t feat)
+{
+	struct bt_mesh_cfg_srv *cfg = conf;
+
+	if (!cfg) {
+		BT_WARN("No configuaration server context available");
+		return;
+	}
+
+	if (src != cfg->hb_sub.src || dst != cfg->hb_sub.dst) {
+		BT_WARN("No subscription for received heartbeat");
+		return;
+	}
+
+	if (k_uptime_get() > cfg->hb_sub.expiry) {
+		BT_WARN("Heartbeat subscription period expired");
+		return;
+	}
+
+	cfg->hb_sub.min_hops = min(cfg->hb_sub.min_hops, hops);
+	cfg->hb_sub.max_hops = max(cfg->hb_sub.max_hops, hops);
+
+	if (cfg->hb_sub.count < 0xffff) {
+		cfg->hb_sub.count++;
+	}
+
+	BT_DBG("src 0x%04x dst 0x%04x hops %u min %u max %u count %u", src,
+	       dst, hops, cfg->hb_sub.min_hops, cfg->hb_sub.max_hops,
+	       cfg->hb_sub.count);
+
+	if (cfg->hb_sub.func) {
+		cfg->hb_sub.func(hops, feat);
+	}
+}
+
+u8_t bt_mesh_net_transmit_get(void)
+{
+	if (conf) {
+		return conf->net_transmit;
+	}
+
+	return 0;
+}
+
+u8_t bt_mesh_relay_get(void)
+{
+	if (conf) {
+		return conf->relay;
+	}
+
+	return BT_MESH_RELAY_NOT_SUPPORTED;
+}
+
+u8_t bt_mesh_friend_get(void)
+{
+	BT_DBG("conf %p conf->frnd 0x%02x", conf, conf->frnd);
+
+	if (conf) {
+		return conf->frnd;
+	}
+
+	return BT_MESH_FRIEND_NOT_SUPPORTED;
+}
+
+u8_t bt_mesh_relay_retransmit_get(void)
+{
+	if (conf) {
+		return conf->relay_retransmit;
+	}
+
+	return 0;
+}
+
+u8_t bt_mesh_beacon_get(void)
+{
+	if (conf) {
+		return conf->beacon;
+	}
+
+	return BT_MESH_BEACON_DISABLED;
+}
+
+u8_t bt_mesh_gatt_proxy_get(void)
+{
+	if (conf) {
+		return conf->gatt_proxy;
+	}
+
+	return BT_MESH_GATT_PROXY_NOT_SUPPORTED;
+}
+
+u8_t bt_mesh_default_ttl_get(void)
+{
+	if (conf) {
+		return conf->default_ttl;
+	}
+
+	return DEFAULT_TTL;
+}
+
+u8_t *bt_mesh_label_uuid_get(u16_t addr)
+{
+	int i;
+
+	BT_DBG("addr 0x%04x", addr);
+
+	for (i = 0; i < ARRAY_SIZE(labels); i++) {
+		if (labels[i].addr == addr) {
+			BT_DBG("Found Label UUID for 0x%04x: %s", addr,
+			       bt_hex(labels[i].uuid, 16));
+			return labels[i].uuid;
+		}
+	}
+
+	BT_WARN("No matching Label UUID for 0x%04x", addr);
+
+	return NULL;
+}
+
+struct bt_mesh_hb_pub *bt_mesh_hb_pub_get(void)
+{
+	if (!conf) {
+		return NULL;
+	}
+
+	return &conf->hb_pub;
+}
+
+void bt_mesh_hb_pub_disable(void)
+{
+	if (conf) {
+		hb_pub_disable(conf);
+	}
+}
+
+struct bt_mesh_cfg_srv *bt_mesh_cfg_get(void)
+{
+	return conf;
+}
+
+void bt_mesh_subnet_del(struct bt_mesh_subnet *sub, bool store)
+{
+	int i;
+
+	BT_DBG("NetIdx 0x%03x store %u", sub->net_idx, store);
+
+	if (conf && conf->hb_pub.net_idx == sub->net_idx) {
+		hb_pub_disable(conf);
+
+		if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+			bt_mesh_store_hb_pub();
+		}
+	}
+
+	/* Delete any app keys bound to this NetKey index */
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+
+		if (key->net_idx == sub->net_idx) {
+			bt_mesh_app_key_del(key, store);
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		bt_mesh_friend_clear_net_idx(sub->net_idx);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+		bt_mesh_clear_subnet(sub);
+	}
+
+	memset(sub, 0, sizeof(*sub));
+	sub->net_idx = BT_MESH_KEY_UNUSED;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/crypto.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/crypto.c
new file mode 100644
index 00000000..b6a0ba21
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/crypto.c
@@ -0,0 +1,911 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_CRYPTO_LOG
+
+#include <string.h>
+#include <stdbool.h>
+#include <errno.h>
+
+#include <tinycrypt/constants.h>
+#include <tinycrypt/utils.h>
+#include <tinycrypt/aes.h>
+#include <tinycrypt/cmac_mode.h>
+#include <tinycrypt/ccm_mode.h>
+
+#include "crypto.h"
+
+#define NET_MIC_LEN(pdu) (((pdu)[1] & 0x80) ? 8 : 4)
+#define APP_MIC_LEN(aszmic) ((aszmic) ? 8 : 4)
+
+int bt_mesh_aes_cmac(const u8_t key[16], struct bt_mesh_sg *sg,
+		     size_t sg_len, u8_t mac[16])
+{
+	struct tc_aes_key_sched_struct sched;
+	struct tc_cmac_struct state;
+
+	if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) {
+		return -EIO;
+	}
+
+	for (; sg_len; sg_len--, sg++) {
+		if (tc_cmac_update(&state, sg->data,
+				   sg->len) == TC_CRYPTO_FAIL) {
+			return -EIO;
+		}
+	}
+
+	if (tc_cmac_final(mac, &state) == TC_CRYPTO_FAIL) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int bt_mesh_k1(const u8_t *ikm, size_t ikm_len, const u8_t salt[16],
+	       const char *info, u8_t okm[16])
+{
+	int err;
+
+	err = bt_mesh_aes_cmac_one(salt, ikm, ikm_len, okm);
+	if (err < 0) {
+		return err;
+	}
+
+	return bt_mesh_aes_cmac_one(okm, info, strlen(info), okm);
+}
+
+int bt_mesh_k2(const u8_t n[16], const u8_t *p, size_t p_len,
+	       u8_t net_id[1], u8_t enc_key[16], u8_t priv_key[16])
+{
+	struct bt_mesh_sg sg[3];
+	u8_t salt[16];
+	u8_t out[16];
+	u8_t t[16];
+	u8_t pad;
+	int err;
+
+	BT_DBG("n %s", bt_hex(n, 16));
+	BT_DBG("p %s", bt_hex(p, p_len));
+
+	err = bt_mesh_s1("smk2", salt);
+	if (err) {
+		return err;
+	}
+
+	err = bt_mesh_aes_cmac_one(salt, n, 16, t);
+	if (err) {
+		return err;
+	}
+
+	pad = 0x01;
+
+	sg[0].data = NULL;
+	sg[0].len  = 0;
+	sg[1].data = p;
+	sg[1].len  = p_len;
+	sg[2].data = &pad;
+	sg[2].len  = sizeof(pad);
+
+	err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out);
+	if (err) {
+		return err;
+	}
+
+	net_id[0] = out[15] & 0x7f;
+
+	sg[0].data = out;
+	sg[0].len  = sizeof(out);
+	pad = 0x02;
+
+	err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out);
+	if (err) {
+		return err;
+	}
+
+	memcpy(enc_key, out, 16);
+
+	pad = 0x03;
+
+	err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out);
+	if (err) {
+		return err;
+	}
+
+	memcpy(priv_key, out, 16);
+
+	BT_DBG("NID 0x%02x enc_key %s", net_id[0], bt_hex(enc_key, 16));
+	BT_DBG("priv_key %s", bt_hex(priv_key, 16));
+
+	return 0;
+}
+
+int bt_mesh_k3(const u8_t n[16], u8_t out[8])
+{
+	u8_t id64[] = { 'i', 'd', '6', '4', 0x01 };
+	u8_t tmp[16];
+	u8_t t[16];
+	int err;
+
+	err = bt_mesh_s1("smk3", tmp);
+	if (err) {
+		return err;
+	}
+
+	err = bt_mesh_aes_cmac_one(tmp, n, 16, t);
+	if (err) {
+		return err;
+	}
+
+	err = bt_mesh_aes_cmac_one(t, id64, sizeof(id64), tmp);
+	if (err) {
+		return err;
+	}
+
+	memcpy(out, tmp + 8, 8);
+
+	return 0;
+}
+
+int bt_mesh_k4(const u8_t n[16], u8_t out[1])
+{
+	u8_t id6[] = { 'i', 'd', '6', 0x01 };
+	u8_t tmp[16];
+	u8_t t[16];
+	int err;
+
+	err = bt_mesh_s1("smk4", tmp);
+	if (err) {
+		return err;
+	}
+
+	err = bt_mesh_aes_cmac_one(tmp, n, 16, t);
+	if (err) {
+		return err;
+	}
+
+	err = bt_mesh_aes_cmac_one(t, id6, sizeof(id6), tmp);
+	if (err) {
+		return err;
+	}
+
+	out[0] = tmp[15] & BIT_MASK(6);
+
+	return 0;
+}
+
+int bt_mesh_id128(const u8_t n[16], const char *s, u8_t out[16])
+{
+	const char *id128 = "id128\x01";
+	u8_t salt[16];
+	int err;
+
+	err = bt_mesh_s1(s, salt);
+	if (err) {
+		return err;
+	}
+
+	return bt_mesh_k1(n, 16, salt, id128, out);
+}
+
+static int bt_mesh_ccm_decrypt(const u8_t key[16], u8_t nonce[13],
+			       const u8_t *enc_msg, size_t msg_len,
+			       const u8_t *aad, size_t aad_len,
+			       u8_t *out_msg, size_t mic_size)
+{
+	u8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16], mic[16];
+	u16_t last_blk, blk_cnt;
+	size_t i, j;
+	int err;
+
+	if (msg_len < 1 || aad_len >= 0xff00) {
+		return -EINVAL;
+	}
+
+	/* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
+	pmsg[0] = 0x01;
+	memcpy(pmsg + 1, nonce, 13);
+	sys_put_be16(0x0000, pmsg + 14);
+
+	err = bt_encrypt_be(key, pmsg, cmic);
+	if (err) {
+		return err;
+	}
+
+	/* X_0 = e(AppKey, 0x09 || nonce || length) */
+	if (mic_size == sizeof(u64_t)) {
+		pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
+	} else {
+		pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
+	}
+
+	memcpy(pmsg + 1, nonce, 13);
+	sys_put_be16(msg_len, pmsg + 14);
+
+	err = bt_encrypt_be(key, pmsg, Xn);
+	if (err) {
+		return err;
+	}
+
+	/* If AAD is being used to authenticate, include it here */
+	if (aad_len) {
+		sys_put_be16(aad_len, pmsg);
+
+		for (i = 0; i < sizeof(u16_t); i++) {
+			pmsg[i] = Xn[i] ^ pmsg[i];
+		}
+
+		j = 0;
+		aad_len += sizeof(u16_t);
+		while (aad_len > 16) {
+			do {
+				pmsg[i] = Xn[i] ^ aad[j];
+				i++, j++;
+			} while (i < 16);
+
+			aad_len -= 16;
+			i = 0;
+
+			err = bt_encrypt_be(key, pmsg, Xn);
+			if (err) {
+				return err;
+			}
+		}
+
+		for (; i < aad_len; i++, j++) {
+			pmsg[i] = Xn[i] ^ aad[j];
+		}
+
+		for (i = aad_len; i < 16; i++) {
+			pmsg[i] = Xn[i];
+		}
+
+		err = bt_encrypt_be(key, pmsg, Xn);
+		if (err) {
+			return err;
+		}
+	}
+
+	last_blk = msg_len % 16;
+	blk_cnt = (msg_len + 15) / 16;
+	if (!last_blk) {
+		last_blk = 16;
+	}
+
+	for (j = 0; j < blk_cnt; j++) {
+		if (j + 1 == blk_cnt) {
+			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+			pmsg[0] = 0x01;
+			memcpy(pmsg + 1, nonce, 13);
+			sys_put_be16(j + 1, pmsg + 14);
+
+			err = bt_encrypt_be(key, pmsg, cmsg);
+			if (err) {
+				return err;
+			}
+
+			/* Encrypted = Payload[0-15] ^ C_1 */
+			for (i = 0; i < last_blk; i++) {
+				msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
+			}
+
+			memcpy(out_msg + (j * 16), msg, last_blk);
+
+			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+			for (i = 0; i < last_blk; i++) {
+				pmsg[i] = Xn[i] ^ msg[i];
+			}
+
+			for (i = last_blk; i < 16; i++) {
+				pmsg[i] = Xn[i] ^ 0x00;
+			}
+
+			err = bt_encrypt_be(key, pmsg, Xn);
+			if (err) {
+				return err;
+			}
+
+			/* MIC = C_mic ^ X_1 */
+			for (i = 0; i < sizeof(mic); i++) {
+				mic[i] = cmic[i] ^ Xn[i];
+			}
+		} else {
+			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+			pmsg[0] = 0x01;
+			memcpy(pmsg + 1, nonce, 13);
+			sys_put_be16(j + 1, pmsg + 14);
+
+			err = bt_encrypt_be(key, pmsg, cmsg);
+			if (err) {
+				return err;
+			}
+
+			/* Encrypted = Payload[0-15] ^ C_1 */
+			for (i = 0; i < 16; i++) {
+				msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
+			}
+
+			memcpy(out_msg + (j * 16), msg, 16);
+
+			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+			for (i = 0; i < 16; i++) {
+				pmsg[i] = Xn[i] ^ msg[i];
+			}
+
+			err = bt_encrypt_be(key, pmsg, Xn);
+			if (err) {
+				return err;
+			}
+		}
+	}
+
+	if (memcmp(mic, enc_msg + msg_len, mic_size)) {
+		return -EBADMSG;
+	}
+
+	return 0;
+}
+
+static int bt_mesh_ccm_encrypt(const u8_t key[16], u8_t nonce[13],
+			       const u8_t *msg, size_t msg_len,
+			       const u8_t *aad, size_t aad_len,
+			       u8_t *out_msg, size_t mic_size)
+{
+	u8_t pmsg[16], cmic[16], cmsg[16], mic[16], Xn[16];
+	u16_t blk_cnt, last_blk;
+	size_t i, j;
+	int err;
+
+	BT_DBG("key %s", bt_hex(key, 16));
+	BT_DBG("nonce %s", bt_hex(nonce, 13));
+	BT_DBG("msg (len %zu) %s", msg_len, bt_hex(msg, msg_len));
+	BT_DBG("aad_len %zu mic_size %zu", aad_len, mic_size);
+
+	/* Unsupported AAD size */
+	if (aad_len >= 0xff00) {
+		return -EINVAL;
+	}
+
+	/* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
+	pmsg[0] = 0x01;
+	memcpy(pmsg + 1, nonce, 13);
+	sys_put_be16(0x0000, pmsg + 14);
+
+	err = bt_encrypt_be(key, pmsg, cmic);
+	if (err) {
+		return err;
+	}
+
+	/* X_0 = e(AppKey, 0x09 || nonce || length) */
+	if (mic_size == sizeof(u64_t)) {
+		pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
+	} else {
+		pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
+	}
+
+	memcpy(pmsg + 1, nonce, 13);
+	sys_put_be16(msg_len, pmsg + 14);
+
+	err = bt_encrypt_be(key, pmsg, Xn);
+	if (err) {
+		return err;
+	}
+
+	/* If AAD is being used to authenticate, include it here */
+	if (aad_len) {
+		sys_put_be16(aad_len, pmsg);
+
+		for (i = 0; i < sizeof(u16_t); i++) {
+			pmsg[i] = Xn[i] ^ pmsg[i];
+		}
+
+		j = 0;
+		aad_len += sizeof(u16_t);
+		while (aad_len > 16) {
+			do {
+				pmsg[i] = Xn[i] ^ aad[j];
+				i++, j++;
+			} while (i < 16);
+
+			aad_len -= 16;
+			i = 0;
+
+			err = bt_encrypt_be(key, pmsg, Xn);
+			if (err) {
+				return err;
+			}
+		}
+
+		for (; i < aad_len; i++, j++) {
+			pmsg[i] = Xn[i] ^ aad[j];
+		}
+
+		for (i = aad_len; i < 16; i++) {
+			pmsg[i] = Xn[i];
+		}
+
+		err = bt_encrypt_be(key, pmsg, Xn);
+		if (err) {
+			return err;
+		}
+	}
+
+	last_blk = msg_len % 16;
+	blk_cnt = (msg_len + 15) / 16;
+	if (!last_blk) {
+		last_blk = 16;
+	}
+
+	for (j = 0; j < blk_cnt; j++) {
+		if (j + 1 == blk_cnt) {
+			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+			for (i = 0; i < last_blk; i++) {
+				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
+			}
+			for (i = last_blk; i < 16; i++) {
+				pmsg[i] = Xn[i] ^ 0x00;
+			}
+
+			err = bt_encrypt_be(key, pmsg, Xn);
+			if (err) {
+				return err;
+			}
+
+			/* MIC = C_mic ^ X_1 */
+			for (i = 0; i < sizeof(mic); i++) {
+				mic[i] = cmic[i] ^ Xn[i];
+			}
+
+			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+			pmsg[0] = 0x01;
+			memcpy(pmsg + 1, nonce, 13);
+			sys_put_be16(j + 1, pmsg + 14);
+
+			err = bt_encrypt_be(key, pmsg, cmsg);
+			if (err) {
+				return err;
+			}
+
+			/* Encrypted = Payload[0-15] ^ C_1 */
+			for (i = 0; i < last_blk; i++) {
+				out_msg[(j * 16) + i] =
+					msg[(j * 16) + i] ^ cmsg[i];
+			}
+		} else {
+			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+			for (i = 0; i < 16; i++) {
+				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
+			}
+
+			err = bt_encrypt_be(key, pmsg, Xn);
+			if (err) {
+				return err;
+			}
+
+			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+			pmsg[0] = 0x01;
+			memcpy(pmsg + 1, nonce, 13);
+			sys_put_be16(j + 1, pmsg + 14);
+
+			err = bt_encrypt_be(key, pmsg, cmsg);
+			if (err) {
+				return err;
+			}
+
+			/* Encrypted = Payload[0-15] ^ C_N */
+			for (i = 0; i < 16; i++) {
+				out_msg[(j * 16) + i] =
+					msg[(j * 16) + i] ^ cmsg[i];
+			}
+
+		}
+	}
+
+	memcpy(out_msg + msg_len, mic, mic_size);
+
+	return 0;
+}
+
+static void create_proxy_nonce(u8_t nonce[13], const u8_t *pdu,
+			       u32_t iv_index)
+{
+	/* Nonce Type */
+	nonce[0] = 0x03;
+
+	/* Pad */
+	nonce[1] = 0x00;
+
+	/* Sequence Number */
+	nonce[2] = pdu[2];
+	nonce[3] = pdu[3];
+	nonce[4] = pdu[4];
+
+	/* Source Address */
+	nonce[5] = pdu[5];
+	nonce[6] = pdu[6];
+
+	/* Pad */
+	nonce[7] = 0;
+	nonce[8] = 0;
+
+	/* IV Index */
+	sys_put_be32(iv_index, &nonce[9]);
+}
+
+static void create_net_nonce(u8_t nonce[13], const u8_t *pdu,
+			     u32_t iv_index)
+{
+	/* Nonce Type */
+	nonce[0] = 0x00;
+
+	/* FRND + TTL */
+	nonce[1] = pdu[1];
+
+	/* Sequence Number */
+	nonce[2] = pdu[2];
+	nonce[3] = pdu[3];
+	nonce[4] = pdu[4];
+
+	/* Source Address */
+	nonce[5] = pdu[5];
+	nonce[6] = pdu[6];
+
+	/* Pad */
+	nonce[7] = 0;
+	nonce[8] = 0;
+
+	/* IV Index */
+	sys_put_be32(iv_index, &nonce[9]);
+}
+
+int bt_mesh_net_obfuscate(u8_t *pdu, u32_t iv_index,
+			  const u8_t privacy_key[16])
+{
+	u8_t priv_rand[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, };
+	u8_t tmp[16];
+	int err, i;
+
+	BT_DBG("IVIndex %u, PrivacyKey %s", (unsigned) iv_index,
+	       bt_hex(privacy_key, 16));
+
+	sys_put_be32(iv_index, &priv_rand[5]);
+	memcpy(&priv_rand[9], &pdu[7], 7);
+
+	BT_DBG("PrivacyRandom %s", bt_hex(priv_rand, 16));
+
+	err = bt_encrypt_be(privacy_key, priv_rand, tmp);
+	if (err) {
+		return err;
+	}
+
+	for (i = 0; i < 6; i++) {
+		pdu[1 + i] ^= tmp[i];
+	}
+
+	return 0;
+}
+
+int bt_mesh_net_encrypt(const u8_t key[16], struct os_mbuf *buf,
+			u32_t iv_index, bool proxy)
+{
+	u8_t mic_len = NET_MIC_LEN(buf->om_data);
+	u8_t nonce[13];
+	int err;
+
+	BT_DBG("IVIndex %u EncKey %s mic_len %u", (unsigned) iv_index,
+	       bt_hex(key, 16), mic_len);
+	BT_DBG("PDU (len %u) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROXY) && proxy) {
+		create_proxy_nonce(nonce, buf->om_data, iv_index);
+	} else {
+		create_net_nonce(nonce, buf->om_data, iv_index);
+	}
+
+	BT_DBG("Nonce %s", bt_hex(nonce, 13));
+
+	err = bt_mesh_ccm_encrypt(key, nonce, &buf->om_data[7], buf->om_len - 7,
+				  NULL, 0, &buf->om_data[7], mic_len);
+	if (!err) {
+		net_buf_simple_add(buf, mic_len);
+	}
+
+	return err;
+}
+
+int bt_mesh_net_decrypt(const u8_t key[16], struct os_mbuf *buf,
+			u32_t iv_index, bool proxy)
+{
+	u8_t mic_len = NET_MIC_LEN(buf->om_data);
+	u8_t nonce[13];
+
+	BT_DBG("PDU (%u bytes) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+	BT_DBG("iv_index %u, key %s mic_len %u", (unsigned) iv_index,
+	       bt_hex(key, 16), mic_len);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROXY) && proxy) {
+		create_proxy_nonce(nonce, buf->om_data, iv_index);
+	} else {
+		create_net_nonce(nonce, buf->om_data, iv_index);
+	}
+
+	BT_DBG("Nonce %s", bt_hex(nonce, 13));
+
+	buf->om_len -= mic_len;
+
+	return bt_mesh_ccm_decrypt(key, nonce, &buf->om_data[7], buf->om_len - 7,
+				   NULL, 0, &buf->om_data[7], mic_len);
+}
+
+static void create_app_nonce(u8_t nonce[13], bool dev_key, u8_t aszmic,
+			     u16_t src, u16_t dst, u32_t seq_num,
+			     u32_t iv_index)
+{
+	if (dev_key) {
+		nonce[0] = 0x02;
+	} else {
+		nonce[0] = 0x01;
+	}
+
+	sys_put_be32((seq_num | ((u32_t)aszmic << 31)), &nonce[1]);
+
+	sys_put_be16(src, &nonce[5]);
+	sys_put_be16(dst, &nonce[7]);
+
+	sys_put_be32(iv_index, &nonce[9]);
+}
+
+static int mesh_app_encrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
+			    struct os_mbuf *buf, const u8_t *ad,
+			    u16_t src, u16_t dst, u32_t seq_num, u32_t iv_index)
+{
+	u8_t nonce[13];
+
+	BT_DBG("AppKey %s", bt_hex(key, 16));
+	BT_DBG("dev_key %u src 0x%04x dst 0x%04x", dev_key, src, dst);
+	BT_DBG("seq_num 0x%08x iv_index 0x%08x", (unsigned) seq_num,
+	       (unsigned) iv_index);
+	BT_DBG("Clear: %s", bt_hex(buf->om_data, buf->om_len));
+
+	create_app_nonce(nonce, dev_key, aszmic, src, dst, seq_num, iv_index);
+
+	BT_DBG("Nonce  %s", bt_hex(nonce, 13));
+
+	return bt_mesh_ccm_encrypt(key, nonce, buf->om_data, buf->om_len, ad,
+				   ad ? 16 : 0, buf->om_data,
+				   APP_MIC_LEN(aszmic));
+}
+
+int bt_mesh_app_encrypt_in_place(const u8_t key[16], bool dev_key, u8_t aszmic,
+				 struct os_mbuf *buf, const u8_t *ad, u16_t src,
+				 u16_t dst, u32_t seq_num, u32_t iv_index)
+{
+	int err;
+
+	err = mesh_app_encrypt(key, dev_key, aszmic, buf, ad, src, dst,
+			       seq_num, iv_index);
+	if (!err) {
+		BT_DBG("Encr: %s", bt_hex(buf->om_data, buf->om_len));
+	}
+
+	return err;
+}
+
+int bt_mesh_app_encrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
+			struct os_mbuf *buf, const u8_t *ad,
+			u16_t src, u16_t dst, u32_t seq_num, u32_t iv_index)
+{
+	int err;
+
+	err = mesh_app_encrypt(key, dev_key, aszmic, buf, ad, src, dst,
+			       seq_num, iv_index);
+
+	if (!err) {
+		net_buf_simple_add(buf, APP_MIC_LEN(aszmic));
+		BT_DBG("Encr: %s", bt_hex(buf->om_data, buf->om_len));
+	}
+
+	return err;
+}
+
+static int mesh_app_decrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
+			    struct os_mbuf *buf, struct os_mbuf *out,
+			    const u8_t *ad, u16_t src, u16_t dst,
+			    u32_t seq_num, u32_t iv_index)
+{
+	u8_t nonce[13];
+
+	BT_DBG("EncData (len %u) %s", buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	create_app_nonce(nonce, dev_key, aszmic, src, dst, seq_num, iv_index);
+
+	BT_DBG("AppKey %s", bt_hex(key, 16));
+	BT_DBG("Nonce  %s", bt_hex(nonce, 13));
+
+	return bt_mesh_ccm_decrypt(key, nonce, buf->om_data, buf->om_len, ad,
+				   ad ? 16 : 0, out->om_data,
+				   APP_MIC_LEN(aszmic));
+}
+
+int bt_mesh_app_decrypt_in_place(const u8_t key[16], bool dev_key, u8_t aszmic,
+				 struct os_mbuf *buf, const u8_t *ad, u16_t src,
+				 u16_t dst, u32_t seq_num, u32_t iv_index)
+{
+	return mesh_app_decrypt(key, dev_key, aszmic, buf, buf,
+				ad, src, dst, seq_num, iv_index);
+}
+
+int bt_mesh_app_decrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
+			struct os_mbuf *buf, struct os_mbuf *out,
+			const u8_t *ad, u16_t src, u16_t dst, u32_t seq_num,
+			u32_t iv_index)
+{
+	int err;
+
+	err = mesh_app_decrypt(key, dev_key, aszmic, buf, out,
+			       ad, src, dst, seq_num, iv_index);
+	if (!err) {
+		net_buf_simple_add(out, buf->om_len);
+	}
+
+	return err;
+}
+
+/* reversed, 8-bit, poly=0x07 */
+static const u8_t crc_table[256] = {
+	0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
+	0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
+	0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
+	0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
+
+	0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
+	0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
+	0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
+	0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
+
+	0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
+	0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
+	0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
+	0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
+
+	0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
+	0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
+	0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
+	0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
+
+	0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
+	0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
+	0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
+	0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
+
+	0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
+	0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
+	0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
+	0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
+
+	0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
+	0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
+	0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
+	0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
+
+	0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
+	0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
+	0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
+	0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
+};
+
+u8_t bt_mesh_fcs_calc(const u8_t *data, u8_t data_len)
+{
+	u8_t fcs = 0xff;
+
+	while (data_len--) {
+		fcs = crc_table[fcs ^ *data++];
+	}
+
+	BT_DBG("fcs 0x%02x", 0xff - fcs);
+
+	return 0xff - fcs;
+}
+
+bool bt_mesh_fcs_check(struct os_mbuf *buf, u8_t received_fcs)
+{
+	const u8_t *data = buf->om_data;
+	u16_t data_len = buf->om_len;
+	u8_t fcs = 0xff;
+
+	while (data_len--) {
+		fcs = crc_table[fcs ^ *data++];
+	}
+
+	return crc_table[fcs ^ received_fcs] == 0xcf;
+}
+
+int bt_mesh_virtual_addr(const u8_t virtual_label[16], u16_t *addr)
+{
+	u8_t salt[16];
+	u8_t tmp[16];
+	int err;
+
+	err = bt_mesh_s1("vtad", salt);
+	if (err) {
+		return err;
+	}
+
+	err = bt_mesh_aes_cmac_one(salt, virtual_label, 16, tmp);
+	if (err) {
+		return err;
+	}
+
+	*addr = (sys_get_be16(&tmp[14]) & 0x3fff) | 0x8000;
+
+	return 0;
+}
+
+int bt_mesh_prov_conf_salt(const u8_t conf_inputs[145], u8_t salt[16])
+{
+	const u8_t conf_salt_key[16] = { 0 };
+
+	return bt_mesh_aes_cmac_one(conf_salt_key, conf_inputs, 145, salt);
+}
+
+int bt_mesh_prov_conf_key(const u8_t dhkey[32], const u8_t conf_salt[16],
+			  u8_t conf_key[16])
+{
+	return bt_mesh_k1(dhkey, 32, conf_salt, "prck", conf_key);
+}
+
+int bt_mesh_prov_conf(const u8_t conf_key[16], const u8_t rand[16],
+		      const u8_t auth[16], u8_t conf[16])
+{
+	struct bt_mesh_sg sg[] = { { rand, 16 }, { auth, 16 } };
+
+	BT_DBG("ConfirmationKey %s", bt_hex(conf_key, 16));
+	BT_DBG("RandomDevice %s", bt_hex(rand, 16));
+	BT_DBG("AuthValue %s", bt_hex(auth, 16));
+
+	return bt_mesh_aes_cmac(conf_key, sg, ARRAY_SIZE(sg), conf);
+}
+
+int bt_mesh_prov_decrypt(const u8_t key[16], u8_t nonce[13],
+			 const u8_t data[25 + 8], u8_t out[25])
+{
+	return bt_mesh_ccm_decrypt(key, nonce, data, 25, NULL, 0, out, 8);
+}
+
+int bt_mesh_prov_encrypt(const u8_t key[16], u8_t nonce[13],
+			 const u8_t data[25], u8_t out[25 + 8])
+{
+	return bt_mesh_ccm_encrypt(key, nonce, data, 25, NULL, 0, out, 8);
+}
+
+int bt_mesh_beacon_auth(const u8_t beacon_key[16], u8_t flags,
+			const u8_t net_id[8], u32_t iv_index,
+			u8_t auth[8])
+{
+	u8_t msg[13], tmp[16];
+	int err;
+
+	BT_DBG("BeaconKey %s", bt_hex(beacon_key, 16));
+	BT_DBG("NetId %s", bt_hex(net_id, 8));
+	BT_DBG("IV Index 0x%08x", (unsigned) iv_index);
+
+	msg[0] = flags;
+	memcpy(&msg[1], net_id, 8);
+	sys_put_be32(iv_index, &msg[9]);
+
+	BT_DBG("BeaconMsg %s", bt_hex(msg, sizeof(msg)));
+
+	err = bt_mesh_aes_cmac_one(beacon_key, msg, sizeof(msg), tmp);
+	if (!err) {
+		memcpy(auth, tmp, 8);
+	}
+
+	return err;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/crypto.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/crypto.h
new file mode 100644
index 00000000..745cf324
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/crypto.h
@@ -0,0 +1,170 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __CRYPTO_H__
+#define __CRYPTO_H__
+
+#include "mesh/mesh.h"
+
+struct bt_mesh_sg {
+	const void *data;
+	size_t len;
+};
+
+int bt_mesh_aes_cmac(const u8_t key[16], struct bt_mesh_sg *sg,
+		     size_t sg_len, u8_t mac[16]);
+
+static inline int bt_mesh_aes_cmac_one(const u8_t key[16], const void *m,
+				       size_t len, u8_t mac[16])
+{
+	struct bt_mesh_sg sg = { m, len };
+
+	return bt_mesh_aes_cmac(key, &sg, 1, mac);
+}
+
+static inline bool bt_mesh_s1(const char *m, u8_t salt[16])
+{
+	const u8_t zero[16] = { 0 };
+
+	return bt_mesh_aes_cmac_one(zero, m, strlen(m), salt);
+}
+
+int bt_mesh_k1(const u8_t *ikm, size_t ikm_len, const u8_t salt[16],
+	       const char *info, u8_t okm[16]);
+
+#define bt_mesh_k1_str(ikm, ikm_len, salt_str, info, okm) \
+({ \
+	const u8_t salt[16] = salt_str; \
+	bt_mesh_k1(ikm, ikm_len, salt, info, okm); \
+})
+
+int bt_mesh_k2(const u8_t n[16], const u8_t *p, size_t p_len,
+	       u8_t net_id[1], u8_t enc_key[16], u8_t priv_key[16]);
+
+int bt_mesh_k3(const u8_t n[16], u8_t out[8]);
+
+int bt_mesh_k4(const u8_t n[16], u8_t out[1]);
+
+int bt_mesh_id128(const u8_t n[16], const char *s, u8_t out[16]);
+
+static inline int bt_mesh_id_resolving_key(const u8_t net_key[16],
+					   u8_t resolving_key[16])
+{
+	return bt_mesh_k1_str(net_key, 16, "smbt", "smbi", resolving_key);
+}
+
+static inline int bt_mesh_identity_key(const u8_t net_key[16],
+				       u8_t identity_key[16])
+{
+	return bt_mesh_id128(net_key, "nkik", identity_key);
+}
+
+static inline int bt_mesh_beacon_key(const u8_t net_key[16],
+				     u8_t beacon_key[16])
+{
+	return bt_mesh_id128(net_key, "nkbk", beacon_key);
+}
+
+int bt_mesh_beacon_auth(const u8_t beacon_key[16], u8_t flags,
+			const u8_t net_id[16], u32_t iv_index,
+			u8_t auth[8]);
+
+static inline int bt_mesh_app_id(const u8_t app_key[16], u8_t app_id[1])
+{
+	return bt_mesh_k4(app_key, app_id);
+}
+
+static inline int bt_mesh_session_key(const u8_t dhkey[32],
+				      const u8_t prov_salt[16],
+				      u8_t session_key[16])
+{
+	return bt_mesh_k1(dhkey, 32, prov_salt, "prsk", session_key);
+}
+
+static inline int bt_mesh_prov_nonce(const u8_t dhkey[32],
+				     const u8_t prov_salt[16],
+				     u8_t nonce[13])
+{
+	u8_t tmp[16];
+	int err;
+
+	err = bt_mesh_k1(dhkey, 32, prov_salt, "prsn", tmp);
+	if (!err) {
+		memcpy(nonce, tmp + 3, 13);
+	}
+
+	return err;
+}
+
+static inline int bt_mesh_dev_key(const u8_t dhkey[32],
+				  const u8_t prov_salt[16],
+				  u8_t dev_key[16])
+{
+	return bt_mesh_k1(dhkey, 32, prov_salt, "prdk", dev_key);
+}
+
+static inline int bt_mesh_prov_salt(const u8_t conf_salt[16],
+				    const u8_t prov_rand[16],
+				    const u8_t dev_rand[16],
+				    u8_t prov_salt[16])
+{
+	const u8_t prov_salt_key[16] = { 0 };
+	struct bt_mesh_sg sg[] = {
+		{ conf_salt, 16 },
+		{ prov_rand, 16 },
+		{ dev_rand, 16 },
+	};
+
+	return bt_mesh_aes_cmac(prov_salt_key, sg, ARRAY_SIZE(sg), prov_salt);
+}
+
+int bt_mesh_net_obfuscate(u8_t *pdu, u32_t iv_index,
+			  const u8_t privacy_key[16]);
+
+int bt_mesh_net_encrypt(const u8_t key[16], struct os_mbuf *buf,
+			u32_t iv_index, bool proxy);
+
+int bt_mesh_net_decrypt(const u8_t key[16], struct os_mbuf *buf,
+			u32_t iv_index, bool proxy);
+
+int bt_mesh_app_encrypt_in_place(const u8_t key[16], bool dev_key, u8_t aszmic,
+				 struct os_mbuf*buf, const u8_t *ad, u16_t src,
+				 u16_t dst, u32_t seq_num, u32_t iv_index);
+
+int bt_mesh_app_encrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
+			struct os_mbuf*buf, const u8_t *ad,
+			u16_t src, u16_t dst, u32_t seq_num, u32_t iv_index);
+
+int bt_mesh_app_decrypt_in_place(const u8_t key[16], bool dev_key, u8_t aszmic,
+				 struct os_mbuf *buf, const u8_t *ad, u16_t src,
+				 u16_t dst, u32_t seq_num, u32_t iv_index);
+
+int bt_mesh_app_decrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
+			struct os_mbuf*buf, struct os_mbuf*out,
+			const u8_t *ad, u16_t src, u16_t dst, u32_t seq_num,
+			u32_t iv_index);
+
+u8_t bt_mesh_fcs_calc(const u8_t *data, u8_t data_len);
+
+bool bt_mesh_fcs_check(struct os_mbuf *buf, u8_t received_fcs);
+
+int bt_mesh_virtual_addr(const u8_t virtual_label[16], u16_t *addr);
+
+int bt_mesh_prov_conf_salt(const u8_t conf_inputs[145], u8_t salt[16]);
+
+int bt_mesh_prov_conf_key(const u8_t dhkey[32], const u8_t conf_salt[16],
+			  u8_t conf_key[16]);
+
+int bt_mesh_prov_conf(const u8_t conf_key[16], const u8_t rand[16],
+		      const u8_t auth[16], u8_t conf[16]);
+
+int bt_mesh_prov_decrypt(const u8_t key[16], u8_t nonce[13],
+			 const u8_t data[25 + 8], u8_t out[25]);
+
+int bt_mesh_prov_encrypt(const u8_t key[16], u8_t nonce[13],
+			 const u8_t data[25], u8_t out[25 + 8]);
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/foundation.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/foundation.h
new file mode 100644
index 00000000..ee615ae9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/foundation.h
@@ -0,0 +1,171 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __FUNDATION_H__
+#define __FUNDATION_H__
+
+#define OP_APP_KEY_ADD                     BT_MESH_MODEL_OP_1(0x00)
+#define OP_APP_KEY_UPDATE                  BT_MESH_MODEL_OP_1(0x01)
+#define OP_DEV_COMP_DATA_STATUS            BT_MESH_MODEL_OP_1(0x02)
+#define OP_MOD_PUB_SET                     BT_MESH_MODEL_OP_1(0x03)
+#define OP_HEALTH_CURRENT_STATUS           BT_MESH_MODEL_OP_1(0x04)
+#define OP_HEALTH_FAULT_STATUS             BT_MESH_MODEL_OP_1(0x05)
+#define OP_HEARTBEAT_PUB_STATUS            BT_MESH_MODEL_OP_1(0x06)
+#define OP_APP_KEY_DEL                     BT_MESH_MODEL_OP_2(0x80, 0x00)
+#define OP_APP_KEY_GET                     BT_MESH_MODEL_OP_2(0x80, 0x01)
+#define OP_APP_KEY_LIST                    BT_MESH_MODEL_OP_2(0x80, 0x02)
+#define OP_APP_KEY_STATUS                  BT_MESH_MODEL_OP_2(0x80, 0x03)
+#define OP_ATTENTION_GET                   BT_MESH_MODEL_OP_2(0x80, 0x04)
+#define OP_ATTENTION_SET                   BT_MESH_MODEL_OP_2(0x80, 0x05)
+#define OP_ATTENTION_SET_UNREL             BT_MESH_MODEL_OP_2(0x80, 0x06)
+#define OP_ATTENTION_STATUS                BT_MESH_MODEL_OP_2(0x80, 0x07)
+#define OP_DEV_COMP_DATA_GET               BT_MESH_MODEL_OP_2(0x80, 0x08)
+#define OP_BEACON_GET                      BT_MESH_MODEL_OP_2(0x80, 0x09)
+#define OP_BEACON_SET                      BT_MESH_MODEL_OP_2(0x80, 0x0a)
+#define OP_BEACON_STATUS                   BT_MESH_MODEL_OP_2(0x80, 0x0b)
+#define OP_DEFAULT_TTL_GET                 BT_MESH_MODEL_OP_2(0x80, 0x0c)
+#define OP_DEFAULT_TTL_SET                 BT_MESH_MODEL_OP_2(0x80, 0x0d)
+#define OP_DEFAULT_TTL_STATUS              BT_MESH_MODEL_OP_2(0x80, 0x0e)
+#define OP_FRIEND_GET                      BT_MESH_MODEL_OP_2(0x80, 0x0f)
+#define OP_FRIEND_SET                      BT_MESH_MODEL_OP_2(0x80, 0x10)
+#define OP_FRIEND_STATUS                   BT_MESH_MODEL_OP_2(0x80, 0x11)
+#define OP_GATT_PROXY_GET                  BT_MESH_MODEL_OP_2(0x80, 0x12)
+#define OP_GATT_PROXY_SET                  BT_MESH_MODEL_OP_2(0x80, 0x13)
+#define OP_GATT_PROXY_STATUS               BT_MESH_MODEL_OP_2(0x80, 0x14)
+#define OP_KRP_GET                         BT_MESH_MODEL_OP_2(0x80, 0x15)
+#define OP_KRP_SET                         BT_MESH_MODEL_OP_2(0x80, 0x16)
+#define OP_KRP_STATUS                      BT_MESH_MODEL_OP_2(0x80, 0x17)
+#define OP_MOD_PUB_GET                     BT_MESH_MODEL_OP_2(0x80, 0x18)
+#define OP_MOD_PUB_STATUS                  BT_MESH_MODEL_OP_2(0x80, 0x19)
+#define OP_MOD_PUB_VA_SET                  BT_MESH_MODEL_OP_2(0x80, 0x1a)
+#define OP_MOD_SUB_ADD                     BT_MESH_MODEL_OP_2(0x80, 0x1b)
+#define OP_MOD_SUB_DEL                     BT_MESH_MODEL_OP_2(0x80, 0x1c)
+#define OP_MOD_SUB_DEL_ALL                 BT_MESH_MODEL_OP_2(0x80, 0x1d)
+#define OP_MOD_SUB_OVERWRITE               BT_MESH_MODEL_OP_2(0x80, 0x1e)
+#define OP_MOD_SUB_STATUS                  BT_MESH_MODEL_OP_2(0x80, 0x1f)
+#define OP_MOD_SUB_VA_ADD                  BT_MESH_MODEL_OP_2(0x80, 0x20)
+#define OP_MOD_SUB_VA_DEL                  BT_MESH_MODEL_OP_2(0x80, 0x21)
+#define OP_MOD_SUB_VA_OVERWRITE            BT_MESH_MODEL_OP_2(0x80, 0x22)
+#define OP_NET_TRANSMIT_GET                BT_MESH_MODEL_OP_2(0x80, 0x23)
+#define OP_NET_TRANSMIT_SET                BT_MESH_MODEL_OP_2(0x80, 0x24)
+#define OP_NET_TRANSMIT_STATUS             BT_MESH_MODEL_OP_2(0x80, 0x25)
+#define OP_RELAY_GET                       BT_MESH_MODEL_OP_2(0x80, 0x26)
+#define OP_RELAY_SET                       BT_MESH_MODEL_OP_2(0x80, 0x27)
+#define OP_RELAY_STATUS                    BT_MESH_MODEL_OP_2(0x80, 0x28)
+#define OP_MOD_SUB_GET                     BT_MESH_MODEL_OP_2(0x80, 0x29)
+#define OP_MOD_SUB_LIST                    BT_MESH_MODEL_OP_2(0x80, 0x2a)
+#define OP_MOD_SUB_GET_VND                 BT_MESH_MODEL_OP_2(0x80, 0x2b)
+#define OP_MOD_SUB_LIST_VND                BT_MESH_MODEL_OP_2(0x80, 0x2c)
+#define OP_LPN_TIMEOUT_GET                 BT_MESH_MODEL_OP_2(0x80, 0x2d)
+#define OP_LPN_TIMEOUT_STATUS              BT_MESH_MODEL_OP_2(0x80, 0x2e)
+#define OP_HEALTH_FAULT_CLEAR              BT_MESH_MODEL_OP_2(0x80, 0x2f)
+#define OP_HEALTH_FAULT_CLEAR_UNREL        BT_MESH_MODEL_OP_2(0x80, 0x30)
+#define OP_HEALTH_FAULT_GET                BT_MESH_MODEL_OP_2(0x80, 0x31)
+#define OP_HEALTH_FAULT_TEST               BT_MESH_MODEL_OP_2(0x80, 0x32)
+#define OP_HEALTH_FAULT_TEST_UNREL         BT_MESH_MODEL_OP_2(0x80, 0x33)
+#define OP_HEALTH_PERIOD_GET               BT_MESH_MODEL_OP_2(0x80, 0x34)
+#define OP_HEALTH_PERIOD_SET               BT_MESH_MODEL_OP_2(0x80, 0x35)
+#define OP_HEALTH_PERIOD_SET_UNREL         BT_MESH_MODEL_OP_2(0x80, 0x36)
+#define OP_HEALTH_PERIOD_STATUS            BT_MESH_MODEL_OP_2(0x80, 0x37)
+#define OP_HEARTBEAT_PUB_GET               BT_MESH_MODEL_OP_2(0x80, 0x38)
+#define OP_HEARTBEAT_PUB_SET               BT_MESH_MODEL_OP_2(0x80, 0x39)
+#define OP_HEARTBEAT_SUB_GET               BT_MESH_MODEL_OP_2(0x80, 0x3a)
+#define OP_HEARTBEAT_SUB_SET               BT_MESH_MODEL_OP_2(0x80, 0x3b)
+#define OP_HEARTBEAT_SUB_STATUS            BT_MESH_MODEL_OP_2(0x80, 0x3c)
+#define OP_MOD_APP_BIND                    BT_MESH_MODEL_OP_2(0x80, 0x3d)
+#define OP_MOD_APP_STATUS                  BT_MESH_MODEL_OP_2(0x80, 0x3e)
+#define OP_MOD_APP_UNBIND                  BT_MESH_MODEL_OP_2(0x80, 0x3f)
+#define OP_NET_KEY_ADD                     BT_MESH_MODEL_OP_2(0x80, 0x40)
+#define OP_NET_KEY_DEL                     BT_MESH_MODEL_OP_2(0x80, 0x41)
+#define OP_NET_KEY_GET                     BT_MESH_MODEL_OP_2(0x80, 0x42)
+#define OP_NET_KEY_LIST                    BT_MESH_MODEL_OP_2(0x80, 0x43)
+#define OP_NET_KEY_STATUS                  BT_MESH_MODEL_OP_2(0x80, 0x44)
+#define OP_NET_KEY_UPDATE                  BT_MESH_MODEL_OP_2(0x80, 0x45)
+#define OP_NODE_IDENTITY_GET               BT_MESH_MODEL_OP_2(0x80, 0x46)
+#define OP_NODE_IDENTITY_SET               BT_MESH_MODEL_OP_2(0x80, 0x47)
+#define OP_NODE_IDENTITY_STATUS            BT_MESH_MODEL_OP_2(0x80, 0x48)
+#define OP_NODE_RESET                      BT_MESH_MODEL_OP_2(0x80, 0x49)
+#define OP_NODE_RESET_STATUS               BT_MESH_MODEL_OP_2(0x80, 0x4a)
+#define OP_SIG_MOD_APP_GET                 BT_MESH_MODEL_OP_2(0x80, 0x4b)
+#define OP_SIG_MOD_APP_LIST                BT_MESH_MODEL_OP_2(0x80, 0x4c)
+#define OP_VND_MOD_APP_GET                 BT_MESH_MODEL_OP_2(0x80, 0x4d)
+#define OP_VND_MOD_APP_LIST                BT_MESH_MODEL_OP_2(0x80, 0x4e)
+
+#define STATUS_SUCCESS                     0x00
+#define STATUS_INVALID_ADDRESS             0x01
+#define STATUS_INVALID_MODEL               0x02
+#define STATUS_INVALID_APPKEY              0x03
+#define STATUS_INVALID_NETKEY              0x04
+#define STATUS_INSUFF_RESOURCES            0x05
+#define STATUS_IDX_ALREADY_STORED          0x06
+#define STATUS_NVAL_PUB_PARAM              0x07
+#define STATUS_NOT_SUB_MOD                 0x08
+#define STATUS_STORAGE_FAIL                0x09
+#define STATUS_FEAT_NOT_SUPP               0x0a
+#define STATUS_CANNOT_UPDATE               0x0b
+#define STATUS_CANNOT_REMOVE               0x0c
+#define STATUS_CANNOT_BIND                 0x0d
+#define STATUS_TEMP_STATE_CHG_FAIL         0x0e
+#define STATUS_CANNOT_SET                  0x0f
+#define STATUS_UNSPECIFIED                 0x10
+#define STATUS_INVALID_BINDING             0x11
+
+enum {
+	BT_MESH_VA_CHANGED,	/* Label information changed */
+};
+
+struct label {
+	u16_t ref;
+	u16_t addr;
+	u8_t  uuid[16];
+	atomic_t flags[1];
+};
+
+void bt_mesh_cfg_reset(void);
+
+void bt_mesh_heartbeat(u16_t src, u16_t dst, u8_t hops, u16_t feat);
+
+void bt_mesh_attention(struct bt_mesh_model *model, u8_t time);
+
+struct label *get_label(u16_t index);
+
+u8_t *bt_mesh_label_uuid_get(u16_t addr);
+
+struct bt_mesh_hb_pub *bt_mesh_hb_pub_get(void);
+void bt_mesh_hb_pub_disable(void);
+struct bt_mesh_cfg_srv *bt_mesh_cfg_get(void);
+
+u8_t bt_mesh_net_transmit_get(void);
+u8_t bt_mesh_relay_get(void);
+u8_t bt_mesh_friend_get(void);
+u8_t bt_mesh_relay_retransmit_get(void);
+u8_t bt_mesh_beacon_get(void);
+u8_t bt_mesh_gatt_proxy_get(void);
+u8_t bt_mesh_default_ttl_get(void);
+
+void bt_mesh_subnet_del(struct bt_mesh_subnet *sub, bool store);
+
+struct bt_mesh_app_key *bt_mesh_app_key_alloc(u16_t app_idx);
+void bt_mesh_app_key_del(struct bt_mesh_app_key *key, bool store);
+
+static inline void key_idx_pack(struct os_mbuf *buf,
+				u16_t idx1, u16_t idx2)
+{
+	net_buf_simple_add_le16(buf, idx1 | ((idx2 & 0x00f) << 12));
+	net_buf_simple_add_u8(buf, idx2 >> 4);
+}
+
+static inline void key_idx_unpack(struct os_mbuf *buf,
+				  u16_t *idx1, u16_t *idx2)
+{
+	*idx1 = sys_get_le16(&buf->om_data[0]) & 0xfff;
+	*idx2 = sys_get_le16(&buf->om_data[1]) >> 4;
+	net_buf_simple_pull(buf, 3);
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/friend.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/friend.c
new file mode 100644
index 00000000..9056a865
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/friend.c
@@ -0,0 +1,1651 @@
+	/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_CRYPTO_LOG
+
+#if MYNEWT_VAL(BLE_MESH_FRIEND)
+
+#include <stdint.h>
+#include <errno.h>
+#include <assert.h>
+
+#include "mesh/mesh.h"
+#include "mesh/slist.h"
+#include "mesh_priv.h"
+#include "crypto.h"
+#include "adv.h"
+#include "net.h"
+#include "transport.h"
+#include "access.h"
+#include "foundation.h"
+#include "friend.h"
+
+/* We reserve one extra buffer for each friendship, since we need to be able
+ * to resend the last sent PDU, which sits separately outside of the queue.
+ */
+#define FRIEND_BUF_COUNT ((MYNEWT_VAL(BLE_MESH_FRIEND_QUEUE_SIZE) + 1) * MYNEWT_VAL(BLE_MESH_FRIEND_LPN_COUNT))
+
+static os_membuf_t friend_buf_mem[OS_MEMPOOL_SIZE(
+		FRIEND_BUF_COUNT,
+		BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE)];
+
+struct os_mbuf_pool friend_os_mbuf_pool;
+static struct os_mempool friend_buf_mempool;
+
+#define NET_BUF_FRAGS        BIT(0)
+
+#define FRIEND_ADV(buf) CONTAINER_OF(BT_MESH_ADV(buf), struct friend_adv, adv)
+
+/* PDUs from Friend to the LPN should only be transmitted once with the
+ * smallest possible interval (20ms).
+ */
+#define FRIEND_XMIT         BT_MESH_TRANSMIT(0, 20)
+
+struct friend_pdu_info {
+	u16_t  src;
+	u16_t  dst;
+
+	u8_t   seq[3];
+
+	u8_t   ttl:7,
+	       ctl:1;
+
+	u32_t  iv_index;
+};
+
+static struct friend_adv {
+	struct bt_mesh_adv adv;
+	u16_t app_idx;
+} adv_pool[FRIEND_BUF_COUNT];
+
+static struct bt_mesh_adv *adv_alloc(int id)
+{
+	adv_pool[id].app_idx = BT_MESH_KEY_UNUSED;
+	return &adv_pool[id].adv;
+}
+
+static bool is_lpn_unicast(struct bt_mesh_friend *frnd, u16_t addr)
+{
+	if (frnd->lpn == BT_MESH_ADDR_UNASSIGNED) {
+		return false;
+	}
+
+	return (addr >= frnd->lpn && addr < (frnd->lpn + frnd->num_elem));
+}
+
+struct bt_mesh_friend *bt_mesh_friend_find(u16_t net_idx, u16_t lpn_addr,
+					   bool valid, bool established)
+{
+	int i;
+
+	BT_DBG("net_idx 0x%04x lpn_addr 0x%04x", net_idx, lpn_addr);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (valid && !frnd->valid) {
+			continue;
+		}
+
+		if (established && !frnd->established) {
+			continue;
+		}
+
+		if (net_idx != BT_MESH_KEY_ANY && frnd->net_idx != net_idx) {
+			continue;
+		}
+
+		if (is_lpn_unicast(frnd, lpn_addr)) {
+			return frnd;
+		}
+	}
+
+	return NULL;
+}
+
+static void purge_buffers(struct net_buf_slist_t *list)
+{
+	struct os_mbuf *buf;
+
+	while (!net_buf_slist_is_empty(list)) {
+		buf = (void *)net_buf_slist_get(list);
+		BT_MESH_ADV(buf)->flags &= ~NET_BUF_FRAGS;
+		net_buf_unref(buf);
+	}
+}
+
+/* Intentionally start a little bit late into the ReceiveWindow when
+ * it's large enough. This may improve reliability with some platforms,
+ * like the PTS, where the receiver might not have sufficiently compensated
+ * for internal latencies required to start scanning.
+ */
+static s32_t recv_delay(struct bt_mesh_friend *frnd)
+{
+#if CONFIG_BT_MESH_FRIEND_RECV_WIN > 50
+	return (s32_t)frnd->recv_delay + (CONFIG_BT_MESH_FRIEND_RECV_WIN / 5);
+#else
+	return frnd->recv_delay;
+#endif
+}
+
+static void friend_clear(struct bt_mesh_friend *frnd)
+{
+	int i;
+
+	BT_DBG("LPN 0x%04x", frnd->lpn);
+
+	k_delayed_work_cancel(&frnd->timer);
+
+	friend_cred_del(frnd->net_idx, frnd->lpn);
+
+	if (frnd->last) {
+		/* Cancel the sending if necessary */
+		if (frnd->pending_buf) {
+			BT_MESH_ADV(frnd->last)->busy = 0;
+		}
+
+		net_buf_unref(frnd->last);
+		frnd->last = NULL;
+	}
+
+	purge_buffers(&frnd->queue);
+
+	for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
+		struct bt_mesh_friend_seg *seg = &frnd->seg[i];
+
+		purge_buffers(&seg->queue);
+		seg->seg_count = 0U;
+	}
+
+	frnd->valid = 0;
+	frnd->established = 0;
+	frnd->pending_buf = 0;
+	frnd->fsn = 0;
+	frnd->queue_size = 0;
+	frnd->pending_req = 0;
+	memset(frnd->sub_list, 0, sizeof(frnd->sub_list));
+}
+
+void bt_mesh_friend_clear_net_idx(u16_t net_idx)
+{
+	int i;
+
+	BT_DBG("net_idx 0x%04x", net_idx);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (frnd->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (net_idx == BT_MESH_KEY_ANY || frnd->net_idx == net_idx) {
+			friend_clear(frnd);
+		}
+	}
+}
+
+void bt_mesh_friend_sec_update(u16_t net_idx)
+{
+	int i;
+
+	BT_DBG("net_idx 0x%04x", net_idx);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (frnd->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (net_idx == BT_MESH_KEY_ANY || frnd->net_idx == net_idx) {
+			frnd->sec_update = 1;
+		}
+	}
+}
+
+int bt_mesh_friend_clear(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_clear *msg = (void *)buf->om_data;
+	struct bt_mesh_friend *frnd;
+	u16_t lpn_addr, lpn_counter;
+	struct bt_mesh_net_tx tx = {
+		.sub  = rx->sub,
+		.ctx  = &rx->ctx,
+		.src  = bt_mesh_primary_addr(),
+		.xmit = bt_mesh_net_transmit_get(),
+	};
+	struct bt_mesh_ctl_friend_clear_confirm cfm;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Clear");
+		return -EINVAL;
+	}
+
+	lpn_addr = sys_be16_to_cpu(msg->lpn_addr);
+	lpn_counter = sys_be16_to_cpu(msg->lpn_counter);
+
+	BT_DBG("LPN addr 0x%04x counter 0x%04x", lpn_addr, lpn_counter);
+
+	frnd = bt_mesh_friend_find(rx->sub->net_idx, lpn_addr, false, false);
+	if (!frnd) {
+		BT_WARN("No matching LPN addr 0x%04x", lpn_addr);
+		return 0;
+	}
+
+	/* A Friend Clear message is considered valid if the result of the
+	 * subtraction of the value of the LPNCounter field of the Friend
+	 * Request message (the one that initiated the friendship) from the
+	 * value of the LPNCounter field of the Friend Clear message, modulo
+	 * 65536, is in the range 0 to 255 inclusive.
+	 */
+	if (lpn_counter - frnd->lpn_counter > 255) {
+		BT_WARN("LPN Counter out of range (old %u new %u)",
+			frnd->lpn_counter, lpn_counter);
+		return 0;
+	}
+
+	tx.ctx->send_ttl = BT_MESH_TTL_MAX;
+
+	cfm.lpn_addr    = msg->lpn_addr;
+	cfm.lpn_counter = msg->lpn_counter;
+
+	bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR_CFM, &cfm,
+			 sizeof(cfm), NULL, NULL, NULL);
+
+	friend_clear(frnd);
+
+	return 0;
+}
+
+static void friend_sub_add(struct bt_mesh_friend *frnd, u16_t addr)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(frnd->sub_list); i++) {
+		if (frnd->sub_list[i] == BT_MESH_ADDR_UNASSIGNED) {
+			frnd->sub_list[i] = addr;
+			return;
+		}
+	}
+
+	BT_WARN("No space in friend subscription list");
+}
+
+static void friend_sub_rem(struct bt_mesh_friend *frnd, u16_t addr)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(frnd->sub_list); i++) {
+		if (frnd->sub_list[i] == addr) {
+			frnd->sub_list[i] = BT_MESH_ADDR_UNASSIGNED;
+			return;
+		}
+	}
+}
+
+static struct os_mbuf *create_friend_pdu(struct bt_mesh_friend *frnd,
+					 struct friend_pdu_info *info,
+					 struct os_mbuf *sdu)
+{
+	struct os_mbuf *buf;
+
+	buf = bt_mesh_adv_create_from_pool(&friend_os_mbuf_pool, adv_alloc,
+					   BT_MESH_ADV_DATA,
+					   FRIEND_XMIT, K_NO_WAIT);
+	if (!buf) {
+		return NULL;
+	}
+
+	net_buf_add_u8(buf, (info->iv_index & 1) << 7); /* Will be reset in encryption */
+
+	if (info->ctl) {
+		net_buf_add_u8(buf, info->ttl | 0x80);
+	} else {
+		net_buf_add_u8(buf, info->ttl);
+	}
+
+	net_buf_add_mem(buf, info->seq, sizeof(info->seq));
+
+	net_buf_add_be16(buf, info->src);
+	net_buf_add_be16(buf, info->dst);
+
+	net_buf_add_mem(buf, sdu->om_data, sdu->om_len);
+
+	return buf;
+}
+
+struct unseg_app_sdu_meta {
+	struct bt_mesh_net_rx net;
+	const u8_t *key;
+	struct bt_mesh_subnet *subnet;
+	bool is_dev_key;
+	u8_t aid;
+	u8_t *ad;
+};
+
+static int unseg_app_sdu_unpack(struct bt_mesh_friend *frnd,
+				struct os_mbuf *buf,
+				struct unseg_app_sdu_meta *meta)
+{
+	u16_t app_idx = FRIEND_ADV(buf)->app_idx;
+	int err;
+
+	meta->subnet = bt_mesh_subnet_get(frnd->net_idx);
+	meta->is_dev_key = (app_idx == BT_MESH_KEY_DEV);
+	meta->is_dev_key = BT_MESH_IS_DEV_KEY(app_idx);
+	bt_mesh_net_header_parse(buf, &meta->net);
+	err = bt_mesh_app_key_get(meta->subnet, app_idx, meta->net.ctx.recv_dst,
+				  &meta->key, &meta->aid);
+	if (err) {
+		return err;
+	}
+
+	if (BT_MESH_ADDR_IS_VIRTUAL(meta->net.ctx.recv_dst)) {
+		meta->ad = bt_mesh_label_uuid_get(meta->net.ctx.recv_dst);
+		if (!meta->ad) {
+			return -ENOENT;
+		}
+	} else {
+		meta->ad = NULL;
+	}
+
+	return 0;
+}
+
+static int unseg_app_sdu_decrypt(struct bt_mesh_friend *frnd,
+				 struct os_mbuf *buf,
+				 const struct unseg_app_sdu_meta *meta)
+{
+	struct net_buf_simple_state state;
+	int err;
+
+	BT_DBG("");
+
+	net_buf_simple_save(buf, &state);
+	net_buf_simple_pull_mem(buf, 10);
+	buf->om_len -= 4;
+
+	err = bt_mesh_app_decrypt_in_place(meta->key, meta->is_dev_key,
+					   0, buf, meta->ad, meta->net.ctx.addr,
+					   meta->net.ctx.recv_dst, meta->net.seq,
+					   BT_MESH_NET_IVI_TX);
+
+	net_buf_simple_restore(buf, &state);
+	return err;
+}
+
+static int unseg_app_sdu_encrypt(struct bt_mesh_friend *frnd,
+				 struct os_mbuf *buf,
+				 const struct unseg_app_sdu_meta *meta)
+{
+	struct net_buf_simple_state state;
+	int err;
+
+	BT_DBG("");
+
+	net_buf_simple_save(buf, &state);
+	net_buf_simple_pull_mem(buf, 10);
+	buf->om_len -= 4;
+
+	err = bt_mesh_app_encrypt_in_place(meta->key, meta->is_dev_key, 0, buf,
+					   meta->ad, meta->net.ctx.addr,
+					   meta->net.ctx.recv_dst, bt_mesh.seq,
+					   BT_MESH_NET_IVI_TX);
+
+	net_buf_simple_restore(buf, &state);
+	return err;
+}
+
+static int unseg_app_sdu_prepare(struct bt_mesh_friend *frnd,
+				 struct os_mbuf *buf)
+{
+	struct unseg_app_sdu_meta meta;
+	int err;
+
+	BT_DBG("");
+
+	if (FRIEND_ADV(buf)->app_idx == BT_MESH_KEY_UNUSED) {
+		return 0;
+	}
+
+	err = unseg_app_sdu_unpack(frnd, buf, &meta);
+	if (err) {
+		return err;
+	}
+
+	/* No need to reencrypt the message if the sequence number is
+	 * unchanged.
+	 */
+	if (meta.net.seq == bt_mesh.seq) {
+		return 0;
+	}
+
+	err = unseg_app_sdu_decrypt(frnd, buf, &meta);
+	if (err) {
+		BT_WARN("Decryption failed! %d", err);
+		return err;
+	}
+
+	err = unseg_app_sdu_encrypt(frnd, buf, &meta);
+	if (err) {
+		BT_WARN("Re-encryption failed! %d", err);
+	}
+
+	return err;
+}
+
+static int encrypt_friend_pdu(struct bt_mesh_friend *frnd, struct os_mbuf *buf,
+			      bool master_cred)
+{
+	struct bt_mesh_subnet *sub = bt_mesh_subnet_get(frnd->net_idx);
+	const u8_t *enc, *priv;
+	u32_t iv_index;
+	u16_t src;
+	u8_t nid;
+	int err;
+
+	if (master_cred) {
+		enc = sub->keys[sub->kr_flag].enc;
+		priv = sub->keys[sub->kr_flag].privacy;
+		nid = sub->keys[sub->kr_flag].nid;
+	} else {
+		if (friend_cred_get(sub, frnd->lpn, &nid, &enc, &priv)) {
+			BT_ERR("friend_cred_get failed");
+			return -ENOENT;
+		}
+	}
+
+	src = sys_get_be16(&buf->om_data[5]);
+
+	if (bt_mesh_elem_find(src)) {
+		u32_t seq;
+
+		if (FRIEND_ADV(buf)->app_idx != BT_MESH_KEY_UNUSED) {
+			err = unseg_app_sdu_prepare(frnd, buf);
+			if (err) {
+				return err;
+			}
+		}
+
+		seq = bt_mesh_next_seq();
+		buf->om_data[2] = seq >> 16;
+		buf->om_data[3] = seq >> 8;
+		buf->om_data[4] = seq;
+
+		iv_index = BT_MESH_NET_IVI_TX;
+		FRIEND_ADV(buf)->app_idx = BT_MESH_KEY_UNUSED;
+	} else {
+		u8_t ivi = (buf->om_data[0] >> 7);
+		iv_index = (bt_mesh.iv_index - ((bt_mesh.iv_index & 1) != ivi));
+	}
+
+	buf->om_data[0] = (nid | (iv_index & 1) << 7);
+
+	if (bt_mesh_net_encrypt(enc, buf, iv_index, false)) {
+		BT_ERR("Encrypting failed");
+		return -EINVAL;
+	}
+
+	if (bt_mesh_net_obfuscate(buf->om_data, iv_index, priv)) {
+		BT_ERR("Obfuscating failed");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static struct os_mbuf *encode_friend_ctl(struct bt_mesh_friend *frnd,
+					 u8_t ctl_op,
+					 struct os_mbuf *sdu)
+{
+	struct friend_pdu_info info;
+
+	BT_DBG("LPN 0x%04x", frnd->lpn);
+
+	net_buf_simple_push_u8(sdu, TRANS_CTL_HDR(ctl_op, 0));
+
+	info.src = bt_mesh_primary_addr();
+	info.dst = frnd->lpn;
+
+	info.ctl = 1;
+	info.ttl = 0;
+
+	memset(info.seq, 0, sizeof(info.seq));
+
+	info.iv_index = BT_MESH_NET_IVI_TX;
+
+	return create_friend_pdu(frnd, &info, sdu);
+}
+
+static struct os_mbuf *encode_update(struct bt_mesh_friend *frnd, u8_t md)
+{
+	struct bt_mesh_ctl_friend_update *upd;
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(1 + sizeof(*upd));
+	struct bt_mesh_subnet *sub = bt_mesh_subnet_get(frnd->net_idx);
+	struct os_mbuf *buf;
+
+	__ASSERT_NO_MSG(sub != NULL);
+
+	BT_DBG("lpn 0x%04x md 0x%02x", frnd->lpn, md);
+
+	net_buf_simple_init(sdu, 1);
+
+	upd = net_buf_simple_add(sdu, sizeof(*upd));
+	upd->flags = bt_mesh_net_flags(sub);
+	upd->iv_index = sys_cpu_to_be32(bt_mesh.iv_index);
+	upd->md = md;
+
+	buf = encode_friend_ctl(frnd, TRANS_CTL_OP_FRIEND_UPDATE, sdu);
+
+	os_mbuf_free_chain(sdu);
+	return buf;
+}
+
+static void enqueue_sub_cfm(struct bt_mesh_friend *frnd, u8_t xact)
+{
+	struct bt_mesh_ctl_friend_sub_confirm *cfm;
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(1 + sizeof(*cfm));
+	struct os_mbuf *buf;
+
+	BT_DBG("lpn 0x%04x xact 0x%02x", frnd->lpn, xact);
+
+	net_buf_simple_init(sdu, 1);
+
+	cfm = net_buf_simple_add(sdu, sizeof(*cfm));
+	cfm->xact = xact;
+
+	buf = encode_friend_ctl(frnd, TRANS_CTL_OP_FRIEND_SUB_CFM, sdu);
+	if (!buf) {
+		BT_ERR("Unable to encode Subscription List Confirmation");
+		goto done;
+	}
+
+	if (encrypt_friend_pdu(frnd, buf, false)) {
+		return;
+	}
+
+	if (frnd->last) {
+		BT_DBG("Discarding last PDU");
+		net_buf_unref(frnd->last);
+	}
+
+	frnd->last = buf;
+	frnd->send_last = 1;
+
+done:
+	os_mbuf_free_chain(sdu);
+}
+
+static void friend_recv_delay(struct bt_mesh_friend *frnd)
+{
+	frnd->pending_req = 1;
+	k_delayed_work_submit(&frnd->timer, recv_delay(frnd));
+	BT_DBG("Waiting RecvDelay of %d ms", (int) recv_delay(frnd));
+}
+
+int bt_mesh_friend_sub_add(struct bt_mesh_net_rx *rx,
+			   struct os_mbuf *buf)
+{
+	struct bt_mesh_friend *frnd;
+	u8_t xact;
+
+	if (buf->om_len < BT_MESH_FRIEND_SUB_MIN_LEN) {
+		BT_WARN("Too short Friend Subscription Add");
+		return -EINVAL;
+	}
+
+	frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, true);
+	if (!frnd) {
+		BT_WARN("No matching LPN addr 0x%04x", rx->ctx.addr);
+		return 0;
+	}
+
+	if (frnd->pending_buf) {
+		BT_WARN("Previous buffer not yet sent!");
+		return 0;
+	}
+
+	friend_recv_delay(frnd);
+
+	xact = net_buf_simple_pull_u8(buf);
+
+	while (buf->om_len >= 2) {
+		friend_sub_add(frnd, net_buf_simple_pull_be16(buf));
+	}
+
+	enqueue_sub_cfm(frnd, xact);
+
+	return 0;
+}
+
+int bt_mesh_friend_sub_rem(struct bt_mesh_net_rx *rx,
+			   struct os_mbuf *buf)
+{
+	struct bt_mesh_friend *frnd;
+	u8_t xact;
+
+	if (buf->om_len < BT_MESH_FRIEND_SUB_MIN_LEN) {
+		BT_WARN("Too short Friend Subscription Remove");
+		return -EINVAL;
+	}
+
+	frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, true);
+	if (!frnd) {
+		BT_WARN("No matching LPN addr 0x%04x", rx->ctx.addr);
+		return 0;
+	}
+
+	if (frnd->pending_buf) {
+		BT_WARN("Previous buffer not yet sent!");
+		return 0;
+	}
+
+	friend_recv_delay(frnd);
+
+	xact = net_buf_simple_pull_u8(buf);
+
+	while (buf->om_len >= 2) {
+		friend_sub_rem(frnd, net_buf_simple_pull_be16(buf));
+	}
+
+	enqueue_sub_cfm(frnd, xact);
+
+	return 0;
+}
+
+static void enqueue_buf(struct bt_mesh_friend *frnd, struct os_mbuf *buf)
+{
+	net_buf_slist_put(&frnd->queue, buf);
+	frnd->queue_size++;
+}
+
+static void enqueue_update(struct bt_mesh_friend *frnd, u8_t md)
+{
+	struct os_mbuf *buf;
+
+	buf = encode_update(frnd, md);
+	if (!buf) {
+		BT_ERR("Unable to encode Friend Update");
+		return;
+	}
+
+	frnd->sec_update = 0;
+	enqueue_buf(frnd, buf);
+}
+
+int bt_mesh_friend_poll(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_poll *msg = (void *)buf->om_data;
+	struct bt_mesh_friend *frnd;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Poll");
+		return -EINVAL;
+	}
+
+	frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, false);
+	if (!frnd) {
+		BT_WARN("No matching LPN addr 0x%04x", rx->ctx.addr);
+		return 0;
+	}
+
+	if (msg->fsn & ~1) {
+		BT_WARN("Prohibited (non-zero) padding bits");
+		return -EINVAL;
+	}
+
+	if (frnd->pending_buf) {
+		BT_WARN("Previous buffer not yet sent");
+		return 0;
+	}
+
+	BT_DBG("msg->fsn %u frnd->fsn %u", (msg->fsn & 1), frnd->fsn);
+
+	friend_recv_delay(frnd);
+
+	if (!frnd->established) {
+		BT_DBG("Friendship established with 0x%04x", frnd->lpn);
+		frnd->established = 1;
+	}
+
+	if (msg->fsn == frnd->fsn && frnd->last) {
+		BT_DBG("Re-sending last PDU");
+		frnd->send_last = 1;
+	} else {
+		if (frnd->last) {
+			net_buf_unref(frnd->last);
+			frnd->last = NULL;
+		}
+
+		frnd->fsn = msg->fsn;
+
+		if (net_buf_slist_is_empty(&frnd->queue)) {
+			enqueue_update(frnd, 0);
+			BT_DBG("Enqueued Friend Update to empty queue");
+		}
+	}
+
+	return 0;
+}
+
+static struct bt_mesh_friend *find_clear(u16_t prev_friend)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (frnd->clear.frnd == prev_friend) {
+			return frnd;
+		}
+	}
+
+	return NULL;
+}
+
+static void friend_clear_sent(int err, void *user_data)
+{
+	struct bt_mesh_friend *frnd = user_data;
+
+	k_delayed_work_submit(&frnd->clear.timer,
+			      K_SECONDS(frnd->clear.repeat_sec));
+	frnd->clear.repeat_sec *= 2;
+}
+
+static const struct bt_mesh_send_cb clear_sent_cb = {
+	.end = friend_clear_sent,
+};
+
+static void send_friend_clear(struct bt_mesh_friend *frnd)
+{
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx  = frnd->net_idx,
+		.app_idx  = BT_MESH_KEY_UNUSED,
+		.addr     = frnd->clear.frnd,
+		.send_ttl = BT_MESH_TTL_MAX,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub  = &bt_mesh.sub[0],
+		.ctx  = &ctx,
+		.src  = bt_mesh_primary_addr(),
+		.xmit = bt_mesh_net_transmit_get(),
+	};
+	struct bt_mesh_ctl_friend_clear req = {
+		.lpn_addr    = sys_cpu_to_be16(frnd->lpn),
+		.lpn_counter = sys_cpu_to_be16(frnd->lpn_counter),
+	};
+
+	BT_DBG("");
+
+	bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR, &req,
+			 sizeof(req), NULL, &clear_sent_cb, frnd);
+}
+
+static void clear_timeout(struct ble_npl_event *work)
+{
+	struct bt_mesh_friend *frnd = ble_npl_event_get_arg(work);
+	u32_t duration;
+
+	BT_DBG("LPN 0x%04x (old) Friend 0x%04x", frnd->lpn, frnd->clear.frnd);
+
+	duration = k_uptime_get_32() - frnd->clear.start;
+	if (duration > 2 * frnd->poll_to) {
+		BT_DBG("Clear Procedure timer expired");
+		frnd->clear.frnd = BT_MESH_ADDR_UNASSIGNED;
+		return;
+	}
+
+	send_friend_clear(frnd);
+}
+
+static void clear_procedure_start(struct bt_mesh_friend *frnd)
+{
+	BT_DBG("LPN 0x%04x (old) Friend 0x%04x", frnd->lpn, frnd->clear.frnd);
+
+	frnd->clear.start = k_uptime_get_32();
+	frnd->clear.repeat_sec = 1U;
+
+	send_friend_clear(frnd);
+}
+
+int bt_mesh_friend_clear_cfm(struct bt_mesh_net_rx *rx,
+			     struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_clear_confirm *msg = (void *)buf->om_data;
+	struct bt_mesh_friend *frnd;
+	u16_t lpn_addr, lpn_counter;
+
+	BT_DBG("");
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Clear Confirm");
+		return -EINVAL;
+	}
+
+	frnd = find_clear(rx->ctx.addr);
+	if (!frnd) {
+		BT_WARN("No pending clear procedure for 0x%02x", rx->ctx.addr);
+		return 0;
+	}
+
+	lpn_addr = sys_be16_to_cpu(msg->lpn_addr);
+	if (lpn_addr != frnd->lpn) {
+		BT_WARN("LPN address mismatch (0x%04x != 0x%04x)",
+			lpn_addr, frnd->lpn);
+		return 0;
+	}
+
+	lpn_counter = sys_be16_to_cpu(msg->lpn_counter);
+	if (lpn_counter != frnd->lpn_counter) {
+		BT_WARN("LPN counter mismatch (0x%04x != 0x%04x)",
+			lpn_counter, frnd->lpn_counter);
+		return 0;
+	}
+
+	k_delayed_work_cancel(&frnd->clear.timer);
+	frnd->clear.frnd = BT_MESH_ADDR_UNASSIGNED;
+
+	return 0;
+}
+
+static void enqueue_offer(struct bt_mesh_friend *frnd, s8_t rssi)
+{
+	struct bt_mesh_ctl_friend_offer *off;
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(1 + sizeof(*off));
+	struct os_mbuf *buf;
+
+	BT_DBG("");
+
+	net_buf_simple_init(sdu, 1);
+
+	off = net_buf_simple_add(sdu, sizeof(*off));
+
+	off->recv_win = CONFIG_BT_MESH_FRIEND_RECV_WIN,
+	off->queue_size = CONFIG_BT_MESH_FRIEND_QUEUE_SIZE,
+	off->sub_list_size = ARRAY_SIZE(frnd->sub_list),
+	off->rssi = rssi,
+	off->frnd_counter = sys_cpu_to_be16(frnd->counter);
+
+	buf = encode_friend_ctl(frnd, TRANS_CTL_OP_FRIEND_OFFER, sdu);
+	if (!buf) {
+		BT_ERR("Unable to encode Friend Offer");
+		goto done;
+	}
+
+	if (encrypt_friend_pdu(frnd, buf, true)) {
+		return;
+	}
+
+	frnd->counter++;
+
+	if (frnd->last) {
+		net_buf_unref(frnd->last);
+	}
+
+	frnd->last = buf;
+	frnd->send_last = 1;
+
+done:
+	os_mbuf_free_chain(sdu);
+}
+
+#define RECV_WIN                  CONFIG_BT_MESH_FRIEND_RECV_WIN
+#define RSSI_FACT(crit)           (((crit) >> 5) & (u8_t)BIT_MASK(2))
+#define RECV_WIN_FACT(crit)       (((crit) >> 3) & (u8_t)BIT_MASK(2))
+#define MIN_QUEUE_SIZE_LOG(crit)  ((crit) & (u8_t)BIT_MASK(3))
+#define MIN_QUEUE_SIZE(crit)      ((u32_t)BIT(MIN_QUEUE_SIZE_LOG(crit)))
+
+static s32_t offer_delay(struct bt_mesh_friend *frnd, s8_t rssi, u8_t crit)
+{
+	/* Scaling factors. The actual values are 1, 1.5, 2 & 2.5, but we
+	 * want to avoid floating-point arithmetic.
+	 */
+	static const u8_t fact[] = { 10, 15, 20, 25 };
+	s32_t delay;
+
+	BT_DBG("ReceiveWindowFactor %u ReceiveWindow %u RSSIFactor %u RSSI %d",
+	       fact[RECV_WIN_FACT(crit)], RECV_WIN,
+	       fact[RSSI_FACT(crit)], rssi);
+
+	/* Delay = ReceiveWindowFactor * ReceiveWindow - RSSIFactor * RSSI */
+	delay = (s32_t)fact[RECV_WIN_FACT(crit)] * RECV_WIN;
+	delay -= (s32_t)fact[RSSI_FACT(crit)] * rssi;
+	delay /= 10;
+
+	BT_DBG("Local Delay calculated as %d ms", (int) delay);
+
+	if (delay < 100) {
+		return K_MSEC(100);
+	}
+
+	return K_MSEC(delay);
+}
+
+int bt_mesh_friend_req(struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_req *msg = (void *)buf->om_data;
+	struct bt_mesh_friend *frnd = NULL;
+	u32_t poll_to;
+	int i;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Request");
+		return -EINVAL;
+	}
+
+	if (msg->recv_delay <= 0x09) {
+		BT_WARN("Prohibited ReceiveDelay (0x%02x)", msg->recv_delay);
+		return -EINVAL;
+	}
+
+	poll_to = (((u32_t)msg->poll_to[0] << 16) |
+		   ((u32_t)msg->poll_to[1] << 8) |
+		   ((u32_t)msg->poll_to[2]));
+
+	if (poll_to <= 0x000009 || poll_to >= 0x34bc00) {
+		BT_WARN("Prohibited PollTimeout (0x%06x)", (unsigned) poll_to);
+		return -EINVAL;
+	}
+
+	if (msg->num_elem == 0x00) {
+		BT_WARN("Prohibited NumElements value (0x00)");
+		return -EINVAL;
+	}
+
+	if (!BT_MESH_ADDR_IS_UNICAST(rx->ctx.addr + msg->num_elem - 1)) {
+		BT_WARN("LPN elements stretch outside of unicast range");
+		return -EINVAL;
+	}
+
+	if (!MIN_QUEUE_SIZE_LOG(msg->criteria)) {
+		BT_WARN("Prohibited Minimum Queue Size in Friend Request");
+		return -EINVAL;
+	}
+
+	if (CONFIG_BT_MESH_FRIEND_QUEUE_SIZE < MIN_QUEUE_SIZE(msg->criteria)) {
+		BT_WARN("We have a too small Friend Queue size (%u < %u)",
+			CONFIG_BT_MESH_FRIEND_QUEUE_SIZE,
+			(unsigned) MIN_QUEUE_SIZE(msg->criteria));
+		return 0;
+	}
+
+	frnd = bt_mesh_friend_find(rx->sub->net_idx, rx->ctx.addr, true, false);
+	if (frnd) {
+		BT_WARN("Existing LPN re-requesting Friendship");
+		friend_clear(frnd);
+		goto init_friend;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		if (!bt_mesh.frnd[i].valid) {
+			frnd = &bt_mesh.frnd[i];
+			frnd->valid = 1;
+			break;
+		}
+	}
+
+	if (!frnd) {
+		BT_WARN("No free Friend contexts for new LPN");
+		return -ENOMEM;
+	}
+
+init_friend:
+	frnd->lpn = rx->ctx.addr;
+	frnd->num_elem = msg->num_elem;
+	frnd->net_idx = rx->sub->net_idx;
+	frnd->recv_delay = msg->recv_delay;
+	frnd->poll_to = poll_to * 100;
+	frnd->lpn_counter = sys_be16_to_cpu(msg->lpn_counter);
+	frnd->clear.frnd = sys_be16_to_cpu(msg->prev_addr);
+
+	BT_DBG("LPN 0x%04x rssi %d recv_delay %u poll_to %ums",
+	       frnd->lpn, rx->ctx.recv_rssi, frnd->recv_delay,
+	       (unsigned) frnd->poll_to);
+
+	if (BT_MESH_ADDR_IS_UNICAST(frnd->clear.frnd) &&
+	    !bt_mesh_elem_find(frnd->clear.frnd)) {
+		clear_procedure_start(frnd);
+	}
+
+	k_delayed_work_submit(&frnd->timer,
+			      offer_delay(frnd, rx->ctx.recv_rssi,
+					  msg->criteria));
+
+	friend_cred_create(rx->sub, frnd->lpn, frnd->lpn_counter,
+			   frnd->counter);
+
+	enqueue_offer(frnd, rx->ctx.recv_rssi);
+
+	return 0;
+}
+
+static bool is_seg(struct bt_mesh_friend_seg *seg, u16_t src, u16_t seq_zero)
+{
+	struct os_mbuf *buf = (void *)net_buf_slist_peek_head(&seg->queue);
+	struct net_buf_simple_state state;
+	u16_t buf_seq_zero;
+	u16_t buf_src;
+
+	if (!buf) {
+		return false;
+	}
+
+	net_buf_simple_save(buf, &state);
+	net_buf_skip(buf, 5);   /* skip IVI, NID, CTL, TTL, SEQ */
+	buf_src = net_buf_pull_be16(buf);
+	net_buf_skip(buf, 3);   /* skip DST, OP/AID */
+	buf_seq_zero = ((net_buf_pull_be16(buf) >> 2) & TRANS_SEQ_ZERO_MASK);
+	net_buf_simple_restore(buf, &state);
+
+	return ((src == buf_src) && (seq_zero == buf_seq_zero));
+}
+
+static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd,
+					  u16_t src, u16_t seq_zero,
+					  u8_t seg_count)
+{
+	struct bt_mesh_friend_seg *unassigned = NULL;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
+		struct bt_mesh_friend_seg *seg = &frnd->seg[i];
+
+		if (is_seg(seg, src, seq_zero)) {
+			return seg;
+		}
+
+		if (!unassigned && !net_buf_slist_peek_head(&seg->queue)) {
+			unassigned = seg;
+		}
+	}
+
+	if (unassigned) {
+		unassigned->seg_count = seg_count;
+	}
+
+	return unassigned;
+}
+
+static void enqueue_friend_pdu(struct bt_mesh_friend *frnd,
+			       enum bt_mesh_friend_pdu_type type,
+			       u16_t src, u8_t seg_count,
+			       struct os_mbuf *buf)
+{
+	struct bt_mesh_friend_seg *seg;
+
+	BT_DBG("type %u", type);
+
+	if (type == BT_MESH_FRIEND_PDU_SINGLE) {
+		if (frnd->sec_update) {
+			enqueue_update(frnd, 1);
+		}
+
+		enqueue_buf(frnd, buf);
+		return;
+	}
+
+	u16_t seq_zero = (((buf->om_data[10] << 8 | buf->om_data[11]) >> 2) & TRANS_SEQ_ZERO_MASK);
+
+	seg = get_seg(frnd, src, seq_zero, seg_count);
+	if (!seg) {
+		BT_ERR("No free friend segment RX contexts for 0x%04x", src);
+		net_buf_unref(buf);
+		return;
+	}
+
+	net_buf_slist_put(&seg->queue, buf);
+
+	if (type == BT_MESH_FRIEND_PDU_COMPLETE) {
+		if (frnd->sec_update) {
+			enqueue_update(frnd, 1);
+		}
+
+		net_buf_slist_merge_slist(&frnd->queue, &seg->queue);
+
+		frnd->queue_size += seg->seg_count;
+		seg->seg_count = 0U;
+	} else {
+		/* Mark the buffer as having more to come after it */
+		BT_MESH_ADV(buf)->flags |= NET_BUF_FRAGS;
+	}
+}
+
+static void buf_send_start(u16_t duration, int err, void *user_data)
+{
+	struct bt_mesh_friend *frnd = user_data;
+
+	BT_DBG("err %d", err);
+
+	frnd->pending_buf = 0;
+
+	/* Friend Offer doesn't follow the re-sending semantics */
+	if (!frnd->established) {
+		net_buf_unref(frnd->last);
+		frnd->last = NULL;
+	}
+}
+
+static void buf_send_end(int err, void *user_data)
+{
+	struct bt_mesh_friend *frnd = user_data;
+
+	BT_DBG("err %d", err);
+
+	if (frnd->pending_req) {
+		BT_WARN("Another request before previous completed sending");
+		return;
+	}
+
+	if (frnd->established) {
+		k_delayed_work_submit(&frnd->timer, frnd->poll_to);
+		BT_DBG("Waiting %u ms for next poll",
+		       (unsigned) frnd->poll_to);
+	} else {
+		/* Friend offer timeout is 1 second */
+		k_delayed_work_submit(&frnd->timer, K_SECONDS(1));
+		BT_DBG("Waiting for first poll");
+	}
+}
+
+static void friend_timeout(struct ble_npl_event *work)
+{
+	struct bt_mesh_friend *frnd = ble_npl_event_get_arg(work);
+	static const struct bt_mesh_send_cb buf_sent_cb = {
+		.start = buf_send_start,
+		.end = buf_send_end,
+	};
+
+	__ASSERT_NO_MSG(frnd->pending_buf == 0);
+
+	BT_DBG("lpn 0x%04x send_last %u last %p", frnd->lpn,
+	       frnd->send_last, frnd->last);
+
+	if (frnd->send_last && frnd->last) {
+		BT_DBG("Sending frnd->last %p", frnd->last);
+		frnd->send_last = 0;
+		goto send_last;
+	}
+
+	if (frnd->established && !frnd->pending_req) {
+		BT_WARN("Friendship lost with 0x%04x", frnd->lpn);
+		friend_clear(frnd);
+		return;
+	}
+
+	frnd->last = (void *)net_buf_slist_get(&frnd->queue);
+	if (!frnd->last) {
+		BT_WARN("Friendship not established with 0x%04x",
+			frnd->lpn);
+		friend_clear(frnd);
+		return;
+	}
+
+	if (encrypt_friend_pdu(frnd, frnd->last, false)) {
+		return;
+	}
+
+	/* Clear the flag we use for segment tracking */
+	BT_MESH_ADV(frnd->last)->flags &= ~NET_BUF_FRAGS;
+
+	BT_DBG("Sending buf %p from Friend Queue of LPN 0x%04x",
+	       frnd->last, frnd->lpn);
+	frnd->queue_size--;
+
+send_last:
+	frnd->pending_req = 0;
+	frnd->pending_buf = 1;
+	bt_mesh_adv_send(frnd->last, &buf_sent_cb, frnd);
+}
+
+int bt_mesh_friend_init(void)
+{
+	int rc;
+	int i;
+
+	rc = os_mempool_init(&friend_buf_mempool, FRIEND_BUF_COUNT,
+			BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE,
+			friend_buf_mem, "friend_buf_pool");
+	assert(rc == 0);
+
+	rc = os_mbuf_pool_init(&friend_os_mbuf_pool, &friend_buf_mempool,
+			BT_MESH_ADV_DATA_SIZE + BT_MESH_MBUF_HEADER_SIZE,
+			FRIEND_BUF_COUNT);
+	assert(rc == 0);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+		int j;
+
+		frnd->net_idx = BT_MESH_KEY_UNUSED;
+
+		net_buf_slist_init(&frnd->queue);
+
+		k_delayed_work_init(&frnd->timer, friend_timeout);
+		k_delayed_work_add_arg(&frnd->timer, frnd);
+		k_delayed_work_init(&frnd->clear.timer, clear_timeout);
+		k_delayed_work_add_arg(&frnd->clear.timer, frnd);
+
+		for (j = 0; j < ARRAY_SIZE(frnd->seg); j++) {
+			net_buf_slist_init(&frnd->seg[j].queue);
+		}
+	}
+
+	return 0;
+}
+
+static bool is_segack(struct os_mbuf *buf, u64_t *seqauth, u16_t src)
+{
+	struct net_buf_simple_state state;
+	bool found = false;
+
+	if (buf->om_len != 16) {
+		return false;
+	}
+
+	net_buf_simple_save(buf, &state);
+
+	net_buf_skip(buf, 1); /* skip IVI, NID */
+
+	if (!(net_buf_pull_u8(buf) >> 7)) {
+		goto end;
+	}
+
+	net_buf_pull(buf, 3); /* skip SEQNUM */
+
+	if (src != net_buf_pull_be16(buf)) {
+		goto end;
+	}
+
+	net_buf_skip(buf, 2); /* skip dst */
+
+	if (TRANS_CTL_OP((u8_t *) net_buf_pull_mem(buf, 1)) != TRANS_CTL_OP_ACK) {
+		goto end;
+	}
+
+	found = ((net_buf_pull_be16(buf) >> 2) & TRANS_SEQ_ZERO_MASK) ==
+		(*seqauth & TRANS_SEQ_ZERO_MASK);
+end:
+	net_buf_simple_restore(buf, &state);
+	return found;
+}
+
+static void friend_purge_old_ack(struct bt_mesh_friend *frnd, u64_t *seq_auth,
+				 u16_t src)
+{
+	struct os_mbuf *cur, *prev = NULL;
+
+	BT_DBG("SeqAuth %llx src 0x%04x", *seq_auth, src);
+
+	for (cur = net_buf_slist_peek_head(&frnd->queue);
+	     cur != NULL; prev = cur, cur = net_buf_slist_peek_next(cur)) {
+		struct os_mbuf *buf = (void *)cur;
+
+		if (is_segack(buf, seq_auth, src)) {
+			BT_DBG("Removing old ack from Friend Queue");
+
+			net_buf_slist_remove(&frnd->queue, prev, cur);
+			frnd->queue_size--;
+
+			net_buf_unref(buf);
+			break;
+		}
+	}
+}
+
+static void friend_lpn_enqueue_rx(struct bt_mesh_friend *frnd,
+				  struct bt_mesh_net_rx *rx,
+				  enum bt_mesh_friend_pdu_type type,
+				  u64_t *seq_auth, u8_t seg_count,
+				  struct os_mbuf *sbuf)
+{
+	struct friend_pdu_info info;
+	struct os_mbuf *buf;
+
+	/* Because of network loopback, tx packets will also be passed into
+	 * this rx function. These packets have already been added to the
+	 * queue, and should be ignored.
+	 */
+	if (bt_mesh_elem_find(rx->ctx.addr)) {
+		return;
+	}
+
+	BT_DBG("LPN 0x%04x queue_size %u", frnd->lpn,
+	       (unsigned) frnd->queue_size);
+
+	if (type == BT_MESH_FRIEND_PDU_SINGLE && seq_auth) {
+		friend_purge_old_ack(frnd, seq_auth, rx->ctx.addr);
+	}
+
+	info.src = rx->ctx.addr;
+	info.dst = rx->ctx.recv_dst;
+
+	if (rx->net_if == BT_MESH_NET_IF_LOCAL) {
+		info.ttl = rx->ctx.recv_ttl;
+	} else {
+		info.ttl = rx->ctx.recv_ttl - 1;
+	}
+
+	info.ctl = rx->ctl;
+
+	info.seq[0] = (rx->seq >> 16);
+	info.seq[1] = (rx->seq >> 8);
+	info.seq[2] = rx->seq;
+
+	info.iv_index = BT_MESH_NET_IVI_RX(rx);
+
+	buf = create_friend_pdu(frnd, &info, sbuf);
+	if (!buf) {
+		BT_ERR("Failed to encode Friend buffer");
+		return;
+	}
+
+	enqueue_friend_pdu(frnd, type, info.src, seg_count, buf);
+
+	BT_DBG("Queued message for LPN 0x%04x, queue_size %u",
+	       frnd->lpn, (unsigned) frnd->queue_size);
+}
+
+static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd,
+				  struct bt_mesh_net_tx *tx,
+				  enum bt_mesh_friend_pdu_type type,
+				  u64_t *seq_auth, u8_t seg_count,
+				  struct os_mbuf *sbuf)
+{
+	struct friend_pdu_info info;
+	struct os_mbuf *buf;
+
+	BT_DBG("LPN 0x%04x", frnd->lpn);
+
+	if (type == BT_MESH_FRIEND_PDU_SINGLE && seq_auth) {
+		friend_purge_old_ack(frnd, seq_auth, tx->src);
+	}
+
+	info.src = tx->src;
+	info.dst = tx->ctx->addr;
+
+	info.ttl = tx->ctx->send_ttl;
+	info.ctl = (tx->ctx->app_idx == BT_MESH_KEY_UNUSED);
+
+	info.seq[0] = (bt_mesh.seq >> 16);
+	info.seq[1] = (bt_mesh.seq >> 8);
+	info.seq[2] = bt_mesh.seq;
+
+	info.iv_index = BT_MESH_NET_IVI_TX;
+
+	buf = create_friend_pdu(frnd, &info, sbuf);
+	if (!buf) {
+		BT_ERR("Failed to encode Friend buffer");
+		return;
+	}
+
+	if (type == BT_MESH_FRIEND_PDU_SINGLE && !info.ctl) {
+		/* Unsegmented application packets may be reencrypted later,
+		 * as they depend on the the sequence number being the same
+		 * when encrypting in transport and network.
+		 */
+		FRIEND_ADV(buf)->app_idx = tx->ctx->app_idx;
+	}
+
+	enqueue_friend_pdu(frnd, type, info.src, seg_count, buf);
+
+	BT_DBG("Queued message for LPN 0x%04x", frnd->lpn);
+}
+
+static bool friend_lpn_matches(struct bt_mesh_friend *frnd, u16_t net_idx,
+			       u16_t addr)
+{
+	int i;
+
+	if (!frnd->established) {
+		return false;
+	}
+
+	if (net_idx != frnd->net_idx) {
+		return false;
+	}
+
+	if (BT_MESH_ADDR_IS_UNICAST(addr)) {
+		return is_lpn_unicast(frnd, addr);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(frnd->sub_list); i++) {
+		if (frnd->sub_list[i] == addr) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool bt_mesh_friend_match(u16_t net_idx, u16_t addr)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (friend_lpn_matches(frnd, net_idx, addr)) {
+			BT_DBG("LPN 0x%04x matched address 0x%04x",
+			       frnd->lpn, addr);
+			return true;
+		}
+	}
+
+	BT_DBG("No matching LPN for address 0x%04x", addr);
+
+	return false;
+}
+
+static bool friend_queue_has_space(struct bt_mesh_friend *frnd, u16_t addr,
+				   u64_t *seq_auth, u8_t seg_count)
+{
+	u32_t total = 0;
+	int i;
+
+	if (seg_count > CONFIG_BT_MESH_FRIEND_QUEUE_SIZE) {
+		return false;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
+		struct bt_mesh_friend_seg *seg = &frnd->seg[i];
+
+		if (seq_auth && is_seg(seg, addr, *seq_auth & TRANS_SEQ_ZERO_MASK)) {
+			/* If there's a segment queue for this message then the
+			 * space verification has already happened.
+			 */
+			return true;
+		}
+
+		total += seg->seg_count;
+	}
+
+	/* If currently pending segments combined with this segmented message
+	 * are more than the Friend Queue Size, then there's no space. This
+	 * is because we don't have a mechanism of aborting already pending
+	 * segmented messages to free up buffers.
+	 */
+	return (CONFIG_BT_MESH_FRIEND_QUEUE_SIZE - total) > seg_count;
+}
+
+bool bt_mesh_friend_queue_has_space(u16_t net_idx, u16_t src, u16_t dst,
+				    u64_t *seq_auth, u8_t seg_count)
+{
+	bool someone_has_space = false, friend_match = false;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (!friend_lpn_matches(frnd, net_idx, dst)) {
+			continue;
+		}
+
+		friend_match = true;
+
+		if (friend_queue_has_space(frnd, src, seq_auth, seg_count)) {
+			someone_has_space = true;
+		}
+	}
+
+	/* If there were no matched LPNs treat this as success, so the
+	 * transport layer can continue its work.
+	 */
+	if (!friend_match) {
+		return true;
+	}
+
+	/* From the transport layers perspective it's good enough that at
+	 * least one Friend Queue has space. If there were multiple Friend
+	 * matches then the destination must be a group address, in which
+	 * case e.g. segment acks are not sent.
+	 */
+	return someone_has_space;
+}
+
+static bool friend_queue_prepare_space(struct bt_mesh_friend *frnd, u16_t addr,
+				       u64_t *seq_auth, u8_t seg_count)
+{
+	bool pending_segments;
+	u8_t avail_space;
+
+	if (!friend_queue_has_space(frnd, addr, seq_auth, seg_count)) {
+		return false;
+	}
+
+	avail_space = CONFIG_BT_MESH_FRIEND_QUEUE_SIZE - frnd->queue_size;
+	pending_segments = false;
+
+	while (pending_segments || avail_space < seg_count) {
+		struct os_mbuf *buf = (void *)net_buf_slist_get(&frnd->queue);
+
+		if (!buf) {
+			BT_ERR("Unable to free up enough buffers");
+			return false;
+		}
+
+		frnd->queue_size--;
+		avail_space++;
+
+		pending_segments = (BT_MESH_ADV(buf)->flags & NET_BUF_FRAGS);
+		BT_DBG("PENDING SEGMENTS %d", pending_segments);
+
+		/* Make sure old slist entry state doesn't remain */
+		BT_MESH_ADV(buf)->flags &= ~NET_BUF_FRAGS;
+
+		net_buf_unref(buf);
+	}
+
+	return true;
+}
+
+void bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx *rx,
+			       enum bt_mesh_friend_pdu_type type,
+			       u64_t *seq_auth, u8_t seg_count,
+			       struct os_mbuf *sbuf)
+{
+	int i;
+
+	if (!rx->friend_match ||
+	    (rx->ctx.recv_ttl <= 1 && rx->net_if != BT_MESH_NET_IF_LOCAL) ||
+	    bt_mesh_friend_get() != BT_MESH_FRIEND_ENABLED) {
+		return;
+	}
+
+	BT_DBG("recv_ttl %u net_idx 0x%04x src 0x%04x dst 0x%04x",
+	       rx->ctx.recv_ttl, rx->sub->net_idx, rx->ctx.addr,
+	       rx->ctx.recv_dst);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (!friend_lpn_matches(frnd, rx->sub->net_idx,
+					rx->ctx.recv_dst)) {
+			continue;
+		}
+
+		if (!friend_queue_prepare_space(frnd, rx->ctx.addr, seq_auth,
+						seg_count)) {
+			continue;
+		}
+
+		friend_lpn_enqueue_rx(frnd, rx, type, seq_auth, seg_count,
+				      sbuf);
+	}
+}
+
+bool bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx *tx,
+			       enum bt_mesh_friend_pdu_type type,
+			       u64_t *seq_auth, u8_t seg_count,
+			       struct os_mbuf *sbuf)
+{
+	bool matched = false;
+	int i;
+
+	if (!bt_mesh_friend_match(tx->sub->net_idx, tx->ctx->addr) ||
+	    bt_mesh_friend_get() != BT_MESH_FRIEND_ENABLED) {
+		return matched;
+	}
+
+	BT_DBG("net_idx 0x%04x dst 0x%04x src 0x%04x", tx->sub->net_idx,
+	       tx->ctx->addr, tx->src);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+
+		if (!friend_lpn_matches(frnd, tx->sub->net_idx,
+					tx->ctx->addr)) {
+			continue;
+		}
+
+		if (!friend_queue_prepare_space(frnd, tx->src, seq_auth,
+						seg_count)) {
+			continue;
+		}
+
+		friend_lpn_enqueue_tx(frnd, tx, type, seq_auth, seg_count,
+				      sbuf);
+		matched = true;
+	}
+
+	return matched;
+}
+
+void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src,
+				     u16_t dst, u64_t *seq_auth)
+{
+	int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) {
+		struct bt_mesh_friend *frnd = &bt_mesh.frnd[i];
+		int j;
+
+		if (!friend_lpn_matches(frnd, sub->net_idx, dst)) {
+			continue;
+		}
+
+		for (j = 0; j < ARRAY_SIZE(frnd->seg); j++) {
+			struct bt_mesh_friend_seg *seg = &frnd->seg[j];
+
+			if (!is_seg(seg, src, *seq_auth & TRANS_SEQ_ZERO_MASK)) {
+				continue;
+			}
+
+			BT_WARN("Clearing incomplete segments for 0x%04x", src);
+
+			purge_buffers(&seg->queue);
+			seg->seg_count = 0U;
+			break;
+		}
+	}
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_FRIEND) */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/friend.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/friend.h
new file mode 100644
index 00000000..10ffa819
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/friend.h
@@ -0,0 +1,56 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __FRIEND_H__
+#define __FRIEND_H__
+
+#include "mesh/mesh.h"
+
+enum bt_mesh_friend_pdu_type {
+	BT_MESH_FRIEND_PDU_SINGLE,
+	BT_MESH_FRIEND_PDU_PARTIAL,
+	BT_MESH_FRIEND_PDU_COMPLETE,
+};
+
+bool bt_mesh_friend_match(u16_t net_idx, u16_t addr);
+
+struct bt_mesh_friend *bt_mesh_friend_find(u16_t net_idx, u16_t lpn_addr,
+					   bool valid, bool established);
+
+bool bt_mesh_friend_queue_has_space(u16_t net_idx, u16_t src, u16_t dst,
+				    u64_t *seq_auth, u8_t seg_count);
+
+void bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx *rx,
+			       enum bt_mesh_friend_pdu_type type,
+			       u64_t *seq_auth, u8_t seg_count,
+			       struct os_mbuf *sbuf);
+bool bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx *tx,
+			       enum bt_mesh_friend_pdu_type type,
+			       u64_t *seq_auth, u8_t seg_count,
+			       struct os_mbuf *sbuf);
+
+void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src,
+				     u16_t dst, u64_t *seq_auth);
+
+void bt_mesh_friend_sec_update(u16_t net_idx);
+
+void bt_mesh_friend_clear_net_idx(u16_t net_idx);
+
+int bt_mesh_friend_poll(struct bt_mesh_net_rx *rx, struct os_mbuf *buf);
+int bt_mesh_friend_req(struct bt_mesh_net_rx *rx, struct os_mbuf *buf);
+int bt_mesh_friend_clear(struct bt_mesh_net_rx *rx, struct os_mbuf *buf);
+int bt_mesh_friend_clear_cfm(struct bt_mesh_net_rx *rx,
+			     struct os_mbuf *buf);
+int bt_mesh_friend_sub_add(struct bt_mesh_net_rx *rx,
+			   struct os_mbuf *buf);
+int bt_mesh_friend_sub_rem(struct bt_mesh_net_rx *rx,
+			   struct os_mbuf *buf);
+
+int bt_mesh_friend_init(void);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/glue.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/glue.c
new file mode 100644
index 00000000..896f3d1a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/glue.c
@@ -0,0 +1,870 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_LOG
+
+#include "mesh/glue.h"
+#include "adv.h"
+#ifndef MYNEWT
+#include "nimble/nimble_port.h"
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_SETTINGS)
+#include "base64/base64.h"
+#endif
+
+extern u8_t g_mesh_addr_type;
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+/* Store configuration for different bearers */
+#define BT_MESH_ADV_IDX          (0)
+#define BT_MESH_GATT_IDX         (1)
+static struct ble_gap_adv_params ble_adv_cur_conf[2];
+#endif
+
+const char *
+bt_hex(const void *buf, size_t len)
+{
+    static const char hex[] = "0123456789abcdef";
+    static char hexbufs[4][137];
+    static u8_t curbuf;
+    const u8_t *b = buf;
+    char *str;
+    int i;
+
+    str = hexbufs[curbuf++];
+    curbuf %= ARRAY_SIZE(hexbufs);
+
+    len = min(len, (sizeof(hexbufs[0]) - 1) / 2);
+
+    for (i = 0; i < len; i++) {
+        str[i * 2] = hex[b[i] >> 4];
+        str[i * 2 + 1] = hex[b[i] & 0xf];
+    }
+
+    str[i * 2] = '\0';
+
+    return str;
+}
+
+void
+net_buf_put(struct ble_npl_eventq *fifo, struct os_mbuf *om)
+{
+    struct ble_npl_event *ev;
+
+    assert(OS_MBUF_IS_PKTHDR(om));
+    ev = &BT_MESH_ADV(om)->ev;
+    assert(ev);
+    assert(ble_npl_event_get_arg(ev));
+
+    ble_npl_eventq_put(fifo, ev);
+}
+
+void *
+net_buf_ref(struct os_mbuf *om)
+{
+    struct bt_mesh_adv *adv;
+
+    /* For bufs with header we count refs*/
+    if (OS_MBUF_USRHDR_LEN(om) == 0) {
+        return om;
+    }
+
+    adv = BT_MESH_ADV(om);
+    adv->ref_cnt++;
+
+    return om;
+}
+
+void
+net_buf_unref(struct os_mbuf *om)
+{
+    struct bt_mesh_adv *adv;
+
+    /* For bufs with header we count refs*/
+    if (OS_MBUF_USRHDR_LEN(om) == 0) {
+        goto free;
+    }
+
+    adv = BT_MESH_ADV(om);
+    if (--adv->ref_cnt > 0) {
+        return;
+    }
+
+free:
+    os_mbuf_free_chain(om);
+}
+
+int
+bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data)
+{
+    struct tc_aes_key_sched_struct s;
+
+    if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    return 0;
+}
+
+uint16_t
+net_buf_simple_pull_le16(struct os_mbuf *om)
+{
+    uint16_t val;
+    struct os_mbuf *old = om;
+
+    om = os_mbuf_pullup(om, sizeof(val));
+    assert(om == old);
+    val = get_le16(om->om_data);
+    os_mbuf_adj(om, sizeof(val));
+
+    return val;
+}
+
+uint16_t
+net_buf_simple_pull_be16(struct os_mbuf *om)
+{
+    uint16_t val;
+    struct os_mbuf *old = om;
+
+    om = os_mbuf_pullup(om, sizeof(val));
+    assert(om == old);
+    val = get_be16(om->om_data);
+    os_mbuf_adj(om, sizeof(val));
+
+    return val;
+}
+
+uint32_t
+net_buf_simple_pull_be32(struct os_mbuf *om)
+{
+    uint32_t val;
+    struct os_mbuf *old = om;
+
+    om = os_mbuf_pullup(om, sizeof(val));
+    assert(om == old);
+    val = get_be32(om->om_data);
+    os_mbuf_adj(om, sizeof(val));
+
+    return val;
+}
+
+uint32_t
+net_buf_simple_pull_le32(struct os_mbuf *om)
+{
+    uint32_t val;
+    struct os_mbuf *old = om;
+
+    om = os_mbuf_pullup(om, sizeof(val));
+    assert(om == old);
+    val = get_le32(om->om_data);
+    os_mbuf_adj(om, sizeof(val));
+
+    return val;
+}
+
+uint8_t
+net_buf_simple_pull_u8(struct os_mbuf *om)
+{
+    uint8_t val;
+    struct os_mbuf *old = om;
+
+    om = os_mbuf_pullup(om, sizeof(val));
+    assert(om == old);
+    val = om->om_data[0];
+    os_mbuf_adj(om, 1);
+
+    return val;
+}
+
+void
+net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val)
+{
+    val = htole16(val);
+    os_mbuf_append(om, &val, sizeof(val));
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val)
+{
+    val = htobe16(val);
+    os_mbuf_append(om, &val, sizeof(val));
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val)
+{
+    val = htobe32(val);
+    os_mbuf_append(om, &val, sizeof(val));
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_le32(struct os_mbuf *om, uint32_t val)
+{
+    val = htole32(val);
+    os_mbuf_append(om, &val, sizeof(val));
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val)
+{
+    os_mbuf_append(om, &val, 1);
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_push_le16(struct os_mbuf *om, uint16_t val)
+{
+    uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len];
+
+    assert(headroom >= 2);
+    om->om_data -= 2;
+    put_le16(om->om_data, val);
+    om->om_len += 2;
+
+    if (om->om_pkthdr_len) {
+        OS_MBUF_PKTHDR(om)->omp_len += 2;
+    }
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_push_be16(struct os_mbuf *om, uint16_t val)
+{
+    uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len];
+
+    assert(headroom >= 2);
+    om->om_data -= 2;
+    put_be16(om->om_data, val);
+    om->om_len += 2;
+
+    if (om->om_pkthdr_len) {
+        OS_MBUF_PKTHDR(om)->omp_len += 2;
+    }
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_push_u8(struct os_mbuf *om, uint8_t val)
+{
+    uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len];
+
+    assert(headroom >= 1);
+    om->om_data -= 1;
+    om->om_data[0] = val;
+    om->om_len += 1;
+
+    if (om->om_pkthdr_len) {
+        OS_MBUF_PKTHDR(om)->omp_len += 1;
+    }
+    ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_add_zeros(struct os_mbuf *om, uint8_t len)
+{
+    uint8_t z[len];
+    int rc;
+
+    memset(z, 0, len);
+
+    rc = os_mbuf_append(om, z, len);
+    if(rc) {
+        assert(0);
+    }
+    ASSERT_NOT_CHAIN(om);
+}
+
+void *
+net_buf_simple_pull(struct os_mbuf *om, uint8_t len)
+{
+    os_mbuf_adj(om, len);
+    return om->om_data;
+}
+
+void *
+net_buf_simple_pull_mem(struct os_mbuf *om, uint8_t len)
+{
+    void *data = om->om_data;
+
+    net_buf_simple_pull(om, len);
+    return data;
+}
+
+void*
+net_buf_simple_add(struct os_mbuf *om, uint8_t len)
+{
+    void * tmp;
+
+    tmp = os_mbuf_extend(om, len);
+    ASSERT_NOT_CHAIN(om);
+
+    return tmp;
+}
+
+bool
+k_fifo_is_empty(struct ble_npl_eventq *q)
+{
+    return ble_npl_eventq_is_empty(q);
+}
+
+void * net_buf_get(struct ble_npl_eventq *fifo, s32_t t)
+{
+    struct ble_npl_event *ev = ble_npl_eventq_get(fifo, 0);
+
+    if (ev) {
+        return ble_npl_event_get_arg(ev);
+    }
+
+    return NULL;
+}
+
+uint8_t *
+net_buf_simple_push(struct os_mbuf *om, uint8_t len)
+{
+    uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len];
+
+    assert(headroom >= len);
+    om->om_data -= len;
+    om->om_len += len;
+
+    return om->om_data;
+}
+
+void
+net_buf_reserve(struct os_mbuf *om, size_t reserve)
+{
+    /* We need reserve to be done on fresh buf */
+    assert(om->om_len == 0);
+    om->om_data += reserve;
+}
+
+void
+k_work_init(struct ble_npl_callout *work, ble_npl_event_fn handler)
+{
+#ifndef MYNEWT
+    ble_npl_callout_init(work, nimble_port_get_dflt_eventq(), handler, NULL);
+#else
+    ble_npl_callout_init(work, ble_npl_eventq_dflt_get(), handler, NULL);
+#endif
+}
+
+void
+k_delayed_work_init(struct k_delayed_work *w, ble_npl_event_fn *f)
+{
+#ifndef MYNEWT
+    ble_npl_callout_init(&w->work, nimble_port_get_dflt_eventq(), f, NULL);
+#else
+    ble_npl_callout_init(&w->work, ble_npl_eventq_dflt_get(), f, NULL);
+#endif
+}
+
+void
+k_delayed_work_cancel(struct k_delayed_work *w)
+{
+    ble_npl_callout_stop(&w->work);
+}
+
+void
+k_delayed_work_submit(struct k_delayed_work *w, uint32_t ms)
+{
+    uint32_t ticks;
+
+    if (ble_npl_time_ms_to_ticks(ms, &ticks) != 0) {
+        assert(0);
+    }
+    ble_npl_callout_reset(&w->work, ticks);
+}
+
+void
+k_work_submit(struct ble_npl_callout *w)
+{
+    ble_npl_callout_reset(w, 0);
+}
+
+void
+k_work_add_arg(struct ble_npl_callout *w, void *arg)
+{
+    ble_npl_callout_set_arg(w, arg);
+}
+
+void
+k_delayed_work_add_arg(struct k_delayed_work *w, void *arg)
+{
+    k_work_add_arg(&w->work, arg);
+}
+
+uint32_t
+k_delayed_work_remaining_get (struct k_delayed_work *w)
+{
+    int sr;
+    ble_npl_time_t t;
+
+    OS_ENTER_CRITICAL(sr);
+
+    t = ble_npl_callout_remaining_ticks(&w->work, ble_npl_time_get());
+
+    OS_EXIT_CRITICAL(sr);
+
+    return ble_npl_time_ticks_to_ms32(t);
+}
+
+int64_t k_uptime_get(void)
+{
+    /* We should return ms */
+    return ble_npl_time_ticks_to_ms32(ble_npl_time_get());
+}
+
+u32_t k_uptime_get_32(void)
+{
+    return k_uptime_get();
+}
+
+void k_sleep(int32_t duration)
+{
+    uint32_t ticks;
+
+    ticks = ble_npl_time_ms_to_ticks32(duration);
+
+    ble_npl_time_delay(ticks);
+}
+
+static uint8_t pub[64];
+static uint8_t priv[32];
+static bool has_pub = false;
+
+int
+bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb)
+{
+    uint8_t dh[32];
+
+    if (ble_sm_alg_gen_dhkey((uint8_t *)&remote_pk[0], (uint8_t *)&remote_pk[32],
+                              priv, dh)) {
+        return -1;
+    }
+
+    cb(dh);
+    return 0;
+}
+
+int
+bt_rand(void *buf, size_t len)
+{
+    int rc;
+    rc = ble_hs_hci_util_rand(buf, len);
+    if (rc != 0) {
+        return -1;
+    }
+
+    return 0;
+}
+
+int
+bt_pub_key_gen(struct bt_pub_key_cb *new_cb)
+{
+
+    if (ble_sm_alg_gen_key_pair(pub, priv)) {
+        assert(0);
+        return -1;
+    }
+
+    new_cb->func(pub);
+    has_pub = true;
+
+    return 0;
+}
+
+uint8_t *
+bt_pub_key_get(void)
+{
+    if (!has_pub) {
+        return NULL;
+    }
+
+    return pub;
+}
+
+static int
+set_ad(const struct bt_data *ad, size_t ad_len, u8_t *buf, u8_t *buf_len)
+{
+    int i;
+
+    for (i = 0; i < ad_len; i++) {
+        buf[(*buf_len)++] = ad[i].data_len + 1;
+        buf[(*buf_len)++] = ad[i].type;
+
+        memcpy(&buf[*buf_len], ad[i].data,
+               ad[i].data_len);
+        *buf_len += ad[i].data_len;
+    }
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_adv_copy_to_ext_param(struct ble_gap_ext_adv_params *ext_param,
+                          const struct ble_gap_adv_params *param)
+{
+    memset(ext_param, 0, sizeof(*ext_param));
+
+    ext_param->legacy_pdu = 1;
+
+    if (param->conn_mode != BLE_GAP_CONN_MODE_NON) {
+        ext_param->connectable = 1;
+        ext_param->scannable = 1;
+    }
+
+    ext_param->itvl_max = param->itvl_max;
+    ext_param->itvl_min = param->itvl_min;
+    ext_param->channel_map = param->channel_map;
+    ext_param->high_duty_directed = param->high_duty_cycle;
+    ext_param->own_addr_type = g_mesh_addr_type;
+}
+
+static int
+ble_adv_conf_adv_instance(const struct ble_gap_adv_params *param, int *instance)
+{
+    struct ble_gap_ext_adv_params ext_params;
+    struct ble_gap_adv_params *cur_conf;
+    int err = 0;
+
+    if (param->conn_mode == BLE_GAP_CONN_MODE_NON) {
+        *instance = BT_MESH_ADV_INST;
+        cur_conf = &ble_adv_cur_conf[BT_MESH_ADV_IDX];
+    } else {
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+        *instance = BT_MESH_ADV_GATT_INST;
+        cur_conf = &ble_adv_cur_conf[BT_MESH_GATT_IDX];
+#else
+        assert(0);
+#endif
+    }
+
+    /* Checking interval max as it has to be in place if instance was configured
+     * before.
+     */
+    if (cur_conf->itvl_max == 0) {
+        goto configure;
+    }
+
+    if (memcmp(param, cur_conf, sizeof(*cur_conf)) == 0) {
+        /* Same parameters - skip reconfiguring */
+        goto done;
+    }
+
+    ble_gap_ext_adv_stop(*instance);
+    err = ble_gap_ext_adv_remove(*instance);
+    if (err) {
+        assert(0);
+        goto done;
+    }
+
+configure:
+    ble_adv_copy_to_ext_param(&ext_params, param);
+
+    err  = ble_gap_ext_adv_configure(*instance, &ext_params, 0,
+                                            ble_adv_gap_mesh_cb, NULL);
+    if (!err) {
+        memcpy(cur_conf, param, sizeof(*cur_conf));
+    }
+
+done:
+    return err;
+}
+
+int
+bt_le_adv_start(const struct ble_gap_adv_params *param,
+                const struct bt_data *ad, size_t ad_len,
+                const struct bt_data *sd, size_t sd_len)
+{
+    struct os_mbuf *data;
+    int instance;
+    int err;
+    uint8_t buf[BLE_HS_ADV_MAX_SZ];
+    uint8_t buf_len = 0;
+
+    err = ble_adv_conf_adv_instance(param, &instance);
+    if (err) {
+        return err;
+    }
+
+    if (ad_len > 0) {
+        err = set_ad(ad, ad_len, buf, &buf_len);
+        if (err) {
+            return err;
+        }
+
+        /* For now let's use msys pool. We are not putting more then legacy */
+        data = os_msys_get_pkthdr(BLE_HS_ADV_MAX_SZ, 0);
+        if (!data) {
+            return OS_ENOMEM;
+        }
+
+        err = os_mbuf_append(data, buf, buf_len);
+        if (err) {
+            goto error;
+        }
+
+        err = ble_gap_ext_adv_set_data(instance, data);
+        if (err) {
+            return err;
+        }
+
+        data = NULL;
+    }
+
+    if (sd_len > 0) {
+        buf_len = 0;
+
+        err = set_ad(sd, sd_len, buf, &buf_len);
+        if (err) {
+            return err;
+        }
+
+        /* For now let's use msys pool. We are not putting more then legace*/
+        data = os_msys_get_pkthdr(BLE_HS_ADV_MAX_SZ, 0);
+        if (!data) {
+            return OS_ENOMEM;
+        }
+
+        err = os_mbuf_append(data, buf, buf_len);
+        if (err) {
+            goto error;
+        }
+
+        err = ble_gap_ext_adv_rsp_set_data(instance, data);
+        if (err) {
+            goto error;
+        }
+    }
+
+    /*TODO: We could use duration and max events in the future */
+    err = ble_gap_ext_adv_start(instance, 0, 0);
+    return err;
+
+error:
+    if (data) {
+        os_mbuf_free_chain(data);
+    }
+
+    return err;
+}
+
+int bt_le_adv_stop(bool proxy)
+{
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+    int rc;
+
+    if (proxy) {
+        rc = ble_gap_ext_adv_stop(BT_MESH_ADV_GATT_INST);
+    } else {
+        rc = ble_gap_ext_adv_stop(BT_MESH_ADV_INST);
+    }
+
+    return rc;
+#else
+    return ble_gap_ext_adv_stop(BT_MESH_ADV_INST);
+#endif
+}
+
+#else
+
+int
+bt_le_adv_start(const struct ble_gap_adv_params *param,
+                const struct bt_data *ad, size_t ad_len,
+                const struct bt_data *sd, size_t sd_len)
+{
+    uint8_t buf[BLE_HS_ADV_MAX_SZ];
+    uint8_t buf_len = 0;
+    int err;
+
+    err = set_ad(ad, ad_len, buf, &buf_len);
+    if (err) {
+        return err;
+    }
+
+    err = ble_gap_adv_set_data(buf, buf_len);
+    if (err != 0) {
+        return err;
+    }
+
+    if (sd) {
+        buf_len = 0;
+
+        err = set_ad(sd, sd_len, buf, &buf_len);
+        if (err) {
+            BT_ERR("Advertising failed: err %d", err);
+            return err;
+        }
+
+        err = ble_gap_adv_rsp_set_data(buf, buf_len);
+        if (err != 0) {
+            BT_ERR("Advertising failed: err %d", err);
+            return err;
+        }
+    }
+
+    err = ble_gap_adv_start(g_mesh_addr_type, NULL, BLE_HS_FOREVER, param,
+                            NULL, NULL);
+    if (err) {
+        BT_ERR("Advertising failed: err %d", err);
+        return err;
+    }
+
+    return 0;
+}
+
+int bt_le_adv_stop(bool proxy)
+{
+	return ble_gap_adv_stop();
+}
+
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+int bt_mesh_proxy_svcs_register(void);
+#endif
+
+void
+bt_mesh_register_gatt(void)
+{
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+    bt_mesh_proxy_svcs_register();
+#endif
+}
+
+void net_buf_slist_init(struct net_buf_slist_t *list)
+{
+	STAILQ_INIT(list);
+}
+
+bool net_buf_slist_is_empty(struct net_buf_slist_t *list)
+{
+	return STAILQ_EMPTY(list);
+}
+
+struct os_mbuf *net_buf_slist_peek_head(struct net_buf_slist_t *list)
+{
+	struct os_mbuf_pkthdr *pkthdr;
+
+	/* Get mbuf pointer from packet header pointer */
+	pkthdr = STAILQ_FIRST(list);
+	if (!pkthdr) {
+		return NULL;
+	}
+
+	return OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+}
+
+struct os_mbuf *net_buf_slist_peek_next(struct os_mbuf *buf)
+{
+	struct os_mbuf_pkthdr *pkthdr;
+
+	/* Get mbuf pointer from packet header pointer */
+	pkthdr = OS_MBUF_PKTHDR(buf);
+	pkthdr = STAILQ_NEXT(pkthdr, omp_next);
+	if (!pkthdr) {
+		return NULL;
+	}
+
+	return OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+}
+
+struct os_mbuf *net_buf_slist_get(struct net_buf_slist_t *list)
+{
+	os_sr_t sr;
+	struct os_mbuf *m;
+
+	m = net_buf_slist_peek_head(list);
+	if (!m) {
+		return NULL;
+	}
+
+	/* Remove from queue */
+	OS_ENTER_CRITICAL(sr);
+	STAILQ_REMOVE_HEAD(list, omp_next);
+	OS_EXIT_CRITICAL(sr);
+	return m;
+}
+
+void net_buf_slist_put(struct net_buf_slist_t *list, struct os_mbuf *buf)
+{
+	struct os_mbuf_pkthdr *pkthdr;
+
+	pkthdr = OS_MBUF_PKTHDR(buf);
+	STAILQ_INSERT_TAIL(list, pkthdr, omp_next);
+}
+
+void net_buf_slist_remove(struct net_buf_slist_t *list, struct os_mbuf *prev,
+			  struct os_mbuf *cur)
+{
+	struct os_mbuf_pkthdr *pkthdr, *cur_pkthdr;
+
+	cur_pkthdr = OS_MBUF_PKTHDR(cur);
+
+	STAILQ_FOREACH(pkthdr, list, omp_next) {
+		if (cur_pkthdr == pkthdr) {
+			STAILQ_REMOVE(list, cur_pkthdr, os_mbuf_pkthdr, omp_next);
+			break;
+		}
+	}
+}
+
+void net_buf_slist_merge_slist(struct net_buf_slist_t *list,
+			       struct net_buf_slist_t *list_to_append)
+{
+	if (!STAILQ_EMPTY(list_to_append)) {
+		*(list)->stqh_last = list_to_append->stqh_first;
+		(list)->stqh_last = list_to_append->stqh_last;
+		STAILQ_INIT(list_to_append);
+	}
+}
+
+#if MYNEWT_VAL(BLE_MESH_SETTINGS)
+
+int settings_bytes_from_str(char *val_str, void *vp, int *len)
+{
+    *len = base64_decode(val_str, vp);
+    return 0;
+}
+
+char *settings_str_from_bytes(const void *vp, int vp_len,
+			      char *buf, int buf_len)
+{
+    if (BASE64_ENCODE_SIZE(vp_len) > buf_len) {
+        return NULL;
+    }
+
+    base64_encode(vp, vp_len, buf, 1);
+
+    return buf;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_SETTINGS) */
+
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/health_cli.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/health_cli.c
new file mode 100644
index 00000000..193279c2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/health_cli.c
@@ -0,0 +1,556 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_MODEL_LOG
+
+#include <string.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include "mesh/mesh.h"
+#include "mesh_priv.h"
+#include "adv.h"
+#include "net.h"
+#include "transport.h"
+#include "foundation.h"
+#include "mesh/health_cli.h"
+
+static s32_t msg_timeout = K_SECONDS(5);
+
+static struct bt_mesh_health_cli *health_cli;
+
+struct health_fault_param {
+	u16_t   cid;
+	u8_t   *expect_test_id;
+	u8_t   *test_id;
+	u8_t   *faults;
+	size_t *fault_count;
+};
+
+static void health_fault_status(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx,
+				struct os_mbuf *buf)
+{
+	struct health_fault_param *param;
+	u8_t test_id;
+	u16_t cid;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (health_cli->op_pending != OP_HEALTH_FAULT_STATUS) {
+		BT_WARN("Unexpected Health Fault Status message");
+		return;
+	}
+
+	param = health_cli->op_param;
+
+	test_id = net_buf_simple_pull_u8(buf);
+	if (param->expect_test_id && test_id != *param->expect_test_id) {
+		BT_WARN("Health fault with unexpected Test ID");
+		return;
+	}
+
+	cid = net_buf_simple_pull_le16(buf);
+	if (cid != param->cid) {
+		BT_WARN("Health fault with unexpected Company ID");
+		return;
+	}
+
+	if (param->test_id) {
+		*param->test_id = test_id;
+	}
+
+	if (buf->om_len > *param->fault_count) {
+		BT_WARN("Got more faults than there's space for");
+	} else {
+		*param->fault_count = buf->om_len;
+	}
+
+	memcpy(param->faults, buf->om_data, *param->fault_count);
+
+	k_sem_give(&health_cli->op_sync);
+}
+
+static void health_current_status(struct bt_mesh_model *model,
+				  struct bt_mesh_msg_ctx *ctx,
+				  struct os_mbuf *buf)
+{
+	struct bt_mesh_health_cli *cli = model->user_data;
+	u8_t test_id;
+	u16_t cid;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	test_id = net_buf_simple_pull_u8(buf);
+	cid = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("Test ID 0x%02x Company ID 0x%04x Fault Count %u",
+	       test_id, cid, buf->om_len);
+
+	if (!cli->current_status) {
+		BT_WARN("No Current Status callback available");
+		return;
+	}
+
+	cli->current_status(cli, ctx->addr, test_id, cid, buf->om_data, buf->om_len);
+}
+
+struct health_period_param {
+	u8_t *divisor;
+};
+
+static void health_period_status(struct bt_mesh_model *model,
+				 struct bt_mesh_msg_ctx *ctx,
+				 struct os_mbuf *buf)
+{
+	struct health_period_param *param;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (health_cli->op_pending != OP_HEALTH_PERIOD_STATUS) {
+		BT_WARN("Unexpected Health Period Status message");
+		return;
+	}
+
+	param = health_cli->op_param;
+
+	*param->divisor = net_buf_simple_pull_u8(buf);
+
+	k_sem_give(&health_cli->op_sync);
+}
+
+struct health_attention_param {
+	u8_t *attention;
+};
+
+static void health_attention_status(struct bt_mesh_model *model,
+				    struct bt_mesh_msg_ctx *ctx,
+				    struct os_mbuf *buf)
+{
+	struct health_attention_param *param;
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (health_cli->op_pending != OP_ATTENTION_STATUS) {
+		BT_WARN("Unexpected Health Attention Status message");
+		return;
+	}
+
+	param = health_cli->op_param;
+
+	if (param->attention) {
+		*param->attention = net_buf_simple_pull_u8(buf);
+	}
+
+	k_sem_give(&health_cli->op_sync);
+}
+
+const struct bt_mesh_model_op bt_mesh_health_cli_op[] = {
+	{ OP_HEALTH_FAULT_STATUS,    3,   health_fault_status },
+	{ OP_HEALTH_CURRENT_STATUS,  3,   health_current_status },
+	{ OP_HEALTH_PERIOD_STATUS,   1,   health_period_status },
+	{ OP_ATTENTION_STATUS,       1,   health_attention_status },
+	BT_MESH_MODEL_OP_END,
+};
+
+static int cli_prepare(void *param, u32_t op)
+{
+	if (!health_cli) {
+		BT_ERR("No available Health Client context!");
+		return -EINVAL;
+	}
+
+	if (health_cli->op_pending) {
+		BT_WARN("Another synchronous operation pending");
+		return -EBUSY;
+	}
+
+	health_cli->op_param = param;
+	health_cli->op_pending = op;
+
+	return 0;
+}
+
+static void cli_reset(void)
+{
+	health_cli->op_pending = 0;
+	health_cli->op_param = NULL;
+}
+
+static int cli_wait(void)
+{
+	int err;
+
+	err = k_sem_take(&health_cli->op_sync, msg_timeout);
+
+	cli_reset();
+
+	return err;
+}
+
+int bt_mesh_health_attention_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+				 u8_t *attention)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_ATTENTION_GET, 0);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_attention_param param = {
+		.attention = attention,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_ATTENTION_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_ATTENTION_GET);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_health_attention_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+				 u8_t attention, u8_t *updated_attention)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_ATTENTION_SET, 1);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_attention_param param = {
+		.attention = updated_attention,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_ATTENTION_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	if (updated_attention) {
+		bt_mesh_model_msg_init(msg, OP_ATTENTION_SET);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_ATTENTION_SET_UNREL);
+	}
+
+	net_buf_simple_add_u8(msg, attention);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!updated_attention) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_health_period_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			      u8_t *divisor)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEALTH_PERIOD_GET, 0);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_period_param param = {
+		.divisor = divisor,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEALTH_PERIOD_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEALTH_PERIOD_GET);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_health_period_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+			      u8_t divisor, u8_t *updated_divisor)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEALTH_PERIOD_SET, 1);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_period_param param = {
+		.divisor = updated_divisor,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEALTH_PERIOD_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	if (updated_divisor) {
+		bt_mesh_model_msg_init(msg, OP_HEALTH_PERIOD_SET);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_HEALTH_PERIOD_SET_UNREL);
+	}
+
+	net_buf_simple_add_u8(msg, divisor);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!updated_divisor) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_health_fault_test(u16_t net_idx, u16_t addr, u16_t app_idx,
+			      u16_t cid, u8_t test_id, u8_t *faults,
+			      size_t *fault_count)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEALTH_FAULT_TEST, 3);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_fault_param param = {
+		.cid = cid,
+		.expect_test_id = &test_id,
+		.faults = faults,
+		.fault_count = fault_count,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEALTH_FAULT_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	if (faults) {
+		bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_TEST);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_TEST_UNREL);
+	}
+
+	net_buf_simple_add_u8(msg, test_id);
+	net_buf_simple_add_le16(msg, cid);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!faults) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_health_fault_clear(u16_t net_idx, u16_t addr, u16_t app_idx,
+			       u16_t cid, u8_t *test_id, u8_t *faults,
+			       size_t *fault_count)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEALTH_FAULT_CLEAR, 2);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_fault_param param = {
+		.cid = cid,
+		.test_id = test_id,
+		.faults = faults,
+		.fault_count = fault_count,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEALTH_FAULT_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	if (test_id) {
+		bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_CLEAR);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_CLEAR_UNREL);
+	}
+
+	net_buf_simple_add_le16(msg, cid);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	if (!test_id) {
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_health_fault_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			     u16_t cid, u8_t *test_id, u8_t *faults,
+			     size_t *fault_count)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEALTH_FAULT_GET, 2);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct health_fault_param param = {
+		.cid = cid,
+		.test_id = test_id,
+		.faults = faults,
+		.fault_count = fault_count,
+	};
+	int err;
+
+	err = cli_prepare(&param, OP_HEALTH_FAULT_STATUS);
+	if (err) {
+		goto done;
+	}
+
+	bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_GET);
+	net_buf_simple_add_le16(msg, cid);
+
+	err = bt_mesh_model_send(health_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		cli_reset();
+		goto done;
+	}
+
+	err = cli_wait();
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+s32_t bt_mesh_health_cli_timeout_get(void)
+{
+	return msg_timeout;
+}
+
+void bt_mesh_health_cli_timeout_set(s32_t timeout)
+{
+	msg_timeout = timeout;
+}
+
+int bt_mesh_health_cli_set(struct bt_mesh_model *model)
+{
+	if (!model->user_data) {
+		BT_ERR("No Health Client context for given model");
+		return -EINVAL;
+	}
+
+	health_cli = model->user_data;
+
+	return 0;
+}
+
+static int health_cli_init(struct bt_mesh_model *model)
+{
+	struct bt_mesh_health_cli *cli = model->user_data;
+
+	BT_DBG("primary %u", bt_mesh_model_in_primary(model));
+
+	if (!cli) {
+		BT_ERR("No Health Client context provided");
+		return -EINVAL;
+	}
+
+	cli = model->user_data;
+	cli->model = model;
+
+	k_sem_init(&cli->op_sync, 0, 1);
+
+	/* Set the default health client pointer */
+	if (!health_cli) {
+		health_cli = cli;
+	}
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb bt_mesh_health_cli_cb = {
+	.init = health_cli_init,
+};
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/health_srv.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/health_srv.c
new file mode 100644
index 00000000..16de83a9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/health_srv.c
@@ -0,0 +1,453 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_MODEL_LOG
+
+#include <string.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include "mesh/mesh.h"
+#include "mesh_priv.h"
+#include "adv.h"
+#include "net.h"
+#include "transport.h"
+#include "access.h"
+#include "foundation.h"
+
+#define HEALTH_TEST_STANDARD 0x00
+
+/* Health Server context of the primary element */
+struct bt_mesh_health_srv *health_srv;
+
+static void health_get_registered(struct bt_mesh_model *mod,
+				  u16_t company_id,
+				  struct os_mbuf *msg)
+{
+	struct bt_mesh_health_srv *srv = mod->user_data;
+	u8_t *test_id;
+
+	BT_DBG("Company ID 0x%04x", company_id);
+
+	bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_STATUS);
+
+	test_id = net_buf_simple_add(msg, 1);
+	net_buf_simple_add_le16(msg, company_id);
+
+	if (srv->cb && srv->cb->fault_get_reg) {
+		u8_t fault_count = net_buf_simple_tailroom(msg) - 4;
+		int err;
+
+		err = srv->cb->fault_get_reg(mod, company_id, test_id,
+					     net_buf_simple_tail(msg),
+					     &fault_count);
+		if (err) {
+			BT_ERR("Failed to get faults (err %d)", err);
+			*test_id = HEALTH_TEST_STANDARD;
+		} else {
+			net_buf_simple_add(msg, fault_count);
+		}
+	} else {
+		BT_WARN("No callback for getting faults");
+		*test_id = HEALTH_TEST_STANDARD;
+	}
+}
+
+static size_t health_get_current(struct bt_mesh_model *mod,
+				 struct os_mbuf *msg)
+{
+	struct bt_mesh_health_srv *srv = mod->user_data;
+	const struct bt_mesh_comp *comp;
+	u8_t *test_id, *company_ptr;
+	u16_t company_id;
+	u8_t fault_count;
+	int err;
+
+	bt_mesh_model_msg_init(msg, OP_HEALTH_CURRENT_STATUS);
+
+	test_id = net_buf_simple_add(msg, 1);
+	company_ptr = net_buf_simple_add(msg, sizeof(company_id));
+	comp = bt_mesh_comp_get();
+
+	if (srv->cb && srv->cb->fault_get_cur) {
+		fault_count = net_buf_simple_tailroom(msg);
+		err = srv->cb->fault_get_cur(mod, test_id, &company_id,
+					     net_buf_simple_tail(msg),
+					     &fault_count);
+		if (err) {
+			BT_ERR("Failed to get faults (err %d)", err);
+			sys_put_le16(comp->cid, company_ptr);
+			*test_id = HEALTH_TEST_STANDARD;
+			fault_count = 0;
+		} else {
+			sys_put_le16(company_id, company_ptr);
+			net_buf_simple_add(msg, fault_count);
+		}
+	} else {
+		BT_WARN("No callback for getting faults");
+		sys_put_le16(comp->cid, company_ptr);
+		*test_id = HEALTH_TEST_STANDARD;
+		fault_count = 0;
+	}
+
+	return fault_count;
+}
+
+static void health_fault_get(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx,
+			     struct os_mbuf *buf)
+{
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	u16_t company_id;
+
+	company_id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("company_id 0x%04x", company_id);
+
+	health_get_registered(model, company_id, sdu);
+
+	if (bt_mesh_model_send(model, ctx, sdu, NULL, NULL)) {
+		BT_ERR("Unable to send Health Current Status response");
+	}
+
+	os_mbuf_free_chain(sdu);
+}
+
+static void health_fault_clear_unrel(struct bt_mesh_model *model,
+				     struct bt_mesh_msg_ctx *ctx,
+				     struct os_mbuf *buf)
+{
+	struct bt_mesh_health_srv *srv = model->user_data;
+	u16_t company_id;
+
+	company_id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("company_id 0x%04x", company_id);
+
+	if (srv->cb && srv->cb->fault_clear) {
+		srv->cb->fault_clear(model, company_id);
+	}
+}
+
+static void health_fault_clear(struct bt_mesh_model *model,
+			       struct bt_mesh_msg_ctx *ctx,
+			       struct os_mbuf *buf)
+{
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	struct bt_mesh_health_srv *srv = model->user_data;
+	u16_t company_id;
+
+	company_id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("company_id 0x%04x", company_id);
+
+	if (srv->cb && srv->cb->fault_clear) {
+		srv->cb->fault_clear(model, company_id);
+	}
+
+	health_get_registered(model, company_id, sdu);
+
+	if (bt_mesh_model_send(model, ctx, sdu, NULL, NULL)) {
+		BT_ERR("Unable to send Health Current Status response");
+	}
+
+	os_mbuf_free_chain(sdu);
+}
+
+static void health_fault_test_unrel(struct bt_mesh_model *model,
+				    struct bt_mesh_msg_ctx *ctx,
+				    struct os_mbuf *buf)
+{
+	struct bt_mesh_health_srv *srv = model->user_data;
+	u16_t company_id;
+	u8_t test_id;
+
+	test_id = net_buf_simple_pull_u8(buf);
+	company_id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("test 0x%02x company 0x%04x", test_id, company_id);
+
+	if (srv->cb && srv->cb->fault_test) {
+		srv->cb->fault_test(model, test_id, company_id);
+	}
+}
+
+static void health_fault_test(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX);
+	struct bt_mesh_health_srv *srv = model->user_data;
+	u16_t company_id;
+	u8_t test_id;
+
+	BT_DBG("");
+
+	test_id = net_buf_simple_pull_u8(buf);
+	company_id = net_buf_simple_pull_le16(buf);
+
+	BT_DBG("test 0x%02x company 0x%04x", test_id, company_id);
+
+	if (srv->cb && srv->cb->fault_test) {
+		int err;
+
+		err = srv->cb->fault_test(model, test_id, company_id);
+		if (err) {
+			BT_WARN("Running fault test failed with err %d", err);
+			goto done;
+		}
+	}
+
+	health_get_registered(model, company_id, sdu);
+
+	if (bt_mesh_model_send(model, ctx, sdu, NULL, NULL)) {
+		BT_ERR("Unable to send Health Current Status response");
+	}
+
+done:
+	os_mbuf_free_chain(sdu);
+}
+
+static void send_attention_status(struct bt_mesh_model *model,
+				  struct bt_mesh_msg_ctx *ctx)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_ATTENTION_STATUS, 1);
+	struct bt_mesh_health_srv *srv = model->user_data;
+	u8_t time;
+
+	time = k_delayed_work_remaining_get(&srv->attn_timer) / 1000;
+	BT_DBG("%u second%s", time, (time == 1) ? "" : "s");
+
+	bt_mesh_model_msg_init(msg, OP_ATTENTION_STATUS);
+
+	net_buf_simple_add_u8(msg, time);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Attention Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void attention_get(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	send_attention_status(model, ctx);
+}
+
+static void attention_set_unrel(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx,
+				struct os_mbuf *buf)
+{
+	u8_t time;
+
+	time = net_buf_simple_pull_u8(buf);
+
+	BT_DBG("%u second%s", time, (time == 1) ? "" : "s");
+
+	bt_mesh_attention(model, time);
+}
+
+static void attention_set(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	attention_set_unrel(model, ctx, buf);
+
+	send_attention_status(model, ctx);
+}
+
+static void send_health_period_status(struct bt_mesh_model *model,
+				      struct bt_mesh_msg_ctx *ctx)
+{
+	struct os_mbuf *msg = BT_MESH_MODEL_BUF(OP_HEALTH_PERIOD_STATUS, 1);
+
+	bt_mesh_model_msg_init(msg, OP_HEALTH_PERIOD_STATUS);
+
+	net_buf_simple_add_u8(msg, model->pub->period_div);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Unable to send Health Period Status");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void health_period_get(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	send_health_period_status(model, ctx);
+}
+
+static void health_period_set_unrel(struct bt_mesh_model *model,
+				    struct bt_mesh_msg_ctx *ctx,
+				    struct os_mbuf *buf)
+{
+	u8_t period;
+
+	period = net_buf_simple_pull_u8(buf);
+	if (period > 15) {
+		BT_WARN("Prohibited period value %u", period);
+		return;
+	}
+
+	BT_DBG("period %u", period);
+
+	model->pub->period_div = period;
+}
+
+static void health_period_set(struct bt_mesh_model *model,
+			      struct bt_mesh_msg_ctx *ctx,
+			      struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	health_period_set_unrel(model, ctx, buf);
+
+	send_health_period_status(model, ctx);
+}
+
+const struct bt_mesh_model_op bt_mesh_health_srv_op[] = {
+	{ OP_HEALTH_FAULT_GET,         2,   health_fault_get },
+	{ OP_HEALTH_FAULT_CLEAR,       2,   health_fault_clear },
+	{ OP_HEALTH_FAULT_CLEAR_UNREL, 2,   health_fault_clear_unrel },
+	{ OP_HEALTH_FAULT_TEST,        3,   health_fault_test },
+	{ OP_HEALTH_FAULT_TEST_UNREL,  3,   health_fault_test_unrel },
+	{ OP_HEALTH_PERIOD_GET,        0,   health_period_get },
+	{ OP_HEALTH_PERIOD_SET,        1,   health_period_set },
+	{ OP_HEALTH_PERIOD_SET_UNREL,  1,   health_period_set_unrel },
+	{ OP_ATTENTION_GET,            0,   attention_get },
+	{ OP_ATTENTION_SET,            1,   attention_set },
+	{ OP_ATTENTION_SET_UNREL,      1,   attention_set_unrel },
+	BT_MESH_MODEL_OP_END,
+};
+
+static int health_pub_update(struct bt_mesh_model *mod)
+{
+	struct bt_mesh_model_pub *pub = mod->pub;
+	size_t count;
+
+	BT_DBG("");
+
+	count = health_get_current(mod, pub->msg);
+	if (count) {
+		pub->fast_period = 1U;
+	} else {
+		pub->fast_period = 0U;
+	}
+
+	return 0;
+}
+
+int bt_mesh_fault_update(struct bt_mesh_elem *elem)
+{
+	struct bt_mesh_model *mod;
+
+	mod = bt_mesh_model_find(elem, BT_MESH_MODEL_ID_HEALTH_SRV);
+	if (!mod) {
+		return -EINVAL;
+	}
+
+	/* Let periodic publishing, if enabled, take care of sending the
+	 * Health Current Status.
+	 */
+	if (bt_mesh_model_pub_period_get(mod)) {
+		return 0;
+	}
+
+	health_pub_update(mod);
+
+	return bt_mesh_model_publish(mod);
+}
+
+static void attention_off(struct ble_npl_event *work)
+{
+	struct bt_mesh_health_srv *srv = ble_npl_event_get_arg(work);
+	BT_DBG("");
+
+	if (srv->cb && srv->cb->attn_off) {
+		srv->cb->attn_off(srv->model);
+	}
+}
+
+static int health_srv_init(struct bt_mesh_model *model)
+{
+	struct bt_mesh_health_srv *srv = model->user_data;
+
+	if (!srv) {
+		if (!bt_mesh_model_in_primary(model)) {
+			return 0;
+		}
+
+		BT_ERR("No Health Server context provided");
+		return -EINVAL;
+	}
+
+	if (!model->pub) {
+		BT_ERR("Health Server has no publication support");
+		return -EINVAL;
+	}
+
+	model->pub->update = health_pub_update;
+
+	k_delayed_work_init(&srv->attn_timer, attention_off);
+	k_delayed_work_add_arg(&srv->attn_timer, srv);
+
+	srv->model = model;
+
+	if (bt_mesh_model_in_primary(model)) {
+		health_srv = srv;
+	}
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb bt_mesh_health_srv_cb = {
+	.init = health_srv_init,
+};
+
+void bt_mesh_attention(struct bt_mesh_model *model, u8_t time)
+{
+	struct bt_mesh_health_srv *srv;
+
+	BT_DBG("bt_mesh_attention");
+	if (!model) {
+		srv = health_srv;
+		if (!srv) {
+			BT_WARN("No Health Server available");
+			return;
+		}
+
+		model = srv->model;
+	} else {
+		srv = model->user_data;
+	}
+
+	if (time) {
+		if (srv->cb && srv->cb->attn_on) {
+			srv->cb->attn_on(model);
+		}
+
+		k_delayed_work_submit(&srv->attn_timer, time * 1000);
+	} else {
+		k_delayed_work_cancel(&srv->attn_timer);
+
+		if (srv->cb && srv->cb->attn_off) {
+			srv->cb->attn_off(model);
+		}
+	}
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/light_model.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/light_model.c
new file mode 100644
index 00000000..b6d83818
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/light_model.c
@@ -0,0 +1,58 @@
+
+#include "syscfg/syscfg.h"
+
+#include "mesh/mesh.h"
+#include "console/console.h"
+#include "light_model.h"
+
+
+static u8_t gen_onoff_state;
+static s16_t gen_level_state;
+
+static void update_light_state(void)
+{
+	console_printf("Light state: onoff=%d lvl=0x%04x\n", gen_onoff_state, (u16_t)gen_level_state);
+}
+
+int light_model_gen_onoff_get(struct bt_mesh_model *model, u8_t *state)
+{
+	*state = gen_onoff_state;
+	return 0;
+}
+
+int light_model_gen_onoff_set(struct bt_mesh_model *model, u8_t state)
+{
+	gen_onoff_state = state;
+	update_light_state();
+	return 0;
+}
+
+int light_model_gen_level_get(struct bt_mesh_model *model, s16_t *level)
+{
+	*level = gen_level_state;
+	return 0;
+}
+
+int light_model_gen_level_set(struct bt_mesh_model *model, s16_t level)
+{
+	gen_level_state = level;
+	if ((u16_t)gen_level_state > 0x0000) {
+		gen_onoff_state = 1;
+	}
+	if ((u16_t)gen_level_state == 0x0000) {
+		gen_onoff_state = 0;
+	}
+	update_light_state();
+	return 0;
+}
+
+int light_model_light_lightness_get(struct bt_mesh_model *model, s16_t *lightness)
+{
+	return light_model_gen_level_get(model, lightness);
+}
+
+int light_model_light_lightness_set(struct bt_mesh_model *model, s16_t lightness)
+{
+	return light_model_gen_level_set(model, lightness);
+}
+
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/light_model.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/light_model.h
new file mode 100644
index 00000000..95fcdb78
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/light_model.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __BT_MESH_LIGHT_MODEL_H
+#define __BT_MESH_LIGHT_MODEL_H
+
+#include "syscfg/syscfg.h"
+#include "mesh/mesh.h"
+
+int light_model_gen_onoff_get(struct bt_mesh_model *model, u8_t *state);
+int light_model_gen_onoff_set(struct bt_mesh_model *model, u8_t state);
+int light_model_gen_level_get(struct bt_mesh_model *model, s16_t *level);
+int light_model_gen_level_set(struct bt_mesh_model *model, s16_t level);
+int light_model_light_lightness_get(struct bt_mesh_model *model, s16_t *lightness);
+int light_model_light_lightness_set(struct bt_mesh_model *model, s16_t lightness);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/lpn.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/lpn.c
new file mode 100644
index 00000000..ec012a5f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/lpn.c
@@ -0,0 +1,1056 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_LOW_POWER_LOG
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER)
+
+#include <stdint.h>
+
+#include "mesh/mesh.h"
+#include "mesh_priv.h"
+#include "crypto.h"
+#include "adv.h"
+#include "net.h"
+#include "transport.h"
+#include "access.h"
+#include "beacon.h"
+#include "foundation.h"
+#include "lpn.h"
+
+#if MYNEWT_VAL(BLE_MESH_LPN_AUTO)
+#define LPN_AUTO_TIMEOUT          K_SECONDS(MYNEWT_VAL(BLE_MESH_LPN_AUTO_TIMEOUT))
+#else
+#define LPN_AUTO_TIMEOUT          0
+#endif
+
+#define LPN_RECV_DELAY            MYNEWT_VAL(BLE_MESH_LPN_RECV_DELAY)
+#define SCAN_LATENCY              min(MYNEWT_VAL(BLE_MESH_LPN_SCAN_LATENCY), \
+				      LPN_RECV_DELAY)
+
+#define FRIEND_REQ_RETRY_TIMEOUT  K_SECONDS(MYNEWT_VAL(BLE_MESH_LPN_RETRY_TIMEOUT))
+
+#define FRIEND_REQ_WAIT           K_MSEC(100)
+#define FRIEND_REQ_SCAN           K_SECONDS(1)
+#define FRIEND_REQ_TIMEOUT        (FRIEND_REQ_WAIT + FRIEND_REQ_SCAN)
+
+#define POLL_RETRY_TIMEOUT        K_MSEC(100)
+
+#define REQ_RETRY_DURATION(lpn)  (4 * (LPN_RECV_DELAY + (lpn)->adv_duration + \
+				       (lpn)->recv_win + POLL_RETRY_TIMEOUT))
+
+#define POLL_TIMEOUT_INIT     (MYNEWT_VAL(BLE_MESH_LPN_INIT_POLL_TIMEOUT) * 100)
+#define POLL_TIMEOUT_MAX(lpn)   ((MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT) * 100) - \
+				 REQ_RETRY_DURATION(lpn))
+#define REQ_ATTEMPTS(lpn)     (POLL_TIMEOUT_MAX(lpn) < K_SECONDS(3) ? 2 : 4)
+
+#define CLEAR_ATTEMPTS        2
+
+#define LPN_CRITERIA ((MYNEWT_VAL(BLE_MESH_LPN_MIN_QUEUE_SIZE)) | \
+		      (MYNEWT_VAL(BLE_MESH_LPN_RSSI_FACTOR) << 3) | \
+		      (MYNEWT_VAL(BLE_MESH_LPN_RECV_WIN_FACTOR) << 5))
+
+#define POLL_TO(to) { (u8_t)((to) >> 16), (u8_t)((to) >> 8), (u8_t)(to) }
+#define LPN_POLL_TO POLL_TO(MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT))
+
+/* 2 transmissions, 20ms interval */
+#define POLL_XMIT BT_MESH_TRANSMIT(1, 20)
+
+static void (*lpn_cb)(u16_t friend_addr, bool established);
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER_LOG_LVL) == LOG_LEVEL_DEBUG
+static const char *state2str(int state)
+{
+	switch (state) {
+	case BT_MESH_LPN_DISABLED:
+		return "disabled";
+	case BT_MESH_LPN_CLEAR:
+		return "clear";
+	case BT_MESH_LPN_TIMER:
+		return "timer";
+	case BT_MESH_LPN_ENABLED:
+		return "enabled";
+	case BT_MESH_LPN_REQ_WAIT:
+		return "req wait";
+	case BT_MESH_LPN_WAIT_OFFER:
+		return "wait offer";
+	case BT_MESH_LPN_ESTABLISHED:
+		return "established";
+	case BT_MESH_LPN_RECV_DELAY:
+		return "recv delay";
+	case BT_MESH_LPN_WAIT_UPDATE:
+		return "wait update";
+	default:
+		return "(unknown)";
+	}
+}
+#endif
+
+static inline void lpn_set_state(int state)
+{
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER_LOG_LVL) == LOG_LEVEL_DEBUG
+	BT_DBG("%s -> %s", state2str(bt_mesh.lpn.state), state2str(state));
+#endif
+	bt_mesh.lpn.state = state;
+}
+
+static inline void group_zero(atomic_t *target)
+{
+#if CONFIG_BT_MESH_LPN_GROUPS > 32
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
+		atomic_set(&target[i], 0);
+	}
+#else
+	atomic_set(target, 0);
+#endif
+}
+
+static inline void group_set(atomic_t *target, atomic_t *source)
+{
+#if CONFIG_BT_MESH_LPN_GROUPS > 32
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
+		atomic_or(&target[i], atomic_get(&source[i]));
+	}
+#else
+	atomic_or(target, atomic_get(source));
+#endif
+}
+
+static inline void group_clear(atomic_t *target, atomic_t *source)
+{
+#if CONFIG_BT_MESH_LPN_GROUPS > 32
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
+		atomic_and(&target[i], ~atomic_get(&source[i]));
+	}
+#else
+	atomic_and(target, ~atomic_get(source));
+#endif
+}
+
+static void clear_friendship(bool force, bool disable);
+
+static void friend_clear_sent(int err, void *user_data)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	/* We're switching away from Low Power behavior, so permanently
+	 * enable scanning.
+	 */
+	bt_mesh_scan_enable();
+
+	lpn->req_attempts++;
+
+	if (err) {
+		BT_ERR("Sending Friend Request failed (err %d)", err);
+		lpn_set_state(BT_MESH_LPN_ENABLED);
+		clear_friendship(false, lpn->disable);
+		return;
+	}
+
+	lpn_set_state(BT_MESH_LPN_CLEAR);
+	k_delayed_work_submit(&lpn->timer, FRIEND_REQ_TIMEOUT);
+}
+
+static const struct bt_mesh_send_cb clear_sent_cb = {
+	.end = friend_clear_sent,
+};
+
+static int send_friend_clear(void)
+{
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx     = bt_mesh.sub[0].net_idx,
+		.app_idx     = BT_MESH_KEY_UNUSED,
+		.addr        = bt_mesh.lpn.frnd,
+		.send_ttl    = 0,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub = &bt_mesh.sub[0],
+		.ctx = &ctx,
+		.src = bt_mesh_primary_addr(),
+		.xmit = bt_mesh_net_transmit_get(),
+	};
+	struct bt_mesh_ctl_friend_clear req = {
+		.lpn_addr    = sys_cpu_to_be16(tx.src),
+		.lpn_counter = sys_cpu_to_be16(bt_mesh.lpn.counter),
+	};
+
+	BT_DBG("");
+
+	return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR, &req,
+				sizeof(req), NULL, &clear_sent_cb, NULL);
+}
+
+static void clear_friendship(bool force, bool disable)
+{
+	struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	BT_DBG("force %u disable %u", force, disable);
+
+	if (!force && lpn->established && !lpn->clear_success &&
+	    lpn->req_attempts < CLEAR_ATTEMPTS) {
+		send_friend_clear();
+		lpn->disable = disable;
+		return;
+	}
+
+	bt_mesh_rx_reset();
+
+	k_delayed_work_cancel(&lpn->timer);
+
+	friend_cred_del(bt_mesh.sub[0].net_idx, lpn->frnd);
+
+	if (lpn->clear_success) {
+		lpn->old_friend = BT_MESH_ADDR_UNASSIGNED;
+	} else {
+		lpn->old_friend = lpn->frnd;
+	}
+
+	if (lpn_cb && lpn->frnd != BT_MESH_ADDR_UNASSIGNED) {
+		lpn_cb(lpn->frnd, false);
+	}
+
+	lpn->frnd = BT_MESH_ADDR_UNASSIGNED;
+	lpn->fsn = 0;
+	lpn->req_attempts = 0;
+	lpn->recv_win = 0;
+	lpn->queue_size = 0;
+	lpn->disable = 0;
+	lpn->sent_req = 0;
+	lpn->established = 0;
+	lpn->clear_success = 0;
+
+	group_zero(lpn->added);
+	group_zero(lpn->pending);
+	group_zero(lpn->to_remove);
+
+	/* Set this to 1 to force group subscription when the next
+	 * Friendship is created, in case lpn->groups doesn't get
+	 * modified meanwhile.
+	 */
+	lpn->groups_changed = 1;
+
+	if (cfg->hb_pub.feat & BT_MESH_FEAT_LOW_POWER) {
+		bt_mesh_heartbeat_send();
+	}
+
+	if (disable) {
+		lpn_set_state(BT_MESH_LPN_DISABLED);
+		return;
+	}
+
+	lpn_set_state(BT_MESH_LPN_ENABLED);
+	k_delayed_work_submit(&lpn->timer, FRIEND_REQ_RETRY_TIMEOUT);
+}
+
+static void friend_req_sent(u16_t duration, int err, void *user_data)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	if (err) {
+		BT_ERR("Sending Friend Request failed (err %d)", err);
+		return;
+	}
+
+	lpn->adv_duration = duration;
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+		k_delayed_work_submit(&lpn->timer, FRIEND_REQ_WAIT);
+		lpn_set_state(BT_MESH_LPN_REQ_WAIT);
+	} else {
+		k_delayed_work_submit(&lpn->timer,
+				      duration + FRIEND_REQ_TIMEOUT);
+		lpn_set_state(BT_MESH_LPN_WAIT_OFFER);
+	}
+}
+
+static const struct bt_mesh_send_cb friend_req_sent_cb = {
+	.start = friend_req_sent,
+};
+
+static int send_friend_req(struct bt_mesh_lpn *lpn)
+{
+	const struct bt_mesh_comp *comp = bt_mesh_comp_get();
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx  = bt_mesh.sub[0].net_idx,
+		.app_idx  = BT_MESH_KEY_UNUSED,
+		.addr     = BT_MESH_ADDR_FRIENDS,
+		.send_ttl = 0,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub = &bt_mesh.sub[0],
+		.ctx = &ctx,
+		.src = bt_mesh_primary_addr(),
+		.xmit = POLL_XMIT,
+	};
+	struct bt_mesh_ctl_friend_req req = {
+		.criteria    = LPN_CRITERIA,
+		.recv_delay  = LPN_RECV_DELAY,
+		.poll_to     = LPN_POLL_TO,
+		.prev_addr   = lpn->old_friend,
+		.num_elem    = comp->elem_count,
+		.lpn_counter = sys_cpu_to_be16(lpn->counter),
+	};
+
+	BT_DBG("");
+
+	return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_REQ, &req,
+				sizeof(req), NULL, &friend_req_sent_cb, NULL);
+}
+
+static void req_sent(u16_t duration, int err, void *user_data)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER_LOG_LVL) == LOG_LEVEL_DEBUG
+	BT_DBG("req 0x%02x duration %u err %d state %s",
+	       lpn->sent_req, duration, err, state2str(lpn->state));
+#endif
+
+	if (err) {
+		BT_ERR("Sending request failed (err %d)", err);
+		lpn->sent_req = 0;
+		group_zero(lpn->pending);
+		return;
+	}
+
+	lpn->req_attempts++;
+	lpn->adv_duration = duration;
+
+	if (lpn->established || IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+		lpn_set_state(BT_MESH_LPN_RECV_DELAY);
+		/* We start scanning a bit early to elimitate risk of missing
+		 * response data due to HCI and other latencies.
+		 */
+		k_delayed_work_submit(&lpn->timer,
+				      LPN_RECV_DELAY - SCAN_LATENCY);
+	} else {
+		k_delayed_work_submit(&lpn->timer,
+				      LPN_RECV_DELAY + duration +
+				      lpn->recv_win);
+	}
+}
+
+static const struct bt_mesh_send_cb req_sent_cb = {
+	.start = req_sent,
+};
+
+static int send_friend_poll(void)
+{
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx     = bt_mesh.sub[0].net_idx,
+		.app_idx     = BT_MESH_KEY_UNUSED,
+		.addr        = bt_mesh.lpn.frnd,
+		.send_ttl    = 0,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub = &bt_mesh.sub[0],
+		.ctx = &ctx,
+		.src = bt_mesh_primary_addr(),
+		.xmit = POLL_XMIT,
+		.friend_cred = true,
+	};
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	u8_t fsn = lpn->fsn;
+	int err;
+
+	BT_DBG("lpn->sent_req 0x%02x", lpn->sent_req);
+
+	if (lpn->sent_req) {
+		if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) {
+			lpn->pending_poll = 1;
+		}
+
+		return 0;
+	}
+
+	err = bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_POLL, &fsn, 1,
+			       NULL, &req_sent_cb, NULL);
+	if (err == 0) {
+		lpn->pending_poll = 0;
+		lpn->sent_req = TRANS_CTL_OP_FRIEND_POLL;
+	}
+
+	return err;
+}
+
+void bt_mesh_lpn_disable(bool force)
+{
+	if (bt_mesh.lpn.state == BT_MESH_LPN_DISABLED) {
+		return;
+	}
+
+	clear_friendship(force, true);
+}
+
+int bt_mesh_lpn_set(bool enable)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	if (enable) {
+		if (lpn->state != BT_MESH_LPN_DISABLED) {
+			return 0;
+		}
+	} else {
+		if (lpn->state == BT_MESH_LPN_DISABLED) {
+			return 0;
+		}
+	}
+
+	if (!bt_mesh_is_provisioned()) {
+		if (enable) {
+			lpn_set_state(BT_MESH_LPN_ENABLED);
+		} else {
+			lpn_set_state(BT_MESH_LPN_DISABLED);
+		}
+
+		return 0;
+	}
+
+	if (enable) {
+		lpn_set_state(BT_MESH_LPN_ENABLED);
+
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+			bt_mesh_scan_disable();
+		}
+
+		send_friend_req(lpn);
+	} else {
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_AUTO) &&
+		    lpn->state == BT_MESH_LPN_TIMER) {
+			k_delayed_work_cancel(&lpn->timer);
+			lpn_set_state(BT_MESH_LPN_DISABLED);
+		} else {
+			bt_mesh_lpn_disable(false);
+		}
+	}
+
+	return 0;
+}
+
+static void friend_response_received(struct bt_mesh_lpn *lpn)
+{
+	BT_DBG("lpn->sent_req 0x%02x", lpn->sent_req);
+
+	if (lpn->sent_req == TRANS_CTL_OP_FRIEND_POLL) {
+		lpn->fsn++;
+	}
+
+	k_delayed_work_cancel(&lpn->timer);
+	bt_mesh_scan_disable();
+	lpn_set_state(BT_MESH_LPN_ESTABLISHED);
+	lpn->req_attempts = 0;
+	lpn->sent_req = 0;
+}
+
+void bt_mesh_lpn_msg_received(struct bt_mesh_net_rx *rx)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	if (lpn->state == BT_MESH_LPN_TIMER) {
+		BT_DBG("Restarting establishment timer");
+		k_delayed_work_submit(&lpn->timer, LPN_AUTO_TIMEOUT);
+		return;
+	}
+
+	if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) {
+		BT_WARN("Unexpected message withouth a preceding Poll");
+		return;
+	}
+
+	friend_response_received(lpn);
+
+	BT_DBG("Requesting more messages from Friend");
+
+	send_friend_poll();
+}
+
+int bt_mesh_lpn_friend_offer(struct bt_mesh_net_rx *rx,
+			     struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_offer *msg = (void *)buf->om_data;
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	struct bt_mesh_subnet *sub = rx->sub;
+	struct friend_cred *cred;
+	u16_t frnd_counter;
+	int err;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Offer");
+		return -EINVAL;
+	}
+
+	if (lpn->state != BT_MESH_LPN_WAIT_OFFER) {
+		BT_WARN("Ignoring unexpected Friend Offer");
+		return 0;
+	}
+
+	if (!msg->recv_win) {
+		BT_WARN("Prohibited ReceiveWindow value");
+		return -EINVAL;
+	}
+
+	frnd_counter = sys_be16_to_cpu(msg->frnd_counter);
+
+	BT_DBG("recv_win %u queue_size %u sub_list_size %u rssi %d counter %u",
+	       msg->recv_win, msg->queue_size, msg->sub_list_size, msg->rssi,
+	       frnd_counter);
+
+	lpn->frnd = rx->ctx.addr;
+
+	cred = friend_cred_create(sub, lpn->frnd, lpn->counter, frnd_counter);
+	if (!cred) {
+		lpn->frnd = BT_MESH_ADDR_UNASSIGNED;
+		return -ENOMEM;
+	}
+
+	/* TODO: Add offer acceptance criteria check */
+
+	k_delayed_work_cancel(&lpn->timer);
+
+	lpn->recv_win = msg->recv_win;
+	lpn->queue_size = msg->queue_size;
+
+	err = send_friend_poll();
+	if (err) {
+		friend_cred_clear(cred);
+		lpn->frnd = BT_MESH_ADDR_UNASSIGNED;
+		lpn->recv_win = 0;
+		lpn->queue_size = 0;
+		return err;
+	}
+
+	lpn->counter++;
+
+	return 0;
+}
+
+int bt_mesh_lpn_friend_clear_cfm(struct bt_mesh_net_rx *rx,
+				 struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_clear_confirm *msg = (void *)buf->om_data;
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	u16_t addr, counter;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Clear Confirm");
+		return -EINVAL;
+	}
+
+	if (lpn->state != BT_MESH_LPN_CLEAR) {
+		BT_WARN("Ignoring unexpected Friend Clear Confirm");
+		return 0;
+	}
+
+	addr = sys_be16_to_cpu(msg->lpn_addr);
+	counter = sys_be16_to_cpu(msg->lpn_counter);
+
+	BT_DBG("LPNAddress 0x%04x LPNCounter 0x%04x", addr, counter);
+
+	if (addr != bt_mesh_primary_addr() || counter != lpn->counter) {
+		BT_WARN("Invalid parameters in Friend Clear Confirm");
+		return 0;
+	}
+
+	lpn->clear_success = 1;
+	clear_friendship(false, lpn->disable);
+
+	return 0;
+}
+
+static void lpn_group_add(u16_t group)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	u16_t *free_slot = NULL;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) {
+		if (lpn->groups[i] == group) {
+			atomic_clear_bit(lpn->to_remove, i);
+			return;
+		}
+
+		if (!free_slot && lpn->groups[i] == BT_MESH_ADDR_UNASSIGNED) {
+			free_slot = &lpn->groups[i];
+		}
+	}
+
+	if (!free_slot) {
+		BT_WARN("Friend Subscription List exceeded!");
+		return;
+	}
+
+	*free_slot = group;
+	lpn->groups_changed = 1;
+}
+
+static void lpn_group_del(u16_t group)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) {
+		if (lpn->groups[i] == group) {
+			if (atomic_test_bit(lpn->added, i) ||
+			    atomic_test_bit(lpn->pending, i)) {
+				atomic_set_bit(lpn->to_remove, i);
+				lpn->groups_changed = 1;
+			} else {
+				lpn->groups[i] = BT_MESH_ADDR_UNASSIGNED;
+			}
+		}
+	}
+}
+
+static inline int group_popcount(atomic_t *target)
+{
+#if CONFIG_BT_MESH_LPN_GROUPS > 32
+	int i, count = 0;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) {
+		count += popcount(atomic_get(&target[i]));
+	}
+#else
+	return popcount(atomic_get(target));
+#endif
+}
+
+static bool sub_update(u8_t op)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	int added_count = group_popcount(lpn->added);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx     = bt_mesh.sub[0].net_idx,
+		.app_idx     = BT_MESH_KEY_UNUSED,
+		.addr        = lpn->frnd,
+		.send_ttl    = 0,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub = &bt_mesh.sub[0],
+		.ctx = &ctx,
+		.src = bt_mesh_primary_addr(),
+		.xmit = POLL_XMIT,
+		.friend_cred = true,
+	};
+	struct bt_mesh_ctl_friend_sub req;
+	size_t i, g;
+
+	BT_DBG("op 0x%02x sent_req 0x%02x", op, lpn->sent_req);
+
+	if (lpn->sent_req) {
+		return false;
+	}
+
+	for (i = 0, g = 0; i < ARRAY_SIZE(lpn->groups); i++) {
+		if (lpn->groups[i] == BT_MESH_ADDR_UNASSIGNED) {
+			continue;
+		}
+
+		if (op == TRANS_CTL_OP_FRIEND_SUB_ADD) {
+			if (atomic_test_bit(lpn->added, i)) {
+				continue;
+			}
+		} else {
+			if (!atomic_test_bit(lpn->to_remove, i)) {
+				continue;
+			}
+		}
+
+		if (added_count + g >= lpn->queue_size) {
+			BT_WARN("Friend Queue Size exceeded");
+			break;
+		}
+
+		req.addr_list[g++] = sys_cpu_to_be16(lpn->groups[i]);
+		atomic_set_bit(lpn->pending, i);
+
+		if (g == ARRAY_SIZE(req.addr_list)) {
+			break;
+		}
+	}
+
+	if (g == 0) {
+		group_zero(lpn->pending);
+		return false;
+	}
+
+	req.xact = lpn->xact_next++;
+
+	if (bt_mesh_ctl_send(&tx, op, &req, 1 + g * 2, NULL,
+			     &req_sent_cb, NULL) < 0) {
+		group_zero(lpn->pending);
+		return false;
+	}
+
+	lpn->xact_pending = req.xact;
+	lpn->sent_req = op;
+	return true;
+}
+
+static void update_timeout(struct bt_mesh_lpn *lpn)
+{
+	if (lpn->established) {
+		BT_WARN("No response from Friend during ReceiveWindow");
+		bt_mesh_scan_disable();
+		lpn_set_state(BT_MESH_LPN_ESTABLISHED);
+		k_delayed_work_submit(&lpn->timer, POLL_RETRY_TIMEOUT);
+	} else {
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+			bt_mesh_scan_disable();
+		}
+
+		if (lpn->req_attempts < 6) {
+			BT_WARN("Retrying first Friend Poll");
+			lpn->sent_req = 0;
+			if (send_friend_poll() == 0) {
+				return;
+			}
+		}
+
+		BT_ERR("Timed out waiting for first Friend Update");
+		clear_friendship(false, false);
+	}
+}
+
+static void lpn_timeout(struct ble_npl_event *work)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER_LOG_LVL) == LOG_LEVEL_DEBUG
+	BT_DBG("state: %s", state2str(lpn->state));
+#endif
+
+	switch (lpn->state) {
+	case BT_MESH_LPN_DISABLED:
+		break;
+	case BT_MESH_LPN_CLEAR:
+		clear_friendship(false, bt_mesh.lpn.disable);
+		break;
+	case BT_MESH_LPN_TIMER:
+		BT_DBG("Starting to look for Friend nodes");
+		lpn_set_state(BT_MESH_LPN_ENABLED);
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+			bt_mesh_scan_disable();
+		}
+		/* fall through */
+	case BT_MESH_LPN_ENABLED:
+		send_friend_req(lpn);
+		break;
+	case BT_MESH_LPN_REQ_WAIT:
+		bt_mesh_scan_enable();
+		k_delayed_work_submit(&lpn->timer,
+				      lpn->adv_duration + FRIEND_REQ_SCAN);
+		lpn_set_state(BT_MESH_LPN_WAIT_OFFER);
+		break;
+	case BT_MESH_LPN_WAIT_OFFER:
+		BT_WARN("No acceptable Friend Offers received");
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+			bt_mesh_scan_disable();
+		}
+		lpn->counter++;
+		lpn_set_state(BT_MESH_LPN_ENABLED);
+		lpn->sent_req = 0U;
+		k_delayed_work_submit(&lpn->timer, FRIEND_REQ_RETRY_TIMEOUT);
+		break;
+	case BT_MESH_LPN_ESTABLISHED:
+		if (lpn->req_attempts < REQ_ATTEMPTS(lpn)) {
+			u8_t req = lpn->sent_req;
+
+			lpn->sent_req = 0;
+
+			if (!req || req == TRANS_CTL_OP_FRIEND_POLL) {
+				send_friend_poll();
+			} else {
+				sub_update(req);
+			}
+
+			break;
+		}
+
+		BT_ERR("No response from Friend after %u retries",
+		       lpn->req_attempts);
+		lpn->req_attempts = 0;
+		clear_friendship(false, false);
+		break;
+	case BT_MESH_LPN_RECV_DELAY:
+		k_delayed_work_submit(&lpn->timer,
+				      lpn->adv_duration + SCAN_LATENCY +
+				      lpn->recv_win);
+		bt_mesh_scan_enable();
+		lpn_set_state(BT_MESH_LPN_WAIT_UPDATE);
+		break;
+	case BT_MESH_LPN_WAIT_UPDATE:
+		update_timeout(lpn);
+		break;
+	default:
+		__ASSERT(0, "Unhandled LPN state");
+		break;
+	}
+}
+
+void bt_mesh_lpn_group_add(u16_t group)
+{
+	BT_DBG("group 0x%04x", group);
+
+	lpn_group_add(group);
+
+	if (!bt_mesh_lpn_established() || bt_mesh.lpn.sent_req) {
+		return;
+	}
+
+	sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD);
+}
+
+void bt_mesh_lpn_group_del(u16_t *groups, size_t group_count)
+{
+	int i;
+
+	for (i = 0; i < group_count; i++) {
+		if (groups[i] != BT_MESH_ADDR_UNASSIGNED) {
+			BT_DBG("group 0x%04x", groups[i]);
+			lpn_group_del(groups[i]);
+		}
+	}
+
+	if (!bt_mesh_lpn_established() || bt_mesh.lpn.sent_req) {
+		return;
+	}
+
+	sub_update(TRANS_CTL_OP_FRIEND_SUB_REM);
+}
+
+static s32_t poll_timeout(struct bt_mesh_lpn *lpn)
+{
+	/* If we're waiting for segment acks keep polling at high freq */
+	if (bt_mesh_tx_in_progress()) {
+		return min(POLL_TIMEOUT_MAX(lpn), K_SECONDS(1));
+	}
+
+	if (lpn->poll_timeout < POLL_TIMEOUT_MAX(lpn)) {
+		lpn->poll_timeout *= 2;
+		lpn->poll_timeout = min(lpn->poll_timeout,
+					POLL_TIMEOUT_MAX(lpn));
+	}
+
+	BT_DBG("Poll Timeout is %ums", (unsigned) lpn->poll_timeout);
+
+	return lpn->poll_timeout;
+}
+
+int bt_mesh_lpn_friend_sub_cfm(struct bt_mesh_net_rx *rx,
+			       struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_sub_confirm *msg = (void *)buf->om_data;
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Subscription Confirm");
+		return -EINVAL;
+	}
+
+	BT_DBG("xact 0x%02x", msg->xact);
+
+	if (!lpn->sent_req) {
+		BT_WARN("No pending subscription list message");
+		return 0;
+	}
+
+	if (msg->xact != lpn->xact_pending) {
+		BT_WARN("Transaction mismatch (0x%02x != 0x%02x)",
+			msg->xact, lpn->xact_pending);
+		return 0;
+	}
+
+	if (lpn->sent_req == TRANS_CTL_OP_FRIEND_SUB_ADD) {
+		group_set(lpn->added, lpn->pending);
+		group_zero(lpn->pending);
+	} else if (lpn->sent_req == TRANS_CTL_OP_FRIEND_SUB_REM) {
+		int i;
+
+		group_clear(lpn->added, lpn->pending);
+
+		for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) {
+			if (atomic_test_and_clear_bit(lpn->pending, i) &&
+			    atomic_test_and_clear_bit(lpn->to_remove, i)) {
+				lpn->groups[i] = BT_MESH_ADDR_UNASSIGNED;
+			}
+		}
+	} else {
+		BT_WARN("Unexpected Friend Subscription Confirm");
+		return 0;
+	}
+
+	friend_response_received(lpn);
+
+	if (lpn->groups_changed) {
+		sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD);
+		sub_update(TRANS_CTL_OP_FRIEND_SUB_REM);
+
+		if (!lpn->sent_req) {
+			lpn->groups_changed = 0;
+		}
+	}
+
+	if (lpn->pending_poll) {
+		send_friend_poll();
+	}
+
+	if (!lpn->sent_req) {
+		k_delayed_work_submit(&lpn->timer, poll_timeout(lpn));
+	}
+
+	return 0;
+}
+
+int bt_mesh_lpn_friend_update(struct bt_mesh_net_rx *rx,
+			      struct os_mbuf *buf)
+{
+	struct bt_mesh_ctl_friend_update *msg = (void *)buf->om_data;
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+	struct bt_mesh_subnet *sub = rx->sub;
+	u32_t iv_index;
+
+	if (buf->om_len < sizeof(*msg)) {
+		BT_WARN("Too short Friend Update");
+		return -EINVAL;
+	}
+
+	if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) {
+		BT_WARN("Unexpected friend update");
+		return 0;
+	}
+
+	if (sub->kr_phase == BT_MESH_KR_PHASE_2 && !rx->new_key) {
+		BT_WARN("Ignoring Phase 2 KR Update secured using old key");
+		return 0;
+	}
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_INITIATOR) &&
+	    (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS) ==
+	     BT_MESH_IV_UPDATE(msg->flags))) {
+		bt_mesh_beacon_ivu_initiator(false);
+	}
+
+	if (!lpn->established) {
+		struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
+
+		/* This is normally checked on the transport layer, however
+		 * in this state we're also still accepting master
+		 * credentials so we need to ensure the right ones (Friend
+		 * Credentials) were used for this message.
+		 */
+		if (!rx->friend_cred) {
+			BT_WARN("Friend Update with wrong credentials");
+			return -EINVAL;
+		}
+
+		lpn->established = 1;
+
+		BT_INFO("Friendship established with 0x%04x", lpn->frnd);
+
+		if (cfg->hb_pub.feat & BT_MESH_FEAT_LOW_POWER) {
+			bt_mesh_heartbeat_send();
+		}
+
+		if (lpn_cb) {
+			lpn_cb(lpn->frnd, true);
+		}
+
+		/* Set initial poll timeout */
+		lpn->poll_timeout = min(POLL_TIMEOUT_MAX(lpn),
+					POLL_TIMEOUT_INIT);
+	}
+
+	friend_response_received(lpn);
+
+	iv_index = sys_be32_to_cpu(msg->iv_index);
+
+	BT_DBG("flags 0x%02x iv_index 0x%08x md %u", msg->flags,
+	       (unsigned) iv_index, msg->md);
+
+	if (bt_mesh_kr_update(sub, BT_MESH_KEY_REFRESH(msg->flags),
+			      rx->new_key)) {
+		bt_mesh_net_beacon_update(sub);
+	}
+
+	bt_mesh_net_iv_update(iv_index, BT_MESH_IV_UPDATE(msg->flags));
+
+	if (lpn->groups_changed) {
+		sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD);
+		sub_update(TRANS_CTL_OP_FRIEND_SUB_REM);
+
+		if (!lpn->sent_req) {
+			lpn->groups_changed = 0;
+		}
+	}
+
+	if (msg->md) {
+		BT_DBG("Requesting for more messages");
+		send_friend_poll();
+	}
+
+	if (!lpn->sent_req) {
+		k_delayed_work_submit(&lpn->timer, poll_timeout(lpn));
+	}
+
+	return 0;
+}
+
+int bt_mesh_lpn_poll(void)
+{
+	if (!bt_mesh.lpn.established) {
+		return -EAGAIN;
+	}
+
+	BT_DBG("Requesting more messages");
+
+	return send_friend_poll();
+}
+
+void bt_mesh_lpn_set_cb(void (*cb)(u16_t friend_addr, bool established))
+{
+	lpn_cb = cb;
+}
+
+int bt_mesh_lpn_init(void)
+{
+	struct bt_mesh_lpn *lpn = &bt_mesh.lpn;
+
+	BT_DBG("");
+
+	k_delayed_work_init(&lpn->timer, lpn_timeout);
+
+	if (lpn->state == BT_MESH_LPN_ENABLED) {
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_ESTABLISHMENT)) {
+			bt_mesh_scan_disable();
+		} else {
+			bt_mesh_scan_enable();
+		}
+
+		send_friend_req(lpn);
+	} else {
+		bt_mesh_scan_enable();
+
+		if (IS_ENABLED(CONFIG_BT_MESH_LPN_AUTO)) {
+			BT_DBG("Waiting %u ms for messages", LPN_AUTO_TIMEOUT);
+			lpn_set_state(BT_MESH_LPN_TIMER);
+			k_delayed_work_submit(&lpn->timer, LPN_AUTO_TIMEOUT);
+		}
+	}
+
+	return 0;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_LOW_POWER) */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/lpn.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/lpn.h
new file mode 100644
index 00000000..0ff6c9cf
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/lpn.h
@@ -0,0 +1,68 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __LPN_H__
+#define __LPN_H__
+
+#include "mesh/mesh.h"
+
+int bt_mesh_lpn_friend_update(struct bt_mesh_net_rx *rx,
+			      struct os_mbuf *buf);
+int bt_mesh_lpn_friend_offer(struct bt_mesh_net_rx *rx,
+			     struct os_mbuf *buf);
+int bt_mesh_lpn_friend_clear_cfm(struct bt_mesh_net_rx *rx,
+				 struct os_mbuf *buf);
+int bt_mesh_lpn_friend_sub_cfm(struct bt_mesh_net_rx *rx,
+			       struct os_mbuf *buf);
+
+static inline bool bt_mesh_lpn_established(void)
+{
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+	return bt_mesh.lpn.established;
+#else
+	return false;
+#endif
+}
+
+static inline bool bt_mesh_lpn_match(u16_t addr)
+{
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+	if (bt_mesh_lpn_established()) {
+		return (addr == bt_mesh.lpn.frnd);
+	}
+#endif
+	return false;
+}
+
+static inline bool bt_mesh_lpn_waiting_update(void)
+{
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+	return (bt_mesh.lpn.state == BT_MESH_LPN_WAIT_UPDATE);
+#else
+	return false;
+#endif
+}
+
+static inline bool bt_mesh_lpn_timer(void)
+{
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER) && MYNEWT_VAL(BLE_MESH_LPN_AUTO)
+	return (bt_mesh.lpn.state == BT_MESH_LPN_TIMER);
+#else
+	return false;
+#endif
+}
+
+void bt_mesh_lpn_msg_received(struct bt_mesh_net_rx *rx);
+
+void bt_mesh_lpn_group_add(u16_t group);
+void bt_mesh_lpn_group_del(u16_t *groups, size_t group_count);
+
+void bt_mesh_lpn_disable(bool force);
+
+int bt_mesh_lpn_init(void);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/mesh.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/mesh.c
new file mode 100644
index 00000000..52fbdbf6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/mesh.c
@@ -0,0 +1,361 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_LOG
+
+#include <stdbool.h>
+#include <errno.h>
+
+#include "os/os_mbuf.h"
+#include "mesh/mesh.h"
+#include "host/ble_uuid.h"
+
+#include "adv.h"
+#include "prov.h"
+#include "net.h"
+#include "beacon.h"
+#include "lpn.h"
+#include "friend.h"
+#include "transport.h"
+#include "access.h"
+#include "foundation.h"
+#include "proxy.h"
+#include "shell.h"
+#include "mesh_priv.h"
+#include "settings.h"
+
+u8_t g_mesh_addr_type;
+static struct ble_gap_event_listener mesh_event_listener;
+
+int bt_mesh_provision(const u8_t net_key[16], u16_t net_idx,
+		      u8_t flags, u32_t iv_index, u16_t addr,
+		      const u8_t dev_key[16])
+{
+	bool pb_gatt_enabled;
+	int err;
+
+	BT_INFO("Primary Element: 0x%04x", addr);
+	BT_DBG("net_idx 0x%04x flags 0x%02x iv_index 0x%04x",
+	       net_idx, flags, (unsigned) iv_index);
+
+	if (atomic_test_and_set_bit(bt_mesh.flags, BT_MESH_VALID)) {
+		return -EALREADY;
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_PB_GATT))) {
+		if (bt_mesh_proxy_prov_disable(false) == 0) {
+			pb_gatt_enabled = true;
+		} else {
+			pb_gatt_enabled = false;
+		}
+	} else {
+		pb_gatt_enabled = false;
+	}
+
+	err = bt_mesh_net_create(net_idx, flags, net_key, iv_index);
+	if (err) {
+		atomic_clear_bit(bt_mesh.flags, BT_MESH_VALID);
+
+		if (MYNEWT_VAL(BLE_MESH_PB_GATT)  && pb_gatt_enabled) {
+			bt_mesh_proxy_prov_enable();
+		}
+
+		return err;
+	}
+
+	bt_mesh.seq = 0;
+
+	bt_mesh_comp_provision(addr);
+
+	memcpy(bt_mesh.dev_key, dev_key, 16);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		BT_DBG("Storing network information persistently");
+		bt_mesh_store_net();
+		bt_mesh_store_subnet(&bt_mesh.sub[0]);
+		bt_mesh_store_iv(false);
+	}
+
+	bt_mesh_net_start();
+
+	return 0;
+}
+
+int bt_mesh_provision_adv(const u8_t uuid[16], u16_t net_idx, u16_t addr,
+			  u8_t attention_duration)
+{
+	if (!atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+		return -EINVAL;
+	}
+
+	if (bt_mesh_subnet_get(net_idx) == NULL) {
+		return -EINVAL;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER) &&
+	    IS_ENABLED(CONFIG_BT_MESH_PB_ADV)) {
+		return bt_mesh_pb_adv_open(uuid, net_idx, addr,
+					   attention_duration);
+	}
+
+	return -ENOTSUP;
+}
+
+void bt_mesh_reset(void)
+{
+	if (!atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+		return;
+	}
+
+	bt_mesh.iv_index = 0U;
+	bt_mesh.seq = 0U;
+
+	memset(bt_mesh.flags, 0, sizeof(bt_mesh.flags));
+
+	k_delayed_work_cancel(&bt_mesh.ivu_timer);
+
+	bt_mesh_cfg_reset();
+
+	bt_mesh_rx_reset();
+	bt_mesh_tx_reset();
+
+	if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) {
+		bt_mesh_lpn_disable(true);
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_FRIEND))) {
+		bt_mesh_friend_clear_net_idx(BT_MESH_KEY_ANY);
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
+		bt_mesh_proxy_gatt_disable();
+	}
+
+	if ((MYNEWT_VAL(BLE_MESH_PB_GATT))) {
+		bt_mesh_proxy_prov_enable();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_clear_net();
+	}
+
+	memset(bt_mesh.dev_key, 0, sizeof(bt_mesh.dev_key));
+
+	bt_mesh_scan_disable();
+	bt_mesh_beacon_disable();
+
+	bt_mesh_comp_unprovision();
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROV)) {
+		bt_mesh_prov_reset();
+	}
+}
+
+bool bt_mesh_is_provisioned(void)
+{
+	return atomic_test_bit(bt_mesh.flags, BT_MESH_VALID);
+}
+
+int bt_mesh_prov_enable(bt_mesh_prov_bearer_t bearers)
+{
+	if (bt_mesh_is_provisioned()) {
+		return -EALREADY;
+	}
+
+	char uuid_buf[BLE_UUID_STR_LEN];
+	const struct bt_mesh_prov *prov = bt_mesh_prov_get();
+	ble_uuid_t *uuid = BLE_UUID128_DECLARE();
+
+	memcpy(BLE_UUID128(uuid)->value, prov->uuid, 16);
+	BT_INFO("Device UUID: %s", ble_uuid_to_str(uuid, uuid_buf));
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PB_ADV) &&
+	    (bearers & BT_MESH_PROV_ADV)) {
+		/* Make sure we're scanning for provisioning inviations */
+		bt_mesh_scan_enable();
+		/* Enable unprovisioned beacon sending */
+		bt_mesh_beacon_enable();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PB_GATT) &&
+	    (bearers & BT_MESH_PROV_GATT)) {
+		bt_mesh_proxy_prov_enable();
+		bt_mesh_adv_update();
+	}
+
+	return 0;
+}
+
+int bt_mesh_prov_disable(bt_mesh_prov_bearer_t bearers)
+{
+	if (bt_mesh_is_provisioned()) {
+		return -EALREADY;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PB_ADV) &&
+	    (bearers & BT_MESH_PROV_ADV)) {
+		bt_mesh_beacon_disable();
+		bt_mesh_scan_disable();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PB_GATT) &&
+	    (bearers & BT_MESH_PROV_GATT)) {
+		bt_mesh_proxy_prov_disable(true);
+	}
+
+	return 0;
+}
+
+static int bt_mesh_gap_event(struct ble_gap_event *event, void *arg)
+{
+	ble_adv_gap_mesh_cb(event, arg);
+
+#if (MYNEWT_VAL(BLE_MESH_PROXY))
+	ble_mesh_proxy_gap_event(event, arg);
+#endif
+
+	return 0;
+}
+
+static void model_suspend(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
+			  bool vnd, bool primary, void *user_data)
+{
+	if (mod->pub && mod->pub->update) {
+		mod->pub->count = 0;
+		k_delayed_work_cancel(&mod->pub->timer);
+	}
+}
+
+int bt_mesh_suspend(void)
+{
+	int err;
+
+	if (!atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+		return -EINVAL;
+	}
+
+	if (atomic_test_and_set_bit(bt_mesh.flags, BT_MESH_SUSPENDED)) {
+		return -EALREADY;
+	}
+
+	err = bt_mesh_scan_disable();
+	if (err) {
+		atomic_clear_bit(bt_mesh.flags, BT_MESH_SUSPENDED);
+		BT_WARN("Disabling scanning failed (err %d)", err);
+		return err;
+	}
+
+	bt_mesh_hb_pub_disable();
+
+	if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED) {
+		bt_mesh_beacon_disable();
+	}
+
+	bt_mesh_model_foreach(model_suspend, NULL);
+
+	return 0;
+}
+
+static void model_resume(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
+			  bool vnd, bool primary, void *user_data)
+{
+	if (mod->pub && mod->pub->update) {
+		s32_t period_ms = bt_mesh_model_pub_period_get(mod);
+
+		if (period_ms) {
+			k_delayed_work_submit(&mod->pub->timer, period_ms);
+		}
+	}
+}
+
+int bt_mesh_resume(void)
+{
+	int err;
+
+	if (!atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+		return -EINVAL;
+	}
+
+	if (!atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_SUSPENDED)) {
+		return -EALREADY;
+	}
+
+	err = bt_mesh_scan_enable();
+	if (err) {
+		BT_WARN("Re-enabling scanning failed (err %d)", err);
+		atomic_set_bit(bt_mesh.flags, BT_MESH_SUSPENDED);
+		return err;
+	}
+
+	if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED) {
+		bt_mesh_beacon_enable();
+	}
+
+	bt_mesh_model_foreach(model_resume, NULL);
+
+	return err;
+}
+
+int bt_mesh_init(uint8_t own_addr_type, const struct bt_mesh_prov *prov,
+		 const struct bt_mesh_comp *comp)
+{
+	int err;
+
+	g_mesh_addr_type = own_addr_type;
+
+	/* initialize SM alg ECC subsystem (it is used directly from mesh code) */
+	ble_sm_alg_ecc_init();
+
+	err = bt_mesh_comp_register(comp);
+	if (err) {
+		return err;
+	}
+
+#if (MYNEWT_VAL(BLE_MESH_PROV))
+	err = bt_mesh_prov_init(prov);
+	if (err) {
+		return err;
+	}
+#endif
+
+#if (MYNEWT_VAL(BLE_MESH_PROXY))
+	bt_mesh_proxy_init();
+#endif
+
+#if (MYNEWT_VAL(BLE_MESH_PROV))
+	/* Need this to proper link.rx.buf allocation */
+	bt_mesh_prov_reset_link();
+#endif
+
+	bt_mesh_net_init();
+	bt_mesh_trans_init();
+	bt_mesh_beacon_init();
+	bt_mesh_adv_init();
+
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+	/* Make sure we're scanning for provisioning inviations */
+	bt_mesh_scan_enable();
+	/* Enable unprovisioned beacon sending */
+
+	bt_mesh_beacon_enable();
+#endif
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	bt_mesh_proxy_prov_enable();
+#endif
+
+	ble_gap_event_listener_register(&mesh_event_listener,
+					bt_mesh_gap_event, NULL);
+
+#if (MYNEWT_VAL(BLE_MESH_SETTINGS))
+	bt_mesh_settings_init();
+#endif
+
+	return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/mesh_priv.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/mesh_priv.h
new file mode 100644
index 00000000..f09bb230
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/mesh_priv.h
@@ -0,0 +1,39 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef __MESH_PRIV_H
+#define __MESH_PRIV_H
+
+#define BT_MESH_KEY_PRIMARY 0x0000
+#define BT_MESH_KEY_ANY     0xffff
+
+#define BT_MESH_ADDR_IS_UNICAST(addr) ((addr) && (addr) < 0x8000)
+#define BT_MESH_ADDR_IS_GROUP(addr) ((addr) >= 0xc000 && (addr) <= 0xff00)
+#define BT_MESH_ADDR_IS_VIRTUAL(addr) ((addr) >= 0x8000 && (addr) < 0xc000)
+#define BT_MESH_ADDR_IS_RFU(addr) ((addr) >= 0xff00 && (addr) <= 0xfffb)
+struct bt_mesh_net;
+
+#define OP_GEN_ONOFF_GET		BT_MESH_MODEL_OP_2(0x82, 0x01)
+#define OP_GEN_ONOFF_SET		BT_MESH_MODEL_OP_2(0x82, 0x02)
+#define OP_GEN_ONOFF_SET_UNACK		BT_MESH_MODEL_OP_2(0x82, 0x03)
+#define OP_GEN_ONOFF_STATUS		BT_MESH_MODEL_OP_2(0x82, 0x04)
+#define OP_GEN_LEVEL_GET		BT_MESH_MODEL_OP_2(0x82, 0x05)
+#define OP_GEN_LEVEL_SET		BT_MESH_MODEL_OP_2(0x82, 0x06)
+#define OP_GEN_LEVEL_SET_UNACK		BT_MESH_MODEL_OP_2(0x82, 0x07)
+#define OP_GEN_LEVEL_STATUS		BT_MESH_MODEL_OP_2(0x82, 0x08)
+#define OP_GEN_DELTA_SET		BT_MESH_MODEL_OP_2(0x82, 0x09)
+#define OP_GEN_DELTA_SET_UNACK		BT_MESH_MODEL_OP_2(0x82, 0x0a)
+#define OP_GEN_MOVE_SET			BT_MESH_MODEL_OP_2(0x82, 0x0b)
+#define OP_GEN_MOVE_SET_UNACK		BT_MESH_MODEL_OP_2(0x82, 0x0c)
+#define OP_LIGHT_LIGHTNESS_GET		BT_MESH_MODEL_OP_2(0x82, 0x4b)
+#define OP_LIGHT_LIGHTNESS_SET		BT_MESH_MODEL_OP_2(0x82, 0x4c)
+#define OP_LIGHT_LIGHTNESS_SET_UNACK	BT_MESH_MODEL_OP_2(0x82, 0x4d)
+#define OP_LIGHT_LIGHTNESS_STATUS	BT_MESH_MODEL_OP_2(0x82, 0x4e)
+
+bool bt_mesh_is_provisioned(void);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/model_cli.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/model_cli.c
new file mode 100644
index 00000000..b00cfa52
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/model_cli.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_MODEL_LOG
+
+#include "mesh/mesh.h"
+#include "mesh/model_cli.h"
+#include "mesh_priv.h"
+
+static s32_t msg_timeout = K_SECONDS(5);
+
+static struct bt_mesh_gen_model_cli *gen_onoff_cli;
+static struct bt_mesh_gen_model_cli *gen_level_cli;
+
+static u8_t transaction_id = 0;
+
+struct gen_onoff_param {
+    u8_t *state;
+};
+
+struct gen_level_param {
+    s16_t *level;
+};
+
+static void gen_onoff_status(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx,
+			     struct os_mbuf *buf)
+{
+	struct bt_mesh_gen_model_cli *cli = model->user_data;
+	struct gen_onoff_param *param;
+	u8_t state;
+
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_GEN_ONOFF_STATUS) {
+		BT_WARN("Unexpected Generic OnOff Status message");
+		return;
+	}
+
+	param = cli->op_param;
+
+	state = net_buf_simple_pull_u8(buf);
+	if (param->state) {
+		*param->state = state;
+	}
+
+	BT_DBG("state: %d", state);
+
+	k_sem_give(&cli->op_sync);
+}
+
+static void gen_level_status(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx,
+			     struct os_mbuf *buf)
+{
+	struct bt_mesh_gen_model_cli *cli = model->user_data;
+	struct gen_level_param *param;
+	s16_t level;
+
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s",
+	       ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	if (cli->op_pending != OP_GEN_LEVEL_STATUS) {
+		BT_WARN("Unexpected Generic LEVEL Status message");
+		return;
+	}
+
+	param = cli->op_param;
+
+	level = net_buf_simple_pull_le16(buf);
+	if (param->level) {
+		*param->level = level;
+	}
+
+	BT_DBG("level: %d", level);
+
+	k_sem_give(&cli->op_sync);
+}
+
+const struct bt_mesh_model_op gen_onoff_cli_op[] = {
+	{ OP_GEN_ONOFF_STATUS, 1, gen_onoff_status },
+	BT_MESH_MODEL_OP_END,
+};
+
+static int onoff_cli_init(struct bt_mesh_model *model)
+{
+	BT_DBG("");
+
+	if (!model->user_data) {
+		BT_ERR("No Generic OnOff Client context provided");
+		return -EINVAL;
+	}
+
+	gen_onoff_cli = model->user_data;
+	gen_onoff_cli->model = model;
+
+	k_sem_init(&gen_onoff_cli->op_sync, 0, 1);
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb bt_mesh_gen_onoff_cli_cb = {
+	.init = onoff_cli_init,
+};
+
+const struct bt_mesh_model_op gen_level_cli_op[] = {
+	{ OP_GEN_LEVEL_STATUS, 2, gen_level_status },
+	BT_MESH_MODEL_OP_END,
+};
+
+static int level_cli_init(struct bt_mesh_model *model)
+{
+	BT_DBG("");
+
+	if (!model->user_data) {
+		BT_ERR("No Generic Level Client context provided");
+		return -EINVAL;
+	}
+
+	gen_level_cli = model->user_data;
+	gen_level_cli->model = model;
+
+	k_sem_init(&gen_level_cli->op_sync, 0, 1);
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb bt_mesh_gen_level_cli_cb = {
+	.init = level_cli_init,
+};
+
+static int cli_wait(struct bt_mesh_gen_model_cli *cli, void *param, u32_t op)
+{
+	int err;
+
+	BT_DBG("");
+
+	cli->op_param = param;
+	cli->op_pending = op;
+
+	err = k_sem_take(&cli->op_sync, msg_timeout);
+
+	cli->op_pending = 0;
+	cli->op_param = NULL;
+
+	return err;
+}
+
+int bt_mesh_gen_onoff_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  u8_t *state)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 0 + 4);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct gen_onoff_param param = {
+		.state = state,
+	};
+	int err;
+
+	bt_mesh_model_msg_init(msg, OP_GEN_ONOFF_GET);
+
+	err = bt_mesh_model_send(gen_onoff_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		goto done;
+	}
+
+	err = cli_wait(gen_onoff_cli, &param, OP_GEN_ONOFF_STATUS);
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_gen_onoff_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  u8_t val, u8_t *state)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 2 + 4);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct gen_onoff_param param = {
+		.state = state,
+	};
+	int err;
+
+	if (state) {
+		bt_mesh_model_msg_init(msg, OP_GEN_ONOFF_SET);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_GEN_ONOFF_SET_UNACK);
+	}
+
+	net_buf_simple_add_u8(msg, val);
+	net_buf_simple_add_u8(msg, transaction_id);
+
+	err = bt_mesh_model_send(gen_onoff_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		goto done;
+	}
+
+	if (!state) {
+		goto done;
+	}
+
+	err = cli_wait(gen_onoff_cli, &param, OP_GEN_ONOFF_STATUS);
+done:
+	if (err == 0) {
+		transaction_id++;
+	}
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_gen_level_get(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  s16_t *level)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 0 + 4);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct gen_level_param param = {
+		.level = level,
+	};
+	int err;
+
+	bt_mesh_model_msg_init(msg, OP_GEN_LEVEL_GET);
+
+	err = bt_mesh_model_send(gen_level_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		goto done;
+	}
+
+	err = cli_wait(gen_level_cli, &param, OP_GEN_LEVEL_STATUS);
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+int bt_mesh_gen_level_set(u16_t net_idx, u16_t addr, u16_t app_idx,
+			  s16_t val, s16_t *state)
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 3 + 4);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net_idx,
+		.app_idx = app_idx,
+		.addr = addr,
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct gen_level_param param = {
+		.level = state,
+	};
+	int err;
+
+	if (state) {
+		bt_mesh_model_msg_init(msg, OP_GEN_LEVEL_SET);
+	} else {
+		bt_mesh_model_msg_init(msg, OP_GEN_LEVEL_SET_UNACK);
+	}
+
+	net_buf_simple_add_le16(msg, val);
+	net_buf_simple_add_u8(msg, transaction_id);
+
+	err = bt_mesh_model_send(gen_level_cli->model, &ctx, msg, NULL, NULL);
+	if (err) {
+		BT_ERR("model_send() failed (err %d)", err);
+		goto done;
+	}
+
+	if (!state) {
+		goto done;
+	}
+
+	err = cli_wait(gen_level_cli, &param, OP_GEN_LEVEL_STATUS);
+done:
+	if (err == 0) {
+		transaction_id++;
+	}
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/model_srv.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/model_srv.c
new file mode 100644
index 00000000..5f5a8df4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/model_srv.c
@@ -0,0 +1,266 @@
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_MODEL_LOG
+
+#include "mesh/mesh.h"
+#include "mesh/model_srv.h"
+#include "mesh_priv.h"
+
+static struct bt_mesh_gen_onoff_srv *gen_onoff_srv;
+static struct bt_mesh_gen_level_srv *gen_level_srv;
+static struct bt_mesh_light_lightness_srv *light_lightness_srv;
+
+static void gen_onoff_status(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx)
+{
+	struct bt_mesh_gen_onoff_srv *cb = model->user_data;
+	struct os_mbuf *msg = NET_BUF_SIMPLE(3);
+	u8_t *state;
+
+	bt_mesh_model_msg_init(msg, OP_GEN_ONOFF_STATUS);
+	state = net_buf_simple_add(msg, 1);
+	if (cb && cb->get) {
+		cb->get(model, state);
+	}
+
+	BT_DBG("state: %d", *state);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Send status failed");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void gen_onoff_get(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	gen_onoff_status(model, ctx);
+}
+
+static void gen_onoff_set_unack(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx,
+				struct os_mbuf *buf)
+{
+	struct bt_mesh_gen_onoff_srv *cb = model->user_data;
+	u8_t state;
+
+	state = buf->om_data[0];
+
+	BT_DBG("state: %d", state);
+
+	if (cb && cb->set) {
+		cb->set(model, state);
+	}
+}
+
+static void gen_onoff_set(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	gen_onoff_set_unack(model, ctx, buf);
+	gen_onoff_status(model, ctx);
+}
+
+static void gen_level_status(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx)
+{
+	struct bt_mesh_gen_level_srv *cb = model->user_data;
+	struct os_mbuf *msg = NET_BUF_SIMPLE(4);
+	s16_t *level;
+
+	bt_mesh_model_msg_init(msg, OP_GEN_LEVEL_STATUS);
+	level = net_buf_simple_add(msg, 2);
+	if (cb && cb->get) {
+		cb->get(model, level);
+	}
+
+	BT_DBG("level: %d", *level);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Send status failed");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void gen_level_get(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	gen_level_status(model, ctx);
+}
+
+static void gen_level_set_unack(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx,
+				struct os_mbuf *buf) {
+	struct bt_mesh_gen_level_srv *cb = model->user_data;
+	s16_t level;
+
+	level = (s16_t) net_buf_simple_pull_le16(buf);
+	BT_DBG("level: %d", level);
+
+	if (cb && cb->set) {
+		cb->set(model, level);
+	}
+}
+
+static void gen_level_set(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	gen_level_set_unack(model, ctx, buf);
+	gen_level_status(model, ctx);
+}
+
+static void light_lightness_status(struct bt_mesh_model *model,
+			     struct bt_mesh_msg_ctx *ctx)
+{
+	struct bt_mesh_light_lightness_srv *cb = model->user_data;
+	struct os_mbuf *msg = NET_BUF_SIMPLE(4);
+	s16_t *lightness;
+
+	bt_mesh_model_msg_init(msg, OP_LIGHT_LIGHTNESS_STATUS);
+	lightness = net_buf_simple_add(msg, 2);
+	if (cb && cb->get) {
+		cb->get(model, lightness);
+	}
+
+	BT_DBG("lightness: %d", *lightness);
+
+	if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) {
+		BT_ERR("Send status failed");
+	}
+
+	os_mbuf_free_chain(msg);
+}
+
+static void light_lightness_get(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	BT_DBG("");
+
+	light_lightness_status(model, ctx);
+}
+
+static void light_lightness_set_unack(struct bt_mesh_model *model,
+				struct bt_mesh_msg_ctx *ctx,
+				struct os_mbuf *buf) {
+	struct bt_mesh_light_lightness_srv *cb = model->user_data;
+	s16_t lightness;
+
+	lightness = (s16_t) net_buf_simple_pull_le16(buf);
+	BT_DBG("lightness: %d", lightness);
+
+	if (cb && cb->set) {
+		cb->set(model, lightness);
+	}
+}
+
+static void light_lightness_set(struct bt_mesh_model *model,
+			  struct bt_mesh_msg_ctx *ctx,
+			  struct os_mbuf *buf)
+{
+	light_lightness_set_unack(model, ctx, buf);
+	light_lightness_status(model, ctx);
+}
+
+const struct bt_mesh_model_op gen_onoff_srv_op[] = {
+	{ OP_GEN_ONOFF_GET, 		0, gen_onoff_get },
+	{ OP_GEN_ONOFF_SET, 		2, gen_onoff_set },
+	{ OP_GEN_ONOFF_SET_UNACK, 	2, gen_onoff_set_unack },
+	BT_MESH_MODEL_OP_END,
+};
+
+const struct bt_mesh_model_op gen_level_srv_op[] = {
+	{ OP_GEN_LEVEL_GET, 		0, gen_level_get },
+	{ OP_GEN_LEVEL_SET, 		3, gen_level_set },
+	{ OP_GEN_LEVEL_SET_UNACK, 	3, gen_level_set_unack },
+	BT_MESH_MODEL_OP_END,
+};
+
+const struct bt_mesh_model_op light_lightness_srv_op[] = {
+	{ OP_LIGHT_LIGHTNESS_GET, 		0, light_lightness_get },
+	{ OP_LIGHT_LIGHTNESS_SET, 		3, light_lightness_set },
+	{ OP_LIGHT_LIGHTNESS_SET_UNACK, 	3, light_lightness_set_unack },
+	BT_MESH_MODEL_OP_END,
+};
+
+static int onoff_srv_init(struct bt_mesh_model *model)
+{
+	struct bt_mesh_gen_onoff_srv *cfg = model->user_data;
+
+	BT_DBG("");
+
+	if (!cfg) {
+		BT_ERR("No Generic OnOff Server context provided");
+		return -EINVAL;
+	}
+
+	cfg->model = model;
+
+	gen_onoff_srv = cfg;
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb gen_onoff_srv_cb = {
+	.init = onoff_srv_init,
+};
+
+static int level_srv_init(struct bt_mesh_model *model)
+{
+	struct bt_mesh_gen_level_srv *cfg = model->user_data;
+
+	BT_DBG("");
+
+	if (!cfg) {
+		BT_ERR("No Generic Level Server context provided");
+		return -EINVAL;
+	}
+
+	cfg->model = model;
+
+	gen_level_srv = cfg;
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb gen_level_srv_cb = {
+	.init = level_srv_init,
+};
+
+static int lightness_srv_init(struct bt_mesh_model *model)
+{
+	struct bt_mesh_light_lightness_srv *cfg = model->user_data;
+
+	BT_DBG("");
+
+	if (!cfg) {
+		BT_ERR("No Light Lightness Server context provided");
+		return -EINVAL;
+	}
+
+	cfg->model = model;
+
+	light_lightness_srv = cfg;
+
+	return 0;
+}
+
+const struct bt_mesh_model_cb light_lightness_srv_cb = {
+	.init = lightness_srv_init,
+};
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/net.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/net.c
new file mode 100644
index 00000000..240314d4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/net.c
@@ -0,0 +1,1433 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_NET_LOG
+
+#include <string.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include "os/os_mbuf.h"
+#include "mesh/mesh.h"
+
+#include "crypto.h"
+#include "adv.h"
+#include "mesh_priv.h"
+#include "net.h"
+#include "lpn.h"
+#include "friend.h"
+#include "proxy.h"
+#include "transport.h"
+#include "access.h"
+#include "foundation.h"
+#include "beacon.h"
+#include "settings.h"
+#include "prov.h"
+
+/* Minimum valid Mesh Network PDU length. The Network headers
+ * themselves take up 9 bytes. After that there is a minumum of 1 byte
+ * payload for both CTL=1 and CTL=0 PDUs (smallest OpCode is 1 byte). CTL=1
+ * PDUs must use a 64-bit (8 byte) NetMIC, whereas CTL=0 PDUs have at least
+ * a 32-bit (4 byte) NetMIC and AppMIC giving again a total of 8 bytes.
+ */
+#define BT_MESH_NET_MIN_PDU_LEN (BT_MESH_NET_HDR_LEN + 1 + 8)
+
+/* Seq limit after IV Update is triggered */
+#define IV_UPDATE_SEQ_LIMIT 8000000
+
+#define IVI(pdu)           ((pdu)[0] >> 7)
+#define NID(pdu)           ((pdu)[0] & 0x7f)
+#define CTL(pdu)           ((pdu)[1] >> 7)
+#define TTL(pdu)           ((pdu)[1] & 0x7f)
+#define SEQ(pdu)           (((u32_t)(pdu)[2] << 16) | \
+			    ((u32_t)(pdu)[3] << 8) | (u32_t)(pdu)[4]);
+#define SRC(pdu)           (sys_get_be16(&(pdu)[5]))
+#define DST(pdu)           (sys_get_be16(&(pdu)[7]))
+
+/* Determine how many friendship credentials we need */
+#if (MYNEWT_VAL(BLE_MESH_FRIEND))
+#define FRIEND_CRED_COUNT MYNEWT_VAL(BLE_MESH_FRIEND_LPN_COUNT)
+#elif (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+#define FRIEND_CRED_COUNT MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)
+#else
+#define FRIEND_CRED_COUNT 0
+#endif
+
+static struct friend_cred friend_cred[FRIEND_CRED_COUNT];
+
+static u64_t msg_cache[MYNEWT_VAL(BLE_MESH_MSG_CACHE_SIZE)];
+static u16_t msg_cache_next;
+
+/* Singleton network context (the implementation only supports one) */
+struct bt_mesh_net bt_mesh = {
+	.local_queue = STAILQ_HEAD_INITIALIZER(bt_mesh.local_queue),
+	.sub = {
+		[0 ... (MYNEWT_VAL(BLE_MESH_SUBNET_COUNT) - 1)] = {
+			.net_idx = BT_MESH_KEY_UNUSED,
+		}
+	},
+	.app_keys = {
+		[0 ... (MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT) - 1)] = {
+			.net_idx = BT_MESH_KEY_UNUSED,
+		}
+	},
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+	.nodes = {
+		[0 ... (CONFIG_BT_MESH_NODE_COUNT - 1)] = {
+			.net_idx = BT_MESH_KEY_UNUSED,
+		}
+	},
+#endif
+};
+
+static u32_t dup_cache[4];
+static int   dup_cache_next;
+
+static bool check_dup(struct os_mbuf *data)
+{
+	const u8_t *tail = net_buf_simple_tail(data);
+	u32_t val;
+	int i;
+
+	val = sys_get_be32(tail - 4) ^ sys_get_be32(tail - 8);
+
+	for (i = 0; i < ARRAY_SIZE(dup_cache); i++) {
+		if (dup_cache[i] == val) {
+			return true;
+		}
+	}
+
+	dup_cache[dup_cache_next++] = val;
+	dup_cache_next %= ARRAY_SIZE(dup_cache);
+
+	return false;
+}
+
+static u64_t msg_hash(struct bt_mesh_net_rx *rx, struct os_mbuf *pdu)
+{
+	u32_t hash1, hash2;
+
+	/* Three least significant bytes of IVI + first byte of SEQ */
+	hash1 = (BT_MESH_NET_IVI_RX(rx) << 8) | pdu->om_data[2];
+
+	/* Two last bytes of SEQ + SRC */
+	memcpy(&hash2, &pdu->om_data[3], 4);
+
+	return (u64_t)hash1 << 32 | (u64_t)hash2;
+}
+
+static bool msg_cache_match(struct bt_mesh_net_rx *rx,
+			    struct os_mbuf *pdu)
+{
+	u64_t hash = msg_hash(rx, pdu);
+	u16_t i;
+
+	for (i = 0; i < ARRAY_SIZE(msg_cache); i++) {
+		if (msg_cache[i] == hash) {
+			return true;
+		}
+	}
+
+	/* Add to the cache */
+	rx->msg_cache_idx = msg_cache_next++;
+	msg_cache[rx->msg_cache_idx] = hash;
+	msg_cache_next %= ARRAY_SIZE(msg_cache);
+
+	return false;
+}
+
+struct bt_mesh_subnet *bt_mesh_subnet_get(u16_t net_idx)
+{
+	int i;
+
+	if (net_idx == BT_MESH_KEY_ANY) {
+		return &bt_mesh.sub[0];
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		if (bt_mesh.sub[i].net_idx == net_idx) {
+			return &bt_mesh.sub[i];
+		}
+	}
+
+	return NULL;
+}
+
+int bt_mesh_net_keys_create(struct bt_mesh_subnet_keys *keys,
+			    const u8_t key[16])
+{
+	u8_t p[] = { 0 };
+	u8_t nid;
+	int err;
+
+	err = bt_mesh_k2(key, p, sizeof(p), &nid, keys->enc, keys->privacy);
+	if (err) {
+		BT_ERR("Unable to generate NID, EncKey & PrivacyKey");
+		return err;
+	}
+
+	memcpy(keys->net, key, 16);
+
+	keys->nid = nid;
+
+	BT_DBG("NID 0x%02x EncKey %s", keys->nid, bt_hex(keys->enc, 16));
+	BT_DBG("PrivacyKey %s", bt_hex(keys->privacy, 16));
+
+	err = bt_mesh_k3(key, keys->net_id);
+	if (err) {
+		BT_ERR("Unable to generate Net ID");
+		return err;
+	}
+
+	BT_DBG("NetID %s", bt_hex(keys->net_id, 8));
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+	err = bt_mesh_identity_key(key, keys->identity);
+	if (err) {
+		BT_ERR("Unable to generate IdentityKey");
+		return err;
+	}
+
+	BT_DBG("IdentityKey %s", bt_hex(keys->identity, 16));
+#endif /* GATT_PROXY */
+
+	err = bt_mesh_beacon_key(key, keys->beacon);
+	if (err) {
+		BT_ERR("Unable to generate beacon key");
+		return err;
+	}
+
+	BT_DBG("BeaconKey %s", bt_hex(keys->beacon, 16));
+
+	return 0;
+}
+
+int friend_cred_set(struct friend_cred *cred, u8_t idx, const u8_t net_key[16])
+{
+	u16_t lpn_addr, frnd_addr;
+	int err;
+	u8_t p[9];
+
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+	if (cred->addr == bt_mesh.lpn.frnd) {
+		lpn_addr = bt_mesh_primary_addr();
+		frnd_addr = cred->addr;
+	} else {
+		lpn_addr = cred->addr;
+		frnd_addr = bt_mesh_primary_addr();
+	}
+#else
+	lpn_addr = cred->addr;
+	frnd_addr = bt_mesh_primary_addr();
+#endif
+
+	BT_DBG("LPNAddress 0x%04x FriendAddress 0x%04x", lpn_addr, frnd_addr);
+	BT_DBG("LPNCounter 0x%04x FriendCounter 0x%04x", cred->lpn_counter,
+	       cred->frnd_counter);
+
+	p[0] = 0x01;
+	sys_put_be16(lpn_addr, p + 1);
+	sys_put_be16(frnd_addr, p + 3);
+	sys_put_be16(cred->lpn_counter, p + 5);
+	sys_put_be16(cred->frnd_counter, p + 7);
+
+	err = bt_mesh_k2(net_key, p, sizeof(p), &cred->cred[idx].nid,
+			 cred->cred[idx].enc, cred->cred[idx].privacy);
+	if (err) {
+		BT_ERR("Unable to generate NID, EncKey & PrivacyKey");
+		return err;
+	}
+
+	BT_DBG("Friend NID 0x%02x EncKey %s", cred->cred[idx].nid,
+	       bt_hex(cred->cred[idx].enc, 16));
+	BT_DBG("Friend PrivacyKey %s", bt_hex(cred->cred[idx].privacy, 16));
+
+	return 0;
+}
+
+void friend_cred_refresh(u16_t net_idx)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(friend_cred); i++) {
+		struct friend_cred *cred = &friend_cred[i];
+
+		if (cred->addr != BT_MESH_ADDR_UNASSIGNED &&
+		    cred->net_idx == net_idx) {
+			memcpy(&cred->cred[0], &cred->cred[1],
+			       sizeof(cred->cred[0]));
+		}
+	}
+}
+
+int friend_cred_update(struct bt_mesh_subnet *sub)
+{
+	int err, i;
+
+	BT_DBG("net_idx 0x%04x", sub->net_idx);
+
+	for (i = 0; i < ARRAY_SIZE(friend_cred); i++) {
+		struct friend_cred *cred = &friend_cred[i];
+
+		if (cred->addr == BT_MESH_ADDR_UNASSIGNED ||
+		    cred->net_idx != sub->net_idx) {
+			continue;
+		}
+
+		err = friend_cred_set(cred, 1, sub->keys[1].net);
+		if (err) {
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+struct friend_cred *friend_cred_create(struct bt_mesh_subnet *sub, u16_t addr,
+				       u16_t lpn_counter, u16_t frnd_counter)
+{
+	struct friend_cred *cred;
+	int i, err;
+
+	BT_DBG("net_idx 0x%04x addr 0x%04x", sub->net_idx, addr);
+
+	for (cred = NULL, i = 0; i < ARRAY_SIZE(friend_cred); i++) {
+		if ((friend_cred[i].addr == BT_MESH_ADDR_UNASSIGNED) ||
+		    (friend_cred[i].addr == addr &&
+		     friend_cred[i].net_idx == sub->net_idx)) {
+			cred = &friend_cred[i];
+			break;
+		}
+	}
+
+	if (!cred) {
+		BT_WARN("No free friend credential slots");
+		return NULL;
+	}
+
+	cred->net_idx = sub->net_idx;
+	cred->addr = addr;
+	cred->lpn_counter = lpn_counter;
+	cred->frnd_counter = frnd_counter;
+
+	err = friend_cred_set(cred, 0, sub->keys[0].net);
+	if (err) {
+		friend_cred_clear(cred);
+		return NULL;
+	}
+
+	if (sub->kr_flag) {
+		err = friend_cred_set(cred, 1, sub->keys[1].net);
+		if (err) {
+			friend_cred_clear(cred);
+			return NULL;
+		}
+	}
+
+	return cred;
+}
+
+void friend_cred_clear(struct friend_cred *cred)
+{
+	cred->net_idx = BT_MESH_KEY_UNUSED;
+	cred->addr = BT_MESH_ADDR_UNASSIGNED;
+	cred->lpn_counter = 0;
+	cred->frnd_counter = 0;
+	memset(cred->cred, 0, sizeof(cred->cred));
+}
+
+int friend_cred_del(u16_t net_idx, u16_t addr)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(friend_cred); i++) {
+		struct friend_cred *cred = &friend_cred[i];
+
+		if (cred->addr == addr && cred->net_idx == net_idx) {
+			friend_cred_clear(cred);
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
+
+int friend_cred_get(struct bt_mesh_subnet *sub, u16_t addr, u8_t *nid,
+		    const u8_t **enc, const u8_t **priv)
+{
+	int i;
+
+	BT_DBG("net_idx 0x%04x addr 0x%04x", sub->net_idx, addr);
+
+	for (i = 0; i < ARRAY_SIZE(friend_cred); i++) {
+		struct friend_cred *cred = &friend_cred[i];
+
+		if (cred->net_idx != sub->net_idx) {
+			continue;
+		}
+
+		if (addr != BT_MESH_ADDR_UNASSIGNED && cred->addr != addr) {
+			continue;
+		}
+
+		if (nid) {
+			*nid = cred->cred[sub->kr_flag].nid;
+		}
+
+		if (enc) {
+			*enc = cred->cred[sub->kr_flag].enc;
+		}
+
+		if (priv) {
+			*priv = cred->cred[sub->kr_flag].privacy;
+		}
+
+		return 0;
+	}
+
+	return -ENOENT;
+}
+
+u8_t bt_mesh_net_flags(struct bt_mesh_subnet *sub)
+{
+	u8_t flags = 0x00;
+
+	if (sub && sub->kr_flag) {
+		flags |= BT_MESH_NET_FLAG_KR;
+	}
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS)) {
+		flags |= BT_MESH_NET_FLAG_IVU;
+	}
+
+	return flags;
+}
+
+int bt_mesh_net_beacon_update(struct bt_mesh_subnet *sub)
+{
+	u8_t flags = bt_mesh_net_flags(sub);
+	struct bt_mesh_subnet_keys *keys;
+
+	if (sub->kr_flag) {
+		BT_DBG("NetIndex %u Using new key", sub->net_idx);
+		keys = &sub->keys[1];
+	} else {
+		BT_DBG("NetIndex %u Using current key", sub->net_idx);
+		keys = &sub->keys[0];
+	}
+
+	BT_DBG("flags 0x%02x, IVI 0x%08x", flags, (unsigned) bt_mesh.iv_index);
+
+	return bt_mesh_beacon_auth(keys->beacon, flags, keys->net_id,
+				   bt_mesh.iv_index, sub->auth);
+}
+
+int bt_mesh_net_create(u16_t idx, u8_t flags, const u8_t key[16],
+		       u32_t iv_index)
+{
+	struct bt_mesh_subnet *sub;
+	int err;
+
+	BT_DBG("idx %u flags 0x%02x iv_index %u", idx, flags,
+	       (unsigned) iv_index);
+
+	BT_DBG("NetKey %s", bt_hex(key, 16));
+
+	(void)memset(msg_cache, 0, sizeof(msg_cache));
+	msg_cache_next = 0U;
+
+	sub = &bt_mesh.sub[0];
+
+	sub->kr_flag = BT_MESH_KEY_REFRESH(flags);
+	if (sub->kr_flag) {
+		err = bt_mesh_net_keys_create(&sub->keys[1], key);
+		if (err) {
+			return -EIO;
+		}
+
+		sub->kr_phase = BT_MESH_KR_PHASE_2;
+	} else {
+		err = bt_mesh_net_keys_create(&sub->keys[0], key);
+		if (err) {
+			return -EIO;
+		}
+	}
+
+	sub->net_idx = idx;
+
+	if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) {
+		sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED;
+	} else {
+		sub->node_id = BT_MESH_NODE_IDENTITY_NOT_SUPPORTED;
+	}
+
+	bt_mesh.iv_index = iv_index;
+	atomic_set_bit_to(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS,
+			  BT_MESH_IV_UPDATE(flags));
+
+	/* Set minimum required hours, since the 96-hour minimum requirement
+	 * doesn't apply straight after provisioning (since we can't know how
+	 * long has actually passed since the network changed its state).
+	 */
+	bt_mesh.ivu_duration = BT_MESH_IVU_MIN_HOURS;
+
+	/* Make sure we have valid beacon data to be sent */
+	bt_mesh_net_beacon_update(sub);
+
+	return 0;
+}
+
+void bt_mesh_net_revoke_keys(struct bt_mesh_subnet *sub)
+{
+	int i;
+
+	BT_DBG("idx 0x%04x", sub->net_idx);
+
+	memcpy(&sub->keys[0], &sub->keys[1], sizeof(sub->keys[0]));
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+
+		if (key->net_idx != sub->net_idx || !key->updated) {
+			continue;
+		}
+
+		memcpy(&key->keys[0], &key->keys[1], sizeof(key->keys[0]));
+		key->updated = false;
+	}
+}
+
+bool bt_mesh_kr_update(struct bt_mesh_subnet *sub, u8_t new_kr, bool new_key)
+{
+	if (new_kr != sub->kr_flag && sub->kr_phase == BT_MESH_KR_NORMAL) {
+		BT_WARN("KR change in normal operation. Are we blacklisted?");
+		return false;
+	}
+
+	sub->kr_flag = new_kr;
+
+	if (sub->kr_flag) {
+		if (sub->kr_phase == BT_MESH_KR_PHASE_1) {
+			BT_DBG("Phase 1 -> Phase 2");
+			sub->kr_phase = BT_MESH_KR_PHASE_2;
+			return true;
+		}
+	} else {
+		switch (sub->kr_phase) {
+		case BT_MESH_KR_PHASE_1:
+			if (!new_key) {
+				/* Ignore */
+				break;
+			}
+		/* Upon receiving a Secure Network beacon with the KR flag set
+		 * to 0 using the new NetKey in Phase 1, the node shall
+		 * immediately transition to Phase 3, which effectively skips
+		 * Phase 2.
+		 *
+		 * Intentional fall-through.
+		 */
+		case BT_MESH_KR_PHASE_2:
+			BT_DBG("KR Phase 0x%02x -> Normal", sub->kr_phase);
+			bt_mesh_net_revoke_keys(sub);
+			if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) ||
+			    (MYNEWT_VAL(BLE_MESH_FRIEND))) {
+				friend_cred_refresh(sub->net_idx);
+			}
+			sub->kr_phase = BT_MESH_KR_NORMAL;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void bt_mesh_rpl_reset(void)
+{
+	int i;
+
+	/* Discard "old old" IV Index entries from RPL and flag
+	 * any other ones (which are valid) as old.
+	 */
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) {
+		struct bt_mesh_rpl *rpl = &bt_mesh.rpl[i];
+
+		if (rpl->src) {
+			if (rpl->old_iv) {
+				memset(rpl, 0, sizeof(*rpl));
+			} else {
+				rpl->old_iv = true;
+			}
+		}
+	}
+}
+
+#if MYNEWT_VAL(BLE_MESH_IV_UPDATE_TEST)
+void bt_mesh_iv_update_test(bool enable)
+{
+	atomic_set_bit_to(bt_mesh.flags, BT_MESH_IVU_TEST, enable);
+	/* Reset the duration variable - needed for some PTS tests */
+	bt_mesh.ivu_duration = 0;
+}
+
+bool bt_mesh_iv_update(void)
+{
+	if (!bt_mesh_is_provisioned()) {
+		BT_ERR("Not yet provisioned");
+		return false;
+	}
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS)) {
+		bt_mesh_net_iv_update(bt_mesh.iv_index, false);
+	} else {
+		bt_mesh_net_iv_update(bt_mesh.iv_index + 1, true);
+	}
+
+	bt_mesh_net_sec_update(NULL);
+
+	return atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS);
+}
+#endif /* CONFIG_BT_MESH_IV_UPDATE_TEST */
+
+/* Used for sending immediate beacons to Friend queues and GATT clients */
+void bt_mesh_net_sec_update(struct bt_mesh_subnet *sub)
+{
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		bt_mesh_friend_sec_update(sub ? sub->net_idx : BT_MESH_KEY_ANY);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY) &&
+	    bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED) {
+		bt_mesh_proxy_beacon_send(sub);
+	}
+}
+
+bool bt_mesh_net_iv_update(u32_t iv_index, bool iv_update)
+{
+	int i;
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS)) {
+		/* We're currently in IV Update mode */
+
+		if (iv_index != bt_mesh.iv_index) {
+			BT_WARN("IV Index mismatch: 0x%08x != 0x%08x",
+				(unsigned) iv_index,
+				(unsigned) bt_mesh.iv_index);
+			return false;
+		}
+
+		if (iv_update) {
+			/* Nothing to do */
+			BT_DBG("Already in IV Update in Progress state");
+			return false;
+		}
+	} else {
+		/* We're currently in Normal mode */
+
+		if (iv_index == bt_mesh.iv_index) {
+			BT_DBG("Same IV Index in normal mode");
+			return false;
+		}
+
+		if (iv_index < bt_mesh.iv_index ||
+		    iv_index > bt_mesh.iv_index + 42) {
+			BT_ERR("IV Index out of sync: 0x%08x != 0x%08x",
+			       (unsigned) iv_index,
+			       (unsigned) bt_mesh.iv_index);
+			return false;
+		}
+
+		if (iv_index > bt_mesh.iv_index + 1) {
+			BT_WARN("Performing IV Index Recovery");
+			memset(bt_mesh.rpl, 0, sizeof(bt_mesh.rpl));
+			bt_mesh.iv_index = iv_index;
+			bt_mesh.seq = 0;
+			goto do_update;
+		}
+
+		if (iv_index == bt_mesh.iv_index + 1 && !iv_update) {
+			BT_WARN("Ignoring new index in normal mode");
+			return false;
+		}
+
+		if (!iv_update) {
+			/* Nothing to do */
+			BT_DBG("Already in Normal state");
+			return false;
+		}
+	}
+
+	if (!(IS_ENABLED(CONFIG_BT_MESH_IV_UPDATE_TEST) &&
+	      atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_TEST))) {
+		if (bt_mesh.ivu_duration < BT_MESH_IVU_MIN_HOURS) {
+			BT_WARN("IV Update before minimum duration");
+			return false;
+		}
+	}
+
+	/* Defer change to Normal Operation if there are pending acks */
+	if (!iv_update && bt_mesh_tx_in_progress()) {
+		BT_WARN("IV Update deferred because of pending transfer");
+		atomic_set_bit(bt_mesh.flags, BT_MESH_IVU_PENDING);
+		return false;
+	}
+
+do_update:
+	atomic_set_bit_to(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS, iv_update);
+	bt_mesh.ivu_duration = 0U;
+
+	if (iv_update) {
+		bt_mesh.iv_index = iv_index;
+		BT_DBG("IV Update state entered. New index 0x%08x",
+		       (unsigned) bt_mesh.iv_index);
+
+		bt_mesh_rpl_reset();
+	} else {
+		BT_DBG("Normal mode entered");
+		bt_mesh.seq = 0;
+	}
+
+	k_delayed_work_submit(&bt_mesh.ivu_timer, BT_MESH_IVU_TIMEOUT);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		if (bt_mesh.sub[i].net_idx != BT_MESH_KEY_UNUSED) {
+			bt_mesh_net_beacon_update(&bt_mesh.sub[i]);
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_iv(false);
+	}
+
+	return true;
+}
+
+u32_t bt_mesh_next_seq(void)
+{
+	u32_t seq = bt_mesh.seq++;
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_seq();
+	}
+
+	if (!atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS) &&
+	    bt_mesh.seq > IV_UPDATE_SEQ_LIMIT &&
+	    bt_mesh_subnet_get(BT_MESH_KEY_PRIMARY)) {
+		bt_mesh_beacon_ivu_initiator(true);
+		bt_mesh_net_iv_update(bt_mesh.iv_index + 1, true);
+		bt_mesh_net_sec_update(NULL);
+	}
+
+	return seq;
+}
+
+int bt_mesh_net_resend(struct bt_mesh_subnet *sub, struct os_mbuf *buf,
+		       bool new_key, const struct bt_mesh_send_cb *cb,
+		       void *cb_data)
+{
+	const u8_t *enc, *priv;
+	u32_t seq;
+	u16_t dst;
+	int err;
+
+	BT_DBG("net_idx 0x%04x new_key %u len %u", sub->net_idx, new_key,
+	       buf->om_len);
+
+	enc = sub->keys[new_key].enc;
+	priv = sub->keys[new_key].privacy;
+
+	err = bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_TX, priv);
+	if (err) {
+		BT_ERR("deobfuscate failed (err %d)", err);
+		return err;
+	}
+
+	err = bt_mesh_net_decrypt(enc, buf, BT_MESH_NET_IVI_TX, false);
+	if (err) {
+		BT_ERR("decrypt failed (err %d)", err);
+		return err;
+	}
+
+	seq = bt_mesh_next_seq();
+	buf->om_data[2] = seq >> 16;
+	buf->om_data[3] = seq >> 8;
+	buf->om_data[4] = seq;
+
+	/* Get destination, in case it's a proxy client */
+	dst = DST(buf->om_data);
+
+	err = bt_mesh_net_encrypt(enc, buf, BT_MESH_NET_IVI_TX, false);
+	if (err) {
+		BT_ERR("encrypt failed (err %d)", err);
+		return err;
+	}
+
+	err = bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_TX, priv);
+	if (err) {
+		BT_ERR("obfuscate failed (err %d)", err);
+		return err;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY) &&
+	    bt_mesh_proxy_relay(buf, dst)) {
+		send_cb_finalize(cb, cb_data);
+	} else {
+		bt_mesh_adv_send(buf, cb, cb_data);
+	}
+
+	return 0;
+}
+
+static void bt_mesh_net_local(struct ble_npl_event *work)
+{
+	struct os_mbuf *buf;
+
+	while ((buf = net_buf_slist_get(&bt_mesh.local_queue))) {
+		BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+		bt_mesh_net_recv(buf, 0, BT_MESH_NET_IF_LOCAL);
+		net_buf_unref(buf);
+	}
+}
+
+int bt_mesh_net_encode(struct bt_mesh_net_tx *tx, struct os_mbuf *buf,
+		       bool proxy)
+{
+	const bool ctl = (tx->ctx->app_idx == BT_MESH_KEY_UNUSED);
+	u32_t seq_val;
+	u8_t nid;
+	const u8_t *enc, *priv;
+	u8_t *seq;
+	int err;
+
+	if (ctl && net_buf_simple_tailroom(buf) < 8) {
+		BT_ERR("Insufficient MIC space for CTL PDU");
+		return -EINVAL;
+	} else if (net_buf_simple_tailroom(buf) < 4) {
+		BT_ERR("Insufficient MIC space for PDU");
+		return -EINVAL;
+	}
+
+	BT_DBG("src 0x%04x dst 0x%04x ctl %u seq 0x%06x",
+	       tx->src, tx->ctx->addr, ctl, bt_mesh.seq);
+
+	net_buf_simple_push_be16(buf, tx->ctx->addr);
+	net_buf_simple_push_be16(buf, tx->src);
+
+	seq = net_buf_simple_push(buf, 3);
+	seq_val = bt_mesh_next_seq();
+	seq[0] = seq_val >> 16;
+	seq[1] = seq_val >> 8;
+	seq[2] = seq_val;
+
+	if (ctl) {
+		net_buf_simple_push_u8(buf, tx->ctx->send_ttl | 0x80);
+	} else {
+		net_buf_simple_push_u8(buf, tx->ctx->send_ttl);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER) && tx->friend_cred) {
+		if (friend_cred_get(tx->sub, BT_MESH_ADDR_UNASSIGNED,
+				    &nid, &enc, &priv)) {
+			BT_WARN("Falling back to master credentials");
+
+			tx->friend_cred = 0;
+
+			nid = tx->sub->keys[tx->sub->kr_flag].nid;
+			enc = tx->sub->keys[tx->sub->kr_flag].enc;
+			priv = tx->sub->keys[tx->sub->kr_flag].privacy;
+		}
+	} else {
+		tx->friend_cred = 0;
+		nid = tx->sub->keys[tx->sub->kr_flag].nid;
+		enc = tx->sub->keys[tx->sub->kr_flag].enc;
+		priv = tx->sub->keys[tx->sub->kr_flag].privacy;
+	}
+
+	net_buf_simple_push_u8(buf, (nid | (BT_MESH_NET_IVI_TX & 1) << 7));
+
+	err = bt_mesh_net_encrypt(enc, buf, BT_MESH_NET_IVI_TX, proxy);
+	if (err) {
+		return err;
+	}
+
+	return bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_TX, priv);
+}
+
+int bt_mesh_net_send(struct bt_mesh_net_tx *tx, struct os_mbuf *buf,
+		     const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	int err;
+
+	BT_DBG("src 0x%04x dst 0x%04x len %u headroom %zu tailroom %zu",
+	       tx->src, tx->ctx->addr, buf->om_len, net_buf_headroom(buf),
+	       net_buf_tailroom(buf));
+	BT_DBG("Payload len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+	BT_DBG("Seq 0x%06x", bt_mesh.seq);
+
+	if (tx->ctx->send_ttl == BT_MESH_TTL_DEFAULT) {
+		tx->ctx->send_ttl = bt_mesh_default_ttl_get();
+	}
+
+	err = bt_mesh_net_encode(tx, buf, false);
+	if (err) {
+		goto done;
+	}
+
+	BT_DBG("encoded %u bytes: %s", buf->om_len,
+		   bt_hex(buf->om_data, buf->om_len));
+
+	/* Deliver to GATT Proxy Clients if necessary. Mesh spec 3.4.5.2:
+	 * "The output filter of the interface connected to advertising or
+	 * GATT bearers shall drop all messages with TTL value set to 1."
+	 */
+	if (MYNEWT_VAL(BLE_MESH_GATT_PROXY) &&
+	    tx->ctx->send_ttl != 1) {
+		if (bt_mesh_proxy_relay(buf, tx->ctx->addr) &&
+		    BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) {
+			/* Notify completion if this only went
+			 * through the Mesh Proxy.
+			 */
+			send_cb_finalize(cb, cb_data);
+
+			err = 0;
+			goto done;
+		}
+	}
+
+	/* Deliver to local network interface if necessary */
+	if (bt_mesh_fixed_group_match(tx->ctx->addr) ||
+	    bt_mesh_elem_find(tx->ctx->addr)) {
+		if (cb && cb->start) {
+			cb->start(0, 0, cb_data);
+		}
+		net_buf_slist_put(&bt_mesh.local_queue, net_buf_ref(buf));
+		if (cb && cb->end) {
+			cb->end(0, cb_data);
+		}
+		k_work_submit(&bt_mesh.local_work);
+	} else if (tx->ctx->send_ttl != 1) {
+		/* Deliver to to the advertising network interface. Mesh spec
+		 * 3.4.5.2: "The output filter of the interface connected to
+		 * advertising or GATT bearers shall drop all messages with
+		 * TTL value set to 1."
+		 */
+		bt_mesh_adv_send(buf, cb, cb_data);
+	}
+
+done:
+	net_buf_unref(buf);
+	return err;
+}
+
+static bool auth_match(struct bt_mesh_subnet_keys *keys,
+		       const u8_t net_id[8], u8_t flags,
+		       u32_t iv_index, const u8_t auth[8])
+{
+	u8_t net_auth[8];
+
+	if (memcmp(net_id, keys->net_id, 8)) {
+		return false;
+	}
+
+	bt_mesh_beacon_auth(keys->beacon, flags, keys->net_id, iv_index,
+			    net_auth);
+
+	if (memcmp(auth, net_auth, 8)) {
+		BT_WARN("Authentication Value %s != %s",
+			bt_hex(auth, 8), bt_hex(net_auth, 8));
+		return false;
+	}
+
+	return true;
+}
+
+struct bt_mesh_subnet *bt_mesh_subnet_find(const u8_t net_id[8], u8_t flags,
+					   u32_t iv_index, const u8_t auth[8],
+					   bool *new_key)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (auth_match(&sub->keys[0], net_id, flags, iv_index, auth)) {
+			*new_key = false;
+			return sub;
+		}
+
+		if (sub->kr_phase == BT_MESH_KR_NORMAL) {
+			continue;
+		}
+
+		if (auth_match(&sub->keys[1], net_id, flags, iv_index, auth)) {
+			*new_key = true;
+			return sub;
+		}
+	}
+
+	return NULL;
+}
+
+static int net_decrypt(struct bt_mesh_subnet *sub, const u8_t *enc,
+		       const u8_t *priv, const u8_t *data,
+		       size_t data_len, struct bt_mesh_net_rx *rx,
+		       struct os_mbuf *buf)
+{
+	BT_DBG("NID 0x%02x net_idx 0x%04x", NID(data), sub->net_idx);
+	BT_DBG("IVI %u net->iv_index 0x%08x", IVI(data),
+	       (unsigned) bt_mesh.iv_index);
+
+	rx->old_iv = (IVI(data) != (bt_mesh.iv_index & 0x01));
+
+	net_buf_simple_init(buf, 0);
+	memcpy(net_buf_simple_add(buf, data_len), data, data_len);
+
+	if (bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_RX(rx), priv)) {
+		return -ENOENT;
+	}
+
+	if (rx->net_if == BT_MESH_NET_IF_ADV && msg_cache_match(rx, buf)) {
+		BT_WARN("Duplicate found in Network Message Cache");
+		return -EALREADY;
+	}
+
+	rx->ctx.addr = SRC(buf->om_data);
+	if (!BT_MESH_ADDR_IS_UNICAST(rx->ctx.addr)) {
+		BT_WARN("Ignoring non-unicast src addr 0x%04x", rx->ctx.addr);
+		return -EINVAL;
+	}
+
+	BT_DBG("src 0x%04x", rx->ctx.addr);
+
+	if ((MYNEWT_VAL(BLE_MESH_PROXY)) &&
+	    rx->net_if == BT_MESH_NET_IF_PROXY_CFG) {
+		return bt_mesh_net_decrypt(enc, buf, BT_MESH_NET_IVI_RX(rx),
+					   true);
+	}
+
+	return bt_mesh_net_decrypt(enc, buf, BT_MESH_NET_IVI_RX(rx), false);
+}
+
+static int friend_decrypt(struct bt_mesh_subnet *sub, const u8_t *data,
+			  size_t data_len, struct bt_mesh_net_rx *rx,
+			  struct os_mbuf *buf)
+{
+	int i;
+
+	BT_DBG("NID 0x%02x net_idx 0x%04x", NID(data), sub->net_idx);
+
+	for (i = 0; i < ARRAY_SIZE(friend_cred); i++) {
+		struct friend_cred *cred = &friend_cred[i];
+
+		if (cred->net_idx != sub->net_idx) {
+			continue;
+		}
+
+		if (NID(data) == cred->cred[0].nid &&
+		    !net_decrypt(sub, cred->cred[0].enc, cred->cred[0].privacy,
+				 data, data_len, rx, buf)) {
+			return 0;
+		}
+
+		if (sub->kr_phase == BT_MESH_KR_NORMAL) {
+			continue;
+		}
+
+		if (NID(data) == cred->cred[1].nid &&
+		    !net_decrypt(sub, cred->cred[1].enc, cred->cred[1].privacy,
+				 data, data_len, rx, buf)) {
+			rx->new_key = 1;
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
+
+static bool net_find_and_decrypt(const u8_t *data, size_t data_len,
+				 struct bt_mesh_net_rx *rx,
+				 struct os_mbuf *buf)
+{
+	struct bt_mesh_subnet *sub;
+	unsigned int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		sub = &bt_mesh.sub[i];
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if ((IS_ENABLED(CONFIG_BT_MESH_LOW_POWER) ||
+		     IS_ENABLED(CONFIG_BT_MESH_FRIEND)) &&
+		    !friend_decrypt(sub, data, data_len, rx, buf)) {
+			rx->friend_cred = 1U;
+			rx->ctx.net_idx = sub->net_idx;
+			rx->sub = sub;
+			return true;
+		}
+
+		if (NID(data) == sub->keys[0].nid &&
+		    !net_decrypt(sub, sub->keys[0].enc, sub->keys[0].privacy,
+				 data, data_len, rx, buf)) {
+			rx->ctx.net_idx = sub->net_idx;
+			rx->sub = sub;
+			return true;
+		}
+
+		if (sub->kr_phase == BT_MESH_KR_NORMAL) {
+			continue;
+		}
+
+		if (NID(data) == sub->keys[1].nid &&
+		    !net_decrypt(sub, sub->keys[1].enc, sub->keys[1].privacy,
+				 data, data_len, rx, buf)) {
+			rx->new_key = 1;
+			rx->ctx.net_idx = sub->net_idx;
+			rx->sub = sub;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+/* Relaying from advertising to the advertising bearer should only happen
+ * if the Relay state is set to enabled. Locally originated packets always
+ * get sent to the advertising bearer. If the packet came in through GATT,
+ * then we should only relay it if the GATT Proxy state is enabled.
+ */
+static bool relay_to_adv(enum bt_mesh_net_if net_if)
+{
+	switch (net_if) {
+	case BT_MESH_NET_IF_LOCAL:
+		return true;
+	case BT_MESH_NET_IF_ADV:
+		return (bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED);
+	case BT_MESH_NET_IF_PROXY:
+		return (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED);
+	default:
+		return false;
+	}
+}
+
+static void bt_mesh_net_relay(struct os_mbuf *sbuf,
+			      struct bt_mesh_net_rx *rx)
+{
+	const u8_t *enc, *priv;
+	struct os_mbuf *buf;
+	u8_t nid, transmit;
+
+	if (rx->net_if == BT_MESH_NET_IF_LOCAL) {
+		/* Locally originated PDUs with TTL=1 will only be delivered
+		 * to local elements as per Mesh Profile 1.0 section 3.4.5.2:
+		 * "The output filter of the interface connected to
+		 * advertising or GATT bearers shall drop all messages with
+		 * TTL value set to 1."
+		 */
+		if (rx->ctx.recv_ttl == 1) {
+			return;
+		}
+	} else {
+		if (rx->ctx.recv_ttl <= 1) {
+			return;
+		}
+	}
+
+	if (rx->net_if == BT_MESH_NET_IF_ADV &&
+	    bt_mesh_relay_get() != BT_MESH_RELAY_ENABLED &&
+	    bt_mesh_gatt_proxy_get() != BT_MESH_GATT_PROXY_ENABLED) {
+		return;
+	}
+
+	BT_DBG("TTL %u CTL %u dst 0x%04x", rx->ctx.recv_ttl, rx->ctl,
+	       rx->ctx.recv_dst);
+
+	/* The Relay Retransmit state is only applied to adv-adv relaying.
+	 * Anything else (like GATT to adv, or locally originated packets)
+	 * use the Network Transmit state.
+	 */
+	if (rx->net_if == BT_MESH_NET_IF_ADV) {
+		transmit = bt_mesh_relay_retransmit_get();
+	} else {
+		transmit = bt_mesh_net_transmit_get();
+	}
+
+	buf = bt_mesh_adv_create(BT_MESH_ADV_DATA, transmit, K_NO_WAIT);
+	if (!buf) {
+		BT_ERR("Out of relay buffers");
+		return;
+	}
+
+	/* Only decrement TTL for non-locally originated packets */
+	if (rx->net_if != BT_MESH_NET_IF_LOCAL) {
+		/* Leave CTL bit intact */
+		sbuf->om_data[1] &= 0x80;
+		sbuf->om_data[1] |= rx->ctx.recv_ttl - 1;
+	}
+
+	net_buf_add_mem(buf, sbuf->om_data, sbuf->om_len);
+
+	enc = rx->sub->keys[rx->sub->kr_flag].enc;
+	priv = rx->sub->keys[rx->sub->kr_flag].privacy;
+	nid = rx->sub->keys[rx->sub->kr_flag].nid;
+
+	BT_DBG("Relaying packet. TTL is now %u", TTL(buf->om_data));
+
+	/* Update NID if RX or RX was with friend credentials */
+	if (rx->friend_cred) {
+		buf->om_data[0] &= 0x80; /* Clear everything except IVI */
+		buf->om_data[0] |= nid;
+	}
+
+	/* We re-encrypt and obfuscate using the received IVI rather than
+	 * the normal TX IVI (which may be different) since the transport
+	 * layer nonce includes the IVI.
+	 */
+	if (bt_mesh_net_encrypt(enc, buf, BT_MESH_NET_IVI_RX(rx), false)) {
+		BT_ERR("Re-encrypting failed");
+		goto done;
+	}
+
+	if (bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_RX(rx), priv)) {
+		BT_ERR("Re-obfuscating failed");
+		goto done;
+	}
+
+	BT_DBG("encoded %u bytes: %s", buf->om_len,
+	       bt_hex(buf->om_data, buf->om_len));
+
+	/* Sending to the GATT bearer should only happen if GATT Proxy
+	 * is enabled or the message originates from the local node.
+	 */
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY) &&
+	    (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED ||
+	     rx->net_if == BT_MESH_NET_IF_LOCAL)) {
+		if (bt_mesh_proxy_relay(buf, rx->ctx.recv_dst) &&
+		    BT_MESH_ADDR_IS_UNICAST(rx->ctx.recv_dst)) {
+			goto done;
+		}
+	}
+
+	if (relay_to_adv(rx->net_if)) {
+		bt_mesh_adv_send(buf, NULL, NULL);
+	}
+
+done:
+	net_buf_unref(buf);
+}
+
+void bt_mesh_net_header_parse(struct os_mbuf *buf,
+			      struct bt_mesh_net_rx *rx)
+{
+	rx->old_iv = (IVI(buf->om_data) != (bt_mesh.iv_index & 0x01));
+	rx->ctl = CTL(buf->om_data);
+	rx->ctx.recv_ttl = TTL(buf->om_data);
+	rx->seq = SEQ(buf->om_data);
+	rx->ctx.addr = SRC(buf->om_data);
+	rx->ctx.recv_dst = DST(buf->om_data);
+}
+
+int bt_mesh_net_decode(struct os_mbuf *data, enum bt_mesh_net_if net_if,
+		       struct bt_mesh_net_rx *rx, struct os_mbuf *buf)
+{
+	if (data->om_len < BT_MESH_NET_MIN_PDU_LEN) {
+		BT_WARN("Dropping too short mesh packet (len %u)", data->om_len);
+		BT_WARN("%s", bt_hex(data->om_data, data->om_len));
+		return -EINVAL;
+	}
+
+	if (net_if == BT_MESH_NET_IF_ADV && check_dup(data)) {
+		BT_DBG("duplicate packet; dropping %u bytes: %s", data->om_len,
+			   bt_hex(data->om_data, data->om_len));
+		return -EINVAL;
+	}
+
+	BT_DBG("%u bytes: %s", data->om_len, bt_hex(data->om_data, data->om_len));
+
+	rx->net_if = net_if;
+
+	if (!net_find_and_decrypt(data->om_data, data->om_len, rx, buf)) {
+		BT_DBG("Unable to find matching net for packet");
+		return -ENOENT;
+	}
+
+	/* Initialize AppIdx to a sane value */
+	rx->ctx.app_idx = BT_MESH_KEY_UNUSED;
+
+	rx->ctx.recv_ttl = TTL(buf->om_data);
+
+	/* Default to responding with TTL 0 for non-routed messages */
+	if (rx->ctx.recv_ttl == 0) {
+		rx->ctx.send_ttl = 0;
+	} else {
+		rx->ctx.send_ttl = BT_MESH_TTL_DEFAULT;
+	}
+
+	rx->ctl = CTL(buf->om_data);
+	rx->seq = SEQ(buf->om_data);
+	rx->ctx.recv_dst = DST(buf->om_data);
+
+	BT_DBG("Decryption successful. Payload len %u: %s", buf->om_len,
+		   bt_hex(buf->om_data, buf->om_len));
+
+	if (net_if != BT_MESH_NET_IF_PROXY_CFG &&
+	    rx->ctx.recv_dst == BT_MESH_ADDR_UNASSIGNED) {
+		BT_ERR("Destination address is unassigned; dropping packet");
+		return -EBADMSG;
+	}
+
+	if (BT_MESH_ADDR_IS_RFU(rx->ctx.recv_dst)) {
+		BT_ERR("Destination address is RFU; dropping packet");
+		return -EBADMSG;
+	}
+
+	if (net_if != BT_MESH_NET_IF_LOCAL && bt_mesh_elem_find(rx->ctx.addr)) {
+		BT_DBG("Dropping locally originated packet");
+		return -EBADMSG;
+	}
+
+	BT_DBG("src 0x%04x dst 0x%04x ttl %u", rx->ctx.addr, rx->ctx.recv_dst,
+	       rx->ctx.recv_ttl);
+	BT_DBG("PDU: %s", bt_hex(buf->om_data, buf->om_len));
+
+	return 0;
+}
+
+void bt_mesh_net_recv(struct os_mbuf *data, s8_t rssi,
+		      enum bt_mesh_net_if net_if)
+{
+	struct os_mbuf *buf = NET_BUF_SIMPLE(29);
+	struct bt_mesh_net_rx rx = { .ctx.recv_rssi = rssi };
+	struct net_buf_simple_state state;
+
+	BT_DBG("rssi %d net_if %u", rssi, net_if);
+
+	if (!bt_mesh_is_provisioned()) {
+		BT_ERR("Not provisioned; dropping packet");
+		goto done;
+	}
+
+	if (bt_mesh_net_decode(data, net_if, &rx, buf)) {
+		goto done;
+	}
+
+	/* Save the state so the buffer can later be relayed */
+	net_buf_simple_save(buf, &state);
+
+	rx.local_match = (bt_mesh_fixed_group_match(rx.ctx.recv_dst) ||
+			  bt_mesh_elem_find(rx.ctx.recv_dst));
+
+	if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY)) &&
+	    net_if == BT_MESH_NET_IF_PROXY) {
+		bt_mesh_proxy_addr_add(data, rx.ctx.addr);
+
+		if (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_DISABLED &&
+		    !rx.local_match) {
+			BT_INFO("Proxy is disabled; ignoring message");
+			goto done;
+		}
+	}
+
+	/* The transport layer has indicated that it has rejected the message,
+	 * but would like to see it again if it is received in the future.
+	 * This can happen if a message is received when the device is in
+	 * Low Power mode, but the message was not encrypted with the friend
+	 * credentials. Remove it from the message cache so that we accept
+	 * it again in the future.
+	 */
+	if (bt_mesh_trans_recv(buf, &rx) == -EAGAIN) {
+		BT_WARN("Removing rejected message from Network Message Cache");
+		msg_cache[rx.msg_cache_idx] = 0ULL;
+		/* Rewind the next index now that we're not using this entry */
+		msg_cache_next = rx.msg_cache_idx;
+	}
+
+	/* Relay if this was a group/virtual address, or if the destination
+	 * was neither a local element nor an LPN we're Friends for.
+	 */
+	if (!BT_MESH_ADDR_IS_UNICAST(rx.ctx.recv_dst) ||
+	    (!rx.local_match && !rx.friend_match)) {
+		net_buf_simple_restore(buf, &state);
+		bt_mesh_net_relay(buf, &rx);
+	}
+
+done:
+    os_mbuf_free_chain(buf);
+}
+
+static void ivu_refresh(struct ble_npl_event *work)
+{
+	bt_mesh.ivu_duration += BT_MESH_IVU_HOURS;
+
+	BT_DBG("%s for %u hour%s",
+	       atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS) ?
+	       "IVU in Progress" : "IVU Normal mode",
+	       bt_mesh.ivu_duration, bt_mesh.ivu_duration == 1 ? "" : "s");
+
+	if (bt_mesh.ivu_duration < BT_MESH_IVU_MIN_HOURS) {
+		if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+			bt_mesh_store_iv(true);
+		}
+
+		k_delayed_work_submit(&bt_mesh.ivu_timer, BT_MESH_IVU_TIMEOUT);
+		return;
+	}
+
+	if (atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS)) {
+		bt_mesh_beacon_ivu_initiator(true);
+		bt_mesh_net_iv_update(bt_mesh.iv_index, false);
+	} else if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_iv(true);
+	}
+}
+
+void bt_mesh_net_start(void)
+{
+	if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED) {
+		bt_mesh_beacon_enable();
+	} else {
+		bt_mesh_beacon_disable();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY) &&
+	    bt_mesh_gatt_proxy_get() != BT_MESH_GATT_PROXY_NOT_SUPPORTED) {
+		bt_mesh_proxy_gatt_enable();
+		bt_mesh_adv_update();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER)) {
+		bt_mesh_lpn_init();
+	} else {
+		bt_mesh_scan_enable();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		bt_mesh_friend_init();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROV)) {
+		u16_t net_idx = bt_mesh.sub[0].net_idx;
+		u16_t addr = bt_mesh_primary_addr();
+
+		bt_mesh_prov_complete(net_idx, addr);
+	}
+}
+
+void bt_mesh_net_init(void)
+{
+	k_delayed_work_init(&bt_mesh.ivu_timer, ivu_refresh);
+
+	k_work_init(&bt_mesh.local_work, bt_mesh_net_local);
+	net_buf_slist_init(&bt_mesh.local_queue);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/net.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/net.h
new file mode 100644
index 00000000..976da005
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/net.h
@@ -0,0 +1,412 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __NET_H__
+#define __NET_H__
+
+#define BT_MESH_NET_FLAG_KR       BIT(0)
+#define BT_MESH_NET_FLAG_IVU      BIT(1)
+
+#define BT_MESH_KR_NORMAL         0x00
+#define BT_MESH_KR_PHASE_1        0x01
+#define BT_MESH_KR_PHASE_2        0x02
+#define BT_MESH_KR_PHASE_3        0x03
+
+#define BT_MESH_IV_UPDATE(flags)   ((flags >> 1) & 0x01)
+#define BT_MESH_KEY_REFRESH(flags) (flags & 0x01)
+
+#include <stdbool.h>
+#include "atomic.h"
+#include "mesh/mesh.h"
+#include "mesh/glue.h"
+
+/* How many hours in between updating IVU duration */
+#define BT_MESH_IVU_MIN_HOURS      96
+#define BT_MESH_IVU_HOURS          (BT_MESH_IVU_MIN_HOURS /     \
+				    CONFIG_BT_MESH_IVU_DIVIDER)
+#define BT_MESH_IVU_TIMEOUT        K_HOURS(BT_MESH_IVU_HOURS)
+
+struct bt_mesh_app_key {
+	u16_t net_idx;
+	u16_t app_idx;
+	bool  updated;
+	struct bt_mesh_app_keys {
+		u8_t id;
+		u8_t val[16];
+	} keys[2];
+};
+
+struct bt_mesh_node {
+	u16_t addr;
+	u16_t net_idx;
+	u8_t  dev_key[16];
+	u8_t  num_elem;
+};
+
+struct bt_mesh_subnet {
+	u32_t beacon_sent;        /* Timestamp of last sent beacon */
+	u8_t  beacons_last;       /* Number of beacons during last
+				   * observation window
+				   */
+	u8_t  beacons_cur;        /* Number of beaconds observed during
+				   * currently ongoing window.
+				   */
+
+	u8_t  beacon_cache[21];   /* Cached last authenticated beacon */
+
+	u16_t net_idx;            /* NetKeyIndex */
+
+	bool  kr_flag;            /* Key Refresh Flag */
+	u8_t  kr_phase;           /* Key Refresh Phase */
+
+	u8_t  node_id;            /* Node Identity State */
+	u32_t node_id_start;      /* Node Identity started timestamp */
+
+	u8_t  auth[8];            /* Beacon Authentication Value */
+
+	struct bt_mesh_subnet_keys {
+		u8_t net[16];       /* NetKey */
+		u8_t nid;           /* NID */
+		u8_t enc[16];       /* EncKey */
+		u8_t net_id[8];     /* Network ID */
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+		u8_t identity[16];  /* IdentityKey */
+#endif
+		u8_t privacy[16];   /* PrivacyKey */
+		u8_t beacon[16];    /* BeaconKey */
+	} keys[2];
+};
+
+struct bt_mesh_rpl {
+	u16_t src;
+	bool  old_iv;
+#if (MYNEWT_VAL(BLE_MESH_SETTINGS))
+	bool  store;
+#endif
+	u32_t seq;
+};
+
+#if MYNEWT_VAL(BLE_MESH_FRIEND)
+#define FRIEND_SEG_RX MYNEWT_VAL(BLE_MESH_FRIEND_SEG_RX)
+#define FRIEND_SUB_LIST_SIZE MYNEWT_VAL(BLE_MESH_FRIEND_SUB_LIST_SIZE)
+#else
+#define FRIEND_SEG_RX 0
+#define FRIEND_SUB_LIST_SIZE 0
+#endif
+
+struct bt_mesh_friend {
+	u16_t lpn;
+	u8_t  recv_delay;
+	u8_t  fsn:1,
+	      send_last:1,
+	      pending_req:1,
+	      sec_update:1,
+	      pending_buf:1,
+	      valid:1,
+	      established:1;
+	s32_t poll_to;
+	u8_t  num_elem;
+	u16_t lpn_counter;
+	u16_t counter;
+
+	u16_t net_idx;
+
+	u16_t sub_list[FRIEND_SUB_LIST_SIZE];
+
+	struct k_delayed_work timer;
+
+	struct bt_mesh_friend_seg {
+		struct net_buf_slist_t queue;
+
+		/* The target number of segments, i.e. not necessarily
+		 * the current number of segments, in the queue. This is
+		 * used for Friend Queue free space calculations.
+		 */
+		u8_t        seg_count;
+	} seg[FRIEND_SEG_RX];
+
+	struct os_mbuf *last;
+
+	struct net_buf_slist_t queue;
+	u32_t queue_size;
+
+	/* Friend Clear Procedure */
+	struct {
+		u32_t start;                  /* Clear Procedure start */
+		u16_t frnd;                   /* Previous Friend's address */
+		u16_t repeat_sec;             /* Repeat timeout in seconds */
+		struct k_delayed_work timer;  /* Repeat timer */
+	} clear;
+};
+
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+#define LPN_GROUPS CONFIG_BT_MESH_LPN_GROUPS
+#else
+#define LPN_GROUPS 0
+#endif
+
+/* Low Power Node state */
+struct bt_mesh_lpn {
+	enum __packed {
+		BT_MESH_LPN_DISABLED,     /* LPN feature is disabled */
+		BT_MESH_LPN_CLEAR,        /* Clear in progress */
+		BT_MESH_LPN_TIMER,        /* Waiting for auto timer expiry */
+		BT_MESH_LPN_ENABLED,      /* LPN enabled, but no Friend */
+		BT_MESH_LPN_REQ_WAIT,     /* Wait before scanning for offers */
+		BT_MESH_LPN_WAIT_OFFER,   /* Friend Req sent */
+		BT_MESH_LPN_ESTABLISHED,  /* Friendship established */
+		BT_MESH_LPN_RECV_DELAY,   /* Poll sent, waiting ReceiveDelay */
+		BT_MESH_LPN_WAIT_UPDATE,  /* Waiting for Update or message */
+	} state;
+
+	/* Transaction Number (used for subscription list) */
+	u8_t xact_next;
+	u8_t xact_pending;
+	u8_t sent_req;
+
+	/* Address of our Friend when we're a LPN. Unassigned if we don't
+	 * have a friend yet.
+	 */
+	u16_t frnd;
+
+	/* Value from the friend offer */
+	u8_t  recv_win;
+
+	u8_t  req_attempts;     /* Number of Request attempts */
+
+	s32_t poll_timeout;
+
+	u8_t  groups_changed:1, /* Friend Subscription List needs updating */
+	      pending_poll:1,   /* Poll to be sent after subscription */
+	      disable:1,        /* Disable LPN after clearing */
+	      fsn:1,            /* Friend Sequence Number */
+	      established:1,    /* Friendship established */
+	      clear_success:1;  /* Friend Clear Confirm received */
+
+	/* Friend Queue Size */
+	u8_t  queue_size;
+
+	/* LPNCounter */
+	u16_t counter;
+
+	/* Previous Friend of this LPN */
+	u16_t old_friend;
+
+	/* Duration reported for last advertising packet */
+	u16_t adv_duration;
+
+	/* Next LPN related action timer */
+	struct k_delayed_work timer;
+
+	/* Subscribed groups */
+	u16_t groups[LPN_GROUPS];
+
+	/* Bit fields for tracking which groups the Friend knows about */
+	ATOMIC_DEFINE(added, LPN_GROUPS);
+	ATOMIC_DEFINE(pending, LPN_GROUPS);
+	ATOMIC_DEFINE(to_remove, LPN_GROUPS);
+};
+
+/* bt_mesh_net.flags */
+enum {
+	BT_MESH_VALID,           /* We have been provisioned */
+	BT_MESH_SUSPENDED,       /* Network is temporarily suspended */
+	BT_MESH_IVU_IN_PROGRESS, /* IV Update in Progress */
+	BT_MESH_IVU_INITIATOR,   /* IV Update initiated by us */
+	BT_MESH_IVU_TEST,        /* IV Update test mode */
+	BT_MESH_IVU_PENDING,     /* Update blocked by SDU in progress */
+
+	/* pending storage actions, must reside within first 32 flags */
+	BT_MESH_RPL_PENDING,
+	BT_MESH_KEYS_PENDING,
+	BT_MESH_NET_PENDING,
+	BT_MESH_IV_PENDING,
+	BT_MESH_SEQ_PENDING,
+	BT_MESH_HB_PUB_PENDING,
+	BT_MESH_CFG_PENDING,
+	BT_MESH_MOD_PENDING,
+	BT_MESH_VA_PENDING,
+	BT_MESH_NODES_PENDING,
+
+	/* Don't touch - intentionally last */
+	BT_MESH_FLAG_COUNT,
+};
+
+struct bt_mesh_net {
+	u32_t iv_index; /* Current IV Index */
+	u32_t seq;      /* Next outgoing sequence number (24 bits) */
+
+	ATOMIC_DEFINE(flags, BT_MESH_FLAG_COUNT);
+
+	/* Local network interface */
+	struct ble_npl_callout local_work;
+	struct net_buf_slist_t local_queue;
+
+#if MYNEWT_VAL(BLE_MESH_FRIEND)
+	/* Friend state, unique for each LPN that we're Friends for */
+	struct bt_mesh_friend frnd[MYNEWT_VAL(BLE_MESH_FRIEND_LPN_COUNT)];
+#endif
+
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+	struct bt_mesh_lpn lpn;  /* Low Power Node state */
+#endif
+
+	/* Number of hours in current IV Update state */
+	u8_t  ivu_duration;
+
+	/* Timer to track duration in current IV Update state */
+	struct k_delayed_work ivu_timer;
+
+	u8_t dev_key[16];
+
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+	struct bt_mesh_node nodes[MYNEWT_VAL(BLE_MESH_NODE_COUNT)];
+#endif
+
+	struct bt_mesh_app_key app_keys[MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT)];
+
+	struct bt_mesh_subnet sub[MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)];
+
+	struct bt_mesh_rpl rpl[MYNEWT_VAL(BLE_MESH_CRPL)];
+};
+
+/* Network interface */
+enum bt_mesh_net_if {
+	BT_MESH_NET_IF_ADV,
+	BT_MESH_NET_IF_LOCAL,
+	BT_MESH_NET_IF_PROXY,
+	BT_MESH_NET_IF_PROXY_CFG,
+};
+
+/* Decoding context for Network/Transport data */
+struct bt_mesh_net_rx {
+	struct bt_mesh_subnet *sub;
+	struct bt_mesh_msg_ctx ctx;
+	u32_t  seq;            /* Sequence Number */
+	u8_t   old_iv:1,       /* iv_index - 1 was used */
+	       new_key:1,      /* Data was encrypted with updated key */
+	       friend_cred:1,  /* Data was encrypted with friend cred */
+	       ctl:1,          /* Network Control */
+	       net_if:2,       /* Network interface */
+	       local_match:1,  /* Matched a local element */
+	       friend_match:1; /* Matched an LPN we're friends for */
+	u16_t  msg_cache_idx;  /* Index of entry in message cache */
+};
+
+/* Encoding context for Network/Transport data */
+struct bt_mesh_net_tx {
+	struct bt_mesh_subnet *sub;
+	struct bt_mesh_msg_ctx *ctx;
+	u16_t src;
+	u8_t  xmit;
+	u8_t  friend_cred:1,
+	      aszmic:1,
+	      aid:6;
+};
+
+extern struct bt_mesh_net bt_mesh;
+
+#define BT_MESH_NET_IVI_TX (bt_mesh.iv_index - \
+			    atomic_test_bit(bt_mesh.flags, \
+					    BT_MESH_IVU_IN_PROGRESS))
+#define BT_MESH_NET_IVI_RX(rx) (bt_mesh.iv_index - (rx)->old_iv)
+
+#define BT_MESH_NET_HDR_LEN 9
+
+int bt_mesh_net_keys_create(struct bt_mesh_subnet_keys *keys,
+			    const u8_t key[16]);
+
+int bt_mesh_net_create(u16_t idx, u8_t flags, const u8_t key[16],
+		       u32_t iv_index);
+
+u8_t bt_mesh_net_flags(struct bt_mesh_subnet *sub);
+
+bool bt_mesh_kr_update(struct bt_mesh_subnet *sub, u8_t new_kr, bool new_key);
+
+void bt_mesh_net_revoke_keys(struct bt_mesh_subnet *sub);
+
+int bt_mesh_net_beacon_update(struct bt_mesh_subnet *sub);
+
+void bt_mesh_rpl_reset(void);
+
+bool bt_mesh_net_iv_update(u32_t iv_index, bool iv_update);
+
+void bt_mesh_net_sec_update(struct bt_mesh_subnet *sub);
+
+struct bt_mesh_subnet *bt_mesh_subnet_get(u16_t net_idx);
+
+struct bt_mesh_subnet *bt_mesh_subnet_find(const u8_t net_id[8], u8_t flags,
+					   u32_t iv_index, const u8_t auth[8],
+					   bool *new_key);
+
+int bt_mesh_net_encode(struct bt_mesh_net_tx *tx, struct os_mbuf *buf,
+		       bool proxy);
+
+int bt_mesh_net_send(struct bt_mesh_net_tx *tx, struct os_mbuf *buf,
+		     const struct bt_mesh_send_cb *cb, void *cb_data);
+
+int bt_mesh_net_resend(struct bt_mesh_subnet *sub, struct os_mbuf *buf,
+		       bool new_key, const struct bt_mesh_send_cb *cb,
+		       void *cb_data);
+
+int bt_mesh_net_decode(struct os_mbuf *data, enum bt_mesh_net_if net_if,
+		       struct bt_mesh_net_rx *rx, struct os_mbuf *buf);
+
+void bt_mesh_net_recv(struct os_mbuf *data, s8_t rssi,
+		      enum bt_mesh_net_if net_if);
+
+u32_t bt_mesh_next_seq(void);
+
+void bt_mesh_net_start(void);
+
+void bt_mesh_net_init(void);
+void bt_mesh_net_header_parse(struct os_mbuf *buf,
+			      struct bt_mesh_net_rx *rx);
+
+/* Friendship Credential Management */
+struct friend_cred {
+	u16_t net_idx;
+	u16_t addr;
+
+	u16_t lpn_counter;
+	u16_t frnd_counter;
+
+	struct {
+		u8_t nid;         /* NID */
+		u8_t enc[16];     /* EncKey */
+		u8_t privacy[16]; /* PrivacyKey */
+	} cred[2];
+};
+
+int friend_cred_get(struct bt_mesh_subnet *sub, u16_t addr, u8_t *nid,
+			    const u8_t **enc, const u8_t **priv);
+int friend_cred_set(struct friend_cred *cred, u8_t idx, const u8_t net_key[16]);
+void friend_cred_refresh(u16_t net_idx);
+int friend_cred_update(struct bt_mesh_subnet *sub);
+struct friend_cred *friend_cred_create(struct bt_mesh_subnet *sub, u16_t addr,
+				       u16_t lpn_counter, u16_t frnd_counter);
+void friend_cred_clear(struct friend_cred *cred);
+int friend_cred_del(u16_t net_idx, u16_t addr);
+
+static inline void send_cb_finalize(const struct bt_mesh_send_cb *cb,
+				    void *cb_data)
+{
+	if (!cb) {
+		return;
+	}
+
+	if (cb->start) {
+		cb->start(0, 0, cb_data);
+	}
+
+	if (cb->end) {
+		cb->end(0, cb_data);
+	}
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/nodes.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/nodes.c
new file mode 100644
index 00000000..127ef21e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/nodes.c
@@ -0,0 +1,161 @@
+/*
+ * Copyright (c) 2019 Tobias Svehagen
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_PROV_LOG
+
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+
+#include "mesh/mesh.h"
+
+#include "mesh_priv.h"
+#include "net.h"
+#include "access.h"
+#include "settings.h"
+
+/*
+ * Check if an address range from addr_start for addr_start + num_elem - 1 is
+ * free for use. When a conflict is found, next will be set to the next address
+ * available after the conflicting range and -EAGAIN will be returned.
+ */
+static int addr_is_free(u16_t addr_start, u8_t num_elem, u16_t *next)
+{
+	const struct bt_mesh_comp *comp = bt_mesh_comp_get();
+	u16_t addr_end = addr_start + num_elem - 1;
+	u16_t other_start, other_end;
+	int i;
+
+	if (comp == NULL) {
+		return -EINVAL;
+	}
+
+	if (!BT_MESH_ADDR_IS_UNICAST(addr_start) ||
+	    !BT_MESH_ADDR_IS_UNICAST(addr_end) ||
+	    num_elem == 0 || next == NULL) {
+		return -EINVAL;
+	}
+
+	other_start = bt_mesh_primary_addr();
+	other_end = other_start + comp->elem_count - 1;
+
+	/* Compare with local element addresses */
+	if (!(addr_end < other_start || addr_start > other_end)) {
+		*next = other_end + 1;
+		return -EAGAIN;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.nodes); i++) {
+		struct bt_mesh_node *node = &bt_mesh.nodes[i];
+
+		if (node->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		other_start = node->addr;
+		other_end = other_start + node->num_elem - 1;
+
+		if (!(addr_end < other_start || addr_start > other_end)) {
+			*next = other_end + 1;
+			return -EAGAIN;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Find the lowest possible starting address that can fit num_elem elements. If
+ * a free address range cannot be found, BT_MESH_ADDR_UNASSIGNED will be
+ * returned. Otherwise the first address in the range is returned.
+ *
+ * NOTE: This is quite an ineffective algorithm as it might need to look
+ *       through the array of nodes N+2 times. A more effective algorithm
+ *       could be used if the nodes were stored in a sorted list.
+ */
+static u16_t find_lowest_free_addr(u8_t num_elem)
+{
+	u16_t addr = 1, next;
+	int err, i;
+
+	/*
+	 * It takes a maximum of node count + 2 to find a free address if there
+	 * is any. +1 for our own address and +1 for making sure that the
+	 * address range is valid.
+	 */
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.nodes) + 2; ++i) {
+		err = addr_is_free(addr, num_elem, &next);
+		if (err == 0) {
+			break;
+		} else if (err != -EAGAIN) {
+			addr = BT_MESH_ADDR_UNASSIGNED;
+			break;
+		}
+
+		addr = next;
+	}
+
+	return addr;
+}
+
+struct bt_mesh_node *bt_mesh_node_find(u16_t addr)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.nodes); i++) {
+		struct bt_mesh_node *node = &bt_mesh.nodes[i];
+
+		if (addr >= node->addr &&
+		    addr <= node->addr + node->num_elem - 1) {
+			return node;
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_mesh_node *bt_mesh_node_alloc(u16_t addr, u8_t num_elem,
+					u16_t net_idx)
+{
+	int i;
+
+	BT_DBG("");
+
+	if (addr == BT_MESH_ADDR_UNASSIGNED) {
+		addr = find_lowest_free_addr(num_elem);
+		if (addr == BT_MESH_ADDR_UNASSIGNED) {
+			return NULL;
+		}
+	} else if (!addr_is_free(addr, num_elem, NULL)) {
+		return NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.nodes); i++) {
+		struct bt_mesh_node *node = &bt_mesh.nodes[i];
+
+		if (node->addr == BT_MESH_ADDR_UNASSIGNED) {
+			node->addr = addr;
+			node->num_elem = num_elem;
+			node->net_idx = net_idx;
+			return node;
+		}
+	}
+
+	return NULL;
+}
+
+void bt_mesh_node_del(struct bt_mesh_node *node, bool store)
+{
+	BT_DBG("Node addr 0x%04x store %u", node->addr, store);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS) && store) {
+		bt_mesh_clear_node(node);
+	}
+
+	node->addr = BT_MESH_ADDR_UNASSIGNED;
+	(void)memset(node->dev_key, 0, sizeof(node->dev_key));
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/nodes.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/nodes.h
new file mode 100644
index 00000000..f86193d9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/nodes.h
@@ -0,0 +1,10 @@
+/*
+ * Copyright (c) 2019 Tobias Svehagen
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+struct bt_mesh_node *bt_mesh_node_find(u16_t addr);
+struct bt_mesh_node *bt_mesh_node_alloc(u16_t addr, u8_t num_elem,
+					u16_t net_idx);
+void bt_mesh_node_del(struct bt_mesh_node *node, bool store);
\ No newline at end of file
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/prov.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/prov.c
new file mode 100644
index 00000000..fe92c0e3
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/prov.c
@@ -0,0 +1,2008 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_PROV_LOG
+
+#if MYNEWT_VAL(BLE_MESH_PROV)
+
+#include <errno.h>
+
+#include "mesh/mesh.h"
+#include "mesh_priv.h"
+
+#include "crypto.h"
+#include "atomic.h"
+#include "adv.h"
+#include "net.h"
+#include "access.h"
+#include "foundation.h"
+#include "proxy.h"
+#include "prov.h"
+#include "testing.h"
+#include "settings.h"
+#include "nodes.h"
+
+/* 3 transmissions, 20ms interval */
+#define PROV_XMIT              BT_MESH_TRANSMIT(2, 20)
+
+#define AUTH_METHOD_NO_OOB     0x00
+#define AUTH_METHOD_STATIC     0x01
+#define AUTH_METHOD_OUTPUT     0x02
+#define AUTH_METHOD_INPUT      0x03
+
+#define OUTPUT_OOB_BLINK       0x00
+#define OUTPUT_OOB_BEEP        0x01
+#define OUTPUT_OOB_VIBRATE     0x02
+#define OUTPUT_OOB_NUMBER      0x03
+#define OUTPUT_OOB_STRING      0x04
+
+#define INPUT_OOB_PUSH         0x00
+#define INPUT_OOB_TWIST        0x01
+#define INPUT_OOB_NUMBER       0x02
+#define INPUT_OOB_STRING       0x03
+
+#define PUB_KEY_NO_OOB         0x00
+#define PUB_KEY_OOB            0x01
+
+#define PROV_ERR_NONE          0x00
+#define PROV_ERR_NVAL_PDU      0x01
+#define PROV_ERR_NVAL_FMT      0x02
+#define PROV_ERR_UNEXP_PDU     0x03
+#define PROV_ERR_CFM_FAILED    0x04
+#define PROV_ERR_RESOURCES     0x05
+#define PROV_ERR_DECRYPT       0x06
+#define PROV_ERR_UNEXP_ERR     0x07
+#define PROV_ERR_ADDR          0x08
+
+#define PROV_INVITE            0x00
+#define PROV_CAPABILITIES      0x01
+#define PROV_START             0x02
+#define PROV_PUB_KEY           0x03
+#define PROV_INPUT_COMPLETE    0x04
+#define PROV_CONFIRM           0x05
+#define PROV_RANDOM            0x06
+#define PROV_DATA              0x07
+#define PROV_COMPLETE          0x08
+#define PROV_FAILED            0x09
+
+#define PROV_NO_PDU            0xff
+
+#define PROV_ALG_P256          0x00
+
+#define GPCF(gpc)              (gpc & 0x03)
+#define GPC_START(last_seg)    (((last_seg) << 2) | 0x00)
+#define GPC_ACK                0x01
+#define GPC_CONT(seg_id)       (((seg_id) << 2) | 0x02)
+#define GPC_CTL(op)            (((op) << 2) | 0x03)
+
+#define START_PAYLOAD_MAX      20
+#define CONT_PAYLOAD_MAX       23
+
+#define START_LAST_SEG(gpc)    (gpc >> 2)
+#define CONT_SEG_INDEX(gpc)    (gpc >> 2)
+
+#define BEARER_CTL(gpc)        (gpc >> 2)
+#define LINK_OPEN              0x00
+#define LINK_ACK               0x01
+#define LINK_CLOSE             0x02
+
+#define CLOSE_REASON_SUCCESS   0x00
+#define CLOSE_REASON_TIMEOUT   0x01
+#define CLOSE_REASON_FAILED    0x02
+
+#define XACT_SEG_DATA(_seg) (&link.rx.buf->om_data[20 + ((_seg - 1) * 23)])
+#define XACT_SEG_RECV(_seg) (link.rx.seg &= ~(1 << (_seg)))
+
+#define XACT_NVAL              0xff
+
+enum {
+	WAIT_PUB_KEY,          /* Waiting for local PubKey to be generated */
+	LINK_ACTIVE,           /* Link has been opened */
+	LINK_ACK_RECVD,        /* Ack for link has been received */
+	LINK_CLOSING,          /* Link is closing down */
+	SEND_PUB_KEY,          /* Waiting to send PubKey */
+	WAIT_NUMBER,           /* Waiting for number input from user */
+	WAIT_STRING,           /* Waiting for string input from user */
+	NOTIFY_INPUT_COMPLETE, /* Notify that input has been completed. */
+	LINK_INVALID,          /* Error occurred during provisioning */
+	PROVISIONER,           /* The link was opened as provisioner */
+
+	NUM_FLAGS,
+};
+
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+#define PROVISIONER_LINK 1
+#else
+#define PROVISIONER_LINK 0
+#endif
+
+struct provisioner_link {
+	struct bt_mesh_node *node;
+	u16_t addr;
+	u16_t net_idx;
+	u8_t  attention_duration;
+};
+
+struct prov_link {
+	ATOMIC_DEFINE(flags, NUM_FLAGS);
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	uint16_t conn_handle;    /* GATT connection */
+#endif
+	struct provisioner_link provisioner[PROVISIONER_LINK];
+
+	u8_t  dhkey[32];         /* Calculated DHKey */
+	u8_t  expect;            /* Next expected PDU */
+
+	u8_t  oob_method;
+	u8_t  oob_action;
+	u8_t  oob_size;
+
+	u8_t  conf[16];          /* Remote Confirmation */
+	u8_t  rand[16];          /* Local Random */
+	u8_t  auth[16];          /* Authentication Value */
+
+	u8_t  conf_salt[16];     /* ConfirmationSalt */
+	u8_t  conf_key[16];      /* ConfirmationKey */
+	u8_t  conf_inputs[145];  /* ConfirmationInputs */
+	u8_t  prov_salt[16];     /* Provisioning Salt */
+
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+	u32_t id;                /* Link ID */
+
+	struct {
+		u8_t  id;        /* Transaction ID */
+		u8_t  prev_id;   /* Previous Transaction ID */
+		u8_t  seg;       /* Bit-field of unreceived segments */
+		u8_t  last_seg;  /* Last segment (to check length) */
+		u8_t  fcs;       /* Expected FCS value */
+		struct os_mbuf *buf;
+	} rx;
+
+	struct {
+		/* Start timestamp of the transaction */
+		s64_t start;
+
+		/* Transaction id*/
+		u8_t id;
+
+		/* Pending outgoing buffer(s) */
+		struct os_mbuf *buf[3];
+
+		/* Retransmit timer */
+		struct k_delayed_work retransmit;
+	} tx;
+#endif
+
+	struct k_delayed_work prot_timer;
+};
+
+struct prov_rx {
+	u32_t link_id;
+	u8_t  xact_id;
+	u8_t  gpc;
+};
+
+#define RETRANSMIT_TIMEOUT   K_MSEC(500)
+#define BUF_TIMEOUT          K_MSEC(400)
+#define CLOSING_TIMEOUT      K_SECONDS(3)
+#define TRANSACTION_TIMEOUT  K_SECONDS(30)
+#define PROTOCOL_TIMEOUT     K_SECONDS(60)
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+#define PROV_BUF_HEADROOM 5
+#else
+#define PROV_BUF_HEADROOM 0
+static struct os_mbuf *rx_buf;
+#endif
+
+#define PROV_BUF(len) NET_BUF_SIMPLE(PROV_BUF_HEADROOM + len)
+
+static struct prov_link link;
+
+static const struct bt_mesh_prov *prov;
+
+static void pub_key_ready(const u8_t *pkey);
+
+static int reset_state(void)
+{
+	static struct bt_pub_key_cb pub_key_cb = {
+		.func = pub_key_ready,
+	};
+	int err;
+
+	k_delayed_work_cancel(&link.prot_timer);
+
+	/* Disable Attention Timer if it was set */
+	if (link.conf_inputs[0]) {
+		bt_mesh_attention(NULL, 0);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER) &&
+	    link.provisioner->node != NULL) {
+		bt_mesh_node_del(link.provisioner->node, false);
+	}
+
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+	/* Clear everything except the retransmit and protocol timer
+	 * delayed work objects.
+	 */
+	(void)memset(&link, 0, offsetof(struct prov_link, tx.retransmit));
+	link.rx.prev_id = XACT_NVAL;
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	link.rx.buf = bt_mesh_proxy_get_buf();
+#else
+	if (!rx_buf) {
+	    rx_buf = NET_BUF_SIMPLE(65);
+	}
+	net_buf_simple_init(rx_buf, 0);
+	link.rx.buf = rx_buf;
+#endif /* PB_GATT */
+
+#else /* !PB_ADV */
+	/* Clear everything except the protocol timer (k_delayed_work) */
+	(void)memset(&link, 0, offsetof(struct prov_link, prot_timer));
+#endif /* PB_ADV */
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	link.conn_handle = BLE_HS_CONN_HANDLE_NONE;
+#endif
+
+	err = bt_pub_key_gen(&pub_key_cb);
+	if (err) {
+		BT_ERR("Failed to generate public key (%d)", err);
+		return err;
+	}
+
+	return 0;
+}
+
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+static void buf_sent(int err, void *user_data)
+{
+	BT_DBG("buf_sent");
+
+	if (!link.tx.buf[0]) {
+		return;
+	}
+
+	k_delayed_work_submit(&link.tx.retransmit, RETRANSMIT_TIMEOUT);
+}
+
+static struct bt_mesh_send_cb buf_sent_cb = {
+	.end = buf_sent,
+};
+
+static void free_segments(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(link.tx.buf); i++) {
+		struct os_mbuf *buf = link.tx.buf[i];
+
+		if (!buf) {
+			break;
+		}
+
+		link.tx.buf[i] = NULL;
+		/* Mark as canceled */
+		BT_MESH_ADV(buf)->busy = 0;
+		net_buf_unref(buf);
+	}
+}
+
+static void prov_clear_tx(void)
+{
+	BT_DBG("");
+
+	k_delayed_work_cancel(&link.tx.retransmit);
+
+	free_segments();
+}
+
+static void reset_adv_link(void)
+{
+	prov_clear_tx();
+
+	if (prov->link_close) {
+		prov->link_close(BT_MESH_PROV_ADV);
+	}
+
+	reset_state();
+}
+
+static struct os_mbuf *adv_buf_create(void)
+{
+	struct os_mbuf *buf;
+
+	buf = bt_mesh_adv_create(BT_MESH_ADV_PROV, PROV_XMIT, BUF_TIMEOUT);
+	if (!buf) {
+		BT_ERR("Out of provisioning buffers");
+		assert(0);
+		return NULL;
+	}
+
+	return buf;
+}
+
+static u8_t pending_ack = XACT_NVAL;
+
+static void ack_complete(u16_t duration, int err, void *user_data)
+{
+	BT_DBG("xact %u complete", (u8_t)pending_ack);
+	pending_ack = XACT_NVAL;
+}
+
+static void gen_prov_ack_send(u8_t xact_id)
+{
+	static const struct bt_mesh_send_cb cb = {
+		.start = ack_complete,
+	};
+	const struct bt_mesh_send_cb *complete;
+	struct os_mbuf *buf;
+
+	BT_DBG("xact_id %u", xact_id);
+
+	if (pending_ack == xact_id) {
+		BT_DBG("Not sending duplicate ack");
+		return;
+	}
+
+	buf = adv_buf_create();
+	if (!buf) {
+		return;
+	}
+
+	if (pending_ack == XACT_NVAL) {
+		pending_ack = xact_id;
+		complete = &cb;
+	} else {
+		complete = NULL;
+	}
+
+	net_buf_add_be32(buf, link.id);
+	net_buf_add_u8(buf, xact_id);
+	net_buf_add_u8(buf, GPC_ACK);
+
+	bt_mesh_adv_send(buf, complete, NULL);
+	net_buf_unref(buf);
+}
+
+static void send_reliable(void)
+{
+	int i;
+
+	link.tx.start = k_uptime_get();
+
+	for (i = 0; i < ARRAY_SIZE(link.tx.buf); i++) {
+		struct os_mbuf *buf = link.tx.buf[i];
+
+		if (!buf) {
+			break;
+		}
+
+		if (i + 1 < ARRAY_SIZE(link.tx.buf) && link.tx.buf[i + 1]) {
+			bt_mesh_adv_send(buf, NULL, NULL);
+		} else {
+			bt_mesh_adv_send(buf, &buf_sent_cb, NULL);
+		}
+	}
+}
+
+static int bearer_ctl_send(u8_t op, const void *data, u8_t data_len)
+{
+	struct os_mbuf *buf;
+
+	BT_DBG("op 0x%02x data_len %u", op, data_len);
+
+	prov_clear_tx();
+
+	buf = adv_buf_create();
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	net_buf_add_be32(buf, link.id);
+	/* Transaction ID, always 0 for Bearer messages */
+	net_buf_add_u8(buf, 0x00);
+	net_buf_add_u8(buf, GPC_CTL(op));
+	net_buf_add_mem(buf, data, data_len);
+
+	link.tx.buf[0] = buf;
+	send_reliable();
+
+	return 0;
+}
+
+static u8_t last_seg(u8_t len)
+{
+	if (len <= START_PAYLOAD_MAX) {
+		return 0;
+	}
+
+	len -= START_PAYLOAD_MAX;
+
+	return 1 + (len / CONT_PAYLOAD_MAX);
+}
+
+static inline u8_t next_transaction_id(void)
+{
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		if (link.tx.id != 0x7F) {
+			link.tx.id++;
+		} else {
+			link.tx.id = 0;
+		}
+	} else {
+		if (link.tx.id != 0U && link.tx.id != 0xFF) {
+			link.tx.id++;
+		} else {
+			link.tx.id = 0x80;
+		}
+	}
+
+	return link.tx.id;
+}
+
+static int prov_send_adv(struct os_mbuf *msg)
+{
+	struct os_mbuf *start, *buf;
+	u8_t seg_len, seg_id;
+	u8_t xact_id;
+
+	BT_DBG("len %u: %s", msg->om_len, bt_hex(msg->om_data, msg->om_len));
+
+	prov_clear_tx();
+
+	start = adv_buf_create();
+	if (!start) {
+		return -ENOBUFS;
+	}
+
+	xact_id = next_transaction_id();
+	net_buf_add_be32(start, link.id);
+	net_buf_add_u8(start, xact_id);
+
+	net_buf_add_u8(start, GPC_START(last_seg(msg->om_len)));
+	net_buf_add_be16(start, msg->om_len);
+	net_buf_add_u8(start, bt_mesh_fcs_calc(msg->om_data, msg->om_len));
+
+	link.tx.buf[0] = start;
+
+	seg_len = min(msg->om_len, START_PAYLOAD_MAX);
+	BT_DBG("seg 0 len %u: %s", seg_len, bt_hex(msg->om_data, seg_len));
+	net_buf_add_mem(start, msg->om_data, seg_len);
+	net_buf_simple_pull(msg, seg_len);
+
+	buf = start;
+	for (seg_id = 1; msg->om_len > 0; seg_id++) {
+		if (seg_id >= ARRAY_SIZE(link.tx.buf)) {
+			BT_ERR("Too big message");
+			free_segments();
+			return -E2BIG;
+		}
+
+		buf = adv_buf_create();
+		if (!buf) {
+			free_segments();
+			return -ENOBUFS;
+		}
+
+		link.tx.buf[seg_id] = buf;
+
+		seg_len = min(msg->om_len, CONT_PAYLOAD_MAX);
+
+		BT_DBG("seg_id %u len %u: %s", seg_id, seg_len,
+		       bt_hex(msg->om_data, seg_len));
+
+		net_buf_add_be32(buf, link.id);
+		net_buf_add_u8(buf, xact_id);
+		net_buf_add_u8(buf, GPC_CONT(seg_id));
+		net_buf_add_mem(buf, msg->om_data, seg_len);
+		net_buf_simple_pull(msg, seg_len);
+	}
+
+	send_reliable();
+
+	return 0;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_PB_ADV) */
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+static int prov_send_gatt(struct os_mbuf *msg)
+{
+	if (link.conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+		BT_ERR("No connection handle!?");
+		return -ENOTCONN;
+	}
+
+	return bt_mesh_proxy_send(link.conn_handle, BT_MESH_PROXY_PROV, msg);
+}
+#endif /* MYNEWT_VAL(BLE_MESH_PB_GATT) */
+
+static inline int prov_send(struct os_mbuf *buf)
+{
+	k_delayed_work_submit(&link.prot_timer, PROTOCOL_TIMEOUT);
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	if (link.conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+		return prov_send_gatt(buf);
+	}
+#endif
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+	return prov_send_adv(buf);
+#else
+	return 0;
+#endif
+}
+
+static void prov_buf_init(struct os_mbuf *buf, u8_t type)
+{
+	net_buf_simple_init(buf, PROV_BUF_HEADROOM);
+	net_buf_simple_add_u8(buf, type);
+}
+
+static void prov_send_fail_msg(u8_t err)
+{
+	struct os_mbuf *buf = PROV_BUF(2);
+
+	prov_buf_init(buf, PROV_FAILED);
+	net_buf_simple_add_u8(buf, err);
+
+	if (prov_send(buf)) {
+		BT_ERR("Failed to send Provisioning Failed message");
+	}
+
+	atomic_set_bit(link.flags, LINK_INVALID);
+
+	os_mbuf_free_chain(buf);
+}
+
+static void prov_invite(const u8_t *data)
+{
+	struct os_mbuf *buf = PROV_BUF(12);
+
+	BT_DBG("Attention Duration: %u seconds", data[0]);
+
+	if (data[0]) {
+		bt_mesh_attention(NULL, data[0]);
+	}
+
+	link.conf_inputs[0] = data[0];
+
+	prov_buf_init(buf, PROV_CAPABILITIES);
+
+	/* Number of Elements supported */
+	net_buf_simple_add_u8(buf, bt_mesh_elem_count());
+
+	/* Supported algorithms - FIPS P-256 Eliptic Curve */
+	net_buf_simple_add_be16(buf, BIT(PROV_ALG_P256));
+
+	/* Public Key Type, Only "No OOB" Public Key is supported*/
+	net_buf_simple_add_u8(buf, PUB_KEY_NO_OOB);
+
+	/* Static OOB Type */
+	net_buf_simple_add_u8(buf, prov->static_val ? BIT(0) : 0x00);
+
+	/* Output OOB Size */
+	net_buf_simple_add_u8(buf, prov->output_size);
+
+	/* Output OOB Action */
+	net_buf_simple_add_be16(buf, prov->output_actions);
+
+	/* Input OOB Size */
+	net_buf_simple_add_u8(buf, prov->input_size);
+
+	/* Input OOB Action */
+	net_buf_simple_add_be16(buf, prov->input_actions);
+
+	memcpy(&link.conf_inputs[1], &buf->om_data[1], 11);
+
+	if (prov_send(buf)) {
+		BT_ERR("Failed to send capabilities");
+		goto done;
+	}
+
+	link.expect = PROV_START;
+
+done:
+	os_mbuf_free_chain(buf);
+}
+
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+static void send_invite(void)
+{
+	struct os_mbuf *inv = PROV_BUF(2);
+
+	BT_DBG("");
+
+	prov_buf_init(inv, PROV_INVITE);
+	net_buf_simple_add_u8(inv, link.provisioner->attention_duration);
+
+	link.conf_inputs[0] = link.provisioner->attention_duration;
+
+	if (prov_send(inv)) {
+		BT_ERR("Failed to send invite");
+		goto done;
+	}
+
+	link.expect = PROV_CAPABILITIES;
+
+done:
+	os_mbuf_free_chain(inv);
+}
+#endif
+
+static void send_start(void)
+{
+	struct os_mbuf *start = PROV_BUF(6);
+
+	BT_DBG("");
+
+	prov_buf_init(start, PROV_START);
+
+	net_buf_simple_add_u8(start, PROV_ALG_P256);
+	net_buf_simple_add_u8(start, PUB_KEY_NO_OOB);
+	net_buf_simple_add_u8(start, AUTH_METHOD_NO_OOB);
+	memset(link.auth, 0, sizeof(link.auth));
+
+	net_buf_simple_add_u8(start, 0); /* Auth Action */
+	net_buf_simple_add_u8(start, 0); /* Auth Size */
+
+	memcpy(&link.conf_inputs[12], &start->om_data[1], 5);
+
+	if (prov_send(start)) {
+		BT_ERR("Failed to send start");
+	}
+
+	os_mbuf_free_chain(start);
+}
+
+static void prov_capabilities(const u8_t *data)
+{
+	u16_t algorithms, output_action, input_action;
+
+	if (!IS_ENABLED(CONFIG_BT_MESH_PROVISIONER)) {
+		return;
+	}
+
+	BT_DBG("Elements: %u", data[0]);
+
+	algorithms = sys_get_be16(&data[1]);
+	BT_DBG("Algorithms:        %u", algorithms);
+
+	BT_DBG("Public Key Type:   0x%02x", data[3]);
+	BT_DBG("Static OOB Type:   0x%02x", data[4]);
+	BT_DBG("Output OOB Size:   %u", data[5]);
+
+	output_action = sys_get_be16(&data[6]);
+	BT_DBG("Output OOB Action: 0x%04x", output_action);
+
+	BT_DBG("Input OOB Size:    %u", data[8]);
+
+	input_action = sys_get_be16(&data[9]);
+	BT_DBG("Input OOB Action:  0x%04x", input_action);
+
+	if (data[0] == 0) {
+		BT_ERR("Invalid number of elements");
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	link.provisioner->node = bt_mesh_node_alloc(link.provisioner->addr,
+						    data[0],
+						    link.provisioner->net_idx);
+	if (link.provisioner->node == NULL) {
+		prov_send_fail_msg(PROV_ERR_RESOURCES);
+		return;
+	}
+
+	memcpy(&link.conf_inputs[1], data, 11);
+
+	atomic_set_bit(link.flags, SEND_PUB_KEY);
+
+	send_start();
+}
+
+static bt_mesh_output_action_t output_action(u8_t action)
+{
+	switch (action) {
+	case OUTPUT_OOB_BLINK:
+		return BT_MESH_BLINK;
+	case OUTPUT_OOB_BEEP:
+		return BT_MESH_BEEP;
+	case OUTPUT_OOB_VIBRATE:
+		return BT_MESH_VIBRATE;
+	case OUTPUT_OOB_NUMBER:
+		return BT_MESH_DISPLAY_NUMBER;
+	case OUTPUT_OOB_STRING:
+		return BT_MESH_DISPLAY_STRING;
+	default:
+		return BT_MESH_NO_OUTPUT;
+	}
+}
+
+static bt_mesh_input_action_t input_action(u8_t action)
+{
+	switch (action) {
+	case INPUT_OOB_PUSH:
+		return BT_MESH_PUSH;
+	case INPUT_OOB_TWIST:
+		return BT_MESH_TWIST;
+	case INPUT_OOB_NUMBER:
+		return BT_MESH_ENTER_NUMBER;
+	case INPUT_OOB_STRING:
+		return BT_MESH_ENTER_STRING;
+	default:
+		return BT_MESH_NO_INPUT;
+	}
+}
+
+static int prov_auth(u8_t method, u8_t action, u8_t size)
+{
+	bt_mesh_output_action_t output;
+	bt_mesh_input_action_t input;
+
+	switch (method) {
+	case AUTH_METHOD_NO_OOB:
+		if (action || size) {
+			return -EINVAL;
+		}
+
+		memset(link.auth, 0, sizeof(link.auth));
+		return 0;
+	case AUTH_METHOD_STATIC:
+		if (action || size) {
+			return -EINVAL;
+		}
+
+		memcpy(link.auth + 16 - prov->static_val_len,
+		       prov->static_val, prov->static_val_len);
+		memset(link.auth, 0, sizeof(link.auth) - prov->static_val_len);
+		return 0;
+
+	case AUTH_METHOD_OUTPUT:
+		output = output_action(action);
+		if (!output) {
+			return -EINVAL;
+		}
+
+		if (!(prov->output_actions & output)) {
+			return -EINVAL;
+		}
+
+		if (size > prov->output_size) {
+			return -EINVAL;
+		}
+
+		atomic_set_bit(link.flags, NOTIFY_INPUT_COMPLETE);
+
+		if (output == BT_MESH_DISPLAY_STRING) {
+			unsigned char str[9];
+			u8_t i;
+
+			bt_rand(str, size);
+
+			/* Normalize to '0' .. '9' & 'A' .. 'Z' */
+			for (i = 0; i < size; i++) {
+				str[i] %= 36;
+				if (str[i] < 10) {
+					str[i] += '0';
+				} else {
+					str[i] += 'A' - 10;
+				}
+			}
+			str[size] = '\0';
+
+			memcpy(link.auth, str, size);
+			memset(link.auth + size, 0, sizeof(link.auth) - size);
+
+			return prov->output_string((char *)str);
+		} else {
+			u32_t div[8] = { 10, 100, 1000, 10000, 100000,
+					    1000000, 10000000, 100000000 };
+			u32_t num;
+
+			bt_rand(&num, sizeof(num));
+			num %= div[size - 1];
+
+			sys_put_be32(num, &link.auth[12]);
+			memset(link.auth, 0, 12);
+
+			return prov->output_number(output, num);
+		}
+
+	case AUTH_METHOD_INPUT:
+		input = input_action(action);
+		if (!input) {
+			return -EINVAL;
+		}
+
+		if (!(prov->input_actions & input)) {
+			return -EINVAL;
+		}
+
+		if (size > prov->input_size) {
+			return -EINVAL;
+		}
+
+		if (input == BT_MESH_ENTER_STRING) {
+			atomic_set_bit(link.flags, WAIT_STRING);
+		} else {
+			atomic_set_bit(link.flags, WAIT_NUMBER);
+		}
+
+		return prov->input(input, size);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static void prov_start(const u8_t *data)
+{
+	BT_DBG("Algorithm:   0x%02x", data[0]);
+	BT_DBG("Public Key:  0x%02x", data[1]);
+	BT_DBG("Auth Method: 0x%02x", data[2]);
+	BT_DBG("Auth Action: 0x%02x", data[3]);
+	BT_DBG("Auth Size:   0x%02x", data[4]);
+
+	if (data[0] != PROV_ALG_P256) {
+		BT_ERR("Unknown algorithm 0x%02x", data[0]);
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	if (data[1] != PUB_KEY_NO_OOB) {
+		BT_ERR("Invalid public key type: 0x%02x", data[1]);
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	memcpy(&link.conf_inputs[12], data, 5);
+
+	/* TODO: reset link when auth fails? */
+	link.expect = PROV_PUB_KEY;
+
+	if (prov_auth(data[2], data[3], data[4]) < 0) {
+		BT_ERR("Invalid authentication method: 0x%02x; "
+			"action: 0x%02x; size: 0x%02x", data[2], data[3],
+			data[4]);
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+	}
+}
+
+static void send_confirm(void)
+{
+	struct os_mbuf *cfm = PROV_BUF(17);
+
+	BT_DBG("ConfInputs[0]   %s", bt_hex(link.conf_inputs, 64));
+	BT_DBG("ConfInputs[64]  %s", bt_hex(&link.conf_inputs[64], 64));
+	BT_DBG("ConfInputs[128] %s", bt_hex(&link.conf_inputs[128], 17));
+
+	if (bt_mesh_prov_conf_salt(link.conf_inputs, link.conf_salt)) {
+		BT_ERR("Unable to generate confirmation salt");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("ConfirmationSalt: %s", bt_hex(link.conf_salt, 16));
+
+	if (bt_mesh_prov_conf_key(link.dhkey, link.conf_salt, link.conf_key)) {
+		BT_ERR("Unable to generate confirmation key");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("ConfirmationKey: %s", bt_hex(link.conf_key, 16));
+
+	if (bt_rand(link.rand, 16)) {
+		BT_ERR("Unable to generate random number");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("LocalRandom: %s", bt_hex(link.rand, 16));
+
+	prov_buf_init(cfm, PROV_CONFIRM);
+
+	if (bt_mesh_prov_conf(link.conf_key, link.rand, link.auth,
+			      net_buf_simple_add(cfm, 16))) {
+		BT_ERR("Unable to generate confirmation value");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	if (prov_send(cfm)) {
+		BT_ERR("Failed to send Provisioning Confirm");
+		goto done;
+	}
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		link.expect = PROV_CONFIRM;
+	} else {
+		link.expect = PROV_RANDOM;
+	}
+
+done:
+	os_mbuf_free_chain(cfm);
+}
+
+static void send_input_complete(void)
+{
+	struct os_mbuf *buf = PROV_BUF(1);
+
+	prov_buf_init(buf, PROV_INPUT_COMPLETE);
+	if (prov_send(buf)) {
+		BT_ERR("Failed to send Provisioning Input Complete");
+	}
+	link.expect = PROV_CONFIRM;
+
+	os_mbuf_free_chain(buf);
+}
+
+int bt_mesh_input_number(u32_t num)
+{
+	BT_DBG("%u", (unsigned) num);
+
+	if (!atomic_test_and_clear_bit(link.flags, WAIT_NUMBER)) {
+		return -EINVAL;
+	}
+
+	sys_put_be32(num, &link.auth[12]);
+
+	send_input_complete();
+
+	return 0;
+}
+
+int bt_mesh_input_string(const char *str)
+{
+	BT_DBG("%s", str);
+
+	if (!atomic_test_and_clear_bit(link.flags, WAIT_STRING)) {
+		return -EINVAL;
+	}
+
+	strncpy((char *)link.auth, str, prov->input_size);
+
+	send_input_complete();
+
+	return 0;
+}
+
+static void send_pub_key(void)
+{
+	struct os_mbuf *buf = PROV_BUF(65);
+	const u8_t *key;
+
+	key = bt_pub_key_get();
+	if (!key) {
+		BT_ERR("No public key available");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("Local Public Key: %s", bt_hex(key, 64));
+
+	prov_buf_init(buf, PROV_PUB_KEY);
+
+	/* Swap X and Y halves independently to big-endian */
+	sys_memcpy_swap(net_buf_simple_add(buf, 32), key, 32);
+	sys_memcpy_swap(net_buf_simple_add(buf, 32), &key[32], 32);
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		/* PublicKeyProvisioner */
+		memcpy(&link.conf_inputs[17], &buf->om_data[1], 64);
+	} else {
+		/* PublicKeyRemote */
+		memcpy(&link.conf_inputs[81], &buf->om_data[1], 64);
+	}
+
+	if (prov_send(buf)) {
+		BT_ERR("Failed to send Public Key");
+		goto done;
+	}
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		link.expect = PROV_PUB_KEY;
+	} else {
+		if (atomic_test_bit(link.flags, WAIT_NUMBER) ||
+		    atomic_test_bit(link.flags, WAIT_STRING)) {
+			link.expect = PROV_NO_PDU; /* Wait for input */
+		} else {
+			link.expect = PROV_CONFIRM;
+		}
+	}
+
+done:
+	os_mbuf_free_chain(buf);
+}
+
+static void prov_dh_key_cb(const u8_t dhkey[32])
+{
+	BT_DBG("%p", dhkey);
+
+	if (!dhkey) {
+		BT_ERR("DHKey generation failed");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		return;
+	}
+
+	sys_memcpy_swap(link.dhkey, dhkey, 32);
+
+	BT_DBG("DHkey: %s", bt_hex(link.dhkey, 32));
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		send_confirm();
+	} else {
+		send_pub_key();
+	}
+}
+
+static void prov_dh_key_gen(void)
+{
+	u8_t remote_pk_le[64], *remote_pk;
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		remote_pk = &link.conf_inputs[81];
+	} else {
+		remote_pk = &link.conf_inputs[17];
+	}
+
+	/* Copy remote key in little-endian for bt_dh_key_gen().
+	 * X and Y halves are swapped independently. The bt_dh_key_gen()
+	 * will also take care of validating the remote public key.
+	 */
+	sys_memcpy_swap(remote_pk_le, remote_pk, 32);
+	sys_memcpy_swap(&remote_pk_le[32], &remote_pk[32], 32);
+
+	if (bt_dh_key_gen(remote_pk_le, prov_dh_key_cb)) {
+		BT_ERR("Failed to generate DHKey");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+	}
+}
+
+static void prov_pub_key(const u8_t *data)
+{
+	BT_DBG("Remote Public Key: %s", bt_hex(data, 64));
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		/* PublicKeyDevice */
+		memcpy(&link.conf_inputs[81], data, 64);
+
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+		prov_clear_tx();
+#endif
+	} else {
+		/* PublicKeyProvisioner */
+		memcpy(&link.conf_inputs[17], data, 64);
+
+		if (!bt_pub_key_get()) {
+			/* Clear retransmit timer */
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+			prov_clear_tx();
+#endif
+
+			atomic_set_bit(link.flags, WAIT_PUB_KEY);
+			BT_WARN("Waiting for local public key");
+			return;
+		}
+	}
+
+	prov_dh_key_gen();
+}
+
+static void pub_key_ready(const u8_t *pkey)
+{
+	if (!pkey) {
+		BT_WARN("Public key not available");
+		return;
+	}
+
+	BT_DBG("Local public key ready");
+
+	if (atomic_test_and_clear_bit(link.flags, WAIT_PUB_KEY)) {
+		if (atomic_test_bit(link.flags, PROVISIONER)) {
+			send_pub_key();
+		} else {
+			prov_dh_key_gen();
+		}
+	}
+}
+
+static void notify_input_complete(void)
+{
+	if (atomic_test_and_clear_bit(link.flags, NOTIFY_INPUT_COMPLETE) &&
+	    prov->input_complete) {
+		prov->input_complete();
+	}
+}
+
+static void prov_input_complete(const u8_t *data)
+{
+	BT_DBG("");
+	notify_input_complete();
+}
+
+static void send_prov_data(void)
+{
+	struct os_mbuf *pdu = PROV_BUF(34);
+	struct bt_mesh_subnet *sub;
+	u8_t session_key[16];
+	u8_t nonce[13];
+	int err;
+
+	err = bt_mesh_session_key(link.dhkey, link.prov_salt, session_key);
+	if (err) {
+		BT_ERR("Unable to generate session key");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("SessionKey: %s", bt_hex(session_key, 16));
+
+	err = bt_mesh_prov_nonce(link.dhkey, link.prov_salt, nonce);
+	if (err) {
+		BT_ERR("Unable to generate session nonce");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("Nonce: %s", bt_hex(nonce, 13));
+
+	err = bt_mesh_dev_key(link.dhkey, link.prov_salt,
+			      link.provisioner->node->dev_key);
+	if (err) {
+		BT_ERR("Unable to generate device key");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("DevKey: %s", bt_hex(link.provisioner->node->dev_key, 16));
+
+	sub = bt_mesh_subnet_get(link.provisioner->node->net_idx);
+	if (sub == NULL) {
+		BT_ERR("No subnet with net_idx %u",
+		       link.provisioner->node->net_idx);
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	prov_buf_init(pdu, PROV_DATA);
+	net_buf_simple_add_mem(pdu, sub->keys[sub->kr_flag].net, 16);
+	net_buf_simple_add_be16(pdu, link.provisioner->node->net_idx);
+	net_buf_simple_add_u8(pdu, bt_mesh_net_flags(sub));
+	net_buf_simple_add_be32(pdu, bt_mesh.iv_index);
+	net_buf_simple_add_be16(pdu, link.provisioner->node->addr);
+	net_buf_simple_add(pdu, 8); /* For MIC */
+
+	BT_DBG("net_idx %u, iv_index 0x%08x, addr 0x%04x",
+	       link.provisioner->node->net_idx, bt_mesh.iv_index,
+	       link.provisioner->node->addr);
+
+	err = bt_mesh_prov_encrypt(session_key, nonce, &pdu->om_data[1],
+				   &pdu->om_data[1]);
+	if (err) {
+		BT_ERR("Unable to encrypt provisioning data");
+		prov_send_fail_msg(PROV_ERR_DECRYPT);
+		goto done;
+	}
+
+	if (prov_send(pdu)) {
+		BT_ERR("Failed to send Provisioning Data");
+		goto done;
+	}
+
+	link.expect = PROV_COMPLETE;
+
+done:
+	os_mbuf_free_chain(pdu);
+}
+
+static void prov_complete(const u8_t *data)
+{
+	if (!IS_ENABLED(CONFIG_BT_MESH_PROVISIONER)) {
+		return;
+	}
+
+	struct bt_mesh_node *node = link.provisioner->node;
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+	u8_t reason = CLOSE_REASON_SUCCESS;
+#endif
+
+	BT_DBG("key %s, net_idx %u, num_elem %u, addr 0x%04x",
+	       bt_hex(node->dev_key, 16), node->net_idx, node->num_elem,
+	       node->addr);
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_node(node);
+	}
+
+	link.provisioner->node = NULL;
+	link.expect = PROV_NO_PDU;
+	atomic_set_bit(link.flags, LINK_CLOSING);
+
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+	bearer_ctl_send(LINK_CLOSE, &reason, sizeof(reason));
+#endif
+
+	bt_mesh_prov_node_added(node->net_idx, node->addr, node->num_elem);
+
+	/*
+	 * According to mesh profile spec (5.3.1.4.3), the close message should
+	 * be restransmitted at least three times. Retransmit the LINK_CLOSE
+	 * message until CLOSING_TIMEOUT has elapsed instead of resetting the
+	 * link here.
+	 */
+}
+
+static void send_random(void)
+{
+	struct os_mbuf *rnd = PROV_BUF(17);
+
+	prov_buf_init(rnd, PROV_RANDOM);
+	net_buf_simple_add_mem(rnd, link.rand, 16);
+
+	if (prov_send(rnd)) {
+		BT_ERR("Failed to send Provisioning Random");
+		goto done;
+	}
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		link.expect = PROV_RANDOM;
+	} else {
+		link.expect = PROV_DATA;
+	}
+
+done:
+	os_mbuf_free_chain(rnd);
+}
+
+static void prov_random(const u8_t *data)
+{
+	u8_t conf_verify[16];
+	const u8_t *prov_rand, *dev_rand;
+
+	BT_DBG("Remote Random: %s", bt_hex(data, 16));
+
+	if (bt_mesh_prov_conf(link.conf_key, data, link.auth, conf_verify)) {
+		BT_ERR("Unable to calculate confirmation verification");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		return;
+	}
+
+	if (memcmp(conf_verify, link.conf, 16)) {
+		BT_ERR("Invalid confirmation value");
+		BT_DBG("Received:   %s", bt_hex(link.conf, 16));
+		BT_DBG("Calculated: %s",  bt_hex(conf_verify, 16));
+		prov_send_fail_msg(PROV_ERR_CFM_FAILED);
+		return;
+	}
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		prov_rand = link.rand;
+		dev_rand = data;
+	} else {
+		prov_rand = data;
+		dev_rand = link.rand;
+	}
+
+	if (bt_mesh_prov_salt(link.conf_salt, prov_rand, dev_rand,
+			      link.prov_salt)) {
+		BT_ERR("Failed to generate provisioning salt");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		return;
+	}
+
+	BT_DBG("ProvisioningSalt: %s", bt_hex(link.prov_salt, 16));
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER) &&
+	    atomic_test_bit(link.flags, PROVISIONER)) {
+		send_prov_data();
+	} else {
+		send_random();
+	}
+}
+
+static void prov_confirm(const u8_t *data)
+{
+	BT_DBG("Remote Confirm: %s", bt_hex(data, 16));
+
+	memcpy(link.conf, data, 16);
+
+	notify_input_complete();
+
+	if (atomic_test_bit(link.flags, PROVISIONER)) {
+		send_random();
+	} else {
+		send_confirm();
+	}
+}
+
+static inline bool is_pb_gatt(void)
+{
+#if MYNEWT_VAL(BLE_MESH_PB_GATT)
+	return (link.conn_handle != BLE_HS_CONN_HANDLE_NONE);
+#else
+	return false;
+#endif
+}
+
+static void prov_data(const u8_t *data)
+{
+	struct os_mbuf *msg = PROV_BUF(1);
+	u8_t session_key[16];
+	u8_t nonce[13];
+	u8_t dev_key[16];
+	u8_t pdu[25];
+	u8_t flags;
+	u32_t iv_index;
+	u16_t addr;
+	u16_t net_idx;
+	int err;
+	bool identity_enable;
+
+	BT_DBG("");
+
+	err = bt_mesh_session_key(link.dhkey, link.prov_salt, session_key);
+	if (err) {
+		BT_ERR("Unable to generate session key");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("SessionKey: %s", bt_hex(session_key, 16));
+
+	err = bt_mesh_prov_nonce(link.dhkey, link.prov_salt, nonce);
+	if (err) {
+		BT_ERR("Unable to generate session nonce");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("Nonce: %s", bt_hex(nonce, 13));
+
+	err = bt_mesh_prov_decrypt(session_key, nonce, data, pdu);
+	if (err) {
+		BT_ERR("Unable to decrypt provisioning data");
+		prov_send_fail_msg(PROV_ERR_DECRYPT);
+		goto done;
+	}
+
+	err = bt_mesh_dev_key(link.dhkey, link.prov_salt, dev_key);
+	if (err) {
+		BT_ERR("Unable to generate device key");
+		prov_send_fail_msg(PROV_ERR_UNEXP_ERR);
+		goto done;
+	}
+
+	BT_DBG("DevKey: %s", bt_hex(dev_key, 16));
+
+	net_idx = sys_get_be16(&pdu[16]);
+	flags = pdu[18];
+	iv_index = sys_get_be32(&pdu[19]);
+	addr = sys_get_be16(&pdu[23]);
+
+	BT_DBG("net_idx %u iv_index 0x%08x, addr 0x%04x",
+	       net_idx, (unsigned) iv_index, addr);
+
+	prov_buf_init(msg, PROV_COMPLETE);
+	if (prov_send(msg)) {
+		BT_ERR("Failed to send Provisioning Complete");
+		goto done;
+	}
+
+	/* Ignore any further PDUs on this link */
+	link.expect = PROV_NO_PDU;
+
+	/* Store info, since bt_mesh_provision() will end up clearing it */
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY)) {
+		identity_enable = is_pb_gatt();
+	} else {
+		identity_enable = false;
+	}
+
+	err = bt_mesh_provision(pdu, net_idx, flags, iv_index, addr, dev_key);
+	if (err) {
+		BT_ERR("Failed to provision (err %d)", err);
+		goto done;
+	}
+
+	/* After PB-GATT provisioning we should start advertising
+	 * using Node Identity.
+	 */
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY) && identity_enable) {
+		bt_mesh_proxy_identity_enable();
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+}
+
+static void prov_failed(const u8_t *data)
+{
+	BT_WARN("Error: 0x%02x", data[0]);
+}
+
+static const struct {
+	void (*func)(const u8_t *data);
+	u16_t len;
+} prov_handlers[] = {
+	{ prov_invite, 1 },
+	{ prov_capabilities, 11 },
+	{ prov_start, 5, },
+	{ prov_pub_key, 64 },
+	{ prov_input_complete, 0 },
+	{ prov_confirm, 16 },
+	{ prov_random, 16 },
+	{ prov_data, 33 },
+	{ prov_complete, 0 },
+	{ prov_failed, 1 },
+};
+
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+static void prov_retransmit(struct ble_npl_event *work)
+{
+	int i, timeout;
+
+	BT_DBG("");
+
+	if (!atomic_test_bit(link.flags, LINK_ACTIVE)) {
+		BT_WARN("Link not active");
+		return;
+	}
+
+	if (atomic_test_bit(link.flags, LINK_CLOSING)) {
+		timeout = CLOSING_TIMEOUT;
+	} else {
+		timeout = TRANSACTION_TIMEOUT;
+	}
+
+	if (k_uptime_get() - link.tx.start > timeout) {
+		BT_WARN("Giving up transaction");
+		reset_adv_link();
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(link.tx.buf); i++) {
+		struct os_mbuf *buf = link.tx.buf[i];
+
+		if (!buf) {
+			break;
+		}
+
+		if (BT_MESH_ADV(buf)->busy) {
+			continue;
+		}
+
+		BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+		if (i + 1 < ARRAY_SIZE(link.tx.buf) && link.tx.buf[i + 1]) {
+			bt_mesh_adv_send(buf, NULL, NULL);
+		} else {
+			bt_mesh_adv_send(buf, &buf_sent_cb, NULL);
+		}
+
+	}
+}
+
+static void link_open(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	BT_DBG("link open: len %u", buf->om_len);
+
+	if (buf->om_len < 16) {
+		BT_ERR("Too short bearer open message (len %u)", buf->om_len);
+		return;
+	}
+
+	if (atomic_test_bit(link.flags, LINK_ACTIVE)) {
+		/* Send another link ack if the provisioner missed the last */
+		if (link.id == rx->link_id && link.expect == PROV_INVITE) {
+			BT_DBG("Resending link ack");
+			bearer_ctl_send(LINK_ACK, NULL, 0);
+		} else {
+			BT_WARN("Ignoring bearer open: link already active");
+		}
+
+		return;
+	}
+
+	if (memcmp(buf->om_data, prov->uuid, 16)) {
+		BT_DBG("Bearer open message not for us");
+		return;
+	}
+
+	if (prov->link_open) {
+		prov->link_open(BT_MESH_PROV_ADV);
+	}
+
+	link.id = rx->link_id;
+	atomic_set_bit(link.flags, LINK_ACTIVE);
+	net_buf_simple_init(link.rx.buf, 0);
+
+	bearer_ctl_send(LINK_ACK, NULL, 0);
+
+	link.expect = PROV_INVITE;
+}
+
+static void link_ack(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	BT_DBG("Link ack: len %u", buf->om_len);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER) &&
+	    atomic_test_bit(link.flags, PROVISIONER)) {
+		if (atomic_test_and_set_bit(link.flags, LINK_ACK_RECVD)) {
+			return;
+		}
+
+		prov_clear_tx();
+
+		if (prov->link_open) {
+			prov->link_open(BT_MESH_PROV_ADV);
+		}
+
+		send_invite();
+	}
+}
+
+static void link_close(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	BT_DBG("Link close: len %u", buf->om_len);
+
+	reset_adv_link();
+}
+
+static void gen_prov_ctl(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	BT_DBG("op 0x%02x len %u", BEARER_CTL(rx->gpc), buf->om_len);
+
+	switch (BEARER_CTL(rx->gpc)) {
+	case LINK_OPEN:
+		link_open(rx, buf);
+		break;
+	case LINK_ACK:
+		if (!atomic_test_bit(link.flags, LINK_ACTIVE)) {
+			return;
+		}
+
+		link_ack(rx, buf);
+		break;
+	case LINK_CLOSE:
+		if (!atomic_test_bit(link.flags, LINK_ACTIVE)) {
+			return;
+		}
+
+		link_close(rx, buf);
+		break;
+	default:
+		BT_ERR("Unknown bearer opcode: 0x%02x", BEARER_CTL(rx->gpc));
+
+		if (IS_ENABLED(CONFIG_BT_TESTING)) {
+			bt_test_mesh_prov_invalid_bearer(BEARER_CTL(rx->gpc));
+		}
+
+		return;
+	}
+}
+
+static void prov_msg_recv(void)
+{
+	u8_t type = link.rx.buf->om_data[0];
+
+	BT_DBG("type 0x%02x len %u", type, link.rx.buf->om_len);
+
+	k_delayed_work_submit(&link.prot_timer, PROTOCOL_TIMEOUT);
+
+	if (!bt_mesh_fcs_check(link.rx.buf, link.rx.fcs)) {
+		BT_ERR("Incorrect FCS");
+		return;
+	}
+
+	gen_prov_ack_send(link.rx.id);
+	link.rx.prev_id = link.rx.id;
+	link.rx.id = 0;
+
+	if (atomic_test_bit(link.flags, LINK_INVALID)) {
+		BT_WARN("Unexpected msg 0x%02x on invalidated link", type);
+		prov_send_fail_msg(PROV_ERR_UNEXP_PDU);
+		return;
+	}
+
+	if (type != PROV_FAILED && type != link.expect) {
+		BT_WARN("Unexpected msg 0x%02x != 0x%02x", type, link.expect);
+		prov_send_fail_msg(PROV_ERR_UNEXP_PDU);
+		return;
+	}
+
+	if (type >= ARRAY_SIZE(prov_handlers)) {
+		BT_ERR("Unknown provisioning PDU type 0x%02x", type);
+		prov_send_fail_msg(PROV_ERR_NVAL_PDU);
+		return;
+	}
+
+	if (1 + prov_handlers[type].len != link.rx.buf->om_len) {
+		BT_ERR("Invalid length %u for type 0x%02x",
+		       link.rx.buf->om_len, type);
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	prov_handlers[type].func(&link.rx.buf->om_data[1]);
+}
+
+static void gen_prov_cont(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	u8_t seg = CONT_SEG_INDEX(rx->gpc);
+
+	BT_DBG("len %u, seg_index %u", buf->om_len, seg);
+
+	if (!link.rx.seg && link.rx.prev_id == rx->xact_id) {
+		BT_WARN("Resending ack");
+		gen_prov_ack_send(rx->xact_id);
+		return;
+	}
+
+	if (rx->xact_id != link.rx.id) {
+		BT_WARN("Data for unknown transaction (%u != %u)",
+			rx->xact_id, link.rx.id);
+		return;
+	}
+
+	if (seg > link.rx.last_seg) {
+		BT_ERR("Invalid segment index %u", seg);
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	} else if (seg == link.rx.last_seg) {
+		u8_t expect_len;
+
+		expect_len = (link.rx.buf->om_len - 20 -
+			      ((link.rx.last_seg - 1) * 23));
+		if (expect_len != buf->om_len) {
+			BT_ERR("Incorrect last seg len: %u != %u",
+			       expect_len, buf->om_len);
+			prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+			return;
+		}
+	}
+
+	if (!(link.rx.seg & BIT(seg))) {
+		BT_WARN("Ignoring already received segment");
+		return;
+	}
+
+	memcpy(XACT_SEG_DATA(seg), buf->om_data, buf->om_len);
+	XACT_SEG_RECV(seg);
+
+	if (!link.rx.seg) {
+		prov_msg_recv();
+	}
+}
+
+static void gen_prov_ack(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	BT_DBG("len %u", buf->om_len);
+
+	if (!link.tx.buf[0]) {
+		return;
+	}
+
+	if (rx->xact_id == link.tx.id) {
+		/* Don't clear resending of LINK_CLOSE messages */
+		if (!atomic_test_bit(link.flags, LINK_CLOSING)) {
+			prov_clear_tx();
+		}
+
+		/* Send the PubKey when the the Start message is ACK'ed */
+		if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER) &&
+		    atomic_test_and_clear_bit(link.flags, SEND_PUB_KEY)) {
+			if (!bt_pub_key_get()) {
+				atomic_set_bit(link.flags, WAIT_PUB_KEY);
+				BT_WARN("Waiting for local public key");
+			} else {
+				send_pub_key();
+			}
+		}
+	}
+}
+
+static void gen_prov_start(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	u16_t trailing_space = 0;
+
+	if (link.rx.seg) {
+		BT_WARN("Got Start while there are unreceived segments");
+		return;
+	}
+
+	if (link.rx.prev_id == rx->xact_id) {
+		BT_WARN("Resending ack");
+		gen_prov_ack_send(rx->xact_id);
+		return;
+	}
+
+	trailing_space = OS_MBUF_TRAILINGSPACE(link.rx.buf);
+
+	link.rx.buf->om_len = net_buf_simple_pull_be16(buf);
+	link.rx.id  = rx->xact_id;
+	link.rx.fcs = net_buf_simple_pull_u8(buf);
+
+	BT_DBG("len %u last_seg %u total_len %u fcs 0x%02x", buf->om_len,
+	       START_LAST_SEG(rx->gpc), link.rx.buf->om_len, link.rx.fcs);
+
+	if (link.rx.buf->om_len < 1) {
+		BT_ERR("Ignoring zero-length provisioning PDU");
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	if (link.rx.buf->om_len > trailing_space) {
+		BT_ERR("Too large provisioning PDU (%u bytes)",
+		       link.rx.buf->om_len);
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	if (START_LAST_SEG(rx->gpc) > 0 && link.rx.buf->om_len <= 20) {
+		BT_ERR("Too small total length for multi-segment PDU");
+		prov_send_fail_msg(PROV_ERR_NVAL_FMT);
+		return;
+	}
+
+	link.rx.seg = (1 << (START_LAST_SEG(rx->gpc) + 1)) - 1;
+	link.rx.last_seg = START_LAST_SEG(rx->gpc);
+	memcpy(link.rx.buf->om_data, buf->om_data, buf->om_len);
+	XACT_SEG_RECV(0);
+
+	if (!link.rx.seg) {
+		prov_msg_recv();
+	}
+}
+
+static const struct {
+	void (*func)(struct prov_rx *rx, struct os_mbuf *buf);
+	bool require_link;
+	u8_t min_len;
+} gen_prov[] = {
+	{ gen_prov_start, true, 3 },
+	{ gen_prov_ack, true, 0 },
+	{ gen_prov_cont, true, 0 },
+	{ gen_prov_ctl, false, 0 },
+};
+
+static void gen_prov_recv(struct prov_rx *rx, struct os_mbuf *buf)
+{
+	if (buf->om_len < gen_prov[GPCF(rx->gpc)].min_len) {
+		BT_ERR("Too short GPC message type %u", GPCF(rx->gpc));
+		return;
+	}
+
+	if (!atomic_test_bit(link.flags, LINK_ACTIVE) &&
+	    gen_prov[GPCF(rx->gpc)].require_link) {
+		BT_DBG("Ignoring message that requires active link");
+		return;
+	}
+
+	BT_DBG("prov_action: %d", GPCF(rx->gpc));
+	gen_prov[GPCF(rx->gpc)].func(rx, buf);
+}
+
+void bt_mesh_pb_adv_recv(struct os_mbuf *buf)
+{
+	struct prov_rx rx;
+
+	if (!bt_prov_active() && bt_mesh_is_provisioned()) {
+		BT_DBG("Ignoring provisioning PDU - already provisioned");
+		return;
+	}
+
+	if (buf->om_len < 6) {
+		BT_WARN("Too short provisioning packet (len %u)", buf->om_len);
+		return;
+	}
+
+	rx.link_id = net_buf_simple_pull_be32(buf);
+	rx.xact_id = net_buf_simple_pull_u8(buf);
+	rx.gpc = net_buf_simple_pull_u8(buf);
+
+	BT_DBG("link_id 0x%08x xact_id %u", (unsigned) rx.link_id, rx.xact_id);
+
+	if (atomic_test_bit(link.flags, LINK_ACTIVE) && link.id != rx.link_id) {
+		BT_DBG("Ignoring mesh beacon for unknown link");
+		return;
+	}
+
+	gen_prov_recv(&rx, buf);
+}
+
+int bt_mesh_pb_adv_open(const u8_t uuid[16], u16_t net_idx, u16_t addr,
+			u8_t attention_duration)
+{
+	BT_DBG("uuid %s", bt_hex(uuid, 16));
+
+	if (atomic_test_and_set_bit(link.flags, LINK_ACTIVE)) {
+		return -EBUSY;
+	}
+
+	atomic_set_bit(link.flags, PROVISIONER);
+
+	bt_rand(&link.id, sizeof(link.id));
+	link.tx.id = 0x7F;
+	link.provisioner->addr = addr;
+	link.provisioner->net_idx = net_idx;
+	link.provisioner->attention_duration = attention_duration;
+
+	net_buf_simple_init(link.rx.buf, 0);
+
+	bearer_ctl_send(LINK_OPEN, uuid, 16);
+
+	return 0;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_PB_ADV) */
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+int bt_mesh_pb_gatt_recv(uint16_t conn_handle, struct os_mbuf *buf)
+{
+	u8_t type;
+
+	BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	if (link.conn_handle != conn_handle) {
+		BT_WARN("Data for unexpected connection");
+		return -ENOTCONN;
+	}
+
+	if (buf->om_len < 1) {
+		BT_WARN("Too short provisioning packet (len %u)", buf->om_len);
+		return -EINVAL;
+	}
+
+	type = net_buf_simple_pull_u8(buf);
+	if (type != PROV_FAILED && type != link.expect) {
+		BT_WARN("Unexpected msg 0x%02x != 0x%02x", type, link.expect);
+		prov_send_fail_msg(PROV_ERR_UNEXP_PDU);
+		return -EINVAL;
+	}
+
+	if (type >= ARRAY_SIZE(prov_handlers)) {
+		BT_ERR("Unknown provisioning PDU type 0x%02x", type);
+		return -EINVAL;
+	}
+
+	if (prov_handlers[type].len != buf->om_len) {
+		BT_ERR("Invalid length %u for type 0x%02x", buf->om_len, type);
+		return -EINVAL;
+	}
+
+	prov_handlers[type].func(buf->om_data);
+
+	return 0;
+}
+
+int bt_mesh_pb_gatt_open(uint16_t conn_handle)
+{
+	BT_DBG("conn_handle %d", conn_handle);
+
+	if (atomic_test_and_set_bit(link.flags, LINK_ACTIVE)) {
+		BT_ERR("Link already opened?");
+		return -EBUSY;
+	}
+
+	link.conn_handle = conn_handle;
+	link.expect = PROV_INVITE;
+
+	if (prov->link_open) {
+		prov->link_open(BT_MESH_PROV_GATT);
+	}
+
+	return 0;
+}
+
+int bt_mesh_pb_gatt_close(uint16_t conn_handle)
+{
+	BT_DBG("conn_handle %d", conn_handle);
+
+	if (link.conn_handle != conn_handle) {
+		BT_ERR("Not connected");
+		return -ENOTCONN;
+	}
+
+	if (prov->link_close) {
+		prov->link_close(BT_MESH_PROV_GATT);
+	}
+
+	return reset_state();
+}
+#endif /* MYNEWT_VAL(BLE_MESH_PB_GATT) */
+
+const struct bt_mesh_prov *bt_mesh_prov_get(void)
+{
+	return prov;
+}
+
+bool bt_prov_active(void)
+{
+	return atomic_test_bit(link.flags, LINK_ACTIVE);
+}
+
+static void protocol_timeout(struct ble_npl_event *work)
+{
+	BT_DBG("Protocol timeout");
+
+#if MYNEWT_VAL(BLE_MESH_PB_GATT)
+	if (link.conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+		bt_mesh_pb_gatt_close(link.conn_handle);
+		return;
+	}
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+	u8_t reason = CLOSE_REASON_TIMEOUT;
+
+	link.rx.seg = 0U;
+	bearer_ctl_send(LINK_CLOSE, &reason, sizeof(reason));
+
+	reset_state();
+#endif
+}
+
+int bt_mesh_prov_init(const struct bt_mesh_prov *prov_info)
+{
+	if (!prov_info) {
+		BT_ERR("No provisioning context provided");
+		return -EINVAL;
+	}
+
+	k_delayed_work_init(&link.prot_timer, protocol_timeout);
+
+	prov = prov_info;
+
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+	k_delayed_work_init(&link.tx.retransmit, prov_retransmit);
+#endif
+
+	return reset_state();
+}
+
+void bt_mesh_prov_reset_link(void)
+{
+#if (MYNEWT_VAL(BLE_MESH_PB_ADV))
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	link.rx.buf = bt_mesh_proxy_get_buf();
+#else
+	net_buf_simple_init(rx_buf, 0);
+	link.rx.buf = rx_buf;
+#endif
+#endif
+}
+
+void bt_mesh_prov_complete(u16_t net_idx, u16_t addr)
+{
+	if (prov->complete) {
+		prov->complete(net_idx, addr);
+	}
+}
+
+void bt_mesh_prov_node_added(u16_t net_idx, u16_t addr, u8_t num_elem)
+{
+	if (prov->node_added) {
+		prov->node_added(net_idx, addr, num_elem);
+	}
+}
+
+void bt_mesh_prov_reset(void)
+{
+	if (prov->reset) {
+		prov->reset();
+	}
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_PROV) */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/prov.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/prov.h
new file mode 100644
index 00000000..96e5a447
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/prov.h
@@ -0,0 +1,37 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __PROV_H__
+#define __PROV_H__
+
+#include "os/os_mbuf.h"
+#include "mesh/mesh.h"
+#include "../src/ble_hs_conn_priv.h"
+
+int bt_mesh_pb_adv_open(const u8_t uuid[16], u16_t net_idx, u16_t addr,
+			u8_t attention_duration);
+
+void bt_mesh_pb_adv_recv(struct os_mbuf *buf);
+
+bool bt_prov_active(void);
+
+int bt_mesh_pb_gatt_open(uint16_t conn_handle);
+int bt_mesh_pb_gatt_close(uint16_t conn_handle);
+int bt_mesh_pb_gatt_recv(uint16_t conn_handle, struct os_mbuf *buf);
+
+const struct bt_mesh_prov *bt_mesh_prov_get(void);
+
+int bt_mesh_prov_init(const struct bt_mesh_prov *prov);
+
+void bt_mesh_prov_reset_link(void);
+
+void bt_mesh_prov_complete(u16_t net_idx, u16_t addr);
+void bt_mesh_prov_node_added(u16_t net_idx, u16_t addr, u8_t num_elem);
+void bt_mesh_prov_reset(void);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/proxy.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/proxy.c
new file mode 100644
index 00000000..134a36dd
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/proxy.c
@@ -0,0 +1,1499 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_PROXY_LOG
+
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+
+#include "mesh/mesh.h"
+#include "host/ble_att.h"
+#include "services/gatt/ble_svc_gatt.h"
+#include "../../host/src/ble_hs_priv.h"
+
+#include "mesh_priv.h"
+#include "adv.h"
+#include "net.h"
+#include "prov.h"
+#include "beacon.h"
+#include "foundation.h"
+#include "access.h"
+#include "proxy.h"
+
+#define PDU_TYPE(data)     (data[0] & BIT_MASK(6))
+#define PDU_SAR(data)      (data[0] >> 6)
+
+/* Mesh Profile 1.0 Section 6.6:
+ * "The timeout for the SAR transfer is 20 seconds. When the timeout
+ *  expires, the Proxy Server shall disconnect."
+ */
+#define PROXY_SAR_TIMEOUT  K_SECONDS(20)
+
+#define SAR_COMPLETE       0x00
+#define SAR_FIRST          0x01
+#define SAR_CONT           0x02
+#define SAR_LAST           0x03
+
+#define CFG_FILTER_SET     0x00
+#define CFG_FILTER_ADD     0x01
+#define CFG_FILTER_REMOVE  0x02
+#define CFG_FILTER_STATUS  0x03
+
+/** @def BT_UUID_MESH_PROV
+ *  @brief Mesh Provisioning Service
+ */
+ble_uuid16_t BT_UUID_MESH_PROV                 = BLE_UUID16_INIT(0x1827);
+#define BT_UUID_MESH_PROV_VAL             0x1827
+/** @def BT_UUID_MESH_PROXY
+ *  @brief Mesh Proxy Service
+ */
+ble_uuid16_t BT_UUID_MESH_PROXY                = BLE_UUID16_INIT(0x1828);
+#define BT_UUID_MESH_PROXY_VAL            0x1828
+/** @def BT_UUID_GATT_CCC
+ *  @brief GATT Client Characteristic Configuration
+ */
+ble_uuid16_t BT_UUID_GATT_CCC                  = BLE_UUID16_INIT(0x2902);
+#define BT_UUID_GATT_CCC_VAL              0x2902
+/** @def BT_UUID_MESH_PROV_DATA_IN
+ *  @brief Mesh Provisioning Data In
+ */
+ble_uuid16_t BT_UUID_MESH_PROV_DATA_IN         = BLE_UUID16_INIT(0x2adb);
+#define BT_UUID_MESH_PROV_DATA_IN_VAL     0x2adb
+/** @def BT_UUID_MESH_PROV_DATA_OUT
+ *  @brief Mesh Provisioning Data Out
+ */
+ble_uuid16_t BT_UUID_MESH_PROV_DATA_OUT        = BLE_UUID16_INIT(0x2adc);
+#define BT_UUID_MESH_PROV_DATA_OUT_VAL    0x2adc
+/** @def BT_UUID_MESH_PROXY_DATA_IN
+ *  @brief Mesh Proxy Data In
+ */
+ble_uuid16_t BT_UUID_MESH_PROXY_DATA_IN        = BLE_UUID16_INIT(0x2add);
+#define BT_UUID_MESH_PROXY_DATA_IN_VAL    0x2add
+/** @def BT_UUID_MESH_PROXY_DATA_OUT
+ *  @brief Mesh Proxy Data Out
+ */
+ble_uuid16_t BT_UUID_MESH_PROXY_DATA_OUT       = BLE_UUID16_INIT(0x2ade);
+#define BT_UUID_MESH_PROXY_DATA_OUT_VAL   0x2ade
+
+#define PDU_HDR(sar, type) (sar << 6 | (type & BIT_MASK(6)))
+
+#define CLIENT_BUF_SIZE 68
+
+static const struct ble_gap_adv_params slow_adv_param = {
+	.conn_mode = (BLE_GAP_CONN_MODE_UND),
+	.disc_mode = (BLE_GAP_DISC_MODE_GEN),
+	.itvl_min = BT_GAP_ADV_SLOW_INT_MIN,
+	.itvl_max = BT_GAP_ADV_SLOW_INT_MAX,
+};
+
+static const struct ble_gap_adv_params fast_adv_param = {
+	.conn_mode = (BLE_GAP_CONN_MODE_UND),
+	.disc_mode = (BLE_GAP_DISC_MODE_GEN),
+	.itvl_min = BT_GAP_ADV_FAST_INT_MIN_2,
+	.itvl_max = BT_GAP_ADV_FAST_INT_MAX_2,
+};
+
+static bool proxy_adv_enabled;
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+static void proxy_send_beacons(struct ble_npl_event *work);
+#endif
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+static bool prov_fast_adv;
+#endif
+
+static struct bt_mesh_proxy_client {
+	uint16_t conn_handle;
+	u16_t filter[MYNEWT_VAL(BLE_MESH_PROXY_FILTER_SIZE)];
+	enum __packed {
+		NONE,
+		WHITELIST,
+		BLACKLIST,
+		PROV,
+	} filter_type;
+	u8_t msg_type;
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+	struct ble_npl_callout send_beacons;
+#endif
+	struct k_delayed_work sar_timer;
+	struct os_mbuf    *buf;
+} clients[MYNEWT_VAL(BLE_MAX_CONNECTIONS)] = {
+	[0 ... (MYNEWT_VAL(BLE_MAX_CONNECTIONS) - 1)] = { 0 },
+};
+
+/* Track which service is enabled */
+static enum {
+	MESH_GATT_NONE,
+	MESH_GATT_PROV,
+	MESH_GATT_PROXY,
+} gatt_svc = MESH_GATT_NONE;
+
+static struct {
+	uint16_t proxy_h;
+	uint16_t proxy_data_out_h;
+	uint16_t prov_h;
+	uint16_t prov_data_in_h;
+	uint16_t prov_data_out_h;
+} svc_handles;
+
+static void resolve_svc_handles(void)
+{
+	int rc;
+
+	/* Either all handles are already resolved, or none of them */
+	if (svc_handles.prov_data_out_h) {
+		return;
+	}
+
+	/*
+	 * We assert if attribute is not found since at this stage all attributes
+	 * shall be already registered and thus shall be found.
+	 */
+
+	rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL),
+				&svc_handles.proxy_h);
+	assert(rc == 0);
+
+	rc = ble_gatts_find_chr(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL),
+				BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_DATA_OUT_VAL),
+				NULL, &svc_handles.proxy_data_out_h);
+	assert(rc == 0);
+
+	rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL),
+				&svc_handles.prov_h);
+	assert(rc == 0);
+
+	rc = ble_gatts_find_chr(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL),
+				BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_IN_VAL),
+				NULL, &svc_handles.prov_data_in_h);
+	assert(rc == 0);
+
+	rc = ble_gatts_find_chr(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL),
+				BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_OUT_VAL),
+				NULL, &svc_handles.prov_data_out_h);
+	assert(rc == 0);
+}
+
+static struct bt_mesh_proxy_client *find_client(uint16_t conn_handle)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		if (clients[i].conn_handle == conn_handle) {
+			return &clients[i];
+		}
+	}
+
+	return NULL;
+}
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+/* Next subnet in queue to be advertised */
+static int next_idx;
+
+static int proxy_segment_and_send(uint16_t conn_handle, u8_t type,
+				  struct os_mbuf *msg);
+
+static int filter_set(struct bt_mesh_proxy_client *client,
+		      struct os_mbuf *buf)
+{
+	u8_t type;
+
+	if (buf->om_len < 1) {
+		BT_WARN("Too short Filter Set message");
+		return -EINVAL;
+	}
+
+	type = net_buf_simple_pull_u8(buf);
+	BT_DBG("type 0x%02x", type);
+
+	switch (type) {
+	case 0x00:
+		memset(client->filter, 0, sizeof(client->filter));
+		client->filter_type = WHITELIST;
+		break;
+	case 0x01:
+		memset(client->filter, 0, sizeof(client->filter));
+		client->filter_type = BLACKLIST;
+		break;
+	default:
+		BT_WARN("Prohibited Filter Type 0x%02x", type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void filter_add(struct bt_mesh_proxy_client *client, u16_t addr)
+{
+	int i;
+
+	BT_DBG("addr 0x%04x", addr);
+
+	if (addr == BT_MESH_ADDR_UNASSIGNED) {
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(client->filter); i++) {
+		if (client->filter[i] == addr) {
+			return;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(client->filter); i++) {
+		if (client->filter[i] == BT_MESH_ADDR_UNASSIGNED) {
+			client->filter[i] = addr;
+			return;
+		}
+	}
+}
+
+static void filter_remove(struct bt_mesh_proxy_client *client, u16_t addr)
+{
+	int i;
+
+	BT_DBG("addr 0x%04x", addr);
+
+	if (addr == BT_MESH_ADDR_UNASSIGNED) {
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(client->filter); i++) {
+		if (client->filter[i] == addr) {
+			client->filter[i] = BT_MESH_ADDR_UNASSIGNED;
+			return;
+		}
+	}
+}
+
+static void send_filter_status(struct bt_mesh_proxy_client *client,
+			       struct bt_mesh_net_rx *rx,
+			       struct os_mbuf *buf)
+{
+	struct bt_mesh_net_tx tx = {
+		.sub = rx->sub,
+		.ctx = &rx->ctx,
+		.src = bt_mesh_primary_addr(),
+	};
+	u16_t filter_size;
+	int i, err;
+
+	/* Configuration messages always have dst unassigned */
+	tx.ctx->addr = BT_MESH_ADDR_UNASSIGNED;
+
+	net_buf_simple_init(buf, 10);
+
+	net_buf_simple_add_u8(buf, CFG_FILTER_STATUS);
+
+	if (client->filter_type == WHITELIST) {
+		net_buf_simple_add_u8(buf, 0x00);
+	} else {
+		net_buf_simple_add_u8(buf, 0x01);
+	}
+
+	for (filter_size = 0, i = 0; i < ARRAY_SIZE(client->filter); i++) {
+		if (client->filter[i] != BT_MESH_ADDR_UNASSIGNED) {
+			filter_size++;
+		}
+	}
+
+	net_buf_simple_add_be16(buf, filter_size);
+
+	BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	err = bt_mesh_net_encode(&tx, buf, true);
+	if (err) {
+		BT_ERR("Encoding Proxy cfg message failed (err %d)", err);
+		return;
+	}
+
+	err = proxy_segment_and_send(client->conn_handle, BT_MESH_PROXY_CONFIG, buf);
+	if (err) {
+		BT_ERR("Failed to send proxy cfg message (err %d)", err);
+	}
+}
+
+static void proxy_cfg(struct bt_mesh_proxy_client *client)
+{
+	struct os_mbuf *buf = NET_BUF_SIMPLE(29);
+	struct bt_mesh_net_rx rx;
+	u8_t opcode;
+	int err;
+
+	err = bt_mesh_net_decode(client->buf, BT_MESH_NET_IF_PROXY_CFG,
+				 &rx, buf);
+	if (err) {
+		BT_ERR("Failed to decode Proxy Configuration (err %d)", err);
+		goto done;
+	}
+
+	/* Remove network headers */
+	net_buf_simple_pull(buf, BT_MESH_NET_HDR_LEN);
+
+	BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	if (buf->om_len < 1) {
+		BT_WARN("Too short proxy configuration PDU");
+		goto done;
+	}
+
+	opcode = net_buf_simple_pull_u8(buf);
+	switch (opcode) {
+	case CFG_FILTER_SET:
+		filter_set(client, buf);
+		send_filter_status(client, &rx, buf);
+		break;
+	case CFG_FILTER_ADD:
+		while (buf->om_len >= 2) {
+			u16_t addr;
+
+			addr = net_buf_simple_pull_be16(buf);
+			filter_add(client, addr);
+		}
+		send_filter_status(client, &rx, buf);
+		break;
+	case CFG_FILTER_REMOVE:
+		while (buf->om_len >= 2) {
+			u16_t addr;
+
+			addr = net_buf_simple_pull_be16(buf);
+			filter_remove(client, addr);
+		}
+		send_filter_status(client, &rx, buf);
+		break;
+	default:
+		BT_WARN("Unhandled configuration OpCode 0x%02x", opcode);
+		break;
+	}
+
+done:
+	os_mbuf_free_chain(buf);
+}
+
+static int beacon_send(uint16_t conn_handle, struct bt_mesh_subnet *sub)
+{
+	struct os_mbuf *buf = NET_BUF_SIMPLE(23);
+	int rc;
+
+	net_buf_simple_init(buf, 1);
+	bt_mesh_beacon_create(sub, buf);
+
+	rc = proxy_segment_and_send(conn_handle, BT_MESH_PROXY_BEACON, buf);
+	os_mbuf_free_chain(buf);
+	return rc;
+}
+
+static void proxy_send_beacons(struct ble_npl_event *work)
+{
+	struct bt_mesh_proxy_client *client;
+	int i;
+
+
+	client = ble_npl_event_get_arg(work);
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx != BT_MESH_KEY_UNUSED) {
+			beacon_send(client->conn_handle, sub);
+		}
+	}
+}
+
+static void proxy_sar_timeout(struct ble_npl_event *work)
+{
+	struct bt_mesh_proxy_client *client;
+	int rc;
+
+	BT_WARN("Proxy SAR timeout");
+
+	client = ble_npl_event_get_arg(work);
+	assert(client != NULL);
+
+	if ((client->conn_handle != BLE_HS_CONN_HANDLE_NONE)) {
+		rc = ble_gap_terminate(client->conn_handle,
+				       BLE_ERR_REM_USER_CONN_TERM);
+		assert(rc == 0);
+	}
+}
+
+void bt_mesh_proxy_beacon_send(struct bt_mesh_subnet *sub)
+{
+	int i;
+
+	if (!sub) {
+		/* NULL means we send on all subnets */
+		for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+			if (bt_mesh.sub[i].net_idx != BT_MESH_KEY_UNUSED) {
+				bt_mesh_proxy_beacon_send(&bt_mesh.sub[i]);
+			}
+		}
+
+		return;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		if (clients[i].conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+			beacon_send(clients[i].conn_handle, sub);
+		}
+	}
+}
+
+void bt_mesh_proxy_identity_start(struct bt_mesh_subnet *sub)
+{
+	sub->node_id = BT_MESH_NODE_IDENTITY_RUNNING;
+	sub->node_id_start = k_uptime_get_32();
+
+	/* Prioritize the recently enabled subnet */
+	next_idx = sub - bt_mesh.sub;
+}
+
+void bt_mesh_proxy_identity_stop(struct bt_mesh_subnet *sub)
+{
+	sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED;
+	sub->node_id_start = 0;
+}
+
+int bt_mesh_proxy_identity_enable(void)
+{
+	int i, count = 0;
+
+	BT_DBG("");
+
+	if (!bt_mesh_is_provisioned()) {
+		return -EAGAIN;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (sub->node_id == BT_MESH_NODE_IDENTITY_NOT_SUPPORTED) {
+			continue;
+		}
+
+		bt_mesh_proxy_identity_start(sub);
+		count++;
+	}
+
+	if (count) {
+		bt_mesh_adv_update();
+	}
+
+	return 0;
+}
+
+#endif /* GATT_PROXY */
+
+static void proxy_complete_pdu(struct bt_mesh_proxy_client *client)
+{
+	switch (client->msg_type) {
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+	case BT_MESH_PROXY_NET_PDU:
+		BT_INFO("Mesh Network PDU");
+		bt_mesh_net_recv(client->buf, 0, BT_MESH_NET_IF_PROXY);
+		break;
+	case BT_MESH_PROXY_BEACON:
+		BT_INFO("Mesh Beacon PDU");
+		bt_mesh_beacon_recv(client->buf);
+		break;
+	case BT_MESH_PROXY_CONFIG:
+		BT_INFO("Mesh Configuration PDU");
+		proxy_cfg(client);
+		break;
+#endif
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	case BT_MESH_PROXY_PROV:
+		BT_INFO("Mesh Provisioning PDU");
+		bt_mesh_pb_gatt_recv(client->conn_handle, client->buf);
+		break;
+#endif
+	default:
+		BT_WARN("Unhandled Message Type 0x%02x", client->msg_type);
+		break;
+	}
+
+	net_buf_simple_init(client->buf, 0);
+}
+
+static int proxy_recv(uint16_t conn_handle, uint16_t attr_handle,
+		      struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+	struct bt_mesh_proxy_client *client;
+	const u8_t *data = ctxt->om->om_data;
+	u16_t len = ctxt->om->om_len;
+
+	client = find_client(conn_handle);
+
+	if (!client) {
+		return -ENOTCONN;
+	}
+
+	if (len < 1) {
+		BT_WARN("Too small Proxy PDU");
+		return -EINVAL;
+	}
+
+	if ((attr_handle == svc_handles.prov_data_in_h) !=
+	    (PDU_TYPE(data) == BT_MESH_PROXY_PROV)) {
+		BT_WARN("Proxy PDU type doesn't match GATT service");
+		return -EINVAL;
+	}
+
+	if (len - 1 > net_buf_simple_tailroom(client->buf)) {
+		BT_WARN("Too big proxy PDU");
+		return -EINVAL;
+	}
+
+	switch (PDU_SAR(data)) {
+	case SAR_COMPLETE:
+		if (client->buf->om_len) {
+			BT_WARN("Complete PDU while a pending incomplete one");
+			return -EINVAL;
+		}
+
+		client->msg_type = PDU_TYPE(data);
+		net_buf_simple_add_mem(client->buf, data + 1, len - 1);
+		proxy_complete_pdu(client);
+		break;
+
+	case SAR_FIRST:
+		if (client->buf->om_len) {
+			BT_WARN("First PDU while a pending incomplete one");
+			return -EINVAL;
+		}
+
+		k_delayed_work_submit(&client->sar_timer, PROXY_SAR_TIMEOUT);
+		client->msg_type = PDU_TYPE(data);
+		net_buf_simple_add_mem(client->buf, data + 1, len - 1);
+		break;
+
+	case SAR_CONT:
+		if (!client->buf->om_len) {
+			BT_WARN("Continuation with no prior data");
+			return -EINVAL;
+		}
+
+		if (client->msg_type != PDU_TYPE(data)) {
+			BT_WARN("Unexpected message type in continuation");
+			return -EINVAL;
+		}
+
+		k_delayed_work_submit(&client->sar_timer, PROXY_SAR_TIMEOUT);
+		net_buf_simple_add_mem(client->buf, data + 1, len - 1);
+		break;
+
+	case SAR_LAST:
+		if (!client->buf->om_len) {
+			BT_WARN("Last SAR PDU with no prior data");
+			return -EINVAL;
+		}
+
+		if (client->msg_type != PDU_TYPE(data)) {
+			BT_WARN("Unexpected message type in last SAR PDU");
+			return -EINVAL;
+		}
+
+		k_delayed_work_cancel(&client->sar_timer);
+		net_buf_simple_add_mem(client->buf, data + 1, len - 1);
+		proxy_complete_pdu(client);
+		break;
+	}
+
+	return len;
+}
+
+static int conn_count;
+
+static void proxy_connected(uint16_t conn_handle)
+{
+	struct bt_mesh_proxy_client *client;
+	int i;
+
+	BT_INFO("conn_handle %d", conn_handle);
+
+	conn_count++;
+
+	/* Since we use ADV_OPT_ONE_TIME */
+	proxy_adv_enabled = false;
+
+	/* Try to re-enable advertising in case it's possible */
+	if (conn_count < CONFIG_BT_MAX_CONN) {
+		bt_mesh_adv_update();
+	}
+
+	for (client = NULL, i = 0; i < ARRAY_SIZE(clients); i++) {
+		if (clients[i].conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+			client = &clients[i];
+			break;
+		}
+	}
+
+	if (!client) {
+		BT_ERR("No free Proxy Client objects");
+		return;
+	}
+
+	client->conn_handle = conn_handle;
+	client->filter_type = NONE;
+	memset(client->filter, 0, sizeof(client->filter));
+	net_buf_simple_init(client->buf, 0);
+}
+
+static void proxy_disconnected(uint16_t conn_handle, int reason)
+{
+	int i;
+	bool disconnected = false;
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		struct bt_mesh_proxy_client *client = &clients[i];
+
+		if (client->conn_handle == conn_handle) {
+			if ((MYNEWT_VAL(BLE_MESH_PB_GATT)) &&
+			    client->filter_type == PROV) {
+				bt_mesh_pb_gatt_close(conn_handle);
+			}
+
+			k_delayed_work_cancel(&client->sar_timer);
+			client->conn_handle = BLE_HS_CONN_HANDLE_NONE;
+			conn_count--;
+			disconnected = true;
+			break;
+		}
+	}
+
+	if (disconnected) {
+		BT_INFO("conn_handle %d reason %d", conn_handle, reason);
+		bt_mesh_adv_update();
+	}
+}
+
+struct os_mbuf *bt_mesh_proxy_get_buf(void)
+{
+	struct os_mbuf *buf = clients[0].buf;
+
+	if (buf != NULL) {
+		net_buf_simple_init(buf, 0);
+	}
+
+	return buf;
+}
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+static void prov_ccc_write(uint16_t conn_handle)
+{
+	struct bt_mesh_proxy_client *client;
+
+	BT_DBG("conn_handle %d", conn_handle);
+
+	/* If a connection exists there must be a client */
+	client = find_client(conn_handle);
+	__ASSERT(client, "No client for connection");
+
+	if (client->filter_type == NONE) {
+		client->filter_type = PROV;
+		bt_mesh_pb_gatt_open(conn_handle);
+	}
+}
+
+int bt_mesh_proxy_prov_enable(void)
+{
+	uint16_t handle;
+	int rc;
+	int i;
+
+	BT_DBG("");
+
+	if (gatt_svc == MESH_GATT_PROV) {
+		return -EALREADY;
+	}
+
+	if (gatt_svc != MESH_GATT_NONE) {
+		return -EBUSY;
+	}
+
+	rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), &handle);
+	assert(rc == 0);
+	ble_gatts_svc_set_visibility(handle, 1);
+	/* FIXME: figure out end handle */
+	ble_svc_gatt_changed(svc_handles.prov_h, 0xffff);
+
+	gatt_svc = MESH_GATT_PROV;
+	prov_fast_adv = true;
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		if (clients[i].conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+			clients[i].filter_type = PROV;
+		}
+	}
+
+
+	return 0;
+}
+
+int bt_mesh_proxy_prov_disable(bool disconnect)
+{
+	uint16_t handle;
+	int rc;
+	int i;
+
+	BT_DBG("");
+
+	if (gatt_svc == MESH_GATT_NONE) {
+		return -EALREADY;
+	}
+
+	if (gatt_svc != MESH_GATT_PROV) {
+		return -EBUSY;
+	}
+
+	rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), &handle);
+	assert(rc == 0);
+	ble_gatts_svc_set_visibility(handle, 0);
+	/* FIXME: figure out end handle */
+	ble_svc_gatt_changed(svc_handles.prov_h, 0xffff);
+
+	gatt_svc = MESH_GATT_NONE;
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		struct bt_mesh_proxy_client *client = &clients[i];
+
+		if ((client->conn_handle == BLE_HS_CONN_HANDLE_NONE)
+		    || (client->filter_type != PROV)) {
+			continue;
+		}
+
+		if (disconnect) {
+			rc = ble_gap_terminate(client->conn_handle,
+					       BLE_ERR_REM_USER_CONN_TERM);
+			assert(rc == 0);
+		} else {
+			bt_mesh_pb_gatt_close(client->conn_handle);
+			client->filter_type = NONE;
+		}
+	}
+
+	bt_mesh_adv_update();
+
+	return 0;
+}
+#endif /* MYNEWT_VAL(BLE_MESH_PB_GATT) */
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+static void proxy_ccc_write(uint16_t conn_handle)
+{
+	struct bt_mesh_proxy_client *client;
+
+	BT_DBG("conn_handle %d", conn_handle);
+
+	client = find_client(conn_handle);
+	__ASSERT(client, "No client for connection");
+
+	if (client->filter_type == NONE) {
+		client->filter_type = WHITELIST;
+		k_work_add_arg(&client->send_beacons, client);
+		k_work_submit(&client->send_beacons);
+	}
+}
+
+int bt_mesh_proxy_gatt_enable(void)
+{
+	uint16_t handle;
+	int rc;
+	int i;
+
+	BT_DBG("");
+
+	if (gatt_svc == MESH_GATT_PROXY) {
+		return -EALREADY;
+	}
+
+	if (gatt_svc != MESH_GATT_NONE) {
+		return -EBUSY;
+	}
+
+	rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), &handle);
+	assert(rc == 0);
+	ble_gatts_svc_set_visibility(handle, 1);
+	/* FIXME: figure out end handle */
+	ble_svc_gatt_changed(svc_handles.proxy_h, 0xffff);
+
+	gatt_svc = MESH_GATT_PROXY;
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		if (clients[i].conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+			clients[i].filter_type = WHITELIST;
+		}
+	}
+
+	return 0;
+}
+
+void bt_mesh_proxy_gatt_disconnect(void)
+{
+	int rc;
+	int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		struct bt_mesh_proxy_client *client = &clients[i];
+
+		if ((client->conn_handle != BLE_HS_CONN_HANDLE_NONE) &&
+		    (client->filter_type == WHITELIST ||
+		     client->filter_type == BLACKLIST)) {
+			client->filter_type = NONE;
+			rc = ble_gap_terminate(client->conn_handle,
+			                       BLE_ERR_REM_USER_CONN_TERM);
+			assert(rc == 0);
+		}
+	}
+}
+
+int bt_mesh_proxy_gatt_disable(void)
+{
+	uint16_t handle;
+	int rc;
+
+	BT_DBG("");
+
+	if (gatt_svc == MESH_GATT_NONE) {
+		return -EALREADY;
+	}
+
+	if (gatt_svc != MESH_GATT_PROXY) {
+		return -EBUSY;
+	}
+
+	bt_mesh_proxy_gatt_disconnect();
+
+	rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), &handle);
+	assert(rc == 0);
+	ble_gatts_svc_set_visibility(handle, 0);
+	/* FIXME: figure out end handle */
+	ble_svc_gatt_changed(svc_handles.proxy_h, 0xffff);
+
+	gatt_svc = MESH_GATT_NONE;
+
+	return 0;
+}
+
+void bt_mesh_proxy_addr_add(struct os_mbuf *buf, u16_t addr)
+{
+	struct bt_mesh_proxy_client *client = NULL;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		client = &clients[i];
+		if (client->buf == buf) {
+			break;
+		}
+	}
+
+	assert(client);
+
+	BT_DBG("filter_type %u addr 0x%04x", client->filter_type, addr);
+
+	if (client->filter_type == WHITELIST) {
+		filter_add(client, addr);
+	} else if (client->filter_type == BLACKLIST) {
+		filter_remove(client, addr);
+	}
+}
+
+static bool client_filter_match(struct bt_mesh_proxy_client *client,
+				u16_t addr)
+{
+	int i;
+
+	BT_DBG("filter_type %u addr 0x%04x", client->filter_type, addr);
+
+	if (client->filter_type == BLACKLIST) {
+		for (i = 0; i < ARRAY_SIZE(client->filter); i++) {
+			if (client->filter[i] == addr) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	if (addr == BT_MESH_ADDR_ALL_NODES) {
+		return true;
+	}
+
+	if (client->filter_type == WHITELIST) {
+		for (i = 0; i < ARRAY_SIZE(client->filter); i++) {
+			if (client->filter[i] == addr) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+bool bt_mesh_proxy_relay(struct os_mbuf *buf, u16_t dst)
+{
+	bool relayed = false;
+	int i;
+
+	BT_DBG("%u bytes to dst 0x%04x", buf->om_len, dst);
+
+	for (i = 0; i < ARRAY_SIZE(clients); i++) {
+		struct bt_mesh_proxy_client *client = &clients[i];
+		struct os_mbuf *msg;
+
+		if (client->conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+			continue;
+		}
+
+		if (!client_filter_match(client, dst)) {
+			continue;
+		}
+
+		/* Proxy PDU sending modifies the original buffer,
+		 * so we need to make a copy.
+		 */
+		msg = NET_BUF_SIMPLE(32);
+		net_buf_simple_init(msg, 1);
+		net_buf_simple_add_mem(msg, buf->om_data, buf->om_len);
+
+		bt_mesh_proxy_send(client->conn_handle, BT_MESH_PROXY_NET_PDU, msg);
+		os_mbuf_free_chain(msg);
+		relayed = true;
+	}
+
+	return relayed;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_GATT_PROXY) */
+
+static int proxy_send(uint16_t conn_handle, const void *data, u16_t len)
+{
+	struct os_mbuf *om;
+
+	BT_DBG("%u bytes: %s", len, bt_hex(data, len));
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+	if (gatt_svc == MESH_GATT_PROXY) {
+		om = ble_hs_mbuf_from_flat(data, len);
+		assert(om);
+		ble_gattc_notify_custom(conn_handle, svc_handles.proxy_data_out_h, om);
+	}
+#endif
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	if (gatt_svc == MESH_GATT_PROV) {
+		om = ble_hs_mbuf_from_flat(data, len);
+		assert(om);
+		ble_gattc_notify_custom(conn_handle, svc_handles.prov_data_out_h, om);
+	}
+#endif
+
+	return 0;
+}
+
+static int proxy_segment_and_send(uint16_t conn_handle, u8_t type,
+				  struct os_mbuf *msg)
+{
+	u16_t mtu;
+
+	BT_DBG("conn_handle %d type 0x%02x len %u: %s", conn_handle, type, msg->om_len,
+	       bt_hex(msg->om_data, msg->om_len));
+
+	/* ATT_MTU - OpCode (1 byte) - Handle (2 bytes) */
+	mtu = ble_att_mtu(conn_handle) - 3;
+	if (mtu > msg->om_len) {
+		net_buf_simple_push_u8(msg, PDU_HDR(SAR_COMPLETE, type));
+		return proxy_send(conn_handle, msg->om_data, msg->om_len);
+	}
+
+	net_buf_simple_push_u8(msg, PDU_HDR(SAR_FIRST, type));
+	proxy_send(conn_handle, msg->om_data, mtu);
+	net_buf_simple_pull(msg, mtu);
+
+	while (msg->om_len) {
+		if (msg->om_len + 1 < mtu) {
+			net_buf_simple_push_u8(msg, PDU_HDR(SAR_LAST, type));
+			proxy_send(conn_handle, msg->om_data, msg->om_len);
+			break;
+		}
+
+		net_buf_simple_push_u8(msg, PDU_HDR(SAR_CONT, type));
+		proxy_send(conn_handle, msg->om_data, mtu);
+		net_buf_simple_pull(msg, mtu);
+	}
+
+	return 0;
+}
+
+int bt_mesh_proxy_send(uint16_t conn_handle, u8_t type,
+		       struct os_mbuf *msg)
+{
+	struct bt_mesh_proxy_client *client = find_client(conn_handle);
+
+	if (!client) {
+		BT_ERR("No Proxy Client found");
+		return -ENOTCONN;
+	}
+
+	if ((client->filter_type == PROV) != (type == BT_MESH_PROXY_PROV)) {
+		BT_ERR("Invalid PDU type for Proxy Client");
+		return -EINVAL;
+	}
+
+	return proxy_segment_and_send(conn_handle, type, msg);
+}
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+static u8_t prov_svc_data[20] = { 0x27, 0x18, };
+
+static const struct bt_data prov_ad[] = {
+	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
+	BT_DATA_BYTES(BT_DATA_UUID16_ALL, 0x27, 0x18),
+	BT_DATA(BT_DATA_SVC_DATA16, prov_svc_data, sizeof(prov_svc_data)),
+};
+#endif /* PB_GATT */
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+
+#define ID_TYPE_NET  0x00
+#define ID_TYPE_NODE 0x01
+
+#define NODE_ID_LEN  19
+#define NET_ID_LEN   11
+
+#define NODE_ID_TIMEOUT K_SECONDS(CONFIG_BT_MESH_NODE_ID_TIMEOUT)
+
+static u8_t proxy_svc_data[NODE_ID_LEN] = { 0x28, 0x18, };
+
+static const struct bt_data node_id_ad[] = {
+	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
+	BT_DATA_BYTES(BT_DATA_UUID16_ALL, 0x28, 0x18),
+	BT_DATA(BT_DATA_SVC_DATA16, proxy_svc_data, NODE_ID_LEN),
+};
+
+static const struct bt_data net_id_ad[] = {
+	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
+	BT_DATA_BYTES(BT_DATA_UUID16_ALL, 0x28, 0x18),
+	BT_DATA(BT_DATA_SVC_DATA16, proxy_svc_data, NET_ID_LEN),
+};
+
+static int node_id_adv(struct bt_mesh_subnet *sub)
+{
+	u8_t tmp[16];
+	int err;
+
+	BT_DBG("");
+
+	proxy_svc_data[2] = ID_TYPE_NODE;
+
+	err = bt_rand(proxy_svc_data + 11, 8);
+	if (err) {
+		return err;
+	}
+
+	memset(tmp, 0, 6);
+	memcpy(tmp + 6, proxy_svc_data + 11, 8);
+	sys_put_be16(bt_mesh_primary_addr(), tmp + 14);
+
+	err = bt_encrypt_be(sub->keys[sub->kr_flag].identity, tmp, tmp);
+	if (err) {
+		return err;
+	}
+
+	memcpy(proxy_svc_data + 3, tmp + 8, 8);
+
+	err = bt_le_adv_start(&fast_adv_param, node_id_ad,
+			      ARRAY_SIZE(node_id_ad), NULL, 0);
+	if (err) {
+		BT_WARN("Failed to advertise using Node ID (err %d)", err);
+		return err;
+	}
+
+	proxy_adv_enabled = true;
+
+	return 0;
+}
+
+static int net_id_adv(struct bt_mesh_subnet *sub)
+{
+	int err;
+
+	BT_DBG("");
+
+	proxy_svc_data[2] = ID_TYPE_NET;
+
+	BT_DBG("Advertising with NetId %s",
+	       bt_hex(sub->keys[sub->kr_flag].net_id, 8));
+
+	memcpy(proxy_svc_data + 3, sub->keys[sub->kr_flag].net_id, 8);
+
+	err = bt_le_adv_start(&slow_adv_param, net_id_ad,
+			      ARRAY_SIZE(net_id_ad), NULL, 0);
+	if (err) {
+		BT_WARN("Failed to advertise using Network ID (err %d)", err);
+		return err;
+	}
+
+	proxy_adv_enabled = true;
+
+	return 0;
+}
+
+static bool advertise_subnet(struct bt_mesh_subnet *sub)
+{
+	if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+		return false;
+	}
+
+	return (sub->node_id == BT_MESH_NODE_IDENTITY_RUNNING ||
+		bt_mesh_gatt_proxy_get() != BT_MESH_GATT_PROXY_NOT_SUPPORTED);
+}
+
+static struct bt_mesh_subnet *next_sub(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub;
+
+		sub = &bt_mesh.sub[(i + next_idx) % ARRAY_SIZE(bt_mesh.sub)];
+		if (advertise_subnet(sub)) {
+			next_idx = (next_idx + 1) % ARRAY_SIZE(bt_mesh.sub);
+			return sub;
+		}
+	}
+
+	return NULL;
+}
+
+static int sub_count(void)
+{
+	int i, count = 0;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+
+		if (advertise_subnet(sub)) {
+			count++;
+		}
+	}
+
+	return count;
+}
+
+static s32_t gatt_proxy_advertise(struct bt_mesh_subnet *sub)
+{
+	s32_t remaining = K_FOREVER;
+	int subnet_count;
+
+	BT_DBG("");
+
+	if (conn_count == CONFIG_BT_MAX_CONN) {
+		BT_DBG("Connectable advertising deferred (max connections)");
+		return remaining;
+	}
+
+	if (!sub) {
+		BT_WARN("No subnets to advertise on");
+		return remaining;
+	}
+
+	if (sub->node_id == BT_MESH_NODE_IDENTITY_RUNNING) {
+		u32_t active = k_uptime_get_32() - sub->node_id_start;
+
+		if (active < NODE_ID_TIMEOUT) {
+			remaining = NODE_ID_TIMEOUT - active;
+			BT_DBG("Node ID active for %u ms, %d ms remaining",
+			       (unsigned) active, (int) remaining);
+			node_id_adv(sub);
+		} else {
+			bt_mesh_proxy_identity_stop(sub);
+			BT_DBG("Node ID stopped");
+		}
+	}
+
+	if (sub->node_id == BT_MESH_NODE_IDENTITY_STOPPED) {
+		net_id_adv(sub);
+	}
+
+	subnet_count = sub_count();
+	BT_DBG("sub_count %u", subnet_count);
+	if (subnet_count > 1) {
+		s32_t max_timeout;
+
+		/* We use NODE_ID_TIMEOUT as a starting point since it may
+		 * be less than 60 seconds. Divide this period into at least
+		 * 6 slices, but make sure that a slice is at least one
+		 * second long (to avoid excessive rotation).
+		 */
+		max_timeout = NODE_ID_TIMEOUT / max(subnet_count, 6);
+		max_timeout = max(max_timeout, K_SECONDS(1));
+
+		if (remaining > max_timeout || remaining < 0) {
+			remaining = max_timeout;
+		}
+	}
+
+	BT_DBG("Advertising %d ms for net_idx 0x%04x",
+	       (int) remaining, sub->net_idx);
+
+	return remaining;
+}
+#endif /* GATT_PROXY */
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+static size_t gatt_prov_adv_create(struct bt_data prov_sd[2])
+{
+	const struct bt_mesh_prov *prov = bt_mesh_prov_get();
+	const char *name = CONFIG_BT_DEVICE_NAME;
+	size_t name_len = strlen(name);
+	size_t prov_sd_len = 0;
+	size_t sd_space = 31;
+
+	memcpy(prov_svc_data + 2, prov->uuid, 16);
+	sys_put_be16(prov->oob_info, prov_svc_data + 18);
+
+	if (prov->uri) {
+		size_t uri_len = strlen(prov->uri);
+
+		if (uri_len > 29) {
+			/* There's no way to shorten an URI */
+			BT_WARN("Too long URI to fit advertising packet");
+		} else {
+			prov_sd[0].type = BT_DATA_URI;
+			prov_sd[0].data_len = uri_len;
+			prov_sd[0].data = (void *)prov->uri;
+			sd_space -= 2 + uri_len;
+			prov_sd_len++;
+		}
+	}
+
+	if (sd_space > 2 && name_len > 0) {
+		sd_space -= 2;
+
+		if (sd_space < name_len) {
+			prov_sd[prov_sd_len].type = BT_DATA_NAME_SHORTENED;
+			prov_sd[prov_sd_len].data_len = sd_space;
+		} else {
+			prov_sd[prov_sd_len].type = BT_DATA_NAME_COMPLETE;
+			prov_sd[prov_sd_len].data_len = name_len;
+		}
+
+		prov_sd[prov_sd_len].data = (void *)name;
+		prov_sd_len++;
+	}
+
+	return prov_sd_len;
+}
+#endif /* PB_GATT */
+
+s32_t bt_mesh_proxy_adv_start(void)
+{
+	BT_DBG("");
+
+	if (gatt_svc == MESH_GATT_NONE) {
+		return K_FOREVER;
+	}
+
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+	if (!bt_mesh_is_provisioned()) {
+		const struct ble_gap_adv_params *param;
+		struct bt_data prov_sd[2];
+		size_t prov_sd_len;
+
+		if (prov_fast_adv) {
+			param = &fast_adv_param;
+		} else {
+			param = &slow_adv_param;
+		}
+
+		prov_sd_len = gatt_prov_adv_create(prov_sd);
+
+		if (bt_le_adv_start(param, prov_ad, ARRAY_SIZE(prov_ad),
+				    prov_sd, prov_sd_len) == 0) {
+			proxy_adv_enabled = true;
+
+			/* Advertise 60 seconds using fast interval */
+			if (prov_fast_adv) {
+				prov_fast_adv = false;
+				return K_SECONDS(60);
+			}
+		}
+	}
+#endif /* PB_GATT */
+
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+	if (bt_mesh_is_provisioned()) {
+		return gatt_proxy_advertise(next_sub());
+	}
+#endif /* GATT_PROXY */
+
+	return K_FOREVER;
+}
+
+void bt_mesh_proxy_adv_stop(void)
+{
+	int err;
+
+	BT_DBG("adv_enabled %u", proxy_adv_enabled);
+
+	if (!proxy_adv_enabled) {
+		return;
+	}
+
+	err = bt_le_adv_stop(true);
+	if (err) {
+		BT_ERR("Failed to stop advertising (err %d)", err);
+	} else {
+		proxy_adv_enabled = false;
+	}
+}
+
+static void ble_mesh_handle_connect(struct ble_gap_event *event, void *arg)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV)
+	/* When EXT ADV is enabled then mesh proxy is connected
+	 * when proxy advertising instance is completed.
+	 * Therefore no need to handle BLE_GAP_EVENT_CONNECT
+	 */
+	if (event->type == BLE_GAP_EVENT_ADV_COMPLETE) {
+		/* Reason 0 means advertising has been completed because
+		 * connection has been established
+		 */
+		if (event->adv_complete.reason != 0) {
+			return;
+		}
+
+		if (event->adv_complete.instance != BT_MESH_ADV_GATT_INST) {
+			return;
+		}
+
+		proxy_connected(event->adv_complete.conn_handle);
+	}
+#else
+	if (event->type == BLE_GAP_EVENT_CONNECT) {
+		proxy_connected(event->connect.conn_handle);
+	}
+#endif
+}
+
+int ble_mesh_proxy_gap_event(struct ble_gap_event *event, void *arg)
+{
+	if ((event->type == BLE_GAP_EVENT_CONNECT) ||
+		(event->type == BLE_GAP_EVENT_ADV_COMPLETE)) {
+		ble_mesh_handle_connect(event, arg);
+	} else if (event->type == BLE_GAP_EVENT_DISCONNECT) {
+		proxy_disconnected(event->disconnect.conn.conn_handle,
+				   event->disconnect.reason);
+	} else if (event->type == BLE_GAP_EVENT_SUBSCRIBE) {
+		if (event->subscribe.attr_handle == svc_handles.proxy_data_out_h) {
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+			proxy_ccc_write(event->subscribe.conn_handle);
+#endif
+		} else if (event->subscribe.attr_handle ==
+			   svc_handles.prov_data_out_h) {
+#if (MYNEWT_VAL(BLE_MESH_PB_GATT))
+			prov_ccc_write(event->subscribe.conn_handle);
+#endif
+		}
+	}
+
+	return 0;
+}
+
+static int
+dummy_access_cb(uint16_t conn_handle, uint16_t attr_handle,
+		struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+	/*
+	 * We should never never enter this callback - it's attached to notify-only
+	 * characteristic which are notified directly from mbuf. And we can't pass
+	 * NULL as access_cb because gatts will assert on init...
+	 */
+	BLE_HS_DBG_ASSERT(0);
+	return 0;
+}
+
+static const struct ble_gatt_svc_def svc_defs [] = {
+	{
+		.type = BLE_GATT_SVC_TYPE_PRIMARY,
+		.uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL),
+		.characteristics = (struct ble_gatt_chr_def[]) { {
+				.uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_DATA_IN_VAL),
+				.access_cb = proxy_recv,
+				.flags = BLE_GATT_CHR_F_WRITE_NO_RSP,
+			}, {
+				.uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_DATA_OUT_VAL),
+				.access_cb = dummy_access_cb,
+				.flags = BLE_GATT_CHR_F_NOTIFY,
+			}, {
+				0, /* No more characteristics in this service. */
+			} },
+	}, {
+		.type = BLE_GATT_SVC_TYPE_PRIMARY,
+		.uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL),
+		.characteristics = (struct ble_gatt_chr_def[]) { {
+				.uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_IN_VAL),
+				.access_cb = proxy_recv,
+				.flags = BLE_GATT_CHR_F_WRITE_NO_RSP,
+			}, {
+				.uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_OUT_VAL),
+				.access_cb = dummy_access_cb,
+				.flags = BLE_GATT_CHR_F_NOTIFY,
+			}, {
+				0, /* No more characteristics in this service. */
+			} },
+	}, {
+		0, /* No more services. */
+	},
+};
+
+int bt_mesh_proxy_svcs_register(void)
+{
+	int rc;
+
+	rc = ble_gatts_count_cfg(svc_defs);
+	assert(rc == 0);
+
+	rc = ble_gatts_add_svcs(svc_defs);
+	assert(rc == 0);
+
+	return 0;
+}
+
+int bt_mesh_proxy_init(void)
+{
+	int i;
+
+	for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); ++i) {
+#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY))
+		k_work_init(&clients[i].send_beacons, proxy_send_beacons);
+#endif
+		clients[i].buf = NET_BUF_SIMPLE(CLIENT_BUF_SIZE);
+		clients[i].conn_handle = BLE_HS_CONN_HANDLE_NONE;
+
+		k_delayed_work_init(&clients[i].sar_timer, proxy_sar_timeout);
+		k_delayed_work_add_arg(&clients[i].sar_timer, &clients[i]);
+	}
+
+	resolve_svc_handles();
+
+	ble_gatts_svc_set_visibility(svc_handles.proxy_h, 0);
+	ble_gatts_svc_set_visibility(svc_handles.prov_h, 0);
+
+	return 0;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_PROXY) */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/proxy.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/proxy.h
new file mode 100644
index 00000000..64338a0a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/proxy.h
@@ -0,0 +1,45 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __PROXY_H__
+#define __PROXY_H__
+
+#define BT_MESH_PROXY_NET_PDU   0x00
+#define BT_MESH_PROXY_BEACON    0x01
+#define BT_MESH_PROXY_CONFIG    0x02
+#define BT_MESH_PROXY_PROV      0x03
+
+#include "mesh/mesh.h"
+
+int bt_mesh_proxy_send(uint16_t conn_handle, u8_t type, struct os_mbuf *msg);
+
+int bt_mesh_proxy_prov_enable(void);
+int bt_mesh_proxy_prov_disable(bool disconnect);
+
+int bt_mesh_proxy_gatt_enable(void);
+int bt_mesh_proxy_gatt_disable(void);
+void bt_mesh_proxy_gatt_disconnect(void);
+
+void bt_mesh_proxy_beacon_send(struct bt_mesh_subnet *sub);
+
+struct os_mbuf *bt_mesh_proxy_get_buf(void);
+
+s32_t bt_mesh_proxy_adv_start(void);
+void bt_mesh_proxy_adv_stop(void);
+
+void bt_mesh_proxy_identity_start(struct bt_mesh_subnet *sub);
+void bt_mesh_proxy_identity_stop(struct bt_mesh_subnet *sub);
+
+bool bt_mesh_proxy_relay(struct os_mbuf *buf, u16_t dst);
+void bt_mesh_proxy_addr_add(struct os_mbuf *buf, u16_t addr);
+
+int bt_mesh_proxy_init(void);
+
+int ble_mesh_proxy_gap_event(struct ble_gap_event *event, void *arg);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/settings.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/settings.c
new file mode 100644
index 00000000..88d9b302
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/settings.c
@@ -0,0 +1,2083 @@
+/*
+ * Copyright (c) 2018 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_SETTINGS_LOG
+
+#if MYNEWT_VAL(BLE_MESH_SETTINGS)
+
+#include "mesh/mesh.h"
+#include "mesh/glue.h"
+#include "net.h"
+#include "crypto.h"
+#include "transport.h"
+#include "access.h"
+#include "foundation.h"
+#include "proxy.h"
+#include "settings.h"
+#include "nodes.h"
+
+#include "config/config.h"
+
+/* Tracking of what storage changes are pending for App and Net Keys. We
+ * track this in a separate array here instead of within the respective
+ * bt_mesh_app_key and bt_mesh_subnet structs themselves, since once a key
+ * gets deleted its struct becomes invalid and may be reused for other keys.
+ */
+static struct key_update {
+	u16_t key_idx:12,    /* AppKey or NetKey Index */
+	      valid:1,       /* 1 if this entry is valid, 0 if not */
+	      app_key:1,     /* 1 if this is an AppKey, 0 if a NetKey */
+	      clear:1;       /* 1 if key needs clearing, 0 if storing */
+} key_updates[CONFIG_BT_MESH_APP_KEY_COUNT + CONFIG_BT_MESH_SUBNET_COUNT];
+
+static struct k_delayed_work pending_store;
+
+/* Mesh network storage information */
+struct net_val {
+	u16_t primary_addr;
+	u8_t  dev_key[16];
+} __packed;
+
+/* Sequence number storage */
+struct seq_val {
+	u8_t val[3];
+} __packed;
+
+/* Heartbeat Publication storage */
+struct hb_pub_val {
+	u16_t dst;
+	u8_t  period;
+	u8_t  ttl;
+	u16_t feat;
+	u16_t net_idx:12,
+	      indefinite:1;
+};
+
+/* Miscelaneous configuration server model states */
+struct cfg_val {
+	u8_t net_transmit;
+	u8_t relay;
+	u8_t relay_retransmit;
+	u8_t beacon;
+	u8_t gatt_proxy;
+	u8_t frnd;
+	u8_t default_ttl;
+};
+
+/* IV Index & IV Update storage */
+struct iv_val {
+	u32_t iv_index;
+	u8_t  iv_update:1,
+	      iv_duration:7;
+} __packed;
+
+/* Replay Protection List storage */
+struct rpl_val {
+	u32_t seq:24,
+	      old_iv:1;
+};
+
+/* NetKey storage information */
+struct net_key_val {
+	u8_t kr_flag:1,
+	     kr_phase:7;
+	u8_t val[2][16];
+} __packed;
+
+/* AppKey storage information */
+struct app_key_val {
+	u16_t net_idx;
+	bool  updated;
+	u8_t  val[2][16];
+} __packed;
+
+struct mod_pub_val {
+	u16_t addr;
+	u16_t key;
+	u8_t  ttl;
+	u8_t  retransmit;
+	u8_t  period;
+	u8_t  period_div:4,
+	      cred:1;
+};
+
+/* Virtual Address information */
+struct va_val {
+	u16_t ref;
+	u16_t addr;
+	u8_t uuid[16];
+} __packed;
+
+/* Node storage information */
+struct node_val {
+	u16_t net_idx;
+	u8_t  dev_key[16];
+	u8_t  num_elem;
+} __packed;
+
+struct node_update {
+	u16_t addr;
+	bool clear;
+};
+
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+static struct node_update node_updates[CONFIG_BT_MESH_NODE_COUNT];
+#else
+static struct node_update node_updates[0];
+#endif
+
+/* We need this so we don't overwrite app-hardcoded values in case FCB
+ * contains a history of changes but then has a NULL at the end.
+ */
+static struct {
+	bool valid;
+	struct cfg_val cfg;
+} stored_cfg;
+
+static int net_set(int argc, char **argv, char *val)
+{
+	struct net_val net;
+	int len, err;
+
+	BT_DBG("val %s", val ? val : "(null)");
+
+	if (!val) {
+		bt_mesh_comp_unprovision();
+		memset(bt_mesh.dev_key, 0, sizeof(bt_mesh.dev_key));
+		return 0;
+	}
+
+	len = sizeof(net);
+	err = settings_bytes_from_str(val, &net, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(net)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len, sizeof(net));
+		return -EINVAL;
+	}
+
+	memcpy(bt_mesh.dev_key, net.dev_key, sizeof(bt_mesh.dev_key));
+	bt_mesh_comp_provision(net.primary_addr);
+
+	BT_DBG("Provisioned with primary address 0x%04x", net.primary_addr);
+	BT_DBG("Recovered DevKey %s", bt_hex(bt_mesh.dev_key, 16));
+
+	return 0;
+}
+
+static int iv_set(int argc, char **argv, char *val)
+{
+	struct iv_val iv;
+	int len, err;
+
+	BT_DBG("val %s", val ? val : "(null)");
+
+	if (!val) {
+		bt_mesh.iv_index = 0U;
+		atomic_clear_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS);
+		return 0;
+	}
+
+	len = sizeof(iv);
+	err = settings_bytes_from_str(val, &iv, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(iv)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len, sizeof(iv));
+		return -EINVAL;
+	}
+
+	bt_mesh.iv_index = iv.iv_index;
+	atomic_set_bit_to(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS, iv.iv_update);
+	bt_mesh.ivu_duration = iv.iv_duration;
+
+	BT_DBG("IV Index 0x%04x (IV Update Flag %u) duration %u hours",
+	       (unsigned) iv.iv_index, iv.iv_update, iv.iv_duration);
+
+	return 0;
+}
+
+static int seq_set(int argc, char **argv, char *val)
+{
+	struct seq_val seq;
+	int len, err;
+
+	BT_DBG("val %s", val ? val : "(null)");
+
+	if (!val) {
+		bt_mesh.seq = 0;
+		return 0;
+	}
+
+	len = sizeof(seq);
+	err = settings_bytes_from_str(val, &seq, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(seq)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len, sizeof(seq));
+		return -EINVAL;
+	}
+
+	bt_mesh.seq = ((u32_t)seq.val[0] | ((u32_t)seq.val[1] << 8) |
+		       ((u32_t)seq.val[2] << 16));
+
+	if (CONFIG_BT_MESH_SEQ_STORE_RATE > 0) {
+		/* Make sure we have a large enough sequence number. We
+		 * subtract 1 so that the first transmission causes a write
+		 * to the settings storage.
+		 */
+		bt_mesh.seq += (CONFIG_BT_MESH_SEQ_STORE_RATE -
+				(bt_mesh.seq % CONFIG_BT_MESH_SEQ_STORE_RATE));
+		bt_mesh.seq--;
+	}
+
+	BT_DBG("Sequence Number 0x%06x", bt_mesh.seq);
+
+	return 0;
+}
+
+static struct bt_mesh_rpl *rpl_find(u16_t src)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) {
+		if (bt_mesh.rpl[i].src == src) {
+			return &bt_mesh.rpl[i];
+		}
+	}
+
+	return NULL;
+}
+
+static struct bt_mesh_rpl *rpl_alloc(u16_t src)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) {
+		if (!bt_mesh.rpl[i].src) {
+			bt_mesh.rpl[i].src = src;
+			return &bt_mesh.rpl[i];
+		}
+	}
+
+	return NULL;
+}
+
+static int rpl_set(int argc, char **argv, char *val)
+{
+	struct bt_mesh_rpl *entry;
+	struct rpl_val rpl;
+	int len, err;
+	u16_t src;
+
+	if (argc < 1) {
+		BT_ERR("Invalid argc (%d)", argc);
+		return -ENOENT;
+	}
+
+	BT_DBG("argv[0] %s val %s", argv[0], val ? val : "(null)");
+
+	src = strtol(argv[0], NULL, 16);
+	entry = rpl_find(src);
+
+	if (!val) {
+		if (entry) {
+			memset(entry, 0, sizeof(*entry));
+		} else {
+			BT_WARN("Unable to find RPL entry for 0x%04x", src);
+		}
+
+		return 0;
+	}
+
+	if (!entry) {
+		entry = rpl_alloc(src);
+		if (!entry) {
+			BT_ERR("Unable to allocate RPL entry for 0x%04x", src);
+			return -ENOMEM;
+		}
+	}
+
+	len = sizeof(rpl);
+	err = settings_bytes_from_str(val, &rpl, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(rpl)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len, sizeof(rpl));
+		return -EINVAL;
+	}
+
+	entry->seq = rpl.seq;
+	entry->old_iv = rpl.old_iv;
+
+	BT_DBG("RPL entry for 0x%04x: Seq 0x%06x old_iv %u", entry->src,
+	       (unsigned) entry->seq, entry->old_iv);
+
+	return 0;
+}
+
+static int net_key_set(int argc, char **argv, char *val)
+{
+	struct bt_mesh_subnet *sub;
+	struct net_key_val key;
+	int len, i, err;
+	u16_t net_idx;
+
+	BT_DBG("argv[0] %s val %s", argv[0], val ? val : "(null)");
+
+	net_idx = strtol(argv[0], NULL, 16);
+	sub = bt_mesh_subnet_get(net_idx);
+
+	if (!val) {
+		if (!sub) {
+			BT_ERR("No subnet with NetKeyIndex 0x%03x", net_idx);
+			return -ENOENT;
+		}
+
+		BT_DBG("Deleting NetKeyIndex 0x%03x", net_idx);
+		bt_mesh_subnet_del(sub, false);
+		return 0;
+	}
+
+	len = sizeof(key);
+	err = settings_bytes_from_str(val, &key, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(key)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len, sizeof(key));
+		return -EINVAL;
+	}
+
+	if (sub) {
+		BT_DBG("Updating existing NetKeyIndex 0x%03x", net_idx);
+
+		sub->kr_flag = key.kr_flag;
+		sub->kr_phase = key.kr_phase;
+		memcpy(sub->keys[0].net, &key.val[0], 16);
+		memcpy(sub->keys[1].net, &key.val[1], 16);
+
+		return 0;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		if (bt_mesh.sub[i].net_idx == BT_MESH_KEY_UNUSED) {
+			sub = &bt_mesh.sub[i];
+			break;
+		}
+	}
+
+	if (!sub) {
+		BT_ERR("No space to allocate a new subnet");
+		return -ENOMEM;
+	}
+
+	sub->net_idx = net_idx;
+	sub->kr_flag = key.kr_flag;
+	sub->kr_phase = key.kr_phase;
+	memcpy(sub->keys[0].net, &key.val[0], 16);
+	memcpy(sub->keys[1].net, &key.val[1], 16);
+
+	BT_DBG("NetKeyIndex 0x%03x recovered from storage", net_idx);
+
+	return 0;
+}
+
+static int app_key_set(int argc, char **argv, char *val)
+{
+	struct bt_mesh_app_key *app;
+	struct app_key_val key;
+	u16_t app_idx;
+	int len, err;
+
+	BT_DBG("argv[0] %s val %s", argv[0], val ? val : "(null)");
+
+	app_idx = strtol(argv[0], NULL, 16);
+
+	if (!val) {
+		BT_DBG("Deleting AppKeyIndex 0x%03x", app_idx);
+
+		app = bt_mesh_app_key_find(app_idx);
+		if (app) {
+			bt_mesh_app_key_del(app, false);
+		}
+
+		return 0;
+	}
+
+	len = sizeof(key);
+	err = settings_bytes_from_str(val, &key, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(key)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len, sizeof(key));
+		return -EINVAL;
+	}
+
+	app = bt_mesh_app_key_find(app_idx);
+	if (!app) {
+		app = bt_mesh_app_key_alloc(app_idx);
+	}
+
+	if (!app) {
+		BT_ERR("No space for a new app key");
+		return -ENOMEM;
+	}
+
+	app->net_idx = key.net_idx;
+	app->app_idx = app_idx;
+	app->updated = key.updated;
+	memcpy(app->keys[0].val, key.val[0], 16);
+	memcpy(app->keys[1].val, key.val[1], 16);
+
+	bt_mesh_app_id(app->keys[0].val, &app->keys[0].id);
+	bt_mesh_app_id(app->keys[1].val, &app->keys[1].id);
+
+	BT_DBG("AppKeyIndex 0x%03x recovered from storage", app_idx);
+
+	return 0;
+}
+
+static int hb_pub_set(int argc, char **argv, char *val)
+{
+	struct bt_mesh_hb_pub *pub = bt_mesh_hb_pub_get();
+	struct hb_pub_val hb_val;
+	int len, err;
+
+	BT_DBG("val %s", val ? val : "(null)");
+
+	if (!pub) {
+		return -ENOENT;
+	}
+
+	if (!val) {
+		pub->dst = BT_MESH_ADDR_UNASSIGNED;
+		pub->count = 0;
+		pub->ttl = 0;
+		pub->period = 0;
+		pub->feat = 0;
+
+		BT_DBG("Cleared heartbeat publication");
+		return 0;
+	}
+
+	len = sizeof(hb_val);
+	err = settings_bytes_from_str(val, &hb_val, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(hb_val)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len,
+		       sizeof(hb_val));
+		return -EINVAL;
+	}
+
+	pub->dst = hb_val.dst;
+	pub->period = hb_val.period;
+	pub->ttl = hb_val.ttl;
+	pub->feat = hb_val.feat;
+	pub->net_idx = hb_val.net_idx;
+
+	if (hb_val.indefinite) {
+		pub->count = 0xffff;
+	} else {
+		pub->count = 0;
+	}
+
+	BT_DBG("Restored heartbeat publication");
+
+	return 0;
+}
+
+static int cfg_set(int argc, char **argv, char *val)
+{
+	struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
+	int len, err;
+
+	BT_DBG("val %s", val ? val : "(null)");
+
+	if (!cfg) {
+		return -ENOENT;
+	}
+
+	if (!val) {
+		stored_cfg.valid = false;
+		BT_DBG("Cleared configuration state");
+		return 0;
+	}
+
+	len = sizeof(stored_cfg.cfg);
+	err = settings_bytes_from_str(val, &stored_cfg.cfg, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return err;
+	}
+
+	if (len != sizeof(stored_cfg.cfg)) {
+		BT_ERR("Unexpected value length (%d != %zu)", len,
+		       sizeof(stored_cfg.cfg));
+		return -EINVAL;
+	}
+
+	stored_cfg.valid = true;
+	BT_DBG("Restored configuration state");
+
+	return 0;
+}
+
+static int mod_set_bind(struct bt_mesh_model *mod, char *val)
+{
+	int len, err, i;
+
+	/* Start with empty array regardless of cleared or set value */
+	for (i = 0; i < ARRAY_SIZE(mod->keys); i++) {
+		mod->keys[i] = BT_MESH_KEY_UNUSED;
+	}
+
+	if (!val) {
+		BT_DBG("Cleared bindings for model");
+		return 0;
+	}
+
+	len = sizeof(mod->keys);
+	err = settings_bytes_from_str(val, mod->keys, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return -EINVAL;
+	}
+
+	BT_DBG("Decoded %u bound keys for model", len / sizeof(mod->keys[0]));
+	return 0;
+}
+
+static int mod_set_sub(struct bt_mesh_model *mod, char *val)
+{
+	int len, err;
+
+	/* Start with empty array regardless of cleared or set value */
+	memset(mod->groups, 0, sizeof(mod->groups));
+
+	if (!val) {
+		BT_DBG("Cleared subscriptions for model");
+		return 0;
+	}
+
+	len = sizeof(mod->groups);
+	err = settings_bytes_from_str(val, mod->groups, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return -EINVAL;
+	}
+
+	BT_DBG("Decoded %u subscribed group addresses for model",
+	       len / sizeof(mod->groups[0]));
+	return 0;
+}
+
+static int mod_set_pub(struct bt_mesh_model *mod, char *val)
+{
+	struct mod_pub_val pub;
+	int len, err;
+
+	if (!mod->pub) {
+		BT_WARN("Model has no publication context!");
+		return -EINVAL;
+	}
+
+	if (!val) {
+		mod->pub->addr = BT_MESH_ADDR_UNASSIGNED;
+		mod->pub->key = 0;
+		mod->pub->cred = 0;
+		mod->pub->ttl = 0;
+		mod->pub->period = 0;
+		mod->pub->retransmit = 0;
+		mod->pub->count = 0;
+
+		BT_DBG("Cleared publication for model");
+		return 0;
+	}
+
+	len = sizeof(pub);
+	err = settings_bytes_from_str(val, &pub, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return -EINVAL;
+	}
+
+	if (len != sizeof(pub)) {
+		BT_ERR("Invalid length for model publication");
+		return -EINVAL;
+	}
+
+	mod->pub->addr = pub.addr;
+	mod->pub->key = pub.key;
+	mod->pub->cred = pub.cred;
+	mod->pub->ttl = pub.ttl;
+	mod->pub->period = pub.period;
+	mod->pub->retransmit = pub.retransmit;
+	mod->pub->count = 0;
+
+	BT_DBG("Restored model publication, dst 0x%04x app_idx 0x%03x",
+	       pub.addr, pub.key);
+
+	return 0;
+}
+
+static int mod_set(bool vnd, int argc, char **argv, char *val)
+{
+	struct bt_mesh_model *mod;
+	u8_t elem_idx, mod_idx;
+	u16_t mod_key;
+
+	if (argc < 2) {
+		BT_ERR("Too small argc (%d)", argc);
+		return -ENOENT;
+	}
+
+	mod_key = strtol(argv[0], NULL, 16);
+	elem_idx = mod_key >> 8;
+	mod_idx = mod_key;
+
+	BT_DBG("Decoded mod_key 0x%04x as elem_idx %u mod_idx %u",
+	       mod_key, elem_idx, mod_idx);
+
+	mod = bt_mesh_model_get(vnd, elem_idx, mod_idx);
+	if (!mod) {
+		BT_ERR("Failed to get model for elem_idx %u mod_idx %u",
+		       elem_idx, mod_idx);
+		return -ENOENT;
+	}
+
+	if (!strcmp(argv[1], "bind")) {
+		return mod_set_bind(mod, val);
+	}
+
+	if (!strcmp(argv[1], "sub")) {
+		return mod_set_sub(mod, val);
+	}
+
+	if (!strcmp(argv[1], "pub")) {
+		return mod_set_pub(mod, val);
+	}
+
+	if (!strcmp(argv[1], "data")) {
+		mod->flags |= BT_MESH_MOD_DATA_PRESENT;
+
+		if (mod->cb && mod->cb->settings_set) {
+			return mod->cb->settings_set(mod, val);
+		}
+	}
+
+	BT_WARN("Unknown module key %s", argv[1]);
+	return -ENOENT;
+}
+
+static int sig_mod_set(int argc, char **argv, char *val)
+{
+	return mod_set(false, argc, argv, val);
+}
+
+static int vnd_mod_set(int argc, char **argv, char *val)
+{
+	return mod_set(true, argc, argv, val);
+}
+
+#if CONFIG_BT_MESH_LABEL_COUNT > 0
+static int va_set(int argc, char **argv, char *val)
+{
+	struct va_val va;
+	struct label *lab;
+	u16_t index;
+	int len, err;
+
+	if (argc < 1) {
+		BT_ERR("Insufficient number of arguments");
+		return -ENOENT;
+	}
+
+	index = strtol(argv[0], NULL, 16);
+
+	if (val == NULL) {
+		BT_WARN("Mesh Virtual Address length = 0");
+		return 0;
+	}
+
+	err = settings_bytes_from_str(val, &va, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return -EINVAL;
+	}
+
+	if (len != sizeof(struct va_val)) {
+		BT_ERR("Invalid length for virtual address");
+		return -EINVAL;
+	}
+
+	if (va.ref == 0) {
+		BT_WARN("Ignore Mesh Virtual Address ref = 0");
+		return 0;
+	}
+
+	lab = get_label(index);
+	if (lab == NULL) {
+		BT_WARN("Out of labels buffers");
+		return -ENOBUFS;
+	}
+
+	memcpy(lab->uuid, va.uuid, 16);
+	lab->addr = va.addr;
+	lab->ref = va.ref;
+
+	BT_DBG("Restored Virtual Address, addr 0x%04x ref 0x%04x",
+	       lab->addr, lab->ref);
+
+	return 0;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+static int node_set(int argc, char **argv, char *str)
+{
+	struct bt_mesh_node *node;
+	struct node_val val;
+	u16_t addr;
+	int len, err;
+
+	if (argc < 1) {
+		BT_ERR("Insufficient number of arguments");
+		return -ENOENT;
+	}
+
+	addr = strtol(argv[0], NULL, 16);
+
+	if (str == NULL) {
+		BT_DBG("val (null)");
+		BT_DBG("Deleting node 0x%04x", addr);
+
+		node = bt_mesh_node_find(addr);
+		if (node) {
+			bt_mesh_node_del(node, false);
+		}
+
+		return 0;
+	}
+
+	err = settings_bytes_from_str(str, &val, &len);
+	if (err) {
+		BT_ERR("Failed to decode value %s (err %d)", val, err);
+		return -EINVAL;
+	}
+
+	if (len != sizeof(struct node_val)) {
+		BT_ERR("Invalid length for node_val");
+		return -EINVAL;
+	}
+
+	node = bt_mesh_node_find(addr);
+	if (!node) {
+		node = bt_mesh_node_alloc(addr, val.num_elem, val.net_idx);
+	}
+
+	if (!node) {
+		BT_ERR("No space for a new node");
+		return -ENOMEM;
+	}
+
+	memcpy(node->dev_key, &val.dev_key, 16);
+
+	BT_DBG("Node 0x%04x recovered from storage", addr);
+
+	return 0;
+}
+#endif
+
+const struct mesh_setting {
+	const char *name;
+	int (*func)(int argc, char **argv, char *val);
+} settings[] = {
+	{ "Net", net_set },
+	{ "IV", iv_set },
+	{ "Seq", seq_set },
+	{ "RPL", rpl_set },
+	{ "NetKey", net_key_set },
+	{ "AppKey", app_key_set },
+	{ "HBPub", hb_pub_set },
+	{ "Cfg", cfg_set },
+	{ "s", sig_mod_set },
+	{ "v", vnd_mod_set },
+#if CONFIG_BT_MESH_LABEL_COUNT > 0
+	{ "Va", va_set },
+#endif
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+	{ "Node", node_set },
+#endif
+};
+
+static int mesh_set(int argc, char **argv, char *val)
+{
+	int i;
+
+	if (argc < 1) {
+		BT_ERR("Insufficient number of arguments");
+		return -EINVAL;
+	}
+
+	BT_DBG("argv[0] %s val %s", argv[0], val ? val : "(null)");
+
+	for (i = 0; i < ARRAY_SIZE(settings); i++) {
+		if (!strcmp(settings[i].name, argv[0])) {
+			argc--;
+			argv++;
+
+			return settings[i].func(argc, argv, val);
+		}
+	}
+
+	BT_WARN("No matching handler for key %s", argv[0]);
+
+	return -ENOENT;
+}
+
+static int subnet_init(struct bt_mesh_subnet *sub)
+{
+	int err;
+
+	err = bt_mesh_net_keys_create(&sub->keys[0], sub->keys[0].net);
+	if (err) {
+		BT_ERR("Unable to generate keys for subnet");
+		return -EIO;
+	}
+
+	if (sub->kr_phase != BT_MESH_KR_NORMAL) {
+		err = bt_mesh_net_keys_create(&sub->keys[1], sub->keys[1].net);
+		if (err) {
+			BT_ERR("Unable to generate keys for subnet");
+			memset(&sub->keys[0], 0, sizeof(sub->keys[0]));
+			return -EIO;
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_GATT_PROXY)) {
+		sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED;
+	} else {
+		sub->node_id = BT_MESH_NODE_IDENTITY_NOT_SUPPORTED;
+	}
+
+	/* Make sure we have valid beacon data to be sent */
+	bt_mesh_net_beacon_update(sub);
+
+	return 0;
+}
+
+static void commit_mod(struct bt_mesh_model *mod, struct bt_mesh_elem *elem,
+		       bool vnd, bool primary, void *user_data)
+{
+	if (mod->pub && mod->pub->update &&
+	    mod->pub->addr != BT_MESH_ADDR_UNASSIGNED) {
+		s32_t ms = bt_mesh_model_pub_period_get(mod);
+		if (ms) {
+			BT_DBG("Starting publish timer (period %u ms)",
+			       (unsigned) ms);
+			k_delayed_work_submit(&mod->pub->timer, ms);
+		}
+	}
+
+	if (mod->cb && mod->cb->settings_commit)  {
+		mod->cb->settings_commit(mod);
+	}
+}
+
+static int mesh_commit(void)
+{
+	struct bt_mesh_hb_pub *hb_pub;
+	struct bt_mesh_cfg_srv *cfg;
+	int i;
+
+	BT_DBG("sub[0].net_idx 0x%03x", bt_mesh.sub[0].net_idx);
+
+	if (bt_mesh.sub[0].net_idx == BT_MESH_KEY_UNUSED) {
+		/* Nothing to do since we're not yet provisioned */
+		return 0;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PB_GATT)) {
+		bt_mesh_proxy_prov_disable(true);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) {
+		struct bt_mesh_subnet *sub = &bt_mesh.sub[i];
+		int err;
+
+		if (sub->net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		err = subnet_init(sub);
+		if (err) {
+			BT_ERR("Failed to init subnet 0x%03x", sub->net_idx);
+		}
+	}
+
+	if (bt_mesh.ivu_duration < BT_MESH_IVU_MIN_HOURS) {
+		k_delayed_work_submit(&bt_mesh.ivu_timer, BT_MESH_IVU_TIMEOUT);
+	}
+
+	bt_mesh_model_foreach(commit_mod, NULL);
+
+	hb_pub = bt_mesh_hb_pub_get();
+	if (hb_pub && hb_pub->dst != BT_MESH_ADDR_UNASSIGNED &&
+	    hb_pub->count && hb_pub->period) {
+		BT_DBG("Starting heartbeat publication");
+		k_work_submit(&hb_pub->timer.work);
+	}
+
+	cfg = bt_mesh_cfg_get();
+	if (cfg && stored_cfg.valid) {
+		cfg->net_transmit = stored_cfg.cfg.net_transmit;
+		cfg->relay = stored_cfg.cfg.relay;
+		cfg->relay_retransmit = stored_cfg.cfg.relay_retransmit;
+		cfg->beacon = stored_cfg.cfg.beacon;
+		cfg->gatt_proxy = stored_cfg.cfg.gatt_proxy;
+		cfg->frnd = stored_cfg.cfg.frnd;
+		cfg->default_ttl = stored_cfg.cfg.default_ttl;
+	}
+
+	atomic_set_bit(bt_mesh.flags, BT_MESH_VALID);
+
+	bt_mesh_net_start();
+
+	return 0;
+}
+
+/* Pending flags that use K_NO_WAIT as the storage timeout */
+#define NO_WAIT_PENDING_BITS (BIT(BT_MESH_NET_PENDING) |           \
+			      BIT(BT_MESH_IV_PENDING) |            \
+			      BIT(BT_MESH_SEQ_PENDING))
+
+/* Pending flags that use CONFIG_BT_MESH_STORE_TIMEOUT */
+#define GENERIC_PENDING_BITS (BIT(BT_MESH_KEYS_PENDING) |          \
+			      BIT(BT_MESH_HB_PUB_PENDING) |        \
+			      BIT(BT_MESH_CFG_PENDING) |           \
+			      BIT(BT_MESH_MOD_PENDING) |           \
+			      BIT(BT_MESH_NODES_PENDING))
+
+static void schedule_store(int flag)
+{
+	s32_t timeout, remaining;
+
+	atomic_set_bit(bt_mesh.flags, flag);
+
+	if (atomic_get(bt_mesh.flags) & NO_WAIT_PENDING_BITS) {
+		timeout = K_NO_WAIT;
+	} else if (atomic_test_bit(bt_mesh.flags, BT_MESH_RPL_PENDING) &&
+		   (!(atomic_get(bt_mesh.flags) & GENERIC_PENDING_BITS) ||
+		    (CONFIG_BT_MESH_RPL_STORE_TIMEOUT <
+		     CONFIG_BT_MESH_STORE_TIMEOUT))) {
+		timeout = K_SECONDS(CONFIG_BT_MESH_RPL_STORE_TIMEOUT);
+	} else {
+		timeout = K_SECONDS(CONFIG_BT_MESH_STORE_TIMEOUT);
+	}
+
+	remaining = k_delayed_work_remaining_get(&pending_store);
+	if (remaining && remaining < timeout) {
+		BT_DBG("Not rescheduling due to existing earlier deadline");
+		return;
+	}
+
+	BT_DBG("Waiting %d seconds", (int) (timeout / MSEC_PER_SEC));
+
+	k_delayed_work_submit(&pending_store, timeout);
+}
+
+static void clear_iv(void)
+{
+	int err;
+
+	err = settings_save_one("bt_mesh/IV", NULL);
+	if (err) {
+		BT_ERR("Failed to clear IV");
+	} else {
+		BT_DBG("Cleared IV");
+	}
+}
+
+static void clear_net(void)
+{
+	int err;
+
+	err = settings_save_one("bt_mesh/Net", NULL);
+	if (err) {
+		BT_ERR("Failed to clear Network");
+	} else {
+		BT_DBG("Cleared Network");
+	}
+}
+
+static void store_pending_net(void)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct net_val))];
+	struct net_val net;
+	char *str;
+	int err;
+
+	BT_DBG("addr 0x%04x DevKey %s", bt_mesh_primary_addr(),
+	       bt_hex(bt_mesh.dev_key, 16));
+
+	net.primary_addr = bt_mesh_primary_addr();
+	memcpy(net.dev_key, bt_mesh.dev_key, 16);
+
+	str = settings_str_from_bytes(&net, sizeof(net), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode Network as value");
+		return;
+	}
+
+	BT_DBG("Saving Network as value %s", str);
+	err = settings_save_one("bt_mesh/Net", str);
+	if (err) {
+		BT_ERR("Failed to store Network");
+	} else {
+		BT_DBG("Stored Network");
+	}
+}
+
+void bt_mesh_store_net(void)
+{
+	schedule_store(BT_MESH_NET_PENDING);
+}
+
+static void store_pending_iv(void)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct iv_val))];
+	struct iv_val iv;
+	char *str;
+	int err;
+
+	iv.iv_index = bt_mesh.iv_index;
+	iv.iv_update = atomic_test_bit(bt_mesh.flags, BT_MESH_IVU_IN_PROGRESS);
+	iv.iv_duration = bt_mesh.ivu_duration;
+
+	str = settings_str_from_bytes(&iv, sizeof(iv), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode IV as value");
+		return;
+	}
+
+	BT_DBG("Saving IV as value %s", str);
+	err = settings_save_one("bt_mesh/IV", str);
+	if (err) {
+		BT_ERR("Failed to store IV");
+	} else {
+		BT_DBG("Stored IV");
+	}
+}
+
+void bt_mesh_store_iv(bool only_duration)
+{
+	schedule_store(BT_MESH_IV_PENDING);
+
+	if (!only_duration) {
+		/* Always update Seq whenever IV changes */
+		schedule_store(BT_MESH_SEQ_PENDING);
+	}
+}
+
+static void store_pending_seq(void)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct seq_val))];
+	struct seq_val seq;
+	char *str;
+	int err;
+
+	seq.val[0] = bt_mesh.seq;
+	seq.val[1] = bt_mesh.seq >> 8;
+	seq.val[2] = bt_mesh.seq >> 16;
+
+	str = settings_str_from_bytes(&seq, sizeof(seq), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode Seq as value");
+		return;
+	}
+
+	BT_DBG("Saving Seq as value %s", str);
+	err = settings_save_one("bt_mesh/Seq", str);
+	if (err) {
+		BT_ERR("Failed to store Seq");
+	} else {
+		BT_DBG("Stored Seq");
+	}
+}
+
+void bt_mesh_store_seq(void)
+{
+	if (CONFIG_BT_MESH_SEQ_STORE_RATE &&
+	    (bt_mesh.seq % CONFIG_BT_MESH_SEQ_STORE_RATE)) {
+		return;
+	}
+
+	schedule_store(BT_MESH_SEQ_PENDING);
+}
+
+static void store_rpl(struct bt_mesh_rpl *entry)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct rpl_val))];
+	struct rpl_val rpl;
+	char path[18];
+	char *str;
+	int err;
+
+	BT_DBG("src 0x%04x seq 0x%06x old_iv %u", entry->src,
+	       (unsigned) entry->seq, entry->old_iv);
+
+	rpl.seq = entry->seq;
+	rpl.old_iv = entry->old_iv;
+
+	str = settings_str_from_bytes(&rpl, sizeof(rpl), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode RPL as value");
+		return;
+	}
+
+	snprintk(path, sizeof(path), "bt_mesh/RPL/%x", entry->src);
+
+	BT_DBG("Saving RPL %s as value %s", path, str);
+	err = settings_save_one(path, str);
+	if (err) {
+		BT_ERR("Failed to store RPL");
+	} else {
+		BT_DBG("Stored RPL");
+	}
+}
+
+static void clear_rpl(void)
+{
+	int i, err;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) {
+		struct bt_mesh_rpl *rpl = &bt_mesh.rpl[i];
+		char path[18];
+
+		if (!rpl->src) {
+			continue;
+		}
+
+		snprintk(path, sizeof(path), "bt_mesh/RPL/%x", rpl->src);
+		err = settings_save_one(path, NULL);
+		if (err) {
+			BT_ERR("Failed to clear RPL");
+		} else {
+			BT_DBG("Cleared RPL");
+		}
+
+		memset(rpl, 0, sizeof(*rpl));
+	}
+}
+
+static void store_pending_rpl(void)
+{
+	int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) {
+		struct bt_mesh_rpl *rpl = &bt_mesh.rpl[i];
+
+		if (rpl->store) {
+			rpl->store = false;
+			store_rpl(rpl);
+		}
+	}
+}
+
+static void store_pending_hb_pub(void)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct hb_pub_val))];
+	struct bt_mesh_hb_pub *pub = bt_mesh_hb_pub_get();
+	struct hb_pub_val val;
+	char *str;
+	int err;
+
+	if (!pub) {
+		return;
+	}
+
+	if (pub->dst == BT_MESH_ADDR_UNASSIGNED) {
+		str = NULL;
+	} else {
+		val.indefinite = (pub->count == 0xffff);
+		val.dst = pub->dst;
+		val.period = pub->period;
+		val.ttl = pub->ttl;
+		val.feat = pub->feat;
+		val.net_idx = pub->net_idx;
+
+		str = settings_str_from_bytes(&val, sizeof(val),
+					      buf, sizeof(buf));
+		if (!str) {
+			BT_ERR("Unable to encode hb pub as value");
+			return;
+		}
+	}
+
+	BT_DBG("Saving Heartbeat Publication as value %s",
+	       str ? str : "(null)");
+	err = settings_save_one("bt_mesh/HBPub", str);
+	if (err) {
+		BT_ERR("Failed to store Heartbeat Publication");
+	} else {
+		BT_DBG("Stored Heartbeat Publication");
+	}
+}
+
+static void store_pending_cfg(void)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct cfg_val))];
+	struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
+	struct cfg_val val;
+	char *str;
+	int err;
+
+	if (!cfg) {
+		return;
+	}
+
+	val.net_transmit = cfg->net_transmit;
+	val.relay = cfg->relay;
+	val.relay_retransmit = cfg->relay_retransmit;
+	val.beacon = cfg->beacon;
+	val.gatt_proxy = cfg->gatt_proxy;
+	val.frnd = cfg->frnd;
+	val.default_ttl = cfg->default_ttl;
+
+	str = settings_str_from_bytes(&val, sizeof(val), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode configuration as value");
+		return;
+	}
+
+	BT_DBG("Saving configuration as value %s", str);
+	err = settings_save_one("bt_mesh/Cfg", str);
+	if (err) {
+		BT_ERR("Failed to store configuration");
+	} else {
+		BT_DBG("Stored configuration");
+	}
+}
+
+static void clear_cfg(void)
+{
+	int err;
+
+	err = settings_save_one("bt_mesh/Cfg", NULL);
+	if (err) {
+		BT_ERR("Failed to clear configuration");
+	} else {
+		BT_DBG("Cleared configuration");
+	}
+}
+
+static void clear_app_key(u16_t app_idx)
+{
+	char path[20];
+	int err;
+
+	BT_DBG("AppKeyIndex 0x%03x", app_idx);
+
+	snprintk(path, sizeof(path), "bt_mesh/AppKey/%x", app_idx);
+	err = settings_save_one(path, NULL);
+	if (err) {
+		BT_ERR("Failed to clear AppKeyIndex 0x%03x", app_idx);
+	} else {
+		BT_DBG("Cleared AppKeyIndex 0x%03x", app_idx);
+	}
+}
+
+static void clear_net_key(u16_t net_idx)
+{
+	char path[20];
+	int err;
+
+	BT_DBG("NetKeyIndex 0x%03x", net_idx);
+
+	snprintk(path, sizeof(path), "bt_mesh/NetKey/%x", net_idx);
+	err = settings_save_one(path, NULL);
+	if (err) {
+		BT_ERR("Failed to clear NetKeyIndex 0x%03x", net_idx);
+	} else {
+		BT_DBG("Cleared NetKeyIndex 0x%03x", net_idx);
+	}
+}
+
+static void store_net_key(struct bt_mesh_subnet *sub)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct net_key_val))];
+	struct net_key_val key;
+	char path[20];
+	char *str;
+	int err;
+
+	BT_DBG("NetKeyIndex 0x%03x NetKey %s", sub->net_idx,
+	       bt_hex(sub->keys[0].net, 16));
+
+	memcpy(&key.val[0], sub->keys[0].net, 16);
+	memcpy(&key.val[1], sub->keys[1].net, 16);
+	key.kr_flag = sub->kr_flag;
+	key.kr_phase = sub->kr_phase;
+
+	str = settings_str_from_bytes(&key, sizeof(key), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode NetKey as value");
+		return;
+	}
+
+	snprintk(path, sizeof(path), "bt_mesh/NetKey/%x", sub->net_idx);
+
+	BT_DBG("Saving NetKey %s as value %s", path, str);
+	err = settings_save_one(path, str);
+	if (err) {
+		BT_ERR("Failed to store NetKey");
+	} else {
+		BT_DBG("Stored NetKey");
+	}
+}
+
+static void store_app_key(struct bt_mesh_app_key *app)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct app_key_val))];
+	struct app_key_val key;
+	char path[20];
+	char *str;
+	int err;
+
+	key.net_idx = app->net_idx;
+	key.updated = app->updated;
+	memcpy(key.val[0], app->keys[0].val, 16);
+	memcpy(key.val[1], app->keys[1].val, 16);
+
+	str = settings_str_from_bytes(&key, sizeof(key), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode AppKey as value");
+		return;
+	}
+
+	snprintk(path, sizeof(path), "bt_mesh/AppKey/%x", app->app_idx);
+
+	BT_DBG("Saving AppKey %s as value %s", path, str);
+	err = settings_save_one(path, str);
+	if (err) {
+		BT_ERR("Failed to store AppKey");
+	} else {
+		BT_DBG("Stored AppKey");
+	}
+}
+
+static void store_pending_keys(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(key_updates); i++) {
+		struct key_update *update = &key_updates[i];
+
+		if (!update->valid) {
+			continue;
+		}
+
+		if (update->clear) {
+			if (update->app_key) {
+				clear_app_key(update->key_idx);
+			} else {
+				clear_net_key(update->key_idx);
+			}
+		} else {
+			if (update->app_key) {
+				struct bt_mesh_app_key *key;
+
+				key = bt_mesh_app_key_find(update->key_idx);
+				if (key) {
+					store_app_key(key);
+				} else {
+					BT_WARN("AppKeyIndex 0x%03x not found",
+					       update->key_idx);
+				}
+
+			} else {
+				struct bt_mesh_subnet *sub;
+
+				sub = bt_mesh_subnet_get(update->key_idx);
+				if (sub) {
+					store_net_key(sub);
+				} else {
+					BT_WARN("NetKeyIndex 0x%03x not found",
+					       update->key_idx);
+				}
+			}
+		}
+
+		update->valid = 0;
+	}
+}
+
+static void store_node(struct bt_mesh_node *node)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct node_val))];
+	struct node_val val;
+	char path[20];
+	char *str;
+	int err;
+
+	val.net_idx = node->net_idx;
+	val.num_elem = node->num_elem;
+	memcpy(val.dev_key, node->dev_key, 16);
+
+	snprintk(path, sizeof(path), "bt_mesh/Node/%x", node->addr);
+
+	str = settings_str_from_bytes(&val, sizeof(val), buf, sizeof(buf));
+	if (!str) {
+		BT_ERR("Unable to encode Node as value");
+		return;
+	}
+
+
+	err = settings_save_one(path, str);
+	if (err) {
+		BT_ERR("Failed to store Node %s value", path);
+	} else {
+		BT_DBG("Stored Node %s value", path);
+	}
+}
+
+static void clear_node(u16_t addr)
+{
+	char path[20];
+	int err;
+
+	BT_DBG("Node 0x%04x", addr);
+
+	snprintk(path, sizeof(path), "bt_mesh/Node/%x", addr);
+	err = settings_save_one(path, NULL);
+	if (err) {
+		BT_ERR("Failed to clear Node 0x%04x", addr);
+	} else {
+		BT_DBG("Cleared Node 0x%04x", addr);
+	}
+}
+
+static void store_pending_nodes(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(node_updates); ++i) {
+		struct node_update *update = &node_updates[i];
+
+		if (update->addr == BT_MESH_ADDR_UNASSIGNED) {
+			continue;
+		}
+
+		if (update->clear) {
+			clear_node(update->addr);
+		} else {
+			struct bt_mesh_node *node;
+
+			node = bt_mesh_node_find(update->addr);
+			if (node) {
+				store_node(node);
+			} else {
+				BT_WARN("Node 0x%04x not found", update->addr);
+			}
+		}
+
+		update->addr = BT_MESH_ADDR_UNASSIGNED;
+	}
+}
+
+static struct node_update *node_update_find(u16_t addr,
+					    struct node_update **free_slot)
+{
+	struct node_update *match;
+	int i;
+
+	match = NULL;
+	*free_slot = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(node_updates); i++) {
+		struct node_update *update = &node_updates[i];
+
+		if (update->addr == BT_MESH_ADDR_UNASSIGNED) {
+			*free_slot = update;
+			continue;
+		}
+
+		if (update->addr == addr) {
+			match = update;
+		}
+	}
+
+	return match;
+}
+
+static void encode_mod_path(struct bt_mesh_model *mod, bool vnd,
+			    const char *key, char *path, size_t path_len)
+{
+	u16_t mod_key = (((u16_t)mod->elem_idx << 8) | mod->mod_idx);
+
+	if (vnd) {
+		snprintk(path, path_len, "bt_mesh/v/%x/%s", mod_key, key);
+	} else {
+		snprintk(path, path_len, "bt_mesh/s/%x/%s", mod_key, key);
+	}
+}
+
+static void store_pending_mod_bind(struct bt_mesh_model *mod, bool vnd)
+{
+	u16_t keys[CONFIG_BT_MESH_MODEL_KEY_COUNT];
+	char buf[BT_SETTINGS_SIZE(sizeof(keys))];
+	char path[20];
+	int i, count, err;
+	char *val;
+
+	for (i = 0, count = 0; i < ARRAY_SIZE(mod->keys); i++) {
+		if (mod->keys[i] != BT_MESH_KEY_UNUSED) {
+			keys[count++] = mod->keys[i];
+		}
+	}
+
+	if (count) {
+		val = settings_str_from_bytes(keys, count * sizeof(keys[0]),
+					      buf, sizeof(buf));
+		if (!val) {
+			BT_ERR("Unable to encode model bindings as value");
+			return;
+		}
+	} else {
+		val = NULL;
+	}
+
+	encode_mod_path(mod, vnd, "bind", path, sizeof(path));
+
+	BT_DBG("Saving %s as %s", path, val ? val : "(null)");
+	err = settings_save_one(path, val);
+	if (err) {
+		BT_ERR("Failed to store bind");
+	} else {
+		BT_DBG("Stored bind");
+	}
+}
+
+static void store_pending_mod_sub(struct bt_mesh_model *mod, bool vnd)
+{
+	u16_t groups[CONFIG_BT_MESH_MODEL_GROUP_COUNT];
+	char buf[BT_SETTINGS_SIZE(sizeof(groups))];
+	char path[20];
+	int i, count, err;
+	char *val;
+
+	for (i = 0, count = 0; i < CONFIG_BT_MESH_MODEL_GROUP_COUNT; i++) {
+		if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) {
+			groups[count++] = mod->groups[i];
+		}
+	}
+
+	if (count) {
+		val = settings_str_from_bytes(groups, count * sizeof(groups[0]),
+					      buf, sizeof(buf));
+		if (!val) {
+			BT_ERR("Unable to encode model subscription as value");
+			return;
+		}
+	} else {
+		val = NULL;
+	}
+
+	encode_mod_path(mod, vnd, "sub", path, sizeof(path));
+
+	BT_DBG("Saving %s as %s", path, val ? val : "(null)");
+	err = settings_save_one(path, val);
+	if (err) {
+		BT_ERR("Failed to store sub");
+	} else {
+		BT_DBG("Stored sub");
+	}
+}
+
+static void store_pending_mod_pub(struct bt_mesh_model *mod, bool vnd)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct mod_pub_val))];
+	struct mod_pub_val pub;
+	char path[20];
+	char *val;
+	int err;
+
+	if (!mod->pub || mod->pub->addr == BT_MESH_ADDR_UNASSIGNED) {
+		val = NULL;
+	} else {
+		pub.addr = mod->pub->addr;
+		pub.key = mod->pub->key;
+		pub.ttl = mod->pub->ttl;
+		pub.retransmit = mod->pub->retransmit;
+		pub.period = mod->pub->period;
+		pub.period_div = mod->pub->period_div;
+		pub.cred = mod->pub->cred;
+
+		val = settings_str_from_bytes(&pub, sizeof(pub),
+					      buf, sizeof(buf));
+		if (!val) {
+			BT_ERR("Unable to encode model publication as value");
+			return;
+		}
+	}
+
+	encode_mod_path(mod, vnd, "pub", path, sizeof(path));
+
+	BT_DBG("Saving %s as %s", path, val ? val : "(null)");
+	err = settings_save_one(path, val);
+	if (err) {
+		BT_ERR("Failed to store pub");
+	} else {
+		BT_DBG("Stored pub");
+	}
+}
+
+static void store_pending_mod(struct bt_mesh_model *mod,
+			      struct bt_mesh_elem *elem, bool vnd,
+			      bool primary, void *user_data)
+{
+	if (!mod->flags) {
+		return;
+	}
+
+	if (mod->flags & BT_MESH_MOD_BIND_PENDING) {
+		mod->flags &= ~BT_MESH_MOD_BIND_PENDING;
+		store_pending_mod_bind(mod, vnd);
+	}
+
+	if (mod->flags & BT_MESH_MOD_SUB_PENDING) {
+		mod->flags &= ~BT_MESH_MOD_SUB_PENDING;
+		store_pending_mod_sub(mod, vnd);
+	}
+
+	if (mod->flags & BT_MESH_MOD_PUB_PENDING) {
+		mod->flags &= ~BT_MESH_MOD_PUB_PENDING;
+		store_pending_mod_pub(mod, vnd);
+	}
+}
+
+#define IS_VA_DEL(_label)	((_label)->ref == 0)
+static void store_pending_va(void)
+{
+	char buf[BT_SETTINGS_SIZE(sizeof(struct va_val))];
+	struct label *lab;
+	struct va_val va;
+	char path[18];
+	char *val;
+	u16_t i;
+	int err = 0;
+
+	for (i = 0; (lab = get_label(i)) != NULL; i++) {
+		if (!atomic_test_and_clear_bit(lab->flags,
+					       BT_MESH_VA_CHANGED)) {
+			continue;
+		}
+
+		snprintk(path, sizeof(path), "bt_mesh/Va/%x", i);
+
+		if (IS_VA_DEL(lab)) {
+			val = NULL;
+		} else {
+			va.ref = lab->ref;
+			va.addr = lab->addr;
+			memcpy(va.uuid, lab->uuid, 16);
+
+			val = settings_str_from_bytes(&va, sizeof(va),
+						      buf, sizeof(buf));
+			if (!val) {
+				BT_ERR("Unable to encode model publication as value");
+				return;
+			}
+
+			err = settings_save_one(path, val);
+		}
+
+		if (err) {
+			BT_ERR("Failed to %s %s value (err %d)",
+			       IS_VA_DEL(lab) ? "delete" : "store", path, err);
+		} else {
+			BT_DBG("%s %s value",
+			       IS_VA_DEL(lab) ? "Deleted" : "Stored", path);
+		}
+	}
+}
+
+static void store_pending(struct ble_npl_event *work)
+{
+	BT_DBG("");
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_RPL_PENDING)) {
+		if (atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+			store_pending_rpl();
+		} else {
+			clear_rpl();
+		}
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_KEYS_PENDING)) {
+		store_pending_keys();
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_NET_PENDING)) {
+		if (atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+			store_pending_net();
+		} else {
+			clear_net();
+		}
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_IV_PENDING)) {
+		if (atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+			store_pending_iv();
+		} else {
+			clear_iv();
+		}
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_SEQ_PENDING)) {
+		store_pending_seq();
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_HB_PUB_PENDING)) {
+		store_pending_hb_pub();
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_CFG_PENDING)) {
+		if (atomic_test_bit(bt_mesh.flags, BT_MESH_VALID)) {
+			store_pending_cfg();
+		} else {
+			clear_cfg();
+		}
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_MOD_PENDING)) {
+		bt_mesh_model_foreach(store_pending_mod, NULL);
+	}
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_VA_PENDING)) {
+		store_pending_va();
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER) &&
+	    atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_NODES_PENDING)) {
+		store_pending_nodes();
+	}
+}
+
+void bt_mesh_store_rpl(struct bt_mesh_rpl *entry)
+{
+	entry->store = true;
+	schedule_store(BT_MESH_RPL_PENDING);
+}
+
+static struct key_update *key_update_find(bool app_key, u16_t key_idx,
+					  struct key_update **free_slot)
+{
+	struct key_update *match;
+	int i;
+
+	match = NULL;
+	*free_slot = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(key_updates); i++) {
+		struct key_update *update = &key_updates[i];
+
+		if (!update->valid) {
+			*free_slot = update;
+			continue;
+		}
+
+		if (update->app_key != app_key) {
+			continue;
+		}
+
+		if (update->key_idx == key_idx) {
+			match = update;
+		}
+	}
+
+	return match;
+}
+
+void bt_mesh_store_subnet(struct bt_mesh_subnet *sub)
+{
+	struct key_update *update, *free_slot;
+
+	BT_DBG("NetKeyIndex 0x%03x", sub->net_idx);
+
+	update = key_update_find(false, sub->net_idx, &free_slot);
+	if (update) {
+		update->clear = 0;
+		schedule_store(BT_MESH_KEYS_PENDING);
+		return;
+	}
+
+	if (!free_slot) {
+		store_net_key(sub);
+		return;
+	}
+
+	free_slot->valid = 1;
+	free_slot->key_idx = sub->net_idx;
+	free_slot->app_key = 0;
+	free_slot->clear = 0;
+
+	schedule_store(BT_MESH_KEYS_PENDING);
+}
+
+void bt_mesh_store_app_key(struct bt_mesh_app_key *key)
+{
+	struct key_update *update, *free_slot;
+
+	BT_DBG("AppKeyIndex 0x%03x", key->app_idx);
+
+	update = key_update_find(true, key->app_idx, &free_slot);
+	if (update) {
+		update->clear = 0;
+		schedule_store(BT_MESH_KEYS_PENDING);
+		return;
+	}
+
+	if (!free_slot) {
+		store_app_key(key);
+		return;
+	}
+
+	free_slot->valid = 1;
+	free_slot->key_idx = key->app_idx;
+	free_slot->app_key = 1;
+	free_slot->clear = 0;
+
+	schedule_store(BT_MESH_KEYS_PENDING);
+}
+
+void bt_mesh_store_hb_pub(void)
+{
+	schedule_store(BT_MESH_HB_PUB_PENDING);
+}
+
+void bt_mesh_store_cfg(void)
+{
+	schedule_store(BT_MESH_CFG_PENDING);
+}
+
+void bt_mesh_clear_net(void)
+{
+	schedule_store(BT_MESH_NET_PENDING);
+	schedule_store(BT_MESH_IV_PENDING);
+	schedule_store(BT_MESH_CFG_PENDING);
+}
+
+void bt_mesh_clear_subnet(struct bt_mesh_subnet *sub)
+{
+	struct key_update *update, *free_slot;
+
+	BT_DBG("NetKeyIndex 0x%03x", sub->net_idx);
+
+	update = key_update_find(false, sub->net_idx, &free_slot);
+	if (update) {
+		update->clear = 1;
+		schedule_store(BT_MESH_KEYS_PENDING);
+		return;
+	}
+
+	if (!free_slot) {
+		clear_net_key(sub->net_idx);
+		return;
+	}
+
+	free_slot->valid = 1;
+	free_slot->key_idx = sub->net_idx;
+	free_slot->app_key = 0;
+	free_slot->clear = 1;
+
+	schedule_store(BT_MESH_KEYS_PENDING);
+}
+
+void bt_mesh_clear_app_key(struct bt_mesh_app_key *key)
+{
+	struct key_update *update, *free_slot;
+
+	BT_DBG("AppKeyIndex 0x%03x", key->app_idx);
+
+	update = key_update_find(true, key->app_idx, &free_slot);
+	if (update) {
+		update->clear = 1;
+		schedule_store(BT_MESH_KEYS_PENDING);
+		return;
+	}
+
+	if (!free_slot) {
+		clear_app_key(key->app_idx);
+		return;
+	}
+
+	free_slot->valid = 1;
+	free_slot->key_idx = key->app_idx;
+	free_slot->app_key = 1;
+	free_slot->clear = 1;
+
+	schedule_store(BT_MESH_KEYS_PENDING);
+}
+
+void bt_mesh_clear_rpl(void)
+{
+	schedule_store(BT_MESH_RPL_PENDING);
+}
+
+void bt_mesh_store_mod_bind(struct bt_mesh_model *mod)
+{
+	mod->flags |= BT_MESH_MOD_BIND_PENDING;
+	schedule_store(BT_MESH_MOD_PENDING);
+}
+
+void bt_mesh_store_mod_sub(struct bt_mesh_model *mod)
+{
+	mod->flags |= BT_MESH_MOD_SUB_PENDING;
+	schedule_store(BT_MESH_MOD_PENDING);
+}
+
+void bt_mesh_store_mod_pub(struct bt_mesh_model *mod)
+{
+	mod->flags |= BT_MESH_MOD_PUB_PENDING;
+	schedule_store(BT_MESH_MOD_PENDING);
+}
+
+
+void bt_mesh_store_label(void)
+{
+	schedule_store(BT_MESH_VA_PENDING);
+}
+
+void bt_mesh_store_node(struct bt_mesh_node *node)
+{
+	struct node_update *update, *free_slot;
+
+	BT_DBG("Node 0x%04x", node->addr);
+
+	update = node_update_find(node->addr, &free_slot);
+	if (update) {
+		update->clear = false;
+		schedule_store(BT_MESH_NODES_PENDING);
+		return;
+	}
+
+	if (!free_slot) {
+		store_node(node);
+		return;
+	}
+
+	free_slot->addr = node->addr;
+
+	schedule_store(BT_MESH_NODES_PENDING);
+}
+
+void bt_mesh_clear_node(struct bt_mesh_node *node)
+{
+	struct node_update *update, *free_slot;
+
+	BT_DBG("Node 0x%04x", node->addr);
+
+	update = node_update_find(node->addr, &free_slot);
+	if (update) {
+		update->clear = true;
+		schedule_store(BT_MESH_NODES_PENDING);
+		return;
+	}
+
+	if (!free_slot) {
+		clear_node(node->addr);
+		return;
+	}
+
+	free_slot->addr = node->addr;
+
+	schedule_store(BT_MESH_NODES_PENDING);
+}
+
+int bt_mesh_model_data_store(struct bt_mesh_model *mod, bool vnd,
+			     const void *data, size_t data_len)
+{
+	char path[20];
+	char buf[BT_SETTINGS_SIZE(sizeof(struct mod_pub_val))];
+	char *val;
+	int err;
+
+	encode_mod_path(mod, vnd, "data", path, sizeof(path));
+
+	if (data_len) {
+		mod->flags |= BT_MESH_MOD_DATA_PRESENT;
+		val = settings_str_from_bytes(data, data_len,
+					      buf, sizeof(buf));
+		if (!val) {
+			BT_ERR("Unable to encode model publication as value");
+			return -EINVAL;
+		}
+		err = settings_save_one(path, val);
+	} else if (mod->flags & BT_MESH_MOD_DATA_PRESENT) {
+		mod->flags &= ~BT_MESH_MOD_DATA_PRESENT;
+		err = settings_save_one(path, NULL);
+	} else {
+		/* Nothing to delete */
+		err = 0;
+	}
+
+	if (err) {
+		BT_ERR("Failed to store %s value", path);
+	} else {
+		BT_DBG("Stored %s value", path);
+	}
+	return err;
+}
+
+static struct conf_handler bt_mesh_settings_conf_handler = {
+	.ch_name = "bt_mesh",
+	.ch_get = NULL,
+	.ch_set = mesh_set,
+	.ch_commit = mesh_commit,
+	.ch_export = NULL,
+};
+
+void bt_mesh_settings_init(void)
+{
+	int rc;
+
+	rc = conf_register(&bt_mesh_settings_conf_handler);
+
+	SYSINIT_PANIC_ASSERT_MSG(rc == 0,
+				 "Failed to register bt_mesh_settings conf");
+
+	k_delayed_work_init(&pending_store, store_pending);
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH_SETTINGS) */
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/settings.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/settings.h
new file mode 100644
index 00000000..c630814e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/settings.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2018 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+void bt_mesh_store_net(void);
+void bt_mesh_store_iv(bool only_duration);
+void bt_mesh_store_seq(void);
+void bt_mesh_store_rpl(struct bt_mesh_rpl *rpl);
+void bt_mesh_store_subnet(struct bt_mesh_subnet *sub);
+void bt_mesh_store_app_key(struct bt_mesh_app_key *key);
+void bt_mesh_store_hb_pub(void);
+void bt_mesh_store_cfg(void);
+void bt_mesh_store_mod_bind(struct bt_mesh_model *mod);
+void bt_mesh_store_mod_sub(struct bt_mesh_model *mod);
+void bt_mesh_store_mod_pub(struct bt_mesh_model *mod);
+void bt_mesh_store_label(void);
+void bt_mesh_store_node(struct bt_mesh_node *node);
+
+void bt_mesh_clear_net(void);
+void bt_mesh_clear_subnet(struct bt_mesh_subnet *sub);
+void bt_mesh_clear_app_key(struct bt_mesh_app_key *key);
+void bt_mesh_clear_rpl(void);
+void bt_mesh_clear_node(struct bt_mesh_node *node);
+
+void bt_mesh_settings_init(void);
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/shell.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/shell.c
new file mode 100644
index 00000000..91fbd978
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/shell.c
@@ -0,0 +1,2819 @@
+/** @file
+ *  @brief Bluetooth Mesh shell
+ *
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BLE_MESH_SHELL)
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <errno.h>
+#include "shell/shell.h"
+#include "console/console.h"
+#include "mesh/mesh.h"
+#include "mesh/main.h"
+#include "mesh/glue.h"
+#include "mesh/testing.h"
+
+/* Private includes for raw Network & Transport layer access */
+#include "net.h"
+#include "access.h"
+#include "mesh_priv.h"
+#include "lpn.h"
+#include "transport.h"
+#include "foundation.h"
+#include "testing.h"
+#include "settings.h"
+
+#if MYNEWT_VAL(BLE_MESH_SHELL_MODELS)
+#include "mesh/model_srv.h"
+#include "mesh/model_cli.h"
+#include "light_model.h"
+#endif
+
+/* This should be higher priority (lower value) than main task priority */
+#define BLE_MESH_SHELL_TASK_PRIO 126
+#define BLE_MESH_SHELL_STACK_SIZE 768
+
+OS_TASK_STACK_DEFINE(g_blemesh_shell_stack, BLE_MESH_SHELL_STACK_SIZE);
+
+struct os_task mesh_shell_task;
+static struct os_eventq mesh_shell_queue;
+
+#define CID_NVAL   0xffff
+#define CID_VENDOR  0x05C3
+
+/* Vendor Model data */
+#define VND_MODEL_ID_1 0x1234
+
+/* Default net, app & dev key values, unless otherwise specified */
+static const u8_t default_key[16] = {
+	0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef,
+	0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef,
+};
+
+static struct {
+	u16_t local;
+	u16_t dst;
+	u16_t net_idx;
+	u16_t app_idx;
+} net = {
+	.local = BT_MESH_ADDR_UNASSIGNED,
+	.dst = BT_MESH_ADDR_UNASSIGNED,
+};
+
+static struct bt_mesh_cfg_srv cfg_srv = {
+	.relay = BT_MESH_RELAY_DISABLED,
+	.beacon = BT_MESH_BEACON_ENABLED,
+#if MYNEWT_VAL(BLE_MESH_FRIEND)
+	.frnd = BT_MESH_FRIEND_DISABLED,
+#else
+	.frnd = BT_MESH_FRIEND_NOT_SUPPORTED,
+#endif
+#if MYNEWT_VAL(BLE_MESH_GATT_PROXY)
+	.gatt_proxy = BT_MESH_GATT_PROXY_DISABLED,
+#else
+	.gatt_proxy = BT_MESH_GATT_PROXY_NOT_SUPPORTED,
+#endif
+
+	.default_ttl = 7,
+
+	/* 3 transmissions with 20ms interval */
+	.net_transmit = BT_MESH_TRANSMIT(2, 20),
+	.relay_retransmit = BT_MESH_TRANSMIT(2, 20),
+};
+
+#define CUR_FAULTS_MAX 4
+
+static u8_t cur_faults[CUR_FAULTS_MAX];
+static u8_t reg_faults[CUR_FAULTS_MAX * 2];
+
+static void get_faults(u8_t *faults, u8_t faults_size, u8_t *dst, u8_t *count)
+{
+	u8_t i, limit = *count;
+
+	for (i = 0, *count = 0; i < faults_size && *count < limit; i++) {
+		if (faults[i]) {
+			*dst++ = faults[i];
+			(*count)++;
+		}
+	}
+}
+
+static int fault_get_cur(struct bt_mesh_model *model, u8_t *test_id,
+			 u16_t *company_id, u8_t *faults, u8_t *fault_count)
+{
+	printk("Sending current faults\n");
+
+	*test_id = 0x00;
+	*company_id = CID_VENDOR;
+
+	get_faults(cur_faults, sizeof(cur_faults), faults, fault_count);
+
+	return 0;
+}
+
+static int fault_get_reg(struct bt_mesh_model *model, u16_t cid,
+			 u8_t *test_id, u8_t *faults, u8_t *fault_count)
+{
+	if (cid != CID_VENDOR) {
+		printk("Faults requested for unknown Company ID 0x%04x\n", cid);
+		return -EINVAL;
+	}
+
+	printk("Sending registered faults\n");
+
+	*test_id = 0x00;
+
+	get_faults(reg_faults, sizeof(reg_faults), faults, fault_count);
+
+	return 0;
+}
+
+static int fault_clear(struct bt_mesh_model *model, uint16_t cid)
+{
+	if (cid != CID_VENDOR) {
+		return -EINVAL;
+	}
+
+	memset(reg_faults, 0, sizeof(reg_faults));
+
+	return 0;
+}
+
+static int fault_test(struct bt_mesh_model *model, uint8_t test_id,
+		      uint16_t cid)
+{
+	if (cid != CID_VENDOR) {
+		return -EINVAL;
+	}
+
+	if (test_id != 0x00) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct bt_mesh_health_srv_cb health_srv_cb = {
+	.fault_get_cur = fault_get_cur,
+	.fault_get_reg = fault_get_reg,
+	.fault_clear = fault_clear,
+	.fault_test = fault_test,
+};
+
+static struct bt_mesh_health_srv health_srv = {
+	.cb = &health_srv_cb,
+};
+
+static struct bt_mesh_model_pub health_pub;
+
+static void
+health_pub_init(void)
+{
+	health_pub.msg  = BT_MESH_HEALTH_FAULT_MSG(CUR_FAULTS_MAX);
+}
+#if MYNEWT_VAL(BLE_MESH_CFG_CLI)
+
+static struct bt_mesh_cfg_cli cfg_cli = {
+};
+
+#endif /* MYNEWT_VAL(BLE_MESH_CFG_CLI) */
+
+#if MYNEWT_VAL(BLE_MESH_HEALTH_CLI)
+void show_faults(u8_t test_id, u16_t cid, u8_t *faults, size_t fault_count)
+{
+	size_t i;
+
+	if (!fault_count) {
+		printk("Health Test ID 0x%02x Company ID 0x%04x: no faults\n",
+		       test_id, cid);
+		return;
+	}
+
+	printk("Health Test ID 0x%02x Company ID 0x%04x Fault Count %zu:\n",
+	       test_id, cid, fault_count);
+
+	for (i = 0; i < fault_count; i++) {
+		printk("\t0x%02x\n", faults[i]);
+	}
+}
+
+static void health_current_status(struct bt_mesh_health_cli *cli, u16_t addr,
+				  u8_t test_id, u16_t cid, u8_t *faults,
+				  size_t fault_count)
+{
+	printk("Health Current Status from 0x%04x\n", addr);
+	show_faults(test_id, cid, faults, fault_count);
+}
+
+static struct bt_mesh_health_cli health_cli = {
+	.current_status = health_current_status,
+};
+
+#endif /* MYNEWT_VAL(BLE_MESH_HEALTH_CLI) */
+
+#if MYNEWT_VAL(BLE_MESH_SHELL_MODELS)
+static struct bt_mesh_gen_model_cli gen_onoff_cli;
+static struct bt_mesh_model_pub gen_onoff_cli_pub;
+static struct bt_mesh_model_pub gen_onoff_srv_pub;
+static struct bt_mesh_gen_model_cli gen_level_cli;
+static struct bt_mesh_model_pub gen_level_cli_pub;
+static struct bt_mesh_model_pub gen_level_srv_pub;
+static struct bt_mesh_model_pub light_lightness_pub;
+static struct bt_mesh_gen_onoff_srv gen_onoff_srv = {
+	.get = light_model_gen_onoff_get,
+	.set = light_model_gen_onoff_set,
+};
+static struct bt_mesh_gen_level_srv gen_level_srv = {
+	.get = light_model_gen_level_get,
+	.set = light_model_gen_level_set,
+};
+static struct bt_mesh_light_lightness_srv light_lightness_srv = {
+	.get = light_model_light_lightness_get,
+	.set = light_model_light_lightness_set,
+};
+
+void bt_mesh_set_gen_onoff_srv_cb(int (*get)(struct bt_mesh_model *model, u8_t *state),
+				  int (*set)(struct bt_mesh_model *model, u8_t state))
+{
+	gen_onoff_srv.get = get;
+	gen_onoff_srv.set = set;
+}
+
+void bt_mesh_set_gen_level_srv_cb(int (*get)(struct bt_mesh_model *model, s16_t *level),
+				  int (*set)(struct bt_mesh_model *model, s16_t level))
+{
+	gen_level_srv.get = get;
+	gen_level_srv.set = set;
+}
+
+void bt_mesh_set_light_lightness_srv_cb(int (*get)(struct bt_mesh_model *model, s16_t *level),
+					int (*set)(struct bt_mesh_model *model, s16_t level))
+{
+	light_lightness_srv.get = get;
+	light_lightness_srv.set = set;
+}
+#endif
+
+static struct bt_mesh_model root_models[] = {
+	BT_MESH_MODEL_CFG_SRV(&cfg_srv),
+	BT_MESH_MODEL_HEALTH_SRV(&health_srv, &health_pub),
+#if MYNEWT_VAL(BLE_MESH_CFG_CLI)
+	BT_MESH_MODEL_CFG_CLI(&cfg_cli),
+#endif
+#if MYNEWT_VAL(BLE_MESH_HEALTH_CLI)
+	BT_MESH_MODEL_HEALTH_CLI(&health_cli),
+#endif
+#if MYNEWT_VAL(BLE_MESH_SHELL_MODELS)
+	BT_MESH_MODEL_GEN_ONOFF_SRV(&gen_onoff_srv, &gen_onoff_srv_pub),
+	BT_MESH_MODEL_GEN_ONOFF_CLI(&gen_onoff_cli, &gen_onoff_cli_pub),
+	BT_MESH_MODEL_GEN_LEVEL_SRV(&gen_level_srv, &gen_level_srv_pub),
+	BT_MESH_MODEL_GEN_LEVEL_CLI(&gen_level_cli, &gen_level_cli_pub),
+	BT_MESH_MODEL_LIGHT_LIGHTNESS_SRV(&light_lightness_srv, &light_lightness_pub),
+#endif
+};
+
+static struct bt_mesh_model vnd_models[] = {
+	BT_MESH_MODEL_VND(CID_VENDOR, VND_MODEL_ID_1,
+			  BT_MESH_MODEL_NO_OPS, NULL, NULL),
+};
+
+static struct bt_mesh_elem elements[] = {
+	BT_MESH_ELEM(0, root_models, vnd_models),
+};
+
+static const struct bt_mesh_comp comp = {
+	.cid = CID_VENDOR,
+	.elem = elements,
+	.elem_count = ARRAY_SIZE(elements),
+};
+
+static u8_t hex2val(char c)
+{
+	if (c >= '0' && c <= '9') {
+		return c - '0';
+	} else if (c >= 'a' && c <= 'f') {
+		return c - 'a' + 10;
+	} else if (c >= 'A' && c <= 'F') {
+		return c - 'A' + 10;
+	} else {
+		return 0;
+	}
+}
+
+static size_t hex2bin(const char *hex, u8_t *bin, size_t bin_len)
+{
+	size_t len = 0;
+
+	while (*hex && len < bin_len) {
+		bin[len] = hex2val(*hex++) << 4;
+
+		if (!*hex) {
+			len++;
+			break;
+		}
+
+		bin[len++] |= hex2val(*hex++);
+	}
+
+	return len;
+}
+
+static void prov_complete(u16_t net_idx, u16_t addr)
+{
+	printk("Local node provisioned, net_idx 0x%04x address 0x%04x\n",
+	       net_idx, addr);
+	net.local = addr;
+	net.net_idx = net_idx,
+	net.dst = addr;
+}
+
+static void prov_node_added(u16_t net_idx, u16_t addr, u8_t num_elem)
+{
+	printk("Node provisioned, net_idx 0x%04x address "
+	       "0x%04x elements %d", net_idx, addr, num_elem);
+
+	net.net_idx = net_idx,
+	net.dst = addr;
+}
+
+static void prov_input_complete(void)
+{
+	printk("Input complete");
+}
+
+static void prov_reset(void)
+{
+	printk("The local node has been reset and needs reprovisioning\n");
+}
+
+static int output_number(bt_mesh_output_action_t action, uint32_t number)
+{
+	printk("OOB Number: %lu\n", number);
+	return 0;
+}
+
+static int output_string(const char *str)
+{
+	printk("OOB String: %s\n", str);
+	return 0;
+}
+
+static bt_mesh_input_action_t input_act;
+static u8_t input_size;
+
+static int cmd_input_num(int argc, char *argv[])
+{
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	if (input_act != BT_MESH_ENTER_NUMBER) {
+		printk("A number hasn't been requested!\n");
+		return 0;
+	}
+
+	if (strlen(argv[1]) < input_size) {
+		printk("Too short input (%u digits required)\n",
+		       input_size);
+		return 0;
+	}
+
+	err = bt_mesh_input_number(strtoul(argv[1], NULL, 10));
+	if (err) {
+		printk("Numeric input failed (err %d)\n", err);
+		return 0;
+	}
+
+	input_act = BT_MESH_NO_INPUT;
+	return 0;
+}
+
+struct shell_cmd_help cmd_input_num_help = {
+	NULL, "<number>", NULL
+};
+
+static int cmd_input_str(int argc, char *argv[])
+{
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	if (input_act != BT_MESH_ENTER_STRING) {
+		printk("A string hasn't been requested!\n");
+		return 0;
+	}
+
+	if (strlen(argv[1]) < input_size) {
+		printk("Too short input (%u characters required)\n",
+		       input_size);
+		return 0;
+	}
+
+	err = bt_mesh_input_string(argv[1]);
+	if (err) {
+		printk("String input failed (err %d)\n", err);
+		return 0;
+	}
+
+	input_act = BT_MESH_NO_INPUT;
+	return 0;
+}
+
+struct shell_cmd_help cmd_input_str_help = {
+	NULL, "<string>", NULL
+};
+
+static int input(bt_mesh_input_action_t act, u8_t size)
+{
+	switch (act) {
+	case BT_MESH_ENTER_NUMBER:
+		printk("Enter a number (max %u digits) with: input-num <num>\n",
+		       size);
+		break;
+	case BT_MESH_ENTER_STRING:
+		printk("Enter a string (max %u chars) with: input-str <str>\n",
+		       size);
+		break;
+	default:
+		printk("Unknown input action %u (size %u) requested!\n",
+		       act, size);
+		return -EINVAL;
+	}
+
+	input_act = act;
+	input_size = size;
+	return 0;
+}
+
+static const char *bearer2str(bt_mesh_prov_bearer_t bearer)
+{
+	switch (bearer) {
+	case BT_MESH_PROV_ADV:
+		return "PB-ADV";
+	case BT_MESH_PROV_GATT:
+		return "PB-GATT";
+	default:
+		return "unknown";
+	}
+}
+
+static void link_open(bt_mesh_prov_bearer_t bearer)
+{
+	printk("Provisioning link opened on %s\n", bearer2str(bearer));
+}
+
+static void link_close(bt_mesh_prov_bearer_t bearer)
+{
+	printk("Provisioning link closed on %s\n", bearer2str(bearer));
+}
+
+static u8_t dev_uuid[16] = MYNEWT_VAL(BLE_MESH_DEV_UUID);
+
+static u8_t static_val[16];
+
+static struct bt_mesh_prov prov = {
+	.uuid = dev_uuid,
+	.link_open = link_open,
+	.link_close = link_close,
+	.complete = prov_complete,
+	.node_added = prov_node_added,
+	.reset = prov_reset,
+	.static_val = NULL,
+	.static_val_len = 0,
+	.output_size = MYNEWT_VAL(BLE_MESH_OOB_OUTPUT_SIZE),
+	.output_actions = MYNEWT_VAL(BLE_MESH_OOB_OUTPUT_ACTIONS),
+	.output_number = output_number,
+	.output_string = output_string,
+	.input_size = MYNEWT_VAL(BLE_MESH_OOB_INPUT_SIZE),
+	.input_actions = MYNEWT_VAL(BLE_MESH_OOB_INPUT_ACTIONS),
+	.input = input,
+	.input_complete = prov_input_complete,
+};
+
+static int cmd_static_oob(int argc, char *argv[])
+{
+	if (argc < 2) {
+		prov.static_val = NULL;
+		prov.static_val_len = 0;
+	} else {
+		prov.static_val_len = hex2bin(argv[1], static_val, 16);
+		if (prov.static_val_len) {
+			prov.static_val = static_val;
+		} else {
+			prov.static_val = NULL;
+		}
+	}
+
+	if (prov.static_val) {
+		printk("Static OOB value set (length %u)\n",
+		       prov.static_val_len);
+	} else {
+		printk("Static OOB value cleared\n");
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_static_oob_help = {
+	NULL, "[val: 1-16 hex values]", NULL
+};
+
+static int cmd_uuid(int argc, char *argv[])
+{
+	u8_t uuid[16];
+	size_t len;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	len = hex2bin(argv[1], uuid, sizeof(uuid));
+	if (len < 1) {
+		return -EINVAL;
+	}
+
+	memcpy(dev_uuid, uuid, len);
+	memset(dev_uuid + len, 0, sizeof(dev_uuid) - len);
+
+	printk("Device UUID set\n");
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_uuid_help = {
+	NULL, "<UUID: 1-16 hex values>", NULL
+};
+
+static int cmd_reset(int argc, char *argv[])
+{
+	bt_mesh_reset();
+	printk("Local node reset complete\n");
+	return 0;
+}
+
+static u8_t str2u8(const char *str)
+{
+	if (isdigit(str[0])) {
+		return strtoul(str, NULL, 0);
+	}
+
+	return (!strcmp(str, "on") || !strcmp(str, "enable"));
+}
+
+static bool str2bool(const char *str)
+{
+	return str2u8(str);
+}
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER)
+static int cmd_lpn(int argc, char *argv[])
+{
+	static bool enabled;
+	int err;
+
+	if (argc < 2) {
+		printk("%s\n", enabled ? "enabled" : "disabled");
+		return 0;
+	}
+
+	if (str2bool(argv[1])) {
+		if (enabled) {
+			printk("LPN already enabled\n");
+			return 0;
+		}
+
+		err = bt_mesh_lpn_set(true);
+		if (err) {
+			printk("Enabling LPN failed (err %d)\n", err);
+		} else {
+			enabled = true;
+		}
+	} else {
+		if (!enabled) {
+			printk("LPN already disabled\n");
+			return 0;
+		}
+
+		err = bt_mesh_lpn_set(false);
+		if (err) {
+			printk("Enabling LPN failed (err %d)\n", err);
+		} else {
+			enabled = false;
+		}
+	}
+
+	return 0;
+}
+
+static int cmd_poll(int argc, char *argv[])
+{
+	int err;
+
+	err = bt_mesh_lpn_poll();
+	if (err) {
+		printk("Friend Poll failed (err %d)\n", err);
+	}
+
+	return 0;
+}
+
+static void lpn_cb(u16_t friend_addr, bool established)
+{
+	if (established) {
+		printk("Friendship (as LPN) established to Friend 0x%04x\n",
+		       friend_addr);
+	} else {
+		printk("Friendship (as LPN) lost with Friend 0x%04x\n",
+		       friend_addr);
+	}
+}
+
+struct shell_cmd_help cmd_lpn_help = {
+	NULL, "<value: off, on>", NULL
+};
+
+#endif /* MESH_LOW_POWER */
+
+static int check_pub_addr_unassigned(void)
+{
+#ifdef ARCH_sim
+	return 0;
+#else
+	uint8_t zero_addr[BLE_DEV_ADDR_LEN] = { 0 };
+
+	return memcmp(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR),
+		      zero_addr, BLE_DEV_ADDR_LEN) == 0;
+#endif
+}
+
+int cmd_mesh_init(int argc, char *argv[])
+{
+	int err;
+	ble_addr_t addr;
+
+	if (check_pub_addr_unassigned()) {
+		/* Use NRPA */
+		err = ble_hs_id_gen_rnd(1, &addr);
+		assert(err == 0);
+		err = ble_hs_id_set_rnd(addr.val);
+		assert(err == 0);
+
+		err = bt_mesh_init(addr.type, &prov, &comp);
+	}
+	else {
+		err = bt_mesh_init(0, &prov, &comp);
+	}
+
+	if (err) {
+		printk("Mesh initialization failed (err %d)\n", err);
+	}
+
+	printk("Mesh initialized\n");
+
+	if (IS_ENABLED(CONFIG_SETTINGS)) {
+		settings_load();
+	}
+
+	if (bt_mesh_is_provisioned()) {
+		printk("Mesh network restored from flash\n");
+	} else {
+		printk("Use \"pb-adv on\" or \"pb-gatt on\" to enable"
+		       " advertising\n");
+	}
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER)
+	bt_mesh_lpn_set_cb(lpn_cb);
+#endif
+
+	return 0;
+}
+
+#if MYNEWT_VAL(BLE_MESH_GATT_PROXY)
+static int cmd_ident(int argc, char *argv[])
+{
+	int err;
+
+	err = bt_mesh_proxy_identity_enable();
+	if (err) {
+		printk("Failed advertise using Node Identity (err %d)\n", err);
+	}
+
+	return 0;
+}
+#endif /* MESH_GATT_PROXY */
+
+static int cmd_dst(int argc, char *argv[])
+{
+	if (argc < 2) {
+		printk("Destination address: 0x%04x%s\n", net.dst,
+		       net.dst == net.local ? " (local)" : "");
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "local")) {
+		net.dst = net.local;
+	} else {
+		net.dst = strtoul(argv[1], NULL, 0);
+	}
+
+	printk("Destination address set to 0x%04x%s\n", net.dst,
+	       net.dst == net.local ? " (local)" : "");
+	return 0;
+}
+
+struct shell_cmd_help cmd_dst_help = {
+	NULL, "[destination address]", NULL
+};
+
+static int cmd_netidx(int argc, char *argv[])
+{
+	if (argc < 2) {
+		printk("NetIdx: 0x%04x\n", net.net_idx);
+		return 0;
+	}
+
+	net.net_idx = strtoul(argv[1], NULL, 0);
+	printk("NetIdx set to 0x%04x\n", net.net_idx);
+	return 0;
+}
+
+struct shell_cmd_help cmd_netidx_help = {
+	NULL, "[NetIdx]", NULL
+};
+
+static int cmd_appidx(int argc, char *argv[])
+{
+	if (argc < 2) {
+		printk("AppIdx: 0x%04x\n", net.app_idx);
+		return 0;
+	}
+
+	net.app_idx = strtoul(argv[1], NULL, 0);
+	printk("AppIdx set to 0x%04x\n", net.app_idx);
+	return 0;
+}
+
+struct shell_cmd_help cmd_appidx_help = {
+	NULL, "[AppIdx]", NULL
+};
+
+static int cmd_net_send(int argc, char *argv[])
+{
+	struct os_mbuf *msg = NET_BUF_SIMPLE(32);
+	struct bt_mesh_msg_ctx ctx = {
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+		.net_idx = net.net_idx,
+		.addr = net.dst,
+		.app_idx = net.app_idx,
+
+	};
+	struct bt_mesh_net_tx tx = {
+		.ctx = &ctx,
+		.src = net.local,
+		.xmit = bt_mesh_net_transmit_get(),
+		.sub = bt_mesh_subnet_get(net.net_idx),
+	};
+	size_t len;
+	int err = 0;
+
+	if (argc < 2) {
+		err = -EINVAL;
+		goto done;
+	}
+
+	if (!tx.sub) {
+		printk("No matching subnet for NetKey Index 0x%04x\n",
+		       net.net_idx);
+		goto done;
+	}
+
+	net_buf_simple_init(msg, 0);
+	len = hex2bin(argv[1], msg->om_data, net_buf_simple_tailroom(msg) - 4);
+	net_buf_simple_add(msg, len);
+
+	err = bt_mesh_trans_send(&tx, msg, NULL, NULL);
+	if (err) {
+		printk("Failed to send (err %d)\n", err);
+	}
+
+done:
+	os_mbuf_free_chain(msg);
+	return err;
+}
+
+struct shell_cmd_help cmd_net_send_help = {
+	NULL, "<hex string>", NULL
+};
+
+static int cmd_rpl_clear(int argc, char *argv[])
+{
+	bt_mesh_rpl_clear();
+	return 0;
+}
+
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER)
+static int cmd_lpn_subscribe(int argc, char *argv[])
+{
+	u16_t address;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	address = strtoul(argv[1], NULL, 0);
+
+	printk("address 0x%04x", address);
+
+	bt_mesh_lpn_group_add(address);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_lpn_subscribe_help = {
+	NULL, "<addr>", NULL
+};
+
+static int cmd_lpn_unsubscribe(int argc, char *argv[])
+{
+	u16_t address;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	address = strtoul(argv[1], NULL, 0);
+
+	printk("address 0x%04x", address);
+
+	bt_mesh_lpn_group_del(&address, 1);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_lpn_unsubscribe_help = {
+	NULL, "<addr>", NULL
+};
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_IV_UPDATE_TEST)
+static int cmd_iv_update(int argc, char *argv[])
+{
+	if (bt_mesh_iv_update()) {
+		printk("Transitioned to IV Update In Progress state\n");
+	} else {
+		printk("Transitioned to IV Update Normal state\n");
+	}
+
+	printk("IV Index is 0x%08lx\n", bt_mesh.iv_index);
+
+	return 0;
+}
+
+static int cmd_iv_update_test(int argc, char *argv[])
+{
+	bool enable;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	enable = str2bool(argv[1]);
+	if (enable) {
+		printk("Enabling IV Update test mode\n");
+	} else {
+		printk("Disabling IV Update test mode\n");
+	}
+
+	bt_mesh_iv_update_test(enable);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_iv_update_test_help = {
+	NULL, "<value: off, on>", NULL
+};
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_CFG_CLI)
+
+int cmd_timeout(int argc, char *argv[])
+{
+	s32_t timeout;
+
+	if (argc < 2) {
+		timeout = bt_mesh_cfg_cli_timeout_get();
+		if (timeout == K_FOREVER) {
+			printk("Message timeout: forever\n");
+		} else {
+			printk("Message timeout: %lu seconds\n",
+			       timeout / 1000);
+		}
+
+		return 0;
+	}
+
+	timeout = strtol(argv[1], NULL, 0);
+	if (timeout < 0 || timeout > (INT32_MAX / 1000)) {
+		timeout = K_FOREVER;
+	} else {
+		timeout = timeout * 1000;
+	}
+
+	bt_mesh_cfg_cli_timeout_set(timeout);
+	if (timeout == K_FOREVER) {
+		printk("Message timeout: forever\n");
+	} else {
+		printk("Message timeout: %lu seconds\n",
+		       timeout / 1000);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_timeout_help = {
+	NULL, "[timeout in seconds]", NULL
+};
+
+
+static int cmd_get_comp(int argc, char *argv[])
+{
+	struct os_mbuf *comp = NET_BUF_SIMPLE(32);
+	u8_t status, page = 0x00;
+	int err = 0;
+
+	if (argc > 1) {
+		page = strtol(argv[1], NULL, 0);
+	}
+
+	net_buf_simple_init(comp, 0);
+	err = bt_mesh_cfg_comp_data_get(net.net_idx, net.dst, page,
+					&status, comp);
+	if (err) {
+		printk("Getting composition failed (err %d)\n", err);
+		goto done;
+	}
+
+	if (status != 0x00) {
+		printk("Got non-success status 0x%02x\n", status);
+		goto done;
+	}
+
+	printk("Got Composition Data for 0x%04x:\n", net.dst);
+	printk("\tCID      0x%04x\n", net_buf_simple_pull_le16(comp));
+	printk("\tPID      0x%04x\n", net_buf_simple_pull_le16(comp));
+	printk("\tVID      0x%04x\n", net_buf_simple_pull_le16(comp));
+	printk("\tCRPL     0x%04x\n", net_buf_simple_pull_le16(comp));
+	printk("\tFeatures 0x%04x\n", net_buf_simple_pull_le16(comp));
+
+	while (comp->om_len > 4) {
+		u8_t sig, vnd;
+		u16_t loc;
+		int i;
+
+		loc = net_buf_simple_pull_le16(comp);
+		sig = net_buf_simple_pull_u8(comp);
+		vnd = net_buf_simple_pull_u8(comp);
+
+		printk("\n\tElement @ 0x%04x:\n", loc);
+
+		if (comp->om_len < ((sig * 2) + (vnd * 4))) {
+			printk("\t\t...truncated data!\n");
+			break;
+		}
+
+		if (sig) {
+			printk("\t\tSIG Models:\n");
+		} else {
+			printk("\t\tNo SIG Models\n");
+		}
+
+		for (i = 0; i < sig; i++) {
+			u16_t mod_id = net_buf_simple_pull_le16(comp);
+
+			printk("\t\t\t0x%04x\n", mod_id);
+		}
+
+		if (vnd) {
+			printk("\t\tVendor Models:\n");
+		} else {
+			printk("\t\tNo Vendor Models\n");
+		}
+
+		for (i = 0; i < vnd; i++) {
+			u16_t cid = net_buf_simple_pull_le16(comp);
+			u16_t mod_id = net_buf_simple_pull_le16(comp);
+
+			printk("\t\t\tCompany 0x%04x: 0x%04x\n", cid, mod_id);
+		}
+	}
+
+done:
+	os_mbuf_free_chain(comp);
+	return err;
+}
+
+struct shell_cmd_help cmd_get_comp_help = {
+	NULL, "[page]", NULL
+};
+
+static int cmd_beacon(int argc, char *argv[])
+{
+	u8_t status;
+	int err;
+
+	if (argc < 2) {
+		err = bt_mesh_cfg_beacon_get(net.net_idx, net.dst, &status);
+	} else {
+		u8_t val = str2u8(argv[1]);
+
+		err = bt_mesh_cfg_beacon_set(net.net_idx, net.dst, val,
+					     &status);
+	}
+
+	if (err) {
+		printk("Unable to send Beacon Get/Set message (err %d)\n", err);
+		return 0;
+	}
+
+	printk("Beacon state is 0x%02x\n", status);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_beacon_help = {
+	NULL, "[val: off, on]", NULL
+};
+
+static int cmd_ttl(int argc, char *argv[])
+{
+	u8_t ttl;
+	int err;
+
+	if (argc < 2) {
+		err = bt_mesh_cfg_ttl_get(net.net_idx, net.dst, &ttl);
+	} else {
+		u8_t val = strtoul(argv[1], NULL, 0);
+
+		err = bt_mesh_cfg_ttl_set(net.net_idx, net.dst, val, &ttl);
+	}
+
+	if (err) {
+		printk("Unable to send Default TTL Get/Set (err %d)\n", err);
+		return 0;
+	}
+
+	printk("Default TTL is 0x%02x\n", ttl);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_ttl_help = {
+	NULL, "[ttl: 0x00, 0x02-0x7f]", NULL
+};
+
+static int cmd_friend(int argc, char *argv[])
+{
+	u8_t frnd;
+	int err;
+
+	if (argc < 2) {
+		err = bt_mesh_cfg_friend_get(net.net_idx, net.dst, &frnd);
+	} else {
+		u8_t val = str2u8(argv[1]);
+
+		err = bt_mesh_cfg_friend_set(net.net_idx, net.dst, val, &frnd);
+	}
+
+	if (err) {
+		printk("Unable to send Friend Get/Set (err %d)\n", err);
+		return 0;
+	}
+
+	printk("Friend is set to 0x%02x\n", frnd);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_friend_help = {
+	NULL, "[val: off, on]", NULL
+};
+
+static int cmd_gatt_proxy(int argc, char *argv[])
+{
+	u8_t proxy;
+	int err;
+
+	if (argc < 2) {
+		err = bt_mesh_cfg_gatt_proxy_get(net.net_idx, net.dst, &proxy);
+	} else {
+		u8_t val = str2u8(argv[1]);
+
+		err = bt_mesh_cfg_gatt_proxy_set(net.net_idx, net.dst, val,
+						 &proxy);
+	}
+
+	if (err) {
+		printk("Unable to send GATT Proxy Get/Set (err %d)\n", err);
+		return 0;
+	}
+
+	printk("GATT Proxy is set to 0x%02x\n", proxy);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_gatt_proxy_help = {
+	NULL, "[val: off, on]", NULL
+};
+
+static int cmd_relay(int argc, char *argv[])
+{
+	u8_t relay, transmit;
+	int err;
+
+	if (argc < 2) {
+		err = bt_mesh_cfg_relay_get(net.net_idx, net.dst, &relay,
+					    &transmit);
+	} else {
+		u8_t val = str2u8(argv[1]);
+		u8_t count, interval, new_transmit;
+
+		if (val) {
+			if (argc > 2) {
+				count = strtoul(argv[2], NULL, 0);
+			} else {
+				count = 2;
+			}
+
+			if (argc > 3) {
+				interval = strtoul(argv[3], NULL, 0);
+			} else {
+				interval = 20;
+			}
+
+			new_transmit = BT_MESH_TRANSMIT(count, interval);
+		} else {
+			new_transmit = 0;
+		}
+
+		err = bt_mesh_cfg_relay_set(net.net_idx, net.dst, val,
+					    new_transmit, &relay, &transmit);
+	}
+
+	if (err) {
+		printk("Unable to send Relay Get/Set (err %d)\n", err);
+		return 0;
+	}
+
+	printk("Relay is 0x%02x, Transmit 0x%02x (count %u interval %ums)\n",
+	       relay, transmit, BT_MESH_TRANSMIT_COUNT(transmit),
+	       BT_MESH_TRANSMIT_INT(transmit));
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_relay_help = {
+	NULL, "[val: off, on] [count: 0-7] [interval: 0-32]", NULL
+};
+
+static int cmd_net_key_add(int argc, char *argv[])
+{
+	u8_t key_val[16];
+	u16_t key_net_idx;
+	u8_t status;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	key_net_idx = strtoul(argv[1], NULL, 0);
+
+	if (argc > 2) {
+		size_t len;
+
+		len = hex2bin(argv[3], key_val, sizeof(key_val));
+		memset(key_val, 0, sizeof(key_val) - len);
+	} else {
+		memcpy(key_val, default_key, sizeof(key_val));
+	}
+
+	err = bt_mesh_cfg_net_key_add(net.net_idx, net.dst, key_net_idx,
+				      key_val, &status);
+	if (err) {
+		printk("Unable to send NetKey Add (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("NetKeyAdd failed with status 0x%02x\n", status);
+	} else {
+		printk("NetKey added with NetKey Index 0x%03x\n", key_net_idx);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_net_key_add_help = {
+	NULL, "<NetKeyIndex> [val]", NULL
+};
+
+static int cmd_app_key_add(int argc, char *argv[])
+{
+	u8_t key_val[16];
+	u16_t key_net_idx, key_app_idx;
+	u8_t status;
+	int err;
+
+	if (argc < 3) {
+		return -EINVAL;
+	}
+
+	key_net_idx = strtoul(argv[1], NULL, 0);
+	key_app_idx = strtoul(argv[2], NULL, 0);
+
+	if (argc > 3) {
+		size_t len;
+
+		len = hex2bin(argv[3], key_val, sizeof(key_val));
+		memset(key_val, 0, sizeof(key_val) - len);
+	} else {
+		memcpy(key_val, default_key, sizeof(key_val));
+	}
+
+	err = bt_mesh_cfg_app_key_add(net.net_idx, net.dst, key_net_idx,
+				      key_app_idx, key_val, &status);
+	if (err) {
+		printk("Unable to send App Key Add (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("AppKeyAdd failed with status 0x%02x\n", status);
+	} else {
+		printk("AppKey added, NetKeyIndex 0x%04x AppKeyIndex 0x%04x\n",
+		       key_net_idx, key_app_idx);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_app_key_add_help = {
+	NULL, "<NetKeyIndex> <AppKeyIndex> [val]", NULL
+};
+
+static int cmd_mod_app_bind(int argc, char *argv[])
+{
+	u16_t elem_addr, mod_app_idx, mod_id, cid;
+	u8_t status;
+	int err;
+
+	if (argc < 4) {
+		return -EINVAL;
+	}
+
+	elem_addr = strtoul(argv[1], NULL, 0);
+	mod_app_idx = strtoul(argv[2], NULL, 0);
+	mod_id = strtoul(argv[3], NULL, 0);
+
+	if (argc > 4) {
+		cid = strtoul(argv[4], NULL, 0);
+		err = bt_mesh_cfg_mod_app_bind_vnd(net.net_idx, net.dst,
+						   elem_addr, mod_app_idx,
+						   mod_id, cid, &status);
+	} else {
+		err = bt_mesh_cfg_mod_app_bind(net.net_idx, net.dst, elem_addr,
+					       mod_app_idx, mod_id, &status);
+	}
+
+	if (err) {
+		printk("Unable to send Model App Bind (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Model App Bind failed with status 0x%02x\n", status);
+	} else {
+		printk("AppKey successfully bound\n");
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_mod_app_bind_help = {
+	NULL, "<addr> <AppIndex> <Model ID> [Company ID]", NULL
+};
+
+static int cmd_mod_sub_add(int argc, char *argv[])
+{
+	u16_t elem_addr, sub_addr, mod_id, cid;
+	u8_t status;
+	int err;
+
+	if (argc < 4) {
+		return -EINVAL;
+	}
+
+	elem_addr = strtoul(argv[1], NULL, 0);
+	sub_addr = strtoul(argv[2], NULL, 0);
+	mod_id = strtoul(argv[3], NULL, 0);
+
+	if (argc > 4) {
+		cid = strtoul(argv[4], NULL, 0);
+		err = bt_mesh_cfg_mod_sub_add_vnd(net.net_idx, net.dst,
+						  elem_addr, sub_addr, mod_id,
+						  cid, &status);
+	} else {
+		err = bt_mesh_cfg_mod_sub_add(net.net_idx, net.dst, elem_addr,
+					      sub_addr, mod_id, &status);
+	}
+
+	if (err) {
+		printk("Unable to send Model Subscription Add (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Model Subscription Add failed with status 0x%02x\n",
+		       status);
+	} else {
+		printk("Model subscription was successful\n");
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_mod_sub_add_help = {
+	NULL, "<elem addr> <sub addr> <Model ID> [Company ID]", NULL
+};
+
+static int cmd_mod_sub_del(int argc, char *argv[])
+{
+	u16_t elem_addr, sub_addr, mod_id, cid;
+	u8_t status;
+	int err;
+
+	if (argc < 4) {
+		return -EINVAL;
+	}
+
+	elem_addr = strtoul(argv[1], NULL, 0);
+	sub_addr = strtoul(argv[2], NULL, 0);
+	mod_id = strtoul(argv[3], NULL, 0);
+
+	if (argc > 4) {
+		cid = strtoul(argv[4], NULL, 0);
+		err = bt_mesh_cfg_mod_sub_del_vnd(net.net_idx, net.dst,
+						  elem_addr, sub_addr, mod_id,
+						  cid, &status);
+	} else {
+		err = bt_mesh_cfg_mod_sub_del(net.net_idx, net.dst, elem_addr,
+					      sub_addr, mod_id, &status);
+	}
+
+	if (err) {
+		printk("Unable to send Model Subscription Delete (err %d)\n",
+		       err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Model Subscription Delete failed with status 0x%02x\n",
+		       status);
+	} else {
+		printk("Model subscription deltion was successful\n");
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_mod_sub_del_help = {
+	NULL, "<elem addr> <sub addr> <Model ID> [Company ID]", NULL
+};
+
+static int cmd_mod_sub_add_va(int argc, char *argv[])
+{
+	u16_t elem_addr, sub_addr, mod_id, cid;
+	u8_t label[16];
+	u8_t status;
+	size_t len;
+	int err;
+
+	if (argc < 4) {
+		return -EINVAL;
+	}
+
+	elem_addr = strtoul(argv[1], NULL, 0);
+
+	len = hex2bin(argv[2], label, sizeof(label));
+	memset(label + len, 0, sizeof(label) - len);
+
+	mod_id = strtoul(argv[3], NULL, 0);
+
+	if (argc > 4) {
+		cid = strtoul(argv[4], NULL, 0);
+		err = bt_mesh_cfg_mod_sub_va_add_vnd(net.net_idx, net.dst,
+						     elem_addr, label, mod_id,
+						     cid, &sub_addr, &status);
+	} else {
+		err = bt_mesh_cfg_mod_sub_va_add(net.net_idx, net.dst,
+						 elem_addr, label, mod_id,
+						 &sub_addr, &status);
+	}
+
+	if (err) {
+		printk("Unable to send Mod Sub VA Add (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Mod Sub VA Add failed with status 0x%02x\n",
+		       status);
+	} else {
+		printk("0x%04x subscribed to Label UUID %s (va 0x%04x)\n",
+		       elem_addr, argv[2], sub_addr);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_mod_sub_add_va_help = {
+	NULL, "<elem addr> <Label UUID> <Model ID> [Company ID]", NULL
+};
+
+static int cmd_mod_sub_del_va(int argc, char *argv[])
+{
+	u16_t elem_addr, sub_addr, mod_id, cid;
+	u8_t label[16];
+	u8_t status;
+	size_t len;
+	int err;
+
+	if (argc < 4) {
+		return -EINVAL;
+	}
+
+	elem_addr = strtoul(argv[1], NULL, 0);
+
+	len = hex2bin(argv[2], label, sizeof(label));
+	memset(label + len, 0, sizeof(label) - len);
+
+	mod_id = strtoul(argv[3], NULL, 0);
+
+	if (argc > 4) {
+		cid = strtoul(argv[4], NULL, 0);
+		err = bt_mesh_cfg_mod_sub_va_del_vnd(net.net_idx, net.dst,
+						     elem_addr, label, mod_id,
+						     cid, &sub_addr, &status);
+	} else {
+		err = bt_mesh_cfg_mod_sub_va_del(net.net_idx, net.dst,
+						 elem_addr, label, mod_id,
+						 &sub_addr, &status);
+	}
+
+	if (err) {
+		printk("Unable to send Model Subscription Delete (err %d)\n",
+		       err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Model Subscription Delete failed with status 0x%02x\n",
+		       status);
+	} else {
+		printk("0x%04x unsubscribed from Label UUID %s (va 0x%04x)\n",
+		       elem_addr, argv[2], sub_addr);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_mod_sub_del_va_help = {
+	NULL, "<elem addr> <Label UUID> <Model ID> [Company ID]", NULL
+};
+
+static int mod_pub_get(u16_t addr, u16_t mod_id, u16_t cid)
+{
+	struct bt_mesh_cfg_mod_pub pub;
+	u8_t status;
+	int err;
+
+	if (cid == CID_NVAL) {
+		err = bt_mesh_cfg_mod_pub_get(net.net_idx, net.dst, addr,
+					      mod_id, &pub, &status);
+	} else {
+		err = bt_mesh_cfg_mod_pub_get_vnd(net.net_idx, net.dst, addr,
+						  mod_id, cid, &pub, &status);
+	}
+
+	if (err) {
+		printk("Model Publication Get failed (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Model Publication Get failed (status 0x%02x)\n",
+		       status);
+		return 0;
+	}
+
+	printk("Model Publication for Element 0x%04x, Model 0x%04x:\n"
+	       "\tPublish Address:                0x%04x\n"
+	       "\tAppKeyIndex:                    0x%04x\n"
+	       "\tCredential Flag:                %u\n"
+	       "\tPublishTTL:                     %u\n"
+	       "\tPublishPeriod:                  0x%02x\n"
+	       "\tPublishRetransmitCount:         %u\n"
+	       "\tPublishRetransmitInterval:      %ums\n",
+	       addr, mod_id, pub.addr, pub.app_idx, pub.cred_flag, pub.ttl,
+	       pub.period, BT_MESH_PUB_TRANSMIT_COUNT(pub.transmit),
+	       BT_MESH_PUB_TRANSMIT_INT(pub.transmit));
+
+	return 0;
+}
+
+static int mod_pub_set(u16_t addr, u16_t mod_id, u16_t cid, char *argv[])
+{
+	struct bt_mesh_cfg_mod_pub pub;
+	u8_t status, count;
+	u16_t interval;
+	int err;
+
+	pub.addr = strtoul(argv[0], NULL, 0);
+	pub.app_idx = strtoul(argv[1], NULL, 0);
+	pub.cred_flag = str2bool(argv[2]);
+	pub.ttl = strtoul(argv[3], NULL, 0);
+	pub.period = strtoul(argv[4], NULL, 0);
+
+	count = strtoul(argv[5], NULL, 0);
+	if (count > 7) {
+		printk("Invalid retransmit count\n");
+		return -EINVAL;
+	}
+
+	interval = strtoul(argv[6], NULL, 0);
+	if (interval > (31 * 50) || (interval % 50)) {
+		printk("Invalid retransmit interval %u\n", interval);
+		return -EINVAL;
+	}
+
+	pub.transmit = BT_MESH_PUB_TRANSMIT(count, interval);
+
+	if (cid == CID_NVAL) {
+		err = bt_mesh_cfg_mod_pub_set(net.net_idx, net.dst, addr,
+					      mod_id, &pub, &status);
+	} else {
+		err = bt_mesh_cfg_mod_pub_set_vnd(net.net_idx, net.dst, addr,
+						  mod_id, cid, &pub, &status);
+	}
+
+	if (err) {
+		printk("Model Publication Set failed (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Model Publication Set failed (status 0x%02x)\n",
+		       status);
+	} else {
+		printk("Model Publication successfully set\n");
+	}
+
+	return 0;
+}
+
+static int cmd_mod_pub(int argc, char *argv[])
+{
+	u16_t addr, mod_id, cid;
+
+	if (argc < 3) {
+		return -EINVAL;
+	}
+
+	addr = strtoul(argv[1], NULL, 0);
+	mod_id = strtoul(argv[2], NULL, 0);
+
+	argc -= 3;
+	argv += 3;
+
+	if (argc == 1 || argc == 8) {
+		cid = strtoul(argv[0], NULL, 0);
+		argc--;
+		argv++;
+	} else {
+		cid = CID_NVAL;
+	}
+
+	if (argc > 0) {
+		if (argc < 7) {
+			return -EINVAL;
+		}
+
+		return mod_pub_set(addr, mod_id, cid, argv);
+	} else {
+		return mod_pub_get(addr, mod_id, cid);
+	}
+}
+
+struct shell_cmd_help cmd_mod_pub_help = {
+	NULL, "<addr> <mod id> [cid] [<PubAddr> "
+	"<AppKeyIndex> <cred> <ttl> <period> <count> <interval>]" , NULL
+};
+
+static void hb_sub_print(struct bt_mesh_cfg_hb_sub *sub)
+{
+	printk("Heartbeat Subscription:\n"
+	       "\tSource:      0x%04x\n"
+	       "\tDestination: 0x%04x\n"
+	       "\tPeriodLog:   0x%02x\n"
+	       "\tCountLog:    0x%02x\n"
+	       "\tMinHops:     %u\n"
+	       "\tMaxHops:     %u\n",
+	       sub->src, sub->dst, sub->period, sub->count,
+	       sub->min, sub->max);
+}
+
+static int hb_sub_get(int argc, char *argv[])
+{
+	struct bt_mesh_cfg_hb_sub sub;
+	u8_t status;
+	int err;
+
+	err = bt_mesh_cfg_hb_sub_get(net.net_idx, net.dst, &sub, &status);
+	if (err) {
+		printk("Heartbeat Subscription Get failed (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Heartbeat Subscription Get failed (status 0x%02x)\n",
+		       status);
+	} else {
+		hb_sub_print(&sub);
+	}
+
+	return 0;
+}
+
+static int hb_sub_set(int argc, char *argv[])
+{
+	struct bt_mesh_cfg_hb_sub sub;
+	u8_t status;
+	int err;
+
+	sub.src = strtoul(argv[1], NULL, 0);
+	sub.dst = strtoul(argv[2], NULL, 0);
+	sub.period = strtoul(argv[3], NULL, 0);
+
+	err = bt_mesh_cfg_hb_sub_set(net.net_idx, net.dst, &sub, &status);
+	if (err) {
+		printk("Heartbeat Subscription Set failed (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Heartbeat Subscription Set failed (status 0x%02x)\n",
+		       status);
+	} else {
+		hb_sub_print(&sub);
+	}
+
+	return 0;
+}
+
+static int cmd_hb_sub(int argc, char *argv[])
+{
+	if (argc > 1) {
+		if (argc < 4) {
+			return -EINVAL;
+		}
+
+		return hb_sub_set(argc, argv);
+	} else {
+		return hb_sub_get(argc, argv);
+	}
+}
+
+struct shell_cmd_help cmd_hb_sub_help = {
+	NULL, "<src> <dst> <period>", NULL
+};
+
+static int hb_pub_get(int argc, char *argv[])
+{
+	struct bt_mesh_cfg_hb_pub pub;
+	u8_t status;
+	int err;
+
+	err = bt_mesh_cfg_hb_pub_get(net.net_idx, net.dst, &pub, &status);
+	if (err) {
+		printk("Heartbeat Publication Get failed (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Heartbeat Publication Get failed (status 0x%02x)\n",
+		       status);
+		return 0;
+	}
+
+	printk("Heartbeat publication:\n");
+	printk("\tdst 0x%04x count 0x%02x period 0x%02x\n",
+	       pub.dst, pub.count, pub.period);
+	printk("\tttl 0x%02x feat 0x%04x net_idx 0x%04x\n",
+	       pub.ttl, pub.feat, pub.net_idx);
+
+	return 0;
+}
+
+static int hb_pub_set(int argc, char *argv[])
+{
+	struct bt_mesh_cfg_hb_pub pub;
+	u8_t status;
+	int err;
+
+	pub.dst = strtoul(argv[1], NULL, 0);
+	pub.count = strtoul(argv[2], NULL, 0);
+	pub.period = strtoul(argv[3], NULL, 0);
+	pub.ttl = strtoul(argv[4], NULL, 0);
+	pub.feat = strtoul(argv[5], NULL, 0);
+	pub.net_idx = strtoul(argv[5], NULL, 0);
+
+	err = bt_mesh_cfg_hb_pub_set(net.net_idx, net.dst, &pub, &status);
+	if (err) {
+		printk("Heartbeat Publication Set failed (err %d)\n", err);
+		return 0;
+	}
+
+	if (status) {
+		printk("Heartbeat Publication Set failed (status 0x%02x)\n",
+		       status);
+	} else {
+		printk("Heartbeat publication successfully set\n");
+	}
+
+	return 0;
+}
+
+static int cmd_hb_pub(int argc, char *argv[])
+{
+	if (argc > 1) {
+		if (argc < 7) {
+			return -EINVAL;
+		}
+
+		return hb_pub_set(argc, argv);
+	} else {
+		return hb_pub_get(argc, argv);
+	}
+}
+
+struct shell_cmd_help cmd_hb_pub_help = {
+	NULL, "<dst> <count> <period> <ttl> <features> <NetKeyIndex>" , NULL
+};
+
+#endif /* MYNEWT_VAL(BLE_MESH_CFG_CLI) */
+
+#if MYNEWT_VAL(BLE_MESH_PROV)
+static int cmd_pb(bt_mesh_prov_bearer_t bearer, int argc, char *argv[])
+{
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	if (str2bool(argv[1])) {
+		err = bt_mesh_prov_enable(bearer);
+		if (err) {
+			printk("Failed to enable %s (err %d)\n",
+			       bearer2str(bearer), err);
+		} else {
+			printk("%s enabled\n", bearer2str(bearer));
+		}
+	} else {
+		err = bt_mesh_prov_disable(bearer);
+		if (err) {
+			printk("Failed to disable %s (err %d)\n",
+			       bearer2str(bearer), err);
+		} else {
+			printk("%s disabled\n", bearer2str(bearer));
+		}
+	}
+
+	return 0;
+
+}
+
+struct shell_cmd_help cmd_pb_help = {
+	NULL, "<val: off, on>", NULL
+};
+
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+static int cmd_pb_adv(int argc, char *argv[])
+{
+	return cmd_pb(BT_MESH_PROV_ADV, argc, argv);
+}
+
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+static int cmd_provision_adv(int argc, char *argv[])
+{
+	u8_t uuid[16];
+	u8_t attention_duration;
+	u16_t net_idx;
+	u16_t addr;
+	size_t len;
+	int err;
+
+	len = hex2bin(argv[1], uuid, sizeof(uuid));
+	(void)memset(uuid + len, 0, sizeof(uuid) - len);
+
+	net_idx = strtoul(argv[2], NULL, 0);
+	addr = strtoul(argv[3], NULL, 0);
+	attention_duration = strtoul(argv[4], NULL, 0);
+
+	err = bt_mesh_provision_adv(uuid, net_idx, addr, attention_duration);
+	if (err) {
+		printk("Provisioning failed (err %d)", err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_provision_adv_help = {
+	NULL, "<UUID> <NetKeyIndex> <addr> <AttentionDuration>" , NULL
+};
+#endif /* CONFIG_BT_MESH_PROVISIONER */
+
+#endif /* CONFIG_BT_MESH_PB_ADV */
+
+#if MYNEWT_VAL(BLE_MESH_PB_GATT)
+static int cmd_pb_gatt(int argc, char *argv[])
+{
+	return cmd_pb(BT_MESH_PROV_GATT, argc, argv);
+}
+#endif /* CONFIG_BT_MESH_PB_GATT */
+
+static int cmd_provision(int argc, char *argv[])
+{
+	u16_t net_idx, addr;
+	u32_t iv_index;
+	int err;
+
+	if (argc < 3) {
+		return -EINVAL;
+	}
+
+	net_idx = strtoul(argv[1], NULL, 0);
+	addr = strtoul(argv[2], NULL, 0);
+
+	if (argc > 3) {
+		iv_index = strtoul(argv[3], NULL, 0);
+	} else {
+		iv_index = 0;
+	}
+
+	err = bt_mesh_provision(default_key, net_idx, 0, iv_index, addr,
+				default_key);
+	if (err) {
+		printk("Provisioning failed (err %d)\n", err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_provision_help = {
+	NULL, "<NetKeyIndex> <addr> [IVIndex]" , NULL
+};
+
+#if MYNEWT_VAL(BLE_MESH_HEALTH_CLI)
+
+static int cmd_fault_get(int argc, char *argv[])
+{
+	u8_t faults[32];
+	size_t fault_count;
+	u8_t test_id;
+	u16_t cid;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	cid = strtoul(argv[1], NULL, 0);
+	fault_count = sizeof(faults);
+
+	err = bt_mesh_health_fault_get(net.net_idx, net.dst, net.app_idx, cid,
+				       &test_id, faults, &fault_count);
+	if (err) {
+		printk("Failed to send Health Fault Get (err %d)\n", err);
+	} else {
+		show_faults(test_id, cid, faults, fault_count);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_fault_get_help = {
+	NULL, "<Company ID>", NULL
+};
+
+static int cmd_fault_clear(int argc, char *argv[])
+{
+	u8_t faults[32];
+	size_t fault_count;
+	u8_t test_id;
+	u16_t cid;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	cid = strtoul(argv[1], NULL, 0);
+	fault_count = sizeof(faults);
+
+	err = bt_mesh_health_fault_clear(net.net_idx, net.dst, net.app_idx,
+					 cid, &test_id, faults, &fault_count);
+	if (err) {
+		printk("Failed to send Health Fault Clear (err %d)\n", err);
+	} else {
+		show_faults(test_id, cid, faults, fault_count);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_fault_clear_help = {
+	NULL, "<Company ID>", NULL
+};
+
+static int cmd_fault_clear_unack(int argc, char *argv[])
+{
+	u16_t cid;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	cid = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_health_fault_clear(net.net_idx, net.dst, net.app_idx,
+					 cid, NULL, NULL, NULL);
+	if (err) {
+		printk("Health Fault Clear Unacknowledged failed (err %d)\n",
+		       err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_fault_clear_unack_help = {
+	NULL, "<Company ID>", NULL
+};
+
+static int cmd_fault_test(int argc, char *argv[])
+{
+	u8_t faults[32];
+	size_t fault_count;
+	u8_t test_id;
+	u16_t cid;
+	int err;
+
+	if (argc < 3) {
+		return -EINVAL;
+	}
+
+	cid = strtoul(argv[1], NULL, 0);
+	test_id = strtoul(argv[2], NULL, 0);
+	fault_count = sizeof(faults);
+
+	err = bt_mesh_health_fault_test(net.net_idx, net.dst, net.app_idx,
+					cid, test_id, faults, &fault_count);
+	if (err) {
+		printk("Failed to send Health Fault Test (err %d)\n", err);
+	} else {
+		show_faults(test_id, cid, faults, fault_count);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_fault_test_help = {
+	NULL, "<Company ID> <Test ID>", NULL
+};
+
+static int cmd_fault_test_unack(int argc, char *argv[])
+{
+	u16_t cid;
+	u8_t test_id;
+	int err;
+
+	if (argc < 3) {
+		return -EINVAL;
+	}
+
+	cid = strtoul(argv[1], NULL, 0);
+	test_id = strtoul(argv[2], NULL, 0);
+
+	err = bt_mesh_health_fault_test(net.net_idx, net.dst, net.app_idx,
+					cid, test_id, NULL, NULL);
+	if (err) {
+		printk("Health Fault Test Unacknowledged failed (err %d)\n",
+		       err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_fault_test_unack_help = {
+	NULL, "<Company ID> <Test ID>", NULL
+};
+
+static int cmd_period_get(int argc, char *argv[])
+{
+	u8_t divisor;
+	int err;
+
+	err = bt_mesh_health_period_get(net.net_idx, net.dst, net.app_idx,
+					&divisor);
+	if (err) {
+		printk("Failed to send Health Period Get (err %d)\n", err);
+	} else {
+		printk("Health FastPeriodDivisor: %u\n", divisor);
+	}
+
+	return 0;
+}
+
+static int cmd_period_set(int argc, char *argv[])
+{
+	u8_t divisor, updated_divisor;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	divisor = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_health_period_set(net.net_idx, net.dst, net.app_idx,
+					divisor, &updated_divisor);
+	if (err) {
+		printk("Failed to send Health Period Set (err %d)\n", err);
+	} else {
+		printk("Health FastPeriodDivisor: %u\n", updated_divisor);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_period_set_help = {
+	NULL, "<divisor>", NULL
+};
+
+static int cmd_period_set_unack(int argc, char *argv[])
+{
+	u8_t divisor;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	divisor = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_health_period_set(net.net_idx, net.dst, net.app_idx,
+					divisor, NULL);
+	if (err) {
+		printk("Failed to send Health Period Set (err %d)\n", err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_period_set_unack_help = {
+	NULL, "<divisor>", NULL
+};
+
+static int cmd_attention_get(int argc, char *argv[])
+{
+	u8_t attention;
+	int err;
+
+	err = bt_mesh_health_attention_get(net.net_idx, net.dst, net.app_idx,
+					   &attention);
+	if (err) {
+		printk("Failed to send Health Attention Get (err %d)\n", err);
+	} else {
+		printk("Health Attention Timer: %u\n", attention);
+	}
+
+	return 0;
+}
+
+static int cmd_attention_set(int argc, char *argv[])
+{
+	u8_t attention, updated_attention;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	attention = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_health_attention_set(net.net_idx, net.dst, net.app_idx,
+					   attention, &updated_attention);
+	if (err) {
+		printk("Failed to send Health Attention Set (err %d)\n", err);
+	} else {
+		printk("Health Attention Timer: %u\n", updated_attention);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_attention_set_help = {
+	NULL, "<timer>", NULL
+};
+
+static int cmd_attention_set_unack(int argc, char *argv[])
+{
+	u8_t attention;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	attention = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_health_attention_set(net.net_idx, net.dst, net.app_idx,
+					   attention, NULL);
+	if (err) {
+		printk("Failed to send Health Attention Set (err %d)\n", err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_attention_set_unack_help = {
+	NULL, "<timer>", NULL
+};
+
+#endif /* MYNEWT_VAL(BLE_MESH_HEALTH_CLI) */
+
+static int cmd_add_fault(int argc, char *argv[])
+{
+	u8_t fault_id;
+	u8_t i;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	fault_id = strtoul(argv[1], NULL, 0);
+	if (!fault_id) {
+		printk("The Fault ID must be non-zero!\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < sizeof(cur_faults); i++) {
+		if (!cur_faults[i]) {
+			cur_faults[i] = fault_id;
+			break;
+		}
+	}
+
+	if (i == sizeof(cur_faults)) {
+		printk("Fault array is full. Use \"del-fault\" to clear it\n");
+		return 0;
+	}
+
+	for (i = 0; i < sizeof(reg_faults); i++) {
+		if (!reg_faults[i]) {
+			reg_faults[i] = fault_id;
+			break;
+		}
+	}
+
+	if (i == sizeof(reg_faults)) {
+		printk("No space to store more registered faults\n");
+	}
+
+	bt_mesh_fault_update(&elements[0]);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_add_fault_help = {
+	NULL, "<Fault ID>", NULL
+};
+
+static int cmd_del_fault(int argc, char *argv[])
+{
+	u8_t fault_id;
+	u8_t i;
+
+	if (argc < 2) {
+		memset(cur_faults, 0, sizeof(cur_faults));
+		printk("All current faults cleared\n");
+		bt_mesh_fault_update(&elements[0]);
+		return 0;
+	}
+
+	fault_id = strtoul(argv[1], NULL, 0);
+	if (!fault_id) {
+		printk("The Fault ID must be non-zero!\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < sizeof(cur_faults); i++) {
+		if (cur_faults[i] == fault_id) {
+			cur_faults[i] = 0;
+			printk("Fault cleared\n");
+		}
+	}
+
+	bt_mesh_fault_update(&elements[0]);
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_del_fault_help = {
+	NULL, "[Fault ID]", NULL
+};
+
+#if MYNEWT_VAL(BLE_MESH_SHELL_MODELS)
+static int cmd_gen_onoff_get(int argc, char *argv[])
+{
+	u8_t state;
+	int err;
+
+	err = bt_mesh_gen_onoff_get(net.net_idx, net.dst, net.app_idx,
+				    &state);
+	if (err) {
+		printk("Failed to send Generic OnOff Get (err %d)\n", err);
+	} else {
+		printk("Gen OnOff State %d\n", state);
+	}
+
+	return 0;
+}
+
+static int cmd_gen_onoff_set(int argc, char *argv[])
+{
+	u8_t state;
+	u8_t val;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	val = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_gen_onoff_set(net.net_idx, net.dst, net.app_idx,
+				    val, &state);
+	if (err) {
+		printk("Failed to send Generic OnOff Get (err %d)\n", err);
+	} else {
+		printk("Gen OnOff State %d\n", state);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_gen_onoff_set_help = {
+	NULL, "<0|1>", NULL
+};
+
+static int cmd_gen_onoff_set_unack(int argc, char *argv[])
+{
+	u8_t val;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	val = strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_gen_onoff_set(net.net_idx, net.dst, net.app_idx,
+				    val, NULL);
+	if (err) {
+		printk("Failed to send Generic OnOff Get (err %d)\n", err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_gen_onoff_set_unack_help = {
+	NULL, "<0|1>", NULL
+};
+
+static int cmd_gen_level_get(int argc, char *argv[])
+{
+	s16_t state;
+	int err;
+
+	err = bt_mesh_gen_level_get(net.net_idx, net.dst, net.app_idx,
+				    &state);
+	if (err) {
+		printk("Failed to send Generic Level Get (err %d)\n", err);
+	} else {
+		printk("Gen Level State %d\n", state);
+	}
+
+	return 0;
+}
+
+static int cmd_gen_level_set(int argc, char *argv[])
+{
+	s16_t state;
+	s16_t val;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	val = (s16_t)strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_gen_level_set(net.net_idx, net.dst, net.app_idx,
+				    val, &state);
+	if (err) {
+		printk("Failed to send Generic Level Get (err %d)\n", err);
+	} else {
+		printk("Gen Level State %d\n", state);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_gen_level_set_help = {
+	NULL, "<level>", NULL
+};
+
+static int cmd_gen_level_set_unack(int argc, char *argv[])
+{
+	s16_t val;
+	int err;
+
+	if (argc < 2) {
+		return -EINVAL;
+	}
+
+	val = (s16_t)strtoul(argv[1], NULL, 0);
+
+	err = bt_mesh_gen_level_set(net.net_idx, net.dst, net.app_idx,
+				    val, NULL);
+	if (err) {
+		printk("Failed to send Generic Level Get (err %d)\n", err);
+	}
+
+	return 0;
+}
+
+struct shell_cmd_help cmd_gen_level_set_unack_help = {
+	NULL, "<level>", NULL
+};
+
+#endif /* MYNEWT_VAL(BLE_MESH_SHELL_MODELS) */
+
+static int cmd_print_credentials(int argc, char *argv[])
+{
+	bt_test_print_credentials();
+	return 0;
+}
+
+static void print_comp_elem(struct bt_mesh_elem *elem,
+			    bool primary)
+{
+	struct bt_mesh_model *mod;
+	int i;
+
+	printk("Loc: %u\n", elem->loc);
+	printk("Model count: %u\n", elem->model_count);
+	printk("Vnd model count: %u\n", elem->vnd_model_count);
+
+	for (i = 0; i < elem->model_count; i++) {
+		mod = &elem->models[i];
+		printk("  Model: %u\n", i);
+		printk("    ID: 0x%04x\n", mod->id);
+		printk("    Opcode: 0x%08lx\n", mod->op->opcode);
+	}
+
+	for (i = 0; i < elem->vnd_model_count; i++) {
+		mod = &elem->vnd_models[i];
+		printk("  Vendor model: %u\n", i);
+		printk("    Company: 0x%04x\n", mod->vnd.company);
+		printk("    ID: 0x%04x\n", mod->vnd.id);
+		printk("    Opcode: 0x%08lx\n", mod->op->opcode);
+	}
+}
+
+static int cmd_print_composition_data(int argc, char *argv[])
+{
+	const struct bt_mesh_comp *comp;
+	int i;
+
+	comp = bt_mesh_comp_get();
+
+	printk("CID: %u\n", comp->cid);
+	printk("PID: %u\n", comp->pid);
+	printk("VID: %u\n", comp->vid);
+
+	for (i = 0; i < comp->elem_count; i++) {
+		print_comp_elem(&comp->elem[i], i == 0);
+	}
+
+	return 0;
+}
+
+
+static const struct shell_cmd mesh_commands[] = {
+    {
+        .sc_cmd = "init",
+        .sc_cmd_func = cmd_mesh_init,
+        .help = NULL,
+    },
+#if MYNEWT_VAL(BLE_MESH_PB_ADV)
+    {
+        .sc_cmd = "pb-adv",
+        .sc_cmd_func = cmd_pb_adv,
+        .help = &cmd_pb_help,
+    },
+#if MYNEWT_VAL(BLE_MESH_PROVISIONER)
+    {
+        .sc_cmd = "provision-adv",
+        .sc_cmd_func = cmd_provision_adv,
+        .help = &cmd_provision_adv_help,
+    },
+#endif
+#endif
+#if MYNEWT_VAL(BLE_MESH_PB_GATT)
+    {
+        .sc_cmd = "pb-gatt",
+        .sc_cmd_func = cmd_pb_gatt,
+        .help = &cmd_pb_help,
+    },
+#endif
+    {
+        .sc_cmd = "reset",
+        .sc_cmd_func = cmd_reset,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "uuid",
+        .sc_cmd_func = cmd_uuid,
+        .help = &cmd_uuid_help,
+    },
+    {
+        .sc_cmd = "input-num",
+        .sc_cmd_func = cmd_input_num,
+        .help = &cmd_input_num_help,
+    },
+    {
+        .sc_cmd = "input-str",
+        .sc_cmd_func = cmd_input_str,
+        .help = &cmd_input_str_help,
+    },
+    {
+        .sc_cmd = "static-oob",
+        .sc_cmd_func = cmd_static_oob,
+        .help = &cmd_static_oob_help,
+    },
+    {
+        .sc_cmd = "provision",
+        .sc_cmd_func = cmd_provision,
+        .help = &cmd_provision_help,
+    },
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER)
+    {
+        .sc_cmd = "lpn",
+        .sc_cmd_func = cmd_lpn,
+        .help = &cmd_lpn_help,
+    },
+    {
+        .sc_cmd = "poll",
+        .sc_cmd_func = cmd_poll,
+        .help = NULL,
+    },
+#endif
+#if MYNEWT_VAL(BLE_MESH_GATT_PROXY)
+    {
+        .sc_cmd = "ident",
+        .sc_cmd_func = cmd_ident,
+        .help = NULL,
+    },
+#endif
+    {
+        .sc_cmd = "dst",
+        .sc_cmd_func = cmd_dst,
+        .help = &cmd_dst_help,
+    },
+    {
+        .sc_cmd = "netidx",
+        .sc_cmd_func = cmd_netidx,
+        .help = &cmd_netidx_help,
+    },
+    {
+        .sc_cmd = "appidx",
+        .sc_cmd_func = cmd_appidx,
+        .help = &cmd_appidx_help,
+    },
+
+    /* Commands which access internal APIs, for testing only */
+    {
+        .sc_cmd = "net-send",
+        .sc_cmd_func = cmd_net_send,
+        .help = &cmd_net_send_help,
+    },
+#if MYNEWT_VAL(BLE_MESH_IV_UPDATE_TEST)
+    {
+        .sc_cmd = "iv-update",
+        .sc_cmd_func = cmd_iv_update,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "iv-update-test",
+        .sc_cmd_func = cmd_iv_update_test,
+        .help = &cmd_iv_update_test_help,
+    },
+#endif
+    {
+        .sc_cmd = "rpl-clear",
+        .sc_cmd_func = cmd_rpl_clear,
+        .help = NULL,
+    },
+#if MYNEWT_VAL(BLE_MESH_LOW_POWER)
+    {
+        .sc_cmd = "lpn-subscribe",
+        .sc_cmd_func = cmd_lpn_subscribe,
+        .help = &cmd_lpn_subscribe_help,
+    },
+    {
+        .sc_cmd = "lpn-unsubscribe",
+        .sc_cmd_func = cmd_lpn_unsubscribe,
+        .help = &cmd_lpn_unsubscribe_help,
+    },
+#endif
+    {
+        .sc_cmd = "print-credentials",
+        .sc_cmd_func = cmd_print_credentials,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "print-composition-data",
+        .sc_cmd_func = cmd_print_composition_data,
+        .help = NULL,
+    },
+
+
+#if MYNEWT_VAL(BLE_MESH_CFG_CLI)
+    /* Configuration Client Model operations */
+    {
+        .sc_cmd = "timeout",
+        .sc_cmd_func = cmd_timeout,
+        .help = &cmd_timeout_help,
+    },
+    {
+        .sc_cmd = "get-comp",
+        .sc_cmd_func = cmd_get_comp,
+        .help = &cmd_get_comp_help,
+    },
+    {
+        .sc_cmd = "beacon",
+        .sc_cmd_func = cmd_beacon,
+        .help = &cmd_beacon_help,
+    },
+    {
+        .sc_cmd = "ttl",
+        .sc_cmd_func = cmd_ttl,
+        .help = &cmd_ttl_help,
+    },
+    {
+        .sc_cmd = "friend",
+        .sc_cmd_func = cmd_friend,
+        .help = &cmd_friend_help,
+    },
+    {
+        .sc_cmd = "gatt-proxy",
+        .sc_cmd_func = cmd_gatt_proxy,
+        .help = &cmd_gatt_proxy_help,
+    },
+    {
+        .sc_cmd = "relay",
+        .sc_cmd_func = cmd_relay,
+        .help = &cmd_relay_help,
+    },
+    {
+        .sc_cmd = "net-key-add",
+        .sc_cmd_func = cmd_net_key_add,
+        .help = &cmd_net_key_add_help,
+    },
+    {
+        .sc_cmd = "app-key-add",
+        .sc_cmd_func = cmd_app_key_add,
+        .help = &cmd_app_key_add_help,
+    },
+    {
+        .sc_cmd = "mod-app-bind",
+        .sc_cmd_func = cmd_mod_app_bind,
+        .help = &cmd_mod_app_bind_help,
+    },
+    {
+        .sc_cmd = "mod-pub",
+        .sc_cmd_func = cmd_mod_pub,
+        .help = &cmd_mod_pub_help,
+    },
+    {
+        .sc_cmd = "mod-sub-add",
+        .sc_cmd_func = cmd_mod_sub_add,
+        .help = &cmd_mod_sub_add_help,
+    },
+    {
+        .sc_cmd = "mod-sub-del",
+        .sc_cmd_func = cmd_mod_sub_del,
+        .help = &cmd_mod_sub_del_help,
+    },
+    {
+        .sc_cmd = "mod-sub-add-va",
+        .sc_cmd_func = cmd_mod_sub_add_va,
+        .help = &cmd_mod_sub_add_va_help,
+    },
+    {
+        .sc_cmd = "mod-sub-del-va",
+        .sc_cmd_func = cmd_mod_sub_del_va,
+        .help = &cmd_mod_sub_del_va_help,
+    },
+    {
+        .sc_cmd = "hb-sub",
+        .sc_cmd_func = cmd_hb_sub,
+        .help = &cmd_hb_sub_help,
+    },
+    {
+        .sc_cmd = "hb-pub",
+        .sc_cmd_func = cmd_hb_pub,
+        .help = &cmd_hb_pub_help,
+    },
+#endif
+
+#if MYNEWT_VAL(BLE_MESH_HEALTH_CLI)
+    /* Health Client Model Operations */
+    {
+        .sc_cmd = "fault-get",
+        .sc_cmd_func = cmd_fault_get,
+        .help = &cmd_fault_get_help,
+    },
+    {
+        .sc_cmd = "fault-clear",
+        .sc_cmd_func = cmd_fault_clear,
+        .help = &cmd_fault_clear_help,
+    },
+    {
+        .sc_cmd = "fault-clear-unack",
+        .sc_cmd_func = cmd_fault_clear_unack,
+        .help = &cmd_fault_clear_unack_help,
+    },
+    {
+        .sc_cmd = "fault-test",
+        .sc_cmd_func = cmd_fault_test,
+        .help = &cmd_fault_test_help,
+    },
+    {
+        .sc_cmd = "fault-test-unack",
+        .sc_cmd_func = cmd_fault_test_unack,
+        .help = &cmd_fault_test_unack_help,
+    },
+    {
+        .sc_cmd = "period-get",
+        .sc_cmd_func = cmd_period_get,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "period-set",
+        .sc_cmd_func = cmd_period_set,
+        .help = &cmd_period_set_help,
+    },
+    {
+        .sc_cmd = "period-set-unack",
+        .sc_cmd_func = cmd_period_set_unack,
+        .help = &cmd_period_set_unack_help,
+    },
+    {
+        .sc_cmd = "attention-get",
+        .sc_cmd_func = cmd_attention_get,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "attention-set",
+        .sc_cmd_func = cmd_attention_set,
+        .help = &cmd_attention_set_help,
+    },
+    {
+        .sc_cmd = "attention-set-unack",
+        .sc_cmd_func = cmd_attention_set_unack,
+        .help = &cmd_attention_set_unack_help,
+    },
+#endif
+
+    /* Health Server Model Operations */
+    {
+        .sc_cmd = "add-fault",
+        .sc_cmd_func = cmd_add_fault,
+        .help = &cmd_add_fault_help,
+    },
+    {
+        .sc_cmd = "del-fault",
+        .sc_cmd_func = cmd_del_fault,
+        .help = &cmd_del_fault_help,
+    },
+
+#if MYNEWT_VAL(BLE_MESH_SHELL_MODELS)
+    /* Generic Client Model Operations */
+    {
+        .sc_cmd = "gen-onoff-get",
+        .sc_cmd_func = cmd_gen_onoff_get,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "gen-onoff-set",
+        .sc_cmd_func = cmd_gen_onoff_set,
+        .help = &cmd_gen_onoff_set_help,
+    },
+    {
+        .sc_cmd = "gen-onoff-set-unack",
+        .sc_cmd_func = cmd_gen_onoff_set_unack,
+        .help = &cmd_gen_onoff_set_unack_help,
+    },
+    {
+        .sc_cmd = "gen-level-get",
+        .sc_cmd_func = cmd_gen_level_get,
+        .help = NULL,
+    },
+    {
+        .sc_cmd = "gen-level-set",
+        .sc_cmd_func = cmd_gen_level_set,
+        .help = &cmd_gen_level_set_help,
+    },
+    {
+        .sc_cmd = "gen-level-set-unack",
+        .sc_cmd_func = cmd_gen_level_set_unack,
+        .help = &cmd_gen_level_set_unack_help,
+    },
+#endif
+
+    { 0 },
+};
+
+static void mesh_shell_thread(void *args)
+{
+	while (1) {
+		os_eventq_run(&mesh_shell_queue);
+	}
+}
+
+static void bt_mesh_shell_task_init(void)
+{
+	os_eventq_init(&mesh_shell_queue);
+
+	os_task_init(&mesh_shell_task, "mesh_sh", mesh_shell_thread, NULL,
+		     BLE_MESH_SHELL_TASK_PRIO, OS_WAIT_FOREVER, g_blemesh_shell_stack,
+		     BLE_MESH_SHELL_STACK_SIZE);
+}
+#endif
+
+void ble_mesh_shell_init(void)
+{
+#if (MYNEWT_VAL(BLE_MESH_SHELL))
+
+	/* Initialize health pub message */
+	health_pub_init();
+
+	/* Shell and other mesh clients should use separate task to
+	   avoid deadlocks with mesh message processing queue */
+	bt_mesh_shell_task_init();
+	shell_evq_set(&mesh_shell_queue);
+	shell_register("mesh", mesh_commands);
+
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/shell.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/shell.h
new file mode 100644
index 00000000..53cc83a2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/shell.h
@@ -0,0 +1,6 @@
+#ifndef __SHELL_H__
+#define __SHELL_H__
+
+void ble_mesh_shell_init(void);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/testing.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/testing.c
new file mode 100644
index 00000000..d0a05376
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/testing.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stddef.h>
+
+#include "console/console.h"
+#include "mesh/testing.h"
+#include "mesh/slist.h"
+#include "mesh/glue.h"
+#include "mesh/access.h"
+
+#include "net.h"
+#include "testing.h"
+#include "access.h"
+#include "foundation.h"
+#include "lpn.h"
+#include "transport.h"
+
+static sys_slist_t cb_slist;
+
+void bt_test_cb_register(struct bt_test_cb *cb)
+{
+	sys_slist_append(&cb_slist, &cb->node);
+}
+
+void bt_test_cb_unregister(struct bt_test_cb *cb)
+{
+	sys_slist_find_and_remove(&cb_slist, &cb->node);
+}
+
+void bt_test_mesh_net_recv(u8_t ttl, u8_t ctl, u16_t src, u16_t dst,
+			   const void *payload, size_t payload_len)
+{
+	struct bt_test_cb *cb;
+
+	SYS_SLIST_FOR_EACH_CONTAINER(&cb_slist, cb, node) {
+		if (cb->mesh_net_recv) {
+			cb->mesh_net_recv(ttl, ctl, src, dst, payload,
+					  payload_len);
+		}
+	}
+}
+
+void bt_test_mesh_model_bound(u16_t addr, struct bt_mesh_model *model,
+			      u16_t key_idx)
+{
+	struct bt_test_cb *cb;
+
+	SYS_SLIST_FOR_EACH_CONTAINER(&cb_slist, cb, node) {
+		if (cb->mesh_model_bound) {
+			cb->mesh_model_bound(addr, model, key_idx);
+		}
+	}
+}
+
+void bt_test_mesh_model_unbound(u16_t addr, struct bt_mesh_model *model,
+				u16_t key_idx)
+{
+	struct bt_test_cb *cb;
+
+	SYS_SLIST_FOR_EACH_CONTAINER(&cb_slist, cb, node) {
+		if (cb->mesh_model_unbound) {
+			cb->mesh_model_unbound(addr, model, key_idx);
+		}
+	}
+}
+
+void bt_test_mesh_prov_invalid_bearer(u8_t opcode)
+{
+	struct bt_test_cb *cb;
+
+	SYS_SLIST_FOR_EACH_CONTAINER(&cb_slist, cb, node) {
+		if (cb->mesh_prov_invalid_bearer) {
+			cb->mesh_prov_invalid_bearer(opcode);
+		}
+	}
+}
+
+void bt_test_mesh_trans_incomp_timer_exp(void)
+{
+	struct bt_test_cb *cb;
+
+	SYS_SLIST_FOR_EACH_CONTAINER(&cb_slist, cb, node) {
+		if (cb->mesh_trans_incomp_timer_exp) {
+			cb->mesh_trans_incomp_timer_exp();
+		}
+	}
+}
+
+int bt_test_mesh_lpn_group_add(u16_t group)
+{
+	bt_mesh_lpn_group_add(group);
+
+	return 0;
+}
+
+int bt_test_mesh_lpn_group_remove(u16_t *groups, size_t groups_count)
+{
+	bt_mesh_lpn_group_del(groups, groups_count);
+
+	return 0;
+}
+
+int bt_test_mesh_rpl_clear(void)
+{
+	bt_mesh_rpl_clear();
+
+	return 0;
+}
+
+void bt_test_print_credentials(void)
+{
+	int i;
+	u8_t nid;
+	const u8_t *enc;
+	const u8_t *priv;
+	struct bt_mesh_subnet *sub;
+	struct bt_mesh_app_key *app_key;
+
+	console_printf("IV Index: %08lx\n", (long) bt_mesh.iv_index);
+	console_printf("Dev key: %s\n", bt_hex(bt_mesh.dev_key, 16));
+
+	for (i = 0; i < MYNEWT_VAL(BLE_MESH_SUBNET_COUNT); ++i)
+	{
+		if (bt_mesh.app_keys[i].net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		sub = &bt_mesh.sub[i];
+
+		console_printf("Subnet: %d\n", i);
+		console_printf("\tNetKeyIdx: %04x\n",
+			       sub->net_idx);
+		console_printf("\tNetKey: %s\n",
+			       bt_hex(sub->keys[sub->kr_flag].net, 16));
+	}
+
+	for (i = 0; i < MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT); ++i)
+	{
+		if (bt_mesh.app_keys[i].net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		sub = &bt_mesh.sub[i];
+		app_key = &bt_mesh.app_keys[i];
+
+		console_printf("AppKey: %d\n", i);
+		console_printf("\tNetKeyIdx: %04x\n",
+			       app_key->net_idx);
+		console_printf("\tAppKeyIdx: %04x\n",
+			       app_key->app_idx);
+		console_printf("\tAppKey: %s\n",
+			       bt_hex(app_key->keys[sub->kr_flag].val, 16));
+	}
+
+	for (i = 0; i < MYNEWT_VAL(BLE_MESH_SUBNET_COUNT); ++i)
+	{
+		if (bt_mesh.sub[i].net_idx == BT_MESH_KEY_UNUSED) {
+			continue;
+		}
+
+		if (friend_cred_get(&bt_mesh.sub[i], BT_MESH_ADDR_UNASSIGNED,
+				&nid, &enc, &priv)) {
+			return;
+		}
+
+		console_printf("Friend cred: %d\n", i);
+		console_printf("\tNetKeyIdx: %04x\n",
+			       bt_mesh.sub[i].net_idx);
+		console_printf("\tNID: %02x\n", nid);
+		console_printf("\tEncKey: %s\n",
+			       bt_hex(enc, 16));
+		console_printf("\tPrivKey: %s\n",
+			       bt_hex(priv, 16));
+	}
+}
+
+int bt_test_shell_init(void)
+{
+#if MYNEWT_VAL(BLE_MESH_SHELL)
+	return cmd_mesh_init(0, NULL);
+#else
+	return -ENOTSUP;
+#endif
+}
+
+int bt_test_bind_app_key_to_model(struct bt_mesh_model *model, u16_t key_idx, u16_t id)
+{
+	struct bt_mesh_model *found_model;
+
+	found_model = bt_mesh_model_find(bt_mesh_model_elem(model), id);
+	if (!found_model) {
+		return STATUS_INVALID_MODEL;
+	}
+
+	return mod_bind(found_model, key_idx);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/testing.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/testing.h
new file mode 100644
index 00000000..166a9eea
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/testing.h
@@ -0,0 +1,23 @@
+/**
+ * @file testing.h
+ * @brief Internal API for Bluetooth testing.
+ */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mesh/glue.h"
+#include "mesh/access.h"
+
+void bt_test_mesh_model_bound(u16_t addr, struct bt_mesh_model *model,
+			      u16_t key_idx);
+void bt_test_mesh_model_unbound(u16_t addr, struct bt_mesh_model *model,
+				u16_t key_idx);
+void bt_test_mesh_prov_invalid_bearer(u8_t opcode);
+void bt_test_mesh_net_recv(u8_t ttl, u8_t ctl, u16_t src, u16_t dst,
+			   const void *payload, size_t payload_len);
+void bt_test_mesh_trans_incomp_timer_exp(void);
+void bt_test_print_credentials(void);
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/transport.c b/src/libs/mynewt-nimble/nimble/host/mesh/src/transport.c
new file mode 100644
index 00000000..caf1b4f1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/transport.c
@@ -0,0 +1,1668 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#define MESH_LOG_MODULE BLE_MESH_TRANS_LOG
+
+#include <errno.h>
+#include <string.h>
+
+#include "mesh/mesh.h"
+#include "mesh_priv.h"
+
+#include "crypto.h"
+#include "adv.h"
+#include "net.h"
+#include "lpn.h"
+#include "friend.h"
+#include "access.h"
+#include "foundation.h"
+#include "settings.h"
+#include "transport.h"
+#include "testing.h"
+#include "nodes.h"
+
+/* The transport layer needs at least three buffers for itself to avoid
+ * deadlocks. Ensure that there are a sufficient number of advertising
+ * buffers available compared to the maximum supported outgoing segment
+ * count.
+ */
+BUILD_ASSERT(CONFIG_BT_MESH_ADV_BUF_COUNT >= (CONFIG_BT_MESH_TX_SEG_MAX + 3));
+
+#define AID_MASK                    ((u8_t)(BIT_MASK(6)))
+
+#define SEG(data)                   ((data)[0] >> 7)
+#define AKF(data)                   (((data)[0] >> 6) & 0x01)
+#define AID(data)                   ((data)[0] & AID_MASK)
+#define ASZMIC(data)                (((data)[1] >> 7) & 1)
+
+#define APP_MIC_LEN(aszmic)         ((aszmic) ? 8 : 4)
+
+#define UNSEG_HDR(akf, aid)         ((akf << 6) | (aid & AID_MASK))
+#define SEG_HDR(akf, aid)           (UNSEG_HDR(akf, aid) | 0x80)
+
+#define BLOCK_COMPLETE(seg_n)       (u32_t)(((u64_t)1 << (seg_n + 1)) - 1)
+
+#define SEQ_AUTH(iv_index, seq)     (((u64_t)iv_index) << 24 | (u64_t)seq)
+
+/* Number of retransmit attempts (after the initial transmit) per segment */
+#define SEG_RETRANSMIT_ATTEMPTS     (MYNEWT_VAL(BLE_MESH_SEG_RETRANSMIT_ATTEMPTS))
+
+/* "This timer shall be set to a minimum of 200 + 50 * TTL milliseconds.".
+ * We use 400 since 300 is a common send duration for standard HCI, and we
+ * need to have a timeout that's bigger than that.
+ */
+#define SEG_RETRANSMIT_TIMEOUT(tx) (K_MSEC(400) + 50 * (tx)->ttl)
+
+/* How long to wait for available buffers before giving up */
+#define BUF_TIMEOUT                 K_NO_WAIT
+
+static struct seg_tx {
+	struct bt_mesh_subnet   *sub;
+	struct os_mbuf          *seg[CONFIG_BT_MESH_TX_SEG_MAX];
+	u64_t                    seq_auth;
+	u16_t                    dst;
+	u8_t                     seg_n:5,       /* Last segment index */
+				 new_key:1;     /* New/old key */
+	u8_t                     nack_count;    /* Number of unacked segs */
+	u8_t                     ttl;
+	const struct bt_mesh_send_cb *cb;
+	void                    *cb_data;
+	struct k_delayed_work    retransmit; /* Retransmit timer */
+} seg_tx[MYNEWT_VAL(BLE_MESH_TX_SEG_MSG_COUNT)];
+
+static struct seg_rx {
+	struct bt_mesh_subnet   *sub;
+	u64_t                    seq_auth;
+	u8_t                     seg_n:5,
+				 ctl:1,
+				 in_use:1,
+				 obo:1;
+	u8_t                     hdr;
+	u8_t                     ttl;
+	u16_t                    src;
+	u16_t                    dst;
+	u32_t                    block;
+	u32_t                    last;
+	struct k_delayed_work    ack;
+	struct os_mbuf    *buf;
+} seg_rx[MYNEWT_VAL(BLE_MESH_RX_SEG_MSG_COUNT)] = {
+	[0 ... (MYNEWT_VAL(BLE_MESH_RX_SEG_MSG_COUNT) - 1)] = { 0 },
+};
+
+static u16_t hb_sub_dst = BT_MESH_ADDR_UNASSIGNED;
+
+void bt_mesh_set_hb_sub_dst(u16_t addr)
+{
+	hb_sub_dst = addr;
+}
+
+static int send_unseg(struct bt_mesh_net_tx *tx, struct os_mbuf *sdu,
+		      const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	struct os_mbuf *buf;
+
+	BT_DBG("src 0x%04x dst 0x%04x app_idx 0x%04x sdu_len %u",
+	       tx->src, tx->ctx->addr, tx->ctx->app_idx, sdu->om_len);
+
+	buf = bt_mesh_adv_create(BT_MESH_ADV_DATA, tx->xmit, BUF_TIMEOUT);
+	if (!buf) {
+		BT_ERR("Out of network buffers");
+		return -ENOBUFS;
+	}
+
+	net_buf_reserve(buf, BT_MESH_NET_HDR_LEN);
+
+	if (BT_MESH_IS_DEV_KEY(tx->ctx->app_idx)) {
+		net_buf_add_u8(buf, UNSEG_HDR(0, 0));
+	} else {
+		net_buf_add_u8(buf, UNSEG_HDR(1, tx->aid));
+	}
+
+	net_buf_add_mem(buf, sdu->om_data, sdu->om_len);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		if (!bt_mesh_friend_queue_has_space(tx->sub->net_idx,
+						    tx->src, tx->ctx->addr,
+						    NULL, 1)) {
+			if (BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) {
+				BT_ERR("Not enough space in Friend Queue");
+				net_buf_unref(buf);
+				return -ENOBUFS;
+			} else {
+				BT_WARN("No space in Friend Queue");
+				goto send;
+			}
+		}
+
+		if (bt_mesh_friend_enqueue_tx(tx, BT_MESH_FRIEND_PDU_SINGLE,
+					      NULL, 1, buf) &&
+		    BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) {
+			/* PDUs for a specific Friend should only go
+			 * out through the Friend Queue.
+			 */
+			net_buf_unref(buf);
+			send_cb_finalize(cb, cb_data);
+			return 0;
+		}
+	}
+
+send:
+	return bt_mesh_net_send(tx, buf, cb, cb_data);
+}
+
+bool bt_mesh_tx_in_progress(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(seg_tx); i++) {
+		if (seg_tx[i].nack_count) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void seg_tx_reset(struct seg_tx *tx)
+{
+	int i;
+
+	k_delayed_work_cancel(&tx->retransmit);
+
+	tx->cb = NULL;
+	tx->cb_data = NULL;
+	tx->seq_auth = 0;
+	tx->sub = NULL;
+	tx->dst = BT_MESH_ADDR_UNASSIGNED;
+
+	if (!tx->nack_count) {
+		return;
+	}
+
+	for (i = 0; i <= tx->seg_n; i++) {
+		if (!tx->seg[i]) {
+			continue;
+		}
+
+		net_buf_unref(tx->seg[i]);
+		tx->seg[i] = NULL;
+	}
+
+	tx->nack_count = 0U;
+
+	if (atomic_test_and_clear_bit(bt_mesh.flags, BT_MESH_IVU_PENDING)) {
+		BT_DBG("Proceding with pending IV Update");
+
+		/* bt_mesh_net_iv_update() will re-enable the flag if this
+		 * wasn't the only transfer.
+		 */
+		if (bt_mesh_net_iv_update(bt_mesh.iv_index, false)) {
+			bt_mesh_net_sec_update(NULL);
+		}
+	}
+}
+
+static inline void seg_tx_complete(struct seg_tx *tx, int err)
+{
+	if (tx->cb && tx->cb->end) {
+		tx->cb->end(err, tx->cb_data);
+	}
+
+	seg_tx_reset(tx);
+}
+
+static void seg_first_send_start(u16_t duration, int err, void *user_data)
+{
+	struct seg_tx *tx = user_data;
+
+	if (tx->cb && tx->cb->start) {
+		tx->cb->start(duration, err, tx->cb_data);
+	}
+}
+
+static void seg_send_start(u16_t duration, int err, void *user_data)
+{
+	struct seg_tx *tx = user_data;
+
+	/* If there's an error in transmitting the 'sent' callback will never
+	 * be called. Make sure that we kick the retransmit timer also in this
+	 * case since otherwise we risk the transmission of becoming stale.
+	 */
+	if (err) {
+		k_delayed_work_submit(&tx->retransmit,
+				      SEG_RETRANSMIT_TIMEOUT(tx));
+	}
+}
+
+static void seg_sent(int err, void *user_data)
+{
+	struct seg_tx *tx = user_data;
+
+	k_delayed_work_submit(&tx->retransmit,
+			      SEG_RETRANSMIT_TIMEOUT(tx));
+}
+
+static const struct bt_mesh_send_cb first_sent_cb = {
+	.start = seg_first_send_start,
+	.end = seg_sent,
+};
+
+static const struct bt_mesh_send_cb seg_sent_cb = {
+	.start = seg_send_start,
+	.end = seg_sent,
+};
+
+static void seg_tx_send_unacked(struct seg_tx *tx)
+{
+	int i, err;
+
+	for (i = 0; i <= tx->seg_n; i++) {
+		struct os_mbuf *seg = tx->seg[i];
+
+		if (!seg) {
+			continue;
+		}
+
+		if (BT_MESH_ADV(seg)->busy) {
+			BT_DBG("Skipping segment that's still advertising");
+			continue;
+		}
+
+		if (!(BT_MESH_ADV(seg)->seg.attempts--)) {
+			BT_ERR("Ran out of retransmit attempts");
+			seg_tx_complete(tx, -ETIMEDOUT);
+			return;
+		}
+
+		BT_DBG("resending %u/%u", i, tx->seg_n);
+
+		err = bt_mesh_net_resend(tx->sub, seg, tx->new_key,
+					 &seg_sent_cb, tx);
+		if (err) {
+			BT_ERR("Sending segment failed");
+			seg_tx_complete(tx, -EIO);
+			return;
+		}
+	}
+}
+
+static void seg_retransmit(struct ble_npl_event *work)
+{
+	struct seg_tx *tx = ble_npl_event_get_arg(work);
+	seg_tx_send_unacked(tx);
+}
+
+static int send_seg(struct bt_mesh_net_tx *net_tx, struct os_mbuf *sdu,
+		    const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	u8_t seg_hdr, seg_o;
+	u16_t seq_zero;
+	struct seg_tx *tx;
+	int i;
+
+	BT_DBG("src 0x%04x dst 0x%04x app_idx 0x%04x aszmic %u sdu_len %u",
+	       net_tx->src, net_tx->ctx->addr, net_tx->ctx->app_idx,
+	       net_tx->aszmic, sdu->om_len);
+
+	if (sdu->om_len < 1) {
+		BT_ERR("Zero-length SDU not allowed");
+		return -EINVAL;
+	}
+
+	if (sdu->om_len > BT_MESH_TX_SDU_MAX) {
+		BT_ERR("Not enough segment buffers for length %u", sdu->om_len);
+		return -EMSGSIZE;
+	}
+
+	for (tx = NULL, i = 0; i < ARRAY_SIZE(seg_tx); i++) {
+		if (!seg_tx[i].nack_count) {
+			tx = &seg_tx[i];
+			break;
+		}
+	}
+
+	if (!tx) {
+		BT_ERR("No multi-segment message contexts available");
+		return -EBUSY;
+	}
+
+	if (BT_MESH_IS_DEV_KEY(net_tx->ctx->app_idx)) {
+		seg_hdr = SEG_HDR(0, 0);
+	} else {
+		seg_hdr = SEG_HDR(1, net_tx->aid);
+	}
+
+	seg_o = 0;
+	tx->dst = net_tx->ctx->addr;
+	tx->seg_n = (sdu->om_len - 1) / 12;
+	tx->nack_count = tx->seg_n + 1;
+	tx->seq_auth = SEQ_AUTH(BT_MESH_NET_IVI_TX, bt_mesh.seq);
+	tx->sub = net_tx->sub;
+	tx->new_key = net_tx->sub->kr_flag;
+	tx->cb = cb;
+	tx->cb_data = cb_data;
+
+	if (net_tx->ctx->send_ttl == BT_MESH_TTL_DEFAULT) {
+		tx->ttl = bt_mesh_default_ttl_get();
+	} else {
+		tx->ttl = net_tx->ctx->send_ttl;
+	}
+
+	seq_zero = tx->seq_auth & TRANS_SEQ_ZERO_MASK;
+
+	BT_DBG("SeqZero 0x%04x", seq_zero);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) &&
+	    !bt_mesh_friend_queue_has_space(tx->sub->net_idx, net_tx->src,
+					    tx->dst, &tx->seq_auth,
+					    tx->seg_n + 1) &&
+	    BT_MESH_ADDR_IS_UNICAST(tx->dst)) {
+		BT_ERR("Not enough space in Friend Queue for %u segments",
+		       tx->seg_n + 1);
+		seg_tx_reset(tx);
+		return -ENOBUFS;
+	}
+
+	for (seg_o = 0; sdu->om_len; seg_o++) {
+		struct os_mbuf *seg;
+		u16_t len;
+		int err;
+
+		seg = bt_mesh_adv_create(BT_MESH_ADV_DATA, net_tx->xmit,
+					 BUF_TIMEOUT);
+		if (!seg) {
+			BT_ERR("Out of segment buffers");
+			seg_tx_reset(tx);
+			return -ENOBUFS;
+		}
+
+		BT_MESH_ADV(seg)->seg.attempts = SEG_RETRANSMIT_ATTEMPTS;
+
+		net_buf_reserve(seg, BT_MESH_NET_HDR_LEN);
+
+		net_buf_add_u8(seg, seg_hdr);
+		net_buf_add_u8(seg, (net_tx->aszmic << 7) | seq_zero >> 6);
+		net_buf_add_u8(seg, (((seq_zero & 0x3f) << 2) |
+				     (seg_o >> 3)));
+		net_buf_add_u8(seg, ((seg_o & 0x07) << 5) | tx->seg_n);
+
+		len = min(sdu->om_len, 12);
+		net_buf_add_mem(seg, sdu->om_data, len);
+		net_buf_simple_pull(sdu, len);
+
+		if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+			enum bt_mesh_friend_pdu_type type;
+
+			if (seg_o == tx->seg_n) {
+				type = BT_MESH_FRIEND_PDU_COMPLETE;
+			} else {
+				type = BT_MESH_FRIEND_PDU_PARTIAL;
+			}
+
+			if (bt_mesh_friend_enqueue_tx(net_tx, type,
+						      &tx->seq_auth,
+						      tx->seg_n + 1,
+						      seg) &&
+			    BT_MESH_ADDR_IS_UNICAST(net_tx->ctx->addr)) {
+				/* PDUs for a specific Friend should only go
+				 * out through the Friend Queue.
+				 */
+				net_buf_unref(seg);
+				continue;
+			}
+		}
+
+		tx->seg[seg_o] = net_buf_ref(seg);
+
+		BT_DBG("Sending %u/%u", seg_o, tx->seg_n);
+
+		err = bt_mesh_net_send(net_tx, seg,
+				       seg_o ? &seg_sent_cb : &first_sent_cb,
+				       tx);
+		if (err) {
+			BT_ERR("Sending segment failed");
+			seg_tx_reset(tx);
+			return err;
+		}
+	}
+
+	/* This can happen if segments only went into the Friend Queue */
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) && !tx->seg[0]) {
+		seg_tx_reset(tx);
+
+		/* If there was a callback notify sending immediately since
+		 * there's no other way to track this (at least currently)
+		 * with the Friend Queue.
+		 */
+		send_cb_finalize(cb, cb_data);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER) &&
+	    bt_mesh_lpn_established()) {
+		bt_mesh_lpn_poll();
+	}
+
+	return 0;
+}
+
+struct bt_mesh_app_key *bt_mesh_app_key_find(u16_t app_idx)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+
+		if (key->net_idx != BT_MESH_KEY_UNUSED &&
+		    key->app_idx == app_idx) {
+			return key;
+		}
+	}
+
+	return NULL;
+}
+
+int bt_mesh_trans_send(struct bt_mesh_net_tx *tx, struct os_mbuf *msg,
+		       const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	const u8_t *key;
+	u8_t *ad;
+	u8_t aid;
+	int err;
+
+	if (net_buf_simple_tailroom(msg) < 4) {
+		BT_ERR("Insufficient tailroom for Transport MIC");
+		return -EINVAL;
+	}
+
+	if (msg->om_len > 11) {
+		tx->ctx->send_rel = 1;
+		tx->ctx->send_rel = true;
+	}
+
+	BT_DBG("net_idx 0x%04x app_idx 0x%04x dst 0x%04x", tx->sub->net_idx,
+	       tx->ctx->app_idx, tx->ctx->addr);
+	BT_DBG("len %u: %s", msg->om_len, bt_hex(msg->om_data, msg->om_len));
+
+	err = bt_mesh_app_key_get(tx->sub, tx->ctx->app_idx,
+				  tx->ctx->addr, &key, &aid);
+	if (err) {
+		return err;
+	}
+
+	tx->aid = aid;
+
+	if (!tx->ctx->send_rel || net_buf_simple_tailroom(msg) < 8) {
+		tx->aszmic = 0;
+	} else {
+		tx->aszmic = 1;
+	}
+
+	if (BT_MESH_ADDR_IS_VIRTUAL(tx->ctx->addr)) {
+		ad = bt_mesh_label_uuid_get(tx->ctx->addr);
+	} else {
+		ad = NULL;
+	}
+
+	err = bt_mesh_app_encrypt(key, BT_MESH_IS_DEV_KEY(tx->ctx->app_idx),
+				  tx->aszmic, msg, ad, tx->src, tx->ctx->addr,
+				  bt_mesh.seq, BT_MESH_NET_IVI_TX);
+	if (err) {
+		return err;
+	}
+
+	if (tx->ctx->send_rel) {
+		err = send_seg(tx, msg, cb, cb_data);
+	} else {
+		err = send_unseg(tx, msg, cb, cb_data);
+	}
+
+	return err;
+}
+
+static void update_rpl(struct bt_mesh_rpl *rpl, struct bt_mesh_net_rx *rx)
+{
+	rpl->src = rx->ctx.addr;
+	rpl->seq = rx->seq;
+	rpl->old_iv = rx->old_iv;
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_store_rpl(rpl);
+	}
+}
+
+/* Check the Replay Protection List for a replay attempt. If non-NULL match
+ * parameter is given the RPL slot is returned but it is not immediately
+ * updated (needed for segmented messages), whereas if a NULL match is given
+ * the RPL is immediately updated (used for unsegmented messages).
+ */
+static bool is_replay(struct bt_mesh_net_rx *rx, struct bt_mesh_rpl **match)
+{
+	int i;
+
+	/* Don't bother checking messages from ourselves */
+	if (rx->net_if == BT_MESH_NET_IF_LOCAL) {
+		return false;
+	}
+
+	/* The RPL is used only for the local node */
+	if (!rx->local_match) {
+		return false;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) {
+		struct bt_mesh_rpl *rpl = &bt_mesh.rpl[i];
+
+		/* Empty slot */
+		if (!rpl->src) {
+			if (match) {
+				*match = rpl;
+			} else {
+				update_rpl(rpl, rx);
+			}
+
+			return false;
+		}
+
+		/* Existing slot for given address */
+		if (rpl->src == rx->ctx.addr) {
+			if (rx->old_iv && !rpl->old_iv) {
+				return true;
+			}
+
+			if ((!rx->old_iv && rpl->old_iv) ||
+			    rpl->seq < rx->seq) {
+				if (match) {
+					*match = rpl;
+				} else {
+					update_rpl(rpl, rx);
+				}
+
+				return false;
+			} else {
+				return true;
+			}
+		}
+	}
+
+	BT_ERR("RPL is full!");
+	return true;
+}
+
+static int sdu_recv(struct bt_mesh_net_rx *rx, u32_t seq, u8_t hdr,
+		    u8_t aszmic, struct os_mbuf *buf)
+{
+	struct os_mbuf *sdu =
+		NET_BUF_SIMPLE(MYNEWT_VAL(BLE_MESH_RX_SDU_MAX) - 4);
+	u8_t *ad;
+	u16_t i;
+	int err = 0;
+
+	BT_DBG("ASZMIC %u AKF %u AID 0x%02x", aszmic, AKF(&hdr), AID(&hdr));
+	BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
+
+	if (buf->om_len < 1 + APP_MIC_LEN(aszmic)) {
+		BT_ERR("Too short SDU + MIC");
+		err = -EINVAL;
+		goto done;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) && !rx->local_match) {
+		BT_DBG("Ignoring PDU for LPN 0x%04x of this Friend",
+		       rx->ctx.recv_dst);
+		goto done;
+	}
+
+	if (BT_MESH_ADDR_IS_VIRTUAL(rx->ctx.recv_dst)) {
+		ad = bt_mesh_label_uuid_get(rx->ctx.recv_dst);
+	} else {
+		ad = NULL;
+	}
+
+	/* Adjust the length to not contain the MIC at the end */
+	buf->om_len -= APP_MIC_LEN(aszmic);
+
+	if (!AKF(&hdr)) {
+		net_buf_simple_init(sdu, 0);
+		err = bt_mesh_app_decrypt(bt_mesh.dev_key, true, aszmic, buf,
+					  sdu, ad, rx->ctx.addr,
+					  rx->ctx.recv_dst, seq,
+					  BT_MESH_NET_IVI_RX(rx));
+		if (err) {
+			BT_WARN("Unable to decrypt with local DevKey");
+		} else {
+			rx->ctx.app_idx = BT_MESH_KEY_DEV_LOCAL;
+			bt_mesh_model_recv(rx, sdu);
+			goto done;
+		}
+
+		if (IS_ENABLED(CONFIG_BT_MESH_PROVISIONER)) {
+			struct bt_mesh_node *node;
+
+			/*
+			 * There is no way of knowing if we should use our
+			 * local DevKey or the remote DevKey to decrypt the
+			 * message so we must try both.
+			 */
+
+			node = bt_mesh_node_find(rx->ctx.addr);
+			if (node == NULL) {
+				BT_ERR("No node found for addr 0x%04x",
+				       rx->ctx.addr);
+				return -EINVAL;
+			}
+
+			net_buf_simple_init(sdu, 0);
+			err = bt_mesh_app_decrypt(node->dev_key, true, aszmic,
+						  buf, sdu, ad, rx->ctx.addr,
+						  rx->ctx.recv_dst, seq,
+						  BT_MESH_NET_IVI_RX(rx));
+			if (err) {
+				BT_ERR("Unable to decrypt with node DevKey");
+				return -EINVAL;
+			}
+
+			rx->ctx.app_idx = BT_MESH_KEY_DEV_REMOTE;
+			bt_mesh_model_recv(rx, sdu);
+			return 0;
+		}
+
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) {
+		struct bt_mesh_app_key *key = &bt_mesh.app_keys[i];
+		struct bt_mesh_app_keys *keys;
+
+		/* Check that this AppKey matches received net_idx */
+		if (key->net_idx != rx->sub->net_idx) {
+			continue;
+		}
+
+		if (rx->new_key && key->updated) {
+			keys = &key->keys[1];
+		} else {
+			keys = &key->keys[0];
+		}
+
+		/* Check that the AppKey ID matches */
+		if (AID(&hdr) != keys->id) {
+			continue;
+		}
+
+		net_buf_simple_init(sdu, 0);
+		err = bt_mesh_app_decrypt(keys->val, false, aszmic, buf,
+					  sdu, ad, rx->ctx.addr,
+					  rx->ctx.recv_dst, seq,
+					  BT_MESH_NET_IVI_RX(rx));
+		if (err) {
+			BT_WARN("Unable to decrypt with AppKey 0x%03x",
+				key->app_idx);
+			continue;
+
+		}
+
+		rx->ctx.app_idx = key->app_idx;
+
+		bt_mesh_model_recv(rx, sdu);
+		goto done;
+	}
+
+	BT_WARN("No matching AppKey");
+
+	err = -EINVAL;
+done:
+    os_mbuf_free_chain(sdu);
+    return err;
+}
+
+static struct seg_tx *seg_tx_lookup(u16_t seq_zero, u8_t obo, u16_t addr)
+{
+	struct seg_tx *tx;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(seg_tx); i++) {
+		tx = &seg_tx[i];
+
+		if ((tx->seq_auth & TRANS_SEQ_ZERO_MASK) != seq_zero) {
+			continue;
+		}
+
+		if (tx->dst == addr) {
+			return tx;
+		}
+
+		/* If the expected remote address doesn't match,
+		 * but the OBO flag is set and this is the first
+		 * acknowledgement, assume it's a Friend that's
+		 * responding and therefore accept the message.
+		 */
+		if (obo && tx->nack_count == tx->seg_n + 1) {
+			tx->dst = addr;
+			return tx;
+		}
+	}
+
+	return NULL;
+}
+
+static int trans_ack(struct bt_mesh_net_rx *rx, u8_t hdr,
+		     struct os_mbuf *buf, u64_t *seq_auth)
+{
+	struct seg_tx *tx;
+	unsigned int bit;
+	u32_t ack;
+	u16_t seq_zero;
+	u8_t obo;
+
+	if (buf->om_len < 6) {
+		BT_ERR("Too short ack message");
+		return -EINVAL;
+	}
+
+	seq_zero = net_buf_simple_pull_be16(buf);
+	obo = seq_zero >> 15;
+	seq_zero = (seq_zero >> 2) & TRANS_SEQ_ZERO_MASK;
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) && rx->friend_match) {
+		BT_DBG("Ack for LPN 0x%04x of this Friend", rx->ctx.recv_dst);
+		/* Best effort - we don't have enough info for true SeqAuth */
+		*seq_auth = SEQ_AUTH(BT_MESH_NET_IVI_RX(rx), seq_zero);
+		return 0;
+	}
+
+	ack = net_buf_simple_pull_be32(buf);
+
+	BT_DBG("OBO %u seq_zero 0x%04x ack 0x%08x", obo, seq_zero,
+	       (unsigned) ack);
+
+	tx = seg_tx_lookup(seq_zero, obo, rx->ctx.addr);
+	if (!tx) {
+		BT_WARN("No matching TX context for ack");
+		return -EINVAL;
+	}
+
+	*seq_auth = tx->seq_auth;
+
+	if (!ack) {
+		BT_WARN("SDU canceled");
+		seg_tx_complete(tx, -ECANCELED);
+		return 0;
+	}
+
+	if (find_msb_set(ack) - 1 > tx->seg_n) {
+		BT_ERR("Too large segment number in ack");
+		return -EINVAL;
+	}
+
+	k_delayed_work_cancel(&tx->retransmit);
+
+	while ((bit = find_lsb_set(ack))) {
+		if (tx->seg[bit - 1]) {
+			BT_DBG("seg %u/%u acked", bit - 1, tx->seg_n);
+			net_buf_unref(tx->seg[bit - 1]);
+			tx->seg[bit - 1] = NULL;
+			tx->nack_count--;
+		}
+
+		ack &= ~BIT(bit - 1);
+	}
+
+	if (tx->nack_count) {
+		seg_tx_send_unacked(tx);
+	} else {
+		BT_DBG("SDU TX complete");
+		seg_tx_complete(tx, 0);
+	}
+
+	return 0;
+}
+
+static int trans_heartbeat(struct bt_mesh_net_rx *rx,
+			   struct os_mbuf *buf)
+{
+	u8_t init_ttl, hops;
+	u16_t feat;
+
+	if (buf->om_len < 3) {
+		BT_ERR("Too short heartbeat message");
+		return -EINVAL;
+	}
+
+	if (rx->ctx.recv_dst != hb_sub_dst) {
+		BT_WARN("Ignoring heartbeat to non-subscribed destination");
+		return 0;
+	}
+
+	init_ttl = (net_buf_simple_pull_u8(buf) & 0x7f);
+	feat = net_buf_simple_pull_be16(buf);
+
+	hops = (init_ttl - rx->ctx.recv_ttl + 1);
+
+	BT_DBG("src 0x%04x TTL %u InitTTL %u (%u hop%s) feat 0x%04x",
+	       rx->ctx.addr, rx->ctx.recv_ttl, init_ttl, hops,
+	       (hops == 1) ? "" : "s", feat);
+
+	bt_mesh_heartbeat(rx->ctx.addr, rx->ctx.recv_dst, hops, feat);
+
+	return 0;
+}
+
+static int ctl_recv(struct bt_mesh_net_rx *rx, u8_t hdr,
+		    struct os_mbuf *buf, u64_t *seq_auth)
+{
+	u8_t ctl_op = TRANS_CTL_OP(&hdr);
+
+	BT_DBG("OpCode 0x%02x len %u", ctl_op, buf->om_len);
+
+	switch (ctl_op) {
+	case TRANS_CTL_OP_ACK:
+		return trans_ack(rx, hdr, buf, seq_auth);
+	case TRANS_CTL_OP_HEARTBEAT:
+		return trans_heartbeat(rx, buf);
+	}
+
+	/* Only acks and heartbeats may need processing without local_match */
+	if (!rx->local_match) {
+		return 0;
+	}
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) && !bt_mesh_lpn_established()) {
+		switch (ctl_op) {
+		case TRANS_CTL_OP_FRIEND_POLL:
+			return bt_mesh_friend_poll(rx, buf);
+		case TRANS_CTL_OP_FRIEND_REQ:
+			return bt_mesh_friend_req(rx, buf);
+		case TRANS_CTL_OP_FRIEND_CLEAR:
+			return bt_mesh_friend_clear(rx, buf);
+		case TRANS_CTL_OP_FRIEND_CLEAR_CFM:
+			return bt_mesh_friend_clear_cfm(rx, buf);
+		case TRANS_CTL_OP_FRIEND_SUB_ADD:
+			return bt_mesh_friend_sub_add(rx, buf);
+		case TRANS_CTL_OP_FRIEND_SUB_REM:
+			return bt_mesh_friend_sub_rem(rx, buf);
+		}
+	}
+
+#if (MYNEWT_VAL(BLE_MESH_LOW_POWER))
+	if (ctl_op == TRANS_CTL_OP_FRIEND_OFFER) {
+		return bt_mesh_lpn_friend_offer(rx, buf);
+	}
+
+	if (rx->ctx.addr == bt_mesh.lpn.frnd) {
+		if (ctl_op == TRANS_CTL_OP_FRIEND_CLEAR_CFM) {
+			return bt_mesh_lpn_friend_clear_cfm(rx, buf);
+		}
+
+		if (!rx->friend_cred) {
+			BT_WARN("Message from friend with wrong credentials");
+			return -EINVAL;
+		}
+
+		switch (ctl_op) {
+		case TRANS_CTL_OP_FRIEND_UPDATE:
+			return bt_mesh_lpn_friend_update(rx, buf);
+		case TRANS_CTL_OP_FRIEND_SUB_CFM:
+			return bt_mesh_lpn_friend_sub_cfm(rx, buf);
+		}
+	}
+#endif /* MYNEWT_VAL(BLE_MESH_LOW_POWER) */
+
+	BT_WARN("Unhandled TransOpCode 0x%02x", ctl_op);
+
+	return -ENOENT;
+}
+
+static int trans_unseg(struct os_mbuf *buf, struct bt_mesh_net_rx *rx,
+		       u64_t *seq_auth)
+{
+	u8_t hdr;
+
+	BT_DBG("AFK %u AID 0x%02x", AKF(buf->om_data), AID(buf->om_data));
+
+	if (buf->om_len < 1) {
+		BT_ERR("Too small unsegmented PDU");
+		return -EINVAL;
+	}
+
+	if (is_replay(rx, NULL)) {
+		BT_WARN("Replay: src 0x%04x dst 0x%04x seq 0x%06x",
+			rx->ctx.addr, rx->ctx.recv_dst, (unsigned) rx->seq);
+		return -EINVAL;
+	}
+
+	hdr = net_buf_simple_pull_u8(buf);
+
+	if (rx->ctl) {
+		return ctl_recv(rx, hdr, buf, seq_auth);
+	} else {
+		/* SDUs must match a local element or an LPN of this Friend. */
+		if (!rx->local_match && !rx->friend_match) {
+			return 0;
+		}
+
+		return sdu_recv(rx, rx->seq, hdr, 0, buf);
+	}
+}
+
+static inline s32_t ack_timeout(struct seg_rx *rx)
+{
+	s32_t to;
+	u8_t ttl;
+
+	if (rx->ttl == BT_MESH_TTL_DEFAULT) {
+		ttl = bt_mesh_default_ttl_get();
+	} else {
+		ttl = rx->ttl;
+	}
+
+	/* The acknowledgment timer shall be set to a minimum of
+	 * 150 + 50 * TTL milliseconds.
+	 */
+	to = K_MSEC(150 + (50 * ttl));
+
+	/* 100 ms for every not yet received segment */
+	to += K_MSEC(((rx->seg_n + 1) - popcount(rx->block)) * 100);
+
+	/* Make sure we don't send more frequently than the duration for
+	 * each packet (default is 300ms).
+	 */
+	return max(to, K_MSEC(400));
+}
+
+int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data,
+		     size_t data_len, u64_t *seq_auth,
+		     const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+	struct os_mbuf *buf;
+
+	BT_DBG("src 0x%04x dst 0x%04x ttl 0x%02x ctl 0x%02x", tx->src,
+	       tx->ctx->addr, tx->ctx->send_ttl, ctl_op);
+	BT_DBG("len %zu: %s", data_len, bt_hex(data, data_len));
+
+	buf = bt_mesh_adv_create(BT_MESH_ADV_DATA, tx->xmit, BUF_TIMEOUT);
+	if (!buf) {
+		BT_ERR("Out of transport buffers");
+		return -ENOBUFS;
+	}
+
+	net_buf_reserve(buf, BT_MESH_NET_HDR_LEN);
+
+	net_buf_add_u8(buf, TRANS_CTL_HDR(ctl_op, 0));
+
+	net_buf_add_mem(buf, data, data_len);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		if (bt_mesh_friend_enqueue_tx(tx, BT_MESH_FRIEND_PDU_SINGLE,
+					      seq_auth, 1, buf) &&
+		    BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) {
+			/* PDUs for a specific Friend should only go
+			 * out through the Friend Queue.
+			 */
+			net_buf_unref(buf);
+			return 0;
+		}
+	}
+
+	return bt_mesh_net_send(tx, buf, cb, cb_data);
+}
+
+static int send_ack(struct bt_mesh_subnet *sub, u16_t src, u16_t dst,
+		    u8_t ttl, u64_t *seq_auth, u32_t block, u8_t obo)
+{
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = sub->net_idx,
+		.app_idx = BT_MESH_KEY_UNUSED,
+		.addr = dst,
+		.send_ttl = ttl,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub = sub,
+		.ctx = &ctx,
+		.src = obo ? bt_mesh_primary_addr() : src,
+		.xmit = bt_mesh_net_transmit_get(),
+	};
+	u16_t seq_zero = *seq_auth & TRANS_SEQ_ZERO_MASK;
+	u8_t buf[6];
+
+	BT_DBG("SeqZero 0x%04x Block 0x%08x OBO %u", seq_zero,
+	       (unsigned) block, obo);
+
+	if (bt_mesh_lpn_established()) {
+		BT_WARN("Not sending ack when LPN is enabled");
+		return 0;
+	}
+
+	/* This can happen if the segmented message was destined for a group
+	 * or virtual address.
+	 */
+	if (!BT_MESH_ADDR_IS_UNICAST(src)) {
+		BT_WARN("Not sending ack for non-unicast address");
+		return 0;
+	}
+
+	sys_put_be16(((seq_zero << 2) & 0x7ffc) | (obo << 15), buf);
+	sys_put_be32(block, &buf[2]);
+
+	return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_ACK, buf, sizeof(buf),
+				NULL, NULL, NULL);
+}
+
+static void seg_rx_reset(struct seg_rx *rx, bool full_reset)
+{
+	BT_DBG("rx %p", rx);
+
+	k_delayed_work_cancel(&rx->ack);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) && rx->obo &&
+	    rx->block != BLOCK_COMPLETE(rx->seg_n)) {
+		BT_WARN("Clearing incomplete buffers from Friend queue");
+		bt_mesh_friend_clear_incomplete(rx->sub, rx->src, rx->dst,
+						&rx->seq_auth);
+	}
+
+	rx->in_use = 0;
+
+	/* We don't always reset these values since we need to be able to
+	 * send an ack if we receive a segment after we've already received
+	 * the full SDU.
+	 */
+	if (full_reset) {
+		rx->seq_auth = 0;
+		rx->sub = NULL;
+		rx->src = BT_MESH_ADDR_UNASSIGNED;
+		rx->dst = BT_MESH_ADDR_UNASSIGNED;
+	}
+}
+
+static void seg_ack(struct ble_npl_event *work)
+{
+	struct seg_rx *rx = ble_npl_event_get_arg(work);
+
+	BT_DBG("rx %p", rx);
+
+	if (k_uptime_get_32() - rx->last > K_SECONDS(60)) {
+		BT_WARN("Incomplete timer expired");
+		seg_rx_reset(rx, false);
+
+		if (IS_ENABLED(CONFIG_BT_TESTING)) {
+			bt_test_mesh_trans_incomp_timer_exp();
+		}
+
+		return;
+	}
+
+	send_ack(rx->sub, rx->dst, rx->src, rx->ttl, &rx->seq_auth,
+		 rx->block, rx->obo);
+
+	k_delayed_work_submit(&rx->ack, ack_timeout(rx));
+}
+
+static inline u8_t seg_len(bool ctl)
+{
+	if (ctl) {
+		return 8;
+	} else {
+		return 12;
+	}
+}
+
+static inline bool sdu_len_is_ok(bool ctl, u8_t seg_n)
+{
+	return ((seg_n * seg_len(ctl) + 1) <= MYNEWT_VAL(BLE_MESH_RX_SDU_MAX));
+}
+
+static struct seg_rx *seg_rx_find(struct bt_mesh_net_rx *net_rx,
+				  const u64_t *seq_auth)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(seg_rx); i++) {
+		struct seg_rx *rx = &seg_rx[i];
+
+		if (rx->src != net_rx->ctx.addr ||
+		    rx->dst != net_rx->ctx.recv_dst) {
+			continue;
+		}
+
+		/* Return newer RX context in addition to an exact match, so
+		 * the calling function can properly discard an old SeqAuth.
+		 */
+		if (rx->seq_auth >= *seq_auth) {
+			return rx;
+		}
+
+		if (rx->in_use) {
+			BT_WARN("Duplicate SDU from src 0x%04x",
+				net_rx->ctx.addr);
+
+			/* Clear out the old context since the sender
+			 * has apparently started sending a new SDU.
+			 */
+			seg_rx_reset(rx, true);
+
+			/* Return non-match so caller can re-allocate */
+			return NULL;
+		}
+	}
+
+	return NULL;
+}
+
+static bool seg_rx_is_valid(struct seg_rx *rx, struct bt_mesh_net_rx *net_rx,
+			    const u8_t *hdr, u8_t seg_n)
+{
+	if (rx->hdr != *hdr || rx->seg_n != seg_n) {
+		BT_ERR("Invalid segment for ongoing session");
+		return false;
+	}
+
+	if (rx->src != net_rx->ctx.addr || rx->dst != net_rx->ctx.recv_dst) {
+		BT_ERR("Invalid source or destination for segment");
+		return false;
+	}
+
+	if (rx->ctl != net_rx->ctl) {
+		BT_ERR("Inconsistent CTL in segment");
+		return false;
+	}
+
+	return true;
+}
+
+static struct seg_rx *seg_rx_alloc(struct bt_mesh_net_rx *net_rx,
+				   const u8_t *hdr, const u64_t *seq_auth,
+				   u8_t seg_n)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(seg_rx); i++) {
+		struct seg_rx *rx = &seg_rx[i];
+
+		if (rx->in_use) {
+			continue;
+		}
+
+		rx->in_use = 1;
+		net_buf_simple_init(rx->buf, 0);
+		rx->sub = net_rx->sub;
+		rx->ctl = net_rx->ctl;
+		rx->seq_auth = *seq_auth;
+		rx->seg_n = seg_n;
+		rx->hdr = *hdr;
+		rx->ttl = net_rx->ctx.send_ttl;
+		rx->src = net_rx->ctx.addr;
+		rx->dst = net_rx->ctx.recv_dst;
+		rx->block = 0;
+
+		BT_DBG("New RX context. Block Complete 0x%08x",
+		       (unsigned) BLOCK_COMPLETE(seg_n));
+
+		return rx;
+	}
+
+	return NULL;
+}
+
+static int trans_seg(struct os_mbuf *buf, struct bt_mesh_net_rx *net_rx,
+		     enum bt_mesh_friend_pdu_type *pdu_type, u64_t *seq_auth,
+		     u8_t *seg_count)
+{
+	struct bt_mesh_rpl *rpl = NULL;
+	struct seg_rx *rx;
+	u8_t *hdr = buf->om_data;
+	u16_t seq_zero;
+	u8_t seg_n;
+	u8_t seg_o;
+	int err;
+
+	if (buf->om_len < 5) {
+		BT_ERR("Too short segmented message (len %u)", buf->om_len);
+		return -EINVAL;
+	}
+
+	if (is_replay(net_rx, &rpl)) {
+		BT_WARN("Replay: src 0x%04x dst 0x%04x seq 0x%06x",
+			net_rx->ctx.addr, net_rx->ctx.recv_dst, net_rx->seq);
+		return -EINVAL;
+	}
+
+	BT_DBG("ASZMIC %u AKF %u AID 0x%02x", ASZMIC(hdr), AKF(hdr), AID(hdr));
+
+	net_buf_simple_pull(buf, 1);
+
+	seq_zero = net_buf_simple_pull_be16(buf);
+	seg_o = (seq_zero & 0x03) << 3;
+	seq_zero = (seq_zero >> 2) & TRANS_SEQ_ZERO_MASK;
+	seg_n = net_buf_simple_pull_u8(buf);
+	seg_o |= seg_n >> 5;
+	seg_n &= 0x1f;
+
+	BT_DBG("SeqZero 0x%04x SegO %u SegN %u", seq_zero, seg_o, seg_n);
+
+	if (seg_o > seg_n) {
+		BT_ERR("SegO greater than SegN (%u > %u)", seg_o, seg_n);
+		return -EINVAL;
+	}
+
+	/* According to Mesh 1.0 specification:
+	 * "The SeqAuth is composed of the IV Index and the sequence number
+	 *  (SEQ) of the first segment"
+	 *
+	 * Therefore we need to calculate very first SEQ in order to find
+	 * seqAuth. We can calculate as below:
+	 *
+	 * SEQ(0) = SEQ(n) - (delta between seqZero and SEQ(n) by looking into
+	 * 14 least significant bits of SEQ(n))
+	 *
+	 * Mentioned delta shall be >= 0, if it is not then seq_auth will
+	 * be broken and it will be verified by the code below.
+	 */
+	*seq_auth = SEQ_AUTH(BT_MESH_NET_IVI_RX(net_rx),
+			     (net_rx->seq -
+			      ((((net_rx->seq & BIT_MASK(14)) - seq_zero)) &
+			       BIT_MASK(13))));
+
+	*seg_count = seg_n + 1;
+
+	/* Look for old RX sessions */
+	rx = seg_rx_find(net_rx, seq_auth);
+	if (rx) {
+		/* Discard old SeqAuth packet */
+		if (rx->seq_auth > *seq_auth) {
+			BT_WARN("Ignoring old SeqAuth");
+			return -EINVAL;
+		}
+
+		if (!seg_rx_is_valid(rx, net_rx, hdr, seg_n)) {
+			return -EINVAL;
+		}
+
+		if (rx->in_use) {
+			BT_DBG("Existing RX context. Block 0x%08x",
+			       (unsigned) rx->block);
+			goto found_rx;
+		}
+
+		if (rx->block == BLOCK_COMPLETE(rx->seg_n)) {
+			BT_WARN("Got segment for already complete SDU");
+
+			send_ack(net_rx->sub, net_rx->ctx.recv_dst,
+				 net_rx->ctx.addr, net_rx->ctx.send_ttl,
+				 seq_auth, rx->block, rx->obo);
+
+			if (rpl) {
+				update_rpl(rpl, net_rx);
+			}
+
+			return -EALREADY;
+		}
+
+		/* We ignore instead of sending block ack 0 since the
+		 * ack timer is always smaller than the incomplete
+		 * timer, i.e. the sender is misbehaving.
+		 */
+		BT_WARN("Got segment for canceled SDU");
+		return -EINVAL;
+	}
+
+	/* Bail out early if we're not ready to receive such a large SDU */
+	if (!sdu_len_is_ok(net_rx->ctl, seg_n)) {
+		BT_ERR("Too big incoming SDU length");
+		send_ack(net_rx->sub, net_rx->ctx.recv_dst, net_rx->ctx.addr,
+			 net_rx->ctx.send_ttl, seq_auth, 0,
+			 net_rx->friend_match);
+		return -EMSGSIZE;
+	}
+
+	/* Verify early that there will be space in the Friend Queue(s) in
+	 * case this message is destined to an LPN of ours.
+	 */
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) &&
+	    net_rx->friend_match && !net_rx->local_match &&
+	    !bt_mesh_friend_queue_has_space(net_rx->sub->net_idx,
+					    net_rx->ctx.addr,
+					    net_rx->ctx.recv_dst, seq_auth,
+					    *seg_count)) {
+		BT_ERR("No space in Friend Queue for %u segments", *seg_count);
+		send_ack(net_rx->sub, net_rx->ctx.recv_dst, net_rx->ctx.addr,
+			 net_rx->ctx.send_ttl, seq_auth, 0,
+			 net_rx->friend_match);
+		return -ENOBUFS;
+	}
+
+	/* Look for free slot for a new RX session */
+	rx = seg_rx_alloc(net_rx, hdr, seq_auth, seg_n);
+	if (!rx) {
+		/* Warn but don't cancel since the existing slots willl
+		 * eventually be freed up and we'll be able to process
+		 * this one.
+		 */
+		BT_WARN("No free slots for new incoming segmented messages");
+		return -ENOMEM;
+	}
+
+	rx->obo = net_rx->friend_match;
+
+found_rx:
+	if (BIT(seg_o) & rx->block) {
+		BT_WARN("Received already received fragment");
+		return -EALREADY;
+	}
+
+	/* All segments, except the last one, must either have 8 bytes of
+	 * payload (for 64bit Net MIC) or 12 bytes of payload (for 32bit
+	 * Net MIC).
+	 */
+	if (seg_o == seg_n) {
+		/* Set the expected final buffer length */
+		rx->buf->om_len = seg_n * seg_len(rx->ctl) + buf->om_len;
+		BT_DBG("Target len %u * %u + %u = %u", seg_n, seg_len(rx->ctl),
+		       buf->om_len, rx->buf->om_len);
+
+		if (rx->buf->om_len > MYNEWT_VAL(BLE_MESH_RX_SDU_MAX)) {
+			BT_ERR("Too large SDU len");
+			send_ack(net_rx->sub, net_rx->ctx.recv_dst,
+				 net_rx->ctx.addr, net_rx->ctx.send_ttl,
+				 seq_auth, 0, rx->obo);
+			seg_rx_reset(rx, true);
+			return -EMSGSIZE;
+		}
+	} else {
+		if (buf->om_len != seg_len(rx->ctl)) {
+			BT_ERR("Incorrect segment size for message type");
+			return -EINVAL;
+		}
+	}
+
+	/* Reset the Incomplete Timer */
+	rx->last = k_uptime_get_32();
+
+	if (!k_delayed_work_remaining_get(&rx->ack) &&
+	    !bt_mesh_lpn_established()) {
+		k_delayed_work_submit(&rx->ack, ack_timeout(rx));
+	}
+
+	/* Location in buffer can be calculated based on seg_o & rx->ctl */
+	memcpy(rx->buf->om_data + (seg_o * seg_len(rx->ctl)), buf->om_data, buf->om_len);
+
+	BT_DBG("Received %u/%u", seg_o, seg_n);
+
+	/* Mark segment as received */
+	rx->block |= BIT(seg_o);
+
+	if (rx->block != BLOCK_COMPLETE(seg_n)) {
+		*pdu_type = BT_MESH_FRIEND_PDU_PARTIAL;
+		return 0;
+	}
+
+	BT_DBG("Complete SDU");
+
+	if (rpl) {
+		update_rpl(rpl, net_rx);
+	}
+
+	*pdu_type = BT_MESH_FRIEND_PDU_COMPLETE;
+
+	k_delayed_work_cancel(&rx->ack);
+	send_ack(net_rx->sub, net_rx->ctx.recv_dst, net_rx->ctx.addr,
+		 net_rx->ctx.send_ttl, seq_auth, rx->block, rx->obo);
+
+	if (net_rx->ctl) {
+		err = ctl_recv(net_rx, *hdr, rx->buf, seq_auth);
+	} else {
+		err = sdu_recv(net_rx, (rx->seq_auth & 0xffffff), *hdr,
+			       ASZMIC(hdr), rx->buf);
+	}
+
+	seg_rx_reset(rx, false);
+
+	return err;
+}
+
+int bt_mesh_trans_recv(struct os_mbuf *buf, struct bt_mesh_net_rx *rx)
+{
+	u64_t seq_auth = TRANS_SEQ_AUTH_NVAL;
+	enum bt_mesh_friend_pdu_type pdu_type = BT_MESH_FRIEND_PDU_SINGLE;
+	struct net_buf_simple_state state;
+	u8_t seg_count = 0;
+	int err;
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) {
+		rx->friend_match = bt_mesh_friend_match(rx->sub->net_idx,
+							rx->ctx.recv_dst);
+	} else {
+		rx->friend_match = false;
+	}
+
+	BT_DBG("src 0x%04x dst 0x%04x seq 0x%08x friend_match %u",
+	       rx->ctx.addr, rx->ctx.recv_dst, (unsigned) rx->seq,
+	       rx->friend_match);
+
+	/* Remove network headers */
+	net_buf_simple_pull(buf, BT_MESH_NET_HDR_LEN);
+
+	BT_DBG("Payload %s", bt_hex(buf->om_data, buf->om_len));
+
+	if (IS_ENABLED(CONFIG_BT_TESTING)) {
+		bt_test_mesh_net_recv(rx->ctx.recv_ttl, rx->ctl, rx->ctx.addr,
+				      rx->ctx.recv_dst, buf->om_data, buf->om_len);
+	}
+
+	/* If LPN mode is enabled messages are only accepted when we've
+	 * requested the Friend to send them. The messages must also
+	 * be encrypted using the Friend Credentials.
+	 */
+	if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) &&
+	    bt_mesh_lpn_established() && rx->net_if == BT_MESH_NET_IF_ADV &&
+	    (!bt_mesh_lpn_waiting_update() || !rx->friend_cred)) {
+		BT_WARN("Ignoring unexpected message in Low Power mode");
+		return -EAGAIN;
+	}
+
+	/* Save the app-level state so the buffer can later be placed in
+	 * the Friend Queue.
+	 */
+	net_buf_simple_save(buf, &state);
+
+	if (SEG(buf->om_data)) {
+		/* Segmented messages must match a local element or an
+		 * LPN of this Friend.
+		 */
+		if (!rx->local_match && !rx->friend_match) {
+			return 0;
+		}
+
+		err = trans_seg(buf, rx, &pdu_type, &seq_auth, &seg_count);
+	} else {
+		seg_count = 1;
+		err = trans_unseg(buf, rx, &seq_auth);
+	}
+
+	/* Notify LPN state machine so a Friend Poll will be sent. If the
+	 * message was a Friend Update it's possible that a Poll was already
+	 * queued for sending, however that's fine since then the
+	 * bt_mesh_lpn_waiting_update() function will return false:
+	 * we still need to go through the actual sending to the bearer and
+	 * wait for ReceiveDelay before transitioning to WAIT_UPDATE state.
+	 * Another situation where we want to notify the LPN state machine
+	 * is if it's configured to use an automatic Friendship establishment
+	 * timer, in which case we want to reset the timer at this point.
+	 *
+	 */
+	if (IS_ENABLED(CONFIG_BT_MESH_LOW_POWER) &&
+	    (bt_mesh_lpn_timer() ||
+	     (bt_mesh_lpn_established() && bt_mesh_lpn_waiting_update()))) {
+		bt_mesh_lpn_msg_received(rx);
+	}
+
+	net_buf_simple_restore(buf, &state);
+
+	if (IS_ENABLED(CONFIG_BT_MESH_FRIEND) && rx->friend_match && !err) {
+		if (seq_auth == TRANS_SEQ_AUTH_NVAL) {
+			bt_mesh_friend_enqueue_rx(rx, pdu_type, NULL,
+						  seg_count, buf);
+		} else {
+			bt_mesh_friend_enqueue_rx(rx, pdu_type, &seq_auth,
+						  seg_count, buf);
+		}
+	}
+
+	return err;
+}
+
+void bt_mesh_rx_reset(void)
+{
+	int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(seg_rx); i++) {
+		seg_rx_reset(&seg_rx[i], true);
+	}
+
+	if (IS_ENABLED(CONFIG_BT_SETTINGS)) {
+		bt_mesh_clear_rpl();
+	} else {
+		memset(bt_mesh.rpl, 0, sizeof(bt_mesh.rpl));
+	}
+}
+
+void bt_mesh_tx_reset(void)
+{
+	int i;
+
+	BT_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(seg_tx); i++) {
+		seg_tx_reset(&seg_tx[i]);
+	}
+}
+
+void bt_mesh_trans_init(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(seg_tx); i++) {
+		k_delayed_work_init(&seg_tx[i].retransmit, seg_retransmit);
+		k_delayed_work_add_arg(&seg_tx[i].retransmit, &seg_tx[i]);
+	}
+
+	/* XXX Probably we need mempool for that.
+	 *  For now we increase MSYS_1_BLOCK_COUNT
+	 */
+	for (i = 0; i < ARRAY_SIZE(seg_rx); i++) {
+	    seg_rx[i].buf = NET_BUF_SIMPLE(MYNEWT_VAL(BLE_MESH_RX_SDU_MAX));
+		k_delayed_work_init(&seg_rx[i].ack, seg_ack);
+		k_delayed_work_add_arg(&seg_rx[i].ack, &seg_rx[i]);
+	}
+}
+
+void bt_mesh_rpl_clear(void)
+{
+	BT_DBG("");
+	memset(bt_mesh.rpl, 0, sizeof(bt_mesh.rpl));
+}
+
+void bt_mesh_heartbeat_send(void)
+{
+	struct bt_mesh_cfg_srv *cfg = bt_mesh_cfg_get();
+	u16_t feat = 0U;
+	struct __packed {
+		u8_t  init_ttl;
+		u16_t feat;
+	} hb;
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = cfg->hb_pub.net_idx,
+		.app_idx = BT_MESH_KEY_UNUSED,
+		.addr = cfg->hb_pub.dst,
+		.send_ttl = cfg->hb_pub.ttl,
+	};
+	struct bt_mesh_net_tx tx = {
+		.sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx),
+		.ctx = &ctx,
+		.src = bt_mesh_model_elem(cfg->model)->addr,
+		.xmit = bt_mesh_net_transmit_get(),
+	};
+
+	/* Do nothing if heartbeat publication is not enabled */
+	if (cfg->hb_pub.dst == BT_MESH_ADDR_UNASSIGNED) {
+		return;
+	}
+
+	hb.init_ttl = cfg->hb_pub.ttl;
+
+	if (bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED) {
+		feat |= BT_MESH_FEAT_RELAY;
+	}
+
+	if (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED) {
+		feat |= BT_MESH_FEAT_PROXY;
+	}
+
+	if (bt_mesh_friend_get() == BT_MESH_FRIEND_ENABLED) {
+		feat |= BT_MESH_FEAT_FRIEND;
+	}
+
+	if (bt_mesh_lpn_established()) {
+		feat |= BT_MESH_FEAT_LOW_POWER;
+	}
+
+	hb.feat = sys_cpu_to_be16(feat);
+
+	BT_DBG("InitTTL %u feat 0x%04x", cfg->hb_pub.ttl, feat);
+
+	bt_mesh_ctl_send(&tx, TRANS_CTL_OP_HEARTBEAT, &hb, sizeof(hb),
+			 NULL, NULL, NULL);
+}
+
+int bt_mesh_app_key_get(const struct bt_mesh_subnet *subnet, u16_t app_idx,
+			u16_t addr, const u8_t **key, u8_t *aid)
+{
+	struct bt_mesh_app_key *app_key;
+
+	if (app_idx == BT_MESH_KEY_DEV_LOCAL ||
+	    (app_idx == BT_MESH_KEY_DEV_REMOTE &&
+	     bt_mesh_elem_find(addr) != NULL)) {
+		*aid = 0;
+		*key = bt_mesh.dev_key;
+		return 0;
+	} else if (app_idx == BT_MESH_KEY_DEV_REMOTE) {
+		if (!IS_ENABLED(CONFIG_BT_MESH_PROVISIONER)) {
+			return -EINVAL;
+		}
+
+		struct bt_mesh_node *node = bt_mesh_node_find(addr);
+		if (!node) {
+			return -EINVAL;
+		}
+
+		*key = node->dev_key;
+		*aid = 0;
+		return 0;
+	}
+
+	if (!subnet) {
+		return -EINVAL;
+	}
+
+	app_key = bt_mesh_app_key_find(app_idx);
+	if (!app_key) {
+		return -ENOENT;
+	}
+
+	if (subnet->kr_phase == BT_MESH_KR_PHASE_2 && app_key->updated) {
+		*key = app_key->keys[1].val;
+		*aid = app_key->keys[1].id;
+	} else {
+		*key = app_key->keys[0].val;
+		*aid = app_key->keys[0].id;
+	}
+
+	return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/src/transport.h b/src/libs/mynewt-nimble/nimble/host/mesh/src/transport.h
new file mode 100644
index 00000000..eff768e9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/src/transport.h
@@ -0,0 +1,105 @@
+/*  Bluetooth Mesh */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+#include "mesh/mesh.h"
+
+#define TRANS_SEQ_AUTH_NVAL 0xffffffffffffffff
+
+#define BT_MESH_TX_SDU_MAX             (CONFIG_BT_MESH_TX_SEG_MAX * 12)
+
+#define TRANS_SEQ_ZERO_MASK            ((u16_t)BIT_MASK(13))
+#define TRANS_CTL_OP_MASK              ((u8_t)BIT_MASK(7))
+#define TRANS_CTL_OP(data)             ((data)[0] & TRANS_CTL_OP_MASK)
+#define TRANS_CTL_HDR(op, seg)         ((op & TRANS_CTL_OP_MASK) | (seg << 7))
+
+#define TRANS_CTL_OP_ACK               0x00
+#define TRANS_CTL_OP_FRIEND_POLL       0x01
+#define TRANS_CTL_OP_FRIEND_UPDATE     0x02
+#define TRANS_CTL_OP_FRIEND_REQ        0x03
+#define TRANS_CTL_OP_FRIEND_OFFER      0x04
+#define TRANS_CTL_OP_FRIEND_CLEAR      0x05
+#define TRANS_CTL_OP_FRIEND_CLEAR_CFM  0x06
+#define TRANS_CTL_OP_FRIEND_SUB_ADD    0x07
+#define TRANS_CTL_OP_FRIEND_SUB_REM    0x08
+#define TRANS_CTL_OP_FRIEND_SUB_CFM    0x09
+#define TRANS_CTL_OP_HEARTBEAT         0x0a
+
+struct bt_mesh_ctl_friend_poll {
+	u8_t  fsn;
+}__attribute__((__packed__));
+
+struct bt_mesh_ctl_friend_update {
+	u8_t  flags;
+	u32_t iv_index;
+	u8_t  md;
+}__attribute__((__packed__));
+
+struct bt_mesh_ctl_friend_req {
+	u8_t  criteria;
+	u8_t  recv_delay;
+	u8_t  poll_to[3];
+	u16_t prev_addr;
+	u8_t  num_elem;
+	u16_t lpn_counter;
+}__attribute__((__packed__));
+
+struct bt_mesh_ctl_friend_offer {
+	u8_t  recv_win;
+	u8_t  queue_size;
+	u8_t  sub_list_size;
+	s8_t  rssi;
+	u16_t frnd_counter;
+}__attribute__((__packed__));
+
+struct bt_mesh_ctl_friend_clear {
+	u16_t lpn_addr;
+	u16_t lpn_counter;
+}__attribute__((__packed__));
+
+struct bt_mesh_ctl_friend_clear_confirm {
+	u16_t lpn_addr;
+	u16_t lpn_counter;
+}__attribute__((__packed__));
+
+#define BT_MESH_FRIEND_SUB_MIN_LEN (1 + 2)
+struct bt_mesh_ctl_friend_sub {
+	u8_t  xact;
+	u16_t addr_list[5];
+}__attribute__((__packed__));
+
+struct bt_mesh_ctl_friend_sub_confirm {
+	u8_t  xact;
+}__attribute__((__packed__));
+
+void bt_mesh_set_hb_sub_dst(u16_t addr);
+
+struct bt_mesh_app_key *bt_mesh_app_key_find(u16_t app_idx);
+
+bool bt_mesh_tx_in_progress(void);
+
+void bt_mesh_rx_reset(void);
+void bt_mesh_tx_reset(void);
+
+int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data,
+		     size_t data_len, u64_t *seq_auth,
+		     const struct bt_mesh_send_cb *cb, void *cb_data);
+
+int bt_mesh_trans_send(struct bt_mesh_net_tx *tx, struct os_mbuf *msg,
+		       const struct bt_mesh_send_cb *cb, void *cb_data);
+
+int bt_mesh_trans_recv(struct os_mbuf *buf, struct bt_mesh_net_rx *rx);
+
+void bt_mesh_trans_init(void);
+
+void bt_mesh_rpl_clear(void);
+
+void bt_mesh_heartbeat_send(void);
+
+int bt_mesh_app_key_get(const struct bt_mesh_subnet *subnet, u16_t app_idx,
+			u16_t addr, const u8_t **key, u8_t *aid);
diff --git a/src/libs/mynewt-nimble/nimble/host/mesh/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/mesh/syscfg.yml
new file mode 100644
index 00000000..98632232
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/mesh/syscfg.yml
@@ -0,0 +1,661 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_MESH_PROV:
+        description: >
+            Enable provisioning. It is automatically enabled whenever
+            BLE_MESH_PB_ADV or BLE_MESH_PB_GATT is set.
+        value: 1
+
+    BLE_MESH_PB_ADV:
+        description: >
+           Enable this option to allow the device to be provisioned over
+           the advertising bearer.
+        value: 1
+
+    BLE_MESH_PROVISIONER:
+        description: >
+            Enable this option to have support for provisioning remote devices.
+        value: 0
+        restrictions:
+            - (BLE_MESH_PROV)
+
+    BLE_MESH_NODE_COUNT:
+        description: >
+            This option specifies how many nodes each network can at most
+            save in the provisioning database. Range 1-4096
+        value: 1
+
+    BLE_MESH_PROXY:
+        description: >
+           Enable proxy. This is automatically set whenever BLE_MESH_PB_GATT or
+           BLE_MESH_GATT_PROXY is set.
+        value: 0
+
+    BLE_MESH_PB_GATT:
+        description: >
+           Enable this option to allow the device to be provisioned over
+           the GATT bearer.
+        value: 1
+
+    BLE_MESH_GATT_PROXY:
+        description: >
+            This option enables support for the Mesh GATT Proxy Service,
+            i.e. the ability to act as a proxy between a Mesh GATT Client
+            and a Mesh network.
+        value: 1
+
+    BLE_MESH_NODE_ID_TIMEOUT:
+        description: >
+            This option determines for how long the local node advertises
+            using Node Identity. The given value is in seconds. The
+            specification limits this to 60 seconds, and implies that to
+            be the appropriate value as well, so just leaving this as the
+            default is the safest option.
+        value: 60
+
+    BLE_MESH_PROXY_FILTER_SIZE:
+        descryption: >
+            This option specifies how many Proxy Filter entries the local
+            node supports.
+        value: 1
+
+    BLE_MESH_SUBNET_COUNT:
+        description: >
+            This option specifies how many subnets a Mesh network can
+            participate in at the same time.
+        value: 1
+
+    BLE_MESH_APP_KEY_COUNT:
+        description: >
+            This option specifies how many application keys the device can
+            store per network.
+        value: 1
+
+    BLE_MESH_MODEL_KEY_COUNT:
+        description: >
+             This option specifies how many application keys each model can
+             at most be bound to.
+        value: 1
+
+    BLE_MESH_MODEL_GROUP_COUNT:
+        description: >
+            This option specifies how many group addresses each model can
+            at most be subscribed to.
+        value: 1
+
+    BLE_MESH_LABEL_COUNT:
+        description: >
+            This option specifies how many Label UUIDs can be stored.
+        value: 1
+
+    BLE_MESH_CRPL:
+        description: >
+            This options specifies the maximum capacity of the replay
+            protection list. This option is similar to the network message
+            cache size, but has a different purpose.
+        value: 10
+
+    BLE_MESH_ADV_TASK_PRIO:
+        description: >
+            Advertising task prio (FIXME)
+        type: task_priority
+        value: 9
+
+    BLE_MESH_MSG_CACHE_SIZE:
+        description: >
+            Number of messages that are cached for the network. This description
+            prevent unnecessary decryption operations and unnecessary
+            relays. This option is similar to the replay protection list,
+            but has a different purpose.
+        value: 10
+
+    BLE_MESH_ADV_BUF_COUNT:
+        description: >
+            Number of advertising buffers available. This should be chosen
+            based on what kind of features the local node shoule have. E.g.
+            a relay will perform better the more buffers it has. Another
+            thing to consider is outgoing segmented messages. There must
+            be at least three more advertising buffers than the maximum
+            supported outgoing segment count (BT_MESH_TX_SEG_MAX).
+        value: 6
+
+    BLE_MESH_IVU_DIVIDER:
+        description: >
+            When the IV Update state enters Normal operation or IV Update
+            in Progress, we need to keep track of how many hours has passed
+            in the state, since the specification requires us to remain in
+            the state at least for 96 hours (Update in Progress has an
+            additional upper limit of 144 hours).
+
+            In order to fulfil the above requirement, even if the node might
+            be powered off once in a while, we need to store persistently
+            how many hours the node has been in the state. This doesn't
+            necessarily need to happen every hour (thanks to the flexible
+            duration range). The exact cadence will depend a lot on the
+            ways that the node will be used and what kind of power source it
+            has.
+
+            Since there is no single optimal answer, this configuration
+            option allows specifying a divider, i.e. how many intervals
+            the 96 hour minimum gets split into. After each interval the
+            duration that the node has been in the current state gets
+            stored to flash. E.g. the default value of 4 means that the
+            state is saved every 24 hours (96 / 4).
+        value: 4
+
+    BLE_MESH_TX_SEG_MSG_COUNT:
+        description: >
+            Maximum number of simultaneous outgoing multi-segment and/or
+            reliable messages.
+        value: 4
+
+    BLE_MESH_RX_SEG_MSG_COUNT:
+        description: >
+            Maximum number of simultaneous incoming multi-segment and/or
+            reliable messages.
+        value: 2
+
+    BLE_MESH_RX_SDU_MAX:
+        description: >
+            Maximum incoming Upper Transport Access PDU length. This
+            determines also how many segments incoming segmented messages
+            can have. Each segment can contain 12 bytes, so this value should
+            be set to a multiple of 12 to avoid wasted memory. The minimum
+            requirement is 2 segments (24 bytes) whereas the maximum supported
+            by the Mesh specification is 32 segments (384 bytes).
+        value: 72
+
+    BLE_MESH_TX_SEG_MAX:
+        description: >
+            Maximum number of segments supported for outgoing messages.
+            This value should typically be fine-tuned based on what
+            models the local node supports, i.e. what's the largest
+            message payload that the node needs to be able to send.
+            This value affects memory and call stack consumption, which
+            is why the default is lower than the maximum that the
+            specification would allow (32 segments).
+
+            The maximum outgoing SDU size is 12 times this number (out of
+            which 4 or 8 bytes is used for the Transport Layer MIC). For
+            example, 5 segments means the maximum SDU size is 60 bytes,
+            which leaves 56 bytes for application layer data using a
+            4-byte MIC and 52 bytes using an 8-byte MIC.
+
+            Be sure to specify a sufficient number of advertising buffers
+            when setting this option to a higher value. There must be at
+            least three more advertising buffers (BT_MESH_ADV_BUF_COUNT)
+            as there are outgoing segments.
+        value: 3
+
+    BLE_MESH_SEG_RETRANSMIT_ATTEMPTS:
+        description: >
+            Number of retransmit attempts (after the initial transmit) per segment
+        value: 4
+        retrictions: 'BLE_MESH_SEG_RETRANSMIT_ATTEMPTS > 1'
+
+    BLE_MESH_RELAY:
+        description: >
+            Support for acting as a Mesh Relay Node.
+        value: 0
+
+    BLE_MESH_LOW_POWER:
+        description: >
+           Enable this option to be able to act as a Low Power Node.
+        value: 0
+
+    BLE_MESH_LPN_ESTABLISHMENT:
+        description: >
+           Perform the Friendship establishment using low power, with
+           the help of a reduced scan duty cycle. The downside of this
+           is that the node may miss out on messages intended for it
+           until it has successfully set up Friendship with a Friend
+           node.
+        value: 1
+
+    BLE_MESH_LPN_AUTO:
+        description: >
+           Automatically enable LPN functionality once provisioned and start
+           looking for Friend nodes. If this option is disabled LPN mode
+           needs to be manually enabled by calling bt_mesh_lpn_set(true).
+           node.
+        value: 1
+
+    BLE_MESH_LPN_AUTO_TIMEOUT:
+        description: >
+           Time in seconds from the last received message, that the node
+           will wait before starting to look for Friend nodes.
+        value: 15
+
+    BLE_MESH_LPN_RETRY_TIMEOUT:
+        description: >
+           Time in seconds between Friend Requests, if a previous Friend
+           Request did not receive any acceptable Friend Offers.
+        value: 8
+
+    BLE_MESH_LPN_RSSI_FACTOR:
+        description: >
+            The contribution of the RSSI measured by the Friend node used
+            in Friend Offer Delay calculations. 0 = 1, 1 = 1.5, 2 = 2, 3 = 2.5.
+        value: 0
+
+    BLE_MESH_LPN_RECV_WIN_FACTOR:
+        description: >
+           The contribution of the supported Receive Window used in
+           Friend Offer Delay calculations. 0 = 1, 1 = 1.5, 2 = 2, 3 = 2.5.
+        value: 0
+
+    BLE_MESH_LPN_MIN_QUEUE_SIZE:
+        description: >
+           The MinQueueSizeLog field is defined as log_2(N), where N is
+           the minimum number of maximum size Lower Transport PDUs that
+           the Friend node can store in its Friend Queue. As an example,
+           MinQueueSizeLog value 1 gives N = 2, and value 7 gives N = 128.
+        value: 1
+
+    BLE_MESH_LPN_RECV_DELAY:
+        description: >
+            The ReceiveDelay is the time between the Low Power node
+            sending a request and listening for a response. This delay
+            allows the Friend node time to prepare the response. The value
+            is in units of milliseconds.
+        value: 100
+
+    BLE_MESH_LPN_POLL_TIMEOUT:
+        description: >
+            PollTimeout timer is used to measure time between two
+            consecutive requests sent by the Low Power node. If no
+            requests are received by the Friend node before the
+            PollTimeout timer expires, then the friendship is considered
+            terminated. The value is in units of 100 milliseconds, so e.g.
+            a value of 300 means 30 seconds.
+        value: 300
+
+    BLE_MESH_LPN_INIT_POLL_TIMEOUT:
+        description: >
+            The initial value of the PollTimeout timer when Friendship
+            gets established for the first time. After this the timeout
+            will gradually grow toward the actual PollTimeout, doubling
+            in value for each iteration. The value is in units of 100
+            milliseconds, so e.g. a value of 300 means 3 seconds.
+        value: MYNEWT_VAL_BLE_MESH_LPN_POLL_TIMEOUT
+
+    BLE_MESH_LPN_SCAN_LATENCY:
+        description: >
+            Latency in milliseconds that it takes to enable scanning. This
+            is in practice how much time in advance before the Receive Window
+            that scanning is requested to be enabled.
+        value: 10
+
+    BLE_MESH_LPN_GROUPS:
+        description: >
+            Maximum number of groups that the LPN can subscribe to.
+        value: 10
+
+    BLE_MESH_FRIEND:
+        description: >
+            Enable this option to be able to act as a Friend Node.
+        value: 0
+
+    BLE_MESH_FRIEND_RECV_WIN:
+        description: >
+            Receive Window in milliseconds supported by the Friend node.
+        value: 255
+
+    BLE_MESH_FRIEND_QUEUE_SIZE:
+        description: >
+            Minimum number of buffers available to be stored for each
+            local Friend Queue.
+        value: 16
+
+    BLE_MESH_FRIEND_SUB_LIST_SIZE:
+        description: >
+            Size of the Subscription List that can be supported by a
+            Friend node for a Low Power node.
+        value: 3
+
+    BLE_MESH_FRIEND_LPN_COUNT:
+        description: >
+            Number of Low Power Nodes the Friend can have a Friendship
+            with simultaneously.
+        value: 2
+
+    BLE_MESH_FRIEND_SEG_RX:
+        description: >
+            Number of incomplete segment lists that we track for each LPN
+            that we are Friends for. In other words, this determines how
+            many elements we can simultaneously be receiving segmented
+            messages from when the messages are going into the Friend queue.
+        value: 1
+
+    BLE_MESH_CFG_CLI:
+        description: >
+            Enable support for the configuration client model.
+        value: 0
+
+    BLE_MESH_HEALTH_CLI:
+        description: >
+            Enable support for the health client model.
+        value: 0
+
+    BLE_MESH_SHELL:
+        description: >
+            Activate shell module that provides Bluetooth Mesh commands to
+            the console.
+        value: 0
+
+    BLE_MESH_MODEL_EXTENSIONS:
+        description: >
+            Enable support for the model extension concept, allowing the Access
+            layer to know about Mesh model relationships.
+        value: 0
+
+    BLE_MESH_IV_UPDATE_TEST:
+        description: >
+            This option removes the 96 hour limit of the IV Update
+            Procedure and lets the state be changed at any time.
+        value: 0
+
+    BLE_MESH_TESTING:
+        description: >
+            This option enables testing API.
+        value: 0
+
+    BLE_MESH_DEV_UUID:
+        description: >
+            Device UUID
+        value: ((uint8_t[16]){0x11, 0x22, 0})
+
+    BLE_MESH_SHELL_MODELS:
+        description: >
+            Include implementation of some demo models.
+        value: 0
+
+    BLE_MESH_OOB_OUTPUT_ACTIONS:
+        description: >
+            Supported Output OOB Actions
+            BT_MESH_NO_OUTPUT       = 0,
+            BT_MESH_BLINK           = BIT(0)
+            BT_MESH_BEEP            = BIT(1)
+            BT_MESH_VIBRATE         = BIT(2)
+            BT_MESH_DISPLAY_NUMBER  = BIT(3)
+            BT_MESH_DISPLAY_STRING  = BIT(4)
+        value: ((BT_MESH_DISPLAY_NUMBER))
+
+    BLE_MESH_OOB_OUTPUT_SIZE:
+        description: >
+            Output OOB size
+        value: 4
+
+    BLE_MESH_OOB_INPUT_ACTIONS:
+        description: >
+            Supported Input OOB Actions
+            BT_MESH_NO_INPUT      = 0,
+            BT_MESH_PUSH          = BIT(0)
+            BT_MESH_TWIST         = BIT(1)
+            BT_MESH_ENTER_NUMBER  = BIT(2)
+            BT_MESH_ENTER_STRING  = BIT(3)
+        value: ((BT_MESH_NO_INPUT))
+
+    BLE_MESH_OOB_INPUT_SIZE:
+        description: >
+            Input OOB size
+        value: 4
+
+    BLE_MESH_SETTINGS:
+        description: >
+            This option enables Mesh settings storage.
+        value: 1
+
+    BLE_MESH_STORE_TIMEOUT:
+        description: >
+            This value defines in seconds how soon any pending changes
+            are actually written into persistent storage (flash) after
+            a change occurs.
+        value: 2
+
+    BLE_MESH_SEQ_STORE_RATE:
+        description: >
+            This value defines how often the local sequence number gets
+            updated in persistent storage (i.e. flash). E.g. a value of 100
+            means that the sequence number will be stored to flash on every
+            100th increment. If the node sends messages very frequently a
+            higher value makes more sense, whereas if the node sends
+            infrequently a value as low as 0 (update storage for every
+            increment) can make sense. When the stack gets initialized it
+            will add this number to the last stored one, so that it starts
+            off with a value that's guaranteed to be larger than the last
+            one used before power off.
+        value: 128
+
+    BLE_MESH_RPL_STORE_TIMEOUT:
+        description: >
+            This value defines in seconds how soon the RPL gets written to
+            persistent storage after a change occurs. If the node receives
+            messages frequently it may make sense to have this set to a
+            large value, whereas if the RPL gets updated infrequently a
+            value as low as 0 (write immediately) may make sense. Note that
+            if the node operates a security sensitive use case, and there's
+            a risk of sudden power loss, it may be a security vulnerability
+            to set this value to anything else than 0 (a power loss before
+            writing to storage exposes the node to potential message
+            replay attacks).
+        value: 5
+
+    BLE_MESH_DEVICE_NAME:
+        description: >
+            This value defines BLE Mesh device/node name.
+        value: '"nimble-mesh-node"'
+
+    BLE_MESH_SYSINIT_STAGE:
+        description: >
+            Primary sysinit stage for BLE mesh functionality.
+        value: 500
+
+    BLE_MESH_SYSINIT_STAGE_SHELL:
+        description: >
+            Secondary sysinit stage for BLE mesh functionality.
+        value: 1000
+
+    ### Log settings.
+
+    BLE_MESH_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh log messages.
+        value: 9
+    BLE_MESH_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh log.
+        value: 1
+
+    BLE_MESH_ACCESS_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Access-related log messages.
+        value: 10
+    BLE_MESH_ACCESS_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Access-related log.
+        value: 1
+
+    BLE_MESH_ADV_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh advertising log messages.
+        value: 11
+    BLE_MESH_ADV_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh log.
+        value: 1
+
+    BLE_MESH_BEACON_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Beacon-related log messages.
+        value: 12
+    BLE_MESH_BEACON_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Beacon-related log.
+        value: 1
+
+    BLE_MESH_CRYPTO_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh cryptographic log messages.
+        value: 13
+    BLE_MESH_CRYPTO_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh cryptographic log.
+        value: 1
+
+    BLE_MESH_FRIEND_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Friend log messages.
+        value: 14
+    BLE_MESH_FRIEND_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Friend log.
+        value: 1
+
+    BLE_MESH_LOW_POWER_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Low Power log messages.
+        value: 15
+    BLE_MESH_LOW_POWER_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Low Power log.
+        value: 1
+
+    BLE_MESH_MODEL_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Foundation Models log messages.
+        value: 16
+    BLE_MESH_MODEL_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Foundation Models log.
+        value: 1
+
+    BLE_MESH_NET_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Network layer log messages.
+        value: 17
+    BLE_MESH_NET_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Network layer log.
+        value: 1
+
+    BLE_MESH_PROV_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Provisioning log messages.
+        value: 18
+    BLE_MESH_PROV_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Provisioning log.
+        value: 1
+
+    BLE_MESH_PROXY_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Proxy protocol log messages.
+        value: 19
+    BLE_MESH_PROXY_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Proxy protocol log.
+        value: 1
+
+    BLE_MESH_SETTINGS_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh persistent settings log messages.
+        value: 20
+    BLE_MESH_SETTINGS_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh persistent settings log.
+        value: 1
+
+    BLE_MESH_TRANS_LOG_MOD:
+        description: >
+            Numeric module ID to use for BLE Mesh Transport Layer log messages.
+        value: 21
+    BLE_MESH_TRANS_LOG_LVL:
+        description: >
+            Minimum level for the BLE Mesh Transport Layer log.
+        value: 1
+
+syscfg.logs:
+    BLE_MESH_LOG:
+        module: MYNEWT_VAL(BLE_MESH_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_LOG_LVL)
+
+    BLE_MESH_ACCESS_LOG:
+        module: MYNEWT_VAL(BLE_MESH_ACCESS_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_ACCESS_LOG_LVL)
+
+    BLE_MESH_ADV_LOG:
+        module: MYNEWT_VAL(BLE_MESH_ADV_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_ADV_LOG_LVL)
+
+    BLE_MESH_BEACON_LOG:
+        module: MYNEWT_VAL(BLE_MESH_BEACON_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_BEACON_LOG_LVL)
+
+    BLE_MESH_CRYPTO_LOG:
+        module: MYNEWT_VAL(BLE_MESH_CRYPTO_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_CRYPTO_LOG_LVL)
+
+    BLE_MESH_FRIEND_LOG:
+        module: MYNEWT_VAL(BLE_MESH_FRIEND_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_FRIEND_LOG_LVL)
+
+    BLE_MESH_LOW_POWER_LOG:
+        module: MYNEWT_VAL(BLE_MESH_LOW_POWER_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_LOW_POWER_LOG_LVL)
+
+    BLE_MESH_MODEL_LOG:
+        module: MYNEWT_VAL(BLE_MESH_MODEL_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_MODEL_LOG_LVL)
+
+    BLE_MESH_NET_LOG:
+        module: MYNEWT_VAL(BLE_MESH_NET_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_NET_LOG_LVL)
+
+    BLE_MESH_PROV_LOG:
+        module: MYNEWT_VAL(BLE_MESH_PROV_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_PROV_LOG_LVL)
+
+    BLE_MESH_PROXY_LOG:
+        module: MYNEWT_VAL(BLE_MESH_PROXY_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_PROXY_LOG_LVL)
+
+    BLE_MESH_SETTINGS_LOG:
+        module: MYNEWT_VAL(BLE_MESH_SETTINGS_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_SETTINGS_LOG_LVL)
+
+    BLE_MESH_TRANS_LOG:
+        module: MYNEWT_VAL(BLE_MESH_TRANS_LOG_MOD)
+        level: MYNEWT_VAL(BLE_MESH_TRANS_LOG_LVL)
+
+syscfg.vals.BLE_MESH_SHELL:
+    BLE_MESH_CFG_CLI: 1
+    BLE_MESH_HEALTH_CLI: 1
+    BLE_MESH_IV_UPDATE_TEST: 1
+
+syscfg.vals.BLE_MESH_GATT_PROXY:
+    BLE_MESH_PROXY: 1
+
+syscfg.vals.BLE_MESH_PB_GATT:
+    BLE_MESH_PROXY: 1
+    BLE_MESH_PROV: 1
+
+syscfg.vals.BLE_MESH_PB_ADV:
+    BLE_MESH_PROV: 1
diff --git a/src/libs/mynewt-nimble/nimble/host/pkg.yml b/src/libs/mynewt-nimble/nimble/host/pkg.yml
new file mode 100644
index 00000000..a063a0b6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pkg.yml
@@ -0,0 +1,55 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host
+pkg.description: Host side of the nimble Bluetooth Smart stack.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - "@apache-mynewt-core/kernel/os"
+    - "@apache-mynewt-core/sys/log/modlog"
+    - "@apache-mynewt-core/util/mem"
+    - nimble
+
+pkg.deps.BLE_SM_LEGACY:
+    - "@apache-mynewt-core/crypto/tinycrypt"
+
+pkg.deps.BLE_SM_SC:
+    - "@apache-mynewt-core/crypto/tinycrypt"
+
+pkg.deps.BLE_MONITOR_RTT:
+    - "@apache-mynewt-core/hw/drivers/rtt"
+
+pkg.deps.BLE_MESH:
+    - nimble/host/mesh
+
+pkg.req_apis:
+    - ble_transport
+    - console
+    - stats
+
+pkg.init:
+    ble_hs_init: 'MYNEWT_VAL(BLE_HS_SYSINIT_STAGE)'
+
+pkg.down.BLE_HS_STOP_ON_SHUTDOWN:
+    ble_hs_shutdown: 200
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/README.txt b/src/libs/mynewt-nimble/nimble/host/pts/README.txt
new file mode 100644
index 00000000..bb03b18c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/README.txt
@@ -0,0 +1,8 @@
+This folder contains qualification tests results against BT SIG Profile Test
+Suite.
+
+pts-FOO.txt files contain result for specific profiles or protocols. This
+includes PTS version, test date, enabled tests, results etc.
+
+In addition to tests results 'tpg' folder constains Test Plang Generator
+configuration files that can be imported by PTS for tests configuration.
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/pts-gap.txt b/src/libs/mynewt-nimble/nimble/host/pts/pts-gap.txt
new file mode 100644
index 00000000..29ed2446
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/pts-gap.txt
@@ -0,0 +1,367 @@
+PTS test results for GAP
+
+PTS version: 7.5.0
+Tested: 27-Sept-2019
+
+Results:
+PASS	test passed
+FAIL	test failed
+INC	test is inconclusive
+N/A	test is disabled due to PICS setup
+
+-------------------------------------------------------------------------------
+Test Name		Result	Notes
+-------------------------------------------------------------------------------
+
+GAP/BROB/BCST/BV-01-C	PASS	advertise-configure legacy=1 connectable=0 scannable=0
+GAP/BROB/BCST/BV-02-C	PASS	advertise-configure legacy=1 connectable=0 scannable=0
+
+GAP/BROB/BCST/BV-03-C	PASS	set irk=<IRK> e.g: 00:00:00:00:00:00:00:00:00:00:00:00:00:00:00:11
+				Note: in PTS IXIT please set:
+				TSPX_iut_device_IRK_for_resolvable_privacy_address_generation_procedure=11000000000000000000000000000000
+				set advertise-set-adv-data name=<name> flags=4
+				advertise-configure connectable=0 scannable=0 own_addr_type=rpa_pub
+GAP/BROB/BCST/BV-04-C	PASS    TSPX_advertising_data=07086E696D626C65
+				advertise-set-adv-data name=nimble
+				set addr_type=random addr=01:3e:56:f7:46:21
+				advertise-configure connectable=0 scannable=0 own_addr_type=random
+GAP/BROB/BCST/BV-05-C	N/A
+GAP/BROB/OBSV/BV-01-C	PASS	scan passive
+GAP/BROB/OBSV/BV-02-C	PASS	scan
+GAP/BROB/OBSV/BV-03-C	PASS	scan
+GAP/BROB/OBSV/BV-04-C	PASS	connect peer_addr=<addr>
+				security-set-data bonding=1
+				security-pair conn=<handle>
+				<OK>
+				<OK>
+GAP/BROB/OBSV/BV-05-C	PASS	scan own_addr_type=rpa_pub
+GAP/BROB/OBSV/BV-06-C	PASS	scan own_addr_type=rpa_pub
+-------------------------------------------------------------------------------
+
+GAP/DISC/NONM/BV-01-C	PASS	advertise-configure connectable=0 legacy=1 adverdise=non
+GAP/DISC/NONM/BV-02-C	PASS	advertise-configure connectable=0
+
+GAP/DISC/LIMM/BV-01-C	N/A
+GAP/DISC/LIMM/BV-02-C	N/A
+GAP/DISC/LIMM/BV-03-C	PASS	advertise-configure legacy=1 connectable=0
+				advertise-set-adv-data flags=5
+				advertise-start duration= e.g.3000
+GAP/DISC/LIMM/BV-04-C	PASS	advertise-configure legacy=1 connectable=0
+				advertise-set-adv-data flags=5
+				advertising-start duration=<e.g.3000>
+GAP/DISC/GENM/BV-01-C	N/A
+GAP/DISC/GENM/BV-02-C	N/A
+GAP/DISC/GENM/BV-03-C	PASS	advertise-configure legacy=1 connectable=0
+				advertise-set-adv-data flags=6
+				advertise-start
+GAP/DISC/GENM/BV-04-C	PASS	advertise-configure legacy=1 connectable=0
+				advertise-set-adv-data flags=6
+				advertising-start
+
+GAP/DISC/LIMP/BV-01-C	PASS	scan limited=1 nodups=1
+GAP/DISC/LIMP/BV-02-C	PASS	scan limited=1 nodups=1
+GAP/DISC/LIMP/BV-03-C	PASS	scan limited=1 nodups=1
+GAP/DISC/LIMP/BV-04-C	PASS	scan limited=1 nodups=1
+GAP/DISC/LIMP/BV-05-C	PASS	scan limited=1 nodups=1
+
+GAP/DISC/GENP/BV-01-C	PASS	scan nodups=1
+GAP/DISC/GENP/BV-02-C	PASS	scan nodups=1
+GAP/DISC/GENP/BV-03-C	PASS	scan nodups=1
+				<OK>
+GAP/DISC/GENP/BV-04-C	PASS	scan nodups=1
+				<OK>
+GAP/DISC/GENP/BV-05-C	PASS	scan nodups=1
+
+GAP/DISC/RPA/BV-01-C	N/A		scan nodups=1
+-------------------------------------------------------------------------------
+
+GAP/IDLE/GIN/BV-01-C	N/A
+GAP/IDLE/GIN/BV-02-C	N/A
+GAP/IDLE/NAMP/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertising-start
+				gatt-discover-full conn=<handle>
+				gatt-show
+				<check start end handle for 0x1800>
+				gatt-read conn=<handle> uuid=0x2a00 start=<start> end=<end>
+				disconnect conn=<handle>
+GAP/IDLE/NAMP/BV-02-C	PASS	<answer NO to role question>
+				advertise-configure connectable=1 legacy=1
+				advertising-start
+GAP/IDLE/DED/BV-01-C	N/A
+GAP/IDLE/DED/BV-02-C	N/A
+-------------------------------------------------------------------------------
+
+GAP/CONN/NCON/BV-01-C	PASS	advertise-configure connectable=0 legacy=1
+				advertising-start
+GAP/CONN/NCON/BV-02-C	PASS	advertise-configure connectable=0 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+GAP/CONN/NCON/BV-03-C	PASS	advertise-configure connectable=0 legacy=1
+				advertise-set-adv-data flags=5
+				advertise-start
+
+GAP/CONN/DCON/BV-01-C	PASS	advertise-configure connectable=0 directed=1 peer_addr=<addr>
+				advertise-start
+GAP/CONN/DCON/BV-02-C	N/A
+GAP/CONN/DCON/BV-03-C	N/A
+
+GAP/CONN/UCON/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=4
+				advertise-start
+GAP/CONN/UCON/BV-02_C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=5
+				advertise-start
+GAP/CONN/UCON/BV-03_C	PASS	adbertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+GAP/CONN/UCON/BV-04_C	N/A
+GAP/CONN/UCON/BV-05_C	N/A
+GAP/CONN/UCON/BV-06_C	N/A
+
+GAP/CONN/ACEP/BV-01-C	PASS	white-list addr_type=public addr=<addr>
+				connect
+				disconnect conn=<handle>
+GAP/CONN/ACEP/BV-02-C	N/A
+
+GAP/CONN/GCEP/BV-01-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+GAP/CONN/GCEP/BV-02-C	PASS	connect peer_addr=<addr>
+GAP/CONN/GCEP/BV-03-C	PASS	set irk=<irk>
+				connect peer_addr=<addr> own_addr_type=rpa_pub
+				security-set-data bonding=1 our_key_dist=7 their_key_dist=7
+				security-pair conn=<handle>
+				connect peer_addr=<addr>
+				disconnect conn=1
+GAP/CONN/GCEP/BV-04-C	N/A
+GAP/CONN/SCEP/BV-01-C	PASS	white-list addr_type=public addr=<addr>
+				connect
+				disconnect conn=<handle>
+GAP/CONN/SCEP/BV-02-C	INC
+GAP/CONN/DCEP/BV-01-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+GAP/CONN/DCEP/BV-02-C	INC
+GAP/CONN/DCEP/BV-03-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+GAP/CONN/DCEP/BV-04-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+
+GAP/CONN/CPUP/BV-01-C	PASS	advertise-start
+				conn-update-params conn=<handle>
+GAP/CONN/CPUP/BV-02-C	PASS	advertise-start
+				conn-update-params conn=<handle>
+GAP/CONN/CPUP/BV-03-C	PASS	advertise-start
+				conn-update-params conn=<handle>
+GAP/CONN/CPUP/BV-04-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+GAP/CONN/CPUP/BV-05-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+GAP/CONN/CPUP/BV-06-C	PASS	conect peer_addr=<addr>
+				conn-update-params conn=<handle> eg.latency=20
+				disconnect conn=<handle>
+GAP/CONN/CPUP/BV-08-C	PASS	advertise-configure legacy=1 connectable=1
+				advertise-set-data name=<name>
+				advertise-start
+
+GAP/CONN/TERM/BV-01-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+GAP/CONN/PRDA/BV-01-C	N/A
+GAP/CONN/PRDA/BV-02-C	N/A
+-------------------------------------------------------------------------------
+GAP/BOND/NBON/BV-01-C	PASS	security-set-data bonding=0
+				connect peer_addr=<addr>
+				<ok>
+				connect peer_addr=<addr>
+				<ok>
+GAP/BOND/NBON/BV-02-C	PASS	security-set-data bonding=0
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				<ok>
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				<ok>
+GAP/BOND/NBON/BV-03-C	PASS	security-set-data bonding=0
+				advertise-configure legacy=1 connectable=1
+				advertise-set-data name=<name>
+				advertise-start
+				<ok>
+
+GAP/BOND/BON/BV-01-C	PASS	security-set-data bonding=1 sc=1 our_key_dist=7 their_key_dist=7
+				advertise-configure legacy=1 connectable=1
+				advertise-start
+				security-start conn=<handle>
+				<ok>
+				advertise-start
+				<ok>
+GAP/BOND/BON/BV-02-C	PASS	security-set-data bonding=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				<ok>
+				connect peer_addr=<addr>
+				seccurity-pair conn=<handle>
+				<ok>
+GAP/BOND/BON/BV-03-C	PASS	security-set-sm-data bonding=1 our_key_dist=7 their_key_dist=7
+				advertise-configure legacy=1 connectable=1
+				advertise-start
+				<ok>
+				advertise-start
+				<ok>
+GAP/BOND/BON/BV-04-C	PASS	security-set-data bonding=1
+				connect-peer_addr=<addr>
+				disconnect conn=<handle>
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+GAP/SEC/AUT/BV-11-C	PASS	security-set-data io_capabilities=1 sc=1
+				advertise-configure legacy=1 connectable=1
+				advertising-start
+				Note: in PTS enter handle for characteristics
+				      value which requires encryption for read (gatt-show-local)
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+GAP/SEC/AUT/BV-12-C	PASS	security-set-data io_capabilities=1 bonding=1 mitm_flag=1 sc=1 our_key_dist=7 their_key_dist=7
+				connect peer_addr=<addr>
+				gatt-show-local
+				Note: in PTS enter handle for characteristics
+				      value which requires encryption for read
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+GAP/SEC/AUT/BV-13-C	PASS	Note: in PTS confirm that IUT supports GATT Server
+				security-set-data io_capabilities=1 bonding=1 mitm_flag=1 sc=1 our_key_dist=7 their_key_dist=7
+				connect peer_addr=<addr>
+				gatt-show-local
+				Note: in PTS enter handle for characteristics
+				      value which requires authenticated pairing for read
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+GAP/SEC/AUT/BV-14-C	PASS	security-set-data io_capabilities=1
+				advertise-configure legacy=1 connectable=1
+				advertise-start
+				gatt-show-local
+				Note: in PTS enter handle for characteristics
+				      value which requires authenticated pairing for read
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+GAP/SEC/AUT/BV-15-C	N/A	security-set-data bonding=1 io_capabilities=4 mitm_flag=1 sc=1 our_key_dist=7 their_key_dist=7
+				advertise-configure legacy=1 connectable=1
+				advertise-start
+				auth-passkey conn=<handle> action=2 key=<key>
+				advertise-start
+				gatt-show-local
+				Note: in PTS enter handle for characteristics
+				      value which requires authenticated pairing for read
+GAP/SEC/AUT/BV-16-C	N/A	security-set-data io_capabilities=1 bonding=1 mitm_flag=1 sc=1 our_key_dist=7 their_key_dist=7
+				connect peer_addr=<addr>
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+				connect peer_addr=<addr>
+				gatt-show-local
+				Note: in PTS enter handle for characteristics
+				      value which requires authenticated pairing for read
+GAP/SEC/AUT/BV-17-C	N/A
+GAP/SEC/AUT/BV-18-C	N/A
+GAP/SEC/AUT/BV-19-C	N/A
+GAP/SEC/AUT/BV-20-C	N/A
+GAP/SEC/AUT/BV-21-C	N/A
+GAP/SEC/AUT/BV-22-C	N/A
+GAP/SEC/AUT/BV-23-C	N/A
+GAP/SEC/AUT/BV-24-C	N/A
+
+GAP/SEC/CSIGN/BV-01-C	N/A
+GAP/SEC/CSIGN/BV-02-C	N/A
+
+GAP/SEC/CSIGN/BI-01-C	N/A
+GAP/SEC/CSIGN/BI-02-C	N/A
+GAP/SEC/CSIGN/BI-03-C	N/A
+GAP/SEC/CSIGN/BI-04-C	N/A
+-------------------------------------------------------------------------------
+
+GAP/PRIV/CONN/BV-01-C	N/A
+GAP/PRIV/CONN/BV-02-C	N/A
+GAP/PRIV/CONN/BV-03-C	N/A
+GAP/PRIV/CONN/BV-04-C	INC
+GAP/PRIV/CONN/BV-05-C	N/A
+GAP/PRIV/CONN/BV-06-C	N/A
+GAP/PRIV/CONN/BV-07-C	N/A
+GAP/PRIV/CONN/BV-08-C	N/A
+GAP/PRIV/CONN/BV-09-C	N/A
+GAP/PRIV/CONN/BV-10-C	N/A
+GAP/PRIV/CONN/BV-11-C	N/A
+-------------------------------------------------------------------------------
+
+GAP/ADV/BV-01-C		PASS	advertise-set-adv_data uuid16=0x1802
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-02-C		PASS	advertise-set-adv_data name=<name>
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-03-C		PASS	advertise-set-adv_data flags=6
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-04-C		PASS	advertise-set-adv_data mfg_data=ff:ff
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-05-C		PASS	advertise-set-adv_data tx_pwr_lvl=10
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-08-C		N/A
+GAP/ADV/BV-09-C		N/A
+GAP/ADV/BV-10-C		PASS	advetrise-set-adv_data service_data_uuid16=18:02:ff:ff
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-11-C		PASS	advertise-set -dv_data appearance=12
+				advertise-start
+				advertise-stop
+GAP/ADV/BV-12-C		N/A
+GAP/ADV/BV-13-C		N/A
+GAP/ADV/BV-14-C		N/A
+GAP/ADV/BV-15-C		N/A
+GAP/ADV/BV-16-C		N/A
+GAP/ADV/BV-17-C		PASS	In PTS: TSPX_URI=<bytes>
+				set-adv-data uri=<bytes>
+				advertise-start
+				advertise-stop
+-------------------------------------------------------------------------------
+
+GAP/GAT/BV-01-C		PASS	<if NO>
+				advertising-start
+				<if YES>
+				connect peer_addr=<addr>
+GAP/GAT/BV-02-C		N/A
+GAP/GAT/BV-03-C		N/A
+GAP/GAT/BV-04-C		PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GAP/GAT/BV-05-C		N/A
+GAP/GAT/BV-06-C		N/A
+GAP/GAT/BV-07-C		N/A
+GAP/GAT/BV-08-C		N/A
+----------------------------------------------------------------------------
+
+GAP/DM/NCON/BV-01-C	N/A
+GAP/DM/CON/BV-01-C	N/A
+GAP/DM/NBON/BV-01-C	N/A
+GAP/DM/BON/BV-01-C	N/A
+GAP/DM/GIN/BV-01-C	N/A
+GAP/DM/LIN/BV-01-C	N/A
+GAP/DM/NAD/BV-01-C	N/A
+GAP/DM/NAD/BV-02-C	N/A
+GAP/DM/LEP/BV-01-C	N/A
+GAP/DM/LEP/BV-02-C	N/A
+GAP/DM/LEP/BV-04-C	N/A
+GAP/DM/LEP/BV-05-C	N/A
+GAP/DM/LEP/BV-06-C	N/A
+GAP/DM/LEP/BV-07-C	N/A
+GAP/DM/LEP/BV-08-C	N/A
+GAP/DM/LEP/BV-09-C	N/A
+GAP/DM/LEP/BV-10-C	N/A
+GAP/DM/LEP/BV-11-C	N/A
+-------------------------------------------------------------------------------
+
+GAP/MOD/NDIS/BV-01-C	N/A
+GAP/MOD/LDIS/BV-01-C	N/A
+GAP/MOD/LDIS/BV-02-C	N/A
+GAP/MOD/LDIS/BV-03-C	N/A
+GAP/MOD/GDIS/BV-01-C	N/A
+GAP/MOD/GDIS/BV-02-C	N/A
+GAP/MOD/NCON/BV-01-C	N/A
+GAP/MOD/CON/BV-01-C	N/A
\ No newline at end of file
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/pts-gatt.txt b/src/libs/mynewt-nimble/nimble/host/pts/pts-gatt.txt
new file mode 100644
index 00000000..74c0a2e0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/pts-gatt.txt
@@ -0,0 +1,508 @@
+PTS test results for GATT
+
+PTS version: 7.5.0
+Tested: 27-Sept-2019
+
+Results:
+PASS	test passed
+FAIL	test failed
+INC	test is inconclusive
+N/A	test is disabled due to PICS setup
+
+-------------------------------------------------------------------------------
+Test Name		Result	Notes
+-------------------------------------------------------------------------------
+GATT/CL/GAC/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-exchanche-mtu conn=<handle>
+				gatt-write conn=<handle> long=1 attr=<val_handle> value=<xx:...>
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+GATT/CL/GAD/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-discover-service conn=<handle>
+				gatt-show
+				<answer YES>
+				disconnect conn=<handle>
+				<repeat>
+GATT/CL/GAD/BV-02-C	PASS	connect peer_addr=<addr>
+				gatt-discover-service conn=<handle> uuid=<uuid>
+				gatt-show
+				<answer YES>
+				disconnect conn=<handle>
+				<repeat>
+GATT/CL/GAD/BV-03-C	PASS	connect peer_addr=<addr>
+				gatt-find-included-services conn=<handle> start=1 end=0xffff
+				<answer YES>
+				disconnect conn=<handle>
+				<repeat>
+GATT/CL/GAD/BV-04-C	PASS	connect peer_addr=<addr>
+				gatt-discover-service conn=<handle> uuid=<uuid>
+				gatt-discover-characteristic conn=<handle> start=<start hdl> end=<end hdl>
+				gatt-show
+				<answer YES>
+				disconnect conn=<handle>
+				<repeat>
+GATT/CL/GAD/BV-05-C	PASS	connect peer_addr=<addr>
+				gatt-discover-service conn=<handle>
+				gatt-discover-characteristic conn=<handle> uuid=<uuid> start=<start hdl> end=<end hdl>
+				gatt-show
+				<answer YES>
+				disconnect conn=<handle>
+				<repeat>
+GATT/CL/GAD/BV-06-C	PASS	connect peer_addr=<addr>
+				gatt-discover-service conn=<handle>
+				gatt-discover-characteristic conn=<handle> start=<start-hdl> end=<end-hdl>
+				gatt-discover-descriptor conn=<handle> start=<start-hdl> end=<end-hdl>
+				<answer YES>
+				disconnect conn=<handle>
+				<repeat>
+GATT/CL/GAD/BV-07-C	N/A
+GATT/CL/GAD/BV-08-C	N/A
+-------------------------------------------------------------------------------
+
+GATT/CL/GAR/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-01-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-02-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-03-C	N/A
+
+GATT/CL/GAR/BI-04-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-05-C	PASS    connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BV-03-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> uuid=<uuid> start=1 end=0xffff
+				<answer YES>
+				<repeat>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-06-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> uuid=<uuid> start=<start hdl> end=<end hdl>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-07-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> uuid=<uuid> start=<start hdl> end=<end hdl>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-09-C	N/A
+GATT/CL/GAR/BI-10-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> uuid=<uuid> start=<start hdl> end=<end hdl>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-11-C	PASS    connect perr_addr=<addr>
+				gatt-read conn=<handle> start=<start_hdl> end=<end_hdl>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BV-04-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				<answer YES>
+				<repeat>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-12-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-13-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle> offset=<offset>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-14-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-15-C	N/A
+
+GATT/CL/GAR/BI-16-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-17-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BV-05-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle1> attr=<val_handle2>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-18-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle1> attr=<val_handle2>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-19-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle1> attr=<val_handle2>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-20-C	N/A
+
+GATT/CL/GAR/BI-21-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle1> attr=<val_handle2>
+				disconnect conn=<handle>
+				<answer YES>
+GATT/CL/GAR/BI-22-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle1> attr=<val_handle2>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BV-06-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+GATT/CL/GAR/BV-07-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				<answer YES>
+				<repeat>
+				disconnect conn=<handle>
+GATT/CL/GAR/BI-34-C	N/A
+GATT/CL/GAR/BI-35-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> long=1 attr=<val_handle>
+				<answer YES>
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+GATT/CL/GAW/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-write no_rsp=1 conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BV-02-C	N/A
+
+GATT/CL/GAW/BV-03-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-02-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-03-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-04-C	N/A
+
+GATT/CL/GAW/BI-05-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-06-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BV-05-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-07-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+				disocnnect conn=<handle>
+GATT/CL/GAW/BI-08-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+				diconnect conn=<handle>
+GATT/CL/GAW/BI-09-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val> offset=<offset>
+				diconnect conn=1
+GATT/CL/GAW/BI-11-C	N/A
+
+GATT/CL/GAW/BI-12-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-13-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+				diconnect conn=<handle>
+GATT/CL/GAW/BV-06-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+
+GAAT/CL/GAW/BV-08-C	PASS	connect peer_addr=<addr>
+				gat-write conn=<handle> attr=<val_handle> value=<val>
+
+GATT/CL/GAW/BV-09-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+
+GATT/CL/GAW/BI-32-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-33-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+GATT/CL/GAW/BI-34-C	PASS	connect peer_addr=<addr>
+				gatt-write long=1 conn=<handle> attr=<val_handle> value=<val>
+				disconnect conn=<handle>
+
+-------------------------------------------------------------------------------
+
+GATT/CL/GAN/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=01:00
+				Note: verify that the notification was received
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+GATT/CL/GAI/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=01:00
+				Note: verify that the notification was received
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+GATT/CL/GAS/BV-01-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+GATT/CL/GAT/BV-01-C	PASS	connect peer_addr=<addr>
+				gatt-read conn=<handle> attr=<val_handle>
+GATT/CL/GAT/BV-02-C	PASS	connect peer_addr=<addr>
+				gatt-write conn=<handle> attr=<val_handle> value=<val>
+-------------------------------------------------------------------------------
+
+GATT/CL/GPA/BV-01-C	N/A
+GATT/CL/GPA/BV-02-C	N/A
+GATT/CL/GPA/BV-03-C	N/A
+GATT/CL/GPA/BV-04-C	N/A
+GATT/CL/GPA/BV-05-C	N/A
+GATT/CL/GPA/BV-06-C	N/A
+GATT/CL/GPA/BV-07-C	N/A
+GATT/CL/GPA/BV-08-C	N/A
+GATT/CL/GPA/BV-11-C	N/A
+GATT/CL/GPA/BV-12-C	N/A
+-------------------------------------------------------------------------------
+
+GATT/SR/GAC/BV-01-C	PASS	set mtu=25
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				advertise-start
+-------------------------------------------------------------------------------
+
+GATT/SR/GAD/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				gatt-show-local
+				<YES>
+GATT/SR/GAD/BV-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				gatt-show-local
+				<YES>
+GATT/SR/GAD/BV-03-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				gatt-show-local
+				<YES>
+GATT/SR/GAD/BV-04-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<confirm handles range for services>
+GATT/SR/GAD/BV-05-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAD/BV-06-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAD/BV-07-C	N/A
+GATT/SR/GAD/BV-08-C	N/A
+-------------------------------------------------------------------------------
+
+GATT/SR/GAR/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAR/BI-03-C	N/A
+GATT/SR/GAR/BI-04-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-05-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BV-03-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-06-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				gatt-show-local
+				<enter uuid without READ flag>
+				<enter value handle>
+GATT/SR/GAR/BI-07-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAR/BI-08-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-09-C	N/A
+GATT/SR/GAR/BI-10-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-11-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter characteristic wit READ|READ_ENC flags>
+GATT/SR/GAR/BV-04-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-12-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter long value handle without READ flag>
+GATT/SR/GAR/BI-13-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-14-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAR/BI-15-C	N/A
+GATT/SR/GAR/BI-16-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-17-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BV-05-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-18-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter value handle without READ flag>
+GATT/SR/GAR/BI-19-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAR/BI-20-C	N/A
+GATT/SR/GAR/BI-21-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-22-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-startt
+GATT/SR/GAR/BV-06-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-23-C	N/A
+GATT/SR/GAR/BI-24-C	N/A
+GATT/SR/GAR/BI-25-C	N/A
+GATT/SR/GAR/BI-26-C	N/A
+GATT/SR/GAR/BI-27-C	N/A
+GATT/SR/GAR/BV-07-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BV-08-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAR/BI-28-C	N/A
+GATT/SR/GAR/BI-29-C	N/A
+GATT/SR/GAR/BI-30-C	N/A
+GATT/SR/GAR/BI-31-C	N/A
+GATT/SR/GAR/BI-32-C	N/A
+GATT/SR/GAR/BI-33-C	N/A
+GATT/SR/GAR/BI-34-C	N/A
+GATT/SR/GAR/BI-35-C	N/A
+-------------------------------------------------------------------------------
+
+GATT/SR/GAW/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-02-C	N/A
+GATT/SR/GAW/BI-01-C	N/A
+GATT/SR/GAW/BV-03-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAW/BI-03-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-04-C	N/A
+GATT/SR/GAW/BI-05-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-06-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-05-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-07-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAW/BI-08-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter long value handle without WRITE flag>
+GATT/SR/GAW/BI-09-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-11-C	N/A
+GATT/SR/GAW/BI-12-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-13-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-06-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-10-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-14-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAW/BI-15-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter value handle without WRITE flag>
+GATT/SR/GAW/BI-17-C	N/A
+GATT/SR/GAW/BI-18-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-19-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-11-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-07-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-08-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-20-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAW/BI-21-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter dsc value handle without WRITE flag>
+GATT/SR/GAW/BI-22-C	N/A
+GATT/SR/GAW/BI-23-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-24-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BV-09-C	PASS	advertise-configure connectable=1 legacy=1q
+				advertise-start
+GATT/SR/GAW/BI-25-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter ffff>
+GATT/SR/GAW/BI-26-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<enter dsc value handle without WRITE flag>
+GATT/SR/GAW/BI-27-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-29-C	N/A
+GATT/SR/GAW/BI-30-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-31-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-32-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-33-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-34-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/GAW/BI-35-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+------------------------------------------------------------------------------
+
+GATT/SR/GAN/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				gatt-notify attr=<val_handle>
+------------------------------------------------------------------------------
+
+GATT/SR/GAI/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				gatt-notify attr=<val_handle>
+-------------------------------------------------------------------------------
+
+GATT/SR/GAS/BV-01-C	PASS	Note: set TSPX_security_enabled to TRUE
+				security-set-data bonding=1 our_key_dist=7 their_key_dist=7
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				<click OK>
+				gatt-service-changed start=1 end=0xffff
+				advertise-start
+				security-start conn=<handle>
+-------------------------------------------------------------------------------
+
+GATT/SR/GAT/BV-01-C	PASS	advertise-start
+				gatt-notify attr=0x0008
+------------------------------------------------------------------------------
+
+GATT/SR/GPA/BV-01-C	N/A
+GATT/SR/GPA/BV-02-C	N/A
+GATT/SR/GPA/BV-03-C	N/A
+GATT/SR/GPA/BV-04-C	N/A
+GATT/SR/GPA/BV-05-C	N/A
+GATT/SR/GPA/BV-06-C	N/A
+GATT/SR/GPA/BV-07-C	N/A
+GATT/SR/GPA/BV-08-C	N/A
+GATT/SR/GPA/BV-11-C	N/A
+GATT/SR/GPA/BV-12-C	N/A
+-------------------------------------------------------------------------------
+
+GATT/SR/UNS/BI-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+GATT/SR/UNS/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+
+--------------------------------------------------------------------------------
+
+GATT/SR/GPM/BV-01-C	N/A
+
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/pts-l2cap.txt b/src/libs/mynewt-nimble/nimble/host/pts/pts-l2cap.txt
new file mode 100644
index 00000000..c09add91
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/pts-l2cap.txt
@@ -0,0 +1,304 @@
+PTS test results for L2CAP
+
+PTS version: 7.5.0
+Tested: 07-Oct-2019
+
+syscfg.vals:
+    BLE_EXT_ADV: 1
+    BLE_PUBLIC_DEV_ADDR: "((uint8_t[6]){0x01, 0xff, 0xff, 0xc0, 0xde, 0xc0})"
+    BLE_SM_LEGACY: 1
+    BLE_SM_SC: 1
+    BLE_L2CAP_COC_MAX_NUM: 5
+    BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL: 9
+    BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL: 30
+    BLE_SVC_GAP_PPCP_SUPERVISION_TMO: 2000
+    CONSOLE_HISTORY_SIZE: 10
+
+Results:
+PASS   test passed
+FAIL   test failed
+INC    test is inconclusive
+N/A    test is disabled due to PICS setup
+
+-------------------------------------------------------------------------------
+Test Name		Result	Notes
+-------------------------------------------------------------------------------
+
+L2CAP/COS/CED/BV-01-C	N/A
+L2CAP/COS/CED/BV-03-C	N/A
+L2CAP/COS/CED/BV-04-C	N/A
+L2CAP/COS/CED/BV-05-C	N/A
+L2CAP/COS/CED/BV-07-C	N/A
+L2CAP/COS/CED/BV-08-C	N/A
+L2CAP/COS/CED/BV-09-C	N/A
+L2CAP/COS/CED/BV-10-C	N/A
+L2CAP/COS/CED/BV-11-C	N/A
+L2CAP/COS/CED/BI-01-C	N/A
+-------------------------------------------------------------------------------
+
+L2CAP/COS/CFD/BV-01-C	N/A
+L2CAP/COS/CFD/BV-02-C	N/A
+L2CAP/COS/CFD/BV-03-C	N/A
+L2CAP/COS/CFD/BV-08-C	N/A
+L2CAP/COS/CFD/BV-09-C	N/A
+L2CAP/COS/CFD/BV-10-C	N/A
+L2CAP/COS/CFD/BV-11-C	N/A
+L2CAP/COS/CFD/BV-12-C	N/A
+L2CAP/COS/CFD/BV-13-C	N/A
+-------------------------------------------------------------------------------
+
+L2CAP/COS/IEX/BV-01-C	N/A
+L2CAP/COS/IEX/BV-02-C	N/A
+-------------------------------------------------------------------------------
+
+L2CAP/COS/ECH/BV-01-C	N/A
+L2CAP/COS/ECH/BV-02-C	N/A
+-------------------------------------------------------------------------------
+
+L2CAP/COS/CFC/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+				l2cap-send conn=<handle> idx=0 bytes=15
+				<YES>
+L2CAP/COS/CFC/BV-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+				l2cap-send conn=<handle> idx=0 bytes=15
+				<YES>
+L2CAP/COS/CFC/BV-03-C	PASS	NOTE: #define BTSHELL_COC_MTU = 512
+				advertise-configure connectable=1 legacy=1
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+L2CAP/COS/CFC/BV-04-C	PASS	advertise-configure connectable=1 legacy=1
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+L2CAP/COS/CFC/BV-05-C	PASS	advertise-configure connectable=1 legacy=1
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+				l2cap-connect conn=<handle> psm=<your psm>
+				l2cap-connect conn=<handle> psm=<2nd psm>
+-------------------------------------------------------------------------------
+
+L2CAP/CLS/CLR/BV-01-C	N/A
+-------------------------------------------------------------------------------
+
+L2CAP/CLS/UCD/BV-01-C	N/A
+L2CAP/CLS/UCD/BV-02-C	N/A
+L2CAP/CLS/UCD/BV-03-C	N/A
+-------------------------------------------------------------------------------
+
+L2CAP/EXF/BV-01-C		N/A
+L2CAP/EXF/BV-02-C		N/A
+L2CAP/EXF/BV-03-C		N/A
+L2CAP/EXF/BV-04-C		N/A
+L2CAP/EXF/BV-05-C		N/A
+L2CAP/EXF/BV-06-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/CMC/BV-01-C		N/A
+L2CAP/CMC/BV-02-C		N/A
+L2CAP/CMC/BV-03-C		N/A
+L2CAP/CMC/BV-04-C		N/A
+L2CAP/CMC/BV-05-C		N/A
+L2CAP/CMC/BV-06-C		N/A
+L2CAP/CMC/BV-07-C		N/A
+L2CAP/CMC/BV-08-C		N/A
+L2CAP/CMC/BV-09-C		N/A
+L2CAP/CMC/BV-10-C		N/A
+L2CAP/CMC/BV-11-C		N/A
+L2CAP/CMC/BV-12-C		N/A
+L2CAP/CMC/BV-13-C		N/A
+L2CAP/CMC/BV-14-C		N/A
+L2CAP/CMC/BV-15-C		N/A
+L2CAP/CMC/BI-01-C		N/A
+L2CAP/CMC/BI-02-C		N/A
+L2CAP/CMC/BI-03-C		N/A
+L2CAP/CMC/BI-04-C		N/A
+L2CAP/CMC/BI-05-C		N/A
+L2CAP/CMC/BI-06-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/FOC/BV-01-C		N/A
+L2CAP/FOC/BV-02-C		N/A
+L2CAP/FOC/BV-03-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/OFS/BV-01-C		N/A
+L2CAP/OFS/BV-02-C		N/A
+L2CAP/OFS/BV-03-C		N/A
+L2CAP/OFS/BV-04-C		N/A
+L2CAP/OFS/BV-05-C		N/A
+L2CAP/OFS/BV-06-C		N/A
+L2CAP/OFS/BV-07-C		N/A
+L2CAP/OFS/BV-08-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/ERM/BV-01-C		N/A
+L2CAP/ERM/BV-02-C		N/A
+L2CAP/ERM/BV-03-C		N/A
+L2CAP/ERM/BV-05-C		N/A
+L2CAP/ERM/BV-06-C		N/A
+L2CAP/ERM/BV-07-C		N/A
+L2CAP/ERM/BV-08-C		N/A
+L2CAP/ERM/BV-09-C		N/A
+L2CAP/ERM/BV-10-C		N/A
+L2CAP/ERM/BV-11-C		N/A
+L2CAP/ERM/BV-12-C		N/A
+L2CAP/ERM/BV-13-C		N/A
+L2CAP/ERM/BV-14-C		N/A
+L2CAP/ERM/BV-15-C		N/A
+L2CAP/ERM/BV-16-C		N/A
+L2CAP/ERM/BV-17-C		N/A
+L2CAP/ERM/BV-18-C		N/A
+L2CAP/ERM/BV-19-C		N/A
+L2CAP/ERM/BV-20-C		N/A
+L2CAP/ERM/BV-21-C		N/A
+L2CAP/ERM/BV-22-C		N/A
+L2CAP/ERM/BV-23-C		N/A
+L2CAP/ERM/BI-01-C		N/A
+L2CAP/ERM/BI-02-C		N/A
+L2CAP/ERM/BI-03-C		N/A
+L2CAP/ERM/BI-04-C		N/A
+L2CAP/ERM/BI-05-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/STM/BV-01-C		N/A
+L2CAP/STM/BV-02-C		N/A
+L2CAP/STM/BV-03-C		N/A
+L2CAP/STM/BV-11-C		N/A
+L2CAP/STM/BV-12-C		N/A
+L2CAP/STM/BV-13-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/FIX/BV-01-C		N/A
+L2CAP/FIX/BV-02-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/EWC/BV-01-C		N/A
+L2CAP/EWC/BV-02-C		N/A
+L2CAP/EWC/BV-03-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/LSC/BV-01-C		N/A
+L2CAP/LSC/BV-02-C		N/A
+L2CAP/LSC/BV-03-C		N/A
+L2CAP/LSC/BI-04-C		N/A
+L2CAP/LSC/BI-05-C		N/A
+L2CAP/LSC/BV-06-C		N/A
+L2CAP/LSC/BV-07-C		N/A
+L2CAP/LSC/BV-08-C		N/A
+L2CAP/LSC/BV-09-C		N/A
+L2CAP/LSC/BI-10-C		N/A
+L2CAP/LSC/BI-11-C		N/A
+L2CAP/LSC/BV-12-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/CCH/BV-01-C		N/A
+L2CAP/CCH/BV-02-C		N/A
+L2CAP/CCH/BV-03-C		N/A
+L2CAP/CCH/BV-04-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/ECF/BV-01-C		N/A
+L2CAP/ECF/BV-02-C		N/A
+L2CAP/ECF/BV-03-C		N/A
+L2CAP/ECF/BV-04-C		N/A
+L2CAP/ECF/BV-05-C		N/A
+L2CAP/ECF/BV-06-C		N/A
+L2CAP/ECF/BV-07-C		N/A
+L2CAP/ECF/BV-08-C		N/A
+-------------------------------------------------------------------------------
+
+L2CAP/LE/CPU/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-update conn=<handle>
+L2CAP/LE/CPU/BV-02-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+L2CAP/LE/CPU/BI-01-C	PASS	connect peer_addr=<addr>
+				disconnect conn=<handle>
+L2CAP/LE/CPU/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+-------------------------------------------------------------------------------
+
+L2CAP/LE/REJ/BI-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+L2CAP/LE/REJ/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				disconnect conn=<handle>
+-------------------------------------------------------------------------------
+
+L2CAP/LE/CFC/BV-01-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+L2CAP/LE/CFC/BV-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-connect conn=<handle> psm=90
+L2CAP/LE/CFC/BV-03-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+				l2cap-send conn=<handle> idx=0 bytes=15
+				<YES>
+L2CAP/LE/CFC/BV-04-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-connect conn=<handle> psm=<unsuppported psm from ixit>
+L2CAP/LE/CFC/BV-05-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+L2CAP/LE/CFC/BV-06-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+				l2cap-send conn=<handle> idx=0 bytes=15
+L2CAP/LE/CFC/BV-07-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+L2CAP/LE/CFC/BI-01-C	PASS	advertise-configure connectable=1 legacy=1
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+L2CAP/LE/CFC/BV-08-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<your psm>
+				advertise-start
+				l2cap-disconnect conn=<handle> idx=0
+L2CAP/LE/CFC/BV-09-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+L2CAP/LE/CFC/BV-16-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-connect conn=<handle> psm=90
+L2CAP/LE/CFC/BV-17-C	N/A
+L2CAP/LE/CFC/BV-18-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-connect conn=<handle> psm=90
+L2CAP/LE/CFC/BV-19-C	PASS	NOTE: TSPC_L2CAP_3_16 (multiple channel support) must be checked
+				advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-connect conn=<handle> psm=90
+L2CAP/LE/CFC/BV-20-C	PASS	NOTE: TSPC_L2CAP_3_16 (multiple channel support) must be checked
+				advertise-configure connectable=1 legacy=1
+				l2cap-create-server psm=<TSPX_le_psm from ixit>
+				advertise-start
+L2CAP/LE/CFC/BV-21-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-set-adv-data flags=6
+				advertise-start
+				l2cap-connect conn=<handle> psm=90
+-------------------------------------------------------------------------------
+
+L2CAP/LE/CID/BV-01-C	N/A
+L2CAP/LE/CID/BV-02-C	N/A
+-------------------------------------------------------------------------------
+
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/pts-sm.txt b/src/libs/mynewt-nimble/nimble/host/pts/pts-sm.txt
new file mode 100644
index 00000000..ac26db71
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/pts-sm.txt
@@ -0,0 +1,310 @@
+PTS test results for SM
+
+PTS version: 7.5.0
+Tested: 07-Oct-2019
+
+syscfg.vals:
+    BLE_EXT_ADV: 1
+    BLE_PUBLIC_DEV_ADDR: "((uint8_t[6]){0x01, 0xff, 0xff, 0xc0, 0xde, 0xc0})"
+    BLE_SM_LEGACY: 1
+    BLE_SM_SC: 1
+    BLE_L2CAP_COC_MAX_NUM: 5
+    BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL: 9
+    BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL: 30
+    BLE_SVC_GAP_PPCP_SUPERVISION_TMO: 2000
+    CONSOLE_HISTORY_SIZE: 10
+
+Results:
+PASS	test passed
+FAIL	test failed
+INC		test is inconclusive
+N/A		test is disabled due to PICS setup
+NONE	test result is none
+
+-------------------------------------------------------------------------------
+Test Name		Result	Notes
+-------------------------------------------------------------------------------
+
+SM/MAS/PROT/BV-01-C	PASS	connect peer_addr=<addr>
+				security-set-data bonding=1 sc=1 our_key_dist=7 their_key_dist=7
+				security-pair conn=<handle>
+-------------------------------------------------------------------------------
+
+SM/MAS/JW/BV-01-C	N/A
+SM/MAS/JW/BV-05-C	PASS	connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=<handle>
+                                <repeat>
+SM/MAS/JW/BI-01-C	PASS	connect peer_addr=<addr>
+				security-pair conn=<handle>
+SM/MAS/JW/BI-04-C	PASS	connect peer_addr=<addr>
+				security-set-data bonding=1 sc=1
+				security-pair conn=<handle>
+-------------------------------------------------------------------------------
+
+SM/MAS/PKE/BV-01-C	PASS	security-set-data io_capabilities=1
+				connect peer_addr=<addr>
+				b sec pair conn=<handle>
+				b passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+SM/MAS/PKE/BV-04-C	PASS	security-set-data bonding=1 oob_flag=0
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=<handle>
+SM/MAS/PKE/BI-01-C	PASS	ecurity-set-data io_capabilities=1 oob_flag=0
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter invalid passkey
+SM/MAS/PKE/BI-02-C	PASS	security-set-data io_capabilities=1 oob_flag=0
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+-------------------------------------------------------------------------------
+
+SM/MAS/OOB/BV-01-C	N/A
+SM/MAS/OOB/BV-03-C	N/A
+SM/MAS/OOB/BV-05-C	PASS	security-set-data io_capabilities=1 oob_flag=0
+				connect-peer_addr=<addr>
+				security-pair conn=<handle>
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter '123456' passkey in PTS
+				disconnect conn=1
+SM/MAS/OOB/BV-07-C	PASS	ecurity-set-data io_capabilities=1 oob_flag=0
+				connect-peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/OOB/BV-09-C	N/A
+SM/MAS/OOB/BI-01-C	N/A
+-------------------------------------------------------------------------------
+
+SM/MAS/EKS/BV-01-C	PASS	connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/EKS/BI-01-C	PASS	connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+-------------------------------------------------------------------------------
+
+SM/MAS/SIGN/BV-01-C	N/A
+SM/MAS/SIGN/BV-03-C	N/A
+SM/MAS/SIGN/BI-01-C	N/A
+-------------------------------------------------------------------------------
+
+SM/MAS/KDU/BV-04-C	PASS	security-set-data our_key_dist=4
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/KDU/BV-05-C	PASS	security-set-data our_key_dist=2
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/KDU/BV-06-C	PASS	security-set-data our_key_dist=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/KDU/BV-10-C	PASS	security-set-data our_key_dist=2 sc=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/KDU/BV-11-C	PASS	security-set-data our_key_dist=2 sc=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/KDU/BI-01-C	PASS	connect peer_addr=<addr>
+				disconnect conn=1
+				reset device
+				<OK>
+				<repeat>
+-------------------------------------------------------------------------------
+
+SM/MAS/SIP/BV-02-C	PASS	security-set-data io_capabilities=4
+				connect peer_addr=<addr>
+				disconnect conn=1
+-------------------------------------------------------------------------------
+SM/MAS/SCJW/BV-01-C	PASS	security-set-data sc=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/SCJW/BV-04-C	PASS	security-set-data sc=1 io_capabilities=1 our_key_dist=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+SM/MAS/SCJW/BI-01-C	PASS	security-set-data sc=1 io_capabilities=4
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+				<repeat>
+				<confirm key number with PTS>
+-------------------------------------------------------------------------------
+
+SM/MAS/SCPK/BV-01-C	PASS	security-set-data sc=1 io_capabilities=2
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				auth_passkey conn=1 action=2 key=123456
+				Note: enter '123456' passkey in PTS
+				disconnect conn=1
+SM/MAS/SCPK/BV-04-C	PASS	security-set-data io_capabilities=4 oob_flag=0 our_key_dist=1 their_key_dist=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				auth_passkey conn=1 action=2 key=123456
+				Note: enter '123456' passkey in PTS
+				disconnect conn=1
+SM/MAS/SCPK/BI-01-C	PASS	security-set-data io_capabilities=4 oob_flag=0 sc=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				disconnect conn=1
+				<repeat>
+				auth_passkey conn=1 action=2 key=123456
+				Note: enter '123456' passkey in PTS
+				disconnect conn=1
+SM/MAS/SCPK/BI-02-C	PASS	security-set-data io_capabilities=2 oob_flag=0 bonding=1 sc=1
+				connect peer_addr=<addr>
+				security-pair conn=<handle>
+				auth_passkey conn=1 action=2 key=123456
+				Note: enter '123456' passkey in PTS
+				disconnect conn=1
+-------------------------------------------------------------------------------
+
+SM/MAS/SCOB/BV-01-C	N/A
+SM/MAS/SCOB/BI-04-C	N/A
+SM/MAS/SCOB/BV-01-C	N/A
+SM/MAS/SCOB/BI-04-C	N/A
+-------------------------------------------------------------------------------
+
+SM/MAS/SCCT/BV-01-C	N/A
+SM/MAS/SCCT/BV-03-C	N/A
+SM/MAS/SCCT/BV-05-C	N/A
+SM/MAS/SCCT/BV-07-C	N/A
+SM/MAS/SCCT/BV-09-C	N/A
+-------------------------------------------------------------------------------
+
+SM/SLA/PROT/BV-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+				<wait>
+-------------------------------------------------------------------------------
+
+SM/MAS/JW/BV-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/JW/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/JW/BI-03-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+-------------------------------------------------------------------------------
+
+SM/SLA/PKE/BV-02-C	PASS	security-set-data io_capabilities=4
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				auth-passkey conn=<handle> action=2 key=<key>
+				<OK>
+SM/SLA/PKE/BV-05-C	PASS	security-set-data io_capabilities=4
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/PKE/BI-03-C	PASS	security-set-data io_capabilities=4
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				auth-passkey conn=<handle> action=3 key=123456
+				Note: enter invalid passkey
+-------------------------------------------------------------------------------
+
+SM/SLA/OOB/BV-02-C	N/A
+SM/SLA/OOB/BV-04-C	N/A
+SM/SLA/OOB/BV-06-C	PASS	security-set-data io_capabilities=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				auth-passkey conn=<handle> action=3 key=<key>
+SM/SLA/OOB/BV-08-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/OOB/BV-10-C	N/A
+SM/SLA/OOB/BI-02-C	N/A
+-------------------------------------------------------------------------------
+
+SM/SLA/EKS/BV-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/EKS/BI-02-C	PASS	advertise-configure connectable=1 legacy=1
+				advertise-start
+-------------------------------------------------------------------------------
+
+SM/SLA/KDU/BV-01-C	PASS	security-set-data io_capabilities=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/KDU/BV-02-C	PASS	security-set-data io_capabilities=2
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/KDU/BV-03-C	PASS	security-set-data io_capabilities=4
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/KDU/BV-07-C	PASS	security-set-data our_key_dist=1 bonding=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/KDU/BV-08-C	PASS	security-set-data our_key_dist=2 sc=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/KDU/BV-09-C	PASS	security-set-data our_key_dist=4 bonding=0
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/KDU/BI-01-C	PASS	advertise-configure connectable=1 legacy=1
+				security-set-data sc=1
+				advertise-start
+				<reset device>
+				<repeat>
+-------------------------------------------------------------------------------
+
+SM/SLA/SIP/BV-01-C	PASS	security-set-data io_capabilities=4
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				security-start conn=<handle>
+-------------------------------------------------------------------------------
+
+SM/SLA/SIE/BV-01-C	PASS	security-set-data io_capabilities=3 bonding=1 our_key_dist=1 their_key_dist=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				advertise-start
+				security-start conn=<handle>
+-------------------------------------------------------------------------------
+
+SM/SLA/SCJW/BV-02-C	PASS	security-set-data io_capabilities=4 oob_flag=0 bonding=0 mitm_flag=0 sc=1 our_key_dist=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/SCJW/BV-03-C	PASS	security-set-data io_capabilities=1 our_key_dist=1 oob_flag=0
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+SM/SLA/SCJW/BI-02-C	PASS	security-set-data io_capabilities=1 oob_flag=0 bonding=1 sc=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+-------------------------------------------------------------------------------
+
+SM/SLA/SCPK/BV-02-C	PASS	security-set-data io_capabilities=1 oob_flag=0 sc=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				auth-passkey conn=1 action=4 key=186900 yesno=yy
+SM/SLA/SCPK/BV-03-C	PASS	security-set-data io_capabilities=2 oob_flag=0 our_key_dist=1 their_key_dist=1 sc=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				auth-passkey conn=1 action=2 key=123456
+				Note: enter '123456' passkey in PTS
+SM/SLA/SCPK/BI-03-C	PASS	security-set-data io_capabilities=2 oob_flag=0 sc=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				auth-passkey conn=1 action=2 key=123456 oob=<xx:xx:xx...>
+				Note: enter '123456' passkey in PTS
+SM/SLA/SCPK/BI-04-C	PASS	security-set-data io_capabilities=2 oob_flag=0 mitm_flag=1 sc=1
+				advertise-configure connectable=1 legacy=1
+				advertise-start
+				<repeat>
+				auth-passkey conn=1 action=2 key=123456
+				Note: enter '123456' passkey in PTS
+-------------------------------------------------------------------------------
+
+SM/SLA/SCOB/BV-02-C	N/A
+SM/SLA/SCOB/BI-03-C	N/A
+SM/SLA/SCOB/BV-02-C	N/A
+SM/SLA/SCOB/BI-03-C	N/A
+-------------------------------------------------------------------------------
+
+SM/SLA/SCCT/BV-02-C	N/A
+SM/SLA/SCCT/BV-04-C	N/A
+SM/SLA/SCCT/BV-06-C	N/A
+SM/SLA/SCCT/BV-08-C	N/A
+SM/SLA/SCCT/BV-10-C	N/A
\ No newline at end of file
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/tpg/94654-20170317-085122560.tpg b/src/libs/mynewt-nimble/nimble/host/pts/tpg/94654-20170317-085122560.tpg
new file mode 100644
index 00000000..3cc985fa
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/tpg/94654-20170317-085122560.tpg
@@ -0,0 +1,1026 @@
+<!-- START ENCRYPTED FILE --->
+M'&/JP\$+#X83?)"[ M2E=\N*7*U 5)JSKT(5#=>@^/#]IP*[GTZ0%0]$F(/$
+MEK[V7>349??;!JZJNFZ CK=QGD^FOYV^FU>"DK<@0U*##HV\H9&_%["5IK@1
+MA9F,FJO2P^3:4_]04%-0T%9=^XCMWKF"L$[)%)G>_I 0Q?@:H)&1HX)7VY&.
+M3 *!JH;^YUJ1[_*7JN2'Z5^*0I&NP+[LAE:>S92BP^T:3+B>IKFJJ8A,4Y^L
+MUX+P_C'Z#VN20)A!ANN2@J#!AYY4"KQ\5):1]+[B\TN+BI"AB8NJH ,NZ(()
+MG>K?^<ZA'-8.RL-J$<)(C9[Q!,J#I\O^U //$%L+CAC;BZR I+VL1_?1UG&4
+M!5L,T[F[JEFYD;^4JYE2JO%6O=+:CXZ6CL\:\-LP1(%UF9F8$Q"A4XJ_PM#^
+M^G*CZ8!#KXH7096+O4QZ6O<)Y-4PO:<0N*]7B9PRW"'X3YL(Z^S%XUB5@[F_
+MDKB?NDVX39/(_5[/I!?M_Q@F4A[/?PD7SQS?21+70M!:Y6RP0BX8<[N>A\<!
+MP;[%Q]OXC)K'KI)<7)&;^(F:\LT# \[#W\8ZPLK5 P_-VNG$WL]# ]#5GJU?
+M_XM5B%5INH>-B,<@/IVC+$#XFXJ<65V*OE'"L]7*OEJ*C]CS^."A/) (K_@0
+ME"&)D)^IP]?+')Q(%NM&7:VL@*6#D!-TU,7.WG SERP(@*R+DH"MPH;@X:Y)
+M]X6NG8^EC)N?AH+3[\3SG''R3(?WRGF0NZ#,_4%*E)&>&9R)5[F0OM$A0Y.;
+MA8R7%EGBS&\IDPYNR<DUG*^8[.&)T2! "F\<2L/VCZNXDHN=O2"O2JR,P_'V
+MG^??HX&-:=ZO$Z 2GFNL<++PP(K$$0N4(]"2F,96FJV0FH^?]A"7P_+ Y<M/
+MGZT#BI_?NH.HFKN/E?B#&*\.!<2?KFV9BD>%4ZV$FK^(F[P B/[TU86=H%S^
+MH[H^EYQB:MZ)A:2@IV)4O=//SXNP#%F]5N&<3KZL>(#6'H"D4\DST(O!(![@
+MUYZX#+:1G:V5BE&TGHK]<N.^GDWRF%EQIU^G$*K#YOS0P=X<DJ^=O2$:V::\
+M;-X;,QD[X;?P@%Q"C9N54GVF6KYV8NK8U#<EP^!X#<NJI.V#JI22F'S;3(3Z
+M.>+;@:^JF;;2-&@WD;Z8I9*SB5D+7 &/J8NI6GYGMDGK2(U+_)K!,I$6Q4R%
+MAIE9A<NX;^965G^IIA-2QKVO\TZZK("PEB\(Q=7%#</<JX7X3&6.GP41]*8L
+MY7R<6?M:U)NK >K#]!JAFCWWD:A8K.9O/I(;D(F2G*6A0YUCMIOQ5M*VD#1:
+MG9^)MMZ GIK7,^R0^M"/\%^_B9Y7TLQ/^N?S1 DEF 6XUJU"V_3_U\44H9.;
+M6C/PV(36QT(9B9^.C)*YUP?BN>#F%4ST/JM>\*Y;K)J:S<F0V:R=A)% E4*K
+M896!\7"QW^_2F;!_['8?NR?_P<DI\[R_&X04-D"*V^O$VZ_G'%W;FU01FEL*
+MSO#(?)&QE;>+4[O%?G[S[L"<B<&$DE6: Z&6J=*%J]0=\*/]\+$_F9!R$\4@
+M^=?2 (->^FPVC9/W+<BNPH65S)H$1>).O2<ZY.7_Q1107Z].<XH6I\DGXYA*
+M5E$VH8H7B>3U=;?;2EV+G5"5%KG(EED8SLB#Q["2.9YW6IB4DKT QL7P\,%#
+MAY^ (+Y+V8GD:B</')R,'%225'-E,!4W[\60BHNOAZ$,/R>'!S H%G*;=[-\
+M 0+QQ3;G\H21FY_P4-)38+ =K,/38[24B[6W\(N-L/(9[/_LY.X/OLZ%S;A>
+MO0TN'._"@/.)DDZ*AGY.V1XW%G7:O+R67U"C@:L%+T\24!11KH^]@[UR\Y\T
+M'G.5D/)=H)R"J6R470%+AO24DHF5L -2OV(60>3Z]/#0\HE4<(V%GXBM% 0-
+M3X-@G;H%E[2$DL$P-?W&"JI0-EV0AUP(\>] XIW!BI6DO7I9T33VE._FJ(*_
+MKQ6$@E,QFO?! P_#OYFAJ_:"JEF6?]O2PM3:)3"+BYBAK :/H:SLBX.NP<W#
+MPC,T%:]X55#.:9.- %JZ68*:HWJ&B>OA_3S>U9*&PG[SAU:$=KQR\M>?\!X3
+MZYZ?J;\8N0V1]JOBJ@3;9=L01YQ<:%>>U)-!GAL!T^"]VF8KB/^$EH.ENI:4
+M[(80V,)1T>^'[@;!KIZ5T4.RQW)2M,X;#;8LTO_GV8V[OD6F@JNK JO G$2Q
+MH(=0E:GW!3:0E_/T4ZF!M!D-F1F7NI*^QP;A%%V8M*U:R59]QE^+$NONY](%
+M28E"K!J#K5N5E8#B_O<UU\\.G_G]6Y'(MXM*P=86VM+2&SSPGY_74JN HY0K
+M\-7K*,[/6+]$+*_I?YR:D!9;$_O&TM^>U9L819J(CL]20ZBF"L4*4D#JV,*&
+MYJV<6(JKE<K[D8RGDN4PY162K>ZZ4+^NI?%8D<$.P^*4 Z#=7K><JJ?QDS[5
+MY@*/M()=CJ*L4NR5D-+)!="N< ")5YK:G;!7WV,:E?/@I?)*ABQ;B[J,>XM.
+MU3'S/BX/AP [V4*K!EY0C-/CE//ME"./3?_H5OQM$R!7NN&@21#()=Q)MEIG
+MU_Y)N7IZ .'V51 ]]L:;LEZ+DI"H^K R=CO:@D2&#^H,PQ!"EDZ>@'"*JK)I
+MM9<-XF7"=^(HC+:D156UME\ U?+P=G >A(^3=EW1ZB[7^C_1CZ&R#Y!"F:FH
+MHO_0SL+1D%1$55K!?)*MLC?(]!3G_A,(AXY&\M.G+@>5;>6Z]?^Z&KBV:9N<
+MBRWP0KI!\#7P]MOC;Y)<5(NN%8*2GEL9'\7/SMD 1Q6PCZ]6;0T*&KK)-^4N
+M-OV!3U6TE8(,B;9"K*0WH=?J*<_9J=-(NK)0LY:FO5R)P'*3V.;%;^Z6() 2
+MWEB>F*H-#0RERD0TG[]6I4)#T_26M/[GQ8V5MO]RIY&RN%W_S1G/L829DMR2
+M]('/K/92XJKUML7OR!,>P)J*"H>&-Q0-\]<V+\:+!:) UDM8MG+HQ?/31</O
+MPKV&>("+'L^-EO>=(036Y>P%.:V&"8V-7ZT1<Y"1Y3;]_Y8F#\=7CJ*JM*U 
+M=*"27=K8$<H/QY[AXHB>39&:(YNN@Z VJQCV=M VOMJ:E/XOE(L[GZ4;QT"H
+M$!AY7Y>2^:<R4?72$N>N K"-$'B0L[FEJN#[Q19:D[2;=9F*>)!$@X/4']#"
+M!1&2G)8;"$M&=?M B=+DQ^YQ"-!FB)9'T\^32NDK[68N]-J[6Y^IR:Z(A[..
+M6R<AS,3XATA#DIZ5EX;'N*RBEP71EOK&.EM)C']?0:)1K%F25ZNI^L0JYZXG
+MT<!>G95?0:6^K'2FAK[7\O>>+_9#G(I4O*2<FIOA*Q$2A%"2L::0B:]Q\O.?
+M]A %?("2*Y"0@+JPEAGRU84\?I&P41J]"G=IEH,MY[CUYMO9J?:B@M*0G97*
+MO/6S_192P8VR8:J(&-:^?IZ-Y$6V?A?)0UZ,E%:H/YT@L%'%'@W#Q,7V[)^$
+MA9V7G-+H0)TE*G/;[OI1DZZ6N+I62UQ1XY>=3,_<+@R)UM-9DW*NL+I?DJA^
+M:X8+&-[1=O9MX:R#BIVVVL*?&,-#U928LBR6DI:OR/OQV_;Z"H13GEZ <)J[
+M^8(-)]\+U9V24@V?@HK_A'3=YS&MN]3U&:<&)%JJPHZ6H4 --!,T\L,(ICBY
+M$XOXI)]-\^"U_CHZ!52.^"'OK$V=K;>;@,34R_HCW U3FB1#KKV/@Z#_S.'%
+M9M 26;U3E76LAFN3DZG5@QO8R.G+!?N(V9N<2;,*E9E2@K&$0L$X+707X.^\
+MT*.^GE""6H$?\19Z=D.11X"6D]CR\RWCD_%?L8J<EAF:O>F/FV$&X]!:4**[
+MIJ&85T .P5K[]>8?Q92+TN#S6X[[MHCH"</L\!K"BQ; 5I"/F'9#GO'0Q-8U
+M[@*)<*I%IU^[1LR 62'!U0G+R^.OMXJ,AK8?;R"@VL$R&?9R/=>/&KV'J%?'
+MDXN2D@L"PEG3X\S?#I9!&(61@+:/6K0YEXK-.ODV/_?'<;&3^)J!2E##A^,3
+M^E;>K7N=]K<"P^$PWC05A%.T7+ 72YJ2?GKS[_#*?9/+D5I=DGF1MKNMU-:;
+MTA'SN[>Q@KJ'7 >7QXCJY?#F JZBH%J*TH7WD[S#-.W&]"["KX^N/$.2E8- 
+M\'OI3>S9)=OP3U7$O"FNK.\J5(+0IC<F/^X'*U:4D5&@2,*CL(Q"S,X8* ,%
+MS\;CJZ^7$XNWB5:'Z+;!I/[F]_WUR+BB4$F: XF3TNF3X%Y ']Z0=:R(\3@5
+M,C+#RK_8$G]:&+I7::92S1?U"*^1LY&EE5N [*1761Y$[\8(T8:&NIJJNUB,
+M%8BA.NS"TJ*2FA3] @L'OGJ$ 13S9L66$R:FT)R]4D%-H%VW[>!.XWY;M$\.
+M=$Z/KD^-C/1_0?_L_+W"8^N=B7="A8V64I)?G0DOR_KWS/WPQFUSE4C6DDBH
+MPJ(PFL.YN.[W]D+^>+I&EEF\EPW'VQA<& I.AD"V5P+ .6_&1:%>-O&=E8%*
+M4Z0_F\3GXHI]D-*1FG<:^(&&FQ$__=_Z\-!-I[N#<Z:&OX"_C9/7\_+SD8V2
+MUQ:JNY;RG-CWWN_&-@Z<M[^,(H>]"^O-VP$ U1GN0\!K]*A/C9:4F99SFM+[
+M[/?CV\*/O(Z#ME:'OC&&?BVA3B31],CN[TF*EE6"FYY(_IXY];+QIY%OQ/?P
+MO'*3\1.ABJKLSL+ 7[&CG)>!3%[%>BSWMCX#>%VV+I"SC'*XM%O%*Q$\F$3R
+MFPZ16JEA@IM"\L/P^B,0TK:X6KG'%9"W[*C0N>76+Q%7H<_>PHYZHI3A)I%T
+M'CJE1J;\5+\;VP%IFYWG(<')6(D;XR:/K)56#*O 5YC#VK;WD_H'2JC>IQI2
+M#Q)4X? :TPVM[5T "L<4HRZ>IZZGK_N#H)Z#!2#PQ>W&P5H#TE6?DII]0?_U
+M&*Q64E:.A]I>Y.KMN_K%\W60 9M4G7>#89>?AQ=WP<6$M/BPF(@#DXY8H>HV
+M_98"]E%UHB\\ASAPD.K1\^:UE\.K'Z!]6YL*DABNQ='FFM8U\J>$N;^)5E:;
+M)HY7P\"EFN02YN[;E$Z.O[U&K 1"I3*7,,8P<KN?E-J)M&^3@J92D,$&^M+G
+M^&Q'R8:+F9D<3^M1PK@'F.'1]382>B6.?)NNE%"<+^?)R\BZB9Z6IC<;N)4W
+MEY_GY\JLE%.13J-^"6Q2BLC7\M ,DGJQ)(E!VJ2>JL$T&>X]XL(MD$C9 KM8
+M%Y>>1_Z3H)?PNK"UEQY;MXY:G%GHQVX4[8_K5(P&CO(<C6O:/VW]S?JKSM"/
+M.@R$"PR-C:F.@L3";/;Z/E&;4H27FH>G'D&LVCKQRXG#YNWUT]23B'FSNANK
+M@Z20C[BBQL4UU=+ND/(1D56Y OC->0^5A'RAD:0)&OZ$T.<N^^;'E9V@38ZA
+MVZMIH)M#"0C2K9!2L51W>+\%E=K2E)F21L'SG5?CVIO&C8Z+K@7F\3[O\+J%
+MK ,6OHYV"U3WZ=%^O_I*[+2LOFNZ'JB&3Z[%X$%9K\/(9II5A+\>CLV44)DG
+MY"7V/;KFB)A=4].7@ZASJ'":Q?OL=UEIJBM)BH:'DOQRDJGTL!*[0>HY??Y=
+MFH1(OYRWAUJ2T4S+"ZR@FZZRN4&HP\;T]IH2Q9F!DZV225N_C9);$_T!Y9J6
+MB?F&05X*DJ"#A3HS)L?'&@E>::Z<OORRU[_!P"WMFD<GT*  EIN%LI:_H<*\
+M__KZX^]:>$F&OD%;D[>K$/HNT2&C7!VF$ _$SYEG(;VM@7+&[?+^VI-.F;>X
+M4$N*I)"B-MC^$ ;YV2@#T;JXA(JZ$ N<M*FPG:<1]?/5VD"7H%^=M@L8LP+?
+M(_A<78.Y?'""OPG8P>3WQ-JL"%2M&J##F86WT/<K$LF?R8N0EI'2WXR>BL'P
+MY.VZ(+&==J "\H:'QX.LP/#E[>8/BUZF'QI-5Y"0G*"V[S?D9O%'18Y.OQY.
+MBN.WELSHV]K#\>(NUEP8;K>_4;:W^HE8]=?]M-" G)V?4[6AJYZ2EEO(SYH.
+MTTO,P0*3G@<SBY""JI2@$E#5F^7$\#7 V%&^F52PJEUJU_-$$;91B)8%?(O7
+M\^#^U^XJGYB:FX2=7*FL3:MA?,\=BMOR?)#PT'>V)GMG/YD6U,L<2Y XGUX)
+M5_*33$# Y3?& [B;I:JP:89:E>KG^_6:7=8*G*!8[&^$;(Z@1</8S$[A!,+0
+M2;94883'CNO,1!C:^_4TE?7:R89]>KQ02XQR>D^[0?W8I/O-HL#UDI)0DZAU
+MB0)0^(Z7S,)\T"?Z U#6N@25I]B*U0;.UH2"=%]>6:ZX\2'U/M(5U716I!UU
+MAU>VX _/V?RJT)V2DA"&F9I3<(VJ\_9U\.#3L9.F9)JHII.?H+_E_M'N.AC'
+MLJ26B+DMEGL&T.?A\O3"2VZZG P?TXM@[/:MS>W.P<\+W$D$U;_/>$NY>I>X
+M@7(U+^;^3T^$B;-_E[M7GA&.60E*V?<3XZ;8V;L]]0J6EIF<%& -J5L2]SOF
+M)..<4(JK6EB.FR?/*Y/<H)I=\(N<JYGWP?I2;B5XH9.9]IR\GVPV??+G@MI=
+MEJ.=$Y42&.V%RR5^?KW4 ^&?,$18J<:&E;]$P33A[]8CFY7YJ)R3NIR3#/*3
+MQ,?^U</KD(::E+@?4[!27YD<I<_4ZMVII8RNBUJ N:F,>*3^&/#W\G-)F'B?
+M_I^'2E1MGHKG[)A4BPP,6SO'#EU2L@8-FX'XNKJJMD?P]A#56)N3GJ:]0(]#
+MKQOCD)&4K4(9#][#/G_6O= #>+!255^7FJLR<%C8RX_1K[:$A7N@B;OC]X-8
+MD<#T'M<6O7NCD]Z'?G!;B/,JPSIMY8V7L!]:YYA>@(CT]#ETYC7;3**MC$LL
+MGITZ!Y[MV@],Q_+DGQ6>J,^3K(EMA+=-(OW7L//1@8:QI5G:JH(>;!2"QRXJ
+M+R[F!=L=DUCVD5J60+I_Q%R+1_*UQG;UV46@EY],D=";@:GC\)J8F)M;W8R;
+M*.+3[O8_Q?!5A-&;R((_DEJ? LG"R$Q\%E20$:^YD96)27[S\]/WG8,";9:<
+MB\;UOXW--C77[].KC!/_VJ;VMZ.4P+KM^M R]8]67@PGA[R'HA)=V0W=!13!
+MY%:R5&I>GJT_;)R Q5/7[);$"[J:>U7SEY&\5B95RW<.4*_2T\;"V(-3OI]Y
+M\@__\X:4/1J3QT3^-=.<W"*-N[=<&\V<P)*6B:*4ID??NLKIU<:N)$>J<AZ$
+MDE66KJ92U_*C63)0\X ,CXF;DY/?NY,S].04&].-E(B?LJO2OJ"HS./1[FX%
+MAH>L]O[I3=Q1GB=J]ISE]AB"MH2<GIJ%K::7NH#8 43E]_.77IC&0P9.QQ)-
+MN^R:]3^V]@/KDI^7FHR=F[)&D%O"Q\ /^?G*I0NO&Z^ T=^+6I( 7IC!'O4R
+MQ?:/GHB4>U.UFE?U7^,8K))8C=:WBEBA$ISUM_JO# !"7X>:K!ML#X,"_.J=
+MG!R4+TY!FAEF5YO2PO$U[*- [4:L7U8]CA>_O)CFP<7;TQ"2IKGZN5>GGPAS
+M.;5>5A ;3I:$O*=<CJ^(E '0^8O:]],#UI2HO;N?Y5N \!<,\//_[B8!"=*&
+M@PB.27RR>[#2@N0NZU'H$O7!BZB==5R7@K:4D->IP,'V\CHW@)Z40(>&U;>7
+M1P/+%KBF6Y]?D)!6\L'U->V;]1S=])[\45:#HI67Q<WPL*J3\'U^VPN9@9NZ
+MV//!F^^/O U 1(/<C7X.@(S1*F\7_@"HE\+7BZZT3:6.F,(W.O;V(5_$A+R/
+MG<F[Y@8KP<KDVLSC=(^;_!PM#.%+H9H=)/8__O8^6I"XK)&#EFZ:2FF,EJ+/
+M_H; R=_FUH:N<)O)]8> LF:<'<TJ.=6ZO >M@% 1!+V?BN4)3P"-F-18@GM^
+MB*CJ)3+6X,]04)Y;!?R;@V&PFM7.UT&$4!&III5*O9)WV=L2=^YN&LJ[%^V>
+M^8,L'!J(D2(FN_1!#SIX%YN/5E^Q;BS"YO_$/\M-5H2=KYR&T+"@GN#,(?G!
+M#\A+NYR\%)V%/7N.>R3Q[._&\]JK7Y"+2Y>*JA>IIZE)7I@*XWH6T32[6]6:
+MDE";E_',D)?HU"T:YM[A4""1K*)S\ED S^+T6 O:>(>5&_W',OEWT#[C78">
+M&9/2N;T(C;VI#ML)O9.24?.8_!ULCY#%%\3&^]DT8IN UZJHKOWRJB$S^58O
+MVTM:L8-\#__SHTK3:?&_L/#V[).5W*:>.9*3O%L"P*T-[H-"IR>MSFR?C*V,
+M7@BBPF?DU' *@?*UWU>:HT);D=H.\9>.2TT$P_?4:TX,\+NFJZ@";%6XXS;N
+M\O"]VI6&I%N&,+:S)2L3YC&CGJ]R")Y[PR@P[;/_R7I0<K645WN+L(V9H0,%
+M%I><TE#WN7)1J!_W 1/S8OKPXFO\L[]YQ[)6B^W9^\? $,N.C;G_@K^/ME#N
+M0M3F^^?[TL/;D"6#B#W'%%7(P.1,V.=9P*]7M+Y"DFR]N9^7\O.?/L7MY6JS
+MFYZ*6]JZ7K .^('$V<OM^^!'V!%3O(DII%#8"J& 6M4R-O[U],&0@D.-G+6^
+M@LGYV]Q8W+&_'G29OV<ATSI3[G>,$)*=0Y7SNZ"6D_%]Q="<F9>M%!&:FJS7
+MEMDR]?W +_V8I>";BHB]UK.,S/(QE??CBZ9@20F__):6I('^/,;W]=B/6Y^,
+M@IA>B[!3FPDMX<T#!["6I'A,#PRHKZB7J-CP5.;F+L^]_Y]QGYYK*YN5G[?8
+MW  E6,DNHN6*C'US6A^[&\)CIX/7,_[^/\7;7IQ!C(?9<X+37.$ 3[/3O9:+
+MGGOWT>WV\C 9CG6)O96G H-MEHO!3L#FE)82CY>(F8FACI^'F^\P\%K0YJ>(
+ML\Z3DHZWO$DPP>+ TPT@28J,F+R&64#!QNR0/]!8CU::O8^>O6>AH$+!S/[-
+MY]/4CY#_N8\NCTN@6[?1:OF6]?7%!U>?7UKPC5)"XY*;\,82 ^\%R\-)ZJR9
+M7UK0FZJFL%>MTR9W-<75I9A0@EOP6)^=4-4#Y@^CDZTT45I;R?XY9__P Y!2
+M7D2 ,)(7HJ8:T<,*_863^XETD)J;8I92Y_(E]B_3U(>2X)WV2]:"AIS''MS^
+M-/*(EZQ:@^W7PK6*(.)9Y/8PXI:"! Z?BZMI1I;91/[)R.7"MNN:D Q'<H'9
+MM/2=)R/\_\7N&6>^KU<\5$<:<8!WC4$,SLT-V_Z M$U=GJ"^3D?;FF.T@2?V
+M/K0[HE"0G'@_UI2;]]COX1F<%%&5CISZ>\3'Y-_Z-/.JEI.MH-,7JJ9VE_%=
+M<W'YHK/6FDF,MZ8CF<<Q\Y"6X\ JA&97UXJ^78M&&6J3_CK;.*:D6()'1:6[
+M#<#^)S_N]^:^]E>,@YVK@V2V6]?*7-+JUMJ/6I"5IY^5"W.R$^'S^486[]%*
+MKI$C6Z9.F*)@GKB'B  %&2#&U3&8K:U9&J81DY*05ADDPG'6X$8+D/:8<+Z)
+MN-DM1\'<6@O>?07UF%C),M,P]/X*^GR3AU99G .AEBG-[=NPF9+?A(Q=2]N$
+M3Y\"DKTZQN49B(YX&HF'3K:*FG,;O3KG\ZU6A/.I@RV&4(KP^C<\^\>E#*1%
+MZ!F/CKJYCIO#QX&,RV;0CZ)=J :>#8N")MLI<]7V+._BGI)]FQ>WD5H(:))7
+M8*(+U0L(Y^-8N0X9$+RF0*I?K,Y7S=0YU/Y$\3CVHKREH,*!P0,*T;BFHXV.
+M7;B-1='8T/OGR+F6O@Z65ZFWJ'^;052%R)&@<D6FM)^8J8YYR/#]-=#E%$<.
+M.#JH1MWV7LQK(\7T,/.[%O@;J[BVL+>$5_ 9[CIPQ1=7FHV9>NE*HH6="LC/
+M2-'6V$:D7!^]AX]O>HW]\3@9I^;U\Z:8G)M7LHJ*T'"@4L$-^E[3AL[ \8K=
+MN;>LF@^KLZF0>MCPEI/0N\=:=C>(EEU*5N4,HN"4G(H=I8D2C<CT$S:T._:>
+MMJ"?DX&;MP6R*N#-PW:<D%I?4%&YFZ)?B."1]-7&P_"1VBPJFE.?FA6_X-;U
+M?O)S1P9X6T/JGX^+;]EJU;?5]-KFCY"^G:^_AWB6 >/]WPO6PQ)%,%VJ:ZJ.
+MJ*"2@0+#]^_N]@^JAX:?0J:-O'&1SE>@COOE".;#]^9)FKGW"?9##K:3L'JL
+M].TPY]##+%);A/H'0EL'W(\0CY&TMU"G^KG"XG3:M";)+'G_E:!=@JNX7)F)
+M3T4QOG0#F*7WDZED%C="PC/[_?%4KH^64UAKV%*U0=G'[/04B:N2N,F+GIA&
+MB97"X^[TU_971:1]B:=6N)U&NMCPSXOY#\[9O:.=@$,.0:B25[L'/O;%TE+/
+M2:H9@YY7C2QT08^0S2VJQ\<*U4?B/]2=I5NEKPN2\+:+(O+'-^X^YWF@DYN/
+MHKR7)<O"M R=@$Z041.! 1'4GC7Z8JQ3\Y47MTI;.%.395SS$+& 0?ZD49A#
+MD$ :X3+5-[[/M("7L;B7FT9?@I3PP[DTU -"NI01G,='CK.$ SK8]B2ZQ6EU
+MC^BOCZ^#MEYW30^O_-'+R&]PO9I>KXL-1;176O#6D!?FKJN&QW^?C\J;DNQV
+MBZ'6$*'/'<HG4:*&?*&>ACJ<GG8^6>TYYK!F]%LOM)X9&@F;F0,'![/]D:.-
+M6@C^ N/$\2PMFL.8!!'UEA&8GN2FGL'#0<05D)X=FJ*[F(&6]PWST-7$\+2K
+MCJ2=>EE?E=*\Z?IMU#)+2IK* IQMAX!:O\JJ=<3\]M"+1I'JJYZ]"*2:G5C<
+MR*5C0/&/5I")E(RNJ_B5VLT^.6(ZED^'NX]3G_(MC'NZMIG,S_C5_2?9HM&>
+MG'BS>O!FBY.LEY<!/G7$%D93BA:"75>W6(OR_.$QA8H!T494B8K,]'35DI1&
+MN@@1A)^2_@VAI9E2XLL05! 4W9(U69*P7E+1\/3[E]+@FX6AL]>-#8*!O**Z
+M/:S$SS^ZQ!JKC87[B[@)X_R@QOP%N?>IO(*&A<\1LYLA#$S[(<=1CO8-K8,:
+M;(J0]XCG'_F@8L2:HKQ]V7J<9T*4:%9;04[J"^.+T<, @5^[HIX7N[N4N_!V
+MQ-[O$/_D<)R"D+VDR9Z7 -: AA"$B"R5G9A+!_#@_%;F3PJ<4XZ0E5>KL!(J
+M],+:DH_"0]^D=((*H)I?V.@U;<9ET8%0HI>H@Y1T YCU_F?P\@-)L)!6UDM.
+M#XLMI\ 3]]HP U[WA)Q/.$'/*%"!]]3!:I[U!@<2^@J$F V0ED19VOXSUN;V
+MXKBMJWZLEAN:6*P6F<+"R ,/Q^'W2(C2"4N:$+N"@[B0F_+;Y28:,L"\H)I?
+M]ET;B_7,YQVOL91TGE%:FU+K^>YOUE.?D_ %LY/^FTQ3FLW_J39\\EX4S**Z
+MF:(&]MGKP/?5Q=C*G*B76&C=IEL:HL(NFB?AJ51RJQKO5C!)O '7T_?@[\*B
+MCKR<0UQOBV"F \OL?)D/Y1:#Q/W.@]X.O9)7B4B0+MYR=<N13X6:D;#JMA2C
+MMY,9':'!HR4;\-A+EE$#+(6--A."FZ_)--76E^WC7EG9K;J"N@C(_\+47J&*
+M?7:0B@+HZ9$OY^;"A9!"D19#FZF LO_- L?&788)]7!1'Y>1DYL)P\??[<[2
+MRU*47U>'#*6@O&5B$?I\V+^3H%=:4K9:GX0!5CCZ\_X/:S9<@:^&5I-PM_O9
+MBERO2_; 1PY?A(,/AJMKG?DG,?]^U"?3XY)5$*S/BOF!IC.XR,C0(J+DR\"<
+MQ[(UDU>'AZGX(%:7YSYCYN;>SQ"0B]TV 8NHP0[ \_&6(ATF79>O 3#SKL9O
+MSYJ;M/$>]UG^L%Y;&?KS')ZQMJ^FM)):AD87"JOC+/H+,D>4$9J?DUW&4HHG
+MX#3:-\F-<KBK68]X4J+D&3Y&QA_R4*F%EI6/5DN"I%.71=Y\[07S9>J0>LSC
+ME\";IG1["//YML:23[M=KU5ZSN\6"&6VLR$,Z O'!\S9$"*V>YL C:J<DG0.
+MOU74^1 ^_LJ>=K^=5G>?]\E6\S!0\Y2?DEV" PDYEM?E]<JL-8N?<(++@R16
+MG_$<CM.]M+.<];F8^KBP?0$W]_?OFQ2;0N6:FA+6H/,Z#>CS]_#7391&JIG+
+M4E^5'<<4+3+N;O*.@ER>%(V^@I./NHG$Q,S5P/!'M]P=B_^!;SA63Q+K^3OE
+MO\\^EIT?2I:/&DFF#[C!#,"G^\_^Q39JNERS6Y:;@Y^QAE>9__B;_=IK49CS
+MK821OX*1 O;1F)&1O7*0F%# T^3"=L"C'8@)M8:S4M/&>YK:R0_!CIB;E7*@
+MJ;:ZD'L(U#EZ1L,1F2:@2UJ-6$WPC$CJ_3KURHN_AJNL1Y)&$HSRPM2ZUBS'
+M#I2(3L=:;XUF<E+,_:O3#/+<27.1BDZ.CKZYE@K-,.7^^C+0N70$_JJPAWQ6
+MH):ZR1K3A\O "P](NQJ=D[@6NP!Q@):6X_YMQ?[VSX6@5(R@D9Q?V5=1&*R0
+M6:RPW;Z]0=O\\^3%FUR(L+_']Q^#IE_W"U[3XY"( IV UU.MK8X;Q3_Q_>XC
+M.HNR(@->1T^:D(JBY_62- -#6C@!EIM/E:6*23IM?]>20:D&#>F,AI2[#$9^
+MZ,_@^,4#X ]V48P6EX\8H 9:Q_1#-/+N#P*-A8**1D">5BF&@0D"B<\##JW9
+M-=BN!+M65;CYDJ&.ET<^</V6]\^5@ I[FI#!G1#<\LF<F(&11ER7KX$:[+J6
+M9B>*$X.>LK.*'ZBB+2#*T(95U%2=#(&:@+*@>_,T.?;0(+!+%$&;K&J/CY#*
+MHM+QTI(/1_ [O9Q(A)):NL#$^1)GYL6,GK&,5H>/:Q!>6<W6[:K%I^9M=%!%
+MB72>SV/2725PU=[ U>MOEX]2N/^BCG(,M%:DSM'B^X);Q]:]F"RRDI>KL()C
+MIE<#,KVWF^XB_[*7KH+QKE?E:X-ACY:1]+ZW6+/9(L7G[+_S"K*FG,>\G'*6
+M<#G'W<>05).VA)^%KE."EACQ/N;UQ"?;O5^P"5@;OY!35='"=O746TJ5E+%3
+MA_;<FH0E\#<OQ_2BJ:7XJ8]%Q)*SDAFG0,'*AZ'(5)!0H(N^5U&0<KWA9''W
+MX.WOS4R;M9F<B4Z?@$^3U]Y)SPF'_J4]Z*Y.U5&FJ7*7J9?;\SXMUC_0(Q32
+M4X2068NRY^L*V!V3]I?_BMF^X,(T^L7VVUSX4="00INW8YH]I:/FT%P1M)D%
+MHX+WB%68TOOC,C#RUBE3PEN7D70ZI97(ZL3F]@&C59/:C*N3@';7X/ZX]M;L
+MPH*B'8Q#F,U2H]Y3X,76S?< G(*EC+V6_$4O@%V8X/_A$^?>(*JJ7M#Z!X<>
+M08"Z5X#X#%\3RMCQ8 L.3)+[QEOV5+F:4X5QQ.:P4@F47 I_1:"HF]$&R6.L
+ME%.=H%%>F<DW]3# )LJ:7=Z;4['Z>(" 6>WO#^1'T'((FMD6&\.WL_3B..1&
+MQU:MGF&KFDM\3[*GR3+G\!K!8)9F*9F3?9J;O_&S[.6UQ -?5X> C-N>HJV.
+M6/' I0"3T>9<I9Y$7YRNZ[17GMGH)N[N;]"]EI2S*$Y ^E! IOL@_O[#YPK^
+M\>6:O[58O*>HJEN5I)[!OD:2=N G'!!>=(*9KXW5Z0,0CM!SE4^9OI,*]\,Z
+M=&?/AG&4G?69 %>QM_O7'JO;K?R^O:;97JFL'?L-T.$_^B+P2:>&.1/95X>B
+MH%GT./;B<KN2J'-:@XKV=[Y%4N'D'A3;E)">Z9?;OV-D<)]E#M3YRU$YBY61
+M[$^.F)BAI(MQU/4WY_32HH^<U8'UNM:F982#%P^KAPW-PR4PJI.$I5B&4IB 
+M286MQ20S_^YV$]Q0WA"EMY;?3,S%,"SR?@5&$+BJS13%VY6Z"O!)RYM.<?Z.
+MD):= 0++6(R0@X3TMTX8D):^F<1E^I3#P)J5HD#2CUQR&HW!PO/6=@>[VJ/;
+MJ:-R_Z4  1;EVI#V&6.S_92M2&V;I'9*R-S!T,;VTYU>K,1)EI];N$>3))'5
+M$!3[\+TLFW*6I:B<0:.20N?B3@\K!F6OT^L/J8B#MX]<U*3>+>,2/./%T.ZN
+M6*1Q'I&^FL?<]M"?H)1WMH&:OUG^_)?F]<"=@#9>II%+^JRFOL46PY;^O<&-
+M3I>;O6I:&_<3[=+FPQ"#A$KXEDN;E_&*&SIU^C7/$[:&>YPG<A6?3/%S]-?\
+MQ<5)5@3(ZQ04B+;4B1=*#:J+I3/BIHV4F7^]*X"/UMG",7_V)D.'+?2@K'"#
+M'A)IGYO![!Q!S-WOP)B&WBR:VI>)GH.EQEO,Y,$?POKPG-9QO(Z)UXNHPMKC
+MK***N194FK<"Z#/ZW^;;K8N7K/>7<XNVC:M)%RCPA()0A(#R^KM(>XKM^^?F
+MWB$269JXK1.14E;Q <#^9I+2:U*,AE^;FY)7@L?:PM,>^FS2O_"8A)^^/KF0
+M4AE#1<_(]=5T3(Y_KY2<AHL3V_;2OF52MG;#F96Y"HR;BYRR8/J8XL[;],?,
+MWJ<;ZYY=6Q8/B)IRK92RX3CW-NX7V9^&%)47B(.+#2TOXE"<DY2:1ZI9Q6K_
+M-<0490^3PYZ:$(K7IK>O60O(M%^9M-D2)U*!-/;RT)+C-?(;L9%2L-*;AS^4
+M=8?:P_E[YA6;=+3_$BC=C8#J31.19G#028S_OXY"N%RHI)0+0<!&^0_;\D^2
+M+DV#7$E"H)J#)20WT/HFR(T^G+F"5(^Z4:&7C0D6F_:.#NS#Y,B<$1^"4X=\
+M6*REC2+7E^ V-9L$V(N,FIR[J-#)RI1:E*!4NE"KDMDKW,;T[AG_IEN=5IC 
+MEVV6/\*/V]:L\ G=4I-:#4!'C_"K_17PVTB2HJSPN>MZ7XMAS=_\^L;%;Q)(
+M>IRG5Y>5O@3CDS[F/I""D%6*CMR52JSUEMD<1OT5V1Q&O[F4:7Z6BZ#RDM/B
+M\3;RU\"2GBB3K%YOMYJLGI_-5@OO^<T,]['8V+Z?TK2]6IAD E<8PB=&UC0C
+MG(AR%)R@FIMM#NM4-]"1D9J\SIK!Q.S@MN[%7)RCD57YF%F0G?$I)Z,9A; 2
+MC[6A"?^QAIWK--669]/<6[N,C?A?7Y>0C$4D-7!@T8CUEI.#DT_>BKQ,\.S2
+MNCO6B;JMA>]R:X*J6DT(S\O3]L\1S[11J!:+X8-@1/W3\O/0+OK7 QZ>H9*$
+MSJFGH*>=#5?HP<H#[>$:CY_]@JA,C992@Y>MV_#PY_ EVU06<+E?E8)3!WVE
+M *W3H-"F2KI;9]'Y<M;F6ZVV6EF$ 5)ZIE*9).OGXE9SMIF404^[LY[WR>OU
+MQOH*UJM.1D.)C01P2.B-_M?FY<,^AFA0JZL&MJ<4<<34T#?6(QE2VFCOAFZ 
+ML(W]+?H,[UR@Y)>$C8J_FXNGP9I(TQ;WT[ L#Y&8G-JJI(B>T^!6^-'"49\K
+MS(KE18J5D2$#CH,(LF!=PU'>U,+:]<-4$YN\D@$"EPS*T[65B8N4AAFJ&1?K
+M[)?RP@MY]I:MCZ% DJ&,DJ?)SM*%G(.;F@*^5L)?U^5[T2?&SU1*G^V;@X>]
+MU9;%P7)C>CHE2)9!NP&'6YO3*O$S_6XU^O+"A'Y 0JI;:4765Q',R0\' U6'
+MEH6437W%1J"6K0=I[7OW]L-*CH2*WE(+F?&H5E*:[M *^(?1Q0B+5)Z@G!:[
+MNH)@7%^A4!6^)B+G** +E-")N9K+!M+DK_K+W7!0@H_G]C3P\K(%_Y&>L?+%
+M4[T-NH+EZ440G)12*QH9FIJ5CK<5\O?W\\#)CY:A^U.-J%"EC,#Z-MSR6(>6
+MI:U6RUJ2 X_ UM$F-/?SAQZ,+>^<K;B&6JD'2LCEWUN4YUI<3&[7BT=L-(#7
+M$YD6T!JA3];9F-N7OUPYQJ #18+8\<K]Q!M\[5V1DWN2AIQT)KI+B+!D_J/V
+M WS\H"BTE[;SHH_/M(6SGORDB9I1B20^T#Z: XP)H?V65Y:"J)";PQW9V"Z1
+MHIR465"3AI:71\?5<G?5$), 8%C629*5IT3A%GS:=]N3EZKZEHF_DY*D6.AM
+M^^0ZI[_&=("'GF#OQ-H*H/SM^M-FBKZ.7"R6WHUM;/I;43OY]O[[QU%]AU.1
+MD,E6B>S6@R#J&$;)APO!9DNOKW"?CJNJ6[JEW>CY^77P, "X49I;IZ"3BL#%
+M)O11HDN$Q*5:K\+2K?#&QMM<5(.8LB&V2X9PJ2G_ZU&?H(&>GY._@J)/F8+ 
+M[3HW\N"ZC\&8\8]=OX.^(>'%<BVGC?9AF/R>GJ:A0!DC\^8WU\-71CC825X!
+M*J8 D,3J>,7NSMQ)C\Z,'+X50)0WB<GJT<#VWO*;5K2A@I"8J()FCI+9Q]X!
+M!A3U\1!)G9RCFI"2G%/ ]YO"P^;F>N^+G*(DJ5VW@H/!V2.<G#2FG=*)&H,)
+M,O[[Y<7"A'WV7:>)<JNX1I_-5O,3WDF21?8R\@JB1H,#>CG 8"L0#T^P,:N+
+MAZ17B<"KM=8N*(G7H/NN37^"7Z#8\<,Z,'K*@XY=X(M22T>26K:CP-7?WI,8
+MBS>L'&FH;4\FLM-,WC?$/!?S!UAVDK86KU>#D*:QR<HNQ*O"0_+@GI*^_P/3
+MZ)R>,=J]+,)4PI(6"ZZ1\YFZ2(NHJ<FFB%&9A_UV])I"V.KV]99RQYIUHH>'
+MF).=8Y>:R.+:P8^;D;ZPD5)W!8RXT/;Q)O;U0Z<G@%<2BU@6F(0 JO:PYL=8
+MCZ2[O[B^LY:F\_ 1^N84V8D:K$RIJ+]?A##3<4'!"2,)4\M5B$_IFHT31-NZ
+MV5HE\C_5\\V%F:">E<B<7FQ5OPC(*(3L_M?GT$V.4)4Z%8JL/N&:NN#"Y?60
+MTG%:H:+[E)N0LO*%!<!0H%2$EI"#LZ#F1-#&]J.8=G*)O9&!F.R&@TGOT<BO
+M\ 1[7Y$V%KNGN\4P]O64 (6*FG!_BD=6FIC/T7XDQ![EJU![MUG+KG6 @''V
+MU_O[Q 5NEKF_[K:$FN#6E_+OS_L6PO"6IX:5[ZQM:J.>>.%S]]!NI^Z&]DRQ
+M=IJZBH&B#(W)@@7+QJY;ZT.[MZE;"*)=7Z+CUDNM(N?^P-3BA" 6NQITMBT*
+MBO"+_B*0'8R'G'O@]O3N>OKS3$G2^0^(NE<#GGK#S\M02D/Q_W:4RGNBMXG!
+M,C#V[%#EPY1F0UKKGJ;?3B7W]?0UD6-WIJN;AQ[%F*7A]#Z:\._5BY><3*O[
+MG3NMEDL- <;HR\?PEH!<5,3RGK^UH(M1<L3PYO);1S@=6/IR4X^9H*8:T:_)
+MYLI__!=42*X1 ]X/B*@;B!0Z[3K3/\3UT)3<D-WR4PO]X\2KV$6(A)% 45N8
+MF5XSQK V"I 3H ;6I):ZE,!=P:\"R;>3\M"4&KJ8@I[[\6/!\^0GF%O[MH.2
+MB+WPLL#W]_4P-!L:D$Q=NX=6!KM!T=XY.G#OVIF:K(V#!ZD9N%K#PQWDR-[%
+MY ^F#$SLM[MJN%ZHP4/TYA8P6LB_EK*"?E*:"^@PEV36  <JW<>G4*_[GW-8
+MCY."H[G4^<CH-3OUDLBZ K*51*"6N\=7V+'WH8.-E)6*^<3T,_?2.\ZOD[H[
+MD,,66T!?GLG]3UFL?'(\G8E64WJGMR,HX7^01TN2CZ0:K *8UAN463(3T&W/
+M1Q0I_MI GEZ:A<'J6;:B=!4#D%2AC99-F6 :6UC(6\7E#WAI=8R\UU>$BKB,
+M5P#4V=/E[E.#>'M56@"Y5X*J<YE-WH#&S,K0=?2ZGBRSKM^/$_>@7(/PTF<6
+M[N6FC NQN/>0^9[$PJ75WJ6>65J=BA$A:[E3T/W%6EB G9>5JECAC%?!_0])
+M\* ZA9:AFD*!GYO8\O;V<]K0FI:4B*NY5I1'C%#V=3:V Y*[DML*N)>4DVH(
+M].W&-,;PCU8)[9J%Z[BAUBW*S.L%3R+8EW:=C$M5A)"I^I[ NN$0XOK'S;^+
+MH:HFYJNQPEK#$L9)Q*OL!P\VAK);4*Z@K_;1H8X?<,/3.C_@(X1V?H>ZD+J3
+ME<GRP9V8MET2 QQ#\R(^TC3V4UV05@U:HUN3;-K1S03'X_:BM(4\<HS[M$!7
+M4_?NV_2;W8>2IA^,:IRG\JP'GK;:Y!"#7FQ LY/=EEZ64O(]W^62SX^V>&W/
+M6XV I%^IX,:)R4]#FF,?J8Q##<VKADZ3X?3VMC86BUOVVYF.WD^#A^FGDN47
+M^ K0R I:YEN<O;,:E@N<=F'46\%B\_#Z),.:\-KU3XC:G0U,T;CZEI.9L@5+
+MBU/R.:?V\EE1B%.15?V<FSF;]O/7Q5"$").NEE>:4T1.FJW"+);'V]R*D$:K
+MNJ:6\)<(D_GWQ'XBIIKBB1:=.O(0JJC$)-_V9D^GED3EC(9?3<"0O]G_J,D5
+M6]8I1@^-AYB.B[F0?<C[]3[$]@"[?XE2N*;BEGIMF@,-?>77^"$HT<:+_=&'
+M?@Y+G['&GXD*F<'W]<8*O)""B1X'DIO"_]J9S-02F[>BFA#C\O,_[N;)7[R2
+M'02(GHJFE[K(P_#=J$B;JZ8E>_M,%_T16_#"<AG4B59"F*YG7X60A/5JUW3$
+M#YU'IJ_YF5^"4T;8PD,0--?1@Z>?O(FL1(FT<%+)S=;+(=B63WN.CXF:F;F0
+M5HL2N^3F)?S(DP>$IK>WDI8@99^;Q>QYT\'_C<\":7^9PRF6NX)0A* )\<#W
+M1</DSUJ*@967H)9"\0[8F9R(2X]6"):76?G5_A8OP:Y049S0=YZM0%6Z\?Z 
+MMH?2A+M2BJO=8Y^+4M Y=]736;J.NXC[O01]E90#T?,L]LL?#ZF]0DL>G'. 
+M03CN]L8W\X=>7#VO5:UOS .:S-P.S>O'X@J&1-1&CI2;,/#ZS/8M[AW^!Z],
+M%5,;\JZV7B , QD(J,>*K<G)&AN>#?><\)*;DI 4NO?B>>#WV]5-ED914*'Z
+M"\SNP_91D]&-IEU:D\/6N-65_L\=O88?4]2:7;"0EW$'04F9D,I$IYTJ6_AV
+M@PW &4?ORV2-6BR:DJJ*IM*IX=0LWI)/ Y*DDHI8BEN53?$Q_Q8N5\6LFE&Z
+MAUH%4T9-S2' ZP#2AM"?SYV<J8:-F8!7E]D@]>?BPMEGN%T3S%JO%I>IMHO:
+MPX'$V'S)\]P;FK2%EZ9-ED.@6EOB]C]^W_Y!=8F*CO67OG/A(],5K]A*5))3
+M7X.)*B#N0!*+,!S;G*1QEY*CW#U)_X+<&-"6/(ZE_X%[EIW2T?ST=\/+A_PU
+MU_N(OH^E;,&X93+0(PN4H9U!FWQVD4QEXY;5YO*!E[I0"9S^H2C!I9+2S(TI
+MZ_'+KH2LOX:/O*J4$%/Q,><:WC7' SA0BKHW[KN"#):=T5[!XNH-S:[8D5Z%
+M0YN60IBBK8^;I?;DQ=3OHZV6&'B:HJN=$_ZC&;&:4?R7^).1PI(L_>;$*5BQ
+MF_"%\5J7::6^@7SQPIQ1HIZ.B/);\HZ9#/#@^A^GUKB'PI] CXN?ATX(PM5&
+M-_.Y3A 76X?XU9>7T,'E+9HD"I]4#X'IEXWMQ89]PLP!Q5U%V[NPF!6-/D[-
+MJ >8S9(Y,#J[Y6&(3-> GDN;FJP/N>+B"-[1W*<FY@J>7;=(AKLJOI9WJ/7_
+M[3?N%\^>E*>KFDB#D2W4RN"5D'Z,FM"9T,%^P<7$/E,?EEY>FA&(NN&V6Q3#
+MP-NX<J/]5;>64[FD0H/D^=?PS]!"AFJ/J-M2EE.X4.)LM!7#6Z:ZCZF0K%;7
+M:M+"+5/U]]57''F5BQ>=2[I^KRB**P_CQ]SNM%H-F1:EH(@WJT+P)NX6[/61
+M?:T1 "*"BX)IA*O P_H!Z^A= ]1(F@L@VD:K@@2!AIK'PD7[U]/CM=PCE=*7
+M I<9QO,<N8!0CQ*=J8+%WD4WTMX!?91>54R@DBCB1Y'E^4]D'?  WJ>9=IJF
+MM9TKE^/N-_*;HI.@4H&/N)9)E$/S^,9V!]E08%&SBKW.G^_%T,5_\L/V@U1Z
+M:FU&:VF@I+_3^-:OWO/9GU>6KHJ=CQJV_XE2\?F:U\8GXYK.OZARJH)0K Z>
+MZ1@M]/Q_S.,3RYZ?MT*ZJ)J2IG")\Y \]O8T 4>\0I66D*B!P4TCT9GX4I".
+M49H)V.+T_Y VP)2QDX^6<9Y0DHR=V>?/^:PCE%L0\:GY1!^# L#35A?BLDJ,
+M-E/9NI)4'[R-ZM,7N]6? NC_6ZM.)J"_\.@1-_!NYHU/F=6;6%6[(Y>3R_[)
+M+0L32@R&6KA#7 5*I%I!*/B3N]KMTXN.6P";CD&:0&RRN^S\V,;("A^CX:N-
+MGH&*6H;^D('^7UDQ^4;%Q-NZ2%^6AE*?^L+_#[8&B9:[$'<<C]/4UK7R%A%]
+MH#!4O9BHEZ9/KZF#R-*.F)!\D("2K6:@FLMH.6_$ST:3@&);G%L:!A"=P>*N
+M_I &V5;(&=.9LI>3KZ4?\304]B>*ICWNOL:N@Y*GK5B,#H3! +0Y39Q.29H;
+MBZ&7U^<Q[O[VPL^2FE"72I#- E!LL)/"_>[U'"W>!M#+J#V74Z(;&%.@5OT!
+M]"5N1B;BG*!QL3)S@8G#6L,:1Z&+C573EINHXFSWP-> K)*7D:9=2XM@!JC3
+M%P+ESI3T!O9\BIBF6@/-_NUN\"71BPY$]IZ-TA11J!":_)8UVU*^)I>2DP^/
+MH(^-*;SL[; #2869B$6'G;ESFHKPU FM!L/EA7>_P#F:P8^X%?DAXFSZQ>_U
+MJ0Z(EKBZ@4NAH%>3".SIALF):P-"H[ROMUIR ZX?DG>"C>K6Q_ILP-_\N@Z%
+MI8*ZS=G1UEP1LG:U\*F*S?G&YM(OVKI=MJM[MXG[>)>"T<\@M'Q6DY^46?#Q
+M((*HP78W=C?/X$K=Z8/\CH7<%:C%(#/']L6Y<I"]5Z>>^Y\4&>KT]M26)\S6
+MCXP_+>^M+*:? ^K/$,%8\,*T18^+?HNK@IVZV6@5)\?@((</G)'SAKB[B_F2
+M$*,<*";F"\+BD:NZ!8O^7[FJMB9:ET+SP)_P\ 7ZMI:=AEV[K5*>QQ"0($FO
+M5H-9D\W^->;U^]5<=O.=1HJ*F*@, _#,UU&_MI&;<'%7F+A>J]4R?; _HMP/
+MI&)"D[J&P%75PN+_[]+6D[7SDU:M59*:O\?$/T;Z)B:.FE2,EXALDRQ4N]C 
+MUH55#V OCE@4AIP/2*/;_V$0U/Y YL5*F];95Y"]G+.REILBABFBZ?G-PYV(
+MK+V F):;7K>@7@O!8D:R[M)GO)*TG(^8BJU!!-O@4/9R_I90UD,G,_,_]^Y&
+MD<@+E7+0@=V$A:_K+2\;D=Q4F170BA>[L%GPZGG;[@*6OP>,2*A*A+7>B 'Q
+M^,?0 \]6H"J1BIH<0X[T^C:U[/7#1:1VR>/_G;O$G?_(B$]:Z\76B9Q*G$>&
+MNY"[IH-U<'FTPD3.ZT>LH-&PQ[I3K(VWP>KZ"L>M_//3D7+57YY>F)HVIE6K
+M0O#4]C3EX71TI%$&H[(MD-;;V+FXWE^PT5(ID/INVC)6H*V(?[M6FIF2N!XK
+MQ<W3<X6B(YT&QPI7)I96P33]\'?PT8V%B'>8DI:-B:RI,\;%E_*''8!=5NN2
+M&O\!Q\ W/L>OSHO]D.V-3KZG,/ S"0[>H\OE$%Z?AYR'K$R#,[!XD>G5-#IF
+MSQ":A0B,=>J6,[JCF0-*_/4H M_RP9%'C<.,I$:P0OA0BP*D]?XZ.Z:7\!*?
+M4@F;JT+OPOR0D+"YMY&L>M?_[,>L=J*P")Z?=H&IDZ8U^^/%X4C\5J*HMG>H
+M6*V,347V7)?_YU"=5G*9M@E_EY"\X4'Y.F[78H>0^HAM&J>#+<N7]3_L--M"
+M]XQL.8B>S7!^",7D"\#!VQI)S@^EEUH4FX6'MP$4$SH>E\*[EI674G*XDY8L
+M%YT(Q='2QBGJ#U&Z])]>L\:JG+ZLAIOS-CD^-# #F$&P'J" JYN'+0&P.G(*
+MFUI94ZCQU/GPX"^#]YA?G/J<DKJL6X-%U^$ Q#R86QRE68B 5EK90BTP+"+X
+MVH)L%KJ^AEKW+^#F-O#]I2O'P+JZ6UZ>")PM?G/:]^?9IHY*5^FNE5.&C[K!
+MP'U2RR+8XQ:4Z)RR%-BDD%$H\O;%Y?8 JS*=TKF5Y[H>H$8 \0X$2OC+ .76
+MDJZONUKZ2]Q:;81XB>+%0/OR<:Z4AE$2U]J]\*W1BH9)B?T:6)>"Y_OU]?H^
+MUY>A@IU?DI)8ECV[S>L'29&PD)U:HP.;:/:= 7YS?C3OUKM/L%U2'0Y6(0K8
+MP!'D]^>;H&R:ELM?A(- 09^L\C 0VT,0C(KM7^2Y089+]/KNR.=:V$V41DJK
+M3;ZYJI29\RO1]K*T0J^>'4#7GXV2DX"&F6<'[,7JK<G#.$,?U)J;UF\,<B.%
+M_^#P'6YO]"^4=;&+]<*Z6JW<X</TR8;Q$)FW^_#R).WBW@.L>[);=EUV=V&D
+MD^C' +)<"%^9#KU#N)*$2-$^9/8^)I!"C$!^F@Z<E%^.H3.Y-^[S2P!!@ZI&
+MEXV*B-CZ/37?Y*67-XY47:Z+0)*T<]C*7<%WV32_4(QLQIJ]F4/[MZ7B&6#R
+M58.&EGD1%A!'F( X7:KW!@Y7^$DFVL"0J(71DYJ#@IX1]=T8Z/S\\!#EA8F2
+M4=+7K/?HSQ;UCDN:!7YTF9O!W]3N.N8EG)&3NP_7BINRFD-G"1KPG(@&%Y!Q
+MJIU$)J\#XM/VEM'V#4^D>Y&-1?00JBWZ-7?GP.V0I%:0AJWP\HG ]#;Z]!):
+MGK2'[:_8;-]L]X+8W.'JI\$)AI_>X%Z>K:JYL(,*]^W$V_7PS9*57ZI64)R^
+M,_6N]0+ITOZ'XL$QG8@>M7GTBU9&B*6S8*K&QM#GY7J+H)6F4I/SS,M2X?V(
+M.H6UTT#S"1[4QCI]PP0R6_R>5UZXY9Q*]R+2U86BNE6,D8G#>*3]X\+Q_^X%
+M$D+R(%/QQKA3GTP PC7V']*)5[.^FH*L3Z&< >+VT/;%B,>-G)Q/CHF-D(V_
+MP/S$S5S VZ_:F$B'TA[J[!?_$_?]PNXTSXJ,51]^!A-RE'(0<.W-Q!;.R\/"
+MU$;:!:&7HHU>D\&:NP#S[S8R/>:X@Y^\E%BZGLD-HQ)0E).TP%R6"N?P-9WV
+M\@%=D9J-=I*KEY;<7TC/ZH6%G!.,L)":FS > O?R)?;GVLIH]:.#FBV*\+%-
+MK3+%DL8!0(>@&];FTX)2E@V6Q-;VE@.6FK:<C_B[AL1>$H'(54W'&MB7D%0:
+MZ9R!.L"F"QD2T]?0]"6O"(4UJ@^/0Z1%E9=Q&"A.TLI"6\!+7<6CFG^(D]ZY
+MCM+!,>SNOA037(JD?50#O%DMPUNT#(*:$9_53KU%D&;BE:W/79F)K9V$@KBQ
+MI)C([VO*=8F6E)H)7JB@A:M(^=DUK\/\3\3$F+N*GX!S'Z7VE1KSVYJ7I B"
+M T2$,Z !WD&O]):BB7*$GIZZ#+K&FO?11>WJW=$9BJ7]BD,]ELJYCI*B\Z'Z
+ME^7 IU(<D8Q?B8I^P;?#Y0I,7R@,W4>P0T>K!8ASI_SV8G0;I_+9)BP2!YCP
+MD9":$0.(PRK#"9I4@!^.F8+3#70Y,#SZ2KIP6OBU]XB#:;>#!\U%/"^B I2B
+M7+*]0):3V?+')G;%&HN%D+U:B(>:DT_9DO4[_ .&FGJ"FI/ZII68U\*L[B[O
+MX8=%G[Y4OXNM1C>9)\P$S4OKG!>.E$0\OX22EK*0T.J<G^7P(:JUE1.J!YT<
+M6L6-FO-]_<5.PNQ#UIB'E)*JVHOSI$A.>PD3]?+V+G&X0_>=7IERJ,'?S^"5
+MB!X=A)<H@_2[U?XV=<5>D%)4M!-3K:.V6Q$)BN <U*";CX*LGW"$5R#_Q?X0
+M)=R"L'#;]@URH(N,6_##$O_;6X7LMUN8O:75BL51^97 %]J*AOIMXI*="H(.
+M6/'FV7L+$YF?5(V  UZ\C4NT4N3H[?^2UO'CG;\RG#8&BK&0=?M'RE 5Y@G]
+M"T@3?ZVC6_N>@_9SFB[1D/%P,F_:G8J;KYYWJYO1+L_6AZ&V!9."O$-SZ_7&
+ML#)"A+&BKYW7?JB3GX]PW=#B7IQ2O( WUYE(LI+AZ\;^==D(JI;AUP(GEGI?
+MJ-GT-3>2YTM0^)-[NUPR]I7LZF-F.D:G@Q8NK(*8J\^DO9+,U:L?)O+6NU^6
+MC(<854> #'K!WB[Z-"[BO9B>@SN$DUJFLD2J]_P*)T;2R?;)TH>8@(&0@WI*
+M29* S93'5?_T6ZW<5(W?\JN7$8;NX8BV/JE679IXS3)4_<#VCK^D4GR/B8*=
+MI9""Y,.EU%^ ]JV. JJJJEY102O?$,7IUCF.N+.^BIB2_[Z'_K;'YHJ=O_A6
+MN9*6=]"\R/'Y;AO$TS\4>DQ9C8U&L#JK@LK\R43S$\] %F@7NKR=0X:SQ\(P
+MD#?%K[J<>(%:IXM>"Z&6JZ<JR_/)S\D;X*M?FW7ZA@*3H93T*0#AG/#B;AJ\
+MH'*0\@$" ]G&SQ!<BXF5\IT\C<#[^)37Q=#_@(FLIG2\>H #J^#NT]2_B)8%
+M==SZ@L9?NO>[U>;6Q=2 EZ&ZNH:'GE%, NHU\)X(Y]*2-PF*]EJ11>7R=3?5
+M\,-OE[V=EJYAB6"DN2S*CL%&U1Q?IX?!C*P-2Z&5_\%ZM-;EYD$9N/R1C%R3
+MJYF3IK;8POS1!@J+S_9C?+V;@9N2J'.20YT@YL;V?O8BF+96&9I=&G\55PMF
+MA(B3D)>8J[,-]#UT']?.NJ**1[:*LOZT1X,!6<#6C)!>1*0=RD%@FO_:^^;G
+M]<+VS="MLO*OCOH I:/R,1?N0XJRI$E+GIZ5FX##/GGVX-#'3X:$/(FLSH>Y
+MDC_+ 8Z3SLNV382'BKM,2;MSCE^-<I,4.A )059]F_)R4YJ&8E:W\P_(INS2
+M"M,22Y6,NOR5[=R2+:2"PO'Y9'?DPHV5FXV:H1^([-;&\%"CBZV6,KH9T-23
+M=OK:\TV%^,7VE*I0D?:8\4DC5GR\LH]R5X.*HK.;FO&Y-9/1-H..P!C)B-V@
+MB9?2TNWZE"=#1^9+?)*J%:=:B3[7YC:5P[R-KFF6UI\K@_71P<1/RR&;P(E?
+MGDV=KD&(NK"#"1/'Q_?2Z-\-QY[^%@E:EZ*F@]C6",M.><>'BF^?CU>"T#B;
+MDL&DD,JS].[2YB&T4G);AAR#J4WCBT6828H%D$JH?W&6],?W]@N>B9.1-X""
+MD+0V4LD#KL#^$ JLH_T;F6RV0N*:]71FKO!GGJ K7$*3IA6DQ]Y6DM?50J:6
+M>]ZM6EZ:G<V;L^_T\,]'3EQ7GI>&N4)T=^$ %JKA 10+7ER/YJR\AKFB5P'"
+M]II0K *3WDVSK)2IC(.AAIW9Z'HA@X7^P)F3GJM;J ^;_%*DGJT!$'&6^C6%
+ME'04J=ZS_ZC@S/"3VHO[KYN(^9[2U/74NM!'^*/2A-I7@EZ"IGOA^0[=WZ8Q
+MK=ZQ0EVAI9_A(KG /J"<H/6 42J[3(^0JG#P[OK&2ZN$(8J034I'HZ_'U!/B
+M]<8 KJ1TA .?H(-Q]Y*-%-3HU2%!JY>?GIVMOIB S@-8XC,VFN8*KYH$B[F$
+M@MATJ:>HV-7KURX2_J+42\AY\MNPGKN*N).W3<;!;_/WPHFQFGW4L7Z7PJM;
+M<?F"MG&0E));TQYD\#?OTSR@D:^EG+I+88;[#-WBR$644D^0>3Q+8[!;!S[S
+M[?8%-HB/D%J#CY00N(6@X!$W]>]+592I5HB'GZ-8$_XU%CI_P,T>?YB7L@EH
+MJ W6R0_NVO3U&<*6A[_75>V+HI>;\_[U\#9&X(B^G:""=M#>TJ $6(T-RXJJ
+MR\%!F*O^F5&[OD&8<K!;CP#4,<?WX&/*@+>0LIV8E]3#Y].1D$&M5:?^LT?J
+M)SK%L]JNAE^0LA=[NTV&DL<,RM:\T%.-4%RSJ7A'JJC!M?3'RQ.+5J!7";BK
+MOI,\P7+U]/?+B\:@@)Q''J23C.6S]2_N]N4>M;S,3YR$@SBRB,'5Q(3U ,&)
+MEE^!EYY+4,9^@(CT]V;U]@.M7HUSB)^*LE'!IIW0PO@<S-K<SPF=3CR7MYOK
+MJI.1HE',^CGV-97S?9R@KI9;B8K5!/+"!)CWA+!QG%NAJG7T]M3!7KV2A);'
+M68MJMI=#SNEQBI&F'J8'GE<FFI<'DC32ND4QH-V1JQBJCAZ2G]+ $2 3\8O3
+MAH^#B]Z 4V1#PO<6[I?6E3:8O*OV/YVAED"@V('1 ^?(KO).GD423*K$LE=1
+M]D=_Q^YSAY:1678?HI+^IE9;R,_NH>G RP_(D72$I:N7F1FQH8Z):?[YU#K^
+M IQ3T[E&43.1PHL'T[CP6AZ 0O:0\,;%0KOV HB((927D7RMPMN2Q://M'HV
+M%+BD01H]MI8.P?OMQCX#YDJ6K*JHN5^>=X^%*FRR[\5#5B1:NJC\OO4*67OQ
+M1Y)RH)Z@1\"&FYZHS%*:]]W/R-_Q,)?R.M2_'8F-+)!8R)O$U_HFVUF.L??9
+M_NM#F9#W^(E&:N7&>M[G5D<$^WM^5A**"I2-F.$PQJ_^%=*L4Y_[5O2^0O+[
+M!V9<L!*=GY";NR7R^99&,-J<D 1<C@BV":"5F-CI0\"<0*1=UI"JF9!4G0'Z
+M-? ;]?RID'F;6F,2 I>4PS(S8/K:C79DGJJ3J(^5K=<Q_9X5Q!=#7I!=[(1I
+MB\N?ULC6S\G?\U)%E9!,C1>KSRRB_\#V-.77W@5M1ICUEH2=*HJHC_J@R@A%
+M2%&M\N)M7X6+6(_/?$*B]X+K:J:VML0:^I"PGI(0FH/ JL58?8B7G195+ZK-
+M\>W@^CWCE;P1\5>PFJO@IEL#*V?;D9/47T:=EE= 5%W9TMU<$Z;V6- A@IJ"
+MDA7RBG$R_336B,. 8)*#V1N&!:P!PO3ZU?(2O[Z<5\KX[5,ACIW*S=?Y7=J8
+MBQ>,[ZD5CHJH3ZK1,\3S\% #1])<OJRWFYR+L%J:E.C+@0/C2//0:UA<H*JV
+MKYO;@Z:3U_9U._?$$_CVP!S>&9SXIUW3,ZA<BZE6TWP3S>+W]CXP)52\VJN5
+MB/;SP%.+!=T#286<-E4]B;F)1O"/I_0M/C_#";J&)*FR"+6@(*?E>=-P5L.*
+MI[F*_D"6EJ*4V//D\#;_\HR&5<!/GXM;!E17PP3-_0;CRXNF')14C(%'9!(:
+MY^G3,$ 7TJ^6%9Z:6J^I\\.D>^#/Q$$)Z/?3.:JLA(*V,I"*@ZQ=U\,Z,2[M
+MVJ:<$%).IO";LL?\RN"%HZ(=18"[@=J2Q)7P1>54MHJVAB":G_I:"PSM#]&*
+MG%&[]J>2F:R.2UO^$78OTK@/AY/#>@*2%;J]Y?F5(."@BPX@>HH[+":BE=K@
+M.;0VYO5FGAI,9KB) ZP]JT7.%2CO@]1&MEV 'Q6,B,*Z_P+FE_OR) <-A?M*
+MV@Q;DXI&%:O9!, E+@K* P*;>E"2C%O/JDHIFH$A_F3$Q;_&E+#+L:60K'D9
+M*\_D7HBB]7"A?(K7%[SRQ^Q+<;"3^3!;>@\AEI,;#\7P7[!+G7:;0G]LGJN1
+M^I>OP,5<2;9P HB*C]1!3=#RQ>P0 ,MUDD":0W[P@Z'!<>TF[F7+KZ)059]%
+MG*DAFYL*W LA%L4<S9P6;%:JSQM,=ZN@_G=F=_H/2(8Q I:@23"?B;9"]>^(
+M)LH(#HG("H?=@:R>F4""N+>31-+LUA#MY5!85%&02(H#P<*O\9F+H=26F8("
+M\O+Y]BZET=>0E%VTUUA:H/*6VP,*V%2;WI&5V('_0*:JX1*=[Y9B^ K7D%B6
+MJ)H&5:GPT#"0.H"=0*9>D(MR&_.\\L)%-N[MSXT$""J&ADE+,II7 ,[[!*7:
+M\LRWF@9/K;FG.;(O*1+EQG;@P6FJCIE*A8V\0::'FT$#JA?1CBLNVKL._55:
+MGFJ GI/0>/';N47D8-'5M(J=!=&V?\W9PM.=AK*[U9""K\UB;/3 +\*$6!'\
+MFD&"KKFD5LT% -)_D)@&MA2<K6"7F]E@T,8VP+#FE^F8D8N\E)?LX+[%\#I!
+M3J*V&+:+'YI?A 6T\Q>OQ@*ZD$>7B89-TJR:&QD.?Q'+RO&'=':-B7V%7KI7
+MFR?3[\06/MNYFM"#.)^1B98PFO\E!^[A+@O$]WQ"KHF? B>+J8+#E)[,TOG 
+M=9/!6 &25I"Q7/T91,'!A;!;N9!;BG+8\_/V<O_RKYB+B9(56(*ZIZG S.6Q
+MVI)T7T9"J8*,)9?$]A26/M'@6U^62H)*DGI5G_*RX- 0Y4&-DYM[B]:/FZK'
+M,OTVW#?REUZ0%8.6 5CMOI.C"!3;1].\A*>0C$^8 ;JV6YO8\G[2/?[!BIB=
+M2)J<:ZF7[(Y[P\:IP\S-Z\,!ZHNQBI[&"8RAIE*^Y[CY;CYRCTI1@JR7@+XI
+MP8VB]EJ+AJVR1/JW#;;'-]KURX>E^85:5YY8P!:74,M3580 DY\WE5>)<%:_
+M()Y=GNV"UEJ08X-;C9A:N(1(Z]$>)@'/6XB:W$B,L[&, >HYD!#DP5>5K*&I
+MFX5JHYY[",0&VM[ FX6D#$&)5YR+$;N8@[&7+]8[PH=]MU$:=&>Z7C1:G2?+
+MZ:\*S^_OX$B5K94<6J]<-( 6 L>K[/00[,56  ";4J"^J\3,X0A4"G9=CG"_
+M@ZE8W78@+L??\%3TDI*:5S*T00+YT=:=HJ*579FKEJ 7M\<B$WN2#^.3CL.3
+MRJ\8\Y::]4#5M[O!1H2B *I''E"AON#PT.9@4@Z^AO8OG[85TJ20_0W Z0#>
+MRM&KGG:EI9(!JR('J\>V.=Y4K<]+E_:?7%7"D[>HDOD+S]C7!LL0\P&(JYRQ
+M^E92J+)CH C8\C%_QBX(G$A3\;*!7EK PJ'D+H9^6;"(^D'AWO:B,/K@M+:>
+MCUH509.0?ZMBSZ?$6%R:?H^7NAZKMZT4].;>)-K<RYQ!F-G2DGI?C<,JL7>E
+MQ8JGJ8W9B(V0Q:H JD5$QA_/3([$%%2M@9*L) -3)<VO U$$AKJ:CXD>@9N0
+MFECQ,?7OT/W"DU71$I.:NUP+L]:7TPV!ZL'+BPOQJX7=FI:T1ZJ4@-Y7:1OG
+M\# 663R"DYU0=;J=QX]S/9 (,G7 <7F[<2HVYG9OZKKVH%[P2Y97H'!"YPRC
+MV%P0<IU24?9*L*6;FM05M;Z T*N2DYVN0*B6"X;1,"56-@"-HFW:JXZ2.J%,
+MYT?6T/(TP.>01;^O>$&YIIZ9P4#)!=70V8^#C$&NB*R'HY36QV'LU=WWT]*<
+MK8>NGJG8^>&GBQC=G)'#K]^NV%N8&;::E::"GI&.>L'JXS?MP-"^$(N5CE%2
+MJ-'#H+&^<(I;@A%_C>?4UMOMU\>?LE1Q=I.NUZ1.*Q/.K_A=N%I_CI6)J4.<
+MB/<T=-)&)3&'5JVK ^?]D$"4V2G ]:Q/@5>$J9:#G ZS3\T0N4:5^\5E_%7N
+M29. DTR0>\35Z_K%!<@/GDQ%S*_%7J"@%^%ITQ8F[N-0LKV@EH^(V)"3E$"M
+M"JP+XP''QT/O#G^?2OZ-FI[H!AW9$A'WOE0/+D:QG5?36D/%70/)O8A>K9Q1
+M6:[C*]#PYCY1F%":4;Y5>Y=@FXMQ:H];A\B"4+11G%U!EX'%-O4R/O#82)HP
+MG5.]^(6%Q<+P[/(L&U-:D$M^:I:7HI0E.F5MD\;+9J.>6$.>[,?LFJO-SBS/
+M <K*CM989:J8A8V 7=<8ZJS[]#O!B/[;BUFVZ5I6E8Y0YR?!]LL%2L$0B8Y0
+M@XGWFYFF.'N?V-_U-]J5T;&2D%N<'2G8V,[:\I!2<Y%:<*Y*C3;6F^_D3YX3
+MMBZRO9N/I-2;S=["TH6T2IU'RIX6J)29\_3$M9[5TE,2.8,J:Q:F=0#"D)G:
+M5E*/-D&]?+NN'[* DO(FKL _P+LGE)2#7LF_<H*MPJH,[PL3&$-4GJQ5%AN]
+M9E?] /'&Q9664P?/A)6;I),)][O.64&,>^'' =\GD%^>7=6ZQ+MZBZ& 4L?V
+M/,;2YF".O9^5^C58_H4*[MR0&?J/MX>?^P5 Y.)#\@?%DXN$II*OJ/BV4-C#
+MP=)>@J)%&Y+ZJJQ:4?*K\^8UIO&K6CB_%Y&^I:-,0_1TT.WU4J9H0UZ3WY:3
+M;<3IV.S [\.ZH+B8Z:R]DZ6.N(NGSHDD\>9LWYJ 3RBOKTQ:6(KZUJ(P9 J3
+ME'];@?"C@[BH#GL!'<#!X>N@*_9C1I<75\?HC%.RM;O#/YW4T^R@G!N0B3*=
+M6%=QXT_#_(*!G1ZTDKM8GA4^)? *_!&@D<X(EK_"5EGU!L<9A8.*]5N2W-J-
+MD%-;].UMTJ,1BL^L?HB)>K+[3^CRE93F6UF:J)<9KOA3GT#3_I&:]Y3NGU".
+M1==>0)BL!:[9S?O,U]+0AY>42(*%C8<ALBV-$L7U_O+PBJQ-WY9-@KB>Q-JK
+MX2S%YO71$:=2#OI="ZG>BIS0I:>30/(G$&_F*-85D)WUM9J#26_"XM>2/[R:
+MD1^?P_.C=9*SPTRPBHTE_5F788Z)UP_*R# 448R5D).:LGY"Y/OU+F[NT>N/
+MJ!T#N890DIA+$N,_Y!NBE<![JEM6IE"5U=HPQMHZ"IDGK<;I?+^MH+^] 0"K
+M -;#D+^UK)6&OKN);:13 I/TYM)V#I*7FWZ05(K"I#.&J,<"P"3&Y(B5L$JZ
+M.8N65Z-\H> ?FU+"U6>D[O"XB(I1=;%3EZT,XPNWL9Z[4L.J]\S7UQ+$.X=]
+MG!>^E:4;J,0G@=*NP-&=M;2-5;>V_Z:BCNTSU]3Z&MR.IGFNS!,J-J-$P>0Q
+M^_Z3&Y>M@HA)JX23+\36X2;Z]_"NEEB(BQZA2H::/^G]S,KG(=1LC$2_E+B4
+MD*W;K=DW]-;']O-MA?N?JD1+G':3@%;8PM##XOE8[]9OB)BCC%^:GHI!D(K!
+M.^?%[\)RV8:>D(V1<@-0R0+:_-0&U)I35@''(\3F$"_;JHAP6)^1"8-B=_F-
+M6J,2!;:@NS>AG)UFE@,-^_G\E^_<K]#JB5>)F Y'UN,:_586&[F.<Y>(3TV?
+MES_%EC4@-! /.O2IA9V;C8LD#K?HR*S/\?+ "[N,C$Z25)UR>IC!P-CV\'78
+MBI@<DYRWNH*0<!^;R-O1](T%@P?4B*Z^UZ@4"U: N$^#HN)<\AO6!7W9@;WU
+MN8I1Y]S&D$CP29L0"?N*RL+&WCWZ#U&)@:^R2X.?;%8K85_.W'^RBEGT@=PR
+ML:)!#603+J_#XXE6H)_<B?BFDXG:X/#"9".+MI*#JHL>EZ, 6,+U/O8R)TRD
+MGX2/-YWHNZ:=4\PLWXKBT8F7O$ /KY43:+23@-[%;^Y:6H"6]K[3ODMR7D&F
+M&\&H"NZA38[#R)E648B9EIVN%ZB/C\'^Q_)SY_$ND*![CX-<6RS/!4*>UIZ%
+ME:">GIB3E-9_QHO<M")8IAD66X0/EMDO)N9/B/*UVYBZGW .EP?>Y?J^!WVZ
+MGC""J!C8EE]%A>/%_[;;B5YL6)N'FY92@*CQN7?Z\D;7%Y =HOX B,;/K^T 
+MU (!\P/.MA7L5?KLN*Q5NLCYYIY +87"EZNPV[:)<Y0B3ZM1S,OW#,9B#O"]
+MD@2'LG>;NAN@SY<#PBT[[=:!4!6AO(81VBG'#O$V>I&QD8Y16H/PX.%%U[;7
+M][:^!9JCG(J@7XK'')/F5=#0"521JENHL%[''O4N5M#P.H[@$I:#<E"ZE<3Q
+MK*[$(SZ6D[J1CUBFBZW8GI?"+D0&C/J9F>]&J;JFE9OQ#M8#].$23WYX5&^>
+MO^BLD@O,LNS3[]9W3_Z?2YN>6?ZZJ!8I6:9YQ0ZFJ0[<$TR,L9^2D7):;"13
+M25#VE_ON\)6)H(D0BJF3 M2CPH0+DZMR18*)]W#FEO7MT4J!@964(JJ=^8^X
+MH<?O?)H@I(E625JKD%"ZS-)O-CX+/%*D:+BRNPIPDZ^1T!-0,,.Y=B9*^BER
+MEE*: K[6U?8>)IZ?C^1,KJS)["6;&NW/XO;:T*^0GF"?K&&3LI1:PN2UE]8R
+MTIV-K5Z7MZTKMC'46MGZ$-7)0JO6\IM:F3BIQKBJ=/OV3\'BE/X^U-N<"%ZW
+M<E2VM\#;*]G:EE&<SB"<OL?;\^W$%^"8)K)/H)E>2OA45R==D(#T"Z(Q%CN6
+MN99V0]FR=J_M\UP FB9:FKJ'O8M H/Z=]<+%480V+WJ-FI"0[,%I]K/T=>.,
+MDM_HEWQ9B3JZ#4T%_(XA!3 7LIJ!C+CAJ+@&J8WJD1)21:<(N]4BD):*GD*$
+M>X,!SY#?P1;2P].#4JMRG(Z+C+^YG]E(\NT>D"W;3"&2A(8)D__RQ0/(BG61
+MFU^7&9C@\_CR\'"KW[:3A(13L[JEMGK,@]K$FEF4N;I14?JDGGU$\/D^>]?+
+MJH^VNPQM?X8?)N=^126O@-#62'^+GYA6LT_9=!$^-G[UO7()G(*.7NJL5IK!
+M#HPMKL/1[HX%O!R2+8JL$%L#U//V;:Q+O08K5II6BOZB1I 7VLY-]P'GKN#0
+M*9)1,K:.4MNV+ Z6Q2OT\&80V_VCLEZUB)ROB-W2X89SN'M0$:NS&S/M]^?T
+M+Y63@YU?@5B*JEVKV/]'7%&\4KU66JH#JA>#I^O6L/330$\5L=.,P[6D2I^D
+M\.5F?L*8]K&XOD:'EO/L$NOVUM;LSHZ0?KV$F+R3,L) @MP &G4%0X^:1:B/
+MG\T+ME92P>/Q-L8V08N'](N,^EJJE*9SBR"L0O? #T\*X$-2MYM\=B^R0GCW
+MO5#S\=1P,.6LH]0;6Z"KJ4T7HP!,G8F<MG$*4LWZ9/7^.B*/L;:$M;>H4JFZ
+M@<TNTTB:4!.1>D"6%Y.;DHCR/J)OP./'I:.8FHJ.F_<-\,+U'<8"K8_&FY:'
+M4HZ#.HUR5VXR<!-#AH6 J7B9SX/V 0T(U]P4F>!/@%KJ7YY?TZR&0L7S[N8Z
+MW<O'GX50\O7K>?#  '/C']ND_^G4(Q13K?F_DE>'2E:LIYD8P>W5K,0E=-!;
+ML;N0GIK+QJ,VA'8"AK>:JE,BEO'F0_OG!_&FL82PG)NVGX+-!_48G';_B5N@
+MNGNJ?YO!>]'O]@,PB]N@ UIOBI;S3!L;\77ZP+L&()?(27)66IH!8?;&EM/*
+MAX:L1*NN7IF[C;\2W8#B7L\9OX1QG$-2@*Z1TJO2\C3V$MI)BH91=9XF&QIT
+M$%.7S*SX$]S6JX\4POI<B)Z2#])2N#Y91Y0]._8Z04>"7@6^<8*XQWG#'+V@
+M"G%:H5I]<SISP/?]Q7V0_X2'ERFK9G:7P53;^]!:TJ^7E0);I :#04/XUO#;
+MPF>'C+J:.J^5BL_U%/?@W\5MAZ(3DY+_AG*J ^O%M]7B"QV3NIV^EZF.H9Z*
+MU:G4UJ3K8;NZFJA"B)?-@Y[72/*]4E03T)E*?55:WHJV4GF?@P%"S-=-P]H@
+MF8BN!3*IMXF[D+AT4<<6QR[>4"E_]H*QD(*ZGT<74MA0-/*%3EW#OP'@-9W%
+M[<*JE(*%I?2[G[C.5]?OQ1B&N &%7%%:$W %JQ<KY9*0*MJ(UI [%@N'FHM,
+MI, =-K)@P)9B4I8HN)V0A='J-/K$Q@J)3JY 0P9HT*Q/JM& P [U+]0I=XQ(
+MAY*4F[A;B7+P=L8W^].=G9'7 X"[JO:SC;=:R /W5 3MVM"ZDA]Q&@(-F$O&
+MI=W9Z'4VT$6A>,BR'/:AEY;)!O,3D9!PC*!4@E*%TB=OEKX77%W"O9*RNXNL
+MWH':C,[*F$.2NW8"B=^039U2XC\4$/&02I )J_R"F$X*KL3B-\1U\UJ I),P
+M/ZY;E4W#%[7_W9>!J[.:C$^5OU,2=/_-3(XNH<5$#(ZIE,V&F4UJAE+7(E3G
+M]?_+O9_;BHX&8HFC@!(-]>C\T0'9"AL1IH>-F9:.1UZ)QEN;!2HG\!+^Q9&]
+MGE_279I7H6M:X$6(D;"0B*Z7 )KXMG_6 IJ@%)5:H7HZLG(=)4;:G/K4$OO.
+M 4)#>E"7&+34^M+8\+BGQ +3PI*.$HY;\O,N.B_:G+B;BF*OM?J'X?(YEY?N
+M"X,4G,FYNF%(N#:>H:14 DJ(R(>F6@&"GD"+.'*H\-'M%I9WR)F'AWZ#D$T7
+MMF+0&^+8[/3>%NO#"9"5D?::L&N"FZQ: M'Q^>#R$,-YD:&M4L7ZE_%&VS10
+MH8N>W/21M]-VQ9>UM>/\4DH%#8JK@X1.60=+!THOG/ZTDJ/8OZ);@<<>,.^:
+MX]*1L*0;@ZN'GDBDA?#Q[C/3CUIXG7F[5:*SED/[L7?E]--7!86J;Z@%4L.@
+MB<%(WE^BJM1O1%B(0IV^V, =@N$T-O[VUX_MEBZ9%Y:M&!I,5=T"S,!"G$G1
+M\/ 2K$6+3X/MJEN@=[W!\GSB,O 'E)"@#Y!9UZCQQR_RA%Q3V98@OZ\8\>3D
+M\) +CY*+KO90J).FIZ_'_<>P3*._GY7#K(*LMYO3,_]5L!KUBT+&$XCCGE"%
+M1 SC[/;EYPBF8D*RNMV^BQY"PI4E__YU1Q#:;$Z.2+N#I;[BU]1X!T*TCKR'
+MX(NZO6NCEE?-Z]3\VO7S3861F,O4C?N_K,2[I]WX$JS4H,5@31R%EZK2Z,H7
+MDY./TIZ3%COFXPBRD'U0EY"_ Q4 PE 6"D7>E5.]Q\&_U>;  Q^CMH[[HIR3
+MJ9J?T<[*])1"E%B6N)X3;9"K43+ETMQ%MN<0,+?<2QAZ7Z" _^SP\@^!IX)?
+M*I]<D**AYR/%-?^_]4^<K:_+C(T+L:!_ UW<U<>E,"U64$ ,CI7)MK"8ZK)E
+M5<*OCX\NO5"KMAN*4*N/^*/=R@%  OS)X^U3GX".^CB6>J.%^O#J[._>=NJ5
+MG;.=VY"6'O+^X]RLMO1'\J**@M?J1?_ZE8_>&$2MM?=3G89UMT=LJ]*)EA)?
+MAA*;@JSWFO<3T7WWQ<92M*F:^XJ&7)+ X^I#M<9#D_!JKQ/'?::@G^&6[700
+M%.*O%X67OQ^KO3@GJ5G=W(YG"N OMX6 O)>!AJR?G?,6'T77_]M)+)"3B[*3
+M-HNPT)LG2-)![ @#P_ !7,2PJ99)4Z+F!H#-Y.TFQ\#;7XNCCUI8D[J- Z;#
+MO;83CQN0BIC9XZ42QN?#4%B^&[6EMDNP\(+E5_,0';!+O/J7_'^(4+OEZO$N
+M[\4V;49&PZI*C%Z3'ZOK[1HV\XD.NDBKDI^FD(C-_^[ZQ_1.@I67K\LHKKJL
+M<$L(JBP+Y/$::XR/E+U?G8MFMA$)$+7P+9XIII)&PY:7NZF7H+: P8I: ^SB
+MR^,".Y(I4UR"0_JB8!6NR>/%Q>WV#_RED:Z.L+N7K0[ X+I=@$4"LKG](OXP
+M]#3%A5QV<9&F77NHJY[["R3O";@2BZN,L9J;)EI#663U]]+ P"L6H)B6GPCW
+MLXS:VL7EK@.+7ZA_DH"NEG\%]^K9U_;RRHUZ5;_/F$N[8EN!B4RMAX2GT9\ 
+MGD2)6BVZD*:+030QY;(ZYQL,K*-8<X<6&B8RUUGCVB;_ZOP75;M^A)*HFH:3
+MHKF66MKQ^9 R^^O$"K90CK=9G8*NVK"5DK;0I8"8G0TT.93T$]O,%A'75E&J
+M7>RGN]FCROVX\\FIG-*<FVP.F,+!PS_VV["OOZ!]^N.JDDNL(-3M9_ 2@)!X
+MW7!M<A1:B /;]?(N%N[KVX_LB9A52X:06<%43\3+2S//IDKAGP>_9H2=NPW$
+M??;[M/-#7XZ3BO7/0\NX5)N-'=/'UZ_KQ?:&AIZ:G)7'5O^@VGCI_L<?[N;%
+MN$@1O)9<FZC QMJVG'84D;7=\XL \!RUOCK+#';3O/7Y$JUFIBGG+*-%O9I'
+MG(_55MN%DIK']#S =\* AT6FOYN&OY>5O@?V-^^P1P]>^IVL0]8/GX"A-Y75
+MT& FOJ.07$S(;8MXA9<)+-3.(>#:1590C%5:0+MX]GK!LCGPU_)&TY^%BUFC
+MIU^2(9Y]"8[+X=K=Y\#FC_.1=Y["NY'2>:!7V.)!\N_VQ[^CFHW>0JJ= OS.
+M\GZ)DYVUH5[RI7KL]\74"KF3MENF=SI^N9*]9?GSR(29T;N7U(NX@IRHX39S
+MY="+6I.[D8#V1OA;FV0#%C7%]^.O3FJ[6X>'D)#MB<;3/\601\F:^*RCE)ZO
+M:9VI&P_\0ES#R8^4^D#FG8U/H82=0_0TW_;6(T=2U5-:I9&II[I%@L<."Z3L
+M^LS/&,==OI,2VA"\5K2[GIFA]>P2VH>,49*%\%V[]M*#]='0DCZ1UXD:>5G"
+M%1_&$ ^0(%0;6IVL7Y#0?_#90+C]B?!1C#U8BI664R/R-Q 7DU*;6G"H^^J$
+MNE\-P]/E+,7##IN3ETFHKY.# />>_16N%?5E<+KL%JV)8J"7@NG\P.3#PYF,
+MG9YDKJZ DJWWD^3J-9?<_]JHQKD2O(>/7A8LI[?JB/OTJ@W[\=:[MZZ@NB93
+MJ[B 7I>BT.Z6>_3S4+944)2*JA=GZ4=1M!)(GOVPJM<(T.4V+!_;7:#?GU07
+MFLBQAM^I#0,P<:6CR()=>K^0E'OMY#74+L7@S89A][J13G^C/?#PV?+UVY*=
+MK%=7FP:,6Y7PTY:^>C?0K72/E+E[OH:XCY+RE9W.UL\23Y95C)J_H*J@FHO1
+MP/<2]]!#.WR<D9H":UY?L*6WQ1^D72C*]QL1CI.9I4B/JT.6AIV[]=8O$\/R
+M")Z(?D>6DZ^=V/E#R)RB&E%6H:Z;1<8S;L+'PT1&GER2UT.;+!>*R^L)TII"
+M]@2PP9:6K/H+P12TQ<?@UHM3N(M6&100<[_QP\?WU\_KMFQ#^KJ5I?^:V.K$
+MUJ/6Q\L2\$J#DHR@D!N+%_[(3D=:P+]7CX#O4H6ZQ!HO)6/%/B3P&8*?CJ6#
+MFJM:UF.7PN5#P\T>00[#RIB\G5]8=HG"=$16EPS^M<60%O.Z<$@]5C";@R%?
+MP<.YH!YQO%"KJ(G"7)66E@5Z4X>=#A"+K:.0?]'7<,F$UE"[&Z%V2?&,70TQ
+M\3?<\/:.D^V"]I.VI@L,&>/%L.V*N*:X75.-!XZ3G C#]S_UPL,75E6$KU^-
+MQK!WRUD$),@+T32#\EY*@X9!27GR4J#>^31NY,B+>@U0@Y"3").IFM+W_^X*
+M^ASXE1"2^IV?^Z:KNG*@1%WQP?CT%O3%1;.+D=*1]MOG U4*-)2SW;:1FYL5
+M(G,6]O9CO(.PO9:7VX.FG_=!SR=AF+12O9;QOC_B6I[!Z?'T;]-8CZ80@Y)'
+MLEJ2I=#V-)HZSXB7EKB"4W^.F8_8U!DUY!3S:8"\A[R:.)*6A@#!_.W.+]CT
+MKXV%J*Z:E1.X&I?7(\3 U9)EAUR8@H!TBPA^D"!S -W('/C5 4,UA[Z-L+*V
+M"7[^B+)=\O1YTGXPRPJBOXS\6)./0/D/4GA0GXZ2<9(; >)1)!;$$TR;T F2
+ME9.?J/*7F:GJQ%^AFO%UB5R;;'V_HSXNG:;%"DN]P)N6CYR7DHS"T^W"NO6+
+MDI&=JX]65I5J&M;'+]/ $XZ6F$R/GXV8H+6;P1S.[L[7T(G7JDR#F(V(S9J=
+MPQ/%\O2Z][B.7;^L=8-X"&VVE_#*! K5S<[%2:,>_9%:UKE:GZ!TDL'JE>XV
+MQ>L,G!F>Q)6<?Q'NVQ&=D_.=5;E,@<SK^/>ZY=J45@!1IA>+@7IR4\,K[P0%
+MH))=CKU6FJ"06W%WQ? VPL#M<"Q#G,N+I$N>S?O9]/O/A].BF9P)AEJA0%D4
+M\W?F9>>\E?F 2[N4BK&5F0KY[.Z!<TAOCH>'[UKNJZ64N-D_W-+WY!5GM9V9
+M6H_M&K:XE(NAWOA3S=4OR=6.LA6?TY"JJE+VCZKRO\4@<#8"K%B249H=KE;S
+MU\?@68+:ET22R/;%(=4V%M8KKHH>7(;9?+Z05ZK#WL<]J)S+E-!]5[\AE?,,
+MZBWV_L\4D)20&[:#1YX2UA'PET8]CZN5H+] DIY;DT3-_CCNL/I#%Z!<;:Z&
+MZ4>1DJL2U<K6SR[QKJ65I&M,NTL26[_-<G/ZY]! A[[W%I/229^$>(;7"?C2
+M"_C\J]C82-QTF_Y2XIX)S!J1!>A%P/8L6_QUHKE6<?AYS.ZE=IX07XM>L)H9
+M\=/6FI_&YS>=FX6PG9[#N86]=^G%$ZA1GEV5^KN_M!I*P<+C(]?.U%N-K)O#
+M@UJ&]^3"]Z,0I@JO%^2;J)L>3:>OF?(W8_KWP1^VA<Q+7K^[8E87R._=W*+C
+M/*>FKD6NKMD9N >0QY/YV^9<V0,%J(59FDJ*)GJ6KTWN+N?<S4_Q'*%-G2.;
+MM^>.E**VDPF2>?07K,%45@G[D+.>JQK^V^ %W!!17HO87\/")-+OP@%$E@'8
+MD+NL<^*7@TSO!=;PF9.0_J*\6H@:6)'!T_)WCN9ID*Q#6^UXEJ:%R>ID]D3"
+MB5N!LH.;7H*RO%GF[,6U[>=-I[Y47I>]IWF@<M+?_XC7 ]#INJX%@IZ-YK..
+M6(4P]??V/^]/5XV3LHZ-5E& 5IV@_J@/0<W,:S&-782(@I2 J'&P)9KU\#'U
+MQ!O'_%!4AI*7J8+:V77QO(GP6U)Q=X-"]GCR/^;3\<BAD=JWG):HEX,LUM+1
+MK!9^F4Q1.:BD__L \M5E]=D0 K*TJ5F2KK9>@ W1Q6 R<HZ?A%,<C;NS@4WA
+M(#G:-]?H@U2LE$O232<SLYCR[_X'Z\_0E_>$B+N>/)MFEK;1\>;$+_;S.)YY
+M7RB2B+Z*I)N;14W%]MP$P<\6FZN;58O[Z(.+D9"Y*3YU(#?"[H664)&>\*J2
+MQ]3%, Q %IDE]Q.2R. L^O??D*_[PIOPD7J;H->_0(_#RY"$$ZV6D1RS)H7+
+MP?/FT)SO4&N5H'V#KQAVDVK'UAG5^L/ID#F-7+]%]I*J!_+L[_;6\Y:VO.R9
+MG;W+('*"P43=V-W W$F?JI86AY6XQEWZP33U+BRVT,KSFZ>3IHF"4L&R%_'*
+M @^JW0P+&(>ZK55V]I*<F"SP4L+>^<0U^Z&?F! MIZ%8.QC\A=Q%G".;IHJ(
+M6/+^=*XGQ@:><%9\D%U>K6S0]PM[A3.UEEN.H)&7LTB>6\?RU:_U]5"CE&."
+MJ(.?D)*?[<'5+OQSP7+DDQZ-CA8>H"T_MS>S]?&)5I0*OKR%JKB:/2<:_Z_F
+MVV.O%IIMK7^MJSB6NL4;[/ PUL,=3=E7"E6]=I&@L('U5L##3,7MXM!;OYR*
+MO$20F**-MGK1P?S%L/3%7["T5(V1 9C@QH[6G( 4NU^16%O%-!; /O+6E)Q 
+M'16Q&[VD3D, J>+@&+!*#YY<LX*0)ZK*^ACVWJ7VN*6L?5N+#XRUC@TR;_)0
+M)4=0N)-#@QZ,_I7"EN]@]N_BG=)?P(RLR6UIDZGBI-2#BL$PEH2,/$\:3(M#
+MT*CAGL<^1^71.QY5$KN>QY):C%I#V7PL!?GHS\J00H>M2HB06QI219*:P7'X
+M]9:6=ZZ9AY2:M:I[ ^DC":I8&]BP)U.+P6NTQ[;OU5R<E*S4D?JKEIORR-?:
+MT;RRHIZWE_B[::.:Q][@[^W@T)M;A*J8:PB5LHWS/M?R^L?M5B!XO$T25D64
+MC?G5.A"2\.Z:O$K>J&3OC:2II>I)S^3C'.M>!8Q#6HD:AO>HQSOFOL(P6J]&
+M!9-8NJ>;G[F6LJ#$T,,$RLP#'>JRA5%ZG(>)ENQ'@Z?!\;WZPAJJE"&OHXN\
+M0R7+0L.LF9.$E]N8F<5PQS+Z[<5=H_1-4A26^9&46,U6VV/Z.@J[A?% ?R"6
+MW1LR,_8VP]F3G[I[B8N?GJ=,0/;UT.#32/>SVY['7?>C  $RV7O>+EE95DR*
+MIXB^B^@7BM$ #LPBV=Q4L+I/UYB-NHB06PF2.?0F-N*JA(5SNXR-=\9QHDL0
+M@BG'S" 3H;2BUIU36\Q/&A*L$E_!*!W0]_X Q(B:K)8+?[L"CQ,<++">JZ>2
+M@YG+\C_B/.Q%F!:BC5Z(D[F67Y]-#U,3L9.0JWN8J?-@%XB-]M7P)A/R#9N&
+MFQF9K%#?O-':;R(WYT]&<-*:DQY=WQ12ZO30%_?JEH^>7+_<3HNX59T#($ .
+MW*?DOIKX'H^^U0;,6MOG$O%WY!K"N9;=5;>7BWN!J96RH,_XH^:H^\#\K[V]
+M7ZG/BJFV; 73!7+%%\;LR,?4@(>GD7N#PJ/3"8>\BZ]&EY8"U?#M]G8O]X:@
+M]@6:4I.=N' =R:V768IQBY\2"+I(P)K_3=/Y\A8"PX\$;;>^1X2:@8S),\4T
+M<EJ+D"V8_[];E9.]\3[UP+?6ST.\>HV5%DV[C)":B87^SZ;;W8F2O(2^AZ4'
+MAO +TNG%_A OVHV6/)#VD&"6H;.UB=FN"&'IS=L%]*V$E=B\IKBNP8&0>,,9
+MU/RZ8 5Z@*.%C@E"B,?[QP&:7+9<CG22NQ*V;-?%DMJX')"-3OV8NH.DDO3+
+M -.OLX:/E?U65X"E]^*[]/;'HQ!"]L'[@>B,AJ-%V?;LG^#/ PV2G7R-1[3(
+M[-%IE77<$,^"FCQ)C9Y+:>R/MP5,ZJCA3_9/3;^,!+NM0I!5D/%K]?JV?]F#
+MOY&8K)_+"8F,1EK-K_JOP$CDZO)ME[U>B:>-%D.YAXC'%$;&T#>9D9.*GJ77
+M@HK9%0/CW:(R>W)#EIG)$L;VYJ[1NER?NY;=5JK %9K!"U"24%147+8+F@.5
+MC[GQ=M7G^J4 '0_#W5N8N'*73-C^-Y;Z$P>7K!U!NYV0P4RJX/3R<];3[K2>
+M#9N.'4=F<+K8[OJ,]M=<9/JJ5%3-U9J,DEL9\?;%\N^2NYA5BTH635.29%H!
+MT(P8!Q[%PJM<JW:-5WB01JASK*2O;6CY\!7$&UF0GKN4_9!#0<;2U800$'66
+M4+RKK/0U-_8N14V3GJRGH0.)LZ>/$R[0PEYYEAV2!UF?YC2#P/YVFA +$FUZ
+MN:J*6]>0EXK'(&SP\G.8G;B9<;J,ME6N&?KY,E R#J*5FFQ%C9N+XZ>K94AN
+MS@[8]8>Z3^_CDI2+:9Z:X//6YI8_PP-/ELB,](:IUI/V^,C^:$>!K\7N\$==
+MW)&NMH,7RB95TTCSV-:@Q=NV4Y:;GO=(5X'Y3Q&Z&))PEYB>N(<J]/K E?>X
+M5%:>];?06I!$O\/KV-F$%E^%I'*(CHQ$6RO"P3ID\]!(D[A;F;V>ODL-U?'E
+M]+_ONW:KDII&NK.9@%DR=Q3&&B*[7Y^\"4?/T;BZVLKZS_Q#"T&<^IJ*C):7
+M4H&V6*#JQI#N\@K-_*^52I)8W@@2?;K)U$FGS,<G<_&#?IV8W')3*DO EKMP
+MX'CP=?;!6H.3NW>76IG8^=NT7_;PG(V13D+8]&\VQG[K5!B0GO6PN(JJ6AL1
+M+O,!NG8@G1I;5ET@FH'G%C<]YL"P!U:2J?Q+DG>UK!<CU78;V;C7H/VKR9^T
+MAXKW(.;\.JS;XQ>/E$V<3V_ $)#E"$[MQ.?X0]:T2::X@1*L!H+;<M#PQ,3%
+MDKY=\P!2#U;R8::"S=C1KI12_@4 !Q^LH7X&$IB$./)=(=KDWD7B4/KPL9".
+MB7H+P.OPN9^Z5-V0F)H6H,'1]]0LQ5"(@I6G\5B?3'Z1S5;*U8@"LE57A7GY
+M(;>WBS_MWY//' BVK8,;NOO:59S)).RV\!KND$*"J.NNGM% :9_W-NWOXS]W
+MFH /&XRHC:776LX [^JJT[Z&A+WC'I4'*'IRP/+^Y<?2\+J$BY"(D@B\<8&:
+MVMOHT ;4!V$;X"T*+H*H5KJ"4J!?J^'ATR;UWJ-,T8JME)VL_<T-Y1"&]9F[
+M7[=1D\4JD_ @]_>&&!2?E9*>D_G/^PE?\>**@)"_M8B"?WN5J*3$/27"+["[
+MEK:]@8=.L).<22(\Q_X#AUN6?IH0A;*3#,KJ%K?VIL=K5=KL7X8-1R.G@//H
+MSZ(&XN/OCZ^NBZV4H( 6T<K1E=OS]@Z"A<53&/6-C'XPVED$Y HFS$_X!U"9
+MNYUW_B MFY^DAZKQ_BWONCX*D!B"?HQ=>YT%5QL#V)0^%0Z)7E\#.M7_P+N.
+MFE0>KYZG2HVRC],"CP,06 /".[2GEAUH-@/LZ!$Z-G'&6I;YJ4 /=::?/-GR
+M8S?@\TM&H!^[AU.4@X^A.\<N/C)5CU6$E8JZ7;NIQ=K9+>SN"M/3O5OP;9V&
+MGHMZEK=)0<;Z-_#"C7N>^JKPYYFB8/:+R,( Y*K]RL"YNJB4@WF>1X*VL9"_
+M$V@Y9N7@<*^0EYN/^IFK <\E2'R8HY%6DJF"\=[99.[V UZ 7YU?D8#2LAN]
+MPLO%XI;XI![UT/R#D$X3\NO],]0EM;B?DEDKZ)>?E0Q1GCSZU\N2E+F2D&:'
+MA9E Y-9M?O;Z0VZ6D>@/G>E(QG:?Q\S Q@?"D9^0$:J&GYR#J9J# _OO[<#R
+M&KF'JUM:>KM:GD"05^/"# ?ITL<'' ]WG)-:!ZV?WGF?G0/:Q=96%-M07!I5
+M1AVV*.<BVV(.H;0]EK.<>*<R%?;%VML\T:*=ME=2662FFH*CRK+?NY(%5X%3
+MLJ.6L5>?U/;5@Q"?G&!7N8E^MY>4V-8]U_8FNUI&GP*/*Y21;DT2%2WWY<]#
+M6ZJZA[_=#6@V@><J!!WF"M2BM;VE@[A!KJ1;D05K[9*WY=-)KI'?2XX=_EN[
+MHI[QQ\'!#.[(S\A-&OF;6@0&OXNZ39DG-./2WK[#?Y&$%((5VI+GU\]=A;GV
+MFY"B@@KQ\?:V1-/BC9 SD5(CEX% C@D3+L>3WI8 78^)T_*L%9K!<?W]GDK0
+M*\1&>[I=4HY3  $R;=80!6UWZ9"YDIB0<V#S:C\BT#"C2W>IC(,(ZVUF<_+ 
+MS P>W\.]A96\3XR.A:F $*[@EG;'K[I:[9B-<Q86.Y:;A(V"\4[9QHK.[/-)
+MB4R/"_PENIR4K+):R>O<^G 6+IBV(OV@V5*+=</8TZK;_GB0B[B/!S+#_D3E
+ME81\HKR/H/^9.'>(V.; \'SQ@:UWD0)[>(R#V4 U/#_2PZ^@89^:457&"\XB
+MTL7VQ]CI7X.96H#>)DKI\3;UE^X^T(FCA(A&FZ^-YDZ7Q50DX]_U$8L4+;V?
+M5H'C)/93$I8V]^7Z4.<8B9DIE8U:GGJ.B]%<V,O( M_! EE^AI?[A@]:BI*E
+MEIB0%\8F19M,BX!15*":0T$)0UNX,%!%HXFL>]&?\_KD%\^:5(IQVA>#@:RE
+M^.D7&L15B+>;4AB<'4.^6>/0%/*R<P:]6\0K5EU(1@MHP=KU]&P/2Y60F_J-
+M2Y:Y3,-V]/:V-ML,MO"5F9YK&I!WF.4,[=AU]59I/ZR?@YH-;:"W@PWZ-._6
+M]:#F_YM)JM:'0H1HA!BA_8[!REI'!=2=%X56KD!)'(.L7@+-,M?ED[O.EJ";
+MKW11N(,@V<+1D%!+L272G%L8P;3V]C_+7!"Q_98G@ZUF=K=9SL=0T!:&79(7
+MJJMTA:L!:Y,UQ<7!N)^F4Y)*A_!1 -#STS/&TH:G@[MZK8:]]Y7+T#;^1_:/
+MJ;)5FA:608\YT%BDS"!?P,*SAYJ$F$RNE1*(SE/@M/DZQ)?QNKV'5XH;DJAV
+M:G16Q/X1Z\S*!MLQJ[Z=DI[-[WJZH)>HA_13<,;V ]6<BX;VD4I[34]1V)V!
+MVZ^>H=:8BS?S)E04HX23UER$"OP;.+?VY1<;U+Z#TH=:01I#=!Y[0-03[BZ0
+MV8:4[)*NJU^'BTI9$YW$,%M"HX2!.X?R?=.*H30NYD;ZP[JF4$P6_82+(K8?
+MQWQ S_8.X<:5EE6/$FW8AH2[\3X\QC)[\)"^/I,7I&NIFK)ZF2W]Q/;8_ K!
+MUI-3AP&V]$J^)B060PSC_6;DUD5&G$N'AH=)NM++H\/>D%Z,HY$8_\+;W18V
+M1_?<$]F]1XB3VN& ER4<\M+>]EI$O[#WN,9/BUB2\\;_PQ2'D'236N\&I9*-
+MQ2GU->7%2J2DGI^2J)H?/LSQ[/8T):B+4L6^;[A-NW.[^]'4_-@FPM!>M?R\
+M3XV,AX(.DL3S[;I%OEB#5ER N)>HCE>0=KTAR.F/J,)8 - Y>HRPK(8ZJJ*Q
+MC$B%/C,T\!:E?).R57)2JAW YL+CCZ99KI80@[CQP<;NXM/%5?"P7PZA')*@
+M5'W;RQ""M=#!E]6EF]BLI%V8D_40_<]A[59B6W?/1J>2BA%Q]KOZB.LGIGVV
+MFZQRGTG0Z^TZEG!Q;J2^1KZ_E(JXI%/JB/WXPY'2EX(%G8F3G5-LAI^2L_;^
+M5^_#IQ>9VTP&4JN3I9 !U\VH%,7"#X\QNUX50*F:HIQ9K!W7 3O\EA?UIIQP
+M4:T 2:H]PPM%X;B4]@:F6X/_"7 9[/J0(1S4&JRGHIQZH-^=)^*F\!6&D9:?
+MD99.P%8#2/3TTO?8X:UP1E.<FWA?VXS-P-WNQ<*+E8&=6)F&5'./T-XXM%_R
+MXY:"7>Q7DD"KK)6*V,[-SG;Q'"T"A8I.DKV"I/+8VSNU%^^2$U%XD0/\7DN7
+MDF*EVL+]RJ?H$\#GR*J*'Z62>@]^0F!: P50+,+7L!OWU/*QP'.[D\EJRS20
+M!!XKG;&[K8#R53+F.D+00+.;7*48N62_^L"*XN):I9:1-X>)F6V44Z/9[6[F
+MVS&/4$R"L=L^1%<MX6+U1_[*CU"T4YC'1Y"B@&?T]6_VY]JI6HP\RYAIBR&/
+MBN+MS<X>2LFIL*V\0YB@DZVEDMOFM.WV=D*3J*\5@J22F8*1CGCC+>KDZ=U'
+M:L@15;BSUHQOJ*&@DHK9XF;6]E/E#;1+J9;4FEW"_Z/2=! !K9Y4FHM:\_?0
+M+-;1O9/R+I>BFXCKCA<(SL#9A*3&"492V;^Y@)N$X[,^+?#8B/K[OQ^*C982
+ME-CC_<?WT;J,>[?R0[Q[HNC!=,/7X/?C;YI01$,2A9*IEJO-#'K8W"<4G)"M
+MC<M>:(_AH[WBQ\7FQ\(!BU0<E0&VD-J"P$Y= LC0U=_/JG#DV/Y94:A6'H.?
+ML$__<Q(S]#IL+YR<VDU3BY*;I0S@VZW8MIU4"*Z-IQ#M]=7[PHX42%UT>(*^
+ME/"?QW_/&'RF2YR.TX*]899:V5/$D.:ORYU/DXB/AQ>22Y7,]#<7[_&G$JP"
+MJ*BNI*%M\\3&K[K^4ZV0$8RBF#BI=/.+V?W,U\?NU$.3JF2+KXD/K Z3TSHE
+MNKH?SY"8GK&O_[J#BZJ. ('X^<,'^_\/W*EW^Y*)IDM2N*1>D]"0U/[F-I,%
+MB8NTC9*KJP.O4]P:H;.'7_*"G:5KQ.W&^^4<7%*$E9.>NJ >J,+O]YRTB=ZM
+M$(GY@S.:G2B08]OL4KSMFJV;*HM.-8642/*1O"X#$5!-FW^]#MVS:L7V]/?P
+M9\6<DER(0IB!N)+5BL$)VBP@Q1Q"_)Y41TX%BX 05T4CM?9GLN4NA9:CFYVY
+M4I*$0)KGENC+^NG$[_ 3G[%8F%"9"Y+AL%W9%D4_[/?RGGGVF[:)B]>#2<4#
+MG:!;FZ2)?Y?1$!5ME]/:AE&@10.7>H"H=%KS+LM06'7:488T]IZ-EZB1%#7F
+M-B.P@U>@O;*"E91^AL>^Q930VY".E9:+DH>%V8@7L]5&]BXA'UN8:.^2H5M#
+MDC[J[-@.)";(CYZ4E-:?E(NVG:V ^G7G[E(2NMA=GP./J7NCDG*=Q,H.]2HJ
+MF_7<J(1+ 8.>2:FWLI8#"7_XP#;&VMY:^I64<8.) 5\"L*^4GE^&71N3\RHE
+M?<?"SZY(HHE,<3I9>(X#T0/'% J(M%O_B?Q];8Y)S3_Y5/9:XF?6:*BJAU;>
+MBI1:P^7Z^D</5\AWD9U-_(= H3I]5O66@*N@6EV/AD]3>E>ZT\Q^!O#+$K]6
+M7,:?'"LFJ9?:P-03QO9ORPU6FU^:MY_V2,Q4B\3^Y,2MRP8#$[J?E7]<58\3
+MN[CSJ.34;91VM<+=H%&TEB5;@Z+"V[9\$%.'=;R[@Z#"+1:0]^]46%H4MAV<
+M.I&@20FL\1*0H;*56Y.\TVQ:FQCB.,3 UQ"K=&2_]D.^SD=J!^CNK?I"HU[L
+M2*J.CD#70-/JT)3_[_"[5X^\1IAKJZ&;@R??UD[""\)K5=>43YR!9J.FC0SR
+MX1_PU,>'5IN(ED^^D-YPOY+2U<@75@K%]1RXKE[S7@6[BY@F<-MMGF_B6L3)
+M?HF>7(:=J@/M"R\V1Y/^1KYPK%;2\.7WP-9SC:(2GZ 56XJMII['UV;!Q89R
+MD:.*^OZ0A9/:L;;P].#:A[>DMU*9N ^+'@L^<._MPS]48)U867)7DKW!Z]7O
+MM9?*KX^-C8F\JVJBI'LE*%>"WZ, 3(^$C9M7GVJP3Y_!'G-@$I#KGIY?HYZU
+MJ=F3[ ^:\OS[[LI*[LOPK9X%"/B;CQCR+$2-4\/A;L VY9GYWD5V4IB-$0_"
+MXYGVMMVEH9B?X6HF_N;V6U6P&9Z:%1JZX'7_36D!TGJ!VD:F65"3.X[=Q30W
+M^_#%&%*V>8]7AP>WTH3-*,$F/\_(I&!:JKJ2,@.\$93Y;_KT#[G4NF0,\KT3
+M.;^:I\#4H@H/$)Z.3%6*GLTZP\Y*YW[A\,+>&UN+G[,HC+J6"L&=M_$'K@M&
+M#02KRFJ;G=NN7KM;H8&7DP3^.:XV/\#1M!A%1[6*^<'6PK)=@Z:OEAW\ Q?:
+M.38;[F=1H%[[LY$9VH"GEP,.SQ*L5+0ISYU>_K1: L$^&3_@%="-1H/[?HO>
+ME9>ET;'T[O6BJ)W"=IF;FJ2)2-+T[L+$,\;NAM6-B9>4BKHUEQG:@ P=HM 4
+MIEKHZ5*+8[".32BS]/3P-"N CX7!7@:[N_Y$M%T3%.#JRB*@<1")%EN#FI^'
+MFA>Z4']!ZFSV(/(#?)G6D;97DYM"W?6<NGS6FQQ5^]GQP"_&;J 0N F6F]^ 
+MF[NX?I=Q'6.QF: 0CY*3,KMYDYT:\]46E"#5G9"QC9^ZA8_PB<J[Q/?&6L:'
+MLIJL4PX4447QPOS%W/H@CKJ'CX\3N1/!FOMAR%S"%XKFQX;\F8,>'UN3A)W*
+MZN7^<C+EDJR9$OFEO7B >!>+ OX8#OSIK1=&ZIBMUMQ7JTQ)QE?ZP,#L_O;N
+M)40<2D2Z49H"TL2EPYS0FIZ.@*J2P3[\T/I%]Y"@D8VV5YJ[>E:RTPU0T800
+M5IM:45I=;*)7YQ03__(3XIVWDIJKNWQSL3K9]&% >_.'5B69FKV8E+"_S2+Y
+M;.(N R2VF:>>G$5*LY6:\P0,:M?#'(N5CYQNVXF[A)^JZ233U[WN(VK?C5M#
+MCKBWB+&.4H$/R_=Z! %0M,L]O:*ZEIC27F)^6\'CYL9EU^><D%I<M[D<F]D=
+MVUROU9*<C(*P2LTT]>8S[L%45EZ=7@B>F*6^B\E_(LB&T9X<^E2"6JQ.LL7J
+MK,8?]M*3H&P#BXU>&XJ\[<'5(##S4)83@MN3D@QWA*%!E/_%^L"CLMP9C)(,
+MOVQZ6L+   _'XI.4FEZ5RH9=3N3WC>$Z)Q(E]:BC4["@F[9-FX)XCH,'!,A=
+M7LOJ%>"KKHE*F9I+3J&CT)FJ_N5ZTCO"KI6+^UPW2'K1+<[;G))V>Z1 EY?!
+MD]-T/_ KG)V4K[912GN@4$F3AZ#;G/92/*8=*'F@59WCE&WR.LK=D*)MDY:3
+MDEI[C #1L3?EVIND@JJYA]9,D>IE:.S2>L#3UI:5C"*8GX/@D(O-W^BH0 /=
+MF8"\'FU8B:V1]YT#'O__K]H%2).9L=J06+Y#NKH/0?[!X=7\$\?3ZI^5RQHF
+M[?:+DYI!P\/G/C[E\8QT6AT$>7Q:U0SWV)!1B?[RB?NK\SIF]9?\C[V<@9&D
+MPH""RK[9VN+9R8>06YLD$9S>N(YQ03+36D+0\+ORIIJ"ZI*/EVP9ZC-EX-*"
+M\K@8_&^4A%D, SOM[I1V KF?O$!6AH]-J%>7)\HJS>I/0NM?K8Z<KD$:D[>I
+M)2/QM#6UAPM/AK;VI)&^/J:/B<#?[$<-V^G3R<U2J;,H3$JLI(F2_L?RQO)@
+M^O5?P)XU'YBVJP/)T7/X4K^M-EF*O_%I]-#_X.6/L%.)&EK[EVQ0@O#;V-!/
+M\%N;1%6YG[!V0_.4&3HT1U9-A(W[B+M;A%K!P/+'(CNBZY9C^WJ+J+:9O<-R
+M_3WV-!D+=IV6B9Y>BNFFG=#<W.Z&(\:6CAF "IN,CJWRLP+CU6#0_>]'+'O%
+M?IN9%IX@E%KRULOCU"C/HM([CO]U>J!#4E;Q@)C%^>;$%,73^HF4\:)TJA;0
+MR_,"OMR41II(T'\!/C\TUA##E%"!WO97FIUYEOL0RQ?0E)B;E5^Q&I<B'E;9
+MT#7N<B4TFP^LDK^XK)JEK=C6\_;UV;J@(-BZBU3S\VP)=O5T-C>BG4*<9 ?2
+MK(# IHB:B!A7P,]3HY.&C9F-CY.XAI$%U!D^;A+(B).IE5Y2JI)0.)JY QN:
+M8?Q\V\ZT2YZ? 9[UZTH2H)*7*)[1K_;V&XX0VZ\0D9.7\>_0D)!#H8>%DW)[
+MT?!Y/N9MPKR!BZ]&21*70:*:S5VKY;Q1DM%?G5S;:X29P/;#.C7U] M>N:EZ
+MF)I?H^[)\?3B>IO-):N96XJX<GZ,#=/].O[Z#I^P1&'&B*VO)$8#Q7[N+_"G
+M\Y>:F%J>G<&M1EY_@3OGQF(VX)K_G2%:=H<IOB:DG\75R\?%Y$$'$LJ^O746
+M5-Z+HJ6:_NCA]I^V,.\<E(*;FCR>4\4,@L!<W(*9M8%6*^4J\R_OTB<:G%O\
+MEE&<6J9TG2W6P]A9\DJ5I9V:".R$BRTH;O?V!Y0'3J*9>JU'I($40+'5%O?S
+MG79FKTO+DG>5EAKT%_><NN=/MYGMC5.4ZVS:7=+ Y!;> 1&6GEA O8><IT9Z
+M4N<3QR_[\"4(_I'RVOX#N;*KWZM,U]B*H<[ &WB#'H5#JB8+"%*ZAA[19N7Z
+M\F!AL"9:549#F[=!%5-6_'!R?;;<T%&)WM'_]_;"O9Q66=7QFETS\\CQ O#<
+MG%F2G9R4NHM EGW)TM$0]L<$O;*CEQ*+7E#UKEBZ])8]]V*'D%N"2)QWB9W;
+M\],[]G*!1MJLK(F2/V-@FE.9QOS,(,-6G9"J3)]^/2U,5[?9H\4R=I?';]A^
+MFJI69P.@L\Y0X-C*Q2P>[N.RFW:LFY)2OKFPPD7]U. 4]F9"4)Q3DXFE\9:8
+M\P_C1!264YR3MUA#S3/%+K RHQ!&@YG22)./;':I"WL+YG2#G_R6D=R?D?>2
+MP??L]9>GXFVFH9L>6P>V18SSY].7]\N[D.:95^J+@I%&#?[U/OOFVXF;G$&?
+M$E6_H_+[(0HD_\K/ [LF^81&AZU&H$^7T,3UE>;ZY5*($9E\H(*ZM+&F^_?M
+MSL\(5L/:R:]&>*>.$@**EFRVER+6;_#TQ Z?W%:?@H4:6L#YPL*-@*"'IE%;
+MJ/?&^3HT,D#^D8I96J-^<T&/@Z<=QU&$ @&1@Y*\BH"D7=GH$VXZ!]9'>F6+
+MFZJJCJ".2.KS[C?&C86(&K.9NE(!*<?6Q?I_OL-9![Q)CIZM1H*$>-+-W _B
+MHLF_%GC@O_VNAI(7O1#@V7NZ],N'2(^ E@:]KIZF58-)3='#U<34U0GG\K>^
+MDZ7+FEIXC+;E>O7^9CKRG*/RGW"W<\+9RW,0A(:2A9+TFZK9\E5O]_90U9!6
+MOW"#CYJ@^X$IZ='(BI22U/HU'*NYEGUG\\;\/J+0:YJX,IN2AY7SF-K2<);7
+MRZVFP;^*0W>R7M0MU\4G[_ CBQI03<UTK4>BQH-*S@"<!_&P7,>>OH.N%ZOS
+MI9< P\3[[N;W1K9[5T(0AKQ2>Z1:\M? <MSW@*#9H5W1BI8,1W^4N/)ZVO)O
+M.NW%VJI9D96;TJC_P5W.T9!"B:V#B5F T=HO]_/%KHW<GO6V25J:@8VXU?[O
+MT+ZV<Z]>=5Z:\E6#TL;%;N;/-D.VQ$M:;Y.VLIP(]\4P^J-=M+ 9K$=7T)Z>
+M6RIL]]0P&P]PF8CM6X;KD%6#&6K8!J80ME2GO;Y5$D6=H#6"\?XXU^[_]IB.
+MOQ"3CXE*F^)>?U'?R\<MYP$BT:U7F)5\7JJYEZC0&UOGYW?=[X>\4Y>90(N!
+MJH<# -S1EO&5\+V?LMF0;._2%$^\EH0=E:27WV" 6L',QM)>B%>=^K"3O[.%
+M^L$4GQ(FA;R=%!8;DB\/FH,-P?G%\D/O4Z"AF9R)59YRC!#JX<7'[]574E1*
+MI(J'B+GT?MM<' W*]MP/D#ZAHTRORY!?@\,Z$.06VO.KEE&0F+N&F]Y@1E<#
+MJM'C7-]")IQ3BYT0J R^FO;AI9OR\?:BGN7OFH)2732'>AMRJ^- >DF:!?:'
+M6(KR]K[OUM?U=YA?D*)2WIMD!@O-'L[PNA9#BU>2\8N0)E?'UL7EUA+<ZIV%
+M@TF:EZ1(P"$>_3605XM/IKB^2)X6D(G3/OC&^_;5SYY>BK^=GLOX7E,L_-A(
+MS^*6GU9?E8U604+H-D+E?NS\]^?%O[7$AUN/D4^+H'NJF0[(H0'!S4#&B_[?
+M$((,NYHR<9_1 L#U-?X_UUV <KMRRYN;&.SSD"Q0H(6^4-RK8<(VUA0N!86Y
+M@G%%F+:)<_: X,\5,ZR4GLF:G2@;,$X9S3!%UL?+ N]:D%K8;0WWBU>DD'G'
+M]O"9EFA7*%N83;6.VO+E_O7"P6O6JHH)F(Z->(X8S=_@U\K3"ZU]C!^6QEWK
+MAI!0S69EGFWPHHM2G)=7I*U\H:S6")&CZ!\%U<K!W*J2!)*3EEM6H(V6'0EP
+M[.X0,J>MPAN^\@B+JTU=P+&LECZO<)>+DL$3[>8:^]DUEG.1C**"FT.UJ0TB
+MD1"*H\F<H!&*G8R:JT&:\VXV&QR2E+"K+)M2L(NH8,/WTG^NF9ZLV4N0E9:"
+ME(C:;<?N]:]NEEB]O52)DK)7CQ<,Q((&VY"'DE^!SO^]C2!0J*%QV9:P\E<*
+MFE"U"KW/GO^T@(/A \H?G<\#!M./OENW/Z"YN NKHBC!\=<0,G_/'%">38VE
+MTEORSQ'P6E9031"7LY$'._$@=93'6EP;KYJ=T+F0)ZD#Q.;<,%V+O873LJB@
+MFY-SJ]3?UL+"1X^AJ*J[E%)*B,GJ9?7PXL=:,'VXAQI$LY_)-\=PQ?!SETVX
+M#+[&#H>-EKL3V@H"2M/\GU"6N+L7C4-Y\ZKQ8MF64$6BPJB$GJF66IB+N:6H
+MQ5[84Z#+7L+8ZL^\BON6./^:+%_941(\Q-HOPQC3LX2?D5N)(PL'" 1P"AR'
+MN+R]96H\QC VTIR3FZT025J];).ZQU=:T9P05DF.%?*_N8^K IYX__Y/<$ZF
+MN8U64,Y;GS_%P^7V+ME#I9&/VHL/E9CO@V+VNYZB]TOVD(^_6F<Y8$Z=",& 
+M3HX V*^EG$Q,QI;;,%"9 M3M-C"4&H=_D049W]&;F[I.3\?NQ.K,[B[VT,>]
+MG56HIJN6I"0647%D__02[]-<0!2M4+-*.P>"]4NN>Y11A<N:J/+R%N^U]\&J
+ML/>IGXF<6I3?F,%'0-R$@Y95D$>I@I.:7P4J-/8GHYS I")2JX*N=!.?R^ M
+M._OBB_!&JJ@-&!1#G-JJU.?^-<4+CM6F0PQO:+&WJ<'-B@/<H>)OEKYLA7NM
+M"Z"%W<#RL3;TY$#KQY&3EYY(@K/$EGKE)E&&_ ?.]U62F(T%K)>/N%K$#Y<)
+MY-C:Q<7"A$2CA9.72JE5PRC(C9R*K9K;EBN,W^/2\#X'A%"?E[255X,UEJB@
+MIRMTG_!3AHV5%XMFCY?5M-%R,_?QD(YYL)!3+J !;_+2\3>0RXKWH-.KOZJ0
+MT0WG$9V:ME %GW90E&SRO4*RWAWD_M1>IO&SHPZ>U8.HBYZ GIUQ(''@-A;8
+MC[J8L)R:2():>AZ](O_:%(?"I-#Z.9W^E7.ZKIK2PZ#;46OL]IKEX712BI#W
+M@)R*VNL/\%A#ODMTD5X2X=[Q9OIN$/24<962BI&+AGY(\></6:P3T;R0NUF]
+M1$YZY2)!^_?95+E4(]J"#9VB5TKCYMBT-D*:VX8^F$V'E=J\P>'YDD*4PH^/
+MNKY.J%VOH_J Q=S4_D8AP82$F9YCBFM2EKY8T_#SDO8G]]FOOJ6ZFMLJ2(&U
+MM\,&0<'6=]PC4E[2.YJWC(=2DKI4FJ/7[?:4TM,QMJ*<TB%;_<%#0^'\.!">
+MHG&L7_'6,S8W[\&7M!Z']@"6MV!P>LT7(1FO@[.6I+R+BL97FH?T[>?_PQ.Z
+MCX&/6B:ZI'+!P^KD]F7;XYY%V*AK6%I5@=%^^>)T)8&#\$S5E*V/.L.4^5C(
+M5,+=R+!BQKV E?WM1Z 25TSJ$48UG\:;A*M[OZ:36 J1UAK'PD1WK:KJXO&3
+M5YZW6?Y/7O1J]XG1,:/3P#9PK )T_<Z2G)C,_X]8?-GR1YZAUD'%:C/^+\1C
+M&G:D2<^"EY>YDH'$QO7A_HFV_:8=MM#"#9"H\&:\FL\0!Y20FP)-KD!3'@' 
+M9QS&TPJ6.YUZOIAT4JS9T>7&.L"BOT>N0$D5ZT$Y@(,IS?W^R@/&0Q8&K$F>
+M36U@6A" P96FL/KO@UY]5U9'DUF  ?9)+:)1X7_%VDO2#5@=BZ^%P)B@M!9W
+M\?/Y.F?0PHA0E%F^68(!\<[)6;V#GHZVI5^-I.H8_\9URI!0>G']G)Q^I-93
+M)]\%@*V)@\Z]]*^"@ 690?(M\"? UJN6DYV(3PVRDZ?QP=4N5J.(E[)]JBV3
+ME@-6"<0Y),?WVT]5G>V<JH#O$(7+!>S [L[S]E;PE9:-$DRGHHY7\<)D[C8Z
+M\4Z^1)\L4XB(GB:&T]KOX$\J*@# Q@V;AP"Z\$*?BLF&FL?S_>XR;L%_L%N%
+M0%>6F-K>PO"YB9(KCZ-;JZ#B=/ P%<C0=H))\G"+B6!ZUPF-]0I*E-8>H%R:
+ME[:Z7\TBY?0R\,6#]I.;JB:65]M4HO.=TC_+K87TGY:3F >>ZL7 W-9R/\"&
+MGDP\#-:!HV9P4MC]^DP Y32"G)!%B:^[2V!0>,GVP3WT_\J3._U>FY!#=HN@
+M6KMQ" 76P, 1SM*Y/JLA4E93>::0MI,1,=_&1E;"SLF4CHXA&GLD_P*!B8)0
+M?I6GN@/3_N<ZPC0+%(A?D*95O)<YIX]95_+V%%:3O9^=SI"4,)C1P"[^UL'C
+MY\^@'I&+N*:"E(CAY?#?QU'.08-#@HY2EHHM,=46-_O1AW*IO9M2Y(BV5-L+
+M0=CN%<4YOQ7]KI2.P8F4M'CL^[E@\E7/$TZ$D1N5AJR+N-X+]]W0R__P]*DT
+M!ZKXGP!42YPDM)JKR33UUF__VIVF6YVEE0/[ L\CP!!<FJE&HYG7&9)E,C3^
+M(UR(=ORP6/N]9A13&.^G\YY<LDUUB4'[-82XQQ035C/B\^U%N9F(38_6)03R
+MZM;#Q@-K\"&2J>^?@IJ$T2OW-/3F!UZ,FI1+'[\O.9H9TP %Q\I#XYR@G*6I
+M6HT=.*:=PV+C-C 6\(J<G)N,=XBJIGB/LR*"P>/<"H#WXD>XE4B#\[M"H8&E
+MNJG ]1YN/ML:LY.4I%5: N#6V3!7D#0,E]&"@Z#RDQ8R]AM4"+25C@BZ66)T
+M^<T-)=.7L_&O>DBVBS!0"<&6/3/EKQF'E*))LYNJ7Z>)PO)E[G?3"W+&E_I"
+MKE6*CZS">+YV>_%ICII 3K96BVP"D]&L!(_& T 76K!$'\;ANJRZ\\U^,]6]
+MQ9M:F([&_'"+EE1H7YO-2LG3^,/(@)"&K*Z@FEN_&Y_&7%W%]$7DQO;8O=!6
+MKOJ0G*K"[*"(K)P6.4^<G%K:\_#VW]0AN!R?OY>1*Z\B<JL8SW,9G8&)K)(<
+M^)<RC;J(P"RWW@-R2MI@NI^JAG+RRL36%,1 4**/L%M,N-Z6B97A$FWF]O#B
+MXU)0J ^?Y.K%G\]'S-S&1@_0%I::3;L?G9/@LH/0J_3'UM.*@5Y?@(*>DH.4
+MDYHJ\L@)RP%&U-LXJ(?%E5P%DQ;T>DZ[YQ+&Q;O&PU@1AA^6E9A*\03:T)!0
+MHDL'E)*:S?O5_<?Z4[GQE%6F^*I;0'73X2<#L=^QNOZRN)QS9M"#+.?M[G61
+M/%&B(;O?OUZ7L:_*DA_V_@= QB.8G(OX0I*5R,;!]/7V01W&'%2<F$^2K9(#
+MR,W(ZZ#0Y)]_O;@<ONO#8]*/VO:Q-&+23\B/_UK;MX*V>X:P@8/H+-W*[8>*
+MO8F\F7JWD&NKL997O\#A]6S0^J,<!DN>DE6L=XW'Q?0/\9._DE&QB1G6-] T
+MQ@%=E*.QQ?F^$Y/^2<C/ ,)%G)Y>I;>9_3"=VTE$\<;NQ=Q!#L&ZJH=?VEN/
+MTOY$\.X%:Q>3@K:XVZ7:@)F1E,?&]ZZ>\(V439X,DK)?^.<EWLQ$P)#KEH2=
+MN[V,AZ!>MV4^// 0-L]-G_M2@IJ-"%J CKZ [MC#QLT/Q<A+/H>Q_DY"MX"D
+M)%%QP/3L+L CG;VBD9"PFITEP\K0,8F2K773NAMB\BT:,- ++%.TF[#W0%ML
+M5 . YU/3GKP*VW2%JH*MMMTEN^UO]D\02H6KGKZ3A_>9O(73]O_ZY1-?I:E_
+MDXQ42TW9YO7^-Y>K2;2<P!Z:3(;&7[O1ZAX?T,/@B5><OI>>+4FTE*K1^_:=
+MTO6:ZIX$@IHZAY?RP,R?T0+,SY/O*@L(3XBU&W:_AUP&)J)RTV'0U^; )?22
+M%EU&4)99P-WS\:B$H[7;@WGS)]/1Y_07VZV*6;U45UY?*8Z:]PE%2:X*<YT@
+M@JF;H+>KR7OM-O0JU9U%LAN6@MR@DJP!^_'^4.-/?J&="D*=\HBJ1?''\#9Z
+M\HZRFJ^'=<DGK(:_].A<YMY2\JF6O$J;K<2BH+" K<&9>_(ZPH*8<)5>AX\H
+ME*2/@NTHZ>,C@P4'$DB7E0&[M@-#B^VGD0C#[J#@]8%0$(&[EZ"9@P%/HU&(
+M5D&K@)*Z?<L3E?+4^D]?EJ)=_;=>0OO=EPJ'R].\D*"=$A$6FZ&.GLCS[,;V
+M=M*O1:2+GIM25K%,6? 55/K.J8>ADQ^3!A/^H,R4)^_ZQ=..E9R5A4^^BT:@
+MFPK@U<W'SM:)E9ZMKPB_2K""22EZN<8PQ:."K_OQ%B"@@O)QLXWSR\GTS0L 
+MQ3&-KAZ+-AZ/J%>)A=;QX95L[_8EG9:>A8>)BYL(ZP44#Z:1])"*D8]A]/W5
+MFL2GKX">6W>BB(GH!9G2[L<6#YN1E+?$CDF&VOTGX/3&^EK=IWVX'_JJCXZ@
+MB?<QT3?DYPVDIKM8N))$7JK O_#L[A#EB9"83"E6"XK%E;["S 'X*LKSEYJP
+MG(V,"3UAL+\!X?VN^C7HFYI$@IA J[^6.*6_ ,_^SPW'!]K :I.[I;X&KQ;#
+MNYV/\_3QP]>5\*VR@+NFF+F-K8+:M82(7[> M:Y*A3GUQ-_7VWVFEIZ B,&?
+M;%Z_07SOUH_PH]'V$WR=$9 #K/OM9?K/XX>:D$J#JJRR!TIST<77PHOCHJ8O
+MNE&5I+=, 3YYY\0Z$4*W_8VYA;MKH)03&,^,3<_B_$S&6HIOGE^-8;H+"?+M
+ME_HV15">F5:#H%O\<I"4_?7.3.2+&Q?#."C8_5*J6@J3E&6%^V+$^>^7UZ.Z
+MO#9$E%22B/+_VK2H&%H&DI6*P^23Y=6V18*5DE;VAG264O*'KT<CYL6,D8']
+M59N1F*!/F<C&-\7OPM.?%GFVUX-?MZ69PO+VK^?8C1J6FZJ->%^+1N':Q;I?
+MNN:/U*B<CI9-;3A2<\<J0$S.]36?E)IO@IB-HJ FJPCAW?ZV.B])1<52OY =
+M'E*V$L-)S>X7Q]Y6SQP3AZVCUO^'&Y20\)?UZFWEPL#Q^8@+N5(R_BB9@U/8
+M>9Q4?867FG[(WK6Z9CK'C%P!K;:@DL,YLE?9YZKZK:"+K-^=F:ZLFJ[1.9,N
+M\,*XGQ8PDY.#&(ZPC-/ T1O4 -V:P?U**T9.FX ,TKFW_.93[Z:,.$*2/)D@
+M6J[##:Q4YMOQ@Z2]@$^3SU*( "TG/B7^IO)#BZZ0HIN.BYO4N\":U,[;XLK&
+MUL/<0-^="H)>BY::2+>ZV7+P]:_P)8_)@)U4B;^Z\<?1 %!PUJV:$)A7"1@\
+M]G[2X+P<E)[\@_Q3:[:/X5:*V;2<D+ZF48(:HE97#3'!]<7@EN^2>9><@UA_
+MB6RA:_<67Z.-GZ*_>(F=U8*4), O_/?P%6=.G,V7C V2J)7_4L#$R:E#\9^T
+M?DS%CHBHJZ7XQ;05QKWN\*&J^QO6$Y*!EZF7K:-OPB]/G_SJU$=^SK<?C\>>
+MT[.2FM'[Y.V4^BN$$%&],J*J6Q5.TO0=EA.IAE*RG>TVE3__K=/P78G/O8D:
+M]S!RFU%58S %G/*%59%8$\PPGN<^\7YEV_#+FL&]>^^<=B.EZ?XU=>X%CW"L
+MET.G2"!?G_#DU1+0]MM-< YL2Q*-0)&0>T5O_\U/%<.&]]^,5W^!/6:.70$6
+MQ^_FTE#M?9DFD9"0JI>YE]WRS<S'*M?.ZN 9B)SPEJ7KKXHQDD#E=/G][Y91
+MBK16&U>CB9C0_PLTO?P)SKHP@@ML_^SWMN^)6!!1GY"AJI'CO9U'(T#PK=S3
+MF4;7@KUAH(CJZL<0$,7!C;:YJIJO7;!Q1\7$-;_DQHJ:*/NJ.QS5H)G2]GFE
+MY!#BF51_G#L83+TAM[;GR$C5U_=XCI!)GDD.BXULEXM(U/-W[Y;;4HA?BUI/
+MB9PRK+(-40V+Q<EJ"EK5J:KUE8F:D_Z$>#*O!)&\UB^?V*^"ADM.L(+2H4OQ
+MT(P"HYU7@M.;\L*C.N_NVORP5ID$45F8P<R6C,[)_*VPTYT;@WBM09R"Q-&1
+M,]+*L9&&X;OVCWQ,]K_S^G#LE@NXC_F_2XBXGX%,D=1QO<0MSY:2C909GHL:
+MEE 1)T3*U4[;-.:%F !#&D&ZN:63T-+&KA/ (Q"<31\*1HV*WHF2U_'HVN-3
+M+^_P]F>25*.WD(Z:4'A%@PSZ->_7UZ?XL_Y-M'B!DN4#PM94=DH,M_>87XGF
+M+,?NU@5.?9:<\'U6J,!>BUG#QMQ?H90\E;$(@VP 5]*2967E]=&=ID7RF<J>
+M5$MH0,#$]'/'DI*X]H.#';)1O:"V\5 UDNZ]&Y>\KY>/JZP:&,<L7?VGVN&]
+MI(6^3)^KD4A$ P'"+9?FOL5;>*T?%G>/G)K!LX+(_ YUSPH'X\N(KMO!6)9M
+MGO)*3E,BDC[&)&;/4=A6A10)@ZODV=CX_9(B'YN5JHKRPC5M[_Y0N(NCA5H)
+M\C&CIJW7QO+"FI19K(^ASEVTAD+8H9;UT-'&F\:KJ]@0TM10K$6C]1 ?T6>R
+M;%\*FX>/DT 1'K3NUL;E8Q"]B@V'[1LAVD$)"@ 6!-K(AY;0/)RX9:MLEY_-
+M]-$P]B?;[YQY?H.62*BPI(^:P*I2*M$.2?. NYZ<@U9 FI&2H(_-Q^NN[O;^
+MRY!<DZZ6DMI7S:['T)01FE&@G1&;R+X\DCJ^);R4GAU>6K.18**7H:Z#Q+RC
+MG_F<69ZY0 _#R//F]\6K$HF/D[J2CPA7@HA8T]76U\J[TBP?LI&H=8FO\.'D
+M]^SZ)KM/C> ]<@%3EI(70.W>"N;"Q!22N)P4JX"3N*0^P-;U1L:N0ZB&F?#8
+MC[N6)NR<0LW]J</'".SE&,U^>8JHC8J:%^+5@M&C\S?O]'51H%I&FO"+F]D=
+M$_6X0GY=D%F"6\-R-?+FQ8^=G*2<I]*;F^VGD],KY=Q<$;>M,(M""V/7FR+V
+M-,6:<L'+18.XFJZ'AXNJ*];U-O EQZ:L$YAKOEK?#$R2?>_ZK^>6IMV_OUHK
+MAK&2TT$/ZLCF5SF5=GF4N:C,ZI!T@N+DQS"^,E6_%?&UEC!B@')P3H+9#LCW
+M6LC?&4"+3[=>J?J#FJ)LEI['\#7P;C;2EYRRD9HQEX+Q2O72?IB6G(Q0F8*C
+M,A/P]/( J(.*O)^8B+MIH(+S[84<+_)X6U8(O\.CUE<@\OGW^LBUJ/:D6_'/
+MG[>3O<+V'>0^Q1*2(_*;N(:&G[ZA0/U]_?ONNQ*0+]Z:JXNML B@Z ?H!LN(
+MJ:><@(SZ3IUSEI]!-^<;[O[#:9O=DIO6CU86@))?\?[KYNO(@&_5ARP1D7Q^
+MRISSD86:P/C&NB7D\_2)EM64>KFY@,+:X+PV4;BD=Y?;)_;S-OWPSZQ6RAF>
+M5WFY0]HXRP.NN'JCF)TSE5JIQHR#X1 \]C!7-8U6:9J;HU):GTJ@QL%]XB5&
+M_8R)K@_7L%.-US_NI]8VPI:6OHV]AXV3X%NZVJK4SXH"TKL5K]2)N)5'ME>=
+MPO1%)'_^"KNRK4.)H$U2LF.?B<W-^/;[!IT8X%N]D0**OH:6@8F%PT.R;,7V
+M)O4>6Y1%#9/;G?&)X-2N,_!6DEO,>0+QLS8R_\+^7-N]?;$*W2*$77##\1@J
+M@8&;=Z>06^R"0^#1\;?_T^ 2I&P)2CBJ.J%,R/&VUO("BZ>D0WJA%);U9%+_
+MQ>;\QO.&E!BHETV.JR10\P/^#,%+43VILDF.HWN^BZQ6BQ?6]_;ZUO#K+ASS
+MB;J-J9G!CIW)HAQ?(M_!0N&"GM\+GI)#5X.B-YCQOCE"_\8C68 6G8ZRB:CC
+M52G0/AP+A;"0G/?EP>?6_/IS+!:)KL*7NAEF0#O SN=45""6G0ZALD#CEE/)
+M%#3O]_'3DX!I@U&#OX1WG.S3P20ZP)*7H!.HK[.<%;C:GCSO_G/S ]91G\FH
+M0;ML(%?-R@\:QHK0EUX1CXN+C:]F_XJ#%L7^P&Z(AU97]Y>>J)^F\+Y[2._D
+M0]31 G$(N9/]&H!^F8A8L%.KP/XS.OXZY1J1^["0E*H+@O_STLQ0 Y$WEUJ>
+M\JOLGN64X[Q<@;6>4*^+S');(-L'W*Y;^Y"0DU:"J::9XT(W\,+CP^<7J8BO
+M@T:2D9H$P?COQNNFGY.#6AV2$HL:VI?V^A)'X3T:'$_M1HL.H8Y;%^0)_^?B
+MXH./E$1<BXS3Y!"]4L(E/K14)4NMC(JH4$.HGH9?K1GBZ=<C+B_"UI-[L0J*
+M=HU9H'"008URUL;N\,%V\)F^EZ&.B^</ N.-DH.%AK"H@O+ (7HGLA<ZD_*$
+MOC#(O:2'FL"*P,&:6;:$FGE9K76 F-DV+.WFPL.)7&P:V\>>4X",V3;%=J+:
+MDY>V5S9;K@Y3K,#0[])OUE.-O97MOU^)BL:$5_$*'N/' ,B4M%14G_8@CY:.
+M4Q$V-7OE^EJYAY"UJ:>]=A*[LX.E!EC/S\@LSO&KJ)[SL?62B?*2AYK#*A7N
+MTE?U?G >5:#1]I\%Z8'(#Y."T2 PO(#(T\<ZEO(!4%:^7<^17EUD^AC%U\D<
+M!KV*_W>"VKNB5Y+0L;/O]O-29YHJTI9+C%^5ZB#"/-#^R4:")9?ZJKX5HX!,
+MH?37^BY/G19>FF2N()MMAL*IR$VHWO,<3I:L3;D&[U-F=K;A,A#OYF6A@Y+Y
+MFP+R@7N*:80?AP)*S,\*5,J#JIN,B*FFDII3Q+!WPSKD]N7N\)J3E(MR@,.K
+M$7\5V*YR7@Z:\-B6\</U)")F1Q!+ABWVDU.2B/*3Z<[ L-@27EVRG5FNS-0/
+M09'X[^;.MHJ5EGV;G9^4BH# ]N7O\\_H]CA"^+J61:"$#/8\G=W PKF/K<YE
+MGHR)E(^?0\S?S03RRY>^GJJDJ&"':%6;%SO%]L7_\,N&G:.>]XJ:,J16T*<N
+M&/3'#LW#\YEZ7:%6CXMR-#BUF<W2.787[B>XV;).IO7\'T3)V^(:M?8[1YRL
+MJR22D?0N=>M%EHN<D@%:5ZQ:_N&IXQ80]@O]CG"#ES ,4L<0\3/6&W":G81]
+MBYJ^FD%-U7HO$]!CRXY@ 5J+AJ275LFR]626=\B+%MA9BI^AJ<"7@L#_U>+Q
+M4].#G@9@KYJ(G:P&EW'V-.+'^O/*EM%:T+Z?RJ'YDAOWB*ZG1) OTN6C2O&;
+MC'^/ I!ACA+1X_?UTM.%4));FU:E3$/EUX-AD)#0^Q(3U@W',;\5L+O VI ?
+MG+18F*DXA /'_,\+L+!3JX19J%OD$%W-,!&7]:L<&;V$@JGIE;K;X$F>EWK$
+MVP^>>8-3NHN=I5;EL";WYMK#UU^,; >]C4UCOG?HU"D9Q5LREIN?S R7N4TY
+MI9,%,\3V5A &DH^,VX.5AIX&:;>0D0*LU\>-QL661T^-DT.=JH!*IEZ:PI#E
+MEM>:P7^(0%U.2H*X[1G/\Y0+GBV7E192UZO\^O/N"YIQB\5&M+9;XM:M\1O/
+M'(6V,@2GDIZZH+>"QSO7<CO#0X_V;-=^8WZGM;YP]&?:9,]'\JNH@X*25Y^4
+M!^KF[-HT&)4/T8A'5ZD+;)1?\OP!6?<%T8* AJA.K[B):)^"\I8MM.9D6JM3
+MNX726FN65( @@RCH&-?M!8Q:DFBN&W6;A9.#H;)WFL#>+/J^/>"T>(JNCDB\
+MCZ5*X;6:<Y2[I:>Q_4>1T[_#QO"<VZ!?5I*9NY.VJLW.YOB\$G1WEAN>C_!6
+ME\4B-.X[RW#GIG1= X:MCHM/A^ 3];9PCJ7L0QB37Y33Z*USESO[^]B/4*Q(
+MZ9N;CX7<\7$/VMT6P\F/W+F:2_I-K::"@,SD^?ODTH_HEJRS>O>'4[8%MZV@
+M_H1QR\_$CL&'GYV0J;6)3A& EYKG_O/D]NW%A$B(J5J1'*GKWZ<07)7[D1QS
+M29+ ]G&6/I/+?(N3L0^=_)*4$-L(SP]D.)Q?A5>2FH%AI)<94;7FU@+(4H^@
+M5T.[#Z971@TPY^__SI*,H(M3@HCPA83 T_;2,!0!G)99KY:HJYUCAJ[#5I1\
+MK*4(OW2:1YFXC(:*5[_HU#WMM=;S0K95LUO5COI*;+9#$-@ X_]BRR>T4U>;
+M1Q&0II821I.;)</#\_ZD4)V+NE3'G(A;;=;W"+ <DXVV-Y.8V_13P,;&RO!<
+M4)6Z_0.?IG9!B>Z%%'"@%*^R6))6PYN_</_CW!/!'8K"K!NJNUY7GJK ZE4Z
+MXM'+%:Q9W(*&>A"MY^@NVJ_08HFF&8R]GI]!1D^7P,#.;<\HYH] '8B/7LVK
+M8$1;F9KL\C?Z3XJ2A7=<M[BWDDI3PIKH! 5)R\E#T(J67UZ3EHN7IG*DJN<R
+M]?#V;G%=D)']E8B:60',S])$4]&MEO<JV0DW[,;^[AJ:6Y&=L+E<.:*;FR+'
+M5M8\(%:1!"66JV32/<?RY=I7V)Q3FFR8^)V>U[6LR3C$]MY"PYH0>0F+?XRC
+M3<#^;>[P9AJ)5 9-@IR<CJ906>E( !D,STJ>@(YJ75YHNKG[FM'S[/3P,@-)
+MOME;5J1(G[N OXF% ONCRX_"CKR!E'V%J//GDY3!EY[-,A<L/\0 K[AR4=:5
+M%OMBH]L=G(@'KZ"! (/!:_22;AS%T)R"A:!9_'VXC(+$R\>9G_:*3_>(DXLH
+MAX#;:&SN,O<84)6 FM.I=HR5JL'B_/+D]5*VDIJYNPI&DHW17\=V=C[15W:(
+MO#^&#HZXCDC-#?^O#<NU@_ %C<FHE(/D!I#Q>=/27B_'RI6MPPI&NH.QEIJ*
+M\,9" 5+,Q]K)F7@YFI9:+?R?H)^-I.(]%>[WCSV T;$Z,9:9!=SE$)B2$ECW
+MD1J0P])O^[;$PKP*7H22H)NM0I/=*,-*_)"[FJF5]UH>IHY33>)6_;XK.X*C
+MK;BS@OI&=8X TB&6VD^[GD&7^CV8SZ:M]-XNQ?Y@PVR6GZJ\VH]=H+79V\[*
+MS0;2RK^RNHRL^Z6BK!X;K7*YQ].Z<X.^D3]65H^.0F&&V\S*H ;3+O_#R T_
+MKX* >X^>!&)7O1KSG<;POB.\B()UL'2^6]+*V?*4D9.%4%37@<'F[,)O[\..
+M\(.;EG>"@SF7MJ7)V1 .B+*MFB%<@TR^B,/3D3!4TM!'Q[*=%\<(C[<&X./U
+M^I;6DIV%@MJ2UH9(K:'P_<?6<\^)5X1'GH^6JRBGJM+MZ@_#&,)$TJIH@U*]
+MO:",7<?69W7F_D5(QE&?0?;C?YIBM+['W?AT!Q7OXM6&NGV!FJ>ZEYX%EY_ 
+MU-SN>O+:])R$FP!QB8/1^87 A(A?FU2C?%+"PCCR$.6CNHBSK5H$KH/!WD'9
+M(P/R7"2&B38128FR]YK,\_E %^5)IX_#FKB-?I91@:+D%6[WU4_2;).L;Y^7
+MVK[-UM5VFL8;KJ2. 8FOKI,$L(K)5EP; L7REH_PK+V>1-*[EBG)$N<V\/?!
+MD+:\D[R3AUZ#D;99R6^ X/B/R5K%D)B$BHOPDXR9L<1;83 WQC)DPUV8-EBP
+M6YF=5P7(XYSQL:F4B8*?!31'^F_Z ZV"69%>\5R+E9:*T->.QAJ<D ??H)NO
+M:*97\L8UT'O/M)-4H%L?D+^:23C,]^S6;"Z+5)&2@)J>5L*5TRJO$O 6X4^0
+ME)2'7EZ;I8"!TJ * <F+5NV:# B;6BRM#(Z^V/'Q-9;PU:K_AJ*KE<?#FA.T
+M<74&Z:0BS4[#$;M2_1""EZVO@$B^"0'V'W:7^@-<RY.=,$D<>]G?*\B>0M(-
+M6MF1FM+ ?,?3[N*1EI"$AXF#@6P-FU/4\QF?O!&%CE5*6J2'@QDYV?_04-O)
+M7Z 6N;C7E_4<F/8XVG7AJ7J1LH.IECZ6%G $HMQ2FZX'!S+=;-"'3%ZWD\!W
+L@U91N$5;H(.IY7/\[ +<DK:=41RCLZB8DY:;7E]*"<<2\\K1C(F(AZJX39  
+<!-- END ENCRYPTED FILE --->
diff --git a/src/libs/mynewt-nimble/nimble/host/pts/tpg/94654-20170317-085441153.pts b/src/libs/mynewt-nimble/nimble/host/pts/tpg/94654-20170317-085441153.pts
new file mode 100644
index 00000000..6760d5ec
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/pts/tpg/94654-20170317-085441153.pts
@@ -0,0 +1,289 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project><qdid>94654</qdid><name>mynewt-Updated</name>
+	<pics>
+		<profile>
+		<name>L2CAP</name>
+			<item><table>0</table><row>2</row></item>
+			<item><table>1</table><row>3</row></item>
+			<item><table>1</table><row>4</row></item>
+			<item><table>1</table><row>5</row></item>
+			<item><table>1</table><row>6</row></item>
+			<item><table>2</table><row>40</row></item>
+			<item><table>2</table><row>41</row></item>
+			<item><table>2</table><row>42</row></item>
+			<item><table>2</table><row>43</row></item>
+			<item><table>2</table><row>45</row></item>
+			<item><table>2</table><row>46</row></item>
+			<item><table>2</table><row>47</row></item>
+			<item><table>3</table><row>1</row></item>
+			<item><table>3</table><row>12</row></item>
+			<item><table>3</table><row>16</row></item>
+		</profile>
+		<profile>
+		<name>GAP</name>
+			<item><table>0</table><row>2</row></item>
+			<item><table>0a</table><row>4</row></item>
+			<item><table>10</table><row>1</row></item>
+			<item><table>12</table><row>1</row></item>
+			<item><table>13</table><row>1</row></item>
+			<item><table>13</table><row>2</row></item>
+			<item><table>14</table><row>1</row></item>
+			<item><table>15</table><row>1</row></item>
+			<item><table>16</table><row>1</row></item>
+			<item><table>18</table><row>1</row></item>
+			<item><table>18</table><row>2</row></item>
+			<item><table>19</table><row>1</row></item>
+			<item><table>19</table><row>2</row></item>
+			<item><table>19</table><row>3</row></item>
+			<item><table>20</table><row>1</row></item>
+			<item><table>20</table><row>2</row></item>
+			<item><table>20</table><row>3</row></item>
+			<item><table>20</table><row>4</row></item>
+			<item><table>20A</table><row>1</row></item>
+			<item><table>20A</table><row>10</row></item>
+			<item><table>20A</table><row>11</row></item>
+			<item><table>20A</table><row>12</row></item>
+			<item><table>20A</table><row>13</row></item>
+			<item><table>20A</table><row>14</row></item>
+			<item><table>20A</table><row>15</row></item>
+			<item><table>20A</table><row>16</row></item>
+			<item><table>20A</table><row>17</row></item>
+			<item><table>20A</table><row>2</row></item>
+			<item><table>20A</table><row>3</row></item>
+			<item><table>20A</table><row>4</row></item>
+			<item><table>20A</table><row>5</row></item>
+			<item><table>20A</table><row>7</row></item>
+			<item><table>20A</table><row>8</row></item>
+			<item><table>20A</table><row>9</row></item>
+			<item><table>21</table><row>1</row></item>
+			<item><table>21</table><row>2</row></item>
+			<item><table>21</table><row>3</row></item>
+			<item><table>21</table><row>4</row></item>
+			<item><table>21</table><row>5</row></item>
+			<item><table>21</table><row>6</row></item>
+			<item><table>21</table><row>7</row></item>
+			<item><table>21</table><row>8</row></item>
+			<item><table>21</table><row>9</row></item>
+			<item><table>22</table><row>1</row></item>
+			<item><table>22</table><row>2</row></item>
+			<item><table>22</table><row>3</row></item>
+			<item><table>22</table><row>4</row></item>
+			<item><table>23</table><row>1</row></item>
+			<item><table>23</table><row>2</row></item>
+			<item><table>23</table><row>3</row></item>
+			<item><table>23</table><row>4</row></item>
+			<item><table>23</table><row>5</row></item>
+			<item><table>24</table><row>1</row></item>
+			<item><table>24</table><row>2</row></item>
+			<item><table>24</table><row>3</row></item>
+			<item><table>24</table><row>4</row></item>
+			<item><table>25</table><row>1</row></item>
+			<item><table>25</table><row>2</row></item>
+			<item><table>25</table><row>3</row></item>
+			<item><table>25</table><row>4</row></item>
+			<item><table>25</table><row>7</row></item>
+			<item><table>25</table><row>8</row></item>
+			<item><table>25</table><row>9</row></item>
+			<item><table>26</table><row>1</row></item>
+			<item><table>26</table><row>2</row></item>
+			<item><table>26</table><row>3</row></item>
+			<item><table>26</table><row>4</row></item>
+			<item><table>27</table><row>1</row></item>
+			<item><table>27</table><row>2</row></item>
+			<item><table>28</table><row>1</row></item>
+			<item><table>28</table><row>2</row></item>
+			<item><table>29</table><row>1</row></item>
+			<item><table>29</table><row>2</row></item>
+			<item><table>29</table><row>3</row></item>
+			<item><table>29</table><row>4</row></item>
+			<item><table>30</table><row>1</row></item>
+			<item><table>30</table><row>2</row></item>
+			<item><table>31</table><row>1</row></item>
+			<item><table>31</table><row>2</row></item>
+			<item><table>31</table><row>3</row></item>
+			<item><table>31</table><row>4</row></item>
+			<item><table>31</table><row>5</row></item>
+			<item><table>31</table><row>6</row></item>
+			<item><table>31</table><row>7</row></item>
+			<item><table>31</table><row>8</row></item>
+			<item><table>31</table><row>9</row></item>
+			<item><table>32</table><row>1</row></item>
+			<item><table>32</table><row>2</row></item>
+			<item><table>32</table><row>3</row></item>
+			<item><table>33</table><row>1</row></item>
+			<item><table>33</table><row>2</row></item>
+			<item><table>33</table><row>3</row></item>
+			<item><table>33</table><row>4</row></item>
+			<item><table>33</table><row>5</row></item>
+			<item><table>33</table><row>6</row></item>
+			<item><table>34</table><row>1</row></item>
+			<item><table>34</table><row>2</row></item>
+			<item><table>34</table><row>3</row></item>
+			<item><table>35</table><row>1</row></item>
+			<item><table>35</table><row>2</row></item>
+			<item><table>35</table><row>3</row></item>
+			<item><table>35</table><row>4</row></item>
+			<item><table>35</table><row>5</row></item>
+			<item><table>35</table><row>7</row></item>
+			<item><table>35</table><row>8</row></item>
+			<item><table>35</table><row>9</row></item>
+			<item><table>37</table><row>1</row></item>
+			<item><table>37</table><row>2</row></item>
+			<item><table>37</table><row>3</row></item>
+			<item><table>5</table><row>1</row></item>
+			<item><table>5</table><row>2</row></item>
+			<item><table>5</table><row>3</row></item>
+			<item><table>5</table><row>4</row></item>
+			<item><table>6</table><row>1</row></item>
+			<item><table>7</table><row>1</row></item>
+			<item><table>7</table><row>2</row></item>
+			<item><table>8</table><row>1</row></item>
+			<item><table>8</table><row>2</row></item>
+			<item><table>8a</table><row>3</row></item>
+			<item><table>9</table><row>1</row></item>
+		</profile>
+		<profile>
+		<name>SUM ICS</name>
+		</profile>
+		<profile>
+		<name>PROD</name>
+		</profile>
+		<profile>
+		<name>GATT</name>
+			<item><table>1</table><row>1</row></item>
+			<item><table>1</table><row>2</row></item>
+			<item><table>2</table><row>2</row></item>
+			<item><table>3</table><row>1</row></item>
+			<item><table>3</table><row>10</row></item>
+			<item><table>3</table><row>11</row></item>
+			<item><table>3</table><row>12</row></item>
+			<item><table>3</table><row>14</row></item>
+			<item><table>3</table><row>15</row></item>
+			<item><table>3</table><row>16</row></item>
+			<item><table>3</table><row>17</row></item>
+			<item><table>3</table><row>18</row></item>
+			<item><table>3</table><row>19</row></item>
+			<item><table>3</table><row>2</row></item>
+			<item><table>3</table><row>20</row></item>
+			<item><table>3</table><row>21</row></item>
+			<item><table>3</table><row>22</row></item>
+			<item><table>3</table><row>23</row></item>
+			<item><table>3</table><row>3</row></item>
+			<item><table>3</table><row>4</row></item>
+			<item><table>3</table><row>5</row></item>
+			<item><table>3</table><row>6</row></item>
+			<item><table>3</table><row>7</row></item>
+			<item><table>3</table><row>8</row></item>
+			<item><table>3</table><row>9</row></item>
+			<item><table>4</table><row>1</row></item>
+			<item><table>4</table><row>10</row></item>
+			<item><table>4</table><row>11</row></item>
+			<item><table>4</table><row>12</row></item>
+			<item><table>4</table><row>14</row></item>
+			<item><table>4</table><row>15</row></item>
+			<item><table>4</table><row>16</row></item>
+			<item><table>4</table><row>17</row></item>
+			<item><table>4</table><row>18</row></item>
+			<item><table>4</table><row>19</row></item>
+			<item><table>4</table><row>2</row></item>
+			<item><table>4</table><row>20</row></item>
+			<item><table>4</table><row>21</row></item>
+			<item><table>4</table><row>22</row></item>
+			<item><table>4</table><row>23</row></item>
+			<item><table>4</table><row>3</row></item>
+			<item><table>4</table><row>4</row></item>
+			<item><table>4</table><row>5</row></item>
+			<item><table>4</table><row>6</row></item>
+			<item><table>4</table><row>7</row></item>
+			<item><table>4</table><row>8</row></item>
+			<item><table>4</table><row>9</row></item>
+			<item><table>7</table><row>2</row></item>
+			<item><table>7</table><row>3</row></item>
+			<item><table>7</table><row>4</row></item>
+		</profile>
+		<profile>
+		<name>ATT</name>
+			<item><table>1</table><row>1</row></item>
+			<item><table>1</table><row>2</row></item>
+			<item><table>2</table><row>2</row></item>
+			<item><table>3</table><row>1</row></item>
+			<item><table>3</table><row>10</row></item>
+			<item><table>3</table><row>11</row></item>
+			<item><table>3</table><row>12</row></item>
+			<item><table>3</table><row>13</row></item>
+			<item><table>3</table><row>14</row></item>
+			<item><table>3</table><row>15</row></item>
+			<item><table>3</table><row>16</row></item>
+			<item><table>3</table><row>17</row></item>
+			<item><table>3</table><row>18</row></item>
+			<item><table>3</table><row>19</row></item>
+			<item><table>3</table><row>2</row></item>
+			<item><table>3</table><row>20</row></item>
+			<item><table>3</table><row>22</row></item>
+			<item><table>3</table><row>23</row></item>
+			<item><table>3</table><row>24</row></item>
+			<item><table>3</table><row>25</row></item>
+			<item><table>3</table><row>26</row></item>
+			<item><table>3</table><row>27</row></item>
+			<item><table>3</table><row>28</row></item>
+			<item><table>3</table><row>29</row></item>
+			<item><table>3</table><row>3</row></item>
+			<item><table>3</table><row>4</row></item>
+			<item><table>3</table><row>5</row></item>
+			<item><table>3</table><row>6</row></item>
+			<item><table>3</table><row>7</row></item>
+			<item><table>3</table><row>8</row></item>
+			<item><table>3</table><row>9</row></item>
+			<item><table>4</table><row>1</row></item>
+			<item><table>4</table><row>10</row></item>
+			<item><table>4</table><row>11</row></item>
+			<item><table>4</table><row>12</row></item>
+			<item><table>4</table><row>13</row></item>
+			<item><table>4</table><row>14</row></item>
+			<item><table>4</table><row>15</row></item>
+			<item><table>4</table><row>16</row></item>
+			<item><table>4</table><row>17</row></item>
+			<item><table>4</table><row>18</row></item>
+			<item><table>4</table><row>19</row></item>
+			<item><table>4</table><row>2</row></item>
+			<item><table>4</table><row>20</row></item>
+			<item><table>4</table><row>22</row></item>
+			<item><table>4</table><row>23</row></item>
+			<item><table>4</table><row>24</row></item>
+			<item><table>4</table><row>25</row></item>
+			<item><table>4</table><row>26</row></item>
+			<item><table>4</table><row>27</row></item>
+			<item><table>4</table><row>28</row></item>
+			<item><table>4</table><row>29</row></item>
+			<item><table>4</table><row>3</row></item>
+			<item><table>4</table><row>4</row></item>
+			<item><table>4</table><row>5</row></item>
+			<item><table>4</table><row>6</row></item>
+			<item><table>4</table><row>7</row></item>
+			<item><table>4</table><row>8</row></item>
+			<item><table>4</table><row>9</row></item>
+			<item><table>5</table><row>2</row></item>
+			<item><table>5</table><row>3</row></item>
+			<item><table>5</table><row>4</row></item>
+		</profile>
+		<profile>
+		<name>SM</name>
+			<item><table>1</table><row>1</row></item>
+			<item><table>1</table><row>2</row></item>
+			<item><table>2</table><row>1</row></item>
+			<item><table>2</table><row>2</row></item>
+			<item><table>2</table><row>3</row></item>
+			<item><table>2</table><row>5</row></item>
+			<item><table>3</table><row>1</row></item>
+			<item><table>4</table><row>1</row></item>
+			<item><table>4</table><row>2</row></item>
+			<item><table>5</table><row>1</row></item>
+			<item><table>5</table><row>2</row></item>
+			<item><table>5</table><row>3</row></item>
+			<item><table>5</table><row>4</row></item>
+			<item><table>7</table><row>1</row></item>
+			<item><table>7</table><row>2</row></item>
+			<item><table>7</table><row>3</row></item>
+		</profile>
+	</pics>
+	</project>
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ans/include/services/ans/ble_svc_ans.h b/src/libs/mynewt-nimble/nimble/host/services/ans/include/services/ans/ble_svc_ans.h
new file mode 100644
index 00000000..435e9e11
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ans/include/services/ans/ble_svc_ans.h
@@ -0,0 +1,85 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_ANS_
+#define H_BLE_SVC_ANS_
+
+struct ble_hs_cfg;
+
+/* 16 Bit Alert Notification Service UUID */
+#define BLE_SVC_ANS_UUID16                                  0x1811
+
+/* 16 Bit Alert Notification Service Characteristic UUIDs */
+#define BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT            0x2a47
+#define BLE_SVC_ANS_CHR_UUID16_NEW_ALERT                    0x2a46
+#define BLE_SVC_ANS_CHR_UUID16_SUP_UNR_ALERT_CAT            0x2a48
+#define BLE_SVC_ANS_CHR_UUID16_UNR_ALERT_STAT               0x2a45
+#define BLE_SVC_ANS_CHR_UUID16_ALERT_NOT_CTRL_PT            0x2a44
+
+/* Alert Notification Service Category ID Bit Masks
+ *
+ * TODO: Add remaining 2 optional categories */
+#define BLE_SVC_ANS_CAT_BM_NONE                             0x00
+#define BLE_SVC_ANS_CAT_BM_SIMPLE_ALERT                     0x01
+#define BLE_SVC_ANS_CAT_BM_EMAIL                            0x02
+#define BLE_SVC_ANS_CAT_BM_NEWS                             0x04
+#define BLE_SVC_ANS_CAT_BM_CALL                             0x08
+#define BLE_SVC_ANS_CAT_BM_MISSED_CALL                      0x10
+#define BLE_SVC_ANS_CAT_BM_SMS                              0x20
+#define BLE_SVC_ANS_CAT_BM_VOICE_MAIL                       0x40
+#define BLE_SVC_ANS_CAT_BM_SCHEDULE                         0x80
+
+/* Alert Notification Service Category IDs
+ *
+ * TODO: Add remaining 2 optional categories */
+#define BLE_SVC_ANS_CAT_ID_SIMPLE_ALERT                     0
+#define BLE_SVC_ANS_CAT_ID_EMAIL                            1
+#define BLE_SVC_ANS_CAT_ID_NEWS                             2
+#define BLE_SVC_ANS_CAT_ID_CALL                             3
+#define BLE_SVC_ANS_CAT_ID_MISSED_CALL                      4
+#define BLE_SVC_ANS_CAT_ID_SMS                              5
+#define BLE_SVC_ANS_CAT_ID_VOICE_MAIL                       6
+#define BLE_SVC_ANS_CAT_ID_SCHEDULE                         7
+
+/* Number of valid ANS categories
+ *
+ * TODO: Add remaining 2 optional categories */
+#define BLE_SVC_ANS_CAT_NUM                                 8
+
+/* Alert Notification Control Point Command IDs */
+#define BLE_SVC_ANS_CMD_EN_NEW_ALERT_CAT                    0
+#define BLE_SVC_ANS_CMD_EN_UNR_ALERT_CAT                    1
+#define BLE_SVC_ANS_CMD_DIS_NEW_ALERT_CAT                   2
+#define BLE_SVC_ANS_CMD_DIS_UNR_ALERT_CAT                   3
+#define BLE_SVC_ANS_CMD_NOT_NEW_ALERT_IMMEDIATE             4
+#define BLE_SVC_ANS_CMD_NOT_UNR_ALERT_IMMEDIATE             5
+
+/* Error Definitions */
+#define BLE_SVC_ANS_ERR_CMD_NOT_SUPPORTED                   0xA0
+
+void ble_svc_ans_on_gap_connect(uint16_t conn_handle);
+
+int ble_svc_ans_new_alert_add(uint8_t cat_id,
+                              const char * info_str);
+int ble_svc_ans_unr_alert_add(uint8_t cat_id);
+
+void ble_svc_ans_init(void);
+
+#endif
+
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ans/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/ans/pkg.yml
new file mode 100644
index 00000000..691e566e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ans/pkg.yml
@@ -0,0 +1,34 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/ans
+pkg.description: Alert Notification Service Server.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - ans
+    - nimble
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_ans_init: 'MYNEWT_VAL(BLE_SVC_ANS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ans/src/ble_svc_ans.c b/src/libs/mynewt-nimble/nimble/host/services/ans/src/ble_svc_ans.c
new file mode 100644
index 00000000..5b64f18c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ans/src/ble_svc_ans.c
@@ -0,0 +1,463 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "host/ble_hs.h"
+#include "host/ble_gap.h"
+#include "services/ans/ble_svc_ans.h"
+
+/* Max length of new alert info string */
+#define BLE_SVC_ANS_INFO_STR_MAX_LEN        18
+/* Max length of a new alert notification, max string length + 2 bytes
+ * for category ID and count. */
+#define BLE_SVC_ANS_NEW_ALERT_MAX_LEN   (BLE_SVC_ANS_INFO_STR_MAX_LEN + 2)
+
+/* Supported categories bitmasks */
+static uint8_t ble_svc_ans_new_alert_cat;
+static uint8_t ble_svc_ans_unr_alert_cat;
+
+/* Characteristic values */
+static uint8_t ble_svc_ans_new_alert_val[BLE_SVC_ANS_NEW_ALERT_MAX_LEN];
+static uint16_t ble_svc_ans_new_alert_val_len;
+static uint8_t ble_svc_ans_unr_alert_stat[2];
+static uint8_t ble_svc_ans_alert_not_ctrl_pt[2];
+
+/* Alert counts, one value for each category */
+static uint8_t ble_svc_ans_new_alert_cnt[BLE_SVC_ANS_CAT_NUM];
+static uint8_t ble_svc_ans_unr_alert_cnt[BLE_SVC_ANS_CAT_NUM];
+
+/* Characteristic value handles */
+static uint16_t ble_svc_ans_new_alert_val_handle;
+static uint16_t ble_svc_ans_unr_alert_val_handle;
+
+/* Connection handle
+ *
+ * TODO: In order to support multiple connections we would need to save
+ *       the handles for every connection, not just the most recent. Then
+ *       we would need to notify each connection when needed.
+ * */
+static uint16_t ble_svc_ans_conn_handle;
+
+/* Access function */
+static int
+ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+/* Notify new alert */
+static int
+ble_svc_ans_new_alert_notify(uint8_t cat_id, const char * info_str);
+
+/* Notify unread alert */
+static int
+ble_svc_ans_unr_alert_notify(uint8_t cat_id);
+
+/* Save written value to local characteristic value */
+static int
+ble_svc_ans_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
+                      void *dst, uint16_t *len);
+
+static const struct ble_gatt_svc_def ble_svc_ans_defs[] = {
+    {
+        /*** Alert Notification Service. */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /** Supported New Alert Catagory
+             *
+             * This characteristic exposes what categories of new
+             * alert are supported in the server.
+             */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT),
+            .access_cb = ble_svc_ans_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            /** New Alert
+             *
+             * This characteristic exposes information about
+             * the count of new alerts (for a given category).
+             */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_CHR_UUID16_NEW_ALERT),
+            .access_cb = ble_svc_ans_access,
+            .val_handle = &ble_svc_ans_new_alert_val_handle,
+            .flags = BLE_GATT_CHR_F_NOTIFY,
+        }, {
+            /** Supported Unread Alert Catagory
+             *
+             * This characteristic exposes what categories of
+             * unread alert are supported in the server.
+             */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_CHR_UUID16_SUP_UNR_ALERT_CAT),
+            .access_cb = ble_svc_ans_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            /** Unread Alert Status
+             *
+             * This characteristic exposes the count of unread
+             * alert events existing in the server.
+             */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_CHR_UUID16_UNR_ALERT_STAT),
+            .access_cb = ble_svc_ans_access,
+            .val_handle = &ble_svc_ans_unr_alert_val_handle,
+            .flags = BLE_GATT_CHR_F_NOTIFY,
+        }, {
+            /** Alert Notification Control Point
+             *
+             * This characteristic allows the peer device to
+             * enable/disable the alert notification of new alert
+             * and unread event more selectively than can be done
+             * by setting or clearing the notification bit in the
+             * Client Characteristic Configuration for each alert
+             * characteristic.
+             */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_CHR_UUID16_ALERT_NOT_CTRL_PT),
+            .access_cb = ble_svc_ans_access,
+            .flags = BLE_GATT_CHR_F_WRITE,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * ANS access function
+ */
+static int
+ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle,
+                          struct ble_gatt_access_ctxt *ctxt,
+                          void *arg)
+{
+    uint16_t uuid16;
+    int rc;
+
+    /* ANS Control point command and catagory variables */
+    uint8_t cmd_id;
+    uint8_t cat_id;
+    uint8_t cat_bit_mask;
+    int i;
+
+    uuid16 = ble_uuid_u16(ctxt->chr->uuid);
+    assert(uuid16 != 0);
+
+    switch (uuid16) {
+    case BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ble_svc_ans_new_alert_cat,
+                            sizeof ble_svc_ans_new_alert_cat);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_SVC_ANS_CHR_UUID16_NEW_ALERT:
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = ble_svc_ans_chr_write(ctxt->om, 0,
+                                       sizeof ble_svc_ans_new_alert_val,
+                                       ble_svc_ans_new_alert_val,
+                                       &ble_svc_ans_new_alert_val_len);
+            return rc;
+
+        } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+            rc = os_mbuf_append(ctxt->om, &ble_svc_ans_new_alert_val,
+                                sizeof ble_svc_ans_new_alert_val);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    case BLE_SVC_ANS_CHR_UUID16_SUP_UNR_ALERT_CAT:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ble_svc_ans_unr_alert_cat,
+                            sizeof ble_svc_ans_unr_alert_cat);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_SVC_ANS_CHR_UUID16_UNR_ALERT_STAT:
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = ble_svc_ans_chr_write(ctxt->om,
+                                       sizeof ble_svc_ans_unr_alert_stat,
+                                       sizeof ble_svc_ans_unr_alert_stat,
+                                       &ble_svc_ans_unr_alert_stat,
+                                       NULL);
+            return rc;
+        } else {
+            rc = os_mbuf_append(ctxt->om, &ble_svc_ans_unr_alert_stat,
+                                sizeof ble_svc_ans_unr_alert_stat);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+
+    case BLE_SVC_ANS_CHR_UUID16_ALERT_NOT_CTRL_PT:
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = ble_svc_ans_chr_write(ctxt->om,
+                                       sizeof ble_svc_ans_alert_not_ctrl_pt,
+                                       sizeof ble_svc_ans_alert_not_ctrl_pt,
+                                       &ble_svc_ans_alert_not_ctrl_pt,
+                                       NULL);
+            if (rc != 0) {
+                return rc;
+            }
+
+            /* Get command ID and category ID */
+            cmd_id = ble_svc_ans_alert_not_ctrl_pt[0];
+            cat_id = ble_svc_ans_alert_not_ctrl_pt[1];
+
+
+            /* Set cat_bit_mask to the appropriate bitmask based on cat_id */
+            if (cat_id < BLE_SVC_ANS_CAT_NUM) {
+                cat_bit_mask = (1 << cat_id);
+            } else if (cat_id == 0xff) {
+                cat_bit_mask = cat_id;
+            } else {
+                /* invalid category ID */
+                return BLE_ATT_ERR_UNLIKELY;
+            }
+
+            switch (cmd_id) {
+            case BLE_SVC_ANS_CMD_EN_NEW_ALERT_CAT:
+                ble_svc_ans_new_alert_cat |= cat_bit_mask;
+                break;
+            case BLE_SVC_ANS_CMD_EN_UNR_ALERT_CAT:
+                ble_svc_ans_unr_alert_cat |= cat_bit_mask;
+                break;
+            case BLE_SVC_ANS_CMD_DIS_NEW_ALERT_CAT:
+                ble_svc_ans_new_alert_cat &= ~cat_bit_mask;
+                break;
+            case BLE_SVC_ANS_CMD_DIS_UNR_ALERT_CAT:
+                ble_svc_ans_unr_alert_cat &= ~cat_bit_mask;
+                break;
+            case BLE_SVC_ANS_CMD_NOT_NEW_ALERT_IMMEDIATE:
+                if (cat_id == 0xff) {
+                    /* If cat_id is 0xff, notify on all enabled categories */
+                    for (i = BLE_SVC_ANS_CAT_NUM - 1; i > 0; --i) {
+                        if ((ble_svc_ans_new_alert_cat >> i) & 0x01) {
+                            ble_svc_ans_new_alert_notify(i, NULL);
+                        }
+                    }
+                } else {
+                    ble_svc_ans_new_alert_notify(cat_id, NULL);
+                }
+                break;
+            case BLE_SVC_ANS_CMD_NOT_UNR_ALERT_IMMEDIATE:
+                if (cat_id == 0xff) {
+                    /* If cat_id is 0xff, notify on all enabled categories */
+                    for (i = BLE_SVC_ANS_CAT_NUM - 1; i > 0; --i) {
+                        if ((ble_svc_ans_unr_alert_cat >> i) & 0x01) {
+                            ble_svc_ans_unr_alert_notify(i);
+                        }
+                    }
+                } else {
+                    ble_svc_ans_unr_alert_notify(cat_id);
+                }
+                break;
+            default:
+                return BLE_SVC_ANS_ERR_CMD_NOT_SUPPORTED;
+            }
+            return 0;
+        } else {
+            rc = BLE_ATT_ERR_UNLIKELY;
+        }
+        return rc;
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+/**
+ * This function must be called with the connection handlewhen a gap
+ * connect event is received in order to send notifications to the
+ * client.
+ *
+ * @params conn_handle          The connection handle for the current
+ *                                  connection.
+ */
+void
+ble_svc_ans_on_gap_connect(uint16_t conn_handle)
+{
+    ble_svc_ans_conn_handle = conn_handle;
+}
+
+/**
+ * Adds a new alert to the given category then notifies the client
+ * if the given category is valid and enabled.
+ *
+ * @param cat_flag              The id for the category which should
+ *                                  should be incremented and notified
+ * @param info_str              The info string to be sent to the client
+ *                                  with the notification.
+ *
+ * @return 0 on success, non-zero error code otherwise.
+ */
+int
+ble_svc_ans_new_alert_add(uint8_t cat_id, const char * info_str)
+{
+    uint8_t cat_bit_mask;
+
+    if (cat_id < BLE_SVC_ANS_CAT_NUM) {
+        cat_bit_mask = (1 << cat_id);
+    } else {
+        return BLE_HS_EINVAL;
+    }
+
+    if ((cat_bit_mask & ble_svc_ans_new_alert_cat) == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_svc_ans_new_alert_cnt[cat_id] += 1;
+    return ble_svc_ans_new_alert_notify(cat_id, info_str);
+}
+
+/**
+ * Adds an unread alert to the given category then notifies the client
+ * if the given category is valid and enabled.
+ *
+ * @param cat_flag              The flag for the category which should
+ *                                  should be incremented and notified
+ *
+ * @return 0 on success, non-zero error code otherwise.
+ */
+int
+ble_svc_ans_unr_alert_add(uint8_t cat_id)
+{
+    uint8_t cat_bit_mask;
+
+    if (cat_id < BLE_SVC_ANS_CAT_NUM) {
+        cat_bit_mask = 1 << cat_id;
+    } else {
+        return BLE_HS_EINVAL;
+    }
+
+    if ((cat_bit_mask & ble_svc_ans_unr_alert_cat) == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_svc_ans_unr_alert_cnt[cat_id] += 1;
+    return ble_svc_ans_unr_alert_notify(cat_id);
+}
+
+/**
+ * Send a new alert notification to the given category with the
+ * given info string.
+ *
+ * @param cat_id                The ID of the category to send the
+ *                                  notification to.
+ * @param info_str              The info string to send with the
+ *                                  notification
+ *
+ * @return 0 on success, non-zero error code otherwise.
+ */
+static int
+ble_svc_ans_new_alert_notify(uint8_t cat_id, const char * info_str)
+{
+    int info_str_len;
+
+    /* Clear notification to remove old infomation that may persist */
+    memset(&ble_svc_ans_new_alert_val, '\0',
+           BLE_SVC_ANS_NEW_ALERT_MAX_LEN);
+
+    /* Set ID and count values */
+    ble_svc_ans_new_alert_val[0] = cat_id;
+    ble_svc_ans_new_alert_val[1] = ble_svc_ans_new_alert_cnt[cat_id];
+
+    if (info_str) {
+        info_str_len = strlen(info_str);
+        if (info_str_len > BLE_SVC_ANS_INFO_STR_MAX_LEN) {
+            /* If info_str is longer than the max string length only
+             * write up to the maximum length */
+            memcpy(&ble_svc_ans_new_alert_val[2], info_str,
+                   BLE_SVC_ANS_INFO_STR_MAX_LEN);
+        } else {
+            memcpy(&ble_svc_ans_new_alert_val[2], info_str, info_str_len);
+        }
+    }
+    return ble_gattc_notify(ble_svc_ans_conn_handle,
+                            ble_svc_ans_new_alert_val_handle);
+}
+
+/**
+ * Send an unread alert notification to the given category.
+ *
+ * @param cat_id                The ID of the category to send the
+ *                                  notificaiton to.
+ *
+ * @return 0 on success, non-zer0 error code otherwise.
+ */
+static int
+ble_svc_ans_unr_alert_notify(uint8_t cat_id)
+{
+    ble_svc_ans_unr_alert_stat[0] = cat_id;
+    ble_svc_ans_unr_alert_stat[1] = ble_svc_ans_unr_alert_cnt[cat_id];
+    return ble_gattc_notify(ble_svc_ans_conn_handle,
+                            ble_svc_ans_unr_alert_val_handle);
+}
+
+/**
+ * Writes the received value from a characteristic write to
+ * the given destination.
+ */
+static int
+ble_svc_ans_chr_write(struct os_mbuf *om, uint16_t min_len,
+                      uint16_t max_len, void *dst,
+                      uint16_t *len)
+{
+    uint16_t om_len;
+    int rc;
+
+    om_len = OS_MBUF_PKTLEN(om);
+    if (om_len < min_len || om_len > max_len) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+    }
+
+    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * Initialize the ANS with initial values for enabled categories
+ * for new and unread alert characteristics. Bitwise or the
+ * catagory bitmasks to enable multiple catagories.
+ *
+ * XXX: We should technically be able to change the new alert and
+ *      unread alert catagories when we have no active connections.
+ */
+void
+ble_svc_ans_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_ans_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_ans_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    ble_svc_ans_new_alert_cat = MYNEWT_VAL(BLE_SVC_ANS_NEW_ALERT_CAT);
+    ble_svc_ans_unr_alert_cat = MYNEWT_VAL(BLE_SVC_ANS_UNR_ALERT_CAT);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ans/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/ans/syscfg.yml
new file mode 100644
index 00000000..74de8d96
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ans/syscfg.yml
@@ -0,0 +1,30 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+
+syscfg.defs:
+    BLE_SVC_ANS_NEW_ALERT_CAT:
+        description: "Initial supported new alert category bitmask."
+        value: 0
+
+    BLE_SVC_ANS_UNR_ALERT_CAT:
+        description: "Initial supported unread alert category bitmask."
+        value: 0
+
+    BLE_SVC_ANS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the alert notification service.
+        value: 303
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bas/include/services/bas/ble_svc_bas.h b/src/libs/mynewt-nimble/nimble/host/services/bas/include/services/bas/ble_svc_bas.h
new file mode 100644
index 00000000..2706b5d3
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bas/include/services/bas/ble_svc_bas.h
@@ -0,0 +1,31 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_BAS_
+#define H_BLE_SVC_BAS_
+
+/* 16 Bit Battery Service UUID */
+#define BLE_SVC_BAS_UUID16                                   0x180F
+
+/* 16 Bit Battery Service Characteristic UUIDs */
+#define BLE_SVC_BAS_CHR_UUID16_BATTERY_LEVEL                 0x2A19
+
+int ble_svc_bas_battery_level_set(uint8_t level);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bas/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/bas/pkg.yml
new file mode 100644
index 00000000..afdc6942
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bas/pkg.yml
@@ -0,0 +1,34 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/bas
+pkg.description: Battery Service
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - bas
+    - nimble
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_bas_init: 'MYNEWT_VAL(BLE_SVC_BAS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bas/src/ble_svc_bas.c b/src/libs/mynewt-nimble/nimble/host/services/bas/src/ble_svc_bas.c
new file mode 100644
index 00000000..631519cf
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bas/src/ble_svc_bas.c
@@ -0,0 +1,127 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "host/ble_hs.h"
+#include "host/ble_gap.h"
+#include "services/bas/ble_svc_bas.h"
+
+/* Characteristic value handles */
+#if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0
+static uint16_t ble_svc_bas_battery_handle;
+#endif
+
+/* Battery level */
+uint8_t ble_svc_bas_battery_level;
+
+/* Access function */
+static int
+ble_svc_bas_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_bas_defs[] = {
+    {
+        /*** Battery Service. */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_BAS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+	    /*** Battery level characteristic */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_BAS_CHR_UUID16_BATTERY_LEVEL),
+            .access_cb = ble_svc_bas_access,
+#if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0
+	    .val_handle = &ble_svc_bas_battery_handle,
+#endif
+            .flags = BLE_GATT_CHR_F_READ |
+#if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0
+	             BLE_GATT_CHR_F_NOTIFY |
+#endif
+	             MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_READ_PERM),
+	    }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * BAS access function
+ */
+static int
+ble_svc_bas_access(uint16_t conn_handle, uint16_t attr_handle,
+                          struct ble_gatt_access_ctxt *ctxt,
+                          void *arg)
+{
+    uint16_t uuid16 = ble_uuid_u16(ctxt->chr->uuid);
+    int rc;
+
+    switch (uuid16) {
+    case BLE_SVC_BAS_CHR_UUID16_BATTERY_LEVEL:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ble_svc_bas_battery_level,
+                            sizeof ble_svc_bas_battery_level);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+
+/**
+ * Set the battery level, must be between 0 and 100.
+ * If greater than 100, it will be silently set to 100.
+ */
+int
+ble_svc_bas_battery_level_set(uint8_t level) {
+    if (level > 100)
+	level = 100;
+    if (ble_svc_bas_battery_level != level) {
+	ble_svc_bas_battery_level = level;
+#if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0
+        ble_gatts_chr_updated(ble_svc_bas_battery_handle);
+#endif
+    }
+    return 0;
+}
+
+/**
+ * Initialize the Battery Service.
+ */
+void
+ble_svc_bas_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_bas_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_bas_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bas/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/bas/syscfg.yml
new file mode 100644
index 00000000..279930f1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bas/syscfg.yml
@@ -0,0 +1,34 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+
+syscfg.defs:
+    BLE_SVC_BAS_BATTERY_LEVEL_READ_PERM:
+        description: >
+            Defines permissions for reading "Battery Level" characteristics. Can
+            be zero to allow read without extra permissions or combination of:
+                BLE_GATT_CHR_F_READ_ENC
+                BLE_GATT_CHR_F_READ_AUTHEN
+                BLE_GATT_CHR_F_READ_AUTHOR
+        value: 0
+    BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE:
+        description: >
+            Set to 1 to support notification or 0 to disable it.
+        value: 1
+    BLE_SVC_BAS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the battery level service.
+        value: 303
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bleuart/include/bleuart/bleuart.h b/src/libs/mynewt-nimble/nimble/host/services/bleuart/include/bleuart/bleuart.h
new file mode 100644
index 00000000..ab64b9c0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bleuart/include/bleuart/bleuart.h
@@ -0,0 +1,42 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef _BLEUART_H_
+#define _BLEUART_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void
+bleuart_init(void);
+int
+bleuart_svc_register(void);
+int
+bleuart_gatt_svr_init(void);
+void
+bleuart_set_conn_handle(uint16_t conn_handle);
+
+extern const ble_uuid128_t gatt_svr_svc_uart_uuid;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _BLEUART_H */
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bleuart/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/bleuart/pkg.yml
new file mode 100644
index 00000000..36fe2bc7
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bleuart/pkg.yml
@@ -0,0 +1,37 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/bleuart
+pkg.description: BLE uart service.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - uart
+
+pkg.deps:
+    - "@apache-mynewt-core/kernel/os"
+    - nimble/host
+
+pkg.req_apis:
+    - console
+
+pkg.init:
+    bleuart_init: 'MYNEWT_VAL(BLEUART_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bleuart/src/bleuart.c b/src/libs/mynewt-nimble/nimble/host/services/bleuart/src/bleuart.c
new file mode 100644
index 00000000..15856352
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bleuart/src/bleuart.c
@@ -0,0 +1,203 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "host/ble_uuid.h"
+#include "bleuart/bleuart.h"
+#include "os/endian.h"
+#include "console/console.h"
+
+/* ble uart attr read handle */
+uint16_t g_bleuart_attr_read_handle;
+
+/* ble uart attr write handle */
+uint16_t g_bleuart_attr_write_handle;
+
+/* Pointer to a console buffer */
+char *console_buf;
+
+uint16_t g_console_conn_handle;
+/**
+ * The vendor specific "bleuart" service consists of one write no-rsp characteristic
+ * and one notification only read charateristic
+ *     o "write no-rsp": a single-byte characteristic that can be written only
+ *       over a non-encrypted connection
+ *     o "read": a single-byte characteristic that can always be read only via
+ *       notifications
+ */
+
+/* {6E400001-B5A3-F393-E0A9-E50E24DCCA9E} */
+const ble_uuid128_t gatt_svr_svc_uart_uuid =
+    BLE_UUID128_INIT(0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0,
+                     0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e);
+
+/* {6E400002-B5A3-F393-E0A9-E50E24DCCA9E} */
+const ble_uuid128_t gatt_svr_chr_uart_write_uuid =
+    BLE_UUID128_INIT(0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0,
+                     0x93, 0xf3, 0xa3, 0xb5, 0x02, 0x00, 0x40, 0x6e);
+
+
+/* {6E400003-B5A3-F393-E0A9-E50E24DCCA9E} */
+const ble_uuid128_t gatt_svr_chr_uart_read_uuid =
+    BLE_UUID128_INIT(0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0,
+                     0x93, 0xf3, 0xa3, 0xb5, 0x03, 0x00, 0x40, 0x6e);
+
+static int
+gatt_svr_chr_access_uart_write(uint16_t conn_handle, uint16_t attr_handle,
+                              struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
+    {
+        /* Service: uart */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = &gatt_svr_svc_uart_uuid.u,
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = &gatt_svr_chr_uart_read_uuid.u,
+            .val_handle = &g_bleuart_attr_read_handle,
+            .access_cb = gatt_svr_chr_access_uart_write,
+            .flags = BLE_GATT_CHR_F_NOTIFY,
+        }, {
+            /* Characteristic: Write */
+            .uuid = &gatt_svr_chr_uart_write_uuid.u,
+            .access_cb = gatt_svr_chr_access_uart_write,
+            .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
+            .val_handle = &g_bleuart_attr_write_handle,
+        }, {
+            0, /* No more characteristics in this service */
+        } },
+    },
+
+    {
+        0, /* No more services */
+    },
+};
+
+static int
+gatt_svr_chr_access_uart_write(uint16_t conn_handle, uint16_t attr_handle,
+                               struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    struct os_mbuf *om = ctxt->om;
+    switch (ctxt->op) {
+        case BLE_GATT_ACCESS_OP_WRITE_CHR:
+              while(om) {
+                  console_write((char *)om->om_data, om->om_len);
+                  om = SLIST_NEXT(om, om_next);
+              }
+              console_write("\n", 1);
+              return 0;
+        default:
+            assert(0);
+            return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+/**
+ * bleuart GATT server initialization
+ *
+ * @param eventq
+ * @return 0 on success; non-zero on failure
+ */
+int
+bleuart_gatt_svr_init(void)
+{
+    int rc;
+
+    rc = ble_gatts_count_cfg(gatt_svr_svcs);
+    if (rc != 0) {
+        goto err;
+    }
+
+    rc = ble_gatts_add_svcs(gatt_svr_svcs);
+    if (rc != 0) {
+        return rc;
+    }
+
+err:
+    return rc;
+}
+
+/**
+ * Reads console and sends data over BLE
+ */
+static void
+bleuart_uart_read(void)
+{
+    int rc;
+    int off;
+    int full_line;
+    struct os_mbuf *om;
+
+    off = 0;
+    while (1) {
+        rc = console_read(console_buf + off,
+                          MYNEWT_VAL(BLEUART_MAX_INPUT) - off, &full_line);
+        if (rc <= 0 && !full_line) {
+            continue;
+        }
+        off += rc;
+        if (!full_line) {
+            continue;
+        }
+
+        om = ble_hs_mbuf_from_flat(console_buf, off);
+        if (!om) {
+            return;
+        }
+        ble_gattc_notify_custom(g_console_conn_handle,
+                                g_bleuart_attr_read_handle, om);
+        off = 0;
+        break;
+    }
+}
+
+/**
+ * Sets the global connection handle
+ *
+ * @param connection handle
+ */
+void
+bleuart_set_conn_handle(uint16_t conn_handle) {
+    g_console_conn_handle = conn_handle;
+}
+
+/**
+ * BLEuart console initialization
+ *
+ * @param Maximum input
+ */
+void
+bleuart_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = console_init(bleuart_uart_read);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    console_buf = malloc(MYNEWT_VAL(BLEUART_MAX_INPUT));
+    SYSINIT_PANIC_ASSERT(console_buf != NULL);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/bleuart/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/bleuart/syscfg.yml
new file mode 100644
index 00000000..b0ed73e5
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/bleuart/syscfg.yml
@@ -0,0 +1,28 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLEUART_MAX_INPUT:
+        description: >
+            The size of the largest line that can be received over the UART
+            service.
+        value: 120
+    BLEUART_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the BLE UART service.
+        value: 500
diff --git a/src/libs/mynewt-nimble/nimble/host/services/dis/include/services/dis/ble_svc_dis.h b/src/libs/mynewt-nimble/nimble/host/services/dis/include/services/dis/ble_svc_dis.h
new file mode 100644
index 00000000..d095e959
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/dis/include/services/dis/ble_svc_dis.h
@@ -0,0 +1,113 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_DIS_
+#define H_BLE_SVC_DIS_
+
+/**
+ * Example:
+ *
+ *    char firmware_revision[20] = '?.?.?';
+ *    struct image_version iv;
+ *    if (!imgr_my_version(&iv)) {
+ *	snprintf(firmware_revision, sizeof(firmware_revision),
+ *		 "%u.%u.%u", iv.iv_major, iv.iv_minor, iv.iv_revision);
+ *    }
+ *    ble_svc_dis_manufacturer_name_set("MyNewt");
+ *    ble_svc_dis_firmware_revision_set(firmware_revision);
+ *
+ */
+
+#define BLE_SVC_DIS_UUID16					0x180A
+#define BLE_SVC_DIS_CHR_UUID16_SYSTEM_ID			0x2A23
+#define BLE_SVC_DIS_CHR_UUID16_MODEL_NUMBER			0x2A24
+#define BLE_SVC_DIS_CHR_UUID16_SERIAL_NUMBER			0x2A25
+#define BLE_SVC_DIS_CHR_UUID16_FIRMWARE_REVISION 		0x2A26
+#define BLE_SVC_DIS_CHR_UUID16_HARDWARE_REVISION 		0x2A27
+#define BLE_SVC_DIS_CHR_UUID16_SOFTWARE_REVISION 		0x2A28
+#define BLE_SVC_DIS_CHR_UUID16_MANUFACTURER_NAME		0x2A29
+
+/**
+ * Structure holding data for the main characteristics
+ */
+struct ble_svc_dis_data {
+    /**
+     * Model number.
+     * Represent the model number that is assigned by the device vendor.
+     */
+    const char *model_number;
+    /**
+     * Serial number.
+     * Represent the serial number for a particular instance of the device.
+     */
+    const char *serial_number;
+    /**
+     * Firmware revision.
+     * Represent the firmware revision for the firmware within the device.
+     */
+    const char *firmware_revision;
+    /**
+     * Hardware revision.
+     * Represent the hardware revision for the hardware within the device.
+     */
+    const char *hardware_revision;
+    /**
+     * Software revision.
+     * Represent the software revision for the software within the device.
+     */
+    const char *software_revision;
+    /**
+     * Manufacturer name.
+     * Represent the name of the manufacturer of the device.
+     */
+    const char *manufacturer_name;
+    /**
+     * System ID.
+     * Represent the System Id of the device.
+     */
+    const char *system_id;
+};
+
+/**
+ * Variable holding data for the main characteristics.
+ */
+extern struct ble_svc_dis_data ble_svc_dis_data;
+
+/**
+ * Service initialisation.
+ * Automatically called during package initialisation.
+ */
+void ble_svc_dis_init(void);
+
+const char *ble_svc_dis_model_number(void);
+int ble_svc_dis_model_number_set(const char *value);
+const char *ble_svc_dis_serial_number(void);
+int ble_svc_dis_serial_number_set(const char *value);
+const char *ble_svc_dis_firmware_revision(void);
+int ble_svc_dis_firmware_revision_set(const char *value);
+const char *ble_svc_dis_hardware_revision(void);
+int ble_svc_dis_hardware_revision_set(const char *value);
+const char *ble_svc_dis_software_revision(void);
+int ble_svc_dis_software_revision_set(const char *value);
+const char *ble_svc_dis_manufacturer_name(void);
+int ble_svc_dis_manufacturer_name_set(const char *value);
+const char *ble_svc_dis_system_id(void);
+int ble_svc_dis_system_id_set(const char *value);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/services/dis/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/dis/pkg.yml
new file mode 100644
index 00000000..b914ca90
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/dis/pkg.yml
@@ -0,0 +1,34 @@
+
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/dis
+pkg.description: Device Information Service Implementation.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - dis
+    - nimble
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_dis_init: 'MYNEWT_VAL(BLE_SVC_DIS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/dis/src/ble_svc_dis.c b/src/libs/mynewt-nimble/nimble/host/services/dis/src/ble_svc_dis.c
new file mode 100644
index 00000000..0079a04c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/dis/src/ble_svc_dis.c
@@ -0,0 +1,331 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/dis/ble_svc_dis.h"
+
+/* Device information */
+struct ble_svc_dis_data ble_svc_dis_data = {
+    .model_number      = MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_DEFAULT),
+    .serial_number     = MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_DEFAULT),
+    .firmware_revision = MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_DEFAULT),
+    .hardware_revision = MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_DEFAULT),
+    .software_revision = MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_DEFAULT),
+    .manufacturer_name = MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_DEFAULT),
+    .system_id         = MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_DEFAULT),
+};
+
+/* Access function */
+#if (MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_READ_PERM)      >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_READ_PERM)     >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_READ_PERM) >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_READ_PERM) >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_READ_PERM) >= 0) || \
+    (MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_READ_PERM) >= 0)
+static int
+ble_svc_dis_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+#endif
+
+static const struct ble_gatt_svc_def ble_svc_dis_defs[] = {
+    { /*** Service: Device Information Service (DIS). */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+#if (MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_READ_PERM) >= 0)
+	    /*** Characteristic: Model Number String */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_MODEL_NUMBER),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+	             MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_READ_PERM),
+        }, {
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_READ_PERM) >= 0)
+	    /*** Characteristic: Serial Number String */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_SERIAL_NUMBER),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+	             MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_READ_PERM),
+        }, {
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_READ_PERM) >= 0)
+	    /*** Characteristic: Hardware Revision String */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_HARDWARE_REVISION),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+	             MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_READ_PERM),
+        }, {
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0)
+	    /*** Characteristic: Firmware Revision String */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_FIRMWARE_REVISION),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+	             MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM),
+        }, {
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_READ_PERM) >= 0)
+	    /*** Characteristic: Software Revision String */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_SOFTWARE_REVISION),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+	             MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_READ_PERM),
+        }, {
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_READ_PERM) >= 0)
+	    /*** Characteristic: Manufacturer Name */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_MANUFACTURER_NAME),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+	             MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_READ_PERM),
+        }, {
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_READ_PERM) >= 0)
+      /*** Characteristic: System Id */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_DIS_CHR_UUID16_SYSTEM_ID),
+            .access_cb = ble_svc_dis_access,
+            .flags = BLE_GATT_CHR_F_READ |
+               MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_READ_PERM),
+            }, {
+#endif
+
+            0, /* No more characteristics in this service */
+        }, }
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * Simple read access callback for the device information service
+ * characteristic.
+ */
+#if (MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_READ_PERM)      >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_READ_PERM)     >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_READ_PERM) >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_READ_PERM) >= 0) ||	\
+    (MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_READ_PERM) >= 0) || \
+    (MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_READ_PERM) >= 0)
+static int
+ble_svc_dis_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    uint16_t uuid    = ble_uuid_u16(ctxt->chr->uuid);
+    const char *info = NULL;
+
+    switch(uuid) {
+#if (MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_MODEL_NUMBER:
+        info = ble_svc_dis_data.model_number;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_MODEL_NUMBER_NAME_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_DEFAULT);
+        }
+#endif
+        break;
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_SERIAL_NUMBER:
+        info = ble_svc_dis_data.serial_number;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_SERIAL_NUMBER_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_DEFAULT);
+        }
+#endif
+        break;
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_FIRMWARE_REVISION:
+        info = ble_svc_dis_data.firmware_revision;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_FIRMWARE_REVISION_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_DEFAULT);
+        }
+#endif
+        break;
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_HARDWARE_REVISION:
+        info = ble_svc_dis_data.hardware_revision;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_HARDWARE_REVISION_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_DEFAULT);
+        }
+#endif
+        break;
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_SOFTWARE_REVISION:
+        info = ble_svc_dis_data.software_revision;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_SOFTWARE_REVISION_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_DEFAULT);
+        }
+#endif
+        break;
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_MANUFACTURER_NAME:
+        info = ble_svc_dis_data.manufacturer_name;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_MANUFACTURER_NAME_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_DEFAULT);
+        }
+#endif
+        break;
+#endif
+#if (MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_READ_PERM) >= 0)
+    case BLE_SVC_DIS_CHR_UUID16_SYSTEM_ID:
+        info = ble_svc_dis_data.system_id;
+#ifdef MYNEWT_VAL_BLE_SVC_DIS_SYSTEM_ID_DEFAULT
+        if (info == NULL) {
+            info = MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_DEFAULT);
+        }
+#endif
+        break;
+#endif
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    if (info != NULL) {
+       int rc = os_mbuf_append(ctxt->om, info, strlen(info));
+       return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
+    return 0;
+}
+#endif
+
+const char *
+ble_svc_dis_model_number(void)
+{
+    return ble_svc_dis_data.model_number;
+}
+
+int
+ble_svc_dis_model_number_set(const char *value)
+{
+    ble_svc_dis_data.model_number = value;
+    return 0;
+}
+
+const char *
+ble_svc_dis_serial_number(void)
+{
+    return ble_svc_dis_data.serial_number;
+}
+
+int
+ble_svc_dis_serial_number_set(const char *value)
+{
+    ble_svc_dis_data.serial_number = value;
+    return 0;
+}
+
+const char *
+ble_svc_dis_firmware_revision(void)
+{
+    return ble_svc_dis_data.firmware_revision;
+}
+
+int
+ble_svc_dis_firmware_revision_set(const char *value)
+{
+    ble_svc_dis_data.firmware_revision = value;
+    return 0;
+}
+
+const char *
+ble_svc_dis_hardware_revision(void)
+{
+    return ble_svc_dis_data.hardware_revision;
+}
+
+int
+ble_svc_dis_hardware_revision_set(const char *value)
+{
+    ble_svc_dis_data.hardware_revision = value;
+    return 0;
+}
+
+const char *
+ble_svc_dis_software_revision(void)
+{
+    return ble_svc_dis_data.software_revision;
+}
+
+int
+ble_svc_dis_software_revision_set(const char *value)
+{
+    ble_svc_dis_data.software_revision = value;
+    return 0;
+}
+
+const char *
+ble_svc_dis_manufacturer_name(void)
+{
+    return ble_svc_dis_data.manufacturer_name;
+}
+
+int
+ble_svc_dis_manufacturer_name_set(const char *value)
+{
+    ble_svc_dis_data.manufacturer_name = value;
+    return 0;
+}
+
+const char *
+ble_svc_dis_system_id(void)
+{
+    return ble_svc_dis_data.system_id;
+}
+
+int
+ble_svc_dis_system_id_set(const char *value)
+{
+    ble_svc_dis_data.system_id = value;
+    return 0;
+}
+
+/**
+ * Initialize the DIS package.
+ */
+void
+ble_svc_dis_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_dis_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_dis_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/dis/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/dis/syscfg.yml
new file mode 100644
index 00000000..b306d3bb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/dis/syscfg.yml
@@ -0,0 +1,109 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_SVC_DIS_DEFAULT_READ_PERM:
+        description: >
+            Defines default permissions for reading characteristics. Can be
+            zero to allow read without extra permissions or combination of:
+                BLE_GATT_CHR_F_READ_ENC
+                BLE_GATT_CHR_F_READ_AUTHEN
+                BLE_GATT_CHR_F_READ_AUTHOR
+            Set to '-1' to remove this characteristic.
+        value: -1
+    BLE_SVC_DIS_MODEL_NUMBER_READ_PERM:
+        description: >
+            Defines permissions for reading "Model Number" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: 0
+    BLE_SVC_DIS_MODEL_NUMBER_DEFAULT:
+        description: >
+            Defines a default value for "Model number" if not set with
+            'ble_svc_dis_model_number_set'.
+        value: '"Apache Mynewt NimBLE"'
+    BLE_SVC_DIS_SERIAL_NUMBER_READ_PERM:
+        description: >
+            Defines permissions for reading "Serial Number" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: MYNEWT_VAL(BLE_SVC_DIS_DEFAULT_READ_PERM)
+    BLE_SVC_DIS_SERIAL_NUMBER_DEFAULT:
+        description: >
+            Defines a default value for "Serial number" if not set with
+            'ble_svc_dis_serial_number_set'.
+        value: NULL
+    BLE_SVC_DIS_HARDWARE_REVISION_READ_PERM:
+        description: >
+            Defines permissions for reading "Hardware Revision" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: MYNEWT_VAL(BLE_SVC_DIS_DEFAULT_READ_PERM)
+    BLE_SVC_DIS_HARDWARE_REVISION_DEFAULT:
+        description: >
+            Defines a default value for "Hardware revision" if not set with
+            'ble_svc_dis_hardware_revision_set'.
+        value: NULL
+    BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM:
+        description: >
+            Defines permissions for reading "Firmware Revision" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: MYNEWT_VAL(BLE_SVC_DIS_DEFAULT_READ_PERM)
+    BLE_SVC_DIS_FIRMWARE_REVISION_DEFAULT:
+        description: >
+            Defines a default value for "Software revision" if not set with
+            'ble_svc_dis_firmware_revision_set'.
+        value: NULL
+    BLE_SVC_DIS_SOFTWARE_REVISION_READ_PERM:
+        description: >
+            Defines permissions for reading "Software Revision" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: MYNEWT_VAL(BLE_SVC_DIS_DEFAULT_READ_PERM)
+    BLE_SVC_DIS_SOFTWARE_REVISION_DEFAULT:
+        description: >
+            Defines a default value for "Software revision" if not set with
+            'ble_svc_dis_software_revision_set'.
+        value: NULL
+    BLE_SVC_DIS_MANUFACTURER_NAME_READ_PERM:
+        description: >
+            Defines permissions for reading "Manufacturer name" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: MYNEWT_VAL(BLE_SVC_DIS_DEFAULT_READ_PERM)
+    BLE_SVC_DIS_MANUFACTURER_NAME_DEFAULT:
+        description: >
+            Defines a default value for "Manufacturer name" if not set with
+            'ble_svc_dis_manufacturer_name_set'.
+        value: NULL
+    BLE_SVC_DIS_SYSTEM_ID_READ_PERM:
+        description: >
+            Defines permissions for reading "System ID" characteristics.
+            Can be set to BLE_SVC_DIS_DEFAULT_READ_PERM or use any of the
+            possible values defined for that setting.
+        value: MYNEWT_VAL(BLE_SVC_DIS_DEFAULT_READ_PERM)
+    BLE_SVC_DIS_SYSTEM_ID_DEFAULT:
+        description: >
+            Defines a default value for "System ID" if not set with
+            'ble_svc_dis_manufacturer_name_set'.
+        value: NULL
+    BLE_SVC_DIS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the device information BLE service.
+        value: 303
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gap/include/services/gap/ble_svc_gap.h b/src/libs/mynewt-nimble/nimble/host/services/gap/include/services/gap/ble_svc_gap.h
new file mode 100644
index 00000000..d7b60df4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gap/include/services/gap/ble_svc_gap.h
@@ -0,0 +1,54 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_GAP_
+#define H_BLE_SVC_GAP_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_SVC_GAP_UUID16                                  0x1800
+#define BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME                  0x2a00
+#define BLE_SVC_GAP_CHR_UUID16_APPEARANCE                   0x2a01
+#define BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS      0x2a04
+#define BLE_SVC_GAP_CHR_UUID16_CENTRAL_ADDRESS_RESOLUTION   0x2aa6
+
+#define BLE_SVC_GAP_APPEARANCE_GEN_UNKNOWN                         0
+#define BLE_SVC_GAP_APPEARANCE_GEN_COMPUTER                        128
+#define BLE_SVC_GAP_APPEARANCE_CYC_SPEED_AND_CADENCE_SENSOR        1157
+
+typedef void (ble_svc_gap_chr_changed_fn) (uint16_t uuid);
+
+void ble_svc_gap_set_chr_changed_cb(ble_svc_gap_chr_changed_fn *cb);
+
+const char *ble_svc_gap_device_name(void);
+int ble_svc_gap_device_name_set(const char *name);
+uint16_t ble_svc_gap_device_appearance(void);
+int ble_svc_gap_device_appearance_set(uint16_t appearance);
+
+void ble_svc_gap_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gap/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/gap/pkg.yml
new file mode 100644
index 00000000..a2ef756e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gap/pkg.yml
@@ -0,0 +1,34 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/gap
+pkg.description: Implements the GAP Service.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - gap
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_gap_init: 'MYNEWT_VAL(BLE_SVC_GAP_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gap/src/ble_svc_gap.c b/src/libs/mynewt-nimble/nimble/host/services/gap/src/ble_svc_gap.c
new file mode 100644
index 00000000..e79b2b87
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gap/src/ble_svc_gap.c
@@ -0,0 +1,298 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/gap/ble_svc_gap.h"
+#include "os/endian.h"
+
+#define PPCP_ENABLED \
+    MYNEWT_VAL(BLE_ROLE_PERIPHERAL) && \
+    (MYNEWT_VAL(BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL) || \
+     MYNEWT_VAL(BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL) || \
+     MYNEWT_VAL(BLE_SVC_GAP_PPCP_SLAVE_LATENCY) || \
+     MYNEWT_VAL(BLE_SVC_GAP_PPCP_SUPERVISION_TMO))
+
+#define BLE_SVC_GAP_NAME_MAX_LEN \
+    MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME_MAX_LENGTH)
+
+static ble_svc_gap_chr_changed_fn *ble_svc_gap_chr_changed_cb_fn;
+
+static char ble_svc_gap_name[BLE_SVC_GAP_NAME_MAX_LEN + 1] =
+        MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME);
+static uint16_t ble_svc_gap_appearance = MYNEWT_VAL(BLE_SVC_GAP_APPEARANCE);
+
+static int
+ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_gap_defs[] = {
+    {
+        /*** Service: GAP. */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_GAP_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /*** Characteristic: Device Name. */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ |
+#if MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME_WRITE_PERM) >= 0
+                     BLE_GATT_CHR_F_WRITE |
+                     MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME_WRITE_PERM) |
+#endif
+                     0,
+        }, {
+            /*** Characteristic: Appearance. */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_GAP_CHR_UUID16_APPEARANCE),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ |
+#if MYNEWT_VAL(BLE_SVC_GAP_APPEARANCE_WRITE_PERM) >= 0
+                     BLE_GATT_CHR_F_WRITE |
+                     MYNEWT_VAL(BLE_SVC_GAP_APPEARANCE_WRITE_PERM) |
+#endif
+                     0,
+        }, {
+#if PPCP_ENABLED
+            /*** Characteristic: Peripheral Preferred Connection Parameters. */
+            .uuid =
+                BLE_UUID16_DECLARE(BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+#endif
+#if MYNEWT_VAL(BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION) >= 0
+            /*** Characteristic: Central Address Resolution. */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_GAP_CHR_UUID16_CENTRAL_ADDRESS_RESOLUTION),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+#endif
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+static int
+ble_svc_gap_device_name_read_access(struct ble_gatt_access_ctxt *ctxt)
+{
+    int rc;
+
+    rc = os_mbuf_append(ctxt->om, ble_svc_gap_name, strlen(ble_svc_gap_name));
+
+    return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+}
+
+static int
+ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt)
+{
+#if MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME_WRITE_PERM) < 0
+    assert(0);
+    return 0;
+#else
+    uint16_t om_len;
+    int rc;
+
+    om_len = OS_MBUF_PKTLEN(ctxt->om);
+    if (om_len > BLE_SVC_GAP_NAME_MAX_LEN) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+    }
+
+    rc = ble_hs_mbuf_to_flat(ctxt->om, ble_svc_gap_name, om_len, NULL);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    ble_svc_gap_name[om_len] = '\0';
+
+    if (ble_svc_gap_chr_changed_cb_fn) {
+        ble_svc_gap_chr_changed_cb_fn(BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME);
+    }
+
+    return rc;
+#endif
+}
+
+static int
+ble_svc_gap_appearance_read_access(struct ble_gatt_access_ctxt *ctxt)
+{
+    uint16_t appearance = htole16(ble_svc_gap_appearance);
+    int rc;
+
+    rc = os_mbuf_append(ctxt->om, &appearance, sizeof(appearance));
+
+    return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+}
+
+static int
+ble_svc_gap_appearance_write_access(struct ble_gatt_access_ctxt *ctxt)
+{
+#if MYNEWT_VAL(BLE_SVC_GAP_APPEARANCE_WRITE_PERM) < 0
+    assert(0);
+    return 0;
+#else
+    uint16_t om_len;
+    int rc;
+
+    om_len = OS_MBUF_PKTLEN(ctxt->om);
+    if (om_len != sizeof(ble_svc_gap_appearance)) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+    }
+
+    rc = ble_hs_mbuf_to_flat(ctxt->om, &ble_svc_gap_appearance, om_len, NULL);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    ble_svc_gap_appearance = le16toh(ble_svc_gap_appearance);
+
+    if (ble_svc_gap_chr_changed_cb_fn) {
+        ble_svc_gap_chr_changed_cb_fn(BLE_SVC_GAP_CHR_UUID16_APPEARANCE);
+    }
+
+    return rc;
+#endif
+}
+
+static int
+ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    uint16_t uuid16;
+#if MYNEWT_VAL(BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION) >= 0
+    uint8_t central_ar = MYNEWT_VAL(BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION);
+#endif
+#if PPCP_ENABLED
+    uint16_t ppcp[4] = {
+        htole16(MYNEWT_VAL(BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL)),
+        htole16(MYNEWT_VAL(BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL)),
+        htole16(MYNEWT_VAL(BLE_SVC_GAP_PPCP_SLAVE_LATENCY)),
+        htole16(MYNEWT_VAL(BLE_SVC_GAP_PPCP_SUPERVISION_TMO))
+    };
+#endif
+    int rc;
+
+    uuid16 = ble_uuid_u16(ctxt->chr->uuid);
+    assert(uuid16 != 0);
+
+    switch (uuid16) {
+    case BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME:
+        if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+            rc = ble_svc_gap_device_name_read_access(ctxt);
+        } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = ble_svc_gap_device_name_write_access(ctxt);
+        } else {
+            assert(0);
+            rc = BLE_ATT_ERR_UNLIKELY;
+        }
+        return rc;
+
+    case BLE_SVC_GAP_CHR_UUID16_APPEARANCE:
+        if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+            rc = ble_svc_gap_appearance_read_access(ctxt);
+        } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = ble_svc_gap_appearance_write_access(ctxt);
+        } else {
+            assert(0);
+            rc = BLE_ATT_ERR_UNLIKELY;
+        }
+        return rc;
+
+#if PPCP_ENABLED
+    case BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ppcp, sizeof(ppcp));
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+#endif
+
+#if MYNEWT_VAL(BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION) >= 0
+    case BLE_SVC_GAP_CHR_UUID16_CENTRAL_ADDRESS_RESOLUTION:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &central_ar, sizeof(central_ar));
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+#endif
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+const char *
+ble_svc_gap_device_name(void)
+{
+    return ble_svc_gap_name;
+}
+
+int
+ble_svc_gap_device_name_set(const char *name)
+{
+    int len;
+
+    len = strlen(name);
+    if (len > BLE_SVC_GAP_NAME_MAX_LEN) {
+        return BLE_HS_EINVAL;
+    }
+
+    memcpy(ble_svc_gap_name, name, len);
+    ble_svc_gap_name[len] = '\0';
+
+    return 0;
+}
+
+uint16_t
+ble_svc_gap_device_appearance(void)
+{
+  return ble_svc_gap_appearance;
+}
+
+int
+ble_svc_gap_device_appearance_set(uint16_t appearance)
+{
+    ble_svc_gap_appearance = appearance;
+
+    return 0;
+}
+
+void
+ble_svc_gap_set_chr_changed_cb(ble_svc_gap_chr_changed_fn *cb)
+{
+    ble_svc_gap_chr_changed_cb_fn = cb;
+}
+
+void
+ble_svc_gap_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_gap_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_gap_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gap/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/gap/syscfg.yml
new file mode 100644
index 00000000..ad6aa7ef
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gap/syscfg.yml
@@ -0,0 +1,83 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_SVC_GAP_DEVICE_NAME:
+        description: >
+            Default value for "Device Name" characteristics, unless overwritten
+            by application.
+        value: '"nimble"'
+    BLE_SVC_GAP_DEVICE_NAME_WRITE_PERM:
+        description: >
+            Defines permissions for writing "Device Name" characteristics. Can
+            be zero to allow write without extra permissions or combination of:
+                BLE_GATT_CHR_F_WRITE_ENC
+                BLE_GATT_CHR_F_WRITE_AUTHEN
+                BLE_GATT_CHR_F_WRITE_AUTHOR
+            Set to '-1' to make characteristic read only.
+        value: -1
+    BLE_SVC_GAP_DEVICE_NAME_MAX_LENGTH:
+        description: Maximum length for "Device Name" characteristics
+        value: 31
+    BLE_SVC_GAP_APPEARANCE:
+        description: 'Device appearance'
+        value: 0
+    BLE_SVC_GAP_APPEARANCE_WRITE_PERM:
+        description: >
+            Defines permissions for writing "Appearance" characteristics. Can
+            be zero to allow write without extra permissions or combination of:
+                BLE_GATT_CHR_F_WRITE_ENC
+                BLE_GATT_CHR_F_WRITE_AUTHEN
+                BLE_GATT_CHR_F_WRITE_AUTHOR
+            Set to '-1' to make characteristic read only.
+        value: -1
+
+    # Setting all values for PPCP to '0' will disable characteristic!
+    BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL:
+        description: >
+            Value of "minimum connection interval" of PPCP characteristic as
+            defined by Core specification 5.0, Vol 3, Part C, section 12.3.
+        value: 0
+    BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL:
+        description: >
+            Value of "maximum connection interval" of PPCP characteristic as
+            defined by Core specification 5.0, Vol 3, Part C, section 12.3.
+        value: 0
+    BLE_SVC_GAP_PPCP_SLAVE_LATENCY:
+        description: >
+            Value of "slave latency" of PPCP characteristic as  defined by Core
+            specification 5.0, Vol 3, Part C, section 12.3.
+        value: 0
+    BLE_SVC_GAP_PPCP_SUPERVISION_TMO:
+        description: >
+            Value of "connection supervision timeout multiplier" of PPCP
+            characteristic as defined by Core specification 5.0, Vol 3, Part C,
+            section 12.3.
+        value: 0
+
+    BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION:
+        description: >
+            Value of "Central Address Resolution" characteristics, as defined
+            by Core specification 5.0, Vol 3, Part C, section 12.
+            Set to '-1' to disable.
+        value: -1
+
+    BLE_SVC_GAP_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the GAP BLE service.
+        value: 301
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h b/src/libs/mynewt-nimble/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h
new file mode 100644
index 00000000..06c44784
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h
@@ -0,0 +1,40 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_GATT_
+#define H_BLE_SVC_GATT_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_GATT_CHR_SERVICE_CHANGED_UUID16     0x2a05
+
+void ble_svc_gatt_changed(uint16_t start_handle, uint16_t end_handle);
+void ble_svc_gatt_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gatt/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/gatt/pkg.yml
new file mode 100644
index 00000000..e3704bc1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gatt/pkg.yml
@@ -0,0 +1,34 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/gatt
+pkg.description: Implements the GATT service.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - gatt
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_gatt_init: 'MYNEWT_VAL(BLE_SVC_GATT_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gatt/src/ble_svc_gatt.c b/src/libs/mynewt-nimble/nimble/host/services/gatt/src/ble_svc_gatt.c
new file mode 100644
index 00000000..78b4a068
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gatt/src/ble_svc_gatt.c
@@ -0,0 +1,108 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/gatt/ble_svc_gatt.h"
+
+static uint16_t ble_svc_gatt_changed_val_handle;
+static uint16_t ble_svc_gatt_start_handle;
+static uint16_t ble_svc_gatt_end_handle;
+
+static int
+ble_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle,
+                    struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_gatt_defs[] = {
+    {
+        /*** Service: GATT */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_GATT_SVC_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_SERVICE_CHANGED_UUID16),
+            .access_cb = ble_svc_gatt_access,
+            .val_handle = &ble_svc_gatt_changed_val_handle,
+            .flags = BLE_GATT_CHR_F_INDICATE,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+static int
+ble_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle,
+                    struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    uint8_t *u8p;
+
+    /* The only operation allowed for this characteristic is indicate.  This
+     * access callback gets called by the stack when it needs to read the
+     * characteristic value to populate the outgoing indication command.
+     * Therefore, this callback should only get called during an attempt to
+     * read the characteristic.
+     */
+    assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    assert(ctxt->chr == &ble_svc_gatt_defs[0].characteristics[0]);
+
+    u8p = os_mbuf_extend(ctxt->om, 4);
+    if (u8p == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    put_le16(u8p + 0, ble_svc_gatt_start_handle);
+    put_le16(u8p + 2, ble_svc_gatt_end_handle);
+
+    return 0;
+}
+
+/**
+ * Indicates a change in attribute assignment to all subscribed peers.
+ * Unconnected bonded peers receive an indication when they next connect.
+ *
+ * @param start_handle          The start of the affected handle range.
+ * @param end_handle            The end of the affected handle range.
+ */
+void
+ble_svc_gatt_changed(uint16_t start_handle, uint16_t end_handle)
+{
+    ble_svc_gatt_start_handle = start_handle;
+    ble_svc_gatt_end_handle = end_handle;
+    ble_gatts_chr_updated(ble_svc_gatt_changed_val_handle);
+}
+
+void
+ble_svc_gatt_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_gatt_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_gatt_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/gatt/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/gatt/syscfg.yml
new file mode 100644
index 00000000..6ba1b333
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/gatt/syscfg.yml
@@ -0,0 +1,24 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_SVC_GATT_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the GATT BLE service
+        value: 302
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ias/include/services/ias/ble_svc_ias.h b/src/libs/mynewt-nimble/nimble/host/services/ias/include/services/ias/ble_svc_ias.h
new file mode 100644
index 00000000..26c18d01
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ias/include/services/ias/ble_svc_ias.h
@@ -0,0 +1,38 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_IAS_TPS_
+#define H_BLE_IAS_TPS_
+
+#define BLE_SVC_IAS_UUID16                                      0x1802
+#define BLE_SVC_IAS_CHR_UUID16_ALERT_LEVEL                      0x2a06
+
+/* Alert level definitions */
+#define BLE_SVC_IAS_ALERT_LEVEL_NO_ALERT                        0
+#define BLE_SVC_IAS_ALERT_LEVEL_MILD_ALERT                      1
+#define BLE_SVC_IAS_ALERT_LEVEL_HIGH_ALERT                      2
+
+typedef int ble_svc_ias_event_fn(uint8_t alert_level);
+
+void ble_svc_ias_set_cb(ble_svc_ias_event_fn *cb);
+void ble_svc_ias_init(void);
+
+#endif
+
+
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ias/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/ias/pkg.yml
new file mode 100644
index 00000000..3b0ca074
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ias/pkg.yml
@@ -0,0 +1,34 @@
+
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/ias
+pkg.description: Immediate Alert Service Implementation.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - ias
+    - nimble
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_ias_init: 'MYNEWT_VAL(BLE_SVC_IAS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ias/src/ble_svc_ias.c b/src/libs/mynewt-nimble/nimble/host/services/ias/src/ble_svc_ias.c
new file mode 100644
index 00000000..9f5da148
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ias/src/ble_svc_ias.c
@@ -0,0 +1,149 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/ias/ble_svc_ias.h"
+
+/* Callback function */
+static ble_svc_ias_event_fn *ble_svc_ias_cb_fn;
+
+/* Alert level */
+static uint8_t ble_svc_ias_alert_level;
+
+/* Write characteristic function */
+static int
+ble_svc_ias_chr_write(struct os_mbuf *om, uint16_t min_len,
+                      uint16_t max_len, void *dst,
+                      uint16_t *len);
+
+/* Access function */
+static int
+ble_svc_ias_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_ias_defs[] = {
+    {
+        /*** Service: Immediate Alert Service (IAS). */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_IAS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /*** Characteristic: Alert Level. */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_IAS_CHR_UUID16_ALERT_LEVEL),
+            .access_cb = ble_svc_ias_access,
+            .flags = BLE_GATT_CHR_F_WRITE_NO_RSP,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * Writes the received value from a characteristic write to
+ * the given destination.
+ */
+static int
+ble_svc_ias_chr_write(struct os_mbuf *om, uint16_t min_len,
+                      uint16_t max_len, void *dst,
+                      uint16_t *len)
+{
+    uint16_t om_len;
+    int rc;
+
+    om_len = OS_MBUF_PKTLEN(om);
+    if (om_len < min_len || om_len > max_len) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+    }
+
+    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * Simple write access callback for the alert level
+ * characteristic.
+ */
+static int
+ble_svc_ias_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    int rc;
+
+    assert(ctxt->chr == &ble_svc_ias_defs[0].characteristics[0]);
+
+    switch (ctxt->op) {
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+        rc = ble_svc_ias_chr_write(ctxt->om,
+                                   sizeof ble_svc_ias_alert_level,
+                                   sizeof ble_svc_ias_alert_level,
+                                   &ble_svc_ias_alert_level, NULL);
+	/* Call the IAS event function */
+	if (ble_svc_ias_cb_fn) {
+		ble_svc_ias_cb_fn(ble_svc_ias_alert_level);
+	}
+        return rc;
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * Designates the specified function as the IAS callback.  This callback is
+ * necessary for this service to function properly.
+ *
+ * @param cb                        The callback function to call when
+ *                                      the client signals an alert.
+ */
+void
+ble_svc_ias_set_cb(ble_svc_ias_event_fn *cb)
+{
+    ble_svc_ias_cb_fn = cb;
+}
+
+/**
+ * Initialize the IAS package.
+ */
+void
+ble_svc_ias_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_ias_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_ias_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ias/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/ias/syscfg.yml
new file mode 100644
index 00000000..2cbed3ab
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ias/syscfg.yml
@@ -0,0 +1,23 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_SVC_IAS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the immediate alert BLE service.
+        value: 303
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ipss/include/services/ipss/ble_svc_ipss.h b/src/libs/mynewt-nimble/nimble/host/services/ipss/include/services/ipss/ble_svc_ipss.h
new file mode 100644
index 00000000..d894732c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ipss/include/services/ipss/ble_svc_ipss.h
@@ -0,0 +1,38 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_IPSS_
+#define H_BLE_SVC_IPSS_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_SVC_IPSS_UUID16         0x1820
+
+/**
+ * @brief   Initialize the IPSS service
+ */
+void ble_svc_ipss_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ipss/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/ipss/pkg.yml
new file mode 100644
index 00000000..55be157d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ipss/pkg.yml
@@ -0,0 +1,35 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/ipss
+pkg.description: Implements the IPSS service for IPSP suppoort.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - ipsp
+    - ipss
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_ipss_init: 'MYNEWT_VAL(BLE_SVC_IPSS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ipss/src/ble_svc_ipss.c b/src/libs/mynewt-nimble/nimble/host/services/ipss/src/ble_svc_ipss.c
new file mode 100644
index 00000000..f42ca1e9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ipss/src/ble_svc_ipss.c
@@ -0,0 +1,51 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/ipss/ble_svc_ipss.h"
+
+static const struct ble_gatt_svc_def ble_svc_ipss_defs[] = {
+    {
+        /*** Service: GATT */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_IPSS_UUID16),
+        .characteristics = NULL,
+    },
+    {
+        0, /* No more services. */
+    },
+};
+
+void
+ble_svc_ipss_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_ipss_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_ipss_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/ipss/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/ipss/syscfg.yml
new file mode 100644
index 00000000..dd89608e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/ipss/syscfg.yml
@@ -0,0 +1,24 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_SVC_IPSS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the IPSS BLE service
+        value: 303
diff --git a/src/libs/mynewt-nimble/nimble/host/services/lls/include/services/lls/ble_svc_lls.h b/src/libs/mynewt-nimble/nimble/host/services/lls/include/services/lls/ble_svc_lls.h
new file mode 100644
index 00000000..28cf607d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/lls/include/services/lls/ble_svc_lls.h
@@ -0,0 +1,51 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_LLS_
+#define H_BLE_SVC_LLS_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_LLS_UUID16                                  0x1803
+#define BLE_SVC_LLS_CHR_UUID16_ALERT_LEVEL                  0x2a06
+
+/* Alert level definitions */
+#define BLE_SVC_LLS_ALERT_LEVEL_NO_ALERT                    0
+#define BLE_SVC_LLS_ALERT_LEVEL_MILD_ALERT                  1
+#define BLE_SVC_LLS_ALERT_LEVEL_HIGH_ALERT                  2
+
+typedef int ble_svc_lls_event_fn(uint8_t alert_level);
+
+uint8_t ble_svc_lls_alert_level_get(void);
+int ble_svc_lls_alert_level_set(uint8_t alert_level);
+void ble_svc_lls_on_gap_disconnect(int reason);
+
+void ble_svc_lls_set_cb(ble_svc_lls_event_fn *cb);
+void ble_svc_lls_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/src/libs/mynewt-nimble/nimble/host/services/lls/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/lls/pkg.yml
new file mode 100644
index 00000000..6160f020
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/lls/pkg.yml
@@ -0,0 +1,34 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/lls
+pkg.description: Link Loss Service Implementation.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - lls
+    - nimble
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_lls_init: 'MYNEWT_VAL(BLE_SVC_LLS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/lls/src/ble_svc_lls.c b/src/libs/mynewt-nimble/nimble/host/services/lls/src/ble_svc_lls.c
new file mode 100644
index 00000000..b5f7f9bd
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/lls/src/ble_svc_lls.c
@@ -0,0 +1,192 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/lls/ble_svc_lls.h"
+
+/* Callback function */
+static ble_svc_lls_event_fn *ble_svc_lls_cb_fn;
+
+/* Alert level */
+static uint8_t ble_svc_lls_alert_level;
+
+/* Write characteristic function */
+static int
+ble_svc_lls_chr_write(struct os_mbuf *om, uint16_t min_len,
+                      uint16_t max_len, void *dst,
+                      uint16_t *len);
+
+/* Access function */
+static int
+ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_lls_defs[] = {
+    {
+        /*** Service: Link Loss Service (LLS). */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_LLS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /*** Characteristic: Alert Level. */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_LLS_CHR_UUID16_ALERT_LEVEL),
+            .access_cb = ble_svc_lls_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * Writes the received value from a characteristic write to
+ * the given destination.
+ */
+static int
+ble_svc_lls_chr_write(struct os_mbuf *om, uint16_t min_len,
+                      uint16_t max_len, void *dst,
+                      uint16_t *len)
+{
+    uint16_t om_len;
+    int rc;
+
+    om_len = OS_MBUF_PKTLEN(om);
+    if (om_len < min_len || om_len > max_len) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+    }
+
+    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * Simple read/write access callback for the alert level
+ * characteristic.
+ */
+static int
+ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    assert(ctxt->chr == &ble_svc_lls_defs[0].characteristics[0]);
+    int rc;
+    switch (ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+        rc = os_mbuf_append(ctxt->om, &ble_svc_lls_alert_level,
+                            sizeof ble_svc_lls_alert_level);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+        rc = ble_svc_lls_chr_write(ctxt->om,
+                                   sizeof ble_svc_lls_alert_level,
+                                   sizeof ble_svc_lls_alert_level,
+                                   &ble_svc_lls_alert_level, NULL);
+        return rc;
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * This function is the crux of the link loss service. The application
+ * developer must call this function inside the gap event callback
+ * function when a BLE_GAP_EVENT_DISCONNECT event is received and
+ * pass the disconnect reason into this function.
+ *
+ * Here, we then check if the disconnect reason is due to a timout, and if
+ * so, we call the ble_svc_lls_event_fn callback with the current
+ * alert level. The actual alert implementation is left up to the
+ * developer.
+ *
+ * @param reason            The reason attatched to the GAP disconnect
+ *                              event.
+ */
+void
+ble_svc_lls_on_gap_disconnect(int reason)
+{
+    if (reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_SPVN_TMO)) {
+            ble_svc_lls_cb_fn(ble_svc_lls_alert_level);
+    }
+}
+
+/**
+ * Gets the current alert level.
+ *
+ * @return The current alert level
+ */
+uint8_t
+ble_svc_lls_alert_level_get(void)
+{
+    return ble_svc_lls_alert_level;
+}
+
+/**
+ * Sets the current alert level.
+ *
+ * @return 0 on success, BLE_HS_EINVAL if the given alert level is not valid.
+ */
+int
+ble_svc_lls_alert_level_set(uint8_t alert_level)
+{
+    if (alert_level > BLE_SVC_LLS_ALERT_LEVEL_HIGH_ALERT) {
+        return BLE_HS_EINVAL;
+    }
+
+    memcpy(&ble_svc_lls_alert_level, &alert_level,
+           sizeof alert_level);
+
+    return 0;
+}
+
+void
+ble_svc_lls_set_cb(ble_svc_lls_event_fn *cb)
+{
+    ble_svc_lls_cb_fn = cb;
+}
+
+/**
+ * Initialize the IAS package.
+ */
+void
+ble_svc_lls_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_lls_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_lls_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/lls/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/lls/syscfg.yml
new file mode 100644
index 00000000..312b08a2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/lls/syscfg.yml
@@ -0,0 +1,22 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+
+syscfg.defs:
+    BLE_SVC_LLS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the link loss BLE service.
+        value: 303
diff --git a/src/libs/mynewt-nimble/nimble/host/services/tps/include/services/tps/ble_svc_tps.h b/src/libs/mynewt-nimble/nimble/host/services/tps/include/services/tps/ble_svc_tps.h
new file mode 100644
index 00000000..ec4cd790
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/tps/include/services/tps/ble_svc_tps.h
@@ -0,0 +1,31 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_TPS_
+#define H_BLE_SVC_TPS_
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_TPS_UUID16                                      0x1804
+#define BLE_SVC_TPS_CHR_UUID16_TX_POWER_LEVEL                   0x2a07
+
+void ble_svc_tps_init(void);
+
+#endif
+
diff --git a/src/libs/mynewt-nimble/nimble/host/services/tps/pkg.yml b/src/libs/mynewt-nimble/nimble/host/services/tps/pkg.yml
new file mode 100644
index 00000000..3d4c5e98
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/tps/pkg.yml
@@ -0,0 +1,34 @@
+
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/services/tps
+pkg.description: Tx Power Service adopted specification.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - tps
+    - nimble
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_svc_tps_init: 'MYNEWT_VAL(BLE_SVC_TPS_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/services/tps/src/ble_svc_tps.c b/src/libs/mynewt-nimble/nimble/host/services/tps/src/ble_svc_tps.c
new file mode 100644
index 00000000..9885a921
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/tps/src/ble_svc_tps.c
@@ -0,0 +1,107 @@
+/**
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "host/ble_hs.h"
+#include "services/tps/ble_svc_tps.h"
+
+/* XXX: We shouldn't be including the host's private header files.  The host
+ * API needs to be updated with a function to query the advertising transmit
+ * power.
+ */
+#include "../src/ble_hs_hci_priv.h"
+
+int8_t ble_svc_tps_tx_power_level;
+
+/* Access function */
+static int
+ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_tps_defs[] = {
+    {
+        /*** Service: Tx Power Service. */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_TPS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /*** Characteristic: Tx Power Level. */
+            .uuid = BLE_UUID16_DECLARE(BLE_SVC_TPS_CHR_UUID16_TX_POWER_LEVEL),
+            .access_cb = ble_svc_tps_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * Simple read access callback for the tx power level
+ * characteristic.
+ */
+static int
+ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    int rc;
+
+    assert(ctxt->chr == &ble_svc_tps_defs[0].characteristics[0]);
+
+    switch (ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+        rc = ble_hs_hci_util_read_adv_tx_pwr(&ble_svc_tps_tx_power_level);
+        if (rc != 0) {
+            return BLE_ATT_ERR_UNLIKELY;
+        }
+
+        rc = os_mbuf_append(ctxt->om, &ble_svc_tps_tx_power_level,
+                            sizeof ble_svc_tps_tx_power_level);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * Initialize the TPS
+ */
+void
+ble_svc_tps_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_tps_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_tps_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/services/tps/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/services/tps/syscfg.yml
new file mode 100644
index 00000000..0391e8b1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/services/tps/syscfg.yml
@@ -0,0 +1,23 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+
+syscfg.defs:
+    BLE_SVC_TPS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the transmit power BLE service.
+        value: 303
+
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_att.c b/src/libs/mynewt-nimble/nimble/host/src/ble_att.c
new file mode 100644
index 00000000..cc7a1f11
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_att.c
@@ -0,0 +1,589 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include "ble_hs_priv.h"
+
+static uint16_t ble_att_preferred_mtu_val;
+
+/** Dispatch table for incoming ATT requests.  Sorted by op code. */
+typedef int ble_att_rx_fn(uint16_t conn_handle, struct os_mbuf **om);
+struct ble_att_rx_dispatch_entry {
+    uint8_t bde_op;
+    ble_att_rx_fn *bde_fn;
+};
+
+/** Dispatch table for incoming ATT commands.  Must be ordered by op code. */
+static const struct ble_att_rx_dispatch_entry ble_att_rx_dispatch[] = {
+    { BLE_ATT_OP_ERROR_RSP,            ble_att_clt_rx_error },
+    { BLE_ATT_OP_MTU_REQ,              ble_att_svr_rx_mtu },
+    { BLE_ATT_OP_MTU_RSP,              ble_att_clt_rx_mtu },
+    { BLE_ATT_OP_FIND_INFO_REQ,        ble_att_svr_rx_find_info },
+    { BLE_ATT_OP_FIND_INFO_RSP,        ble_att_clt_rx_find_info },
+    { BLE_ATT_OP_FIND_TYPE_VALUE_REQ,  ble_att_svr_rx_find_type_value },
+    { BLE_ATT_OP_FIND_TYPE_VALUE_RSP,  ble_att_clt_rx_find_type_value },
+    { BLE_ATT_OP_READ_TYPE_REQ,        ble_att_svr_rx_read_type },
+    { BLE_ATT_OP_READ_TYPE_RSP,        ble_att_clt_rx_read_type },
+    { BLE_ATT_OP_READ_REQ,             ble_att_svr_rx_read },
+    { BLE_ATT_OP_READ_RSP,             ble_att_clt_rx_read },
+    { BLE_ATT_OP_READ_BLOB_REQ,        ble_att_svr_rx_read_blob },
+    { BLE_ATT_OP_READ_BLOB_RSP,        ble_att_clt_rx_read_blob },
+    { BLE_ATT_OP_READ_MULT_REQ,        ble_att_svr_rx_read_mult },
+    { BLE_ATT_OP_READ_MULT_RSP,        ble_att_clt_rx_read_mult },
+    { BLE_ATT_OP_READ_GROUP_TYPE_REQ,  ble_att_svr_rx_read_group_type },
+    { BLE_ATT_OP_READ_GROUP_TYPE_RSP,  ble_att_clt_rx_read_group_type },
+    { BLE_ATT_OP_WRITE_REQ,            ble_att_svr_rx_write },
+    { BLE_ATT_OP_WRITE_RSP,            ble_att_clt_rx_write },
+    { BLE_ATT_OP_PREP_WRITE_REQ,       ble_att_svr_rx_prep_write },
+    { BLE_ATT_OP_PREP_WRITE_RSP,       ble_att_clt_rx_prep_write },
+    { BLE_ATT_OP_EXEC_WRITE_REQ,       ble_att_svr_rx_exec_write },
+    { BLE_ATT_OP_EXEC_WRITE_RSP,       ble_att_clt_rx_exec_write },
+    { BLE_ATT_OP_NOTIFY_REQ,           ble_att_svr_rx_notify },
+    { BLE_ATT_OP_INDICATE_REQ,         ble_att_svr_rx_indicate },
+    { BLE_ATT_OP_INDICATE_RSP,         ble_att_clt_rx_indicate },
+    { BLE_ATT_OP_WRITE_CMD,            ble_att_svr_rx_write_no_rsp },
+};
+
+#define BLE_ATT_RX_DISPATCH_SZ \
+    (sizeof ble_att_rx_dispatch / sizeof ble_att_rx_dispatch[0])
+
+STATS_SECT_DECL(ble_att_stats) ble_att_stats;
+STATS_NAME_START(ble_att_stats)
+    STATS_NAME(ble_att_stats, error_rsp_rx)
+    STATS_NAME(ble_att_stats, error_rsp_tx)
+    STATS_NAME(ble_att_stats, mtu_req_rx)
+    STATS_NAME(ble_att_stats, mtu_req_tx)
+    STATS_NAME(ble_att_stats, mtu_rsp_rx)
+    STATS_NAME(ble_att_stats, mtu_rsp_tx)
+    STATS_NAME(ble_att_stats, find_info_req_rx)
+    STATS_NAME(ble_att_stats, find_info_req_tx)
+    STATS_NAME(ble_att_stats, find_info_rsp_rx)
+    STATS_NAME(ble_att_stats, find_info_rsp_tx)
+    STATS_NAME(ble_att_stats, find_type_value_req_rx)
+    STATS_NAME(ble_att_stats, find_type_value_req_tx)
+    STATS_NAME(ble_att_stats, find_type_value_rsp_rx)
+    STATS_NAME(ble_att_stats, find_type_value_rsp_tx)
+    STATS_NAME(ble_att_stats, read_type_req_rx)
+    STATS_NAME(ble_att_stats, read_type_req_tx)
+    STATS_NAME(ble_att_stats, read_type_rsp_rx)
+    STATS_NAME(ble_att_stats, read_type_rsp_tx)
+    STATS_NAME(ble_att_stats, read_req_rx)
+    STATS_NAME(ble_att_stats, read_req_tx)
+    STATS_NAME(ble_att_stats, read_rsp_rx)
+    STATS_NAME(ble_att_stats, read_rsp_tx)
+    STATS_NAME(ble_att_stats, read_blob_req_rx)
+    STATS_NAME(ble_att_stats, read_blob_req_tx)
+    STATS_NAME(ble_att_stats, read_blob_rsp_rx)
+    STATS_NAME(ble_att_stats, read_blob_rsp_tx)
+    STATS_NAME(ble_att_stats, read_mult_req_rx)
+    STATS_NAME(ble_att_stats, read_mult_req_tx)
+    STATS_NAME(ble_att_stats, read_mult_rsp_rx)
+    STATS_NAME(ble_att_stats, read_mult_rsp_tx)
+    STATS_NAME(ble_att_stats, read_group_type_req_rx)
+    STATS_NAME(ble_att_stats, read_group_type_req_tx)
+    STATS_NAME(ble_att_stats, read_group_type_rsp_rx)
+    STATS_NAME(ble_att_stats, read_group_type_rsp_tx)
+    STATS_NAME(ble_att_stats, write_req_rx)
+    STATS_NAME(ble_att_stats, write_req_tx)
+    STATS_NAME(ble_att_stats, write_rsp_rx)
+    STATS_NAME(ble_att_stats, write_rsp_tx)
+    STATS_NAME(ble_att_stats, prep_write_req_rx)
+    STATS_NAME(ble_att_stats, prep_write_req_tx)
+    STATS_NAME(ble_att_stats, prep_write_rsp_rx)
+    STATS_NAME(ble_att_stats, prep_write_rsp_tx)
+    STATS_NAME(ble_att_stats, exec_write_req_rx)
+    STATS_NAME(ble_att_stats, exec_write_req_tx)
+    STATS_NAME(ble_att_stats, exec_write_rsp_rx)
+    STATS_NAME(ble_att_stats, exec_write_rsp_tx)
+    STATS_NAME(ble_att_stats, notify_req_rx)
+    STATS_NAME(ble_att_stats, notify_req_tx)
+    STATS_NAME(ble_att_stats, indicate_req_rx)
+    STATS_NAME(ble_att_stats, indicate_req_tx)
+    STATS_NAME(ble_att_stats, indicate_rsp_rx)
+    STATS_NAME(ble_att_stats, indicate_rsp_tx)
+    STATS_NAME(ble_att_stats, write_cmd_rx)
+    STATS_NAME(ble_att_stats, write_cmd_tx)
+STATS_NAME_END(ble_att_stats)
+
+static const struct ble_att_rx_dispatch_entry *
+ble_att_rx_dispatch_entry_find(uint8_t op)
+{
+    const struct ble_att_rx_dispatch_entry *entry;
+    int i;
+
+    for (i = 0; i < BLE_ATT_RX_DISPATCH_SZ; i++) {
+        entry = ble_att_rx_dispatch + i;
+        if (entry->bde_op == op) {
+            return entry;
+        }
+
+        if (entry->bde_op > op) {
+            break;
+        }
+    }
+
+    return NULL;
+}
+
+int
+ble_att_conn_chan_find(uint16_t conn_handle, struct ble_hs_conn **out_conn,
+                       struct ble_l2cap_chan **out_chan)
+{
+    return ble_hs_misc_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT,
+                                      out_conn, out_chan);
+}
+
+void
+ble_att_inc_tx_stat(uint8_t att_op)
+{
+    switch (att_op) {
+    case BLE_ATT_OP_ERROR_RSP:
+        STATS_INC(ble_att_stats, error_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_MTU_REQ:
+        STATS_INC(ble_att_stats, mtu_req_tx);
+        break;
+
+    case BLE_ATT_OP_MTU_RSP:
+        STATS_INC(ble_att_stats, mtu_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_FIND_INFO_REQ:
+        STATS_INC(ble_att_stats, find_info_req_tx);
+        break;
+
+    case BLE_ATT_OP_FIND_INFO_RSP:
+        STATS_INC(ble_att_stats, find_info_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_FIND_TYPE_VALUE_REQ:
+        STATS_INC(ble_att_stats, find_type_value_req_tx);
+        break;
+
+    case BLE_ATT_OP_FIND_TYPE_VALUE_RSP:
+        STATS_INC(ble_att_stats, find_type_value_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_READ_TYPE_REQ:
+        STATS_INC(ble_att_stats, read_type_req_tx);
+        break;
+
+    case BLE_ATT_OP_READ_TYPE_RSP:
+        STATS_INC(ble_att_stats, read_type_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_READ_REQ:
+        STATS_INC(ble_att_stats, read_req_tx);
+        break;
+
+    case BLE_ATT_OP_READ_RSP:
+        STATS_INC(ble_att_stats, read_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_READ_BLOB_REQ:
+        STATS_INC(ble_att_stats, read_blob_req_tx);
+        break;
+
+    case BLE_ATT_OP_READ_BLOB_RSP:
+        STATS_INC(ble_att_stats, read_blob_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_READ_MULT_REQ:
+        STATS_INC(ble_att_stats, read_mult_req_tx);
+        break;
+
+    case BLE_ATT_OP_READ_MULT_RSP:
+        STATS_INC(ble_att_stats, read_mult_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_READ_GROUP_TYPE_REQ:
+        STATS_INC(ble_att_stats, read_group_type_req_tx);
+        break;
+
+    case BLE_ATT_OP_READ_GROUP_TYPE_RSP:
+        STATS_INC(ble_att_stats, read_group_type_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_WRITE_REQ:
+        STATS_INC(ble_att_stats, write_req_tx);
+        break;
+
+    case BLE_ATT_OP_WRITE_RSP:
+        STATS_INC(ble_att_stats, write_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_PREP_WRITE_REQ:
+        STATS_INC(ble_att_stats, prep_write_req_tx);
+        break;
+
+    case BLE_ATT_OP_PREP_WRITE_RSP:
+        STATS_INC(ble_att_stats, prep_write_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_EXEC_WRITE_REQ:
+        STATS_INC(ble_att_stats, exec_write_req_tx);
+        break;
+
+    case BLE_ATT_OP_EXEC_WRITE_RSP:
+        STATS_INC(ble_att_stats, exec_write_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_NOTIFY_REQ:
+        STATS_INC(ble_att_stats, notify_req_tx);
+        break;
+
+    case BLE_ATT_OP_INDICATE_REQ:
+        STATS_INC(ble_att_stats, indicate_req_tx);
+        break;
+
+    case BLE_ATT_OP_INDICATE_RSP:
+        STATS_INC(ble_att_stats, indicate_rsp_tx);
+        break;
+
+    case BLE_ATT_OP_WRITE_CMD:
+        STATS_INC(ble_att_stats, write_cmd_tx);
+        break;
+
+    default:
+        break;
+    }
+}
+
+static void
+ble_att_inc_rx_stat(uint8_t att_op)
+{
+    switch (att_op) {
+    case BLE_ATT_OP_ERROR_RSP:
+        STATS_INC(ble_att_stats, error_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_MTU_REQ:
+        STATS_INC(ble_att_stats, mtu_req_rx);
+        break;
+
+    case BLE_ATT_OP_MTU_RSP:
+        STATS_INC(ble_att_stats, mtu_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_FIND_INFO_REQ:
+        STATS_INC(ble_att_stats, find_info_req_rx);
+        break;
+
+    case BLE_ATT_OP_FIND_INFO_RSP:
+        STATS_INC(ble_att_stats, find_info_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_FIND_TYPE_VALUE_REQ:
+        STATS_INC(ble_att_stats, find_type_value_req_rx);
+        break;
+
+    case BLE_ATT_OP_FIND_TYPE_VALUE_RSP:
+        STATS_INC(ble_att_stats, find_type_value_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_READ_TYPE_REQ:
+        STATS_INC(ble_att_stats, read_type_req_rx);
+        break;
+
+    case BLE_ATT_OP_READ_TYPE_RSP:
+        STATS_INC(ble_att_stats, read_type_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_READ_REQ:
+        STATS_INC(ble_att_stats, read_req_rx);
+        break;
+
+    case BLE_ATT_OP_READ_RSP:
+        STATS_INC(ble_att_stats, read_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_READ_BLOB_REQ:
+        STATS_INC(ble_att_stats, read_blob_req_rx);
+        break;
+
+    case BLE_ATT_OP_READ_BLOB_RSP:
+        STATS_INC(ble_att_stats, read_blob_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_READ_MULT_REQ:
+        STATS_INC(ble_att_stats, read_mult_req_rx);
+        break;
+
+    case BLE_ATT_OP_READ_MULT_RSP:
+        STATS_INC(ble_att_stats, read_mult_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_READ_GROUP_TYPE_REQ:
+        STATS_INC(ble_att_stats, read_group_type_req_rx);
+        break;
+
+    case BLE_ATT_OP_READ_GROUP_TYPE_RSP:
+        STATS_INC(ble_att_stats, read_group_type_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_WRITE_REQ:
+        STATS_INC(ble_att_stats, write_req_rx);
+        break;
+
+    case BLE_ATT_OP_WRITE_RSP:
+        STATS_INC(ble_att_stats, write_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_PREP_WRITE_REQ:
+        STATS_INC(ble_att_stats, prep_write_req_rx);
+        break;
+
+    case BLE_ATT_OP_PREP_WRITE_RSP:
+        STATS_INC(ble_att_stats, prep_write_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_EXEC_WRITE_REQ:
+        STATS_INC(ble_att_stats, exec_write_req_rx);
+        break;
+
+    case BLE_ATT_OP_EXEC_WRITE_RSP:
+        STATS_INC(ble_att_stats, exec_write_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_NOTIFY_REQ:
+        STATS_INC(ble_att_stats, notify_req_rx);
+        break;
+
+    case BLE_ATT_OP_INDICATE_REQ:
+        STATS_INC(ble_att_stats, indicate_req_rx);
+        break;
+
+    case BLE_ATT_OP_INDICATE_RSP:
+        STATS_INC(ble_att_stats, indicate_rsp_rx);
+        break;
+
+    case BLE_ATT_OP_WRITE_CMD:
+        STATS_INC(ble_att_stats, write_cmd_rx);
+        break;
+
+    default:
+        break;
+    }
+}
+
+void
+ble_att_truncate_to_mtu(const struct ble_l2cap_chan *att_chan,
+                        struct os_mbuf *txom)
+{
+    int32_t extra_len;
+    uint16_t mtu;
+
+    mtu = ble_att_chan_mtu(att_chan);
+    extra_len = OS_MBUF_PKTLEN(txom) - mtu;
+    if (extra_len > 0) {
+        os_mbuf_adj(txom, -extra_len);
+    }
+}
+
+uint16_t
+ble_att_mtu(uint16_t conn_handle)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    uint16_t mtu;
+    int rc;
+
+    ble_hs_lock();
+
+    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    if (rc == 0) {
+        mtu = ble_att_chan_mtu(chan);
+    } else {
+        mtu = 0;
+    }
+
+    ble_hs_unlock();
+
+    return mtu;
+}
+
+void
+ble_att_set_peer_mtu(struct ble_l2cap_chan *chan, uint16_t peer_mtu)
+{
+    if (peer_mtu < BLE_ATT_MTU_DFLT) {
+        peer_mtu = BLE_ATT_MTU_DFLT;
+    }
+
+    chan->peer_mtu = peer_mtu;
+}
+
+uint16_t
+ble_att_chan_mtu(const struct ble_l2cap_chan *chan)
+{
+    uint16_t mtu;
+
+    /* If either side has not exchanged MTU size, use the default.  Otherwise,
+     * use the lesser of the two exchanged values.
+     */
+    if (!(ble_l2cap_is_mtu_req_sent(chan)) ||
+        chan->peer_mtu == 0) {
+
+        mtu = BLE_ATT_MTU_DFLT;
+    } else {
+        mtu = min(chan->my_mtu, chan->peer_mtu);
+    }
+
+    BLE_HS_DBG_ASSERT(mtu >= BLE_ATT_MTU_DFLT);
+
+    return mtu;
+}
+
+static void
+ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle,
+                                  struct os_mbuf **om)
+{
+    /* If this is command (bit6 is set to 1), do nothing */
+    if (op & 0x40) {
+        return;
+    }
+
+    os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om));
+    ble_att_svr_tx_error_rsp(conn_handle, *om, op, 0,
+                             BLE_ATT_ERR_REQ_NOT_SUPPORTED);
+
+    *om = NULL;
+}
+
+static int
+ble_att_rx(struct ble_l2cap_chan *chan)
+{
+    const struct ble_att_rx_dispatch_entry *entry;
+    uint8_t op;
+    uint16_t conn_handle;
+    struct os_mbuf **om;
+    int rc;
+
+    conn_handle = ble_l2cap_get_conn_handle(chan);
+    if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    om = &chan->rx_buf;
+    BLE_HS_DBG_ASSERT(*om != NULL);
+
+    rc = os_mbuf_copydata(*om, 0, 1, &op);
+    if (rc != 0) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    entry = ble_att_rx_dispatch_entry_find(op);
+    if (entry == NULL) {
+        ble_att_rx_handle_unknown_request(op, conn_handle, om);
+        return BLE_HS_ENOTSUP;
+    }
+
+    ble_att_inc_rx_stat(op);
+
+    /* Strip L2CAP ATT header from the front of the mbuf. */
+    os_mbuf_adj(*om, 1);
+
+    rc = entry->bde_fn(conn_handle, om);
+    if (rc != 0) {
+        if (rc == BLE_HS_ENOTSUP) {
+            ble_att_rx_handle_unknown_request(op, conn_handle, om);
+        }
+        return rc;
+    }
+
+    return 0;
+}
+
+uint16_t
+ble_att_preferred_mtu(void)
+{
+    return ble_att_preferred_mtu_val;
+}
+
+int
+ble_att_set_preferred_mtu(uint16_t mtu)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int i;
+
+    if (mtu < BLE_ATT_MTU_DFLT) {
+        return BLE_HS_EINVAL;
+    }
+    if (mtu > BLE_ATT_MTU_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_att_preferred_mtu_val = mtu;
+
+    /* Set my_mtu for established connections that haven't exchanged. */
+    ble_hs_lock();
+
+    i = 0;
+    while ((conn = ble_hs_conn_find_by_idx(i)) != NULL) {
+        chan = ble_hs_conn_chan_find_by_scid(conn, BLE_L2CAP_CID_ATT);
+        BLE_HS_DBG_ASSERT(chan != NULL);
+
+        if (!(chan->flags & BLE_L2CAP_CHAN_F_TXED_MTU)) {
+            chan->my_mtu = mtu;
+        }
+
+        i++;
+    }
+
+    ble_hs_unlock();
+
+    return 0;
+}
+
+struct ble_l2cap_chan *
+ble_att_create_chan(uint16_t conn_handle)
+{
+    struct ble_l2cap_chan *chan;
+
+    chan = ble_l2cap_chan_alloc(conn_handle);
+    if (chan == NULL) {
+        return NULL;
+    }
+
+    chan->scid = BLE_L2CAP_CID_ATT;
+    chan->dcid = BLE_L2CAP_CID_ATT;
+    chan->my_mtu = ble_att_preferred_mtu_val;
+    chan->rx_fn = ble_att_rx;
+
+    return chan;
+}
+
+int
+ble_att_init(void)
+{
+    int rc;
+
+    ble_att_preferred_mtu_val = MYNEWT_VAL(BLE_ATT_PREFERRED_MTU);
+
+    rc = stats_init_and_reg(
+        STATS_HDR(ble_att_stats), STATS_SIZE_INIT_PARMS(ble_att_stats,
+        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_att_stats), "ble_att");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_att_clt.c b/src/libs/mynewt-nimble/nimble/host/src/ble_att_clt.c
new file mode 100644
index 00000000..09fc9ea2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_att_clt.c
@@ -0,0 +1,956 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+#include "os/os_mempool.h"
+#include "nimble/ble.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_priv.h"
+
+/*****************************************************************************
+ * $error response                                                           *
+ *****************************************************************************/
+
+int
+ble_att_clt_rx_error(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+    struct ble_att_error_rsp *rsp;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    rsp = (struct ble_att_error_rsp *)(*rxom)->om_data;
+
+    ble_gattc_rx_err(conn_handle, le16toh(rsp->baep_handle),
+                     le16toh(rsp->baep_error_code));
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $mtu exchange                                                             *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu)
+{
+    struct ble_att_mtu_cmd *req;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    struct os_mbuf *txom;
+    int rc;
+
+    if (mtu < BLE_ATT_MTU_DFLT) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+
+    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    if (rc != 0) {
+        rc = BLE_HS_ENOTCONN;
+    } else if (chan->flags & BLE_L2CAP_CHAN_F_TXED_MTU) {
+        rc = BLE_HS_EALREADY;
+    } else {
+        rc = 0;
+    }
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        return rc;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_MTU_REQ, sizeof(*req), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->bamc_mtu = htole16(mtu);
+
+    rc = ble_att_tx(conn_handle, txom);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_lock();
+
+    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    if (rc == 0) {
+        chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+    struct ble_att_mtu_cmd *cmd;
+    struct ble_l2cap_chan *chan;
+    uint16_t mtu;
+    int rc;
+
+    mtu = 0;
+
+    rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*cmd));
+    if (rc == 0) {
+        cmd = (struct ble_att_mtu_cmd *)(*rxom)->om_data;
+
+        ble_hs_lock();
+
+        rc = ble_att_conn_chan_find(conn_handle, NULL, &chan);
+        if (rc == 0) {
+            ble_att_set_peer_mtu(chan, le16toh(cmd->bamc_mtu));
+            mtu = ble_att_chan_mtu(chan);
+        }
+
+        ble_hs_unlock();
+
+        if (rc == 0) {
+            ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu);
+        }
+    }
+
+    ble_gattc_rx_mtu(conn_handle, rc, mtu);
+    return rc;
+}
+
+/*****************************************************************************
+ * $find information                                                         *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_find_info(uint16_t conn_handle, uint16_t start_handle,
+                         uint16_t end_handle)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_INFO
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_find_info_req *req;
+    struct os_mbuf *txom;
+
+    if (start_handle == 0 || start_handle > end_handle) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_FIND_INFO_REQ, sizeof(*req), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->bafq_start_handle = htole16(start_handle);
+    req->bafq_end_handle = htole16(end_handle);
+
+    return ble_att_tx(conn_handle, txom);
+}
+
+static int
+ble_att_clt_parse_find_info_entry(struct os_mbuf **rxom, uint8_t rsp_format,
+                                  struct ble_att_find_info_idata *idata)
+{
+    int entry_len;
+    int rc;
+
+    switch (rsp_format) {
+    case BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT:
+        entry_len = 2 + 2;
+        break;
+
+    case BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT:
+        entry_len = 2 + 16;
+        break;
+
+    default:
+        return BLE_HS_EBADDATA;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(rxom, entry_len);
+    if (rc != 0) {
+        return rc;
+    }
+
+    idata->attr_handle = get_le16((*rxom)->om_data);
+
+    switch (rsp_format) {
+    case BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT:
+        rc = ble_uuid_init_from_att_mbuf(&idata->uuid, *rxom, 2, 2);
+        if (rc != 0) {
+            return BLE_HS_EBADDATA;
+        }
+        break;
+
+    case BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT:
+        rc = ble_uuid_init_from_att_mbuf(&idata->uuid, *rxom, 2, 16);
+        if (rc != 0) {
+            return BLE_HS_EBADDATA;
+        }
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+
+    os_mbuf_adj(*rxom, entry_len);
+    return 0;
+}
+
+int
+ble_att_clt_rx_find_info(uint16_t conn_handle, struct os_mbuf **om)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_INFO
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_find_info_idata idata;
+    struct ble_att_find_info_rsp *rsp;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(*rsp));
+    if (rc != 0) {
+        goto done;
+    }
+
+    rsp = (struct ble_att_find_info_rsp *)(*om)->om_data;
+
+    /* Strip the response base from the front of the mbuf. */
+    os_mbuf_adj((*om), sizeof(*rsp));
+
+    while (OS_MBUF_PKTLEN(*om) > 0) {
+        rc = ble_att_clt_parse_find_info_entry(om, rsp->bafp_format, &idata);
+        if (rc != 0) {
+            goto done;
+        }
+
+        /* Hand find-info entry to GATT. */
+        ble_gattc_rx_find_info_idata(conn_handle, &idata);
+    }
+
+    rc = 0;
+
+done:
+    /* Notify GATT that response processing is done. */
+    ble_gattc_rx_find_info_complete(conn_handle, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $find by type value                                                       *
+ *****************************************************************************/
+
+/*
+ * TODO consider this to accept UUID instead of value, it is used only for this
+ * anyway
+ */
+int
+ble_att_clt_tx_find_type_value(uint16_t conn_handle, uint16_t start_handle,
+                               uint16_t end_handle, uint16_t attribute_type,
+                               const void *attribute_value, int value_len)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_TYPE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_find_type_value_req *req;
+    struct os_mbuf *txom;
+
+    if (start_handle == 0 || start_handle > end_handle) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, sizeof(*req) + value_len,
+                          &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->bavq_start_handle = htole16(start_handle);
+    req->bavq_end_handle = htole16(end_handle);
+    req->bavq_attr_type = htole16(attribute_type);
+    memcpy(req->bavq_value, attribute_value, value_len);
+
+    return ble_att_tx(conn_handle, txom);
+}
+
+static int
+ble_att_clt_parse_find_type_value_hinfo(
+    struct os_mbuf **om, struct ble_att_find_type_value_hinfo *dst)
+{
+    struct ble_att_handle_group *group;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(*group));
+    if (rc != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    group = (struct ble_att_handle_group *)(*om)->om_data;
+
+    dst->attr_handle = le16toh(group->attr_handle);
+    dst->group_end_handle = le16toh(group->group_end_handle);
+
+    os_mbuf_adj((*om), sizeof(*group));
+
+    return 0;
+}
+
+int
+ble_att_clt_rx_find_type_value(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_TYPE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_find_type_value_hinfo hinfo;
+    int rc;
+
+    /* Parse the Handles-Information-List field, passing each entry to GATT. */
+    rc = 0;
+    while (OS_MBUF_PKTLEN(*rxom) > 0) {
+        rc = ble_att_clt_parse_find_type_value_hinfo(rxom, &hinfo);
+        if (rc != 0) {
+            break;
+        }
+
+        ble_gattc_rx_find_type_value_hinfo(conn_handle, &hinfo);
+    }
+
+    /* Notify GATT client that the full response has been parsed. */
+    ble_gattc_rx_find_type_value_complete(conn_handle, rc);
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $read by type                                                             *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_read_type(uint16_t conn_handle, uint16_t start_handle,
+                         uint16_t end_handle, const ble_uuid_t *uuid)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_TYPE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_type_req *req;
+    struct os_mbuf *txom;
+
+    if (start_handle == 0 || start_handle > end_handle) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_READ_TYPE_REQ,
+                          sizeof(*req) + ble_uuid_length(uuid), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->batq_start_handle = htole16(start_handle);
+    req->batq_end_handle = htole16(end_handle);
+
+    ble_uuid_flat(uuid, req->uuid);
+
+    return ble_att_tx(conn_handle, txom);
+}
+
+int
+ble_att_clt_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_TYPE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_type_adata adata;
+    struct ble_att_attr_data_list *data;
+    struct ble_att_read_type_rsp *rsp;
+    uint8_t data_len;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*rsp));
+    if (rc != 0) {
+        goto done;
+    }
+
+    rsp = (struct ble_att_read_type_rsp *)(*rxom)->om_data;
+
+    data_len = rsp->batp_length;
+
+    /* Strip the response base from the front of the mbuf. */
+    os_mbuf_adj(*rxom, sizeof(*rsp));
+
+    if (data_len < sizeof(*data)) {
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    /* Parse the Attribute Data List field, passing each entry to the GATT. */
+    while (OS_MBUF_PKTLEN(*rxom) > 0) {
+        rc = ble_hs_mbuf_pullup_base(rxom, data_len);
+        if (rc != 0) {
+            break;
+        }
+
+        data = (struct ble_att_attr_data_list *)(*rxom)->om_data;
+
+        adata.att_handle = le16toh(data->handle);
+        adata.value_len = data_len - sizeof(*data);
+        adata.value = data->value;
+
+        ble_gattc_rx_read_type_adata(conn_handle, &adata);
+        os_mbuf_adj(*rxom, data_len);
+    }
+
+done:
+    /* Notify GATT that the response is done being parsed. */
+    ble_gattc_rx_read_type_complete(conn_handle, rc);
+    return rc;
+
+}
+
+/*****************************************************************************
+ * $read                                                                     *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_read(uint16_t conn_handle, uint16_t handle)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_req *req;
+    struct os_mbuf *txom;
+    int rc;
+
+    if (handle == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_READ_REQ, sizeof(*req), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->barq_handle = htole16(handle);
+
+    rc = ble_att_tx(conn_handle, txom);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_att_clt_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ
+    return BLE_HS_ENOTSUP;
+#endif
+
+    /* Pass the Attribute Value field to GATT. */
+    ble_gattc_rx_read_rsp(conn_handle, 0, rxom);
+    return 0;
+}
+
+/*****************************************************************************
+ * $read blob                                                                *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_read_blob(uint16_t conn_handle, uint16_t handle, uint16_t offset)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_BLOB
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_blob_req *req;
+    struct os_mbuf *txom;
+    int rc;
+
+    if (handle == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_READ_BLOB_REQ, sizeof(*req), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->babq_handle = htole16(handle);
+    req->babq_offset = htole16(offset);
+
+    rc = ble_att_tx(conn_handle, txom);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_att_clt_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_BLOB
+    return BLE_HS_ENOTSUP;
+#endif
+
+    /* Pass the Attribute Value field to GATT. */
+    ble_gattc_rx_read_blob_rsp(conn_handle, 0, rxom);
+    return 0;
+}
+
+/*****************************************************************************
+ * $read multiple                                                            *
+ *****************************************************************************/
+int
+ble_att_clt_tx_read_mult(uint16_t conn_handle, const uint16_t *handles,
+                         int num_handles)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_MULT
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_mult_req *req;
+    struct os_mbuf *txom;
+    int i;
+
+    if (num_handles < 1) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_READ_MULT_REQ,
+                          sizeof(req->handles[0]) * num_handles,
+                          &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    for(i = 0; i < num_handles; i++) {
+        req->handles[i] = htole16(handles[i]);
+    }
+
+    return ble_att_tx(conn_handle, txom);
+}
+
+int
+ble_att_clt_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_MULT
+    return BLE_HS_ENOTSUP;
+#endif
+
+    /* Pass the Attribute Value field to GATT. */
+    ble_gattc_rx_read_mult_rsp(conn_handle, 0, rxom);
+    return 0;
+}
+
+/*****************************************************************************
+ * $read by group type                                                       *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_read_group_type(uint16_t conn_handle,
+                               uint16_t start_handle, uint16_t end_handle,
+                               const ble_uuid_t *uuid)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_GROUP_TYPE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_group_type_req *req;
+    struct os_mbuf *txom;
+
+    if (start_handle == 0 || start_handle > end_handle) {
+        return BLE_HS_EINVAL;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_READ_GROUP_TYPE_REQ,
+                          sizeof(*req) + ble_uuid_length(uuid), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->bagq_start_handle = htole16(start_handle);
+    req->bagq_end_handle = htole16(end_handle);
+    ble_uuid_flat(uuid, req->uuid);
+
+    return ble_att_tx(conn_handle, txom);
+}
+
+static int
+ble_att_clt_parse_read_group_type_adata(
+    struct os_mbuf **om, int data_len,
+    struct ble_att_read_group_type_adata *adata)
+{
+    int rc;
+
+    if (data_len < BLE_ATT_READ_GROUP_TYPE_ADATA_BASE_SZ + 1) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(om, data_len);
+    if (rc != 0) {
+        return rc;
+    }
+
+    adata->att_handle = get_le16((*om)->om_data + 0);
+    adata->end_group_handle = get_le16((*om)->om_data + 2);
+    adata->value_len = data_len - BLE_ATT_READ_GROUP_TYPE_ADATA_BASE_SZ;
+    adata->value = (*om)->om_data + BLE_ATT_READ_GROUP_TYPE_ADATA_BASE_SZ;
+
+    return 0;
+}
+
+int
+ble_att_clt_rx_read_group_type(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_GROUP_TYPE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_group_type_adata adata;
+    struct ble_att_read_group_type_rsp *rsp;
+    uint8_t len;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*rsp));
+    if (rc != 0) {
+        goto done;
+    }
+
+    rsp = (struct ble_att_read_group_type_rsp *)(*rxom)->om_data;
+
+    len = rsp->bagp_length;
+
+    /* Strip the base from the front of the response. */
+    os_mbuf_adj(*rxom, sizeof(*rsp));
+
+    /* Parse the Attribute Data List field, passing each entry to GATT. */
+    while (OS_MBUF_PKTLEN(*rxom) > 0) {
+        rc = ble_att_clt_parse_read_group_type_adata(rxom, len, &adata);
+        if (rc != 0) {
+            goto done;
+        }
+
+        ble_gattc_rx_read_group_type_adata(conn_handle, &adata);
+        os_mbuf_adj(*rxom, len);
+    }
+
+done:
+    /* Notify GATT that the response is done being parsed. */
+    ble_gattc_rx_read_group_type_complete(conn_handle, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $write                                                                    *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_write_req(uint16_t conn_handle, uint16_t handle,
+                         struct os_mbuf *txom)
+{
+#if !NIMBLE_BLE_ATT_CLT_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_write_req *req;
+    struct os_mbuf *txom2;
+
+    req = ble_att_cmd_get(BLE_ATT_OP_WRITE_REQ, sizeof(*req), &txom2);
+    if (req == NULL) {
+        os_mbuf_free_chain(txom);
+        return BLE_HS_ENOMEM;
+    }
+
+    req->bawq_handle = htole16(handle);
+    os_mbuf_concat(txom2, txom);
+
+    return ble_att_tx(conn_handle, txom2);
+}
+
+int
+ble_att_clt_tx_write_cmd(uint16_t conn_handle, uint16_t handle,
+                         struct os_mbuf *txom)
+{
+#if !NIMBLE_BLE_ATT_CLT_WRITE_NO_RSP
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_write_cmd *cmd;
+    struct os_mbuf *txom2;
+    uint8_t b;
+    int rc;
+    int i;
+
+    BLE_HS_LOG(DEBUG, "ble_att_clt_tx_write_cmd(): ");
+    for (i = 0; i < OS_MBUF_PKTLEN(txom); i++) {
+        if (i != 0) {
+            BLE_HS_LOG(DEBUG, ":");
+        }
+        rc = os_mbuf_copydata(txom, i, 1, &b);
+        assert(rc == 0);
+        BLE_HS_LOG(DEBUG, "0x%02x", b);
+    }
+
+
+    cmd = ble_att_cmd_get(BLE_ATT_OP_WRITE_CMD, sizeof(*cmd), &txom2);
+    if (cmd == NULL) {
+        os_mbuf_free_chain(txom);
+        return BLE_HS_ENOMEM;
+    }
+
+    cmd->handle = htole16(handle);
+    os_mbuf_concat(txom2, txom);
+
+    return ble_att_tx(conn_handle, txom2);
+}
+
+int
+ble_att_clt_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    /* No payload. */
+    ble_gattc_rx_write_rsp(conn_handle);
+    return 0;
+}
+
+/*****************************************************************************
+ * $prepare write request                                                    *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_prep_write(uint16_t conn_handle, uint16_t handle,
+                          uint16_t offset, struct os_mbuf *txom)
+{
+#if !NIMBLE_BLE_ATT_CLT_PREP_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_prep_write_cmd *req;
+    struct os_mbuf *txom2;
+    int rc;
+
+    if (handle == 0) {
+        rc = BLE_HS_EINVAL;
+        goto err;
+    }
+
+    if (offset + OS_MBUF_PKTLEN(txom) > BLE_ATT_ATTR_MAX_LEN) {
+        rc = BLE_HS_EINVAL;
+        goto err;
+    }
+
+    if (OS_MBUF_PKTLEN(txom) >
+        ble_att_mtu(conn_handle) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ) {
+        rc = BLE_HS_EINVAL;
+        goto err;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_PREP_WRITE_REQ, sizeof(*req), &txom2);
+    if (req == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    req->bapc_handle = htole16(handle);
+    req->bapc_offset = htole16(offset);
+    os_mbuf_concat(txom2, txom);
+
+    return ble_att_tx(conn_handle, txom2);
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
+}
+
+int
+ble_att_clt_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_PREP_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_prep_write_cmd *rsp;
+    uint16_t handle, offset;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    handle = 0;
+    offset = 0;
+
+    rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*rsp));
+    if (rc != 0) {
+        goto done;
+    }
+
+    rsp = (struct ble_att_prep_write_cmd *)(*rxom)->om_data;
+
+    handle = le16toh(rsp->bapc_handle);
+    offset = le16toh(rsp->bapc_offset);
+
+    /* Strip the base from the front of the response. */
+    os_mbuf_adj(*rxom, sizeof(*rsp));
+
+done:
+    /* Notify GATT client that the full response has been parsed. */
+    ble_gattc_rx_prep_write_rsp(conn_handle, rc, handle, offset, rxom);
+    return rc;
+}
+
+/*****************************************************************************
+ * $execute write request                                                    *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_exec_write(uint16_t conn_handle, uint8_t flags)
+{
+#if !NIMBLE_BLE_ATT_CLT_EXEC_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_exec_write_req *req;
+    struct os_mbuf *txom;
+    int rc;
+
+    req = ble_att_cmd_get(BLE_ATT_OP_EXEC_WRITE_REQ, sizeof(*req), &txom);
+    if (req == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req->baeq_flags = flags;
+
+    rc = ble_att_tx(conn_handle, txom);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_att_clt_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_EXEC_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    ble_gattc_rx_exec_write_rsp(conn_handle, 0);
+    return 0;
+}
+
+/*****************************************************************************
+ * $handle value notification                                                *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle,
+                      struct os_mbuf *txom)
+{
+#if !NIMBLE_BLE_ATT_CLT_NOTIFY
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_notify_req *req;
+    struct os_mbuf *txom2;
+    int rc;
+
+    if (handle == 0) {
+        rc = BLE_HS_EINVAL;
+        goto err;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_NOTIFY_REQ, sizeof(*req), &txom2);
+    if (req == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    req->banq_handle = htole16(handle);
+    os_mbuf_concat(txom2, txom);
+
+    return ble_att_tx(conn_handle, txom2);
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
+}
+
+/*****************************************************************************
+ * $handle value indication                                                  *
+ *****************************************************************************/
+
+int
+ble_att_clt_tx_indicate(uint16_t conn_handle, uint16_t handle,
+                        struct os_mbuf *txom)
+{
+#if !NIMBLE_BLE_ATT_CLT_INDICATE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_indicate_req *req;
+    struct os_mbuf *txom2;
+    int rc;
+
+    if (handle == 0) {
+        rc = BLE_HS_EINVAL;
+        goto err;
+    }
+
+    req = ble_att_cmd_get(BLE_ATT_OP_INDICATE_REQ, sizeof(*req), &txom2);
+    if (req == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    req->baiq_handle = htole16(handle);
+    os_mbuf_concat(txom2, txom);
+
+    return ble_att_tx(conn_handle, txom2);
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
+}
+
+int
+ble_att_clt_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_INDICATE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    /* No payload. */
+    ble_gattc_rx_indicate_rsp(conn_handle);
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_att_cmd.c b/src/libs/mynewt-nimble/nimble/host/src/ble_att_cmd.c
new file mode 100644
index 00000000..a123c857
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_att_cmd.c
@@ -0,0 +1,637 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <errno.h>
+#include <string.h>
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "ble_hs_priv.h"
+#include "host/ble_att.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_priv.h"
+
+void *
+ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom)
+{
+    struct ble_att_hdr *hdr;
+
+    if (os_mbuf_extend(txom, sizeof(*hdr) + len) == NULL) {
+        os_mbuf_free_chain(txom);
+        return NULL;
+    }
+
+    hdr = (struct ble_att_hdr *)(txom)->om_data;
+
+    hdr->opcode = opcode;
+
+    return hdr->data;
+}
+
+void *
+ble_att_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom)
+{
+    *txom = ble_hs_mbuf_l2cap_pkt();
+    if (*txom == NULL) {
+        return NULL;
+    }
+
+    return ble_att_cmd_prepare(opcode, len, *txom);
+}
+
+int
+ble_att_tx(uint16_t conn_handle, struct os_mbuf *txom)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    BLE_HS_DBG_ASSERT_EVAL(txom->om_len >= 1);
+    ble_att_inc_tx_stat(txom->om_data[0]);
+
+    ble_hs_lock();
+
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_ATT, &conn,
+                                    &chan);
+    if (chan == NULL) {
+        os_mbuf_free_chain(txom);
+        rc = BLE_HS_ENOTCONN;
+    } else {
+        ble_att_truncate_to_mtu(chan, txom);
+        rc = ble_l2cap_tx(conn, chan, txom);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+static const void *
+ble_att_init_parse(uint8_t op, const void *payload,
+                   int min_len, int actual_len)
+{
+    const uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(actual_len >= min_len);
+
+    u8ptr = payload;
+    BLE_HS_DBG_ASSERT(u8ptr[0] == op);
+
+    return u8ptr + 1;
+}
+
+static void *
+ble_att_init_write(uint8_t op, void *payload, int min_len, int actual_len)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(actual_len >= min_len);
+
+    u8ptr = payload;
+    u8ptr[0] = op;
+
+    return u8ptr + 1;
+}
+
+void
+ble_att_error_rsp_parse(const void *payload, int len,
+                        struct ble_att_error_rsp *dst)
+{
+    const struct ble_att_error_rsp *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_ERROR_RSP, payload,
+                             BLE_ATT_ERROR_RSP_SZ, len);
+
+    dst->baep_req_op = src->baep_req_op;
+    dst->baep_handle = le16toh(src->baep_handle);
+    dst->baep_error_code = src->baep_error_code;
+}
+
+void
+ble_att_error_rsp_write(void *payload, int len,
+                        const struct ble_att_error_rsp *src)
+{
+    struct ble_att_error_rsp *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_ERROR_RSP, payload,
+                             BLE_ATT_ERROR_RSP_SZ, len);
+
+    dst->baep_req_op = src->baep_req_op;
+    dst->baep_handle = htole16(src->baep_handle);
+    dst->baep_error_code = src->baep_error_code;
+}
+
+void
+ble_att_mtu_req_parse(const void *payload, int len,
+                      struct ble_att_mtu_cmd *dst)
+{
+    const struct ble_att_mtu_cmd *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_MTU_REQ, payload, BLE_ATT_MTU_CMD_SZ,
+                             len);
+
+    dst->bamc_mtu = le16toh(src->bamc_mtu);
+}
+
+void
+ble_att_mtu_rsp_parse(const void *payload, int len,
+                      struct ble_att_mtu_cmd *dst)
+{
+    const struct ble_att_mtu_cmd *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_MTU_RSP, payload, BLE_ATT_MTU_CMD_SZ,
+                             len);
+
+    dst->bamc_mtu = le16toh(src->bamc_mtu);
+}
+
+void
+ble_att_mtu_req_write(void *payload, int len,
+                      const struct ble_att_mtu_cmd *src)
+{
+    struct ble_att_mtu_cmd *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_MTU_REQ, payload,
+                             BLE_ATT_MTU_CMD_SZ, len);
+
+    dst->bamc_mtu = htole16(src->bamc_mtu);
+}
+
+void
+ble_att_mtu_rsp_write(void *payload, int len,
+                      const struct ble_att_mtu_cmd *src)
+{
+    struct ble_att_mtu_cmd *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_MTU_RSP, payload,
+                             BLE_ATT_MTU_CMD_SZ, len);
+    dst->bamc_mtu = htole16(src->bamc_mtu);
+}
+
+void
+ble_att_find_info_req_parse(const void *payload, int len,
+                            struct ble_att_find_info_req *dst)
+{
+    const struct ble_att_find_info_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_REQ, payload,
+                             BLE_ATT_FIND_INFO_REQ_SZ, len);
+
+    dst->bafq_start_handle = le16toh(src->bafq_start_handle);
+    dst->bafq_end_handle = le16toh(src->bafq_end_handle);
+}
+
+void
+ble_att_find_info_req_write(void *payload, int len,
+                            const struct ble_att_find_info_req *src)
+{
+    struct ble_att_find_info_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_FIND_INFO_REQ, payload,
+                             BLE_ATT_FIND_INFO_REQ_SZ, len);
+
+    dst->bafq_start_handle = htole16(src->bafq_start_handle);
+    dst->bafq_end_handle = htole16(src->bafq_end_handle);
+}
+
+void
+ble_att_find_info_rsp_parse(const void *payload, int len,
+                            struct ble_att_find_info_rsp *dst)
+{
+    const struct ble_att_find_info_rsp *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_RSP, payload,
+                             BLE_ATT_FIND_INFO_RSP_BASE_SZ, len);
+
+    dst->bafp_format = src->bafp_format;
+}
+
+void
+ble_att_find_info_rsp_write(void *payload, int len,
+                            const struct ble_att_find_info_rsp *src)
+{
+    struct ble_att_find_info_rsp *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_FIND_INFO_RSP, payload,
+                             BLE_ATT_FIND_INFO_RSP_BASE_SZ, len);
+
+    dst->bafp_format = src->bafp_format;
+}
+
+void
+ble_att_find_type_value_req_parse(const void *payload, int len,
+                                  struct ble_att_find_type_value_req *dst)
+{
+    const struct ble_att_find_type_value_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, payload,
+                             BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, len);
+
+    dst->bavq_start_handle = le16toh(src->bavq_start_handle);
+    dst->bavq_end_handle = le16toh(src->bavq_end_handle);
+    dst->bavq_attr_type = le16toh(src->bavq_attr_type);
+}
+
+void
+ble_att_find_type_value_req_write(
+    void *payload, int len, const struct ble_att_find_type_value_req *src)
+{
+    struct ble_att_find_type_value_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, payload,
+                             BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, len);
+
+    dst->bavq_start_handle = htole16(src->bavq_start_handle);
+    dst->bavq_end_handle = htole16(src->bavq_end_handle);
+    dst->bavq_attr_type = htole16(src->bavq_attr_type);
+}
+
+void
+ble_att_read_type_req_parse(const void *payload, int len,
+                            struct ble_att_read_type_req *dst)
+{
+    const struct ble_att_read_type_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_REQ, payload,
+                             BLE_ATT_READ_TYPE_REQ_BASE_SZ, len);
+
+    dst->batq_start_handle = le16toh(src->batq_start_handle);
+    dst->batq_end_handle = le16toh(src->batq_end_handle);
+}
+
+void
+ble_att_read_type_req_write(void *payload, int len,
+                            const struct ble_att_read_type_req *src)
+{
+    struct ble_att_read_type_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_READ_TYPE_REQ, payload,
+                             BLE_ATT_READ_TYPE_REQ_BASE_SZ, len);
+
+    dst->batq_start_handle = htole16(src->batq_start_handle);
+    dst->batq_end_handle = htole16(src->batq_end_handle);
+}
+
+void
+ble_att_read_type_rsp_parse(const void *payload, int len,
+                            struct ble_att_read_type_rsp *dst)
+{
+    const struct ble_att_read_type_rsp *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_RSP, payload,
+                             BLE_ATT_READ_TYPE_RSP_BASE_SZ, len);
+
+    dst->batp_length = src->batp_length;
+}
+
+void
+ble_att_read_type_rsp_write(void *payload, int len,
+                            const struct ble_att_read_type_rsp *src)
+{
+    struct ble_att_read_type_rsp *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_READ_TYPE_RSP, payload,
+                             BLE_ATT_READ_TYPE_RSP_BASE_SZ, len);
+
+    dst->batp_length = src->batp_length;
+}
+
+void
+ble_att_read_req_parse(const void *payload, int len,
+                       struct ble_att_read_req *dst)
+{
+    const struct ble_att_read_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_READ_REQ, payload,
+                             BLE_ATT_READ_REQ_SZ, len);
+
+    dst->barq_handle = le16toh(src->barq_handle);
+}
+
+void
+ble_att_read_req_write(void *payload, int len,
+                       const struct ble_att_read_req *src)
+{
+    struct ble_att_read_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_READ_REQ, payload,
+                             BLE_ATT_READ_REQ_SZ, len);
+
+    dst->barq_handle = htole16(src->barq_handle);
+}
+
+void
+ble_att_read_blob_req_parse(const void *payload, int len,
+                            struct ble_att_read_blob_req *dst)
+{
+    const struct ble_att_read_blob_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_READ_BLOB_REQ, payload,
+                             BLE_ATT_READ_BLOB_REQ_SZ, len);
+
+    dst->babq_handle = le16toh(src->babq_handle);
+    dst->babq_offset = le16toh(src->babq_offset);
+}
+
+void
+ble_att_read_blob_req_write(void *payload, int len,
+                            const struct ble_att_read_blob_req *src)
+{
+    struct ble_att_read_blob_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_READ_BLOB_REQ, payload,
+                             BLE_ATT_READ_BLOB_REQ_SZ, len);
+
+    dst->babq_handle = htole16(src->babq_handle);
+    dst->babq_offset = htole16(src->babq_offset);
+}
+
+void
+ble_att_read_mult_req_parse(const void *payload, int len)
+{
+    ble_att_init_parse(BLE_ATT_OP_READ_MULT_REQ, payload,
+                       BLE_ATT_READ_MULT_REQ_BASE_SZ, len);
+}
+
+void
+ble_att_read_mult_req_write(void *payload, int len)
+{
+    ble_att_init_write(BLE_ATT_OP_READ_MULT_REQ, payload,
+                       BLE_ATT_READ_MULT_REQ_BASE_SZ, len);
+}
+
+void
+ble_att_read_mult_rsp_parse(const void *payload, int len)
+{
+    ble_att_init_parse(BLE_ATT_OP_READ_MULT_RSP, payload,
+                       BLE_ATT_READ_MULT_RSP_BASE_SZ, len);
+}
+
+void
+ble_att_read_mult_rsp_write(void *payload, int len)
+{
+    ble_att_init_write(BLE_ATT_OP_READ_MULT_RSP, payload,
+                       BLE_ATT_READ_MULT_RSP_BASE_SZ, len);
+}
+
+void
+ble_att_read_group_type_req_parse(const void *payload, int len,
+                                  struct ble_att_read_group_type_req *dst)
+{
+    const struct ble_att_read_group_type_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload,
+                             BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len);
+
+    dst->bagq_start_handle = le16toh(src->bagq_start_handle);
+    dst->bagq_end_handle = le16toh(src->bagq_end_handle);
+}
+
+void
+ble_att_read_group_type_req_write(
+    void *payload, int len, const struct ble_att_read_group_type_req *src)
+{
+    struct ble_att_read_group_type_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload,
+                             BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len);
+
+    dst->bagq_start_handle = htole16(src->bagq_start_handle);
+    dst->bagq_end_handle = htole16(src->bagq_end_handle);
+}
+
+void
+ble_att_read_group_type_rsp_parse(const void *payload, int len,
+                                  struct ble_att_read_group_type_rsp *dst)
+{
+    const struct ble_att_read_group_type_rsp *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload,
+                             BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len);
+
+    dst->bagp_length = src->bagp_length;
+}
+
+void
+ble_att_read_group_type_rsp_write(
+    void *payload, int len, const struct ble_att_read_group_type_rsp *src)
+{
+    struct ble_att_read_group_type_rsp *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload,
+                             BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len);
+
+    dst->bagp_length = src->bagp_length;
+}
+
+void
+ble_att_write_req_parse(const void *payload, int len,
+                        struct ble_att_write_req *dst)
+{
+    const struct ble_att_write_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_WRITE_REQ, payload,
+                             BLE_ATT_WRITE_REQ_BASE_SZ, len);
+
+    dst->bawq_handle = le16toh(src->bawq_handle);
+}
+
+void
+ble_att_write_cmd_parse(const void *payload, int len,
+                        struct ble_att_write_req *dst)
+{
+    const struct ble_att_write_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_WRITE_CMD, payload,
+                             BLE_ATT_WRITE_REQ_BASE_SZ, len);
+    dst->bawq_handle = le16toh(src->bawq_handle);
+}
+
+void
+ble_att_write_req_write(void *payload, int len,
+                        const struct ble_att_write_req *src)
+{
+    struct ble_att_write_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_WRITE_REQ, payload,
+                             BLE_ATT_WRITE_REQ_BASE_SZ, len);
+    dst->bawq_handle = htole16(src->bawq_handle);
+}
+
+void
+ble_att_write_cmd_write(void *payload, int len,
+                        const struct ble_att_write_req *src)
+{
+    struct ble_att_write_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_WRITE_CMD, payload,
+                             BLE_ATT_WRITE_REQ_BASE_SZ, len);
+    dst->bawq_handle = htole16(src->bawq_handle);
+}
+
+void
+ble_att_prep_write_req_parse(const void *payload, int len,
+                             struct ble_att_prep_write_cmd *dst)
+{
+    const struct ble_att_prep_write_cmd *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_REQ, payload,
+                             BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
+
+    dst->bapc_handle = le16toh(src->bapc_handle);
+    dst->bapc_offset = le16toh(src->bapc_offset);
+}
+
+void
+ble_att_prep_write_req_write(void *payload, int len,
+                             const struct ble_att_prep_write_cmd *src)
+{
+    struct ble_att_prep_write_cmd *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_PREP_WRITE_REQ, payload,
+                             BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
+
+    dst->bapc_handle = htole16(src->bapc_handle);
+    dst->bapc_offset = htole16(src->bapc_offset);
+}
+
+void
+ble_att_prep_write_rsp_parse(const void *payload, int len,
+                             struct ble_att_prep_write_cmd *dst)
+{
+    const struct ble_att_prep_write_cmd *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_RSP, payload,
+                             BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
+
+    dst->bapc_handle = le16toh(src->bapc_handle);
+    dst->bapc_offset = le16toh(src->bapc_offset);
+}
+
+void
+ble_att_prep_write_rsp_write(void *payload, int len,
+                             const struct ble_att_prep_write_cmd *src)
+{
+    struct ble_att_prep_write_cmd *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_PREP_WRITE_RSP, payload,
+                             BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
+
+    dst->bapc_handle = htole16(src->bapc_handle);
+    dst->bapc_offset = htole16(src->bapc_offset);
+}
+
+void
+ble_att_exec_write_req_parse(const void *payload, int len,
+                             struct ble_att_exec_write_req *dst)
+{
+    const struct ble_att_exec_write_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_REQ, payload,
+                             BLE_ATT_EXEC_WRITE_REQ_SZ, len);
+
+    dst->baeq_flags = src->baeq_flags;
+}
+
+void
+ble_att_exec_write_req_write(void *payload, int len,
+                             const struct ble_att_exec_write_req *src)
+{
+    struct ble_att_exec_write_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_EXEC_WRITE_REQ, payload,
+                             BLE_ATT_EXEC_WRITE_REQ_SZ, len);
+
+    dst->baeq_flags = src->baeq_flags;
+}
+
+void
+ble_att_exec_write_rsp_parse(const void *payload, int len)
+{
+    ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_RSP, payload,
+                       BLE_ATT_EXEC_WRITE_RSP_SZ, len);
+}
+
+void
+ble_att_exec_write_rsp_write(void *payload, int len)
+{
+    ble_att_init_write(BLE_ATT_OP_EXEC_WRITE_RSP, payload,
+                       BLE_ATT_EXEC_WRITE_RSP_SZ, len);
+}
+
+void
+ble_att_notify_req_parse(const void *payload, int len,
+                         struct ble_att_notify_req *dst)
+{
+    const struct ble_att_notify_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_NOTIFY_REQ, payload,
+                             BLE_ATT_NOTIFY_REQ_BASE_SZ, len);
+
+    dst->banq_handle = le16toh(src->banq_handle);
+}
+
+void
+ble_att_notify_req_write(void *payload, int len,
+                         const struct ble_att_notify_req *src)
+{
+    struct ble_att_notify_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_NOTIFY_REQ, payload,
+                             BLE_ATT_NOTIFY_REQ_BASE_SZ, len);
+
+    dst->banq_handle = htole16(src->banq_handle);
+}
+
+void
+ble_att_indicate_req_parse(const void *payload, int len,
+                           struct ble_att_indicate_req *dst)
+{
+    const struct ble_att_indicate_req *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_INDICATE_REQ, payload,
+                             BLE_ATT_INDICATE_REQ_BASE_SZ, len);
+
+    dst->baiq_handle = le16toh(src->baiq_handle);
+}
+
+void
+ble_att_indicate_req_write(void *payload, int len,
+                           const struct ble_att_indicate_req *src)
+{
+    struct ble_att_indicate_req *dst;
+
+    dst = ble_att_init_write(BLE_ATT_OP_INDICATE_REQ, payload,
+                             BLE_ATT_INDICATE_REQ_BASE_SZ, len);
+
+    dst->baiq_handle = htole16(src->baiq_handle);
+}
+
+void
+ble_att_indicate_rsp_parse(const void *payload, int len)
+{
+    ble_att_init_parse(BLE_ATT_OP_INDICATE_RSP, payload,
+                       BLE_ATT_INDICATE_RSP_SZ, len);
+}
+
+void
+ble_att_indicate_rsp_write(void *payload, int len)
+{
+    ble_att_init_write(BLE_ATT_OP_INDICATE_RSP, payload,
+                       BLE_ATT_INDICATE_RSP_SZ, len);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_att_cmd_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_att_cmd_priv.h
new file mode 100644
index 00000000..70f33260
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_att_cmd_priv.h
@@ -0,0 +1,449 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_ATT_CMD_
+#define H_BLE_ATT_CMD_
+
+#include <inttypes.h>
+#include <stddef.h>
+#include "os/os_mbuf.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_att_hdr {
+    uint8_t opcode;
+    uint8_t data[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Request Opcode In Error            | 1                 |
+ * | Attribute Handle In Error          | 2                 |
+ * | Error Code                         | 1                 |
+ */
+#define BLE_ATT_ERROR_RSP_SZ             5
+struct ble_att_error_rsp {
+    uint8_t baep_req_op;
+    uint16_t baep_handle;
+    uint8_t baep_error_code;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Server Rx MTU                      | 2                 |
+ */
+#define BLE_ATT_MTU_CMD_SZ               3
+struct ble_att_mtu_cmd {
+    uint16_t bamc_mtu;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Starting Handle                    | 2                 |
+ * | Ending Handle                      | 2                 |
+ */
+#define BLE_ATT_FIND_INFO_REQ_SZ         5
+struct ble_att_find_info_req {
+    uint16_t bafq_start_handle;
+    uint16_t bafq_end_handle;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Format                             | 1                 |
+ * | Information Data                   | 4 to (ATT_MTU-2)  |
+ */
+#define BLE_ATT_FIND_INFO_RSP_BASE_SZ       2
+struct ble_att_find_info_rsp {
+    uint8_t bafp_format;
+    /* Followed by information data. */
+} __attribute__((packed));
+
+#define BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT   1
+#define BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT  2
+
+#define BLE_ATT_FIND_INFO_IDATA_16_SZ           4
+#define BLE_ATT_FIND_INFO_IDATA_128_SZ          18
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Starting Handle                    | 2                 |
+ * | Ending Handle                      | 2                 |
+ * | Attribute Type                     | 2                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-7)  |
+ */
+#define BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ   7
+struct ble_att_find_type_value_req {
+    uint16_t bavq_start_handle;
+    uint16_t bavq_end_handle;
+    uint16_t bavq_attr_type;
+    uint16_t bavq_value[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Information Data                   | 4 to (ATT_MTU-1)  |
+ */
+#define BLE_ATT_FIND_TYPE_VALUE_RSP_BASE_SZ     1
+#define BLE_ATT_FIND_TYPE_VALUE_HINFO_BASE_SZ   4
+
+struct ble_att_handle_group {
+        uint16_t attr_handle;
+        uint16_t group_end_handle;
+} __attribute__((packed));
+
+struct ble_att_find_type_value_rsp {
+    struct ble_att_handle_group list[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Starting Handle                    | 2                 |
+ * | Ending Handle                      | 2                 |
+ * | Attribute Type                     | 2 or 16           |
+ */
+#define BLE_ATT_READ_TYPE_REQ_BASE_SZ   5
+#define BLE_ATT_READ_TYPE_REQ_SZ_16     7
+#define BLE_ATT_READ_TYPE_REQ_SZ_128    21
+struct ble_att_read_type_req {
+    uint16_t batq_start_handle;
+    uint16_t batq_end_handle;
+    uint8_t uuid[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Length                             | 1                 |
+ * | Attribute Data List                | 2 to (ATT_MTU-2)  |
+ */
+#define BLE_ATT_READ_TYPE_RSP_BASE_SZ       2
+
+struct ble_att_attr_data_list {
+    uint16_t handle;
+    uint8_t value[0];
+} __attribute__((packed));
+
+struct ble_att_read_type_rsp {
+    uint8_t batp_length;
+    struct ble_att_attr_data_list batp_list[0];
+} __attribute__((packed));
+
+#define BLE_ATT_READ_TYPE_ADATA_BASE_SZ     2
+#define BLE_ATT_READ_TYPE_ADATA_SZ_16       6
+#define BLE_ATT_READ_TYPE_ADATA_SZ_128      20
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ */
+#define BLE_ATT_READ_REQ_SZ              3
+struct ble_att_read_req {
+    uint16_t barq_handle;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-1)  |
+ */
+#define BLE_ATT_READ_RSP_BASE_SZ        1
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Value Offset                       | 2                 |
+ */
+#define BLE_ATT_READ_BLOB_REQ_SZ        5
+struct ble_att_read_blob_req {
+    uint16_t babq_handle;
+    uint16_t babq_offset;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-1)  |
+ */
+#define BLE_ATT_READ_BLOB_RSP_BASE_SZ   1
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Set Of Handles                     | 4 to (ATT_MTU-1)  |
+ */
+#define BLE_ATT_READ_MULT_REQ_BASE_SZ   1
+struct ble_att_read_mult_req {
+        uint16_t handles[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Set Of Values                      | 4 to (ATT_MTU-1)  |
+ */
+#define BLE_ATT_READ_MULT_RSP_BASE_SZ   1
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Starting Handle                    | 2                 |
+ * | Ending Handle                      | 2                 |
+ * | Attribute Group Type               | 2 or 16           |
+ */
+#define BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ  5
+#define BLE_ATT_READ_GROUP_TYPE_REQ_SZ_16    7
+#define BLE_ATT_READ_GROUP_TYPE_REQ_SZ_128   21
+struct ble_att_read_group_type_req {
+    uint16_t bagq_start_handle;
+    uint16_t bagq_end_handle;
+    uint8_t uuid[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Length                             | 1                 |
+ * | Attribute Data List                | 2 to (ATT_MTU-2)  |
+ */
+#define BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ  2
+struct ble_att_read_group_type_rsp {
+    uint8_t bagp_length;
+} __attribute__((packed));
+
+#define BLE_ATT_READ_GROUP_TYPE_ADATA_BASE_SZ   4
+#define BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16     6
+#define BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128    20
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-3)  |
+ */
+#define BLE_ATT_WRITE_REQ_BASE_SZ       3
+struct ble_att_write_req {
+    uint16_t bawq_handle;
+    uint8_t value[0];
+} __attribute__((packed));
+
+#define BLE_ATT_WRITE_RSP_SZ            1
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Value Offset                       | 2                 |
+ * | Part Attribute Value               | 0 to (ATT_MTU-5)  |
+ */
+#define BLE_ATT_PREP_WRITE_CMD_BASE_SZ  5
+struct ble_att_prep_write_cmd {
+    uint16_t bapc_handle;
+    uint16_t bapc_offset;
+    uint16_t bapc_value[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Flags                              | 1                 |
+ */
+#define BLE_ATT_EXEC_WRITE_REQ_SZ       2
+struct ble_att_exec_write_req {
+    uint8_t baeq_flags;
+} __attribute__((packed));
+
+#define BLE_ATT_EXEC_WRITE_F_CANCEL     0x00
+#define BLE_ATT_EXEC_WRITE_F_EXECUTE    0x01
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ */
+#define BLE_ATT_EXEC_WRITE_RSP_SZ       1
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-3)  |
+ */
+#define BLE_ATT_NOTIFY_REQ_BASE_SZ      3
+struct ble_att_notify_req {
+    uint16_t banq_handle;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-3)  |
+ */
+#define BLE_ATT_INDICATE_REQ_BASE_SZ    3
+struct ble_att_indicate_req {
+    uint16_t baiq_handle;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ */
+#define BLE_ATT_INDICATE_RSP_SZ         1
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-3)  |
+ */
+#define BLE_ATT_WRITE_CMD_BASE_SZ       3
+struct ble_att_write_cmd {
+    uint16_t handle;
+    uint8_t value[0];
+} __attribute__((packed));
+
+void ble_att_error_rsp_parse(const void *payload, int len,
+                             struct ble_att_error_rsp *rsp);
+void ble_att_error_rsp_write(void *payload, int len,
+                             const struct ble_att_error_rsp *rsp);
+void ble_att_mtu_req_parse(const void *payload, int len,
+                           struct ble_att_mtu_cmd *cmd);
+void ble_att_mtu_req_write(void *payload, int len,
+                           const struct ble_att_mtu_cmd *cmd);
+void ble_att_mtu_rsp_parse(const void *payload, int len,
+                           struct ble_att_mtu_cmd *cmd);
+void ble_att_mtu_rsp_write(void *payload, int len,
+                           const struct ble_att_mtu_cmd *cmd);
+void ble_att_find_info_req_parse(const void *payload, int len,
+                                 struct ble_att_find_info_req *req);
+void ble_att_find_info_req_write(void *payload, int len,
+                                 const struct ble_att_find_info_req *req);
+void ble_att_find_info_rsp_parse(const void *payload, int len,
+                                 struct ble_att_find_info_rsp *rsp);
+void ble_att_find_info_rsp_write(void *payload, int len,
+                                 const struct ble_att_find_info_rsp *rsp);
+void ble_att_find_type_value_req_parse(
+    const void *payload, int len, struct ble_att_find_type_value_req *req);
+void ble_att_find_type_value_req_write(
+    void *payload, int len, const struct ble_att_find_type_value_req *req);
+void ble_att_read_type_req_parse(const void *payload, int len,
+                                 struct ble_att_read_type_req *req);
+void ble_att_read_type_req_write(void *payload, int len,
+                                 const struct ble_att_read_type_req *req);
+void ble_att_read_type_rsp_parse(const void *payload, int len,
+                                 struct ble_att_read_type_rsp *rsp);
+void ble_att_read_type_rsp_write(void *payload, int len,
+                                 const struct ble_att_read_type_rsp *rsp);
+void ble_att_read_req_parse(const void *payload, int len,
+                            struct ble_att_read_req *req);
+void ble_att_read_req_write(void *payload, int len,
+                            const struct ble_att_read_req *req);
+void ble_att_read_blob_req_parse(const void *payload, int len,
+                                 struct ble_att_read_blob_req *req);
+void ble_att_read_blob_req_write(void *payload, int len,
+                                 const struct ble_att_read_blob_req *req);
+void ble_att_read_mult_req_parse(const void *payload, int len);
+void ble_att_read_mult_req_write(void *payload, int len);
+void ble_att_read_mult_rsp_parse(const void *payload, int len);
+void ble_att_read_mult_rsp_write(void *payload, int len);
+void ble_att_read_group_type_req_parse(
+    const void *payload, int len, struct ble_att_read_group_type_req *req);
+void ble_att_read_group_type_req_write(
+    void *payload, int len, const struct ble_att_read_group_type_req *req);
+void ble_att_read_group_type_rsp_parse(
+    const void *payload, int len, struct ble_att_read_group_type_rsp *rsp);
+void ble_att_read_group_type_rsp_write(
+    void *payload, int len, const struct ble_att_read_group_type_rsp *rsp);
+void ble_att_write_req_parse(const void *payload, int len,
+                             struct ble_att_write_req *req);
+void ble_att_write_req_write(void *payload, int len,
+                             const struct ble_att_write_req *req);
+void ble_att_write_cmd_parse(const void *payload, int len,
+                             struct ble_att_write_req *req);
+void ble_att_write_cmd_write(void *payload, int len,
+                             const struct ble_att_write_req *req);
+void ble_att_prep_write_req_parse(const void *payload, int len,
+                                  struct ble_att_prep_write_cmd *cmd);
+void ble_att_prep_write_req_write(void *payload, int len,
+                                  const struct ble_att_prep_write_cmd *cmd);
+void ble_att_prep_write_rsp_parse(const void *payload, int len,
+                                  struct ble_att_prep_write_cmd *cmd);
+void ble_att_prep_write_rsp_write(void *payload, int len,
+                                  const struct ble_att_prep_write_cmd *cmd);
+void ble_att_exec_write_req_parse(const void *payload, int len,
+                                  struct ble_att_exec_write_req *req);
+void ble_att_exec_write_req_write(void *payload, int len,
+                                  const struct ble_att_exec_write_req *req);
+void ble_att_exec_write_rsp_parse(const void *payload, int len);
+void ble_att_exec_write_rsp_write(void *payload, int len);
+void ble_att_notify_req_parse(const void *payload, int len,
+                              struct ble_att_notify_req *req);
+void ble_att_notify_req_write(void *payload, int len,
+                              const struct ble_att_notify_req *req);
+void ble_att_indicate_req_parse(const void *payload, int len,
+                                struct ble_att_indicate_req *req);
+void ble_att_indicate_req_write(void *payload, int len,
+                                const struct ble_att_indicate_req *req);
+void ble_att_indicate_rsp_parse(const void *payload, int len);
+void ble_att_indicate_rsp_write(void *payload, int len);
+
+void *ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom);
+void *ble_att_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom);
+int ble_att_tx(uint16_t conn_handle, struct os_mbuf *txom);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_att_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_att_priv.h
new file mode 100644
index 00000000..a2a9f979
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_att_priv.h
@@ -0,0 +1,300 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_ATT_PRIV_
+#define H_BLE_ATT_PRIV_
+
+#include <inttypes.h>
+#include "stats/stats.h"
+#include "host/ble_att.h"
+#include "host/ble_uuid.h"
+#include "nimble/nimble_npl.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+struct ble_hs_conn;
+struct ble_l2cap_chan;
+struct ble_att_find_info_req;
+struct ble_att_error_rsp;
+struct ble_att_mtu_cmd;
+struct ble_att_read_req;
+struct ble_att_read_blob_req;
+struct ble_att_read_type_req;
+struct ble_att_read_group_type_req;
+struct ble_att_read_group_type_rsp;
+struct ble_att_find_type_value_req;
+struct ble_att_write_req;
+struct ble_att_prep_write_cmd;
+struct ble_att_exec_write_req;
+struct ble_att_notify_req;
+struct ble_att_indicate_req;
+
+STATS_SECT_START(ble_att_stats)
+    STATS_SECT_ENTRY(error_rsp_rx)
+    STATS_SECT_ENTRY(error_rsp_tx)
+    STATS_SECT_ENTRY(mtu_req_rx)
+    STATS_SECT_ENTRY(mtu_req_tx)
+    STATS_SECT_ENTRY(mtu_rsp_rx)
+    STATS_SECT_ENTRY(mtu_rsp_tx)
+    STATS_SECT_ENTRY(find_info_req_rx)
+    STATS_SECT_ENTRY(find_info_req_tx)
+    STATS_SECT_ENTRY(find_info_rsp_rx)
+    STATS_SECT_ENTRY(find_info_rsp_tx)
+    STATS_SECT_ENTRY(find_type_value_req_rx)
+    STATS_SECT_ENTRY(find_type_value_req_tx)
+    STATS_SECT_ENTRY(find_type_value_rsp_rx)
+    STATS_SECT_ENTRY(find_type_value_rsp_tx)
+    STATS_SECT_ENTRY(read_type_req_rx)
+    STATS_SECT_ENTRY(read_type_req_tx)
+    STATS_SECT_ENTRY(read_type_rsp_rx)
+    STATS_SECT_ENTRY(read_type_rsp_tx)
+    STATS_SECT_ENTRY(read_req_rx)
+    STATS_SECT_ENTRY(read_req_tx)
+    STATS_SECT_ENTRY(read_rsp_rx)
+    STATS_SECT_ENTRY(read_rsp_tx)
+    STATS_SECT_ENTRY(read_blob_req_rx)
+    STATS_SECT_ENTRY(read_blob_req_tx)
+    STATS_SECT_ENTRY(read_blob_rsp_rx)
+    STATS_SECT_ENTRY(read_blob_rsp_tx)
+    STATS_SECT_ENTRY(read_mult_req_rx)
+    STATS_SECT_ENTRY(read_mult_req_tx)
+    STATS_SECT_ENTRY(read_mult_rsp_rx)
+    STATS_SECT_ENTRY(read_mult_rsp_tx)
+    STATS_SECT_ENTRY(read_group_type_req_rx)
+    STATS_SECT_ENTRY(read_group_type_req_tx)
+    STATS_SECT_ENTRY(read_group_type_rsp_rx)
+    STATS_SECT_ENTRY(read_group_type_rsp_tx)
+    STATS_SECT_ENTRY(write_req_rx)
+    STATS_SECT_ENTRY(write_req_tx)
+    STATS_SECT_ENTRY(write_rsp_rx)
+    STATS_SECT_ENTRY(write_rsp_tx)
+    STATS_SECT_ENTRY(prep_write_req_rx)
+    STATS_SECT_ENTRY(prep_write_req_tx)
+    STATS_SECT_ENTRY(prep_write_rsp_rx)
+    STATS_SECT_ENTRY(prep_write_rsp_tx)
+    STATS_SECT_ENTRY(exec_write_req_rx)
+    STATS_SECT_ENTRY(exec_write_req_tx)
+    STATS_SECT_ENTRY(exec_write_rsp_rx)
+    STATS_SECT_ENTRY(exec_write_rsp_tx)
+    STATS_SECT_ENTRY(notify_req_rx)
+    STATS_SECT_ENTRY(notify_req_tx)
+    STATS_SECT_ENTRY(indicate_req_rx)
+    STATS_SECT_ENTRY(indicate_req_tx)
+    STATS_SECT_ENTRY(indicate_rsp_rx)
+    STATS_SECT_ENTRY(indicate_rsp_tx)
+    STATS_SECT_ENTRY(write_cmd_rx)
+    STATS_SECT_ENTRY(write_cmd_tx)
+STATS_SECT_END
+extern STATS_SECT_DECL(ble_att_stats) ble_att_stats;
+
+struct ble_att_prep_entry {
+    SLIST_ENTRY(ble_att_prep_entry) bape_next;
+    uint16_t bape_handle;
+    uint16_t bape_offset;
+
+    /* XXX: This is wasteful; we should use one mbuf chain for the entire
+     * prepared write, and compress the data into as few mbufs as possible.
+     */
+    struct os_mbuf *bape_value;
+};
+
+SLIST_HEAD(ble_att_prep_entry_list, ble_att_prep_entry);
+
+struct ble_att_svr_conn {
+    /** This list is sorted by attribute handle ID. */
+    struct ble_att_prep_entry_list basc_prep_list;
+    ble_npl_time_t basc_prep_timeout_at;
+};
+
+/**
+ * Handles a host attribute request.
+ *
+ * @param entry                 The host attribute being requested.
+ * @param op                    The operation being performed on the attribute.
+ * @param arg                   The request data associated with that host
+ *                                  attribute.
+ *
+ * @return                      0 on success;
+ *                              One of the BLE_ATT_ERR_[...] codes on
+ *                                  failure.
+ */
+typedef int ble_att_svr_access_fn(uint16_t conn_handle, uint16_t attr_handle,
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg);
+
+int ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags,
+                         uint8_t min_key_size, uint16_t *handle_id,
+                         ble_att_svr_access_fn *cb, void *cb_arg);
+
+struct ble_att_svr_entry {
+    STAILQ_ENTRY(ble_att_svr_entry) ha_next;
+
+    const ble_uuid_t *ha_uuid;
+    uint8_t ha_flags;
+    uint8_t ha_min_key_size;
+    uint16_t ha_handle_id;
+    ble_att_svr_access_fn *ha_cb;
+    void *ha_cb_arg;
+};
+
+SLIST_HEAD(ble_att_clt_entry_list, ble_att_clt_entry);
+
+/*** @gen */
+
+struct ble_l2cap_chan *ble_att_create_chan(uint16_t conn_handle);
+int ble_att_conn_chan_find(uint16_t conn_handle, struct ble_hs_conn **out_conn,
+                           struct ble_l2cap_chan **out_chan);
+void ble_att_inc_tx_stat(uint8_t att_op);
+void ble_att_truncate_to_mtu(const struct ble_l2cap_chan *att_chan,
+                             struct os_mbuf *txom);
+void ble_att_set_peer_mtu(struct ble_l2cap_chan *chan, uint16_t peer_mtu);
+uint16_t ble_att_chan_mtu(const struct ble_l2cap_chan *chan);
+int ble_att_init(void);
+
+/*** @svr */
+
+int ble_att_svr_start(void);
+
+struct ble_att_svr_entry *
+ble_att_svr_find_by_uuid(struct ble_att_svr_entry *start_at,
+                         const ble_uuid_t *uuid,
+                         uint16_t end_handle);
+uint16_t ble_att_svr_prev_handle(void);
+int ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom);
+struct ble_att_svr_entry *ble_att_svr_find_by_handle(uint16_t handle_id);
+int32_t ble_att_svr_ticks_until_tmo(const struct ble_att_svr_conn *svr,
+                                    ble_npl_time_t now);
+int ble_att_svr_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_svr_rx_find_type_value(uint16_t conn_handle,
+                                   struct os_mbuf **rxom);
+int ble_att_svr_rx_read_type(uint16_t conn_handle,
+                             struct os_mbuf **rxom);
+int ble_att_svr_rx_read_group_type(uint16_t conn_handle,
+                                   struct os_mbuf **rxom);
+int ble_att_svr_rx_read(uint16_t conn_handle,
+                        struct os_mbuf **rxom);
+int ble_att_svr_rx_read_blob(uint16_t conn_handle,
+                             struct os_mbuf **rxom);
+int ble_att_svr_rx_read_mult(uint16_t conn_handle,
+                             struct os_mbuf **rxom);
+int ble_att_svr_rx_write(uint16_t conn_handle,
+                         struct os_mbuf **rxom);
+int ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_svr_rx_prep_write(uint16_t conn_handle,
+                              struct os_mbuf **rxom);
+int ble_att_svr_rx_exec_write(uint16_t conn_handle,
+                              struct os_mbuf **rxom);
+int ble_att_svr_rx_notify(uint16_t conn_handle,
+                          struct os_mbuf **rxom);
+int ble_att_svr_rx_indicate(uint16_t conn_handle,
+                            struct os_mbuf **rxom);
+void ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list);
+int ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
+                            uint16_t offset, struct os_mbuf *om,
+                            uint8_t *out_att_err);
+void ble_att_svr_reset(void);
+int ble_att_svr_init(void);
+
+void ble_att_svr_hide_range(uint16_t start_handle, uint16_t end_handle);
+void ble_att_svr_restore_range(uint16_t start_handle, uint16_t end_handle);
+
+int ble_att_svr_tx_error_rsp(uint16_t conn_handle, struct os_mbuf *txom,
+                             uint8_t req_op, uint16_t handle,
+                             uint8_t error_code);
+/*** $clt */
+
+/** An information-data entry in a find information response. */
+struct ble_att_find_info_idata {
+    uint16_t attr_handle;
+    ble_uuid_any_t uuid;
+};
+
+/** A handles-information entry in a find by type value response. */
+struct ble_att_find_type_value_hinfo {
+    uint16_t attr_handle;
+    uint16_t group_end_handle;
+};
+
+/** An attribute-data entry in a read by type response. */
+struct ble_att_read_type_adata {
+    uint16_t att_handle;
+    int value_len;
+    uint8_t *value;
+
+};
+
+/** An attribute-data entry in a read by group type response. */
+struct ble_att_read_group_type_adata {
+    uint16_t att_handle;
+    uint16_t end_group_handle;
+    int value_len;
+    uint8_t *value;
+};
+
+int ble_att_clt_rx_error(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu);
+int ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_read(uint16_t conn_handle, uint16_t handle);
+int ble_att_clt_rx_read(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_read_blob(uint16_t conn_handle, uint16_t handle,
+                             uint16_t offset);
+int ble_att_clt_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_read_mult(uint16_t conn_handle,
+                             const uint16_t *handles, int num_handles);
+int ble_att_clt_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_read_type(uint16_t conn_handle, uint16_t start_handle,
+                             uint16_t end_handle, const ble_uuid_t *uuid);
+int ble_att_clt_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_read_group_type(uint16_t conn_handle,
+                                   uint16_t start_handle, uint16_t end_handle,
+                                   const ble_uuid_t *uuid128);
+int ble_att_clt_rx_read_group_type(uint16_t conn_handle,
+                                   struct os_mbuf **rxom);
+int ble_att_clt_tx_find_info(uint16_t conn_handle, uint16_t start_handle,
+                             uint16_t end_handle);
+int ble_att_clt_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_find_type_value(uint16_t conn_handle, uint16_t start_handle,
+                                   uint16_t end_handle, uint16_t attribute_type,
+                                   const void *attribute_value, int value_len);
+int ble_att_clt_rx_find_type_value(uint16_t conn_handle,
+                                   struct os_mbuf **rxom);
+int ble_att_clt_tx_write_req(uint16_t conn_handle, uint16_t handle,
+                             struct os_mbuf *txom);
+int ble_att_clt_tx_write_cmd(uint16_t conn_handle, uint16_t handle,
+                             struct os_mbuf *txom);
+int ble_att_clt_tx_prep_write(uint16_t conn_handle, uint16_t handle,
+                              uint16_t offset, struct os_mbuf *txom);
+int ble_att_clt_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_exec_write(uint16_t conn_handle, uint8_t flags);
+int ble_att_clt_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_rx_write(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle,
+                          struct os_mbuf *txom);
+int ble_att_clt_tx_indicate(uint16_t conn_handle, uint16_t handle,
+                            struct os_mbuf *txom);
+int ble_att_clt_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_att_svr.c b/src/libs/mynewt-nimble/nimble/host/src/ble_att_svr.c
new file mode 100644
index 00000000..46a71681
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_att_svr.c
@@ -0,0 +1,2729 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_priv.h"
+
+/**
+ * ATT server - Attribute Protocol
+ *
+ * Notes on buffer reuse:
+ * Most request handlers reuse the request buffer for the reponse.  This is
+ * done to prevent out-of-memory conditions.  However, there are two handlers
+ * which do not reuse the request buffer:
+ *     1. Write request.
+ *     2. Indicate request.
+ *
+ * Both of these handlers attempt to allocate a new buffer for the response
+ * prior to processing the request.  If allocation fails, the request is not
+ * processed, and the request buffer is reused for the transmission of an
+ * "insufficient resources" ATT error response.  These handlers don't reuse the
+ * request mbuf for an affirmative response because the buffer contains the
+ * attribute data that gets passed to the application callback.  The
+ * application may choose to retain the mbuf during the callback, so the stack
+ */
+
+STAILQ_HEAD(ble_att_svr_entry_list, ble_att_svr_entry);
+static struct ble_att_svr_entry_list ble_att_svr_list;
+static struct ble_att_svr_entry_list ble_att_svr_hidden_list;
+
+static uint16_t ble_att_svr_id;
+
+static void *ble_att_svr_entry_mem;
+static struct os_mempool ble_att_svr_entry_pool;
+
+static os_membuf_t ble_att_svr_prep_entry_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
+                    sizeof (struct ble_att_prep_entry))
+];
+
+static struct os_mempool ble_att_svr_prep_entry_pool;
+
+static struct ble_att_svr_entry *
+ble_att_svr_entry_alloc(void)
+{
+    struct ble_att_svr_entry *entry;
+
+    entry = os_memblock_get(&ble_att_svr_entry_pool);
+    if (entry != NULL) {
+        memset(entry, 0, sizeof *entry);
+    }
+
+    return entry;
+}
+
+static void
+ble_att_svr_entry_free(struct ble_att_svr_entry *entry)
+{
+    os_memblock_put(&ble_att_svr_entry_pool, entry);
+}
+
+/**
+ * Allocate the next handle id and return it.
+ *
+ * @return A new 16-bit handle ID.
+ */
+static uint16_t
+ble_att_svr_next_id(void)
+{
+    /* Rollover is fatal. */
+    BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX);
+    return ++ble_att_svr_id;
+}
+
+/**
+ * Register a host attribute with the BLE stack.
+ *
+ * @param ha                    A filled out ble_att structure to register
+ * @param handle_id             A pointer to a 16-bit handle ID, which will be
+ *                                  the handle that is allocated.
+ * @param fn                    The callback function that gets executed when
+ *                                  the attribute is operated on.
+ *
+ * @return 0 on success, non-zero error code on failure.
+ */
+int
+ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags,
+                     uint8_t min_key_size, uint16_t *handle_id,
+                     ble_att_svr_access_fn *cb, void *cb_arg)
+{
+    struct ble_att_svr_entry *entry;
+
+    entry = ble_att_svr_entry_alloc();
+    if (entry == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    entry->ha_uuid = uuid;
+    entry->ha_flags = flags;
+    entry->ha_min_key_size = min_key_size;
+    entry->ha_handle_id = ble_att_svr_next_id();
+    entry->ha_cb = cb;
+    entry->ha_cb_arg = cb_arg;
+
+    STAILQ_INSERT_TAIL(&ble_att_svr_list, entry, ha_next);
+
+    if (handle_id != NULL) {
+        *handle_id = entry->ha_handle_id;
+    }
+
+    return 0;
+}
+
+uint16_t
+ble_att_svr_prev_handle(void)
+{
+    return ble_att_svr_id;
+}
+
+/**
+ * Find a host attribute by handle id.
+ *
+ * @param handle_id             The handle_id to search for
+ * @param ha_ptr                On input: Indicates the starting point of the
+ *                                  walk; null means start at the beginning of
+ *                                  the list, non-null means start at the
+ *                                  following entry.
+ *                              On output: Indicates the last ble_att element
+ *                                  processed, or NULL if the entire list has
+ *                                  been processed.
+ *
+ * @return                      0 on success; BLE_HS_ENOENT on not found.
+ */
+struct ble_att_svr_entry *
+ble_att_svr_find_by_handle(uint16_t handle_id)
+{
+    struct ble_att_svr_entry *entry;
+
+    for (entry = STAILQ_FIRST(&ble_att_svr_list);
+         entry != NULL;
+         entry = STAILQ_NEXT(entry, ha_next)) {
+
+        if (entry->ha_handle_id == handle_id) {
+            return entry;
+        }
+    }
+
+    return NULL;
+}
+
+/**
+ * Find a host attribute by UUID.
+ *
+ * @param uuid                  The ble_uuid_t to search for; null means
+ *                                  find any type of attribute.
+ * @param prev                  On input: Indicates the starting point of the
+ *                                  walk; null means start at the beginning of
+ *                                  the list, non-null means start at the
+ *                                  following entry.
+ *                              On output: Indicates the last ble_att element
+ *                                  processed, or NULL if the entire list has
+ *                                  been processed.
+ *
+ * @return                      0 on success; BLE_HS_ENOENT on not found.
+ */
+struct ble_att_svr_entry *
+ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, const ble_uuid_t *uuid,
+                         uint16_t end_handle)
+{
+    struct ble_att_svr_entry *entry;
+
+    if (prev == NULL) {
+        entry = STAILQ_FIRST(&ble_att_svr_list);
+    } else {
+        entry = STAILQ_NEXT(prev, ha_next);
+    }
+
+    for (;
+         entry != NULL && entry->ha_handle_id <= end_handle;
+         entry = STAILQ_NEXT(entry, ha_next)) {
+
+        if (uuid == NULL || ble_uuid_cmp(entry->ha_uuid, uuid) == 0) {
+            return entry;
+        }
+    }
+
+    return NULL;
+}
+
+static int
+ble_att_svr_pullup_req_base(struct os_mbuf **om, int base_len,
+                            uint8_t *out_att_err)
+{
+    uint8_t att_err;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(om, base_len);
+    if (rc == BLE_HS_ENOMEM) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+    } else {
+        att_err = 0;
+    }
+
+    if (out_att_err != NULL) {
+        *out_att_err = att_err;
+    }
+
+    return rc;
+}
+
+static void
+ble_att_svr_get_sec_state(uint16_t conn_handle,
+                          struct ble_gap_sec_state *out_sec_state)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find_assert(conn_handle);
+    *out_sec_state = conn->bhc_sec_state;
+
+    ble_hs_unlock();
+}
+
+static int
+ble_att_svr_check_perms(uint16_t conn_handle, int is_read,
+                        struct ble_att_svr_entry *entry,
+                        uint8_t *out_att_err)
+{
+    struct ble_gap_sec_state sec_state;
+    struct ble_store_value_sec value_sec;
+    struct ble_store_key_sec key_sec;
+    struct ble_hs_conn_addrs addrs;
+    struct ble_hs_conn *conn;
+    int author;
+    int authen;
+    int enc;
+    int rc;
+
+    if (is_read) {
+        if (!(entry->ha_flags & BLE_ATT_F_READ)) {
+            *out_att_err = BLE_ATT_ERR_READ_NOT_PERMITTED;
+            return BLE_HS_EREJECT;
+        }
+
+        enc = entry->ha_flags & BLE_ATT_F_READ_ENC;
+        authen = entry->ha_flags & BLE_ATT_F_READ_AUTHEN;
+        author = entry->ha_flags & BLE_ATT_F_READ_AUTHOR;
+    } else {
+        if (!(entry->ha_flags & BLE_ATT_F_WRITE)) {
+            *out_att_err = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
+            return BLE_HS_EREJECT;
+        }
+
+        enc = entry->ha_flags & BLE_ATT_F_WRITE_ENC;
+        authen = entry->ha_flags & BLE_ATT_F_WRITE_AUTHEN;
+        author = entry->ha_flags & BLE_ATT_F_WRITE_AUTHOR;
+    }
+
+    /* Bail early if this operation doesn't require security. */
+    if (!enc && !authen && !author) {
+        return 0;
+    }
+
+    ble_att_svr_get_sec_state(conn_handle, &sec_state);
+    if ((enc || authen) && !sec_state.encrypted) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find(conn_handle);
+        if (conn != NULL) {
+            ble_hs_conn_addrs(conn, &addrs);
+
+            memset(&key_sec, 0, sizeof key_sec);
+            key_sec.peer_addr = addrs.peer_id_addr;
+        }
+        ble_hs_unlock();
+
+        rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+        if (rc == 0 && value_sec.ltk_present) {
+            *out_att_err = BLE_ATT_ERR_INSUFFICIENT_ENC;
+        } else {
+            *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
+        }
+
+        return BLE_HS_ATT_ERR(*out_att_err);
+    }
+
+    if (authen && !sec_state.authenticated) {
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
+        return BLE_HS_ATT_ERR(*out_att_err);
+    }
+
+    if (entry->ha_min_key_size > sec_state.key_size) {
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_KEY_SZ;
+        return BLE_HS_ATT_ERR(*out_att_err);
+    }
+
+    if (author) {
+        /* XXX: Prompt user for authorization. */
+    }
+
+    return 0;
+}
+
+/**
+ * Calculates the number of ticks until a queued write times out on the
+ * specified ATT server.  If this server is not in the process of receiving a
+ * queued write, then BLE_HS_FOREVER is returned.  If a timeout just occurred,
+ * 0 is returned.
+ *
+ * @param svr                   The ATT server to check.
+ * @param now                   The current OS time.
+ *
+ * @return                      The number of ticks until the current queued
+ *                                  write times out.
+ */
+int32_t
+ble_att_svr_ticks_until_tmo(const struct ble_att_svr_conn *svr, ble_npl_time_t now)
+{
+#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO == 0
+    return BLE_HS_FOREVER;
+#endif
+
+    int32_t time_diff;
+
+    if (SLIST_EMPTY(&svr->basc_prep_list)) {
+        return BLE_HS_FOREVER;
+    }
+
+    time_diff = svr->basc_prep_timeout_at - now;
+    if (time_diff < 0) {
+        return 0;
+    }
+
+    return time_diff;
+}
+
+/**
+ * Allocates an mbuf to be used for an ATT response.  If an mbuf cannot be
+ * allocated, the received request mbuf is reused for the error response.
+ */
+static int
+ble_att_svr_pkt(struct os_mbuf **rxom, struct os_mbuf **out_txom,
+                uint8_t *out_att_err)
+{
+    *out_txom = ble_hs_mbuf_l2cap_pkt();
+    if (*out_txom != NULL) {
+        return 0;
+    }
+
+    /* Allocation failure.  Reuse receive buffer for response. */
+    *out_txom = *rxom;
+    *rxom = NULL;
+    *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+    return BLE_HS_ENOMEM;
+}
+
+static int
+ble_att_svr_read(uint16_t conn_handle,
+                 struct ble_att_svr_entry *entry,
+                 uint16_t offset,
+                 struct os_mbuf *om,
+                 uint8_t *out_att_err)
+{
+    uint8_t att_err;
+    int rc;
+
+    att_err = 0;    /* Silence gcc warning. */
+
+    if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+        rc = ble_att_svr_check_perms(conn_handle, 1, entry, &att_err);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+
+    BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
+    rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
+                      BLE_ATT_ACCESS_OP_READ, offset, &om, entry->ha_cb_arg);
+    if (rc != 0) {
+        att_err = rc;
+        rc = BLE_HS_EAPP;
+        goto err;
+    }
+
+    return 0;
+
+err:
+    if (out_att_err != NULL) {
+        *out_att_err = att_err;
+    }
+    return rc;
+}
+
+static int
+ble_att_svr_read_flat(uint16_t conn_handle,
+                      struct ble_att_svr_entry *entry,
+                      uint16_t offset,
+                      uint16_t max_len,
+                      void *dst,
+                      uint16_t *out_len,
+                      uint8_t *out_att_err)
+{
+    struct os_mbuf *om;
+    uint16_t len;
+    int rc;
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    len = OS_MBUF_PKTLEN(om);
+    if (len > max_len) {
+        rc = BLE_HS_EMSGSIZE;
+        *out_att_err = BLE_ATT_ERR_UNLIKELY;
+        goto done;
+    }
+
+    rc = os_mbuf_copydata(om, 0, len, dst);
+    BLE_HS_DBG_ASSERT(rc == 0);
+
+    *out_len = len;
+    rc = 0;
+
+done:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
+int
+ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
+                        uint16_t offset, struct os_mbuf *om,
+                        uint8_t *out_att_err)
+{
+    struct ble_att_svr_entry *entry;
+    int rc;
+
+    entry = ble_att_svr_find_by_handle(attr_handle);
+    if (entry == NULL) {
+        if (out_att_err != NULL) {
+            *out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        }
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_bare_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, om,
+                                 NULL);
+    if (rc != 0) {
+        goto err;
+    }
+
+    *out_om = om;
+    return 0;
+
+err:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
+static int
+ble_att_svr_write(uint16_t conn_handle, struct ble_att_svr_entry *entry,
+                  uint16_t offset, struct os_mbuf **om, uint8_t *out_att_err)
+{
+    uint8_t att_err = 0;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+
+    if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+        rc = ble_att_svr_check_perms(conn_handle, 0, entry, &att_err);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+
+    BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
+    rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
+                      BLE_ATT_ACCESS_OP_WRITE, offset, om, entry->ha_cb_arg);
+    if (rc != 0) {
+        att_err = rc;
+        rc = BLE_HS_EAPP;
+        goto done;
+    }
+
+done:
+    if (out_att_err != NULL) {
+        *out_att_err = att_err;
+    }
+    return rc;
+}
+
+static int
+ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
+                         uint16_t offset, struct os_mbuf **om,
+                         uint8_t *out_att_err)
+{
+    struct ble_att_svr_entry *entry;
+    int rc;
+
+    entry = ble_att_svr_find_by_handle(attr_handle);
+    if (entry == NULL) {
+        if (out_att_err != NULL) {
+            *out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        }
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_att_svr_write(conn_handle, entry, offset, om, out_att_err);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_att_svr_tx_error_rsp(uint16_t conn_handle, struct os_mbuf *txom,
+                         uint8_t req_op, uint16_t handle, uint8_t error_code)
+{
+    struct ble_att_error_rsp *rsp;
+
+    BLE_HS_DBG_ASSERT(error_code != 0);
+    BLE_HS_DBG_ASSERT(OS_MBUF_PKTLEN(txom) == 0);
+
+    rsp = ble_att_cmd_prepare(BLE_ATT_OP_ERROR_RSP, sizeof(*rsp), txom);
+    if (rsp == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    rsp->baep_req_op = req_op;
+    rsp->baep_handle = htole16(handle);
+    rsp->baep_error_code = error_code;
+
+    return ble_att_tx(conn_handle, txom);
+}
+
+/**
+ * Transmits a response or error message over the specified connection.
+ *
+ * The specified rc and err_status values control what gets sent as follows:
+ *     o If rc == 0: tx an affirmative response.
+ *     o Else if err_status != 0: tx an error response.
+ *     o Else: tx nothing.
+ *
+ * In addition, if transmission of an affirmative response fails, an error is
+ * sent instead.
+ *
+ * @param conn_handle           The handle of the connection to send over.
+ * @param hs_status             The status indicating whether to transmit an
+ *                                  affirmative response or an error.
+ * @param txom                  Contains the affirmative response payload.
+ * @param att_op                If an error is transmitted, this is the value
+ *                                  of the error message's op field.
+ * @param err_status            If an error is transmitted, this is the value
+ *                                  of the error message's status field.
+ * @param err_handle            If an error is transmitted, this is the value
+ *                                  of the error message's attribute handle
+ *                                  field.
+ */
+static int
+ble_att_svr_tx_rsp(uint16_t conn_handle, int hs_status, struct os_mbuf *om,
+                   uint8_t att_op, uint8_t err_status, uint16_t err_handle)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int do_tx;
+    int rc;
+
+    if (hs_status != 0 && err_status == 0) {
+        /* Processing failed, but err_status of 0 means don't send error. */
+        do_tx = 0;
+    } else {
+        do_tx = 1;
+    }
+
+    if (do_tx) {
+        ble_hs_lock();
+
+        rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+        if (rc != 0) {
+            /* No longer connected. */
+            hs_status = rc;
+        } else {
+            if (hs_status == 0) {
+                BLE_HS_DBG_ASSERT(om != NULL);
+
+                ble_att_inc_tx_stat(om->om_data[0]);
+                ble_att_truncate_to_mtu(chan, om);
+                hs_status = ble_l2cap_tx(conn, chan, om);
+                om = NULL;
+                if (hs_status != 0) {
+                    err_status = BLE_ATT_ERR_UNLIKELY;
+                }
+           }
+        }
+
+        ble_hs_unlock();
+
+        if (hs_status != 0) {
+            STATS_INC(ble_att_stats, error_rsp_tx);
+
+            /* Reuse om for error response. */
+            if (om == NULL) {
+                om = ble_hs_mbuf_l2cap_pkt();
+            } else {
+                os_mbuf_adj(om, OS_MBUF_PKTLEN(om));
+            }
+            if (om != NULL) {
+                ble_att_svr_tx_error_rsp(conn_handle, om, att_op,
+                                         err_handle, err_status);
+                om = NULL;
+            }
+        }
+    }
+
+    /* Free mbuf if it was not consumed (i.e., if the send failed). */
+    os_mbuf_free_chain(om);
+
+    return hs_status;
+}
+
+static int
+ble_att_svr_build_mtu_rsp(uint16_t conn_handle, struct os_mbuf **rxom,
+                          struct os_mbuf **out_txom, uint8_t *att_err)
+{
+    struct ble_att_mtu_cmd *cmd;
+    struct ble_l2cap_chan *chan;
+    struct os_mbuf *txom;
+    uint16_t mtu;
+    int rc;
+
+    *att_err = 0; /* Silence unnecessary warning. */
+    txom = NULL;
+
+    ble_hs_lock();
+    rc = ble_att_conn_chan_find(conn_handle, NULL, &chan);
+    if (rc == 0) {
+        mtu = chan->my_mtu;
+    }
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        goto done;
+    }
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    cmd = ble_att_cmd_prepare(BLE_ATT_OP_MTU_RSP, sizeof(*cmd), txom);
+    if (cmd == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    cmd->bamc_mtu = htole16(mtu);
+
+    rc = 0;
+
+done:
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+    struct ble_att_mtu_cmd *cmd;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    struct os_mbuf *txom;
+    uint16_t mtu;
+    uint8_t att_err;
+    int rc;
+
+    txom = NULL;
+    mtu = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*cmd), &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    cmd = (struct ble_att_mtu_cmd *)(*rxom)->om_data;
+
+    mtu = le16toh(cmd->bamc_mtu);
+
+    rc = ble_att_svr_build_mtu_rsp(conn_handle, rxom, &txom, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_MTU_REQ,
+                            att_err, 0);
+    if (rc == 0) {
+        ble_hs_lock();
+
+        rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+        if (rc == 0) {
+            ble_att_set_peer_mtu(chan, mtu);
+            chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
+            mtu = ble_att_chan_mtu(chan);
+        }
+
+        ble_hs_unlock();
+
+        if (rc == 0) {
+            ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu);
+        }
+    }
+    return rc;
+}
+
+/**
+ * Fills the supplied mbuf with the variable length Information Data field of a
+ * Find Information ATT response.
+ *
+ * @param req                   The Find Information request being responded
+ *                                  to.
+ * @param om                    The destination mbuf where the Information
+ *                                  Data field gets written.
+ * @param mtu                   The ATT L2CAP channel MTU.
+ * @param format                On success, the format field of the response
+ *                                  gets stored here.  One of:
+ *                                     o BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT
+ *                                     o BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+static int
+ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
+                      struct os_mbuf *om, uint16_t mtu, uint8_t *format)
+{
+    struct ble_att_svr_entry *ha;
+    uint8_t *buf;
+    int num_entries;
+    int entry_sz;
+    int rc;
+
+    *format = 0;
+    num_entries = 0;
+    rc = 0;
+
+    STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
+        if (ha->ha_handle_id > end_handle) {
+            rc = 0;
+            goto done;
+        }
+        if (ha->ha_handle_id >= start_handle) {
+            if (ha->ha_uuid->type == BLE_UUID_TYPE_16) {
+                if (*format == 0) {
+                    *format = BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT;
+                } else if (*format != BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT) {
+                    rc = 0;
+                    goto done;
+                }
+
+                entry_sz = 4;
+            } else {
+                if (*format == 0) {
+                    *format = BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT;
+                } else if (*format != BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT) {
+                    rc = 0;
+                    goto done;
+                }
+                entry_sz = 18;
+            }
+
+            if (OS_MBUF_PKTLEN(om) + entry_sz > mtu) {
+                rc = 0;
+                goto done;
+            }
+
+            buf = os_mbuf_extend(om, entry_sz);
+            if (buf == NULL) {
+                rc = BLE_HS_ENOMEM;
+                goto done;
+            }
+
+            put_le16(buf + 0, ha->ha_handle_id);
+
+            ble_uuid_flat(ha->ha_uuid, buf + 2);
+
+            num_entries++;
+        }
+    }
+
+done:
+    if (rc == 0 && num_entries == 0) {
+        return BLE_HS_ENOENT;
+    } else {
+        return rc;
+    }
+}
+
+static int
+ble_att_svr_build_find_info_rsp(uint16_t conn_handle,
+                                uint16_t start_handle, uint16_t end_handle,
+                                struct os_mbuf **rxom,
+                                struct os_mbuf **out_txom,
+                                uint8_t *att_err)
+{
+    struct ble_att_find_info_rsp *rsp;
+    struct os_mbuf *txom;
+    uint16_t mtu;
+    int rc;
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    /* Write the response base at the start of the buffer.  The format field is
+     * unknown at this point; it will be filled in later.
+     */
+    rsp = ble_att_cmd_prepare(BLE_ATT_OP_FIND_INFO_RSP, sizeof(*rsp), txom);
+    if (rsp == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    /* Write the variable length Information Data field, populating the format
+     * field as appropriate.
+     */
+    mtu = ble_att_mtu(conn_handle);
+    rc = ble_att_svr_fill_info(start_handle, end_handle, txom, mtu,
+                               &rsp->bafp_format);
+    if (rc != 0) {
+        *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
+        rc = BLE_HS_ENOENT;
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_FIND_INFO)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_find_info_req *req;
+    struct os_mbuf *txom;
+    uint16_t err_handle, start_handle, end_handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    err_handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        err_handle = 0;
+        goto done;
+    }
+
+    req = (struct ble_att_find_info_req *)(*rxom)->om_data;
+    start_handle = le16toh(req->bafq_start_handle);
+    end_handle = le16toh(req->bafq_end_handle);
+
+    /* Tx error response if start handle is greater than end handle or is equal
+     * to 0 (Vol. 3, Part F, 3.4.3.1).
+     */
+    if (start_handle > end_handle || start_handle == 0) {
+        att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        err_handle = start_handle;
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    rc = ble_att_svr_build_find_info_rsp(conn_handle,
+                                        start_handle, end_handle,
+                                        rxom, &txom, &att_err);
+    if (rc != 0) {
+        err_handle = start_handle;
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_FIND_INFO_REQ,
+                            att_err, err_handle);
+    return rc;
+}
+
+/**
+ * Fills a Find-By-Type-Value-Response with single entry.
+ *
+ * @param om                    The response mbuf.
+ * @param first                 First handle ID in the current group of IDs.
+ * @param last                  Last handle ID in the current group of ID.
+ * @param mtu                   The ATT L2CAP channel MTU.
+ *
+ * @return                      0 if the response should be sent;
+ *                              BLE_HS_EAGAIN if the entry was successfully
+ *                                  processed and subsequent entries can be
+ *                                  inspected.
+ *                              Other nonzero on error.
+ */
+static int
+ble_att_svr_fill_type_value_entry(struct os_mbuf *om, uint16_t first,
+                                     uint16_t last, int mtu,
+                                     uint8_t *out_att_err)
+{
+    uint16_t u16;
+    int rsp_sz;
+    int rc;
+
+    rsp_sz = OS_MBUF_PKTHDR(om)->omp_len + 4;
+    if (rsp_sz > mtu) {
+        return 0;
+    }
+
+    put_le16(&u16, first);
+    rc = os_mbuf_append(om, &u16, 2);
+    if (rc != 0) {
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        return BLE_HS_ENOMEM;
+    }
+
+    put_le16(&u16, last);
+    rc = os_mbuf_append(om, &u16, 2);
+    if (rc != 0) {
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        return BLE_HS_ENOMEM;
+    }
+
+    return BLE_HS_EAGAIN;
+}
+
+static int
+ble_att_svr_is_valid_find_group_type(const ble_uuid_t *uuid)
+{
+    uint16_t uuid16;
+
+    uuid16 = ble_uuid_u16(uuid);
+
+    return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
+           uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE ||
+           uuid16 == BLE_ATT_UUID_CHARACTERISTIC;
+}
+
+static int
+ble_att_svr_is_valid_group_end(const ble_uuid_t *uuid_group,
+                               const ble_uuid_t *uuid)
+{
+    uint16_t uuid16;
+
+    /* Grouping is defined only for 16-bit UUIDs, so any attribute ends group
+     * for non-16-bit UUIDs.
+     */
+    if (uuid_group->type != BLE_UUID_TYPE_16) {
+        return 1;
+    }
+
+    /* Grouping is defined only for 16-bit UUIDs, so non-16-bit UUID attribute
+     * cannot end group.
+     */
+    if (uuid->type != BLE_UUID_TYPE_16) {
+        return 0;
+    }
+
+    switch (ble_uuid_u16(uuid_group)) {
+    case BLE_ATT_UUID_PRIMARY_SERVICE:
+    case BLE_ATT_UUID_SECONDARY_SERVICE:
+        uuid16 = ble_uuid_u16(uuid);
+
+        /* Only Primary or Secondary Service types end service group. */
+        return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
+               uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE;
+    case BLE_ATT_UUID_CHARACTERISTIC:
+        /* Any valid grouping type ends characteristic group */
+        return ble_att_svr_is_valid_find_group_type(uuid);
+    default:
+        /* Any attribute type ends group of non-grouping type */
+        return 1;
+    }
+}
+
+/**
+ * Fills the supplied mbuf with the variable length Handles-Information-List
+ * field of a Find-By-Type-Value ATT response.
+ *
+ * @param req                   The Find-By-Type-Value-Request being responded
+ *                                  to.
+ * @param rxom                  The mbuf containing the received request.
+ * @param txom                  The destination mbuf where the
+ *                                  Handles-Information-List field gets
+ *                                  written.
+ * @param mtu                   The ATT L2CAP channel MTU.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if attribute not found;
+ *                              BLE_HS_EAPP on other error.
+ */
+static int
+ble_att_svr_fill_type_value(uint16_t conn_handle,
+                            uint16_t start_handle, uint16_t end_handle,
+                            ble_uuid16_t attr_type,
+                            struct os_mbuf *rxom, struct os_mbuf *txom,
+                            uint16_t mtu, uint8_t *out_att_err)
+{
+    struct ble_att_svr_entry *ha;
+    uint8_t buf[16];
+    uint16_t attr_len;
+    uint16_t first;
+    uint16_t prev;
+    int any_entries;
+    int rc;
+
+    first = 0;
+    prev = 0;
+    rc = 0;
+
+    /* Iterate through the attribute list, keeping track of the current
+     * matching group.  For each attribute entry, determine if data needs to be
+     * written to the response.
+     */
+    STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
+        if (ha->ha_handle_id < start_handle) {
+            continue;
+        }
+
+        /* Continue to look for end of group in case group is in progress. */
+        if (!first && ha->ha_handle_id > end_handle) {
+            break;
+        }
+
+        /* With group in progress, check if current attribute ends it. */
+        if (first) {
+            if (!ble_att_svr_is_valid_group_end(&attr_type.u, ha->ha_uuid)) {
+                prev = ha->ha_handle_id;
+                continue;
+            }
+
+            rc = ble_att_svr_fill_type_value_entry(txom, first, prev, mtu,
+                                                   out_att_err);
+            if (rc != BLE_HS_EAGAIN) {
+                goto done;
+            }
+
+            first = 0;
+            prev = 0;
+
+            /* Break in case we were just looking for end of group past the end
+             * handle ID. */
+            if (ha->ha_handle_id > end_handle) {
+                break;
+            }
+        }
+
+        /* Compare the attribute type and value to the request fields to
+         * determine if this attribute matches.
+         */
+        if (ble_uuid_cmp(ha->ha_uuid, &attr_type.u) == 0) {
+            rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf,
+                                       &attr_len, out_att_err);
+            if (rc != 0) {
+                goto done;
+            }
+            /* value is at the end of req */
+            rc = os_mbuf_cmpf(rxom, sizeof(struct ble_att_find_type_value_req),
+                              buf, attr_len);
+            if (rc == 0) {
+                first = ha->ha_handle_id;
+                prev = ha->ha_handle_id;
+            }
+        }
+    }
+
+    /* Process last group in case a group was in progress when the end of the
+     * attribute list was reached.
+     */
+    if (first) {
+        rc = ble_att_svr_fill_type_value_entry(txom, first, prev, mtu,
+                                               out_att_err);
+        if (rc == BLE_HS_EAGAIN) {
+            rc = 0;
+        }
+    } else {
+        rc = 0;
+    }
+
+done:
+    any_entries = OS_MBUF_PKTHDR(txom)->omp_len >
+                  BLE_ATT_FIND_TYPE_VALUE_RSP_BASE_SZ;
+    if (rc == 0 && !any_entries) {
+        *out_att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
+        return BLE_HS_ENOENT;
+    } else {
+        return rc;
+    }
+}
+
+static int
+ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle,
+                                      uint16_t start_handle,
+                                      uint16_t end_handle,
+                                      ble_uuid16_t attr_type,
+                                      struct os_mbuf **rxom,
+                                      struct os_mbuf **out_txom,
+                                      uint8_t *out_att_err)
+{
+    struct os_mbuf *txom;
+    uint16_t mtu;
+    uint8_t *buf;
+    int rc;
+
+    rc = ble_att_svr_pkt(rxom, &txom, out_att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    /* info list is filled later on */
+    buf = ble_att_cmd_prepare(BLE_ATT_OP_FIND_TYPE_VALUE_RSP, 0, txom);
+    if (buf == NULL) {
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    /* Write the variable length Information Data field. */
+    mtu = ble_att_mtu(conn_handle);
+
+    rc = ble_att_svr_fill_type_value(conn_handle, start_handle, end_handle,
+                                     attr_type, *rxom, txom, mtu,
+                                     out_att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_find_type_value(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_FIND_TYPE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_find_type_value_req *req;
+    uint16_t start_handle, end_handle;
+    ble_uuid16_t attr_type;
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    err_handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_find_type_value_req *)(*rxom)->om_data;
+    start_handle = le16toh(req->bavq_start_handle);
+    end_handle = le16toh(req->bavq_end_handle);
+    attr_type = (ble_uuid16_t) BLE_UUID16_INIT(le16toh(req->bavq_attr_type));
+
+    /* Tx error response if start handle is greater than end handle or is equal
+     * to 0 (Vol. 3, Part F, 3.4.3.3).
+     */
+    if (start_handle > end_handle || start_handle == 0) {
+        att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        err_handle = start_handle;
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+    rc = ble_att_svr_build_find_type_value_rsp(conn_handle, start_handle,
+                                               end_handle, attr_type, rxom,
+                                               &txom, &att_err);
+    if (rc != 0) {
+        err_handle = start_handle;
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom,
+                            BLE_ATT_OP_FIND_TYPE_VALUE_REQ, att_err,
+                            err_handle);
+    return rc;
+}
+
+static int
+ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
+                                uint16_t start_handle, uint16_t end_handle,
+                                const ble_uuid_t *uuid,
+                                struct os_mbuf **rxom,
+                                struct os_mbuf **out_txom,
+                                uint8_t *att_err,
+                                uint16_t *err_handle)
+{
+    struct ble_att_attr_data_list *data;
+    struct ble_att_read_type_rsp *rsp;
+    struct ble_att_svr_entry *entry;
+    struct os_mbuf *txom;
+    uint16_t attr_len;
+    uint16_t mtu;
+    uint8_t buf[19];
+    int entry_written;
+    int txomlen;
+    int prev_attr_len;
+    int rc;
+
+    *att_err = 0;    /* Silence unnecessary warning. */
+
+    *err_handle = start_handle;
+    entry_written = 0;
+    prev_attr_len = 0;
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    /* Allocate space for the respose base, but don't fill in the fields.  They
+     * get filled in at the end, when we know the value of the length field.
+     */
+
+    rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_TYPE_RSP, sizeof(*rsp), txom);
+    if (rsp == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        *err_handle = 0;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    mtu = ble_att_mtu(conn_handle);
+
+    /* Find all matching attributes, writing a record for each. */
+    entry = NULL;
+    while (1) {
+        entry = ble_att_svr_find_by_uuid(entry, uuid, end_handle);
+        if (entry == NULL) {
+            rc = BLE_HS_ENOENT;
+            break;
+        }
+
+        if (entry->ha_handle_id >= start_handle) {
+            rc = ble_att_svr_read_flat(conn_handle, entry, 0, sizeof buf, buf,
+                                       &attr_len, att_err);
+            if (rc != 0) {
+                *err_handle = entry->ha_handle_id;
+                goto done;
+            }
+
+            if (attr_len > mtu - 4) {
+                attr_len = mtu - 4;
+            }
+
+            if (prev_attr_len == 0) {
+                prev_attr_len = attr_len;
+            } else if (prev_attr_len != attr_len) {
+                break;
+            }
+
+            txomlen = OS_MBUF_PKTHDR(txom)->omp_len + 2 + attr_len;
+            if (txomlen > mtu) {
+                break;
+            }
+
+            data = os_mbuf_extend(txom, 2 + attr_len);
+            if (data == NULL) {
+                *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+                *err_handle = entry->ha_handle_id;
+                rc = BLE_HS_ENOMEM;
+                goto done;
+            }
+
+            data->handle = htole16(entry->ha_handle_id);
+            memcpy(data->value, buf, attr_len);
+            entry_written = 1;
+        }
+    }
+
+done:
+    if (!entry_written) {
+        /* No matching attributes. */
+        if (*att_err == 0) {
+            *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
+        }
+        if (rc == 0) {
+            rc = BLE_HS_ENOENT;
+        }
+    } else {
+        /* Send what we can, even if an error was encountered. */
+        rc = 0;
+        *att_err = 0;
+
+        /* Fill the response base. */
+        rsp->batp_length = htole16(sizeof(*data) + prev_attr_len);
+    }
+
+    *out_txom = txom;
+
+    return rc;
+}
+
+int
+ble_att_svr_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_READ_TYPE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_type_req *req;
+    uint16_t start_handle, end_handle;
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint16_t pktlen;
+    ble_uuid_any_t uuid;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    err_handle = 0;
+    att_err = 0;
+
+    pktlen = OS_MBUF_PKTLEN(*rxom);
+    if (pktlen != sizeof(*req) + 2 && pktlen != sizeof(*req) + 16) {
+        /* Malformed packet */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    rc = ble_att_svr_pullup_req_base(rxom, pktlen, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_read_type_req *)(*rxom)->om_data;
+
+    start_handle = le16toh(req->batq_start_handle);
+    end_handle = le16toh(req->batq_end_handle);
+
+    if (start_handle > end_handle || start_handle == 0) {
+        att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        err_handle = start_handle;
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    rc = ble_uuid_init_from_att_mbuf(&uuid, *rxom, sizeof(*req),
+                                     pktlen - sizeof(*req));
+    if (rc != 0) {
+        att_err = BLE_ATT_ERR_INVALID_PDU;
+        rc = BLE_HS_EMSGSIZE;
+        goto done;
+    }
+
+    rc = ble_att_svr_build_read_type_rsp(conn_handle, start_handle, end_handle,
+                                         &uuid.u, rxom, &txom, &att_err,
+                                         &err_handle);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_TYPE_REQ,
+                            att_err, err_handle);
+    return rc;
+}
+
+int
+ble_att_svr_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_READ)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_req *req;
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    err_handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_read_req *)(*rxom)->om_data;
+
+    err_handle = le16toh(req->barq_handle);
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_READ_RSP, 0, txom) == NULL) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_svr_read_handle(conn_handle, err_handle, 0, txom, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_REQ,
+                            att_err, err_handle);
+    return rc;
+}
+
+int
+ble_att_svr_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_READ_BLOB)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_blob_req *req;
+    struct os_mbuf *txom;
+    uint16_t err_handle, offset;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    err_handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_read_blob_req *)(*rxom)->om_data;
+
+    err_handle = le16toh(req->babq_handle);
+    offset = le16toh(req->babq_offset);
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_READ_BLOB_RSP, 0, txom) == NULL) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_svr_read_handle(conn_handle, err_handle, offset,
+                                 txom, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_BLOB_REQ,
+                            att_err, err_handle);
+    return rc;
+}
+
+static int
+ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,
+                                struct os_mbuf **rxom,
+                                struct os_mbuf **out_txom,
+                                uint8_t *att_err,
+                                uint16_t *err_handle)
+{
+    struct os_mbuf *txom;
+    uint16_t handle;
+    uint16_t mtu;
+    int rc;
+
+    mtu = ble_att_mtu(conn_handle);
+
+    rc = ble_att_svr_pkt(rxom, &txom, att_err);
+    if (rc != 0) {
+        *err_handle = 0;
+        goto done;
+    }
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_READ_MULT_RSP, 0, txom) == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        *err_handle = 0;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    /* Iterate through requested handles, reading the corresponding attribute
+     * for each.  Stop when there are no more handles to process, or the
+     * response is full.
+     */
+    while (OS_MBUF_PKTLEN(*rxom) >= 2 && OS_MBUF_PKTLEN(txom) < mtu) {
+        /* Ensure the full 16-bit handle is contiguous at the start of the
+         * mbuf.
+         */
+        rc = ble_att_svr_pullup_req_base(rxom, 2, att_err);
+        if (rc != 0) {
+            *err_handle = 0;
+            goto done;
+        }
+
+        /* Extract the 16-bit handle and strip it from the front of the
+         * mbuf.
+         */
+        handle = get_le16((*rxom)->om_data);
+        os_mbuf_adj(*rxom, 2);
+
+        rc = ble_att_svr_read_handle(conn_handle, handle, 0, txom, att_err);
+        if (rc != 0) {
+            *err_handle = handle;
+            goto done;
+        }
+    }
+
+    rc = 0;
+
+done:
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_READ_MULT)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    err_handle = 0;
+    att_err = 0;
+
+    rc = ble_att_svr_build_read_mult_rsp(conn_handle, rxom, &txom, &att_err,
+                                         &err_handle);
+
+    return ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_MULT_REQ,
+                              att_err, err_handle);
+}
+
+static int
+ble_att_svr_is_valid_read_group_type(const ble_uuid_t *uuid)
+{
+    uint16_t uuid16;
+
+    uuid16 = ble_uuid_u16(uuid);
+
+    return uuid16 == BLE_ATT_UUID_PRIMARY_SERVICE ||
+           uuid16 == BLE_ATT_UUID_SECONDARY_SERVICE;
+}
+
+static int
+ble_att_svr_service_uuid(struct ble_att_svr_entry *entry,
+                         ble_uuid_any_t *uuid, uint8_t *out_att_err)
+{
+    uint8_t val[16];
+    uint16_t attr_len;
+    int rc;
+
+    rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE, entry, 0, sizeof(val), val,
+                               &attr_len, out_att_err);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_uuid_init_from_buf(uuid, val, attr_len);
+
+    return rc;
+}
+
+static int
+ble_att_svr_read_group_type_entry_write(struct os_mbuf *om, uint16_t mtu,
+                                        uint16_t start_group_handle,
+                                        uint16_t end_group_handle,
+                                        const ble_uuid_t *service_uuid)
+{
+    uint8_t *buf;
+    int len;
+
+    if (service_uuid->type == BLE_UUID_TYPE_16) {
+        len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
+    } else {
+        len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128;
+    }
+    if (OS_MBUF_PKTLEN(om) + len > mtu) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    buf = os_mbuf_extend(om, len);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    put_le16(buf + 0, start_group_handle);
+    put_le16(buf + 2, end_group_handle);
+    ble_uuid_flat(service_uuid, buf + 4);
+
+    return 0;
+}
+
+/**
+ * @return                      0 on success; BLE_HS error code on failure.
+ */
+static int
+ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle,
+                                      uint16_t start_handle,
+                                      uint16_t end_handle,
+                                      const ble_uuid_t *group_uuid,
+                                      struct os_mbuf **rxom,
+                                      struct os_mbuf **out_txom,
+                                      uint8_t *att_err,
+                                      uint16_t *err_handle)
+{
+    struct ble_att_read_group_type_rsp *rsp;
+    struct ble_att_svr_entry *entry;
+    struct os_mbuf *txom;
+    uint16_t start_group_handle;
+    uint16_t end_group_handle;
+    uint16_t mtu;
+    ble_uuid_any_t service_uuid;
+    int rc;
+
+    /* Silence warnings. */
+    end_group_handle = 0;
+
+    *att_err = 0;
+    *err_handle = start_handle;
+
+    mtu = ble_att_mtu(conn_handle);
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    /* Reserve space for the response base. */
+    rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_GROUP_TYPE_RSP, sizeof(*rsp),
+                              txom);
+    if (rsp == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    start_group_handle = 0;
+    rsp->bagp_length = 0;
+    STAILQ_FOREACH(entry, &ble_att_svr_list, ha_next) {
+        if (entry->ha_handle_id < start_handle) {
+            continue;
+        }
+        if (entry->ha_handle_id > end_handle) {
+            /* The full input range has been searched. */
+            rc = 0;
+            goto done;
+        }
+
+        if (start_group_handle != 0) {
+            /* We have already found the start of a group. */
+            if (!ble_att_svr_is_valid_read_group_type(entry->ha_uuid)) {
+                /* This attribute is part of the current group. */
+                end_group_handle = entry->ha_handle_id;
+            } else {
+                /* This attribute marks the end of the group.  Write an entry
+                 * representing the group to the response.
+                 */
+                rc = ble_att_svr_read_group_type_entry_write(
+                    txom, mtu, start_group_handle, end_group_handle,
+                    &service_uuid.u);
+                start_group_handle = 0;
+                end_group_handle = 0;
+                if (rc != 0) {
+                    *err_handle = entry->ha_handle_id;
+                    if (rc == BLE_HS_ENOMEM) {
+                        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+                    } else {
+                        BLE_HS_DBG_ASSERT(rc == BLE_HS_EMSGSIZE);
+                    }
+                    goto done;
+                }
+            }
+        }
+
+        if (start_group_handle == 0) {
+            /* We are looking for the start of a group. */
+            if (ble_uuid_cmp(entry->ha_uuid, group_uuid) == 0) {
+                /* Found a group start.  Read the group UUID. */
+                rc = ble_att_svr_service_uuid(entry, &service_uuid, att_err);
+                if (rc != 0) {
+                    *err_handle = entry->ha_handle_id;
+                    goto done;
+                }
+
+                /* Make sure the group UUID lengths are consistent.  If this
+                 * group has a different length UUID, then cut the response
+                 * short.
+                 */
+                switch (rsp->bagp_length) {
+                case 0:
+                    if (service_uuid.u.type == BLE_UUID_TYPE_16) {
+                        rsp->bagp_length = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
+                    } else {
+                        rsp->bagp_length = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128;
+                    }
+                    break;
+
+                case BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16:
+                    if (service_uuid.u.type != BLE_UUID_TYPE_16) {
+                        rc = 0;
+                        goto done;
+                    }
+                    break;
+
+                case BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128:
+                    if (service_uuid.u.type == BLE_UUID_TYPE_16) {
+                        rc = 0;
+                        goto done;
+                    }
+                    break;
+
+                default:
+                    BLE_HS_DBG_ASSERT(0);
+                    goto done;
+                }
+
+                start_group_handle = entry->ha_handle_id;
+                end_group_handle = entry->ha_handle_id;
+            }
+        }
+    }
+
+    rc = 0;
+
+done:
+    if (rc == 0) {
+        if (start_group_handle != 0) {
+            /* A group was being processed.  Add its corresponding entry to the
+             * response.
+             */
+
+            if (entry == NULL) {
+                /* We have reached the end of the attribute list.  Indicate an
+                 * end handle of 0xffff so that the client knows there are no
+                 * more attributes without needing to send a follow-up request.
+                 */
+                end_group_handle = 0xffff;
+            }
+
+            rc = ble_att_svr_read_group_type_entry_write(txom, mtu,
+                                                         start_group_handle,
+                                                         end_group_handle,
+                                                         &service_uuid.u);
+            if (rc == BLE_HS_ENOMEM) {
+                *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+            }
+        }
+
+        if (OS_MBUF_PKTLEN(txom) <= BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ) {
+            *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
+            rc = BLE_HS_ENOENT;
+        }
+    }
+
+    if (rc == 0 || rc == BLE_HS_EMSGSIZE) {
+        rc = 0;
+    }
+
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_read_group_type(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_READ_GROUP_TYPE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_read_group_type_req *req;
+    struct os_mbuf *txom;
+    ble_uuid_any_t uuid;
+    uint16_t err_handle, start_handle, end_handle;
+    uint16_t pktlen;
+    uint8_t att_err;
+    int om_uuid_len;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    err_handle = 0;
+    att_err = 0;
+
+    pktlen = OS_MBUF_PKTLEN(*rxom);
+    if (pktlen != sizeof(*req) + 2 && pktlen != sizeof(*req) + 16) {
+        /* Malformed packet */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    rc = ble_att_svr_pullup_req_base(rxom, pktlen, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_read_group_type_req *)(*rxom)->om_data;
+
+    start_handle = le16toh(req->bagq_start_handle);
+    end_handle = le16toh(req->bagq_end_handle);
+
+    if (start_handle > end_handle || start_handle == 0) {
+        att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        err_handle = start_handle;
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    om_uuid_len = OS_MBUF_PKTHDR(*rxom)->omp_len - sizeof(*req);
+    rc = ble_uuid_init_from_att_mbuf(&uuid, *rxom, sizeof(*req), om_uuid_len);
+    if (rc != 0) {
+        att_err = BLE_ATT_ERR_INVALID_PDU;
+        err_handle = start_handle;
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    if (!ble_att_svr_is_valid_read_group_type(&uuid.u)) {
+        att_err = BLE_ATT_ERR_UNSUPPORTED_GROUP;
+        err_handle = start_handle;
+        rc = BLE_HS_EREJECT;
+        goto done;
+    }
+
+    rc = ble_att_svr_build_read_group_type_rsp(conn_handle, start_handle,
+                                               end_handle, &uuid.u,
+                                               rxom, &txom, &att_err,
+                                               &err_handle);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom,
+                            BLE_ATT_OP_READ_GROUP_TYPE_REQ, att_err,
+                            err_handle);
+    return rc;
+}
+
+static int
+ble_att_svr_build_write_rsp(struct os_mbuf **rxom, struct os_mbuf **out_txom,
+                            uint8_t *att_err)
+{
+    struct os_mbuf *txom;
+    int rc;
+
+    /* Allocate a new buffer for the response.  A write response never reuses
+     * the request buffer.  See the note at the top of this file for details.
+     */
+    rc = ble_att_svr_pkt(rxom, &txom, att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_WRITE_RSP, 0, txom) == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_WRITE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_write_req *req;
+    struct os_mbuf *txom;
+    uint16_t handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_write_req *)(*rxom)->om_data;
+
+    handle = le16toh(req->bawq_handle);
+
+    /* Allocate the write response.  This must be done prior to processing the
+     * request.  See the note at the top of this file for details.
+     */
+    rc = ble_att_svr_build_write_rsp(rxom, &txom, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    /* Strip the request base from the front of the mbuf. */
+    os_mbuf_adj(*rxom, sizeof(*req));
+
+    rc = ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_WRITE_REQ,
+                            att_err, handle);
+    return rc;
+}
+
+int
+ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_WRITE_NO_RSP)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_write_req *req;
+    uint8_t att_err;
+    uint16_t handle;
+    int rc;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        return rc;
+    }
+
+    req = (struct ble_att_write_req *)(*rxom)->om_data;
+
+    handle = le16toh(req->bawq_handle);
+
+    /* Strip the request base from the front of the mbuf. */
+    os_mbuf_adj(*rxom, sizeof(*req));
+
+    return ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
+}
+
+int
+ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om)
+{
+    int rc;
+
+    rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0,
+                                  &om, NULL);
+
+    /* Free the mbuf if it wasn't relinquished to the application. */
+    os_mbuf_free_chain(om);
+
+    return rc;
+}
+
+static void
+ble_att_svr_prep_free(struct ble_att_prep_entry *entry)
+{
+    if (entry != NULL) {
+        os_mbuf_free_chain(entry->bape_value);
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        memset(entry, 0xff, sizeof *entry);
+#endif
+        os_memblock_put(&ble_att_svr_prep_entry_pool, entry);
+    }
+}
+
+static struct ble_att_prep_entry *
+ble_att_svr_prep_alloc(uint8_t *att_err)
+{
+    struct ble_att_prep_entry *entry;
+
+    entry = os_memblock_get(&ble_att_svr_prep_entry_pool);
+    if (entry == NULL) {
+        *att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
+        return NULL;
+    }
+
+    memset(entry, 0, sizeof *entry);
+    entry->bape_value = ble_hs_mbuf_l2cap_pkt();
+    if (entry->bape_value == NULL) {
+        ble_att_svr_prep_free(entry);
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        return NULL;
+    }
+
+    return entry;
+}
+
+static struct ble_att_prep_entry *
+ble_att_svr_prep_find_prev(struct ble_att_svr_conn *basc, uint16_t handle,
+                           uint16_t offset)
+{
+    struct ble_att_prep_entry *entry;
+    struct ble_att_prep_entry *prev;
+
+    prev = NULL;
+    SLIST_FOREACH(entry, &basc->basc_prep_list, bape_next) {
+        if (entry->bape_handle > handle) {
+            break;
+        }
+
+        if (entry->bape_handle == handle && entry->bape_offset > offset) {
+            break;
+        }
+
+        prev = entry;
+    }
+
+    return prev;
+}
+
+void
+ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list)
+{
+    struct ble_att_prep_entry *entry;
+
+    while ((entry = SLIST_FIRST(prep_list)) != NULL) {
+        SLIST_REMOVE_HEAD(prep_list, bape_next);
+        ble_att_svr_prep_free(entry);
+    }
+}
+
+/**
+ * @return                      0 on success; ATT error code on failure.
+ */
+static int
+ble_att_svr_prep_validate(struct ble_att_prep_entry_list *prep_list,
+                          uint16_t *err_handle)
+{
+    struct ble_att_prep_entry *entry;
+    struct ble_att_prep_entry *prev;
+    int cur_len;
+
+    prev = NULL;
+    SLIST_FOREACH(entry, prep_list, bape_next) {
+        if (prev == NULL || prev->bape_handle != entry->bape_handle) {
+            /* Ensure attribute write starts at offset 0. */
+            if (entry->bape_offset != 0) {
+                *err_handle = entry->bape_handle;
+                return BLE_ATT_ERR_INVALID_OFFSET;
+            }
+        } else {
+            /* Ensure entry continues where previous left off. */
+            if (prev->bape_offset + OS_MBUF_PKTLEN(prev->bape_value) !=
+                entry->bape_offset) {
+
+                *err_handle = entry->bape_handle;
+                return BLE_ATT_ERR_INVALID_OFFSET;
+            }
+        }
+
+        cur_len = entry->bape_offset + OS_MBUF_PKTLEN(entry->bape_value);
+        if (cur_len > BLE_ATT_ATTR_MAX_LEN) {
+            *err_handle = entry->bape_handle;
+            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+        }
+
+        prev = entry;
+    }
+
+    return 0;
+}
+
+static void
+ble_att_svr_prep_extract(struct ble_att_prep_entry_list *prep_list,
+                         uint16_t *out_attr_handle,
+                         struct os_mbuf **out_om)
+{
+    struct ble_att_prep_entry *entry;
+    struct ble_att_prep_entry *first;
+    struct os_mbuf *om;
+    uint16_t attr_handle;
+
+    BLE_HS_DBG_ASSERT(!SLIST_EMPTY(prep_list));
+
+    first = SLIST_FIRST(prep_list);
+    attr_handle = first->bape_handle;
+    om = NULL;
+
+    while ((entry = SLIST_FIRST(prep_list)) != NULL) {
+        if (entry->bape_handle != attr_handle) {
+            break;
+        }
+
+        if (om == NULL) {
+            om = entry->bape_value;
+        } else {
+            os_mbuf_concat(om, entry->bape_value);
+        }
+        entry->bape_value = NULL;
+
+        SLIST_REMOVE_HEAD(prep_list, bape_next);
+        ble_att_svr_prep_free(entry);
+    }
+
+    *out_attr_handle = attr_handle;
+    *out_om = om;
+}
+
+/**
+ * @return                      0 on success; ATT error code on failure.
+ */
+static int
+ble_att_svr_prep_write(uint16_t conn_handle,
+                       struct ble_att_prep_entry_list *prep_list,
+                       uint16_t *err_handle)
+{
+    struct ble_att_svr_entry *attr;
+    struct os_mbuf *om;
+    uint16_t attr_handle;
+    uint8_t att_err;
+    int rc;
+
+    *err_handle = 0; /* Silence unnecessary warning. */
+
+    /* First, validate the contents of the prepare queue. */
+    rc = ble_att_svr_prep_validate(prep_list, err_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Contents are valid; perform the writes. */
+    while (!SLIST_EMPTY(prep_list)) {
+        ble_att_svr_prep_extract(prep_list, &attr_handle, &om);
+
+        /* Attribute existence was verified during prepare-write request
+         * processing.
+         */
+        attr = ble_att_svr_find_by_handle(attr_handle);
+        BLE_HS_DBG_ASSERT(attr != NULL);
+
+        rc = ble_att_svr_write(conn_handle, attr, 0, &om, &att_err);
+        os_mbuf_free_chain(om);
+        if (rc != 0) {
+            *err_handle = attr_handle;
+            return att_err;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_att_svr_insert_prep_entry(uint16_t conn_handle,
+                              uint16_t handle, uint16_t offset,
+                              const struct os_mbuf *rxom,
+                              uint8_t *out_att_err)
+{
+    struct ble_att_prep_entry *prep_entry;
+    struct ble_att_prep_entry *prep_prev;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    conn = ble_hs_conn_find_assert(conn_handle);
+
+    prep_entry = ble_att_svr_prep_alloc(out_att_err);
+    if (prep_entry == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+    prep_entry->bape_handle = handle;
+    prep_entry->bape_offset = offset;
+
+    /* Append attribute value from request onto prep mbuf. */
+    rc = os_mbuf_appendfrom(
+        prep_entry->bape_value,
+        rxom,
+        sizeof(struct ble_att_prep_write_cmd),
+        OS_MBUF_PKTLEN(rxom) - sizeof(struct ble_att_prep_write_cmd));
+    if (rc != 0) {
+        /* Failed to allocate an mbuf to hold the additional data. */
+        ble_att_svr_prep_free(prep_entry);
+
+        /* XXX: We need to differentiate between "prepare queue full" and
+         * "insufficient resources."  Currently, we always indicate prepare
+         * queue full.
+         */
+        *out_att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
+        return rc;
+    }
+
+    prep_prev = ble_att_svr_prep_find_prev(&conn->bhc_att_svr,
+                                           handle, offset);
+    if (prep_prev == NULL) {
+        SLIST_INSERT_HEAD(&conn->bhc_att_svr.basc_prep_list, prep_entry,
+                          bape_next);
+    } else {
+        SLIST_INSERT_AFTER(prep_prev, prep_entry, bape_next);
+    }
+
+#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO != 0
+    conn->bhc_att_svr.basc_prep_timeout_at =
+        ble_npl_time_get() + BLE_HS_ATT_SVR_QUEUED_WRITE_TMO;
+
+    ble_hs_timer_resched();
+#endif
+
+    return 0;
+}
+
+int
+ble_att_svr_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_prep_write_cmd *req;
+    struct ble_att_svr_entry *attr_entry;
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    err_handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_prep_write_cmd *)(*rxom)->om_data;
+
+    err_handle = le16toh(req->bapc_handle);
+
+    attr_entry = ble_att_svr_find_by_handle(le16toh(req->bapc_handle));
+
+    /* A prepare write request gets rejected for the following reasons:
+     * 1. Insufficient authorization.
+     * 2. Insufficient authentication.
+     * 3. Insufficient encryption key size (XXX: Not checked).
+     * 4. Insufficient encryption (XXX: Not checked).
+     * 5. Invalid handle.
+     * 6. Write not permitted.
+     */
+
+    /* <5> */
+    if (attr_entry == NULL) {
+        rc = BLE_HS_ENOENT;
+        att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        goto done;
+    }
+
+    /* <1>, <2>, <4>, <6> */
+    rc = ble_att_svr_check_perms(conn_handle, 0, attr_entry, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_hs_lock();
+    rc = ble_att_svr_insert_prep_entry(conn_handle, le16toh(req->bapc_handle),
+                                       le16toh(req->bapc_offset), *rxom,
+                                       &att_err);
+    ble_hs_unlock();
+
+    /* Reuse rxom for response.  On success, the response is identical to
+     * request except for op code.  On error, the buffer contents will get
+     * cleared before the error gets written.
+     */
+    txom = *rxom;
+    *rxom = NULL;
+
+    if (rc != 0) {
+        goto done;
+    }
+
+    /* adjust for ATT header */
+    os_mbuf_prepend(txom, 1);
+    txom->om_data[0] = BLE_ATT_OP_PREP_WRITE_RSP;
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_PREP_WRITE_REQ,
+                            att_err, err_handle);
+    return rc;
+}
+
+int
+ble_att_svr_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_prep_entry_list prep_list;
+    struct ble_att_exec_write_req *req;
+    struct ble_hs_conn *conn;
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint8_t att_err;
+    uint8_t flags;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    err_handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        flags = 0;
+        goto done;
+    }
+
+    req = (struct ble_att_exec_write_req *)(*rxom)->om_data;
+
+    flags = req->baeq_flags;
+
+    /* Just reuse the request buffer for the response. */
+    txom = *rxom;
+    *rxom = NULL;
+    os_mbuf_adj(txom, OS_MBUF_PKTLEN(txom));
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_EXEC_WRITE_RSP, 0, txom) == NULL) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    if (rc == 0) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(conn_handle);
+
+        /* Extract the list of prepared writes from the connection so
+         * that they can be processed after the mutex is unlocked.  They
+         * aren't processed now because attribute writes involve executing
+         * an application callback.
+         */
+        prep_list = conn->bhc_att_svr.basc_prep_list;
+        SLIST_INIT(&conn->bhc_att_svr.basc_prep_list);
+        ble_hs_unlock();
+
+        if (flags) {
+            /* Perform attribute writes. */
+            att_err = ble_att_svr_prep_write(conn_handle, &prep_list,
+                                             &err_handle);
+            if (att_err != 0) {
+                rc = BLE_HS_EAPP;
+            }
+        }
+
+        /* Free the prep entries. */
+        ble_att_svr_prep_clear(&prep_list);
+    }
+
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_EXEC_WRITE_REQ,
+                            att_err, err_handle);
+    return rc;
+}
+
+int
+ble_att_svr_rx_notify(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_NOTIFY)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_notify_req *req;
+    uint16_t handle;
+    int rc;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
+    if (rc != 0) {
+        return BLE_HS_ENOMEM;
+    }
+
+    req = (struct ble_att_notify_req *)(*rxom)->om_data;
+
+    handle = le16toh(req->banq_handle);
+
+    if (handle == 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    /* Strip the request base from the front of the mbuf. */
+    os_mbuf_adj(*rxom, sizeof(*req));
+
+    ble_gap_notify_rx_event(conn_handle, handle, *rxom, 0);
+    *rxom = NULL;
+
+    return 0;
+}
+
+/**
+ * @return                      0 on success; nonzero on failure.
+ */
+static int
+ble_att_svr_build_indicate_rsp(struct os_mbuf **rxom,
+                               struct os_mbuf **out_txom, uint8_t *out_att_err)
+{
+    struct os_mbuf *txom;
+    int rc;
+
+    /* Allocate a new buffer for the response.  An indicate response never
+     * reuses the request buffer.  See the note at the top of this file for
+     * details.
+     */
+    rc = ble_att_svr_pkt(rxom, &txom, out_att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_INDICATE_RSP, 0, txom) == NULL) {
+        rc = BLE_HS_ENOMEM;
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_INDICATE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_indicate_req *req;
+    struct os_mbuf *txom;
+    uint16_t handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    att_err = 0;
+    handle = 0;
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
+    if (rc != 0) {
+        goto done;
+    }
+
+    req = (struct ble_att_indicate_req *)(*rxom)->om_data;
+
+    handle = le16toh(req->baiq_handle);
+
+    if (handle == 0) {
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    /* Allocate the indicate response.  This must be done prior to processing
+     * the request.  See the note at the top of this file for details.
+     */
+    rc = ble_att_svr_build_indicate_rsp(rxom, &txom, &att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    /* Strip the request base from the front of the mbuf. */
+    os_mbuf_adj(*rxom, sizeof(*req));
+
+    ble_gap_notify_rx_event(conn_handle, handle, *rxom, 1);
+    *rxom = NULL;
+
+    rc = 0;
+
+done:
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_INDICATE_REQ,
+                            att_err, handle);
+    return rc;
+}
+
+static void
+ble_att_svr_move_entries(struct ble_att_svr_entry_list *src,
+                         struct ble_att_svr_entry_list *dst,
+                         uint16_t start_handle, uint16_t end_handle)
+{
+
+    struct ble_att_svr_entry *entry;
+    struct ble_att_svr_entry *prev;
+    struct ble_att_svr_entry *remove;
+    struct ble_att_svr_entry *insert;
+
+    /* Find first matching element to move */
+    remove = NULL;
+    entry = STAILQ_FIRST(src);
+    while (entry && entry->ha_handle_id < start_handle) {
+        remove = entry;
+        entry = STAILQ_NEXT(entry, ha_next);
+    }
+
+    /* Nothing to remove? */
+    if (!entry) {
+        return;
+    }
+
+    /* Find element after which we'll put moved elements */
+    prev = NULL;
+    insert = STAILQ_FIRST(dst);
+    while (insert && insert->ha_handle_id < start_handle) {
+        prev = insert;
+        insert = STAILQ_NEXT(insert, ha_next);
+    }
+    insert = prev;
+
+    /* Move elements */
+    while (entry && entry->ha_handle_id <= end_handle) {
+        /* Remove either from head or after prev (which is current one) */
+        if (remove == NULL) {
+            STAILQ_REMOVE_HEAD(src, ha_next);
+        } else {
+            STAILQ_REMOVE_AFTER(src, remove, ha_next);
+        }
+
+        /* Insert current element */
+        if (insert == NULL) {
+            STAILQ_INSERT_HEAD(dst, entry, ha_next);
+            insert = STAILQ_FIRST(dst);
+        } else {
+            STAILQ_INSERT_AFTER(dst, insert, entry, ha_next);
+            insert = entry;
+        }
+
+        /* Calculate next candidate to remove */
+        if (remove == NULL) {
+            entry = STAILQ_FIRST(src);
+        } else {
+            entry = STAILQ_NEXT(remove, ha_next);
+        }
+    }
+}
+
+void
+ble_att_svr_hide_range(uint16_t start_handle, uint16_t end_handle)
+{
+    ble_att_svr_move_entries(&ble_att_svr_list, &ble_att_svr_hidden_list,
+                             start_handle, end_handle);
+}
+
+void
+ble_att_svr_restore_range(uint16_t start_handle, uint16_t end_handle)
+{
+    ble_att_svr_move_entries(&ble_att_svr_hidden_list, &ble_att_svr_list,
+                             start_handle, end_handle);
+}
+
+void
+ble_att_svr_reset(void)
+{
+    struct ble_att_svr_entry *entry;
+
+    while ((entry = STAILQ_FIRST(&ble_att_svr_list)) != NULL) {
+        STAILQ_REMOVE_HEAD(&ble_att_svr_list, ha_next);
+        ble_att_svr_entry_free(entry);
+    }
+
+    while ((entry = STAILQ_FIRST(&ble_att_svr_hidden_list)) != NULL) {
+        STAILQ_REMOVE_HEAD(&ble_att_svr_hidden_list, ha_next);
+        ble_att_svr_entry_free(entry);
+    }
+
+    /* Note: prep entries do not get freed here because it is assumed there are
+     * no established connections.
+     */
+}
+
+static void
+ble_att_svr_free_start_mem(void)
+{
+    free(ble_att_svr_entry_mem);
+    ble_att_svr_entry_mem = NULL;
+}
+
+int
+ble_att_svr_start(void)
+{
+    int rc;
+
+    ble_att_svr_free_start_mem();
+
+    if (ble_hs_max_attrs > 0) {
+        ble_att_svr_entry_mem = malloc(
+            OS_MEMPOOL_BYTES(ble_hs_max_attrs,
+                             sizeof (struct ble_att_svr_entry)));
+        if (ble_att_svr_entry_mem == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        rc = os_mempool_init(&ble_att_svr_entry_pool, ble_hs_max_attrs,
+                             sizeof (struct ble_att_svr_entry),
+                             ble_att_svr_entry_mem, "ble_att_svr_entry_pool");
+        if (rc != 0) {
+            rc = BLE_HS_EOS;
+            goto err;
+        }
+    }
+
+    return 0;
+
+err:
+    ble_att_svr_free_start_mem();
+    return rc;
+}
+
+int
+ble_att_svr_init(void)
+{
+    int rc;
+
+    if (MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES) > 0) {
+        rc = os_mempool_init(&ble_att_svr_prep_entry_pool,
+                             MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
+                             sizeof (struct ble_att_prep_entry),
+                             ble_att_svr_prep_entry_mem,
+                             "ble_att_svr_prep_entry_pool");
+        if (rc != 0) {
+            return BLE_HS_EOS;
+        }
+    }
+
+    STAILQ_INIT(&ble_att_svr_list);
+    STAILQ_INIT(&ble_att_svr_hidden_list);
+
+    ble_att_svr_id = 0;
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_eddystone.c b/src/libs/mynewt-nimble/nimble/host/src/ble_eddystone.c
new file mode 100644
index 00000000..7d80d134
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_eddystone.c
@@ -0,0 +1,178 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include "os/endian.h"
+#include "host/ble_eddystone.h"
+#include "host/ble_hs_adv.h"
+#include "ble_hs_priv.h"
+
+#define BLE_EDDYSTONE_MAX_SVC_DATA_LEN  22
+#define BLE_EDDYSTONE_SVC_DATA_BASE_SZ  3
+
+#define BLE_EDDYSTONE_SERVICE_UUID      0xfeaa
+
+#define BLE_EDDYSTONE_FRAME_TYPE_UID    0x00
+#define BLE_EDDYSTONE_FRAME_TYPE_URL    0x10
+
+static ble_uuid16_t ble_eddystone_uuids16[BLE_EDDYSTONE_MAX_UUIDS16 + 1];
+static uint8_t ble_eddystone_svc_data[BLE_EDDYSTONE_MAX_SVC_DATA_LEN];
+
+/**
+ * Writes an eddystone header to the global service data buffer.
+ *
+ * @param frame_type                The eddystone frame type; one of the
+ *                                      BLE_EDDYSTONE_FRAME_TYPE_[...] values.
+ *
+ * @return                          A pointer to where the service data payload
+ *                                      should be written.
+ */
+static void *
+ble_eddystone_set_svc_data_base(uint8_t frame_type)
+{
+    put_le16(ble_eddystone_svc_data, BLE_EDDYSTONE_SERVICE_UUID);
+    ble_eddystone_svc_data[2] = frame_type;
+
+    return ble_eddystone_svc_data + BLE_EDDYSTONE_SVC_DATA_BASE_SZ;
+}
+
+/**
+ * Populates the supplied advertisement fields struct to represent an eddystone
+ * advertisement.  Prior to calling this function, you must write the service
+ * data header and payload using the ble_eddystone_set_svc_data_base()
+ * function.
+ *
+ * @param adv_fields            The base advertisement fields to transform into
+ *                                  an eddystone beacon.  All configured fields
+ *                                  are preserved; you probably want to clear
+ *                                  this struct before calling this function.
+ * @param svc_data_len          The amount of data written to the global
+ *                                  service data buffer.
+ *
+ * @return                      0 on success; BLE_HS_E... on failure.
+ */
+static int
+ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields,
+                               uint8_t svc_data_len)
+{
+    int rc;
+
+    if (adv_fields->num_uuids16 > BLE_EDDYSTONE_MAX_UUIDS16) {
+        return BLE_HS_EINVAL;
+    }
+    if (svc_data_len > BLE_EDDYSTONE_MAX_SVC_DATA_LEN) {
+        return BLE_HS_EINVAL;
+    }
+    if (adv_fields->num_uuids16 > 0 && !adv_fields->uuids16_is_complete) {
+        return BLE_HS_EINVAL;
+    }
+    if (adv_fields->svc_data_uuid16_len != 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_eddystone_uuids16[0] =
+        (ble_uuid16_t) BLE_UUID16_INIT(BLE_EDDYSTONE_SERVICE_UUID);
+    memcpy(ble_eddystone_uuids16 + 1, adv_fields->uuids16,
+           adv_fields->num_uuids16 * sizeof(ble_uuid16_t));
+    adv_fields->uuids16 = ble_eddystone_uuids16;
+    adv_fields->num_uuids16++;
+    adv_fields->uuids16_is_complete = 1;
+
+    adv_fields->svc_data_uuid16 = ble_eddystone_svc_data;
+    adv_fields->svc_data_uuid16_len = svc_data_len +
+                                      BLE_EDDYSTONE_SVC_DATA_BASE_SZ;
+
+    rc = ble_gap_adv_set_fields(adv_fields);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields,
+                               void *uid, int8_t measured_power)
+{
+    uint8_t *svc_data;
+    int rc;
+
+    /* Eddystone UUID and frame type (0). */
+    svc_data = ble_eddystone_set_svc_data_base(BLE_EDDYSTONE_FRAME_TYPE_UID);
+
+    /* Measured Power ranging data (Calibrated tx power at 0 meters). */
+    if (measured_power < -100 || measured_power > 20) {
+        return BLE_HS_EINVAL;
+    }
+    svc_data[0] = measured_power;
+
+    /* UID. */
+    memcpy(svc_data + 1, uid, 16);
+
+    /* Reserved. */
+    svc_data[17] = 0x00;
+    svc_data[18] = 0x00;
+
+    rc = ble_eddystone_set_adv_data_gen(adv_fields, 19);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields,
+                               uint8_t url_scheme, char *url_body,
+                               uint8_t url_body_len, uint8_t url_suffix,
+                               int8_t measured_power)
+{
+    uint8_t *svc_data;
+    int url_len;
+    int rc;
+
+    url_len = url_body_len;
+    if (url_suffix != BLE_EDDYSTONE_URL_SUFFIX_NONE) {
+        url_len++;
+    }
+    if (url_len > BLE_EDDYSTONE_URL_MAX_LEN) {
+        return BLE_HS_EINVAL;
+    }
+
+    svc_data = ble_eddystone_set_svc_data_base(BLE_EDDYSTONE_FRAME_TYPE_URL);
+
+    /* Measured Power ranging data (Calibrated tx power at 0 meters). */
+    if (measured_power < -100 || measured_power > 20) {
+        return BLE_HS_EINVAL;
+    }
+    svc_data[0] = measured_power;
+
+    svc_data[1] = url_scheme;
+    memcpy(svc_data + 2, url_body, url_body_len);
+    if (url_suffix != BLE_EDDYSTONE_URL_SUFFIX_NONE) {
+        svc_data[2 + url_body_len] = url_suffix;
+    }
+
+    rc = ble_eddystone_set_adv_data_gen(adv_fields, url_len + 2);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_gap.c b/src/libs/mynewt-nimble/nimble/host/src/ble_gap.c
new file mode 100644
index 00000000..7d1c5252
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_gap.c
@@ -0,0 +1,6073 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include <errno.h>
+#include "nimble/nimble_opt.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_hs_hci.h"
+#include "ble_hs_priv.h"
+
+#if MYNEWT
+#include "bsp/bsp.h"
+#else
+#define bssnz_t
+#endif
+
+/**
+ * GAP - Generic Access Profile.
+ *
+ * Design overview:
+ *
+ * GAP procedures are initiated by the application via function calls.  Such
+ * functions return when either of the following happens:
+ *
+ * (1) The procedure completes (success or failure).
+ * (2) The procedure cannot proceed until a BLE peer responds.
+ *
+ * For (1), the result of the procedure if fully indicated by the function
+ * return code.
+ * For (2), the procedure result is indicated by an application-configured
+ * callback.  The callback is executed when the procedure completes.
+ *
+ * The GAP is always in one of two states:
+ * 1. Free
+ * 2. Preempted
+ *
+ * While GAP is in the free state, new procedures can be started at will.
+ * While GAP is in the preempted state, no new procedures are allowed.  The
+ * host sets GAP to the preempted state when it needs to ensure no ongoing
+ * procedures, a condition required for some HCI commands to succeed.  The host
+ * must take care to take GAP out of the preempted state as soon as possible.
+ *
+ * Notes on thread-safety:
+ * 1. The ble_hs mutex must always be unlocked when an application callback is
+ *    executed.  The purpose of this requirement is to allow callbacks to
+ *    initiate additional host procedures, which may require locking of the
+ *    mutex.
+ * 2. Functions called directly by the application never call callbacks.
+ *    Generally, these functions lock the ble_hs mutex at the start, and only
+ *    unlock it at return.
+ * 3. Functions which do call callbacks (receive handlers and timer
+ *    expirations) generally only lock the mutex long enough to modify
+ *    affected state and make copies of data needed for the callback.  A copy
+ *    of various pieces of data is called a "snapshot" (struct
+ *    ble_gap_snapshot).  The sole purpose of snapshots is to allow callbacks
+ *    to be executed after unlocking the mutex.
+ */
+
+/** GAP procedure op codes. */
+#define BLE_GAP_OP_NULL             0
+#define BLE_GAP_OP_M_DISC           1
+#define BLE_GAP_OP_M_CONN           2
+#define BLE_GAP_OP_S_ADV            1
+#define BLE_GAP_OP_S_PERIODIC_ADV   2
+#define BLE_GAP_OP_SYNC             1
+
+/**
+ * If an attempt to cancel an active procedure fails, the attempt is retried
+ * at this rate (ms).
+ */
+#define BLE_GAP_CANCEL_RETRY_TIMEOUT_MS         100 /* ms */
+
+#define BLE_GAP_UPDATE_TIMEOUT_MS               40000 /* ms */
+
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+static const struct ble_gap_conn_params ble_gap_conn_params_dflt = {
+    .scan_itvl = 0x0010,
+    .scan_window = 0x0010,
+    .itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN,
+    .itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX,
+    .latency = BLE_GAP_INITIAL_CONN_LATENCY,
+    .supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT,
+    .min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN,
+    .max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN,
+};
+#endif
+
+/**
+ * The state of the in-progress master connection.  If no master connection is
+ * currently in progress, then the op field is set to BLE_GAP_OP_NULL.
+ */
+struct ble_gap_master_state {
+    uint8_t op;
+
+    uint8_t exp_set:1;
+    ble_npl_time_t exp_os_ticks;
+
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    /**
+     * Indicates the type of master procedure that was preempted, or
+     * BLE_GAP_OP_NULL if no procedure was preempted.
+     */
+    uint8_t preempted_op;
+
+    union {
+        struct {
+            uint8_t using_wl:1;
+            uint8_t our_addr_type:2;
+            uint8_t cancel:1;
+        } conn;
+
+        struct {
+            uint8_t limited:1;
+        } disc;
+    };
+};
+static bssnz_t struct ble_gap_master_state ble_gap_master;
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+/**
+ * The state of the in-progress sync creation. If no sync creation connection is
+ * currently in progress, then the op field is set to BLE_GAP_OP_NULL.
+ */
+struct ble_gap_sync_state {
+    uint8_t op;
+    struct ble_hs_periodic_sync *psync;
+
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+};
+
+static bssnz_t struct ble_gap_sync_state ble_gap_sync;
+#endif
+
+/**
+ * The state of the in-progress slave connection.  If no slave connection is
+ * currently in progress, then the op field is set to BLE_GAP_OP_NULL.
+ */
+struct ble_gap_slave_state {
+    uint8_t op;
+
+    unsigned int our_addr_type:2;
+    unsigned int preempted:1;  /** Set to 1 if advertising was preempted. */
+    unsigned int connectable:1;
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    unsigned int configured:1; /** If instance is configured */
+    unsigned int scannable:1;
+    unsigned int directed:1;
+    unsigned int high_duty_directed:1;
+    unsigned int legacy_pdu:1;
+    unsigned int rnd_addr_set:1;
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    unsigned int periodic_configured:1;
+    uint8_t      periodic_op;
+#endif
+    uint8_t rnd_addr[6];
+#else
+/* timer is used only with legacy advertising */
+    unsigned int exp_set:1;
+    ble_npl_time_t exp_os_ticks;
+#endif
+
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+};
+
+static bssnz_t struct ble_gap_slave_state ble_gap_slave[BLE_ADV_INSTANCES];
+
+struct ble_gap_update_entry {
+    SLIST_ENTRY(ble_gap_update_entry) next;
+    struct ble_gap_upd_params params;
+    ble_npl_time_t exp_os_ticks;
+    uint16_t conn_handle;
+};
+SLIST_HEAD(ble_gap_update_entry_list, ble_gap_update_entry);
+
+struct ble_gap_snapshot {
+    struct ble_gap_conn_desc *desc;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+};
+
+static SLIST_HEAD(ble_gap_hook_list, ble_gap_event_listener) ble_gap_event_listener_list;
+static os_membuf_t ble_gap_update_entry_mem[
+                        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_GAP_MAX_PENDING_CONN_PARAM_UPDATE),
+                                        sizeof (struct ble_gap_update_entry))];
+static struct os_mempool ble_gap_update_entry_pool;
+static struct ble_gap_update_entry_list ble_gap_update_entries;
+
+static void ble_gap_update_entry_free(struct ble_gap_update_entry *entry);
+
+#if NIMBLE_BLE_CONNECT
+static struct ble_gap_update_entry *
+ble_gap_update_entry_find(uint16_t conn_handle,
+                          struct ble_gap_update_entry **out_prev);
+
+static void
+ble_gap_update_l2cap_cb(uint16_t conn_handle, int status, void *arg);
+#endif
+
+static struct ble_gap_update_entry *
+ble_gap_update_entry_remove(uint16_t conn_handle);
+
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+static int ble_gap_adv_enable_tx(int enable);
+#endif
+
+static int ble_gap_conn_cancel_tx(void);
+
+#if NIMBLE_BLE_SCAN && !MYNEWT_VAL(BLE_EXT_ADV)
+static int ble_gap_disc_enable_tx(int enable, int filter_duplicates);
+#endif
+
+STATS_SECT_DECL(ble_gap_stats) ble_gap_stats;
+STATS_NAME_START(ble_gap_stats)
+    STATS_NAME(ble_gap_stats, wl_set)
+    STATS_NAME(ble_gap_stats, wl_set_fail)
+    STATS_NAME(ble_gap_stats, adv_stop)
+    STATS_NAME(ble_gap_stats, adv_stop_fail)
+    STATS_NAME(ble_gap_stats, adv_start)
+    STATS_NAME(ble_gap_stats, adv_start_fail)
+    STATS_NAME(ble_gap_stats, adv_set_data)
+    STATS_NAME(ble_gap_stats, adv_set_data_fail)
+    STATS_NAME(ble_gap_stats, adv_rsp_set_data)
+    STATS_NAME(ble_gap_stats, adv_rsp_set_data_fail)
+    STATS_NAME(ble_gap_stats, discover)
+    STATS_NAME(ble_gap_stats, discover_fail)
+    STATS_NAME(ble_gap_stats, initiate)
+    STATS_NAME(ble_gap_stats, initiate_fail)
+    STATS_NAME(ble_gap_stats, terminate)
+    STATS_NAME(ble_gap_stats, terminate_fail)
+    STATS_NAME(ble_gap_stats, cancel)
+    STATS_NAME(ble_gap_stats, cancel_fail)
+    STATS_NAME(ble_gap_stats, update)
+    STATS_NAME(ble_gap_stats, update_fail)
+    STATS_NAME(ble_gap_stats, connect_mst)
+    STATS_NAME(ble_gap_stats, connect_slv)
+    STATS_NAME(ble_gap_stats, disconnect)
+    STATS_NAME(ble_gap_stats, rx_disconnect)
+    STATS_NAME(ble_gap_stats, rx_update_complete)
+    STATS_NAME(ble_gap_stats, rx_adv_report)
+    STATS_NAME(ble_gap_stats, rx_conn_complete)
+    STATS_NAME(ble_gap_stats, discover_cancel)
+    STATS_NAME(ble_gap_stats, discover_cancel_fail)
+    STATS_NAME(ble_gap_stats, security_initiate)
+    STATS_NAME(ble_gap_stats, security_initiate_fail)
+STATS_NAME_END(ble_gap_stats)
+
+/*****************************************************************************
+ * $debug                                                                    *
+ *****************************************************************************/
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+int
+ble_gap_dbg_update_active(uint16_t conn_handle)
+{
+    const struct ble_gap_update_entry *entry;
+
+    ble_hs_lock();
+    entry = ble_gap_update_entry_find(conn_handle, NULL);
+    ble_hs_unlock();
+
+    return entry != NULL;
+}
+#endif
+
+/*****************************************************************************
+ * $log                                                                      *
+ *****************************************************************************/
+
+#if NIMBLE_BLE_SCAN && !MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_log_duration(int32_t duration_ms)
+{
+    if (duration_ms == BLE_HS_FOREVER) {
+        BLE_HS_LOG(INFO, "duration=forever");
+    } else {
+        BLE_HS_LOG(INFO, "duration=%dms", duration_ms);
+    }
+}
+#endif
+
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL) && !MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_log_conn(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                 const struct ble_gap_conn_params *params)
+{
+    if (peer_addr != NULL) {
+        BLE_HS_LOG(INFO, "peer_addr_type=%d peer_addr=", peer_addr->type);
+        BLE_HS_LOG_ADDR(INFO, peer_addr->val);
+    }
+
+    BLE_HS_LOG(INFO, " scan_itvl=%d scan_window=%d itvl_min=%d itvl_max=%d "
+                     "latency=%d supervision_timeout=%d min_ce_len=%d "
+                     "max_ce_len=%d own_addr_type=%d",
+               params->scan_itvl, params->scan_window, params->itvl_min,
+               params->itvl_max, params->latency, params->supervision_timeout,
+               params->min_ce_len, params->max_ce_len, own_addr_type);
+}
+#endif
+
+#if NIMBLE_BLE_SCAN && !MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_log_disc(uint8_t own_addr_type, int32_t duration_ms,
+                 const struct ble_gap_disc_params *disc_params)
+{
+    BLE_HS_LOG(INFO, "own_addr_type=%d filter_policy=%d passive=%d limited=%d "
+                     "filter_duplicates=%d ",
+               own_addr_type, disc_params->filter_policy, disc_params->passive,
+               disc_params->limited, disc_params->filter_duplicates);
+    ble_gap_log_duration(duration_ms);
+}
+#endif
+
+#if NIMBLE_BLE_CONNECT
+static void
+ble_gap_log_update(uint16_t conn_handle,
+                   const struct ble_gap_upd_params *params)
+{
+    BLE_HS_LOG(INFO, "connection parameter update; "
+                     "conn_handle=%d itvl_min=%d itvl_max=%d latency=%d "
+                     "supervision_timeout=%d min_ce_len=%d max_ce_len=???",
+               conn_handle, params->itvl_min, params->itvl_max,
+               params->latency, params->supervision_timeout,
+               params->min_ce_len);
+}
+#endif
+
+#if MYNEWT_VAL(BLE_WHITELIST)
+static void
+ble_gap_log_wl(const ble_addr_t *addr, uint8_t white_list_count)
+{
+    int i;
+
+    BLE_HS_LOG(INFO, "count=%d ", white_list_count);
+
+    for (i = 0; i < white_list_count; i++, addr++) {
+        BLE_HS_LOG(INFO, "entry-%d={addr_type=%d addr=", i, addr->type);
+        BLE_HS_LOG_ADDR(INFO, addr->val);
+        BLE_HS_LOG(INFO, "} ");
+    }
+}
+#endif
+
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_log_adv(uint8_t own_addr_type, const ble_addr_t *direct_addr,
+                const struct ble_gap_adv_params *adv_params)
+{
+    BLE_HS_LOG(INFO, "disc_mode=%d", adv_params->disc_mode);
+    if (direct_addr) {
+        BLE_HS_LOG(INFO, " direct_addr_type=%d direct_addr=",
+                   direct_addr->type);
+        BLE_HS_LOG_ADDR(INFO, direct_addr->val);
+    }
+    BLE_HS_LOG(INFO, " adv_channel_map=%d own_addr_type=%d "
+                     "adv_filter_policy=%d adv_itvl_min=%d adv_itvl_max=%d",
+               adv_params->channel_map,
+               own_addr_type,
+               adv_params->filter_policy,
+               adv_params->itvl_min,
+               adv_params->itvl_max);
+}
+#endif
+
+/*****************************************************************************
+ * $snapshot                                                                 *
+ *****************************************************************************/
+
+static void
+ble_gap_fill_conn_desc(struct ble_hs_conn *conn,
+                       struct ble_gap_conn_desc *desc)
+{
+    struct ble_hs_conn_addrs addrs;
+
+    ble_hs_conn_addrs(conn, &addrs);
+
+    desc->our_id_addr = addrs.our_id_addr;
+    desc->peer_id_addr = addrs.peer_id_addr;
+    desc->our_ota_addr = addrs.our_ota_addr;
+    desc->peer_ota_addr = addrs.peer_ota_addr;
+
+    desc->conn_handle = conn->bhc_handle;
+    desc->conn_itvl = conn->bhc_itvl;
+    desc->conn_latency = conn->bhc_latency;
+    desc->supervision_timeout = conn->bhc_supervision_timeout;
+    desc->master_clock_accuracy = conn->bhc_master_clock_accuracy;
+    desc->sec_state = conn->bhc_sec_state;
+
+    if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
+        desc->role = BLE_GAP_ROLE_MASTER;
+    } else {
+        desc->role = BLE_GAP_ROLE_SLAVE;
+    }
+}
+
+static void
+ble_gap_conn_to_snapshot(struct ble_hs_conn *conn,
+                         struct ble_gap_snapshot *snap)
+{
+    ble_gap_fill_conn_desc(conn, snap->desc);
+    snap->cb = conn->bhc_cb;
+    snap->cb_arg = conn->bhc_cb_arg;
+}
+
+static int
+ble_gap_find_snapshot(uint16_t handle, struct ble_gap_snapshot *snap)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(handle);
+    if (conn != NULL) {
+        ble_gap_conn_to_snapshot(conn, snap);
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    } else {
+        return 0;
+    }
+}
+
+int
+ble_gap_conn_find(uint16_t handle, struct ble_gap_conn_desc *out_desc)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(handle);
+    if (conn != NULL && out_desc != NULL) {
+        ble_gap_fill_conn_desc(conn, out_desc);
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    } else {
+        return 0;
+    }
+}
+
+int
+ble_gap_conn_find_by_addr(const ble_addr_t *addr,
+                          struct ble_gap_conn_desc *out_desc)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find_by_addr(addr);
+    if (conn != NULL && out_desc != NULL) {
+        ble_gap_fill_conn_desc(conn, out_desc);
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    return 0;
+}
+
+static int
+ble_gap_extract_conn_cb(uint16_t conn_handle,
+                        ble_gap_event_fn **out_cb, void **out_cb_arg)
+{
+    const struct ble_hs_conn *conn;
+
+    BLE_HS_DBG_ASSERT(conn_handle <= BLE_HCI_LE_CONN_HANDLE_MAX);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        *out_cb = conn->bhc_cb;
+        *out_cb_arg = conn->bhc_cb_arg;
+    } else {
+        *out_cb = NULL;
+        *out_cb_arg = NULL;
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    } else {
+        return 0;
+    }
+}
+
+int
+ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode)
+{
+    return ble_hs_pvcy_set_mode(peer_addr, priv_mode);
+}
+
+int
+ble_gap_read_le_phy(uint16_t conn_handle, uint8_t *tx_phy, uint8_t *rx_phy)
+{
+    struct ble_hci_le_rd_phy_cp cmd;
+    struct ble_hci_le_rd_phy_rp rsp;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    cmd.conn_handle = htole16(conn_handle);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_PHY),
+                           &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* sanity check for response */
+    if (le16toh(rsp.conn_handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    *tx_phy = rsp.tx_phy;
+    *rx_phy = rsp.rx_phy;
+
+    return 0;
+}
+
+int
+ble_gap_set_prefered_default_le_phy(uint8_t tx_phys_mask, uint8_t rx_phys_mask)
+{
+    struct ble_hci_le_set_default_phy_cp cmd;
+
+    if (tx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK |
+                        BLE_HCI_LE_PHY_2M_PREF_MASK |
+                        BLE_HCI_LE_PHY_CODED_PREF_MASK)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (rx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK |
+                        BLE_HCI_LE_PHY_2M_PREF_MASK |
+                        BLE_HCI_LE_PHY_CODED_PREF_MASK)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    memset(&cmd, 0, sizeof(cmd));
+
+    if (tx_phys_mask == 0) {
+        cmd.all_phys |= BLE_HCI_LE_PHY_NO_TX_PREF_MASK;
+    } else {
+        cmd.tx_phys = tx_phys_mask;
+    }
+
+    if (rx_phys_mask == 0) {
+        cmd.all_phys |= BLE_HCI_LE_PHY_NO_RX_PREF_MASK;
+    } else {
+        cmd.rx_phys = rx_phys_mask;
+    }
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_DEFAULT_PHY),
+                            &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_gap_set_prefered_le_phy(uint16_t conn_handle, uint8_t tx_phys_mask,
+                   uint8_t rx_phys_mask, uint16_t phy_opts)
+{
+    struct ble_hci_le_set_phy_cp cmd;
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    if (tx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK |
+                        BLE_HCI_LE_PHY_2M_PREF_MASK |
+                        BLE_HCI_LE_PHY_CODED_PREF_MASK)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (rx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK |
+                        BLE_HCI_LE_PHY_2M_PREF_MASK |
+                        BLE_HCI_LE_PHY_CODED_PREF_MASK)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (phy_opts > BLE_HCI_LE_PHY_CODED_S8_PREF) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.conn_handle = htole16(conn_handle);
+
+    if (tx_phys_mask == 0) {
+        cmd.all_phys |= BLE_HCI_LE_PHY_NO_TX_PREF_MASK;
+    } else {
+        cmd.tx_phys = tx_phys_mask;
+    }
+
+    if (rx_phys_mask == 0) {
+        cmd.all_phys |= BLE_HCI_LE_PHY_NO_RX_PREF_MASK;
+    } else {
+        cmd.rx_phys = rx_phys_mask;
+    }
+
+    cmd.phy_options = htole16(phy_opts);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PHY),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+static int
+ble_gap_event_listener_call(struct ble_gap_event *event);
+
+static int
+ble_gap_call_event_cb(struct ble_gap_event *event,
+                      ble_gap_event_fn *cb, void *cb_arg)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+
+    if (cb != NULL) {
+        rc = cb(event, cb_arg);
+    } else {
+        if (event->type == BLE_GAP_EVENT_CONN_UPDATE_REQ) {
+            /* Just copy peer parameters back into the reply. */
+            *event->conn_update_req.self_params =
+                *event->conn_update_req.peer_params;
+        }
+        rc = 0;
+    }
+
+    return rc;
+}
+
+
+static int
+ble_gap_call_conn_event_cb(struct ble_gap_event *event, uint16_t conn_handle)
+{
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+    int rc;
+
+    rc = ble_gap_extract_conn_cb(conn_handle, &cb, &cb_arg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gap_call_event_cb(event, cb, cb_arg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static bool
+ble_gap_is_preempted(void)
+{
+    int i;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    if (ble_gap_master.preempted_op != BLE_GAP_OP_NULL) {
+        return true;
+    }
+
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        if (ble_gap_slave[i].preempted) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+#if NIMBLE_BLE_CONNECT
+static void
+ble_gap_master_reset_state(void)
+{
+    ble_gap_master.op = BLE_GAP_OP_NULL;
+    ble_gap_master.exp_set = 0;
+    ble_gap_master.conn.cancel = 0;
+
+    ble_hs_timer_resched();
+}
+#endif
+
+static void
+ble_gap_slave_reset_state(uint8_t instance)
+{
+    ble_gap_slave[instance].op = BLE_GAP_OP_NULL;
+
+#if !MYNEWT_VAL(BLE_EXT_ADV)
+    ble_gap_slave[instance].exp_set = 0;
+    ble_hs_timer_resched();
+#endif
+}
+
+#if NIMBLE_BLE_CONNECT
+static bool
+ble_gap_has_client(struct ble_gap_master_state *out_state)
+{
+    if (!out_state) {
+        return 0;
+    }
+
+    return out_state->cb;
+}
+
+static void
+ble_gap_master_extract_state(struct ble_gap_master_state *out_state,
+                             int reset_state)
+{
+    ble_hs_lock();
+
+    *out_state = ble_gap_master;
+
+    if (reset_state) {
+        ble_gap_master_reset_state();
+        ble_gap_master.preempted_op = BLE_GAP_OP_NULL;
+    }
+
+    ble_hs_unlock();
+}
+#endif
+
+static void
+ble_gap_slave_extract_cb(uint8_t instance,
+                         ble_gap_event_fn **out_cb, void **out_cb_arg)
+{
+    ble_hs_lock();
+
+    *out_cb = ble_gap_slave[instance].cb;
+    *out_cb_arg = ble_gap_slave[instance].cb_arg;
+    ble_gap_slave_reset_state(instance);
+
+    ble_hs_unlock();
+}
+
+static void
+ble_gap_adv_finished(uint8_t instance, int reason, uint16_t conn_handle,
+                     uint8_t num_events)
+{
+    struct ble_gap_event event;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_ADV_COMPLETE;
+    event.adv_complete.reason = reason;
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    event.adv_complete.instance = instance;
+    event.adv_complete.conn_handle = conn_handle;
+    event.adv_complete.num_ext_adv_events = num_events;
+#endif
+
+    ble_gap_event_listener_call(&event);
+
+    ble_gap_slave_extract_cb(instance, &cb, &cb_arg);
+    if (cb != NULL) {
+        cb(&event, cb_arg);
+    }
+}
+
+#if NIMBLE_BLE_CONNECT
+static int
+ble_gap_master_connect_failure(int status)
+{
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
+    int rc;
+
+    ble_gap_master_extract_state(&state, 1);
+    if (ble_gap_has_client(&state)) {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_CONNECT;
+        event.connect.status = status;
+
+        rc = state.cb(&event, state.cb_arg);
+    } else {
+        rc = 0;
+    }
+
+    return rc;
+}
+
+static void
+ble_gap_master_connect_cancelled(void)
+{
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
+
+    ble_gap_master_extract_state(&state, 1);
+    if (state.cb != NULL) {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_CONNECT;
+        event.connect.conn_handle = BLE_HS_CONN_HANDLE_NONE;
+        if (state.conn.cancel) {
+            /* Connect procedure successfully cancelled. */
+            event.connect.status = BLE_HS_EAPP;
+        } else {
+            /* Connect procedure timed out. */
+            event.connect.status = BLE_HS_ETIMEOUT;
+        }
+        state.cb(&event, state.cb_arg);
+    }
+}
+#endif
+
+#if NIMBLE_BLE_SCAN
+static void
+ble_gap_disc_report(void *desc)
+{
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    event.type = BLE_GAP_EVENT_EXT_DISC;
+    event.ext_disc = *((struct ble_gap_ext_disc_desc *)desc);
+#else
+    event.type = BLE_GAP_EVENT_DISC;
+    event.disc = *((struct ble_gap_disc_desc *)desc);
+#endif
+
+    ble_gap_master_extract_state(&state, 0);
+    if (ble_gap_has_client(&state)) {
+        state.cb(&event, state.cb_arg);
+    }
+
+    ble_gap_event_listener_call(&event);
+}
+
+static void
+ble_gap_disc_complete(void)
+{
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_DISC_COMPLETE;
+    event.disc_complete.reason = 0;
+
+    ble_gap_master_extract_state(&state, 1);
+    if (ble_gap_has_client(&state)) {
+        ble_gap_call_event_cb(&event, state.cb, state.cb_arg);
+    }
+
+    ble_gap_event_listener_call(&event);
+}
+#endif
+
+static void
+ble_gap_update_notify(uint16_t conn_handle, int status)
+{
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_CONN_UPDATE;
+    event.conn_update.conn_handle = conn_handle;
+    event.conn_update.status = status;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+
+    /* Terminate the connection on procedure timeout. */
+    if (status == BLE_HS_ETIMEOUT) {
+        ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+    }
+}
+
+static uint32_t
+ble_gap_master_ticks_until_exp(void)
+{
+    ble_npl_stime_t ticks;
+
+    if (ble_gap_master.op == BLE_GAP_OP_NULL || !ble_gap_master.exp_set) {
+        /* Timer not set; infinity ticks until next event. */
+        return BLE_HS_FOREVER;
+    }
+
+    ticks = ble_gap_master.exp_os_ticks - ble_npl_time_get();
+    if (ticks > 0) {
+        /* Timer not expired yet. */
+        return ticks;
+    }
+
+    /* Timer just expired. */
+    return 0;
+}
+
+#if !MYNEWT_VAL(BLE_EXT_ADV)
+static uint32_t
+ble_gap_slave_ticks_until_exp(void)
+{
+    ble_npl_stime_t ticks;
+
+    if (ble_gap_slave[0].op == BLE_GAP_OP_NULL || !ble_gap_slave[0].exp_set) {
+        /* Timer not set; infinity ticks until next event. */
+        return BLE_HS_FOREVER;
+    }
+
+    ticks = ble_gap_slave[0].exp_os_ticks - ble_npl_time_get();
+    if (ticks > 0) {
+        /* Timer not expired yet. */
+        return ticks;
+    }
+
+    /* Timer just expired. */
+    return 0;
+}
+#endif
+
+/**
+ * Finds the update procedure that expires soonest.
+ *
+ * @param out_ticks_from_now    On success, the ticks until the update
+ *                                  procedure's expiry time gets written here.
+ *
+ * @return                      The connection handle of the update procedure
+ *                                  that expires soonest, or
+ *                                  BLE_HS_CONN_HANDLE_NONE if there are no
+ *                                  active update procedures.
+ */
+static uint16_t
+ble_gap_update_next_exp(int32_t *out_ticks_from_now)
+{
+    struct ble_gap_update_entry *entry;
+    ble_npl_time_t now;
+    uint16_t conn_handle;
+    int32_t best_ticks;
+    int32_t ticks;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    conn_handle = BLE_HS_CONN_HANDLE_NONE;
+    best_ticks = BLE_HS_FOREVER;
+    now = ble_npl_time_get();
+
+    SLIST_FOREACH(entry, &ble_gap_update_entries, next) {
+        ticks = entry->exp_os_ticks - now;
+        if (ticks <= 0) {
+            ticks = 0;
+        }
+
+        if (ticks < best_ticks) {
+            conn_handle = entry->conn_handle;
+            best_ticks = ticks;
+        }
+    }
+
+    if (out_ticks_from_now != NULL) {
+        *out_ticks_from_now = best_ticks;
+    }
+
+    return conn_handle;
+
+}
+
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+static void
+ble_gap_master_set_timer(uint32_t ticks_from_now)
+{
+    ble_gap_master.exp_os_ticks = ble_npl_time_get() + ticks_from_now;
+    ble_gap_master.exp_set = 1;
+
+    ble_hs_timer_resched();
+}
+#endif
+
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_slave_set_timer(uint32_t ticks_from_now)
+{
+    ble_gap_slave[0].exp_os_ticks = ble_npl_time_get() + ticks_from_now;
+    ble_gap_slave[0].exp_set = 1;
+
+    ble_hs_timer_resched();
+}
+#endif
+
+#if (NIMBLE_BLE_CONNECT || NIMBLE_BLE_SCAN)
+/**
+ * Called when an error is encountered while the master-connection-fsm is
+ * active.
+ */
+static void
+ble_gap_master_failed(int status)
+{
+    switch (ble_gap_master.op) {
+    case BLE_GAP_OP_M_CONN:
+        STATS_INC(ble_gap_stats, initiate_fail);
+        ble_gap_master_connect_failure(status);
+        break;
+
+#if NIMBLE_BLE_SCAN
+    case BLE_GAP_OP_M_DISC:
+        STATS_INC(ble_gap_stats, initiate_fail);
+        ble_gap_disc_complete();
+        ble_gap_master_reset_state();
+        break;
+#endif
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+}
+#endif
+
+#if NIMBLE_BLE_CONNECT
+static void
+ble_gap_update_failed(uint16_t conn_handle, int status)
+{
+    struct ble_gap_update_entry *entry;
+
+    STATS_INC(ble_gap_stats, update_fail);
+
+    ble_hs_lock();
+    entry = ble_gap_update_entry_remove(conn_handle);
+    ble_hs_unlock();
+
+    ble_gap_update_entry_free(entry);
+
+    ble_gap_update_notify(conn_handle, status);
+}
+#endif
+
+void
+ble_gap_conn_broken(uint16_t conn_handle, int reason)
+{
+    struct ble_gap_update_entry *entry;
+    struct ble_gap_snapshot snap;
+    struct ble_gap_event event;
+    int rc;
+
+    memset(&event, 0, sizeof event);
+    snap.desc = &event.disconnect.conn;
+
+    rc = ble_gap_find_snapshot(conn_handle, &snap);
+    if (rc != 0) {
+        /* No longer connected. */
+        return;
+    }
+
+    /* If there was a connection update in progress, indicate to the
+     * application that it did not complete.
+     */
+    ble_hs_lock();
+    entry = ble_gap_update_entry_remove(conn_handle);
+    ble_hs_unlock();
+
+    if (entry != NULL) {
+        ble_gap_update_notify(conn_handle, reason);
+        ble_gap_update_entry_free(entry);
+    }
+
+    /* Indicate the connection termination to each module.  The order matters
+     * here: gatts must come before gattc to ensure the application does not
+     * get informed of spurious notify-tx events.
+     */
+    ble_l2cap_sig_conn_broken(conn_handle, reason);
+    ble_sm_connection_broken(conn_handle);
+    ble_gatts_connection_broken(conn_handle);
+    ble_gattc_connection_broken(conn_handle);
+    ble_hs_flow_connection_broken(conn_handle);;
+
+    ble_hs_atomic_conn_delete(conn_handle);
+
+    event.type = BLE_GAP_EVENT_DISCONNECT;
+    event.disconnect.reason = reason;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_event_cb(&event, snap.cb, snap.cb_arg);
+
+    STATS_INC(ble_gap_stats, disconnect);
+}
+
+#if NIMBLE_BLE_CONNECT
+static void
+ble_gap_update_to_l2cap(const struct ble_gap_upd_params *params,
+                        struct ble_l2cap_sig_update_params *l2cap_params)
+{
+    l2cap_params->itvl_min = params->itvl_min;
+    l2cap_params->itvl_max = params->itvl_max;
+    l2cap_params->slave_latency = params->latency;
+    l2cap_params->timeout_multiplier = params->supervision_timeout;
+}
+#endif
+
+void
+ble_gap_rx_disconn_complete(const struct ble_hci_ev_disconn_cmp *ev)
+{
+#if NIMBLE_BLE_CONNECT
+    struct ble_gap_event event;
+    uint16_t handle = le16toh(ev->conn_handle);
+
+    STATS_INC(ble_gap_stats, rx_disconnect);
+
+    if (ev->status == 0) {
+        ble_gap_conn_broken(handle, BLE_HS_HCI_ERR(ev->reason));
+    } else {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_TERM_FAILURE;
+        event.term_failure.conn_handle = handle;
+        event.term_failure.status = BLE_HS_HCI_ERR(ev->status);
+
+        ble_gap_event_listener_call(&event);
+        ble_gap_call_conn_event_cb(&event, handle);
+    }
+#endif
+}
+
+void
+ble_gap_rx_update_complete(const struct ble_hci_ev_le_subev_conn_upd_complete *ev)
+{
+#if NIMBLE_BLE_CONNECT
+    struct ble_gap_update_entry *entry;
+    struct ble_l2cap_sig_update_params l2cap_params;
+    struct ble_gap_event event;
+    struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+    int cb_status;
+    int call_cb;
+    int rc;
+
+    STATS_INC(ble_gap_stats, rx_update_complete);
+
+    memset(&event, 0, sizeof event);
+    memset(&l2cap_params, 0, sizeof l2cap_params);
+
+    ble_hs_lock();
+
+    conn_handle = le16toh(ev->conn_handle);
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        switch (ev->status) {
+        case 0:
+            /* Connection successfully updated. */
+            conn->bhc_itvl = le16toh(ev->conn_itvl);
+            conn->bhc_latency = le16toh(ev->conn_latency);
+            conn->bhc_supervision_timeout = le16toh(ev->supervision_timeout);
+            break;
+
+        case BLE_ERR_UNSUPP_REM_FEATURE:
+            /* Peer reports that it doesn't support the procedure.  This should
+             * only happen if our controller sent the 4.1 Connection Parameters
+             * Request Procedure.  If we are the slave, fail over to the L2CAP
+             * update procedure.
+             */
+            entry = ble_gap_update_entry_find(conn_handle, NULL);
+            if (entry != NULL && !(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) {
+                ble_gap_update_to_l2cap(&entry->params, &l2cap_params);
+                entry->exp_os_ticks = ble_npl_time_get() +
+                                      ble_npl_time_ms_to_ticks32(BLE_GAP_UPDATE_TIMEOUT_MS);
+            }
+            break;
+
+        default:
+            break;
+        }
+    }
+
+    /* We aren't failing over to L2CAP, the update procedure is complete. */
+    if (l2cap_params.itvl_min == 0) {
+        entry = ble_gap_update_entry_remove(conn_handle);
+        ble_gap_update_entry_free(entry);
+    }
+
+    ble_hs_unlock();
+
+    if (l2cap_params.itvl_min != 0) {
+        rc = ble_l2cap_sig_update(conn_handle, &l2cap_params,
+                                  ble_gap_update_l2cap_cb, NULL);
+        if (rc == 0) {
+            call_cb = 0;
+        } else {
+            call_cb = 1;
+            cb_status = rc;
+        }
+    } else {
+        call_cb = 1;
+        cb_status = BLE_HS_HCI_ERR(ev->status);
+    }
+
+    if (call_cb) {
+        ble_gap_update_notify(conn_handle, cb_status);
+    }
+#endif
+}
+
+/**
+ * Tells you if there is an active central GAP procedure (connect or discover).
+ */
+int
+ble_gap_master_in_progress(void)
+{
+    return ble_gap_master.op != BLE_GAP_OP_NULL;
+}
+
+static int
+ble_gap_adv_active_instance(uint8_t instance)
+{
+    /* Assume read is atomic; mutex not necessary. */
+    return ble_gap_slave[instance].op == BLE_GAP_OP_S_ADV;
+}
+
+/**
+ * Clears advertisement and discovery state.  This function is necessary
+ * when the controller loses its active state (e.g. on stack reset).
+ */
+void
+ble_gap_reset_state(int reason)
+{
+    uint16_t conn_handle;
+
+    while (1) {
+        conn_handle = ble_hs_atomic_first_conn_handle();
+        if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+            break;
+        }
+
+        ble_gap_conn_broken(conn_handle, reason);
+    }
+
+#if NIMBLE_BLE_ADVERTISE
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    uint8_t i;
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        if (ble_gap_adv_active_instance(i)) {
+            /* Indicate to application that advertising has stopped. */
+            ble_gap_adv_finished(i, reason, 0, 0);
+        }
+    }
+#else
+    if (ble_gap_adv_active_instance(0)) {
+        /* Indicate to application that advertising has stopped. */
+        ble_gap_adv_finished(0, reason, 0, 0);
+    }
+#endif
+#endif
+
+#if (NIMBLE_BLE_SCAN || NIMBLE_BLE_CONNECT)
+    ble_gap_master_failed(reason);
+#endif
+}
+
+#if NIMBLE_BLE_CONNECT
+static int
+ble_gap_accept_master_conn(void)
+{
+    int rc;
+
+    switch (ble_gap_master.op) {
+    case BLE_GAP_OP_NULL:
+    case BLE_GAP_OP_M_DISC:
+        rc = BLE_HS_ENOENT;
+        break;
+
+    case BLE_GAP_OP_M_CONN:
+        rc = 0;
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        rc = BLE_HS_ENOENT;
+        break;
+    }
+
+    if (rc == 0) {
+        STATS_INC(ble_gap_stats, connect_mst);
+    }
+
+    return rc;
+}
+
+static int
+ble_gap_accept_slave_conn(uint8_t instance)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+       rc = BLE_HS_ENOENT;
+    } else if (!ble_gap_adv_active_instance(instance)) {
+        rc = BLE_HS_ENOENT;
+    } else {
+        if (ble_gap_slave[instance].connectable) {
+            rc = 0;
+        } else {
+            rc = BLE_HS_ENOENT;
+        }
+    }
+
+    if (rc == 0) {
+        STATS_INC(ble_gap_stats, connect_slv);
+    }
+
+    return rc;
+}
+#endif
+
+#if NIMBLE_BLE_SCAN
+static int
+ble_gap_rx_adv_report_sanity_check(const uint8_t *adv_data, uint8_t adv_data_len)
+{
+    const struct ble_hs_adv_field *flags;
+    int rc;
+
+    STATS_INC(ble_gap_stats, rx_adv_report);
+
+    if (ble_gap_master.op != BLE_GAP_OP_M_DISC) {
+        return -1;
+    }
+
+    /* If a limited discovery procedure is active, discard non-limited
+     * advertisements.
+     */
+    if (ble_gap_master.disc.limited) {
+        rc = ble_hs_adv_find_field(BLE_HS_ADV_TYPE_FLAGS, adv_data,
+                                   adv_data_len, &flags);
+        if ((rc == 0) && (flags->length == 2) &&
+            !(flags->value[0] & BLE_HS_ADV_F_DISC_LTD)) {
+            return -1;
+        }
+    }
+
+    return 0;
+}
+#endif
+
+void
+ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc)
+{
+#if NIMBLE_BLE_SCAN
+    if (ble_gap_rx_adv_report_sanity_check(desc->data, desc->length_data)) {
+        return;
+    }
+
+    ble_gap_disc_report(desc);
+#endif
+}
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+#if NIMBLE_BLE_SCAN
+void
+ble_gap_rx_le_scan_timeout(void)
+{
+    ble_gap_disc_complete();
+}
+
+void
+ble_gap_rx_ext_adv_report(struct ble_gap_ext_disc_desc *desc)
+{
+    if (ble_gap_rx_adv_report_sanity_check(desc->data, desc->length_data)) {
+        return;
+    }
+
+    ble_gap_disc_report(desc);
+}
+#endif
+
+void
+ble_gap_rx_adv_set_terminated(const struct ble_hci_ev_le_subev_adv_set_terminated *ev)
+{
+    uint16_t conn_handle;
+    int reason;
+
+    /* Currently spec allows only 0x3c and 0x43 when advertising was stopped
+     * due to timeout or events limit, mp this for timeout error for now */
+    if (ev->status) {
+        reason = BLE_HS_ETIMEOUT;
+        conn_handle = 0;
+    } else {
+        reason = 0;
+        conn_handle = le16toh(ev->conn_handle);
+    }
+
+    ble_gap_adv_finished(ev->adv_handle, reason, conn_handle, ev->num_events);
+}
+
+void
+ble_gap_rx_scan_req_rcvd(const struct ble_hci_ev_le_subev_scan_req_rcvd *ev)
+{
+    struct ble_gap_event event;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    ble_gap_slave_extract_cb(ev->adv_handle, &cb, &cb_arg);
+    if (cb != NULL) {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_SCAN_REQ_RCVD;
+        event.scan_req_rcvd.instance = ev->adv_handle;
+        event.scan_req_rcvd.scan_addr.type = ev->peer_addr_type;
+        memcpy(event.scan_req_rcvd.scan_addr.val, ev->peer_addr, BLE_DEV_ADDR_LEN);
+        cb(&event, cb_arg);
+    }
+}
+#endif
+
+/* Periodic adv events */
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+
+void
+ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev)
+{
+    uint16_t sync_handle;
+    struct ble_gap_event event;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    memset(&event, 0, sizeof event);
+
+    event.type = BLE_GAP_EVENT_PERIODIC_SYNC;
+    event.periodic_sync.status = ev->status;
+
+    ble_hs_lock();
+
+    BLE_HS_DBG_ASSERT(ble_gap_sync.psync);
+
+    if (!ev->status) {
+        sync_handle = le16toh(ev->sync_handle);
+
+        ble_gap_sync.psync->sync_handle = sync_handle;
+        ble_gap_sync.psync->adv_sid = ev->sid;
+        memcpy(ble_gap_sync.psync->advertiser_addr.val, ev->peer_addr, 6);
+        ble_gap_sync.psync->advertiser_addr.type = ev->peer_addr_type;
+
+        ble_gap_sync.psync->cb = ble_gap_sync.cb;
+        ble_gap_sync.psync->cb_arg = ble_gap_sync.cb_arg;
+
+        event.periodic_sync.sync_handle = sync_handle;
+        event.periodic_sync.sid = ev->sid;
+        event.periodic_sync.adv_addr = ble_gap_sync.psync->advertiser_addr;
+        event.periodic_sync.adv_phy = ev->phy;
+        event.periodic_sync.per_adv_ival = ev->interval;
+        event.periodic_sync.adv_clk_accuracy = ev->aca;
+
+        ble_hs_periodic_sync_insert(ble_gap_sync.psync);
+    } else {
+        ble_hs_periodic_sync_free(ble_gap_sync.psync);
+    }
+
+    cb = ble_gap_sync.cb;
+    cb_arg = ble_gap_sync.cb_arg;
+
+    ble_gap_sync.op = BLE_GAP_OP_NULL;
+    ble_gap_sync.cb_arg = NULL;
+    ble_gap_sync.cb_arg = NULL;
+    ble_gap_sync.psync = NULL;
+
+    ble_hs_unlock();
+
+    ble_gap_event_listener_call(&event);
+    if (cb) {
+        cb(&event, cb_arg);
+    }
+}
+
+void
+ble_gap_rx_periodic_adv_rpt(const struct ble_hci_ev_le_subev_periodic_adv_rpt *ev)
+{
+    struct ble_hs_periodic_sync *psync;
+    struct ble_gap_event event;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    ble_hs_lock();
+    psync = ble_hs_periodic_sync_find_by_handle(le16toh(ev->sync_handle));
+    if (psync) {
+        cb = psync->cb;
+        cb_arg = psync->cb_arg;
+    }
+    ble_hs_unlock();
+
+    if (!psync || !cb) {
+        return;
+    }
+
+    memset(&event, 0, sizeof event);
+
+    event.type = BLE_GAP_EVENT_PERIODIC_REPORT;
+    event.periodic_report.sync_handle = psync->sync_handle;
+    event.periodic_report.tx_power = ev->tx_power;
+    event.periodic_report.rssi = ev->rssi;
+    event.periodic_report.data_status = ev->data_status;
+    event.periodic_report.data_length = ev->data_len;
+    event.periodic_report.data = ev->data;
+
+    /* TODO should we allow for listener too? this can be spammy and is more
+     * like ACL data, not general event
+     */
+     cb(&event, cb_arg);
+}
+
+void
+ble_gap_rx_periodic_adv_sync_lost(const struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev)
+{
+    struct ble_hs_periodic_sync *psync;
+    struct ble_gap_event event;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    ble_hs_lock();
+    /* The handle must be in the list */
+    psync = ble_hs_periodic_sync_find_by_handle(le16toh(ev->sync_handle));
+    BLE_HS_DBG_ASSERT(psync);
+
+    cb = psync->cb;
+    cb_arg = psync->cb_arg;
+
+    /* Remove the handle from the list */
+    ble_hs_periodic_sync_remove(psync);
+    ble_hs_unlock();
+
+    memset(&event, 0, sizeof event);
+
+    event.type = BLE_GAP_EVENT_PERIODIC_SYNC_LOST;
+    event.periodic_sync_lost.sync_handle = psync->sync_handle;
+    event.periodic_sync_lost.reason = BLE_HS_ETIMEOUT;
+
+    /* remove any sync_lost event from queue */
+    ble_npl_eventq_remove(ble_hs_evq_get(), &psync->lost_ev);
+
+    /* Free the memory occupied by psync as it is no longer needed */
+    ble_hs_periodic_sync_free(psync);
+
+    ble_gap_event_listener_call(&event);
+    if (cb) {
+        cb(&event, cb_arg);
+    }
+}
+#endif
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+static int
+periodic_adv_transfer_disable(uint16_t conn_handle)
+{
+    struct ble_hci_le_periodic_adv_sync_transfer_params_cp cmd;
+    struct ble_hci_le_periodic_adv_sync_transfer_params_rp rsp;
+    uint16_t opcode;
+    int rc;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS);
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.sync_cte_type = 0x00;
+    cmd.mode = 0x00;
+    cmd.skip = 0x0000;
+    cmd.sync_timeout = 0x000a;
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (!rc) {
+        BLE_HS_DBG_ASSERT(le16toh(rsp.conn_handle) == conn_handle);
+    }
+
+    return rc;
+}
+
+void
+ble_gap_rx_periodic_adv_sync_transfer(const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev)
+{
+    struct ble_hci_le_periodic_adv_term_sync_cp cmd_term;
+    struct ble_gap_event event;
+    struct ble_hs_conn *conn;
+    ble_gap_event_fn *cb;
+    uint16_t sync_handle;
+    uint16_t conn_handle;
+    uint16_t opcode;
+    void *cb_arg;
+
+    conn_handle = le16toh(ev->conn_handle);
+
+    ble_hs_lock();
+
+    /* Unfortunately spec sucks here as it doesn't explicitly stop
+     * transfer reception on first transfer... for now just disable it on
+     * every transfer event we get.
+     */
+    periodic_adv_transfer_disable(conn_handle);
+
+    conn = ble_hs_conn_find(le16toh(ev->conn_handle));
+    if (!conn || !conn->psync) {
+        /* terminate sync if we didn't expect it */
+        if (!ev->status) {
+            opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC);
+            cmd_term.sync_handle = ev->sync_handle;
+            ble_hs_hci_cmd_tx(opcode, &cmd_term, sizeof(cmd_term), NULL, 0);
+        }
+
+        ble_hs_unlock();
+        return;
+    }
+
+    cb = conn->psync->cb;
+    cb_arg = conn->psync->cb_arg;
+
+    memset(&event, 0, sizeof event);
+
+    event.type = BLE_GAP_EVENT_PERIODIC_TRANSFER;
+    event.periodic_transfer.status = ev->status;
+
+    /* only sync handle is not valid on error */
+    if (ev->status) {
+        sync_handle = 0;
+        ble_hs_periodic_sync_free(conn->psync);
+    } else {
+        sync_handle = le16toh(ev->sync_handle);
+
+        conn->psync->sync_handle = sync_handle;
+        conn->psync->adv_sid = ev->sid;
+        memcpy(conn->psync->advertiser_addr.val, ev->peer_addr, 6);
+        conn->psync->advertiser_addr.type = ev->peer_addr_type;
+        ble_hs_periodic_sync_insert(conn->psync);
+    }
+
+    conn->psync = NULL;
+
+    event.periodic_transfer.sync_handle = sync_handle;
+    event.periodic_transfer.conn_handle = conn_handle;
+    event.periodic_transfer.service_data = le16toh(ev->service_data);
+    event.periodic_transfer.sid = ev->sid;
+    memcpy(event.periodic_transfer.adv_addr.val, ev->peer_addr, 6);
+    event.periodic_transfer.adv_addr.type = ev->peer_addr_type;
+
+    event.periodic_transfer.adv_phy = ev->phy;
+    event.periodic_transfer.per_adv_itvl = le16toh(ev->interval);
+    event.periodic_transfer.adv_clk_accuracy = ev->aca;
+
+    ble_hs_unlock();
+
+    ble_gap_event_listener_call(&event);
+    if (cb) {
+        cb(&event, cb_arg);
+    }
+}
+#endif
+
+#if NIMBLE_BLE_CONNECT
+static int
+ble_gap_rd_rem_sup_feat_tx(uint16_t handle)
+{
+    struct ble_hci_le_rd_rem_feat_cp cmd;
+
+    cmd.conn_handle = htole16(handle);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_RD_REM_FEAT),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+#endif
+
+/**
+ * Processes an incoming connection-complete HCI event.
+ * instance parameter is valid only for slave connection.
+ */
+int
+ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance)
+{
+#if NIMBLE_BLE_CONNECT
+    struct ble_gap_event event;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    STATS_INC(ble_gap_stats, rx_conn_complete);
+
+    /* in that case *only* status field is valid so we determine role
+     * based on error code
+     */
+    if (evt->status != BLE_ERR_SUCCESS) {
+        switch (evt->status) {
+        case BLE_ERR_DIR_ADV_TMO:
+            /* slave role (HD directed advertising)
+             *
+             * with ext advertising this is send from set terminated event
+             */
+#if !MYNEWT_VAL(BLE_EXT_ADV)
+            if (ble_gap_adv_active()) {
+                ble_gap_adv_finished(0, 0, 0, 0);
+            }
+#endif
+            break;
+        case BLE_ERR_UNK_CONN_ID:
+            /* master role */
+            if (ble_gap_master_in_progress()) {
+                /* Connect procedure successfully cancelled. */
+                if (ble_gap_master.preempted_op == BLE_GAP_OP_M_CONN) {
+                    ble_gap_master_failed(BLE_HS_EPREEMPTED);
+                } else {
+                    ble_gap_master_connect_cancelled();
+                }
+            }
+            break;
+        default:
+            /* this should never happen, unless controller is broken */
+            BLE_HS_LOG(INFO, "controller reported invalid error code in conn"
+                             "complete event: %u", evt->status);
+            assert(0);
+            break;
+        }
+        return 0;
+    }
+
+    /* Apply the event to the existing connection if it exists. */
+    if (ble_hs_atomic_conn_flags(evt->connection_handle, NULL) == 0) {
+        /* XXX: Does this ever happen? */
+        return 0;
+    }
+
+    /* This event refers to a new connection. */
+
+    switch (evt->role) {
+    case BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER:
+        rc = ble_gap_accept_master_conn();
+        if (rc != 0) {
+            return rc;
+        }
+        break;
+
+    case BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE:
+        rc = ble_gap_accept_slave_conn(instance);
+        if (rc != 0) {
+            return rc;
+        }
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+
+    /* We verified that there is a free connection when the procedure began. */
+    conn = ble_hs_conn_alloc(evt->connection_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+
+    conn->bhc_itvl = evt->conn_itvl;
+    conn->bhc_latency = evt->conn_latency;
+    conn->bhc_supervision_timeout = evt->supervision_timeout;
+    conn->bhc_master_clock_accuracy = evt->master_clk_acc;
+    if (evt->role == BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER) {
+        conn->bhc_cb = ble_gap_master.cb;
+        conn->bhc_cb_arg = ble_gap_master.cb_arg;
+        conn->bhc_flags |= BLE_HS_CONN_F_MASTER;
+        conn->bhc_our_addr_type = ble_gap_master.conn.our_addr_type;
+        ble_gap_master_reset_state();
+    } else {
+        conn->bhc_cb = ble_gap_slave[instance].cb;
+        conn->bhc_cb_arg = ble_gap_slave[instance].cb_arg;
+        conn->bhc_our_addr_type = ble_gap_slave[instance].our_addr_type;
+#if MYNEWT_VAL(BLE_EXT_ADV)
+        memcpy(conn->bhc_our_rnd_addr, ble_gap_slave[instance].rnd_addr, 6);
+#endif
+        ble_gap_slave_reset_state(instance);
+    }
+
+    conn->bhc_peer_addr.type = evt->peer_addr_type;
+    memcpy(conn->bhc_peer_addr.val, evt->peer_addr, 6);
+
+    conn->bhc_our_rpa_addr.type = BLE_ADDR_RANDOM;
+    memcpy(conn->bhc_our_rpa_addr.val, evt->local_rpa, 6);
+
+    /* If peer RPA is not set in the event and peer address
+     * is RPA then store the peer RPA address so when the peer
+     * address is resolved, the RPA is not forgotten.
+     */
+    if (memcmp(BLE_ADDR_ANY->val, evt->peer_rpa, 6) == 0) {
+        if (BLE_ADDR_IS_RPA(&conn->bhc_peer_addr)) {
+            conn->bhc_peer_rpa_addr = conn->bhc_peer_addr;
+        }
+    } else {
+        conn->bhc_peer_rpa_addr.type = BLE_ADDR_RANDOM;
+        memcpy(conn->bhc_peer_rpa_addr.val, evt->peer_rpa, 6);
+    }
+
+    ble_hs_lock();
+
+    memset(&event, 0, sizeof event);
+    ble_hs_conn_insert(conn);
+
+    ble_hs_unlock();
+
+    event.type = BLE_GAP_EVENT_CONNECT;
+    event.connect.conn_handle = evt->connection_handle;
+    event.connect.status = 0;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, evt->connection_handle);
+
+    ble_gap_rd_rem_sup_feat_tx(evt->connection_handle);
+
+    return 0;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+void
+ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used_feat *ev)
+{
+#if NIMBLE_BLE_CONNECT
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(le16toh(ev->conn_handle));
+    if ((conn != NULL) && (ev->status == 0)) {
+        conn->supported_feat = get_le32(ev->features);
+    }
+
+    ble_hs_unlock();
+#endif
+}
+
+int
+ble_gap_rx_l2cap_update_req(uint16_t conn_handle,
+                            struct ble_gap_upd_params *params)
+{
+    struct ble_gap_event event;
+    int rc;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_L2CAP_UPDATE_REQ;
+    event.conn_update_req.conn_handle = conn_handle;
+    event.conn_update_req.peer_params = params;
+
+    rc = ble_gap_call_conn_event_cb(&event, conn_handle);
+    return rc;
+}
+
+void
+ble_gap_rx_phy_update_complete(const struct ble_hci_ev_le_subev_phy_update_complete *ev)
+{
+    struct ble_gap_event event;
+    uint16_t conn_handle = le16toh(ev->conn_handle);
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_PHY_UPDATE_COMPLETE;
+    event.phy_updated.status = ev->status;
+    event.phy_updated.conn_handle = conn_handle;
+    event.phy_updated.tx_phy = ev->tx_phy;
+    event.phy_updated.rx_phy = ev->rx_phy;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+}
+
+static int32_t
+ble_gap_master_timer(void)
+{
+    uint32_t ticks_until_exp;
+    int rc;
+
+    ticks_until_exp = ble_gap_master_ticks_until_exp();
+    if (ticks_until_exp != 0) {
+        /* Timer not expired yet. */
+        return ticks_until_exp;
+    }
+
+    /*** Timer expired; process event. */
+
+    switch (ble_gap_master.op) {
+    case BLE_GAP_OP_M_CONN:
+        rc = ble_gap_conn_cancel_tx();
+        if (rc != 0) {
+            /* Failed to stop connecting; try again in 100 ms. */
+            return ble_npl_time_ms_to_ticks32(BLE_GAP_CANCEL_RETRY_TIMEOUT_MS);
+        } else {
+            /* Stop the timer now that the cancel command has been acked. */
+            ble_gap_master.exp_set = 0;
+
+            /* Timeout gets reported when we receive a connection complete
+             * event indicating the connect procedure has been cancelled.
+             */
+            /* XXX: Set a timer to reset the controller if a connection
+             * complete event isn't received within a reasonable interval.
+             */
+        }
+        break;
+
+    case BLE_GAP_OP_M_DISC:
+#if NIMBLE_BLE_SCAN && !MYNEWT_VAL(BLE_EXT_ADV)
+        /* When a discovery procedure times out, it is not a failure. */
+        rc = ble_gap_disc_enable_tx(0, 0);
+        if (rc != 0) {
+            /* Failed to stop discovery; try again in 100 ms. */
+            return ble_npl_time_ms_to_ticks32(BLE_GAP_CANCEL_RETRY_TIMEOUT_MS);
+        }
+
+        ble_gap_disc_complete();
+#else
+        assert(0);
+#endif
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+
+    return BLE_HS_FOREVER;
+}
+
+#if !MYNEWT_VAL(BLE_EXT_ADV)
+static int32_t
+ble_gap_slave_timer(void)
+{
+    uint32_t ticks_until_exp;
+    int rc;
+
+    ticks_until_exp = ble_gap_slave_ticks_until_exp();
+    if (ticks_until_exp != 0) {
+        /* Timer not expired yet. */
+        return ticks_until_exp;
+    }
+
+    /*** Timer expired; process event. */
+
+    /* Stop advertising. */
+    rc = ble_gap_adv_enable_tx(0);
+    if (rc != 0) {
+        /* Failed to stop advertising; try again in 100 ms. */
+        return 100;
+    }
+
+    /* Clear the timer and cancel the current procedure. */
+    ble_gap_slave_reset_state(0);
+
+    /* Indicate to application that advertising has stopped. */
+    ble_gap_adv_finished(0, BLE_HS_ETIMEOUT, 0, 0);
+
+    return BLE_HS_FOREVER;
+}
+#endif
+
+static int32_t
+ble_gap_update_timer(void)
+{
+    struct ble_gap_update_entry *entry;
+    int32_t ticks_until_exp;
+    uint16_t conn_handle;
+
+    do {
+        ble_hs_lock();
+
+        conn_handle = ble_gap_update_next_exp(&ticks_until_exp);
+        if (ticks_until_exp == 0) {
+            entry = ble_gap_update_entry_remove(conn_handle);
+        } else {
+            entry = NULL;
+        }
+
+        ble_hs_unlock();
+
+        if (entry != NULL) {
+            ble_gap_update_entry_free(entry);
+        }
+    } while (entry != NULL);
+
+    return ticks_until_exp;
+}
+
+int
+ble_gap_set_event_cb(uint16_t conn_handle, ble_gap_event_fn *cb, void *cb_arg)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        conn->bhc_cb = cb;
+        conn->bhc_cb_arg = cb_arg;
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles timed-out GAP procedures.
+ *
+ * @return                      The number of ticks until this function should
+ *                                  be called again.
+ */
+int32_t
+ble_gap_timer(void)
+{
+    int32_t update_ticks;
+    int32_t master_ticks;
+    int32_t min_ticks;
+
+    master_ticks = ble_gap_master_timer();
+    update_ticks = ble_gap_update_timer();
+
+    min_ticks = min(master_ticks, update_ticks);
+
+#if !MYNEWT_VAL(BLE_EXT_ADV)
+    min_ticks = min(min_ticks, ble_gap_slave_timer());
+#endif
+
+    return min_ticks;
+}
+
+/*****************************************************************************
+ * $white list                                                               *
+ *****************************************************************************/
+
+#if MYNEWT_VAL(BLE_WHITELIST)
+static int
+ble_gap_wl_busy(void)
+{
+    /* Check if an auto or selective connection establishment procedure is in
+     * progress.
+     */
+    return ble_gap_master.op == BLE_GAP_OP_M_CONN &&
+           ble_gap_master.conn.using_wl;
+}
+
+static int
+ble_gap_wl_tx_add(const ble_addr_t *addr)
+{
+    struct ble_hci_le_add_whte_list_cp cmd;
+
+    if (addr->type > BLE_ADDR_RANDOM) {
+        return BLE_HS_EINVAL;
+    }
+
+    memcpy(cmd.addr, addr->val, BLE_DEV_ADDR_LEN);
+    cmd.addr_type = addr->type;
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_ADD_WHITE_LIST),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_gap_wl_tx_clear(void)
+{
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_CLEAR_WHITE_LIST),
+                             NULL, 0, NULL, 0 );
+}
+#endif
+
+int
+ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count)
+{
+#if MYNEWT_VAL(BLE_WHITELIST)
+    int rc;
+    int i;
+
+    STATS_INC(ble_gap_stats, wl_set);
+
+    ble_hs_lock();
+
+    if (white_list_count == 0) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    for (i = 0; i < white_list_count; i++) {
+        if (addrs[i].type != BLE_ADDR_PUBLIC &&
+            addrs[i].type != BLE_ADDR_RANDOM) {
+
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
+    }
+
+    if (ble_gap_wl_busy()) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: set whitelist; ");
+    ble_gap_log_wl(addrs, white_list_count);
+    BLE_HS_LOG(INFO, "\n");
+
+    rc = ble_gap_wl_tx_clear();
+    if (rc != 0) {
+        goto done;
+    }
+
+    for (i = 0; i < white_list_count; i++) {
+        rc = ble_gap_wl_tx_add(addrs + i);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, wl_set_fail);
+    }
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+/*****************************************************************************
+ * $stop advertise                                                           *
+ *****************************************************************************/
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+static int
+ble_gap_adv_enable_tx(int enable)
+{
+    struct ble_hci_le_set_adv_enable_cp cmd;
+
+    cmd.enable = !!enable;
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_gap_adv_stop_no_lock(void)
+{
+    bool active;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    STATS_INC(ble_gap_stats, adv_stop);
+
+    active = ble_gap_adv_active();
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: stop advertising.\n");
+
+    rc = ble_gap_adv_enable_tx(0);
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_slave_reset_state(0);
+
+    if (!active) {
+        rc = BLE_HS_EALREADY;
+    } else {
+        rc = 0;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, adv_stop_fail);
+    }
+
+    return rc;
+}
+#endif
+
+int
+ble_gap_adv_stop(void)
+{
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+    int rc;
+
+    ble_hs_lock();
+    rc = ble_gap_adv_stop_no_lock();
+    ble_hs_unlock();
+
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+/*****************************************************************************
+ * $advertise                                                                *
+ *****************************************************************************/
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+static int
+ble_gap_adv_type(const struct ble_gap_adv_params *adv_params)
+{
+    switch (adv_params->conn_mode) {
+    case BLE_GAP_CONN_MODE_NON:
+        if (adv_params->disc_mode == BLE_GAP_DISC_MODE_NON) {
+            return BLE_HCI_ADV_TYPE_ADV_NONCONN_IND;
+        } else {
+            return BLE_HCI_ADV_TYPE_ADV_SCAN_IND;
+        }
+
+    case BLE_GAP_CONN_MODE_UND:
+        return BLE_HCI_ADV_TYPE_ADV_IND;
+
+    case BLE_GAP_CONN_MODE_DIR:
+        if (adv_params->high_duty_cycle) {
+            return BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD;
+        } else {
+            return BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD;
+        }
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HCI_ADV_TYPE_ADV_IND;
+    }
+}
+
+static void
+ble_gap_adv_dflt_itvls(uint8_t conn_mode,
+                       uint16_t *out_itvl_min, uint16_t *out_itvl_max)
+{
+    switch (conn_mode) {
+    case BLE_GAP_CONN_MODE_NON:
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL2_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL2_MAX;
+        break;
+
+    case BLE_GAP_CONN_MODE_UND:
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
+        break;
+
+    case BLE_GAP_CONN_MODE_DIR:
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
+        break;
+    }
+}
+
+static int
+ble_gap_adv_params_tx(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                      const struct ble_gap_adv_params *adv_params)
+
+{
+    const ble_addr_t *peer_any = BLE_ADDR_ANY;
+    struct ble_hci_le_set_adv_params_cp cmd;
+    uint16_t opcode;
+    uint16_t min;
+    uint16_t max;
+
+    /* Fill optional fields if application did not specify them. */
+    if ((adv_params->itvl_min == 0) && (adv_params->itvl_max == 0)) {
+        ble_gap_adv_dflt_itvls(adv_params->conn_mode, &min, &max);
+        cmd.min_interval = htole16(min);
+        cmd.max_interval = htole16(max);
+    } else {
+        cmd.min_interval = htole16(adv_params->itvl_min);
+        cmd.max_interval = htole16(adv_params->itvl_max);
+    }
+
+    cmd.type = ble_gap_adv_type(adv_params);
+    cmd.own_addr_type = own_addr_type;
+
+    if (peer_addr == NULL) {
+        peer_addr = peer_any;
+    }
+
+    cmd.peer_addr_type = peer_addr->type;
+    memcpy(&cmd.peer_addr, peer_addr->val, sizeof(cmd.peer_addr));
+
+    if (adv_params->channel_map == 0) {
+        cmd.chan_map = BLE_GAP_ADV_DFLT_CHANNEL_MAP;
+    } else {
+        cmd.chan_map = adv_params->channel_map;
+    }
+
+    /* Zero is the default value for filter policy and high duty cycle */
+    cmd.filter_policy = adv_params->filter_policy;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_PARAMS);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_gap_adv_validate(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                     const struct ble_gap_adv_params *adv_params)
+{
+    if (adv_params == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (adv_params->disc_mode >= BLE_GAP_DISC_MODE_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (ble_gap_slave[0].op != BLE_GAP_OP_NULL) {
+        return BLE_HS_EALREADY;
+    }
+
+    switch (adv_params->conn_mode) {
+    case BLE_GAP_CONN_MODE_NON:
+        /* High duty cycle only allowed for directed advertising. */
+        if (adv_params->high_duty_cycle) {
+            return BLE_HS_EINVAL;
+        }
+        break;
+
+    case BLE_GAP_CONN_MODE_UND:
+        /* High duty cycle only allowed for directed advertising. */
+        if (adv_params->high_duty_cycle) {
+            return BLE_HS_EINVAL;
+        }
+
+        /* Don't allow connectable advertising if we won't be able to allocate
+         * a new connection.
+         */
+        if (!ble_hs_conn_can_alloc()) {
+            return BLE_HS_ENOMEM;
+        }
+        break;
+
+    case BLE_GAP_CONN_MODE_DIR:
+        if (peer_addr == NULL) {
+            return BLE_HS_EINVAL;
+        }
+
+        if (peer_addr->type != BLE_ADDR_PUBLIC &&
+            peer_addr->type != BLE_ADDR_RANDOM &&
+            peer_addr->type != BLE_ADDR_PUBLIC_ID &&
+            peer_addr->type != BLE_ADDR_RANDOM_ID) {
+
+            return BLE_HS_EINVAL;
+        }
+
+        /* Don't allow connectable advertising if we won't be able to allocate
+         * a new connection.
+         */
+        if (!ble_hs_conn_can_alloc()) {
+            return BLE_HS_ENOMEM;
+        }
+        break;
+
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+#endif
+
+int
+ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr,
+                  int32_t duration_ms,
+                  const struct ble_gap_adv_params *adv_params,
+                  ble_gap_event_fn *cb, void *cb_arg)
+{
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+    uint32_t duration_ticks;
+    int rc;
+
+    STATS_INC(ble_gap_stats, adv_start);
+
+    ble_hs_lock();
+
+    rc = ble_gap_adv_validate(own_addr_type, direct_addr, adv_params);
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = ble_npl_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
+    }
+
+    if (!ble_hs_is_enabled()) {
+        rc = BLE_HS_EDISABLED;
+        goto done;
+    }
+
+    if (ble_gap_is_preempted()) {
+        rc = BLE_HS_EPREEMPTED;
+        goto done;
+    }
+
+    rc = ble_hs_id_use_addr(own_addr_type);
+    if (rc != 0) {
+        goto done;
+    }
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: advertise; ");
+    ble_gap_log_adv(own_addr_type, direct_addr, adv_params);
+    BLE_HS_LOG(INFO, "\n");
+
+    ble_gap_slave[0].cb = cb;
+    ble_gap_slave[0].cb_arg = cb_arg;
+    ble_gap_slave[0].our_addr_type = own_addr_type;
+
+    if (adv_params->conn_mode != BLE_GAP_CONN_MODE_NON) {
+        ble_gap_slave[0].connectable = 1;
+    } else {
+        ble_gap_slave[0].connectable = 0;
+    }
+
+    rc = ble_gap_adv_params_tx(own_addr_type, direct_addr, adv_params);
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_slave[0].op = BLE_GAP_OP_S_ADV;
+
+    rc = ble_gap_adv_enable_tx(1);
+    if (rc != 0) {
+        ble_gap_slave_reset_state(0);
+        goto done;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_slave_set_timer(duration_ticks);
+    }
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, adv_start_fail);
+    }
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_adv_set_data(const uint8_t *data, int data_len)
+{
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+    struct ble_hci_le_set_adv_data_cp cmd;
+    uint16_t opcode;
+
+    STATS_INC(ble_gap_stats, adv_set_data);
+
+    /* Check for valid parameters */
+    if (((data == NULL) && (data_len != 0)) ||
+            (data_len > BLE_HCI_MAX_ADV_DATA_LEN)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    memcpy(cmd.adv_data, data, data_len);
+    cmd.adv_data_len = data_len;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_DATA);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_adv_rsp_set_data(const uint8_t *data, int data_len)
+{
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+    struct ble_hci_le_set_scan_rsp_data_cp cmd;
+    uint16_t opcode;
+
+
+    /* Check for valid parameters */
+    if (((data == NULL) && (data_len != 0)) ||
+            (data_len > BLE_HCI_MAX_SCAN_RSP_DATA_LEN)) {
+        return BLE_HS_EINVAL;
+    }
+
+    memcpy(cmd.scan_rsp, data, data_len);
+    cmd.scan_rsp_len = data_len;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_adv_set_fields(const struct ble_hs_adv_fields *adv_fields)
+{
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+    uint8_t buf[BLE_HS_ADV_MAX_SZ];
+    uint8_t buf_sz;
+    int rc;
+
+    rc = ble_hs_adv_set_fields(adv_fields, buf, &buf_sz, sizeof buf);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gap_adv_set_data(buf, buf_sz);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_adv_rsp_set_fields(const struct ble_hs_adv_fields *rsp_fields)
+{
+#if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV)
+    uint8_t buf[BLE_HS_ADV_MAX_SZ];
+    uint8_t buf_sz;
+    int rc;
+
+    rc = ble_hs_adv_set_fields(rsp_fields, buf, &buf_sz, sizeof buf);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gap_adv_rsp_set_data(buf, buf_sz);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_adv_active(void)
+{
+    return ble_gap_adv_active_instance(0);
+}
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+static int
+ble_gap_ext_adv_params_tx(uint8_t instance,
+                          const struct ble_gap_ext_adv_params *params,
+                          int8_t *selected_tx_power)
+
+{
+    struct ble_hci_le_set_ext_adv_params_cp cmd;
+    struct ble_hci_le_set_ext_adv_params_rp rsp;
+    int rc;
+
+    memset(&cmd, 0, sizeof(cmd));
+
+    cmd.adv_handle = instance;
+
+    if (params->connectable) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE;
+    }
+    if (params->scannable) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE;
+    }
+    if (params->directed) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED;
+        cmd.peer_addr_type = params->peer.type;
+        memcpy(cmd.peer_addr, params->peer.val, BLE_DEV_ADDR_LEN);
+    }
+    if (params->high_duty_directed) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED;
+    }
+    if (params->legacy_pdu) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY;
+    }
+    if (params->anonymous) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV;
+    }
+    if (params->include_tx_power) {
+        cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR;
+    }
+
+    /* Fill optional fields if application did not specify them. */
+    if (params->itvl_min == 0 && params->itvl_max == 0) {
+        /* TODO for now limited to legacy values*/
+        put_le24(cmd.pri_itvl_min, BLE_GAP_ADV_FAST_INTERVAL1_MIN);
+        put_le24(cmd.pri_itvl_max, BLE_GAP_ADV_FAST_INTERVAL2_MAX);
+    } else {
+        put_le24(cmd.pri_itvl_min, params->itvl_min);
+        put_le24(cmd.pri_itvl_max, params->itvl_max);
+    }
+
+    if (params->channel_map == 0) {
+        cmd.pri_chan_map = BLE_GAP_ADV_DFLT_CHANNEL_MAP;
+    } else {
+        cmd.pri_chan_map = params->channel_map;
+    }
+
+    /* Zero is the default value for filter policy and high duty cycle */
+    cmd.filter_policy = params->filter_policy;
+    cmd.tx_power = params->tx_power;
+
+    if (params->legacy_pdu) {
+        cmd.pri_phy = BLE_HCI_LE_PHY_1M;
+        cmd.sec_phy = BLE_HCI_LE_PHY_1M;
+    } else {
+        cmd.pri_phy = params->primary_phy;
+        cmd.sec_phy = params->secondary_phy;
+    }
+
+    cmd.own_addr_type = params->own_addr_type;
+    cmd.sec_max_skip = 0;
+    cmd.sid = params->sid;
+    cmd.scan_req_notif = params->scan_req_notif;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM),
+                           &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (selected_tx_power) {
+        *selected_tx_power = rsp.tx_power;
+    }
+
+    return 0;
+}
+
+static int
+ble_gap_ext_adv_params_validate(const struct ble_gap_ext_adv_params *params)
+{
+    if (!params) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (params->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* Don't allow connectable advertising if we won't be able to allocate
+     * a new connection.
+     */
+    if (params->connectable && !ble_hs_conn_can_alloc()) {
+        return BLE_HS_ENOMEM;
+    }
+
+    if (params->legacy_pdu) {
+        /* not allowed for legacy PDUs */
+        if (params->anonymous || params->include_tx_power) {
+            return BLE_HS_EINVAL;
+        }
+    }
+
+    if (params->directed) {
+        if (params->scannable && params->connectable) {
+            return BLE_HS_EINVAL;
+        }
+    }
+
+    if (!params->legacy_pdu) {
+        /* not allowed for extended advertising PDUs */
+        if (params->connectable && params->scannable) {
+            return BLE_HS_EINVAL;
+        }
+
+        /* HD directed advertising allowed only for legacy PDUs */
+        if (params->high_duty_directed) {
+            return BLE_HS_EINVAL;
+        }
+    }
+
+    return 0;
+}
+
+int
+ble_gap_ext_adv_configure(uint8_t instance,
+                          const struct ble_gap_ext_adv_params *params,
+                          int8_t *selected_tx_power,
+                          ble_gap_event_fn *cb, void *cb_arg)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    rc = ble_gap_ext_adv_params_validate(params);
+    if (rc) {
+        return rc;
+    }
+
+    ble_hs_lock();
+
+    if (ble_gap_adv_active_instance(instance)) {
+        ble_hs_unlock();
+        return BLE_HS_EBUSY;
+    }
+
+    rc = ble_gap_ext_adv_params_tx(instance, params, selected_tx_power);
+    if (rc) {
+        ble_hs_unlock();
+        return rc;
+    }
+
+    ble_gap_slave[instance].configured = 1;
+    ble_gap_slave[instance].cb = cb;
+    ble_gap_slave[instance].cb_arg = cb_arg;
+    ble_gap_slave[instance].our_addr_type = params->own_addr_type;
+
+    ble_gap_slave[instance].connectable = params->connectable;
+    ble_gap_slave[instance].scannable = params->scannable;
+    ble_gap_slave[instance].directed = params->directed;
+    ble_gap_slave[instance].high_duty_directed = params->high_duty_directed;
+    ble_gap_slave[instance].legacy_pdu = params->legacy_pdu;
+
+    ble_hs_unlock();
+    return 0;
+}
+
+static int
+ble_gap_ext_adv_set_addr_no_lock(uint8_t instance, const uint8_t *addr)
+{
+    struct ble_hci_le_set_adv_set_rnd_addr_cp cmd;
+    int rc;
+
+    cmd.adv_handle = instance;
+    memcpy(cmd.addr, addr, BLE_DEV_ADDR_LEN);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_SET_ADV_SET_RND_ADDR),
+                           &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_gap_slave[instance].rnd_addr_set = 1;
+    memcpy(ble_gap_slave[instance].rnd_addr, addr, 6);
+
+    return 0;
+}
+
+int
+ble_gap_ext_adv_set_addr(uint8_t instance, const ble_addr_t *addr)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES || addr->type != BLE_ADDR_RANDOM) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    rc = ble_gap_ext_adv_set_addr_no_lock(instance, addr->val);
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events)
+{
+    struct ble_hci_le_set_ext_adv_enable_cp *cmd;
+    uint8_t buf[sizeof(*cmd) + sizeof(cmd->sets[0])];
+    const uint8_t *rnd_addr;
+    uint16_t opcode;
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    if (!ble_gap_slave[instance].configured) {
+        ble_hs_unlock();
+        return BLE_HS_EINVAL;
+    }
+
+    if (ble_gap_slave[instance].op != BLE_GAP_OP_NULL) {
+        ble_hs_unlock();
+        return  BLE_HS_EALREADY;
+    }
+
+    /* HD directed duration shall not be 0 or larger than >1.28s */
+    if (ble_gap_slave[instance].high_duty_directed &&
+            ((duration == 0) || (duration > 128)) ) {
+        ble_hs_unlock();
+        return BLE_HS_EINVAL;
+    }
+
+    /* verify own address type if random address for instance wasn't explicitly
+     * set
+     */
+    switch (ble_gap_slave[instance].our_addr_type) {
+    case BLE_OWN_ADDR_RANDOM:
+    case BLE_OWN_ADDR_RPA_RANDOM_DEFAULT:
+        if (ble_gap_slave[instance].rnd_addr_set) {
+            break;
+        }
+    /* fall through */
+    case BLE_OWN_ADDR_PUBLIC:
+    case BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT:
+    default:
+        rc = ble_hs_id_use_addr(ble_gap_slave[instance].our_addr_type);
+        if (rc) {
+            ble_hs_unlock();
+            return BLE_HS_EINVAL;
+        }
+        break;
+    }
+
+    /* fallback to ID static random address if using random address and instance
+     * wasn't configured with own address
+     */
+    if (!ble_gap_slave[instance].rnd_addr_set) {
+        switch (ble_gap_slave[instance].our_addr_type) {
+        case BLE_OWN_ADDR_RANDOM:
+        case BLE_OWN_ADDR_RPA_RANDOM_DEFAULT:
+            rc = ble_hs_id_addr(BLE_ADDR_RANDOM, &rnd_addr, NULL);
+            if (rc != 0) {
+                ble_hs_unlock();
+                return rc;
+            }
+
+            rc = ble_gap_ext_adv_set_addr_no_lock(instance, rnd_addr);
+            if (rc != 0) {
+                ble_hs_unlock();
+                return rc;
+            }
+            break;
+        default:
+            break;
+        }
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE);
+
+    cmd = (void *) buf;
+
+    cmd->enable = 0x01;
+    cmd->num_sets = 1;
+
+    cmd->sets[0].adv_handle = instance;
+    cmd->sets[0].duration = htole16(duration);
+    cmd->sets[0].max_events = max_events;
+
+    rc = ble_hs_hci_cmd_tx(opcode, cmd, sizeof(buf), NULL, 0);
+    if (rc != 0) {
+        ble_hs_unlock();
+        return rc;
+    }
+
+    ble_gap_slave[instance].op = BLE_GAP_OP_S_ADV;
+
+    ble_hs_unlock();
+    return 0;
+}
+
+static int
+ble_gap_ext_adv_stop_no_lock(uint8_t instance)
+{
+    struct ble_hci_le_set_ext_adv_enable_cp *cmd;
+    uint8_t buf[sizeof(*cmd) + sizeof(cmd->sets[0])];
+    uint16_t opcode;
+    bool active;
+    int rc;
+
+    if (!ble_gap_slave[instance].configured) {
+        return BLE_HS_EINVAL;
+    }
+
+    active = ble_gap_adv_active_instance(instance);
+
+    cmd = (void *) buf;
+
+    cmd->enable = 0x00;
+    cmd->num_sets = 1;
+    cmd->sets[0].adv_handle = instance;
+    cmd->sets[0].duration = 0x0000;
+    cmd->sets[0].max_events = 0x00;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE);
+
+    rc = ble_hs_hci_cmd_tx(opcode, cmd, sizeof(buf), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_gap_slave[instance].op = BLE_GAP_OP_NULL;
+
+    if (!active) {
+        return BLE_HS_EALREADY;
+    } else {
+        return 0;
+    }
+}
+
+int
+ble_gap_ext_adv_stop(uint8_t instance)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    rc = ble_gap_ext_adv_stop_no_lock(instance);
+    ble_hs_unlock();
+
+    return rc;
+}
+
+
+static int
+ble_gap_ext_adv_set_data_validate(uint8_t instance, struct os_mbuf *data)
+{
+    uint16_t len = OS_MBUF_PKTLEN(data);
+
+    if (!ble_gap_slave[instance].configured) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* not allowed with directed advertising for legacy*/
+    if (ble_gap_slave[instance].legacy_pdu && ble_gap_slave[instance].directed) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* always allowed with legacy PDU but limited to legacy length */
+    if (ble_gap_slave[instance].legacy_pdu) {
+        if (len > BLE_HS_ADV_MAX_SZ) {
+            return BLE_HS_EINVAL;
+        }
+
+        return 0;
+    }
+
+    /* if already advertising, data must fit in single HCI command
+     * as per BT 5.0 Vol 2, Part E, 7.8.54. Don't bother Controller with such
+     * a request.
+     */
+    if (ble_gap_slave[instance].op == BLE_GAP_OP_S_ADV) {
+        if (len > min(MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE), 251)) {
+            return BLE_HS_EINVAL;
+        }
+    }
+
+    /* not allowed with scannable advertising */
+    if (ble_gap_slave[instance].scannable) {
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+static int
+ble_gap_ext_adv_set(uint8_t instance, uint16_t opcode, struct os_mbuf **data)
+{
+    /* in that case we always fit all data in single HCI command */
+#if MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE) <= BLE_HCI_MAX_EXT_ADV_DATA_LEN
+    static uint8_t buf[sizeof(struct ble_hci_le_set_ext_adv_data_cp) + \
+                       MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)];
+    struct ble_hci_le_set_ext_adv_data_cp *cmd = (void *)buf;
+    uint16_t len = OS_MBUF_PKTLEN(*data);
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, opcode);
+    cmd->adv_handle = instance;
+    cmd->operation = BLE_HCI_LE_SET_DATA_OPER_COMPLETE;
+    cmd->fragment_pref = 0;
+    cmd->adv_data_len = len;
+    os_mbuf_copydata(*data, 0, len, cmd->adv_data);
+
+    os_mbuf_adj(*data, len);
+    *data = os_mbuf_trim_front(*data);
+
+    return ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                             NULL, 0);
+#else
+    static uint8_t buf[sizeof(struct ble_hci_le_set_ext_adv_data_cp) + \
+                       BLE_HCI_MAX_EXT_ADV_DATA_LEN];
+    struct ble_hci_le_set_ext_adv_data_cp *cmd = (void *)buf;
+    uint16_t len = OS_MBUF_PKTLEN(*data);
+    uint8_t op;
+    int rc;
+
+    opcode =  BLE_HCI_OP(BLE_HCI_OGF_LE, opcode);
+
+    cmd->adv_handle = instance;
+
+    /* complete data */
+    if (len <= BLE_HCI_MAX_EXT_ADV_DATA_LEN) {
+        cmd->operation = BLE_HCI_LE_SET_DATA_OPER_COMPLETE;
+        cmd->fragment_pref = 0;
+        cmd->adv_data_len = len;
+        os_mbuf_copydata(*data, 0, len, cmd->adv_data);
+
+        os_mbuf_adj(*data, len);
+        *data = os_mbuf_trim_front(*data);
+
+        return ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                                 NULL, 0);
+    }
+
+    /* first fragment  */
+    op = BLE_HCI_LE_SET_DATA_OPER_FIRST;
+
+    do {
+        cmd->operation = op;
+        cmd->fragment_pref = 0;
+        cmd->adv_data_len = BLE_HCI_MAX_EXT_ADV_DATA_LEN;
+        os_mbuf_copydata(*data, 0, BLE_HCI_MAX_EXT_ADV_DATA_LEN, cmd->adv_data);
+
+        os_mbuf_adj(*data, BLE_HCI_MAX_EXT_ADV_DATA_LEN);
+        *data = os_mbuf_trim_front(*data);
+
+        rc = ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                               NULL, 0);
+        if (rc) {
+            return rc;
+        }
+
+        len -= BLE_HCI_MAX_EXT_ADV_DATA_LEN;
+        op = BLE_HCI_LE_SET_DATA_OPER_INT;
+    } while (len > BLE_HCI_MAX_EXT_ADV_DATA_LEN);
+
+    /* last fragment */
+    cmd->operation = BLE_HCI_LE_SET_DATA_OPER_LAST;
+    cmd->fragment_pref = 0;
+    cmd->adv_data_len = len;
+    os_mbuf_copydata(*data, 0, len, cmd->adv_data);
+
+    os_mbuf_adj(*data, len);
+    *data = os_mbuf_trim_front(*data);
+
+    return ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                             NULL, 0);
+#endif
+}
+
+int
+ble_gap_ext_adv_set_data(uint8_t instance, struct os_mbuf *data)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    ble_hs_lock();
+    rc = ble_gap_ext_adv_set_data_validate(instance, data);
+    if (rc != 0) {
+        ble_hs_unlock();
+        goto done;
+    }
+
+    rc = ble_gap_ext_adv_set(instance, BLE_HCI_OCF_LE_SET_EXT_ADV_DATA, &data);
+
+    ble_hs_unlock();
+
+done:
+    os_mbuf_free_chain(data);
+    return rc;
+}
+
+static int
+ble_gap_ext_adv_rsp_set_validate(uint8_t instance,  struct os_mbuf *data)
+{
+    uint16_t len = OS_MBUF_PKTLEN(data);
+
+    if (!ble_gap_slave[instance].configured) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* not allowed with directed advertising */
+    if (ble_gap_slave[instance].directed && ble_gap_slave[instance].connectable) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* only allowed with scannable advertising */
+    if (!ble_gap_slave[instance].scannable) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* with legacy PDU limited to legacy length */
+    if (ble_gap_slave[instance].legacy_pdu) {
+        if (len > BLE_HS_ADV_MAX_SZ) {
+            return BLE_HS_EINVAL;
+        }
+
+        return 0;
+    }
+
+    /* if already advertising, data must fit in single HCI command
+     * as per BT 5.0 Vol 2, Part E, 7.8.55. Don't bother Controller with such
+     * a request.
+     */
+    if (ble_gap_slave[instance].op == BLE_GAP_OP_S_ADV) {
+        if (len > min(MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE), 251)) {
+            return BLE_HS_EINVAL;
+        }
+    }
+
+    return 0;
+}
+
+int
+ble_gap_ext_adv_rsp_set_data(uint8_t instance, struct os_mbuf *data)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    ble_hs_lock();
+    rc = ble_gap_ext_adv_rsp_set_validate(instance, data);
+    if (rc != 0) {
+        ble_hs_unlock();
+        goto done;
+    }
+
+    rc = ble_gap_ext_adv_set(instance, BLE_HCI_OCF_LE_SET_EXT_SCAN_RSP_DATA,
+                             &data);
+
+    ble_hs_unlock();
+
+done:
+    os_mbuf_free_chain(data);
+    return rc;
+}
+
+int
+ble_gap_ext_adv_remove(uint8_t instance)
+{
+    struct ble_hci_le_remove_adv_set_cp cmd;
+    uint16_t opcode;
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    if (!ble_gap_slave[instance].configured) {
+        ble_hs_unlock();
+        return BLE_HS_EALREADY;
+    }
+
+    if (ble_gap_slave[instance].op == BLE_GAP_OP_S_ADV) {
+        ble_hs_unlock();
+        return BLE_HS_EBUSY;
+    }
+
+    cmd.adv_handle = instance;
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REMOVE_ADV_SET);
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        ble_hs_unlock();
+        return rc;
+    }
+
+    memset(&ble_gap_slave[instance], 0, sizeof(struct ble_gap_slave_state));
+    ble_hs_unlock();
+
+    return 0;
+}
+
+int
+ble_gap_ext_adv_clear(void)
+{
+    int rc;
+    uint8_t instance;
+    uint16_t opcode;
+
+    ble_hs_lock();
+
+    for (instance = 0; instance < BLE_ADV_INSTANCES; instance++) {
+        /* If there is an active instance or periodic adv instance,
+         * Don't send the command
+         * */
+        if ((ble_gap_slave[instance].op == BLE_GAP_OP_S_ADV)) {
+            ble_hs_unlock();
+            return BLE_HS_EBUSY;
+        }
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+        if (ble_gap_slave[instance].periodic_op == BLE_GAP_OP_S_PERIODIC_ADV) {
+            ble_hs_unlock();
+            return BLE_HS_EBUSY;
+        }
+#endif
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLEAR_ADV_SETS);
+
+    rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, NULL, 0);
+    if (rc != 0) {
+        ble_hs_unlock();
+        return rc;
+    }
+
+    memset(ble_gap_slave, 0, sizeof(ble_gap_slave));
+    ble_hs_unlock();
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+static int
+ble_gap_periodic_adv_params_tx(uint8_t instance,
+                               const struct ble_gap_periodic_adv_params *params)
+
+{
+    struct ble_hci_le_set_periodic_adv_params_cp cmd;
+    uint16_t opcode;
+
+    cmd.adv_handle = instance;
+
+    /* Fill optional fields if application did not specify them. */
+    if (params->itvl_min == 0 && params->itvl_max == 0) {
+        /* TODO defines for those */
+        cmd.min_itvl = htole16(30 / 1.25);   //30 ms
+        cmd.max_itvl = htole16(60 / 1.25);   //150 ms
+
+    } else {
+        cmd.min_itvl = htole16( params->itvl_min);
+        cmd.max_itvl = htole16(params->itvl_max);
+    }
+
+    if (params->include_tx_power) {
+        cmd.props = BLE_HCI_LE_SET_PERIODIC_ADV_PROP_INC_TX_PWR;
+    } else {
+        cmd.props = 0;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_PARAMS);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_gap_periodic_adv_params_validate(
+        const struct ble_gap_periodic_adv_params *params)
+{
+    if (!params) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (params->itvl_min && params->itvl_min < 6) {
+        return BLE_HS_EINVAL;
+    }
+    if (params->itvl_max && params->itvl_max < 6) {
+            return BLE_HS_EINVAL;
+    }
+    return 0;
+}
+
+int
+ble_gap_periodic_adv_configure(uint8_t instance,
+        const struct ble_gap_periodic_adv_params *params)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    rc = ble_gap_periodic_adv_params_validate(params);
+    if (rc) {
+        return rc;
+    }
+
+    ble_hs_lock();
+
+    /* The corresponding extended advertising instance should be configured */
+    if (!ble_gap_slave[instance].configured) {
+        ble_hs_unlock();
+        return ENOMEM;
+    }
+
+    /* Periodic advertising shall not be configured while it is already
+     * running.
+     * Bluetooth Core Specification, Section 7.8.61
+     */
+    if (ble_gap_slave[instance].periodic_op == BLE_GAP_OP_S_PERIODIC_ADV) {
+        ble_hs_unlock();
+        return BLE_HS_EINVAL;
+    }
+
+    rc = ble_gap_periodic_adv_params_tx(instance, params);
+    if (rc) {
+        ble_hs_unlock();
+        return rc;
+    }
+
+    ble_gap_slave[instance].periodic_configured = 1;
+
+    ble_hs_unlock();
+
+    return 0;
+}
+
+int
+ble_gap_periodic_adv_start(uint8_t instance)
+{
+    struct ble_hci_le_set_periodic_adv_enable_cp cmd;
+    uint16_t opcode;
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+
+    /* Periodic advertising cannot start unless it is configured before */
+    if (!ble_gap_slave[instance].periodic_configured) {
+        ble_hs_unlock();
+        return BLE_HS_EINVAL;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE);
+
+    cmd.enable = 0x01;
+    cmd.adv_handle = instance;
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        ble_hs_unlock();
+        return rc;
+    }
+
+    ble_gap_slave[instance].periodic_op = BLE_GAP_OP_S_PERIODIC_ADV;
+
+    ble_hs_unlock();
+    return 0;
+}
+
+static int
+ble_gap_periodic_adv_set(uint8_t instance, struct os_mbuf **data)
+{
+    /* In that case we always fit all data in single HCI command */
+#if MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE) <= BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN
+    static uint8_t buf[sizeof(struct ble_hci_le_set_periodic_adv_data_cp) +
+                       MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)];
+    struct ble_hci_le_set_periodic_adv_data_cp *cmd = (void *) buf;
+    uint16_t len = OS_MBUF_PKTLEN(*data);
+    uint16_t opcode;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA);
+
+    cmd->adv_handle = instance;
+    cmd->operation = BLE_HCI_LE_SET_DATA_OPER_COMPLETE;
+    cmd->adv_data_len = len;
+    os_mbuf_copydata(*data, 0, len, cmd->adv_data);
+
+    os_mbuf_adj(*data, len);
+    *data = os_mbuf_trim_front(*data);
+
+    return ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                             NULL, 0);
+#else
+    static uint8_t buf[sizeof(struct ble_hci_le_set_periodic_adv_data_cp) +
+                       BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN];
+    struct ble_hci_le_set_periodic_adv_data_cp *cmd = (void *) buf;
+    uint16_t len = OS_MBUF_PKTLEN(*data);
+    uint16_t opcode;
+    uint8_t op;
+    int rc;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA);
+    cmd->adv_handle = instance;
+
+    /* Complete data */
+    if (len <= BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN) {
+        cmd->operation = BLE_HCI_LE_SET_DATA_OPER_COMPLETE;
+        cmd->adv_data_len = len;
+        os_mbuf_copydata(*data, 0, len, cmd->adv_data);
+
+        os_mbuf_adj(*data, len);
+        *data = os_mbuf_trim_front(*data);
+
+        return ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                                 NULL, 0);
+    }
+
+    /* If the periodic advertising is already enabled, the periodic advertising
+     * the op code shall be nothing but 0x03
+     * Bluetooth Core Specification, section 7.8.62
+     */
+    if (ble_gap_slave[instance].periodic_op == BLE_GAP_OP_S_PERIODIC_ADV) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* First fragment  */
+    op = BLE_HCI_LE_SET_DATA_OPER_FIRST;
+
+    do{
+        cmd->operation = op;
+        cmd->adv_data_len = BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN;
+        os_mbuf_copydata(*data, 0, BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN,
+                         cmd->adv_data);
+
+        os_mbuf_adj(*data, BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN);
+        *data = os_mbuf_trim_front(*data);
+
+        rc = ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                               NULL, 0);
+        if (rc) {
+            return rc;
+        }
+
+        len -= BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN;
+        op = BLE_HCI_LE_SET_DATA_OPER_INT;
+    } while (len > BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN);
+
+    /* Last fragment */
+    cmd->operation = BLE_HCI_LE_SET_DATA_OPER_LAST;
+    cmd->adv_data_len = len;
+    os_mbuf_copydata(*data, 0, len, cmd->adv_data);
+
+    os_mbuf_adj(*data, len);
+    *data = os_mbuf_trim_front(*data);
+
+    return ble_hs_hci_cmd_tx(opcode, cmd, sizeof(*cmd) + cmd->adv_data_len,
+                             NULL, 0);
+#endif
+}
+
+static int
+ble_gap_periodic_adv_set_data_validate(uint8_t instance,
+                                       struct os_mbuf *data)
+{
+    /* The corresponding extended advertising instance should be configured */
+    if (!ble_gap_slave[instance].configured) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (ble_gap_slave[instance].legacy_pdu) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* One more check states that if the periodic advertising is already
+     * enabled, the operation shall be 0x03 (Complete).
+     * This check is handled during sending the data to the controller, as the
+     * length checks are already checked there, so this saves duplicate code
+     */
+
+    return 0;
+}
+
+int
+ble_gap_periodic_adv_set_data(uint8_t instance, struct os_mbuf *data)
+{
+    int rc;
+    if (instance >= BLE_ADV_INSTANCES) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    ble_hs_lock();
+
+    rc = ble_gap_periodic_adv_set_data_validate(instance, data);
+    if (rc != 0) {
+        ble_hs_unlock();
+        goto done;
+    }
+
+    rc = ble_gap_periodic_adv_set(instance, &data);
+
+    ble_hs_unlock();
+
+done:
+    os_mbuf_free_chain(data);
+    return rc;
+}
+
+static int
+ble_gap_periodic_adv_stop_no_lock(uint8_t instance)
+{
+    struct ble_hci_le_set_periodic_adv_enable_cp cmd;
+    uint16_t opcode;
+    int rc;
+
+    cmd.enable = 0x00;
+    cmd.adv_handle = instance;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE);
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_gap_slave[instance].periodic_op = BLE_GAP_OP_NULL;
+
+    return 0;
+}
+
+int
+ble_gap_periodic_adv_stop(uint8_t instance)
+{
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    rc = ble_gap_periodic_adv_stop_no_lock(instance);
+    ble_hs_unlock();
+
+    return rc;
+}
+
+static void
+ble_gap_npl_sync_lost(struct ble_npl_event *ev)
+{
+    struct ble_hs_periodic_sync *psync;
+    struct ble_gap_event event;
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    /* this psync is no longer on list so no lock needed */
+    psync = ble_npl_event_get_arg(ev);
+    cb = psync->cb;
+    cb_arg = psync->cb_arg;
+
+    memset(&event, 0, sizeof event);
+
+    event.type = BLE_GAP_EVENT_PERIODIC_SYNC_LOST;
+    event.periodic_sync_lost.sync_handle = psync->sync_handle;
+    event.periodic_sync_lost.reason = BLE_HS_EDONE;
+
+    /* Free the memory occupied by psync as it is no longer needed */
+    ble_hs_periodic_sync_free(psync);
+
+    ble_gap_event_listener_call(&event);
+    if (cb) {
+        cb(&event, cb_arg);
+    }
+}
+
+int
+ble_gap_periodic_adv_sync_create(const ble_addr_t *addr, uint8_t adv_sid,
+                                 const struct ble_gap_periodic_sync_params *params,
+                                 ble_gap_event_fn *cb, void *cb_arg)
+{
+    struct ble_hci_le_periodic_adv_create_sync_cp cmd;
+    struct ble_hs_periodic_sync *psync;
+    uint16_t opcode;
+    int rc;
+
+    if (addr && (addr->type > BLE_ADDR_RANDOM)) {
+        return BLE_HS_EINVAL;
+    }
+    if (adv_sid > 0x0f) {
+        return BLE_HS_EINVAL;
+    }
+    if ((params->skip > 0x1f3) || (params->sync_timeout > 0x4000) ||
+            (params->sync_timeout < 0x0A)) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+
+    /* No sync can be created if another sync is still pending */
+    if (ble_gap_sync.op == BLE_GAP_OP_SYNC) {
+        ble_hs_unlock();
+        return BLE_HS_EBUSY;
+    }
+
+    /* cannot create another sync if already synchronized */
+    if (ble_hs_periodic_sync_find(addr, adv_sid)) {
+        ble_hs_unlock();
+        return BLE_HS_EALREADY;
+    }
+
+    /* preallocate sync element */
+    psync = ble_hs_periodic_sync_alloc();
+    if (!psync) {
+        ble_hs_unlock();
+        return BLE_HS_ENOMEM;
+    }
+
+    ble_npl_event_init(&psync->lost_ev, ble_gap_npl_sync_lost, psync);
+
+    if (addr) {
+        cmd.options = 0x00;
+        cmd.peer_addr_type = addr->type;
+        memcpy(cmd.peer_addr, addr->val, BLE_DEV_ADDR_LEN);
+    } else {
+        cmd.options = 0x01;
+        cmd.peer_addr_type = BLE_ADDR_ANY->type;
+        memcpy(cmd.peer_addr, BLE_ADDR_ANY->val, BLE_DEV_ADDR_LEN);
+    }
+
+    cmd.sid = adv_sid;
+    cmd.skip = params->skip;
+    cmd.sync_timeout = htole16(params->sync_timeout);
+    cmd.sync_cte_type = 0x00;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC);
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+    if (!rc) {
+        /* This shall be reset upon receiving sync_established event,
+         * or if the sync is cancelled before receiving that event.
+         */
+        ble_gap_sync.op = BLE_GAP_OP_SYNC;
+        ble_gap_sync.cb = cb;
+        ble_gap_sync.cb_arg = cb_arg;
+        ble_gap_sync.psync = psync;
+    } else {
+        ble_hs_periodic_sync_free(psync);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_gap_periodic_adv_sync_create_cancel(void)
+{
+    uint16_t opcode;
+    int rc = 0;
+
+    ble_hs_lock();
+
+    if (ble_gap_sync.op != BLE_GAP_OP_SYNC) {
+        ble_hs_unlock();
+        return BLE_HS_EBUSY;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE,
+                        BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC_CANCEL);
+
+    rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, NULL, 0);
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_gap_periodic_adv_sync_terminate(uint16_t sync_handle)
+{
+    struct ble_hci_le_periodic_adv_term_sync_cp cmd;
+    struct ble_hs_periodic_sync *psync;
+    uint16_t opcode;
+    int rc;
+
+    ble_hs_lock();
+
+    if (ble_gap_sync.op == BLE_GAP_OP_SYNC) {
+        ble_hs_unlock();
+        return BLE_HS_EBUSY;
+    }
+
+    /* The handle must be in the list. If it doesn't exist, it means
+     * that the sync may have been lost at the same moment in which
+     * the app wants to terminate that sync handle
+     */
+    psync = ble_hs_periodic_sync_find_by_handle(sync_handle);
+    if (!psync) {
+        /* Sync already terminated.*/
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC);
+
+    cmd.sync_handle = htole16(sync_handle);
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+    if (rc == 0) {
+        /* Remove the handle from the list */
+        ble_hs_periodic_sync_remove(psync);
+
+        /* send sync_lost event, this is to mimic connection behavior and thus
+         * simplify application error handling
+         */
+        ble_npl_eventq_put(ble_hs_evq_get(), &psync->lost_ev);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+int
+ble_gap_periodic_adv_sync_reporting(uint16_t sync_handle, bool enable)
+{
+    struct ble_hci_le_periodic_adv_receive_enable_cp cmd;
+    struct ble_hs_periodic_sync *psync;
+    uint16_t opcode;
+    int rc;
+
+    ble_hs_lock();
+
+    if (ble_gap_sync.op == BLE_GAP_OP_SYNC) {
+        ble_hs_unlock();
+        return BLE_HS_EBUSY;
+    }
+
+    psync = ble_hs_periodic_sync_find_by_handle(sync_handle);
+    if (!psync) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_RECEIVE_ENABLE);
+
+    cmd.sync_handle = htole16(sync_handle);
+    cmd.enable = enable ? 0x01 : 0x00;
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_gap_periodic_adv_sync_transfer(uint16_t sync_handle, uint16_t conn_handle,
+                                   uint16_t service_data)
+{
+    struct ble_hci_le_periodic_adv_sync_transfer_cp cmd;
+    struct ble_hci_le_periodic_adv_sync_transfer_rp rsp;
+    struct ble_hs_periodic_sync *psync;
+    struct ble_hs_conn *conn;
+    uint16_t opcode;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    psync = ble_hs_periodic_sync_find_by_handle(sync_handle);
+    if (!psync) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER);
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.sync_handle = htole16(sync_handle);
+    cmd.service_data = htole16(service_data);
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (!rc) {
+        BLE_HS_DBG_ASSERT(le16toh(rsp.conn_handle) == conn_handle);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_gap_periodic_adv_sync_set_info(uint8_t instance, uint16_t conn_handle,
+                                   uint16_t service_data)
+{
+    struct ble_hci_le_periodic_adv_set_info_transfer_cp cmd;
+    struct ble_hci_le_periodic_adv_set_info_transfer_rp rsp;
+    struct ble_hs_conn *conn;
+    uint16_t opcode;
+    int rc;
+
+    if (instance >= BLE_ADV_INSTANCES) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    if (ble_gap_slave[instance].periodic_op != BLE_GAP_OP_S_PERIODIC_ADV) {
+        /* periodic adv not enabled */
+        ble_hs_unlock();
+        return BLE_HS_EINVAL;
+    }
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER);
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.adv_handle = instance;
+    cmd.service_data = htole16(service_data);
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (!rc) {
+        BLE_HS_DBG_ASSERT(le16toh(rsp.conn_handle) == conn_handle);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+static int
+periodic_adv_transfer_enable(uint16_t conn_handle,
+                             const struct ble_gap_periodic_sync_params *params)
+{
+    struct ble_hci_le_periodic_adv_sync_transfer_params_cp cmd;
+    struct ble_hci_le_periodic_adv_sync_transfer_params_rp rsp;
+    uint16_t opcode;
+    int rc;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS);
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.sync_cte_type = 0x00;
+    cmd.mode = params->reports_disabled ? 0x01 : 0x02;
+    cmd.skip = htole16(params->skip);
+    cmd.sync_timeout = htole16(params->sync_timeout);
+
+    rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (!rc) {
+        BLE_HS_DBG_ASSERT(le16toh(rsp.conn_handle) == conn_handle);
+    }
+
+    return rc;
+}
+
+int
+ble_gap_periodic_adv_sync_receive(uint16_t conn_handle,
+                                  const struct ble_gap_periodic_sync_params *params,
+                                  ble_gap_event_fn *cb, void *cb_arg)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    if (params) {
+        if (conn->psync) {
+            ble_hs_unlock();
+            return BLE_HS_EALREADY;
+        }
+
+        conn->psync = ble_hs_periodic_sync_alloc();
+        if (!conn->psync) {
+            ble_hs_unlock();
+            return BLE_HS_ENOMEM;
+        }
+
+        rc = periodic_adv_transfer_enable(conn_handle, params);
+        if (rc) {
+            ble_hs_periodic_sync_free(conn->psync);
+            conn->psync = NULL;
+        } else {
+            conn->psync->cb = cb;
+            conn->psync->cb_arg = cb_arg;
+            ble_npl_event_init(&conn->psync->lost_ev, ble_gap_npl_sync_lost,
+                               conn->psync);
+        }
+    } else {
+        if (!conn->psync) {
+            ble_hs_unlock();
+            return BLE_HS_EALREADY;
+        }
+
+        rc = periodic_adv_transfer_disable(conn_handle);
+        if (!rc) {
+            ble_hs_periodic_sync_free(conn->psync);
+            conn->psync = NULL;
+        }
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+#endif
+
+int
+ble_gap_add_dev_to_periodic_adv_list(const ble_addr_t *peer_addr,
+                                     uint8_t adv_sid)
+{
+    struct ble_hci_le_add_dev_to_periodic_adv_list_cp cmd;
+    uint16_t opcode;
+
+    if ((peer_addr->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd.peer_addr_type = peer_addr->type;
+    memcpy(cmd.peer_addr, peer_addr->val, BLE_DEV_ADDR_LEN);
+    cmd.sid = adv_sid;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE,
+                        BLE_HCI_OCF_LE_ADD_DEV_TO_PERIODIC_ADV_LIST);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_gap_rem_dev_from_periodic_adv_list(const ble_addr_t *peer_addr, uint8_t adv_sid)
+{
+    struct ble_hci_le_rem_dev_from_periodic_adv_list_cp cmd;
+    uint16_t opcode;
+
+    if ((peer_addr->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd.peer_addr_type = peer_addr->type;
+    memcpy(cmd.peer_addr, peer_addr->val, BLE_DEV_ADDR_LEN);
+    cmd.sid = adv_sid;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE,
+                        BLE_HCI_OCF_LE_REM_DEV_FROM_PERIODIC_ADV_LIST);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_gap_clear_periodic_adv_list(void)
+{
+    uint16_t opcode;
+    int rc = 0;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST);
+
+    rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, NULL, 0);
+
+    return rc;
+}
+
+int
+ble_gap_read_periodic_adv_list_size(uint8_t *per_adv_list_size)
+{
+    struct ble_hci_le_rd_periodic_adv_list_size_rp rsp;
+    uint16_t opcode;
+    int rc = 0;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE);
+
+    rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    *per_adv_list_size = rsp.list_size;
+
+    return 0;
+}
+#endif
+
+/*****************************************************************************
+ * $discovery procedures                                                     *
+ *****************************************************************************/
+
+#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN
+static int
+ble_gap_ext_disc_tx_params(uint8_t own_addr_type, uint8_t filter_policy,
+                       const struct ble_hs_hci_ext_scan_param *uncoded_params,
+                       const struct ble_hs_hci_ext_scan_param *coded_params)
+{
+    struct ble_hci_le_set_ext_scan_params_cp *cmd;
+    struct scan_params *params;
+    uint8_t buf[sizeof(*cmd) + 2 * sizeof(*params)];
+    uint8_t len = sizeof(*cmd);
+
+    /* Check own addr type */
+    if (own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check scanner filter policy */
+    if (filter_policy > BLE_HCI_SCAN_FILT_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd = (void *) buf;
+    params = cmd->scans;
+
+    cmd->filter_policy = filter_policy;
+    cmd->own_addr_type = own_addr_type;
+    cmd->phys = 0;
+
+    if (uncoded_params) {
+        cmd->phys |= BLE_HCI_LE_PHY_1M_PREF_MASK;
+
+        params->type = uncoded_params->scan_type;
+        params->itvl = htole16(uncoded_params->scan_itvl);
+        params->window = htole16(uncoded_params->scan_window);
+
+        len += sizeof(*params);
+        params++;
+    }
+
+    if (coded_params) {
+        cmd->phys |= BLE_HCI_LE_PHY_CODED_PREF_MASK;
+
+        params->type = coded_params->scan_type;
+        params->itvl = htole16(coded_params->scan_itvl);
+        params->window = htole16(coded_params->scan_window);
+
+        len += sizeof(*params);
+        params++;
+    }
+
+    if (!cmd->phys) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_EXT_SCAN_PARAM),
+                             cmd, len, NULL, 0);
+}
+
+static int
+ble_gap_ext_disc_enable_tx(uint8_t enable, uint8_t filter_duplicates,
+                           uint16_t duration, uint16_t period)
+{
+    struct ble_hci_le_set_ext_scan_enable_cp cmd;
+
+    cmd.enable = enable;
+    cmd.filter_dup = filter_duplicates;
+    cmd.duration = htole16(duration);
+    cmd.period = htole16(period);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_EXT_SCAN_ENABLE),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+#endif
+#endif
+#if NIMBLE_BLE_SCAN
+#if !MYNEWT_VAL(BLE_EXT_ADV)
+static int
+ble_gap_disc_enable_tx(int enable, int filter_duplicates)
+{
+    struct ble_hci_le_set_scan_enable_cp cmd;
+    uint16_t opcode;
+
+    cmd.enable = !!enable;
+    cmd.filter_duplicates = !!filter_duplicates;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_ENABLE);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_gap_disc_tx_params(uint8_t own_addr_type,
+                       const struct ble_gap_disc_params *disc_params)
+{
+    struct ble_hci_le_set_scan_params_cp cmd;
+    uint16_t opcode;
+
+    if (disc_params->passive) {
+        cmd.scan_type = BLE_HCI_SCAN_TYPE_PASSIVE;
+    } else {
+        cmd.scan_type = BLE_HCI_SCAN_TYPE_ACTIVE;
+    }
+
+    cmd.scan_itvl = htole16(disc_params->itvl);
+    cmd.scan_window = htole16(disc_params->window);
+    cmd.own_addr_type = own_addr_type;
+    cmd.filter_policy = disc_params->filter_policy;
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_PARAMS);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+#endif
+
+static int
+ble_gap_disc_disable_tx(void)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    return ble_gap_ext_disc_enable_tx(0, 0, 0, 0);
+#else
+    return ble_gap_disc_enable_tx(0, 0);
+#endif
+}
+
+static int
+ble_gap_disc_cancel_no_lock(void)
+{
+    int rc;
+
+    STATS_INC(ble_gap_stats, discover_cancel);
+
+    if (!ble_gap_disc_active()) {
+        rc = BLE_HS_EALREADY;
+        goto done;
+    }
+
+    rc = ble_gap_disc_disable_tx();
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_master_reset_state();
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, discover_cancel_fail);
+    }
+
+    return rc;
+}
+#endif
+
+int
+ble_gap_disc_cancel(void)
+{
+#if NIMBLE_BLE_SCAN
+    int rc;
+
+    ble_hs_lock();
+    rc = ble_gap_disc_cancel_no_lock();
+    ble_hs_unlock();
+
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+#if NIMBLE_BLE_SCAN
+static int
+ble_gap_disc_ext_validate(uint8_t own_addr_type)
+{
+    if (own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (ble_gap_conn_active()) {
+        return BLE_HS_EBUSY;
+    }
+
+    if (ble_gap_disc_active()) {
+        return BLE_HS_EALREADY;
+    }
+
+    if (!ble_hs_is_enabled()) {
+        return BLE_HS_EDISABLED;
+    }
+
+    if (ble_gap_is_preempted()) {
+        return BLE_HS_EPREEMPTED;
+    }
+
+    return 0;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN
+static void
+ble_gap_ext_disc_fill_dflts(uint8_t limited,
+                            struct ble_hs_hci_ext_scan_param *disc_params)
+{
+   if (disc_params->scan_itvl == 0) {
+        if (limited) {
+            disc_params->scan_itvl = BLE_GAP_LIM_DISC_SCAN_INT;
+        } else {
+            disc_params->scan_itvl = BLE_GAP_SCAN_FAST_INTERVAL_MIN;
+        }
+    }
+
+    if (disc_params->scan_window == 0) {
+        if (limited) {
+            disc_params->scan_window = BLE_GAP_LIM_DISC_SCAN_WINDOW;
+        } else {
+            disc_params->scan_window = BLE_GAP_SCAN_FAST_WINDOW;
+        }
+    }
+}
+
+static void
+ble_gap_ext_scan_params_to_hci(const struct ble_gap_ext_disc_params *params,
+                               struct ble_hs_hci_ext_scan_param *hci_params)
+{
+
+    memset(hci_params, 0, sizeof(*hci_params));
+
+    if (params->passive) {
+        hci_params->scan_type =  BLE_HCI_SCAN_TYPE_PASSIVE;
+    } else {
+        hci_params->scan_type = BLE_HCI_SCAN_TYPE_ACTIVE;
+    }
+
+    hci_params->scan_itvl = params->itvl;
+    hci_params->scan_window = params->window;
+}
+#endif
+
+int
+ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period,
+                 uint8_t filter_duplicates, uint8_t filter_policy,
+                 uint8_t limited,
+                 const struct ble_gap_ext_disc_params *uncoded_params,
+                 const struct ble_gap_ext_disc_params *coded_params,
+                 ble_gap_event_fn *cb, void *cb_arg)
+{
+#if NIMBLE_BLE_SCAN && MYNEWT_VAL(BLE_EXT_ADV)
+    struct ble_hs_hci_ext_scan_param ucp;
+    struct ble_hs_hci_ext_scan_param cp;
+    int rc;
+
+    STATS_INC(ble_gap_stats, discover);
+
+    ble_hs_lock();
+
+    rc = ble_gap_disc_ext_validate(own_addr_type);
+    if (rc != 0) {
+        goto done;
+    }
+
+    /* Make a copy of the parameter structure and fill unspecified values with
+     * defaults.
+     */
+
+    if (uncoded_params) {
+        ble_gap_ext_scan_params_to_hci(uncoded_params, &ucp);
+        ble_gap_ext_disc_fill_dflts(limited, &ucp);
+
+        /* XXX: We should do it only once */
+        if (!uncoded_params->passive) {
+            rc = ble_hs_id_use_addr(own_addr_type);
+            if (rc != 0) {
+                goto done;
+            }
+        }
+    }
+
+    if (coded_params) {
+        ble_gap_ext_scan_params_to_hci(coded_params, &cp);
+        ble_gap_ext_disc_fill_dflts(limited, &cp);
+
+        /* XXX: We should do it only once */
+        if (!coded_params->passive) {
+            rc = ble_hs_id_use_addr(own_addr_type);
+            if (rc != 0) {
+                goto done;
+            }
+        }
+    }
+
+    ble_gap_master.disc.limited = limited;
+    ble_gap_master.cb = cb;
+    ble_gap_master.cb_arg = cb_arg;
+
+    rc = ble_gap_ext_disc_tx_params(own_addr_type, filter_policy,
+                                    uncoded_params ? &ucp : NULL,
+                                    coded_params ? &cp : NULL);
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_master.op = BLE_GAP_OP_M_DISC;
+
+    rc = ble_gap_ext_disc_enable_tx(1, filter_duplicates, duration, period);
+    if (rc != 0) {
+        ble_gap_master_reset_state();
+        goto done;
+    }
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, discover_fail);
+    }
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+#if NIMBLE_BLE_SCAN && !MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_disc_fill_dflts(struct ble_gap_disc_params *disc_params)
+{
+   if (disc_params->itvl == 0) {
+        if (disc_params->limited) {
+            disc_params->itvl = BLE_GAP_LIM_DISC_SCAN_INT;
+        } else {
+            disc_params->itvl = BLE_GAP_SCAN_FAST_INTERVAL_MIN;
+        }
+    }
+
+    if (disc_params->window == 0) {
+        if (disc_params->limited) {
+            disc_params->window = BLE_GAP_LIM_DISC_SCAN_WINDOW;
+        } else {
+            disc_params->window = BLE_GAP_SCAN_FAST_WINDOW;
+        }
+    }
+}
+
+static int
+ble_gap_disc_validate(uint8_t own_addr_type,
+                      const struct ble_gap_disc_params *disc_params)
+{
+    if (disc_params == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* Check interval and window */
+    if ((disc_params->itvl < BLE_HCI_SCAN_ITVL_MIN) ||
+        (disc_params->itvl > BLE_HCI_SCAN_ITVL_MAX) ||
+        (disc_params->window < BLE_HCI_SCAN_WINDOW_MIN) ||
+        (disc_params->window > BLE_HCI_SCAN_WINDOW_MAX) ||
+        (disc_params->itvl < disc_params->window)) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* Check scanner filter policy */
+    if (disc_params->filter_policy > BLE_HCI_SCAN_FILT_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    return ble_gap_disc_ext_validate(own_addr_type);
+}
+#endif
+
+int
+ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms,
+             const struct ble_gap_disc_params *disc_params,
+             ble_gap_event_fn *cb, void *cb_arg)
+{
+#if NIMBLE_BLE_SCAN
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    struct ble_gap_ext_disc_params p = {0};
+
+    p.itvl = disc_params->itvl;
+    p.passive = disc_params->passive;
+    p.window = disc_params->window;
+
+    if (duration_ms == BLE_HS_FOREVER) {
+        duration_ms = 0;
+    } else if (duration_ms == 0) {
+        duration_ms = BLE_GAP_DISC_DUR_DFLT;
+    }
+
+    return ble_gap_ext_disc(own_addr_type, duration_ms/10, 0,
+                          disc_params->filter_duplicates,
+                          disc_params->filter_policy, disc_params->limited,
+                          &p, NULL, cb, cb_arg);
+#else
+    struct ble_gap_disc_params params;
+    uint32_t duration_ticks = 0;
+    int rc;
+
+    STATS_INC(ble_gap_stats, discover);
+
+    ble_hs_lock();
+
+    /* Make a copy of the parameter strcuture and fill unspecified values with
+     * defaults.
+     */
+    params = *disc_params;
+    ble_gap_disc_fill_dflts(&params);
+
+    rc = ble_gap_disc_validate(own_addr_type, &params);
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (duration_ms == 0) {
+        duration_ms = BLE_GAP_DISC_DUR_DFLT;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = ble_npl_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
+    }
+
+    if (!params.passive) {
+        rc = ble_hs_id_use_addr(own_addr_type);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+
+    ble_gap_master.disc.limited = params.limited;
+    ble_gap_master.cb = cb;
+    ble_gap_master.cb_arg = cb_arg;
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: discovery; ");
+    ble_gap_log_disc(own_addr_type, duration_ms, &params);
+    BLE_HS_LOG(INFO, "\n");
+
+    rc = ble_gap_disc_tx_params(own_addr_type, &params);
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_master.op = BLE_GAP_OP_M_DISC;
+
+    rc = ble_gap_disc_enable_tx(1, params.filter_duplicates);
+    if (rc != 0) {
+        ble_gap_master_reset_state();
+        goto done;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_master_set_timer(duration_ticks);
+    }
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, discover_fail);
+    }
+    return rc;
+#endif
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_disc_active(void)
+{
+    /* Assume read is atomic; mutex not necessary. */
+    return ble_gap_master.op == BLE_GAP_OP_M_DISC;
+}
+
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL) && !MYNEWT_VAL(BLE_EXT_ADV)
+/*****************************************************************************
+ * $connection establishment procedures                                      *
+ *****************************************************************************/
+
+static int
+ble_gap_conn_create_tx(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                       const struct ble_gap_conn_params *params)
+{
+    struct ble_hci_le_create_conn_cp cmd;
+    uint16_t opcode;
+
+    cmd.scan_itvl = htole16(params->scan_itvl);
+    cmd.scan_window = htole16(params->scan_window);
+    if (peer_addr == NULL) {
+        /* Application wants to connect to any device in the white list.  The
+         * peer address type and peer address fields are ignored by the
+         * controller; fill them with dummy values.
+         */
+        cmd.filter_policy = BLE_HCI_CONN_FILT_USE_WL;
+        cmd.peer_addr_type = 0;
+        memset(cmd.peer_addr, 0, sizeof(cmd.peer_addr));
+    } else {
+        cmd.filter_policy = BLE_HCI_CONN_FILT_NO_WL;
+        cmd.peer_addr_type = peer_addr->type;
+        memcpy(cmd.peer_addr, peer_addr->val, sizeof(cmd.peer_addr));
+    }
+
+    cmd.own_addr_type = own_addr_type;
+    cmd.min_conn_itvl = htole16(params->itvl_min);
+    cmd.max_conn_itvl = htole16(params->itvl_max);
+    cmd.conn_latency = htole16(params->latency);
+    cmd.tmo = htole16(params->supervision_timeout);
+    cmd.min_ce = htole16(params->min_ce_len);
+    cmd.max_ce = htole16(params->max_ce_len);
+
+    opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN);
+
+    return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0);
+}
+#endif
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+static int
+ble_gap_check_conn_params(uint8_t phy, const struct ble_gap_conn_params *params)
+{
+    if (phy != BLE_HCI_LE_PHY_2M) {
+        /* Check scan interval and window */
+        if ((params->scan_itvl < BLE_HCI_SCAN_ITVL_MIN) ||
+            (params->scan_itvl > BLE_HCI_SCAN_ITVL_MAX) ||
+            (params->scan_window < BLE_HCI_SCAN_WINDOW_MIN) ||
+            (params->scan_window > BLE_HCI_SCAN_WINDOW_MAX) ||
+            (params->scan_itvl < params->scan_window)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+        /* Check connection interval min */
+    if ((params->itvl_min < BLE_HCI_CONN_ITVL_MIN) ||
+        (params->itvl_min > BLE_HCI_CONN_ITVL_MAX)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+    /* Check connection interval max */
+    if ((params->itvl_max < BLE_HCI_CONN_ITVL_MIN) ||
+        (params->itvl_max > BLE_HCI_CONN_ITVL_MAX) ||
+        (params->itvl_max < params->itvl_min)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check connection latency */
+    if ((params->latency < BLE_HCI_CONN_LATENCY_MIN) ||
+        (params->latency > BLE_HCI_CONN_LATENCY_MAX)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check supervision timeout */
+    if ((params->supervision_timeout < BLE_HCI_CONN_SPVN_TIMEOUT_MIN) ||
+        (params->supervision_timeout > BLE_HCI_CONN_SPVN_TIMEOUT_MAX)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check connection event length */
+    if (params->min_ce_len > params->max_ce_len) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return 0;
+}
+
+static int
+ble_gap_ext_conn_create_tx(
+    uint8_t own_addr_type, const ble_addr_t *peer_addr, uint8_t phy_mask,
+    const struct ble_gap_conn_params *phy_1m_conn_params,
+    const struct ble_gap_conn_params *phy_2m_conn_params,
+    const struct ble_gap_conn_params *phy_coded_conn_params)
+{
+    struct ble_hci_le_ext_create_conn_cp *cmd;
+    struct conn_params *params;
+    uint8_t buf[sizeof(*cmd) + 3 * sizeof(*params)];
+    uint8_t len = sizeof(*cmd);
+    int rc;
+
+    /* Check own addr type */
+    if (own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (phy_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK |
+                    BLE_HCI_LE_PHY_2M_PREF_MASK |
+                    BLE_HCI_LE_PHY_CODED_PREF_MASK)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd = (void *) buf;
+    params = cmd->conn_params;
+
+    if (peer_addr == NULL) {
+        /* Application wants to connect to any device in the white list.  The
+         * peer address type and peer address fields are ignored by the
+         * controller; fill them with dummy values.
+         */
+        cmd->filter_policy = BLE_HCI_CONN_FILT_USE_WL;
+        cmd->peer_addr_type = 0;
+        memset(cmd->peer_addr, 0, sizeof(cmd->peer_addr));
+    } else {
+        /* Check peer addr type */
+        if (peer_addr->type > BLE_HCI_CONN_PEER_ADDR_MAX) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        cmd->filter_policy = BLE_HCI_CONN_FILT_NO_WL;
+        cmd->peer_addr_type = peer_addr->type;
+        memcpy(cmd->peer_addr, peer_addr->val, sizeof(cmd->peer_addr));
+    }
+
+    cmd->own_addr_type = own_addr_type;
+    cmd->init_phy_mask = phy_mask;
+
+    if (phy_mask & BLE_GAP_LE_PHY_1M_MASK) {
+        rc = ble_gap_check_conn_params(BLE_HCI_LE_PHY_1M, phy_1m_conn_params);
+        if (rc) {
+            return rc;
+        }
+
+        params->scan_itvl = htole16(phy_1m_conn_params->scan_itvl);
+        params->scan_window = htole16(phy_1m_conn_params->scan_window);
+        params->conn_min_itvl = htole16(phy_1m_conn_params->itvl_min);
+        params->conn_max_itvl = htole16(phy_1m_conn_params->itvl_max);
+        params->conn_latency = htole16(phy_1m_conn_params->latency);
+        params->supervision_timeout = htole16(phy_1m_conn_params->supervision_timeout);
+        params->min_ce = htole16(phy_1m_conn_params->min_ce_len);
+        params->max_ce = htole16(phy_1m_conn_params->max_ce_len);
+
+        params++;
+        len += sizeof(*params);
+    }
+
+    if (phy_mask & BLE_GAP_LE_PHY_2M_MASK) {
+        rc = ble_gap_check_conn_params(BLE_HCI_LE_PHY_2M, phy_2m_conn_params);
+        if (rc) {
+            return rc;
+        }
+
+        params->scan_itvl = htole16(phy_2m_conn_params->scan_itvl);
+        params->scan_window = htole16(phy_2m_conn_params->scan_window);
+        params->conn_min_itvl = htole16(phy_2m_conn_params->itvl_min);
+        params->conn_max_itvl = htole16(phy_2m_conn_params->itvl_max);
+        params->conn_latency = htole16(phy_2m_conn_params->latency);
+        params->supervision_timeout = htole16(phy_2m_conn_params->supervision_timeout);
+        params->min_ce = htole16(phy_2m_conn_params->min_ce_len);
+        params->max_ce = htole16(phy_2m_conn_params->max_ce_len);
+
+        params++;
+        len += sizeof(*params);
+    }
+
+    if (phy_mask & BLE_GAP_LE_PHY_CODED_MASK) {
+        rc = ble_gap_check_conn_params(BLE_HCI_LE_PHY_CODED, phy_coded_conn_params);
+        if (rc) {
+            return rc;
+        }
+
+        params->scan_itvl = htole16(phy_coded_conn_params->scan_itvl);
+        params->scan_window = htole16(phy_coded_conn_params->scan_window);
+        params->conn_min_itvl = htole16(phy_coded_conn_params->itvl_min);
+        params->conn_max_itvl = htole16(phy_coded_conn_params->itvl_max);
+        params->conn_latency = htole16(phy_coded_conn_params->latency);
+        params->supervision_timeout = htole16(phy_coded_conn_params->supervision_timeout);
+        params->min_ce = htole16(phy_coded_conn_params->min_ce_len);
+        params->max_ce = htole16(phy_coded_conn_params->max_ce_len);
+
+        params++;
+        len += sizeof(*params);
+    }
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_EXT_CREATE_CONN),
+                                       cmd, len, NULL, 0);
+}
+#endif
+
+/**
+ * Initiates a connect procedure.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself during connection establishment.
+ *                                      o BLE_OWN_ADDR_PUBLIC
+ *                                      o BLE_OWN_ADDR_RANDOM
+ *                                      o BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ *                                      o BLE_OWN_ADDR_RPA_RANDOM_DEFAULT
+ * @param peer_addr             The address of the peer to connect to.
+ *                                  If this parameter is NULL, the white list
+ *                                  is used.
+ * @param duration_ms           The duration of the discovery procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are milliseconds.
+ * @param phy_mask              Define on which PHYs connection attempt should
+ *                                  be done
+ * @param phy_1m_conn_params     Additional arguments specifying the
+ *                                  particulars of the connect procedure. When
+ *                                  BLE_GAP_LE_PHY_1M_MASK is set in phy_mask
+ *                                  this parameter can be specify to null for
+ *                                  default values.
+ * @param phy_2m_conn_params     Additional arguments specifying the
+ *                                  particulars of the connect procedure. When
+ *                                  BLE_GAP_LE_PHY_2M_MASK is set in phy_mask
+ *                                  this parameter can be specify to null for
+ *                                  default values.
+ * @param phy_coded_conn_params  Additional arguments specifying the
+ *                                  particulars of the connect procedure. When
+ *                                  BLE_GAP_LE_PHY_CODED_MASK is set in
+ *                                  phy_mask this parameter can be specify to
+ *                                  null for default values.
+ * @param cb                    The callback to associate with this connect
+ *                                  procedure.  When the connect procedure
+ *                                  completes, the result is reported through
+ *                                  this callback.  If the connect procedure
+ *                                  succeeds, the connection inherits this
+ *                                  callback as its event-reporting mechanism.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if a connection attempt is
+ *                                  already in progress;
+ *                              BLE_HS_EBUSY if initiating a connection is not
+ *                                  possible because scanning is in progress;
+ *                              BLE_HS_EDONE if the specified peer is already
+ *                                  connected;
+ *                              Other nonzero on error.
+ */
+int
+ble_gap_ext_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                int32_t duration_ms, uint8_t phy_mask,
+                const struct ble_gap_conn_params *phy_1m_conn_params,
+                const struct ble_gap_conn_params *phy_2m_conn_params,
+                const struct ble_gap_conn_params *phy_coded_conn_params,
+                ble_gap_event_fn *cb, void *cb_arg)
+{
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+    ble_npl_time_t duration_ticks;
+    int rc;
+
+    STATS_INC(ble_gap_stats, initiate);
+
+    ble_hs_lock();
+
+    if (ble_gap_conn_active()) {
+        rc = BLE_HS_EALREADY;
+        goto done;
+    }
+
+    if (ble_gap_disc_active()) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
+
+    if (!ble_hs_is_enabled()) {
+        return BLE_HS_EDISABLED;
+    }
+
+    if (ble_gap_is_preempted()) {
+        rc = BLE_HS_EPREEMPTED;
+        goto done;
+    }
+
+    if (!ble_hs_conn_can_alloc()) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    if (peer_addr &&
+        peer_addr->type != BLE_ADDR_PUBLIC &&
+        peer_addr->type != BLE_ADDR_RANDOM &&
+        peer_addr->type != BLE_ADDR_PUBLIC_ID &&
+        peer_addr->type != BLE_ADDR_RANDOM_ID) {
+
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    if ((phy_mask & BLE_GAP_LE_PHY_1M_MASK) && phy_1m_conn_params == NULL) {
+        phy_1m_conn_params = &ble_gap_conn_params_dflt;
+    }
+
+    if ((phy_mask & BLE_GAP_LE_PHY_2M_MASK) && phy_2m_conn_params == NULL) {
+        phy_2m_conn_params = &ble_gap_conn_params_dflt;
+    }
+
+    if ((phy_mask & BLE_GAP_LE_PHY_CODED_MASK) &&
+        phy_coded_conn_params == NULL) {
+
+        phy_coded_conn_params = &ble_gap_conn_params_dflt;
+    }
+
+    if (duration_ms == 0) {
+        duration_ms = BLE_GAP_CONN_DUR_DFLT;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = ble_npl_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
+    }
+
+    /* Verify peer not already connected. */
+    if (ble_hs_conn_find_by_addr(peer_addr) != NULL) {
+        rc = BLE_HS_EDONE;
+        goto done;
+    }
+
+    /* XXX: Verify conn_params. */
+
+    rc = ble_hs_id_use_addr(own_addr_type);
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_master.cb = cb;
+    ble_gap_master.cb_arg = cb_arg;
+    ble_gap_master.conn.using_wl = peer_addr == NULL;
+    ble_gap_master.conn.our_addr_type = own_addr_type;
+
+    ble_gap_master.op = BLE_GAP_OP_M_CONN;
+
+    rc = ble_gap_ext_conn_create_tx(own_addr_type, peer_addr, phy_mask,
+                                    phy_1m_conn_params, phy_2m_conn_params,
+                                    phy_coded_conn_params);
+    if (rc != 0) {
+        ble_gap_master_reset_state();
+        goto done;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_master_set_timer(duration_ticks);
+    }
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, initiate_fail);
+    }
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+
+}
+#endif
+
+int
+ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                int32_t duration_ms,
+                const struct ble_gap_conn_params *conn_params,
+                ble_gap_event_fn *cb, void *cb_arg)
+{
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    return ble_gap_ext_connect(own_addr_type, peer_addr, duration_ms,
+                               BLE_GAP_LE_PHY_1M_MASK,
+                               conn_params, NULL, NULL, cb, cb_arg);
+#else
+    uint32_t duration_ticks;
+    int rc;
+
+    STATS_INC(ble_gap_stats, initiate);
+
+    ble_hs_lock();
+
+    if (ble_gap_conn_active()) {
+        rc = BLE_HS_EALREADY;
+        goto done;
+    }
+
+    if (ble_gap_disc_active()) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
+
+    if (!ble_hs_is_enabled()) {
+        rc = BLE_HS_EDISABLED;
+        goto done;
+    }
+
+    if (ble_gap_is_preempted()) {
+        rc = BLE_HS_EPREEMPTED;
+        goto done;
+    }
+
+    if (!ble_hs_conn_can_alloc()) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    if (peer_addr &&
+        peer_addr->type != BLE_ADDR_PUBLIC &&
+        peer_addr->type != BLE_ADDR_RANDOM &&
+        peer_addr->type != BLE_ADDR_PUBLIC_ID &&
+        peer_addr->type != BLE_ADDR_RANDOM_ID) {
+
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    if (conn_params == NULL) {
+        conn_params = &ble_gap_conn_params_dflt;
+    }
+
+    if (duration_ms == 0) {
+        duration_ms = BLE_GAP_CONN_DUR_DFLT;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = ble_npl_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
+    }
+
+    /* Verify peer not already connected. */
+    if (ble_hs_conn_find_by_addr(peer_addr) != NULL) {
+        rc = BLE_HS_EDONE;
+        goto done;
+    }
+
+    /* XXX: Verify conn_params. */
+
+    rc = ble_hs_id_use_addr(own_addr_type);
+    if (rc != 0) {
+        goto done;
+    }
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: connect; ");
+    ble_gap_log_conn(own_addr_type, peer_addr, conn_params);
+    BLE_HS_LOG(INFO, "\n");
+
+    ble_gap_master.cb = cb;
+    ble_gap_master.cb_arg = cb_arg;
+    ble_gap_master.conn.using_wl = peer_addr == NULL;
+    ble_gap_master.conn.our_addr_type = own_addr_type;
+
+    ble_gap_master.op = BLE_GAP_OP_M_CONN;
+
+    rc = ble_gap_conn_create_tx(own_addr_type, peer_addr,
+                                conn_params);
+    if (rc != 0) {
+        ble_gap_master_reset_state();
+        goto done;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_master_set_timer(duration_ticks);
+    }
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, initiate_fail);
+    }
+    return rc;
+#endif
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+
+}
+
+int
+ble_gap_conn_active(void)
+{
+    /* Assume read is atomic; mutex not necessary. */
+    return ble_gap_master.op == BLE_GAP_OP_M_CONN;
+}
+
+/*****************************************************************************
+ * $terminate connection procedure                                           *
+ *****************************************************************************/
+int
+ble_gap_terminate_with_conn(struct ble_hs_conn *conn, uint8_t hci_reason)
+{
+    struct ble_hci_lc_disconnect_cp cmd;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+    if (conn->bhc_flags & BLE_HS_CONN_F_TERMINATING) {
+        return BLE_HS_EALREADY;
+    }
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: terminate connection; "
+                     "conn_handle=%d hci_reason=%d\n",
+                     conn->bhc_handle, hci_reason);
+
+    cmd.conn_handle = htole16(conn->bhc_handle);
+    cmd.reason = hci_reason;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LINK_CTRL,
+                                      BLE_HCI_OCF_DISCONNECT_CMD),
+                                      &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    conn->bhc_flags |= BLE_HS_CONN_F_TERMINATING;
+    return 0;
+}
+
+int
+ble_gap_terminate(uint16_t conn_handle, uint8_t hci_reason)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    STATS_INC(ble_gap_stats, terminate);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
+    rc = ble_gap_terminate_with_conn(conn, hci_reason);
+
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, terminate_fail);
+    }
+    return rc;
+}
+
+/*****************************************************************************
+ * $cancel                                                                   *
+ *****************************************************************************/
+
+static int
+ble_gap_conn_cancel_tx(void)
+{
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
+                           NULL, 0, NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+#if NIMBLE_BLE_CONNECT
+static int
+ble_gap_conn_cancel_no_lock(void)
+{
+    int rc;
+
+    STATS_INC(ble_gap_stats, cancel);
+
+    if (!ble_gap_conn_active()) {
+        rc = BLE_HS_EALREADY;
+        goto done;
+    }
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: cancel connection\n");
+
+    rc = ble_gap_conn_cancel_tx();
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_gap_master.conn.cancel = 1;
+    rc = 0;
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, cancel_fail);
+    }
+
+    return rc;
+}
+#endif
+
+int
+ble_gap_conn_cancel(void)
+{
+#if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+    int rc;
+
+    ble_hs_lock();
+    rc = ble_gap_conn_cancel_no_lock();
+    ble_hs_unlock();
+
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+
+}
+
+/*****************************************************************************
+ * $update connection parameters                                             *
+ *****************************************************************************/
+
+#if NIMBLE_BLE_CONNECT
+static struct ble_gap_update_entry *
+ble_gap_update_entry_alloc(void)
+{
+    struct ble_gap_update_entry *entry;
+
+    entry = os_memblock_get(&ble_gap_update_entry_pool);
+    if (entry != NULL) {
+        memset(entry, 0, sizeof *entry);
+    }
+
+    return entry;
+}
+#endif
+
+static void
+ble_gap_update_entry_free(struct ble_gap_update_entry *entry)
+{
+    int rc;
+
+    if (entry != NULL) {
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        memset(entry, 0xff, sizeof *entry);
+#endif
+        rc = os_memblock_put(&ble_gap_update_entry_pool, entry);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static struct ble_gap_update_entry *
+ble_gap_update_entry_find(uint16_t conn_handle,
+                          struct ble_gap_update_entry **out_prev)
+{
+    struct ble_gap_update_entry *entry;
+    struct ble_gap_update_entry *prev;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    prev = NULL;
+    SLIST_FOREACH(entry, &ble_gap_update_entries, next) {
+        if (entry->conn_handle == conn_handle) {
+            break;
+        }
+
+        prev = entry;
+    }
+
+    if (out_prev != NULL) {
+        *out_prev = prev;
+    }
+
+    return entry;
+}
+
+static struct ble_gap_update_entry *
+ble_gap_update_entry_remove(uint16_t conn_handle)
+{
+    struct ble_gap_update_entry *entry;
+    struct ble_gap_update_entry *prev;
+
+    entry = ble_gap_update_entry_find(conn_handle, &prev);
+    if (entry != NULL) {
+        if (prev == NULL) {
+            SLIST_REMOVE_HEAD(&ble_gap_update_entries, next);
+        } else {
+            SLIST_NEXT(prev, next) = SLIST_NEXT(entry, next);
+        }
+        ble_hs_timer_resched();
+    }
+
+    return entry;
+}
+
+#if NIMBLE_BLE_CONNECT
+static void
+ble_gap_update_l2cap_cb(uint16_t conn_handle, int status, void *arg)
+{
+    struct ble_gap_update_entry *entry;
+
+    /* Report failures and rejections.  Success gets reported when the
+     * controller sends the connection update complete event.
+     */
+
+    ble_hs_lock();
+    entry = ble_gap_update_entry_remove(conn_handle);
+    ble_hs_unlock();
+
+    if (entry != NULL) {
+        ble_gap_update_entry_free(entry);
+        if (status != 0) {
+            ble_gap_update_notify(conn_handle, status);
+        }
+        /* On success let's wait for the controller to notify about update */
+    }
+}
+
+static int
+ble_gap_tx_param_pos_reply(uint16_t conn_handle,
+                           struct ble_gap_upd_params *params)
+{
+    struct ble_hci_le_rem_conn_param_rr_cp cmd;
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.conn_itvl_min = htole16(params->itvl_min);
+    cmd.conn_itvl_max = htole16(params->itvl_max);
+    cmd.conn_latency = htole16(params->latency);
+    cmd.supervision_timeout = htole16(params->supervision_timeout);
+    cmd.min_ce = htole16(params->min_ce_len);
+    cmd.max_ce = htole16(params->max_ce_len);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_REM_CONN_PARAM_RR),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_gap_tx_param_neg_reply(uint16_t conn_handle, uint8_t reject_reason)
+{
+    struct ble_hci_le_rem_conn_params_nrr_cp cmd;
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.reason = reject_reason;
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR),
+                                     &cmd, sizeof(cmd), NULL, 0);
+}
+#endif
+
+void
+ble_gap_rx_param_req(const struct ble_hci_ev_le_subev_rem_conn_param_req *ev)
+{
+#if NIMBLE_BLE_CONNECT
+    struct ble_gap_upd_params peer_params;
+    struct ble_gap_upd_params self_params;
+    struct ble_gap_event event;
+    uint16_t conn_handle;
+    int rc;
+
+    memset(&event, 0, sizeof event);
+
+    peer_params.itvl_min = le16toh(ev->min_interval);
+    peer_params.itvl_max = le16toh(ev->max_interval);
+    peer_params.latency = le16toh(ev->latency);
+    peer_params.supervision_timeout = le16toh(ev->timeout);
+    peer_params.min_ce_len = 0;
+    peer_params.max_ce_len = 0;
+
+    /* Copy the peer params into the self params to make it easy on the
+     * application.  The application callback will change only the fields which
+     * it finds unsuitable.
+     */
+    self_params = peer_params;
+
+    conn_handle = le16toh(ev->conn_handle);
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_CONN_UPDATE_REQ;
+    event.conn_update_req.conn_handle = conn_handle;
+    event.conn_update_req.self_params = &self_params;
+    event.conn_update_req.peer_params = &peer_params;
+    rc = ble_gap_call_conn_event_cb(&event, conn_handle);
+    if (rc == 0) {
+        rc = ble_gap_tx_param_pos_reply(conn_handle, &self_params);
+        if (rc != 0) {
+            ble_gap_update_failed(conn_handle, rc);
+        }
+    } else {
+        ble_gap_tx_param_neg_reply(conn_handle, rc);
+    }
+#endif
+}
+
+#if NIMBLE_BLE_CONNECT
+static int
+ble_gap_update_tx(uint16_t conn_handle,
+                  const struct ble_gap_upd_params *params)
+{
+    struct ble_hci_le_conn_update_cp cmd;
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.conn_itvl_min = htole16(params->itvl_min);
+    cmd.conn_itvl_max = htole16(params->itvl_max);
+    cmd.conn_latency = htole16(params->latency);
+    cmd.supervision_timeout = htole16(params->supervision_timeout);
+    cmd.min_ce_len = htole16(params->min_ce_len);
+    cmd.max_ce_len = htole16(params->max_ce_len);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_CONN_UPDATE),
+                                        &cmd, sizeof(cmd), NULL, 0);
+}
+
+static bool
+ble_gap_validate_conn_params(const struct ble_gap_upd_params *params)
+{
+
+    /* Requirements from Bluetooth spec. v4.2 [Vol 2, Part E], 7.8.18 */
+    if (params->itvl_min > params->itvl_max) {
+        return false;
+    }
+
+    if (params->itvl_min < 0x0006 || params->itvl_max > 0x0C80) {
+        return false;
+    }
+
+    if (params->latency > 0x01F3) {
+        return false;
+    }
+
+    /* According to specification mentioned above we should make sure that:
+     * supervision_timeout_ms > (1 + latency) * 2 * max_interval_ms
+     *    =>
+     * supervision_timeout * 10 ms > (1 + latency) * 2 * itvl_max * 1.25ms
+     */
+    if (params->supervision_timeout <=
+                   (((1 + params->latency) * params->itvl_max) / 4)) {
+        return false;
+    }
+
+    return true;
+}
+#endif
+
+int
+ble_gap_update_params(uint16_t conn_handle,
+                      const struct ble_gap_upd_params *params)
+{
+#if NIMBLE_BLE_CONNECT
+    struct ble_l2cap_sig_update_params l2cap_params;
+    struct ble_gap_update_entry *entry;
+    struct ble_gap_update_entry *dup;
+    struct ble_hs_conn *conn;
+    int l2cap_update;
+    int rc;
+
+    l2cap_update = 0;
+
+    /* Validate parameters with a spec */
+    if (!ble_gap_validate_conn_params(params)) {
+        return BLE_HS_EINVAL;
+    }
+
+    STATS_INC(ble_gap_stats, update);
+    memset(&l2cap_params, 0, sizeof l2cap_params);
+    entry = NULL;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
+    /* Don't allow two concurrent updates to the same connection. */
+    dup = ble_gap_update_entry_find(conn_handle, NULL);
+    if (dup != NULL) {
+        rc = BLE_HS_EALREADY;
+        goto done;
+    }
+
+    entry = ble_gap_update_entry_alloc();
+    if (entry == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    entry->conn_handle = conn_handle;
+    entry->params = *params;
+
+    entry->exp_os_ticks = ble_npl_time_get() +
+                          ble_npl_time_ms_to_ticks32(BLE_GAP_UPDATE_TIMEOUT_MS);
+
+    BLE_HS_LOG(INFO, "GAP procedure initiated: ");
+    ble_gap_log_update(conn_handle, params);
+    BLE_HS_LOG(INFO, "\n");
+
+    /*
+     * If LL update procedure is not supported on this connection and we are
+     * the slave, fail over to the L2CAP update procedure.
+     */
+    if ((conn->supported_feat & BLE_HS_HCI_LE_FEAT_CONN_PARAM_REQUEST) == 0 &&
+            !(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) {
+        l2cap_update = 1;
+        rc = 0;
+    } else {
+        rc = ble_gap_update_tx(conn_handle, params);
+    }
+
+done:
+    ble_hs_unlock();
+
+    if (!l2cap_update) {
+        ble_hs_timer_resched();
+    } else {
+        ble_gap_update_to_l2cap(params, &l2cap_params);
+
+        rc = ble_l2cap_sig_update(conn_handle, &l2cap_params,
+                                              ble_gap_update_l2cap_cb, NULL);
+    }
+
+    ble_hs_lock();
+    if (rc == 0) {
+        SLIST_INSERT_HEAD(&ble_gap_update_entries, entry, next);
+    } else {
+        ble_gap_update_entry_free(entry);
+        STATS_INC(ble_gap_stats, update_fail);
+    }
+    ble_hs_unlock();
+
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+/*****************************************************************************
+ * $security                                                                 *
+ *****************************************************************************/
+int
+ble_gap_security_initiate(uint16_t conn_handle)
+{
+#if NIMBLE_BLE_SM
+    struct ble_store_value_sec value_sec;
+    struct ble_store_key_sec key_sec;
+    struct ble_hs_conn_addrs addrs;
+    ble_hs_conn_flags_t conn_flags;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    STATS_INC(ble_gap_stats, security_initiate);
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        conn_flags = conn->bhc_flags;
+        ble_hs_conn_addrs(conn, &addrs);
+
+        memset(&key_sec, 0, sizeof key_sec);
+        key_sec.peer_addr = addrs.peer_id_addr;
+    }
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
+    if (conn_flags & BLE_HS_CONN_F_MASTER) {
+        /* Search the security database for an LTK for this peer.  If one
+         * is found, perform the encryption procedure rather than the pairing
+         * procedure.
+         */
+        rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+        if (rc == 0 && value_sec.ltk_present) {
+            rc = ble_sm_enc_initiate(conn_handle, value_sec.key_size,
+                                     value_sec.ltk, value_sec.ediv,
+                                     value_sec.rand_num,
+                                     value_sec.authenticated);
+            if (rc != 0) {
+                goto done;
+            }
+        } else {
+            rc = ble_sm_pair_initiate(conn_handle);
+            if (rc != 0) {
+                goto done;
+            }
+        }
+    } else {
+        rc = ble_sm_slave_initiate(conn_handle);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+
+    rc = 0;
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, security_initiate_fail);
+    }
+
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_pair_initiate(uint16_t conn_handle)
+{
+    int rc;
+
+    rc = ble_sm_pair_initiate(conn_handle);
+
+    return rc;
+}
+
+int
+ble_gap_encryption_initiate(uint16_t conn_handle,
+                            uint8_t key_size,
+                            const uint8_t *ltk,
+                            uint16_t ediv,
+                            uint64_t rand_val,
+                            int auth)
+{
+#if NIMBLE_BLE_SM
+    ble_hs_conn_flags_t conn_flags;
+    int rc;
+
+    rc = ble_hs_atomic_conn_flags(conn_handle, &conn_flags);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (!(conn_flags & BLE_HS_CONN_F_MASTER)) {
+        return BLE_HS_EROLE;
+    }
+
+    rc = ble_sm_enc_initiate(conn_handle, key_size, ltk,
+                             ediv, rand_val, auth);
+    return rc;
+#else
+    return BLE_HS_ENOTSUP;
+#endif
+}
+
+int
+ble_gap_unpair(const ble_addr_t *peer_addr)
+{
+    struct ble_hs_conn *conn;
+
+    if (ble_addr_cmp(peer_addr, BLE_ADDR_ANY) == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find_by_addr(peer_addr);
+    if (conn != NULL) {
+        ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM);
+    }
+
+    ble_hs_unlock();
+
+    ble_hs_pvcy_remove_entry(peer_addr->type,
+                             peer_addr->val);
+
+    return ble_store_util_delete_peer(peer_addr);
+}
+
+int
+ble_gap_unpair_oldest_peer(void)
+{
+    ble_addr_t oldest_peer_id_addr;
+    int num_peers;
+    int rc;
+
+    rc = ble_store_util_bonded_peers(
+            &oldest_peer_id_addr, &num_peers, 1);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (num_peers == 0) {
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_gap_unpair(&oldest_peer_id_addr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_gap_unpair_oldest_except(const ble_addr_t *peer_addr)
+{
+    ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+    int num_peers;
+    int rc, i;
+
+    rc = ble_store_util_bonded_peers(
+            &peer_id_addrs[0], &num_peers, MYNEWT_VAL(BLE_STORE_MAX_BONDS));
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (num_peers == 0) {
+        return BLE_HS_ENOENT;
+    }
+
+    for (i = 0; i < num_peers; i++) {
+        if (ble_addr_cmp(peer_addr, &peer_id_addrs[i]) != 0) {
+            break;
+        }
+    }
+
+    if (i >= num_peers) {
+        return BLE_HS_ENOMEM;
+    }
+
+    return ble_gap_unpair(&peer_id_addrs[i]);
+}
+
+void
+ble_gap_passkey_event(uint16_t conn_handle,
+                      struct ble_gap_passkey_params *passkey_params)
+{
+#if NIMBLE_BLE_SM
+    struct ble_gap_event event;
+
+    BLE_HS_LOG(DEBUG, "send passkey action request %d\n",
+               passkey_params->action);
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_PASSKEY_ACTION;
+    event.passkey.conn_handle = conn_handle;
+    event.passkey.params = *passkey_params;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+#endif
+}
+
+void
+ble_gap_enc_event(uint16_t conn_handle, int status, int security_restored)
+{
+#if NIMBLE_BLE_SM
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_ENC_CHANGE;
+    event.enc_change.conn_handle = conn_handle;
+    event.enc_change.status = status;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+
+    if (status == 0) {
+        if (security_restored) {
+            ble_gatts_bonding_restored(conn_handle);
+        } else {
+            ble_gatts_bonding_established(conn_handle);
+        }
+    }
+#endif
+}
+
+void
+ble_gap_identity_event(uint16_t conn_handle)
+{
+#if NIMBLE_BLE_SM
+    struct ble_gap_event event;
+
+    BLE_HS_LOG(DEBUG, "send identity changed");
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_IDENTITY_RESOLVED;
+    event.identity_resolved.conn_handle = conn_handle;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+#endif
+}
+
+int
+ble_gap_repeat_pairing_event(const struct ble_gap_repeat_pairing *rp)
+{
+#if NIMBLE_BLE_SM
+    struct ble_gap_event event;
+    int rc;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_REPEAT_PAIRING;
+    event.repeat_pairing = *rp;
+    rc = ble_gap_call_conn_event_cb(&event, rp->conn_handle);
+    return rc;
+#else
+    return 0;
+#endif
+}
+
+/*****************************************************************************
+ * $rssi                                                                     *
+ *****************************************************************************/
+
+int
+ble_gap_conn_rssi(uint16_t conn_handle, int8_t *out_rssi)
+{
+    int rc;
+
+    rc = ble_hs_hci_util_read_rssi(conn_handle, out_rssi);
+    return rc;
+}
+
+/*****************************************************************************
+ * $notify                                                                   *
+ *****************************************************************************/
+
+void
+ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle,
+                        struct os_mbuf *om, int is_indication)
+{
+#if !MYNEWT_VAL(BLE_GATT_NOTIFY) && !MYNEWT_VAL(BLE_GATT_INDICATE)
+    return;
+#endif
+
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_NOTIFY_RX;
+    event.notify_rx.conn_handle = conn_handle;
+    event.notify_rx.attr_handle = attr_handle;
+    event.notify_rx.om = om;
+    event.notify_rx.indication = is_indication;
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+
+    os_mbuf_free_chain(event.notify_rx.om);
+}
+
+void
+ble_gap_notify_tx_event(int status, uint16_t conn_handle, uint16_t attr_handle,
+                        int is_indication)
+{
+#if MYNEWT_VAL(BLE_GATT_NOTIFY) || MYNEWT_VAL(BLE_GATT_INDICATE)
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_NOTIFY_TX;
+    event.notify_tx.conn_handle = conn_handle;
+    event.notify_tx.status = status;
+    event.notify_tx.attr_handle = attr_handle;
+    event.notify_tx.indication = is_indication;
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+#endif
+}
+
+/*****************************************************************************
+ * $subscribe                                                                *
+ *****************************************************************************/
+
+void
+ble_gap_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
+                        uint8_t reason,
+                        uint8_t prev_notify, uint8_t cur_notify,
+                        uint8_t prev_indicate, uint8_t cur_indicate)
+{
+    struct ble_gap_event event;
+
+    BLE_HS_DBG_ASSERT(prev_notify != cur_notify ||
+                      prev_indicate != cur_indicate);
+    BLE_HS_DBG_ASSERT(reason == BLE_GAP_SUBSCRIBE_REASON_WRITE ||
+                      reason == BLE_GAP_SUBSCRIBE_REASON_TERM  ||
+                      reason == BLE_GAP_SUBSCRIBE_REASON_RESTORE);
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_SUBSCRIBE;
+    event.subscribe.conn_handle = conn_handle;
+    event.subscribe.attr_handle = attr_handle;
+    event.subscribe.reason = reason;
+    event.subscribe.prev_notify = !!prev_notify;
+    event.subscribe.cur_notify = !!cur_notify;
+    event.subscribe.prev_indicate = !!prev_indicate;
+    event.subscribe.cur_indicate = !!cur_indicate;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+}
+
+/*****************************************************************************
+ * $mtu                                                                      *
+ *****************************************************************************/
+
+void
+ble_gap_mtu_event(uint16_t conn_handle, uint16_t cid, uint16_t mtu)
+{
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_MTU;
+    event.mtu.conn_handle = conn_handle;
+    event.mtu.channel_id = cid;
+    event.mtu.value = mtu;
+
+    ble_gap_event_listener_call(&event);
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+}
+
+/*****************************************************************************
+ * $preempt                                                                  *
+ *****************************************************************************/
+
+void
+ble_gap_preempt_no_lock(void)
+{
+    int rc;
+    int i;
+
+    (void)rc;
+    (void)i;
+
+#if NIMBLE_BLE_ADVERTISE
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        rc = ble_gap_ext_adv_stop_no_lock(i);
+        if (rc == 0) {
+            ble_gap_slave[i].preempted = 1;
+        }
+    }
+#else
+    rc = ble_gap_adv_stop_no_lock();
+    if (rc == 0) {
+        ble_gap_slave[0].preempted = 1;
+    }
+#endif
+#endif
+
+#if NIMBLE_BLE_CONNECT
+    rc = ble_gap_conn_cancel_no_lock();
+    if (rc == 0) {
+        ble_gap_master.preempted_op = BLE_GAP_OP_M_CONN;
+    }
+#endif
+
+#if NIMBLE_BLE_SCAN
+    rc = ble_gap_disc_cancel_no_lock();
+    if (rc == 0) {
+        ble_gap_master.preempted_op = BLE_GAP_OP_M_DISC;
+    }
+#endif
+}
+
+/**
+ * @brief Preempts the GAP if it is not already preempted.
+ *
+ * Aborts all active GAP procedures and prevents new ones from being started.
+ * This function is used to ensure an idle GAP so that the controller's
+ * resolving list can be modified.  When done accessing the resolving list, the
+ * caller must call `ble_gap_preempt_done()` to permit new GAP procedures.
+ *
+ * On preemption, all aborted GAP procedures are reported with a status or
+ * reason code of BLE_HS_EPREEMPTED.  An attempt to initiate a new GAP
+ * procedure during preemption fails with a return code of BLE_HS_EPREEMPTED.
+ */
+void
+ble_gap_preempt(void)
+{
+    ble_hs_lock();
+
+    if (!ble_gap_is_preempted()) {
+        ble_gap_preempt_no_lock();
+    }
+
+    ble_hs_unlock();
+}
+
+/**
+ * Takes GAP out of the preempted state, allowing new GAP procedures to be
+ * initiated.  This function should only be called after a call to
+ * `ble_gap_preempt()`.
+ */
+
+static struct ble_npl_mutex preempt_done_mutex;
+
+void
+ble_gap_preempt_done(void)
+{
+    struct ble_gap_event event;
+    ble_gap_event_fn *master_cb;
+    void *master_arg;
+    int disc_preempted;
+    int i;
+    static struct {
+        ble_gap_event_fn *cb;
+        void *arg;
+    } slaves[BLE_ADV_INSTANCES];
+
+    disc_preempted = 0;
+
+    /* Protects slaves from accessing by multiple threads */
+    ble_npl_mutex_pend(&preempt_done_mutex, 0xFFFFFFFF);
+    memset(slaves, 0, sizeof(slaves));
+
+    ble_hs_lock();
+
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        if (ble_gap_slave[i].preempted) {
+            ble_gap_slave[i].preempted = 0;
+            slaves[i].cb = ble_gap_slave[i].cb;
+            slaves[i].arg = ble_gap_slave[i].cb_arg;
+        }
+    }
+
+    if (ble_gap_master.preempted_op == BLE_GAP_OP_M_DISC) {
+        ble_gap_master.preempted_op = BLE_GAP_OP_NULL;
+        disc_preempted = 1;
+        master_cb = ble_gap_master.cb;
+        master_arg = ble_gap_master.cb_arg;
+    }
+
+    ble_hs_unlock();
+
+    event.type = BLE_GAP_EVENT_ADV_COMPLETE;
+    event.adv_complete.reason = BLE_HS_EPREEMPTED;
+
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        if (slaves[i].cb) {
+#if MYNEWT_VAL(BLE_EXT_ADV)
+            event.adv_complete.instance = i;
+            event.adv_complete.conn_handle = i;
+#endif
+            ble_gap_call_event_cb(&event, slaves[i].cb, slaves[i].arg);
+        }
+    }
+    ble_npl_mutex_release(&preempt_done_mutex);
+
+    if (disc_preempted) {
+        event.type = BLE_GAP_EVENT_DISC_COMPLETE;
+        event.disc_complete.reason = BLE_HS_EPREEMPTED;
+        ble_gap_call_event_cb(&event, master_cb, master_arg);
+    }
+}
+
+int
+ble_gap_event_listener_register(struct ble_gap_event_listener *listener,
+                                ble_gap_event_fn *fn, void *arg)
+{
+    struct ble_gap_event_listener *evl = NULL;
+    int rc;
+
+    SLIST_FOREACH(evl, &ble_gap_event_listener_list, link) {
+        if (evl == listener) {
+            break;
+        }
+    }
+
+    if (!evl) {
+        if (fn) {
+            memset(listener, 0, sizeof(*listener));
+            listener->fn = fn;
+            listener->arg = arg;
+            SLIST_INSERT_HEAD(&ble_gap_event_listener_list, listener, link);
+            rc = 0;
+        } else {
+            rc = BLE_HS_EINVAL;
+        }
+    } else {
+        rc = BLE_HS_EALREADY;
+    }
+
+    return rc;
+}
+
+int
+ble_gap_event_listener_unregister(struct ble_gap_event_listener *listener)
+{
+    struct ble_gap_event_listener *evl = NULL;
+    int rc;
+
+    /*
+     * We check if element exists on the list only for sanity to let caller
+     * know whether it registered its listener before.
+     */
+
+    SLIST_FOREACH(evl, &ble_gap_event_listener_list, link) {
+        if (evl == listener) {
+            break;
+        }
+    }
+
+    if (!evl) {
+        rc = BLE_HS_ENOENT;
+    } else {
+        SLIST_REMOVE(&ble_gap_event_listener_list, listener,
+                     ble_gap_event_listener, link);
+        rc = 0;
+    }
+
+    return rc;
+}
+
+static int
+ble_gap_event_listener_call(struct ble_gap_event *event)
+{
+    struct ble_gap_event_listener *evl = NULL;
+
+    SLIST_FOREACH(evl, &ble_gap_event_listener_list, link) {
+        evl->fn(event, evl->arg);
+    }
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $init                                                                     *
+ *****************************************************************************/
+
+int
+ble_gap_init(void)
+{
+    int rc;
+
+    memset(&ble_gap_master, 0, sizeof(ble_gap_master));
+    memset(ble_gap_slave, 0, sizeof(ble_gap_slave));
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    memset(&ble_gap_sync, 0, sizeof(ble_gap_sync));
+#endif
+
+    ble_npl_mutex_init(&preempt_done_mutex);
+
+    SLIST_INIT(&ble_gap_update_entries);
+    SLIST_INIT(&ble_gap_event_listener_list);
+
+    rc = os_mempool_init(&ble_gap_update_entry_pool,
+                         MYNEWT_VAL(BLE_GAP_MAX_PENDING_CONN_PARAM_UPDATE),
+                         sizeof (struct ble_gap_update_entry),
+                         ble_gap_update_entry_mem,
+                         "ble_gap_update");
+    switch (rc) {
+    case 0:
+        break;
+    case OS_ENOMEM:
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    default:
+        rc = BLE_HS_EOS;
+        goto err;
+    }
+
+    rc = stats_init_and_reg(
+        STATS_HDR(ble_gap_stats), STATS_SIZE_INIT_PARMS(ble_gap_stats,
+        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_gap_stats), "ble_gap");
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    return rc;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_gap_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_gap_priv.h
new file mode 100644
index 00000000..c050435f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_gap_priv.h
@@ -0,0 +1,153 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_GAP_CONN_
+#define H_BLE_GAP_CONN_
+
+#include <inttypes.h>
+#include "syscfg/syscfg.h"
+#include "stats/stats.h"
+#include "host/ble_gap.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct hci_le_conn_upd_complete;
+struct hci_le_conn_param_req;
+struct hci_le_conn_complete;
+struct hci_disconn_complete;
+struct hci_encrypt_change;
+struct ble_hs_hci_ack;
+struct ble_hs_adv;
+
+STATS_SECT_START(ble_gap_stats)
+    STATS_SECT_ENTRY(wl_set)
+    STATS_SECT_ENTRY(wl_set_fail)
+    STATS_SECT_ENTRY(adv_stop)
+    STATS_SECT_ENTRY(adv_stop_fail)
+    STATS_SECT_ENTRY(adv_start)
+    STATS_SECT_ENTRY(adv_start_fail)
+    STATS_SECT_ENTRY(adv_set_data)
+    STATS_SECT_ENTRY(adv_set_data_fail)
+    STATS_SECT_ENTRY(adv_rsp_set_data)
+    STATS_SECT_ENTRY(adv_rsp_set_data_fail)
+    STATS_SECT_ENTRY(discover)
+    STATS_SECT_ENTRY(discover_fail)
+    STATS_SECT_ENTRY(initiate)
+    STATS_SECT_ENTRY(initiate_fail)
+    STATS_SECT_ENTRY(terminate)
+    STATS_SECT_ENTRY(terminate_fail)
+    STATS_SECT_ENTRY(cancel)
+    STATS_SECT_ENTRY(cancel_fail)
+    STATS_SECT_ENTRY(update)
+    STATS_SECT_ENTRY(update_fail)
+    STATS_SECT_ENTRY(connect_mst)
+    STATS_SECT_ENTRY(connect_slv)
+    STATS_SECT_ENTRY(disconnect)
+    STATS_SECT_ENTRY(rx_disconnect)
+    STATS_SECT_ENTRY(rx_update_complete)
+    STATS_SECT_ENTRY(rx_adv_report)
+    STATS_SECT_ENTRY(rx_conn_complete)
+    STATS_SECT_ENTRY(discover_cancel)
+    STATS_SECT_ENTRY(discover_cancel_fail)
+    STATS_SECT_ENTRY(security_initiate)
+    STATS_SECT_ENTRY(security_initiate_fail)
+STATS_SECT_END
+
+extern STATS_SECT_DECL(ble_gap_stats) ble_gap_stats;
+
+#define BLE_GAP_CONN_MODE_MAX               3
+#define BLE_GAP_DISC_MODE_MAX               3
+
+#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN
+void ble_gap_rx_le_scan_timeout(void);
+#endif
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+void ble_gap_rx_ext_adv_report(struct ble_gap_ext_disc_desc *desc);
+void ble_gap_rx_adv_set_terminated(const struct ble_hci_ev_le_subev_adv_set_terminated *ev);
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+void ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev);
+void ble_gap_rx_periodic_adv_rpt(const struct ble_hci_ev_le_subev_periodic_adv_rpt *ev);
+void ble_gap_rx_periodic_adv_sync_lost(const struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev);
+void ble_gap_rx_periodic_adv_sync_transfer(const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev);
+#endif
+void ble_gap_rx_scan_req_rcvd(const struct ble_hci_ev_le_subev_scan_req_rcvd *ev);
+#endif
+void ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc);
+void ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used_feat *ev);
+
+struct ble_gap_conn_complete
+{
+    uint8_t status;
+    uint16_t connection_handle;
+    uint8_t role;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+    uint16_t conn_itvl;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint8_t master_clk_acc;
+    uint8_t local_rpa[BLE_DEV_ADDR_LEN];
+    uint8_t peer_rpa[BLE_DEV_ADDR_LEN];
+};
+
+int ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance);
+void ble_gap_rx_disconn_complete(const struct ble_hci_ev_disconn_cmp *ev);
+void ble_gap_rx_update_complete(const struct ble_hci_ev_le_subev_conn_upd_complete *ev);
+void ble_gap_rx_param_req(const struct ble_hci_ev_le_subev_rem_conn_param_req *ev);
+int ble_gap_rx_l2cap_update_req(uint16_t conn_handle,
+                                struct ble_gap_upd_params *params);
+void ble_gap_rx_phy_update_complete(const struct ble_hci_ev_le_subev_phy_update_complete *ev);
+void ble_gap_enc_event(uint16_t conn_handle, int status,
+                       int security_restored);
+void ble_gap_passkey_event(uint16_t conn_handle,
+                           struct ble_gap_passkey_params *passkey_params);
+void ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle,
+                             struct os_mbuf *om, int is_indication);
+void ble_gap_notify_tx_event(int status, uint16_t conn_handle,
+                             uint16_t attr_handle, int is_indication);
+void ble_gap_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
+                             uint8_t reason,
+                             uint8_t prev_notify, uint8_t cur_notify,
+                             uint8_t prev_indicate, uint8_t cur_indicate);
+void ble_gap_mtu_event(uint16_t conn_handle, uint16_t cid, uint16_t mtu);
+void ble_gap_identity_event(uint16_t conn_handle);
+int ble_gap_repeat_pairing_event(const struct ble_gap_repeat_pairing *rp);
+int ble_gap_master_in_progress(void);
+
+void ble_gap_preempt(void);
+void ble_gap_preempt_done(void);
+
+int ble_gap_terminate_with_conn(struct ble_hs_conn *conn, uint8_t hci_reason);
+void ble_gap_reset_state(int reason);
+void ble_gap_conn_broken(uint16_t conn_handle, int reason);
+int32_t ble_gap_timer(void);
+
+int ble_gap_init(void);
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+int ble_gap_dbg_update_active(uint16_t conn_handle);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_gatt_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_gatt_priv.h
new file mode 100644
index 00000000..4a59635b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_gatt_priv.h
@@ -0,0 +1,199 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_GATT_PRIV_
+#define H_BLE_GATT_PRIV_
+
+#include "syscfg/syscfg.h"
+#include "stats/stats.h"
+#include "host/ble_gatt.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_att_read_type_adata;
+struct ble_att_find_type_value_hinfo;
+struct ble_att_find_info_idata;
+struct ble_att_read_group_type_adata;
+struct ble_att_prep_write_cmd;
+
+STATS_SECT_START(ble_gattc_stats)
+    STATS_SECT_ENTRY(mtu)
+    STATS_SECT_ENTRY(mtu_fail)
+    STATS_SECT_ENTRY(disc_all_svcs)
+    STATS_SECT_ENTRY(disc_all_svcs_fail)
+    STATS_SECT_ENTRY(disc_svc_uuid)
+    STATS_SECT_ENTRY(disc_svc_uuid_fail)
+    STATS_SECT_ENTRY(find_inc_svcs)
+    STATS_SECT_ENTRY(find_inc_svcs_fail)
+    STATS_SECT_ENTRY(disc_all_chrs)
+    STATS_SECT_ENTRY(disc_all_chrs_fail)
+    STATS_SECT_ENTRY(disc_chrs_uuid)
+    STATS_SECT_ENTRY(disc_chrs_uuid_fail)
+    STATS_SECT_ENTRY(disc_all_dscs)
+    STATS_SECT_ENTRY(disc_all_dscs_fail)
+    STATS_SECT_ENTRY(read)
+    STATS_SECT_ENTRY(read_fail)
+    STATS_SECT_ENTRY(read_uuid)
+    STATS_SECT_ENTRY(read_uuid_fail)
+    STATS_SECT_ENTRY(read_long)
+    STATS_SECT_ENTRY(read_long_fail)
+    STATS_SECT_ENTRY(read_mult)
+    STATS_SECT_ENTRY(read_mult_fail)
+    STATS_SECT_ENTRY(write_no_rsp)
+    STATS_SECT_ENTRY(write_no_rsp_fail)
+    STATS_SECT_ENTRY(write)
+    STATS_SECT_ENTRY(write_fail)
+    STATS_SECT_ENTRY(write_long)
+    STATS_SECT_ENTRY(write_long_fail)
+    STATS_SECT_ENTRY(write_reliable)
+    STATS_SECT_ENTRY(write_reliable_fail)
+    STATS_SECT_ENTRY(notify)
+    STATS_SECT_ENTRY(notify_fail)
+    STATS_SECT_ENTRY(indicate)
+    STATS_SECT_ENTRY(indicate_fail)
+    STATS_SECT_ENTRY(proc_timeout)
+STATS_SECT_END
+extern STATS_SECT_DECL(ble_gattc_stats) ble_gattc_stats;
+
+STATS_SECT_START(ble_gatts_stats)
+    STATS_SECT_ENTRY(svcs)
+    STATS_SECT_ENTRY(chrs)
+    STATS_SECT_ENTRY(dscs)
+    STATS_SECT_ENTRY(svc_def_reads)
+    STATS_SECT_ENTRY(svc_inc_reads)
+    STATS_SECT_ENTRY(chr_def_reads)
+    STATS_SECT_ENTRY(chr_val_reads)
+    STATS_SECT_ENTRY(chr_val_writes)
+    STATS_SECT_ENTRY(dsc_reads)
+    STATS_SECT_ENTRY(dsc_writes)
+STATS_SECT_END
+extern STATS_SECT_DECL(ble_gatts_stats) ble_gatts_stats;
+
+#define BLE_GATT_CHR_DECL_SZ_16     5
+#define BLE_GATT_CHR_DECL_SZ_128    19
+
+typedef uint8_t ble_gatts_conn_flags;
+
+struct ble_gatts_conn {
+    struct ble_gatts_clt_cfg *clt_cfgs;
+    int num_clt_cfgs;
+
+    uint16_t indicate_val_handle;
+};
+
+/*** @client. */
+
+int ble_gattc_locked_by_cur_task(void);
+void ble_gatts_indicate_fail_notconn(uint16_t conn_handle);
+
+void ble_gattc_rx_err(uint16_t conn_handle, uint16_t handle, uint16_t status);
+void ble_gattc_rx_mtu(uint16_t conn_handle, int status, uint16_t chan_mtu);
+void ble_gattc_rx_read_type_adata(uint16_t conn_handle,
+                                  struct ble_att_read_type_adata *adata);
+void ble_gattc_rx_read_type_complete(uint16_t conn_handle, int status);
+void ble_gattc_rx_read_rsp(uint16_t conn_handle, int status,
+                           struct os_mbuf **rxom);
+void ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, int status,
+                                struct os_mbuf **rxom);
+void ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, int status,
+                                struct os_mbuf **rxom);
+void ble_gattc_rx_read_group_type_adata(
+    uint16_t conn_handle, struct ble_att_read_group_type_adata *adata);
+void ble_gattc_rx_read_group_type_complete(uint16_t conn_handle, int rc);
+void ble_gattc_rx_find_type_value_hinfo(
+    uint16_t conn_handle, struct ble_att_find_type_value_hinfo *hinfo);
+void ble_gattc_rx_find_type_value_complete(uint16_t conn_handle, int status);
+void ble_gattc_rx_write_rsp(uint16_t conn_handle);
+void ble_gattc_rx_prep_write_rsp(uint16_t conn_handle, int status,
+                                 uint16_t handle, uint16_t offset,
+                                 struct os_mbuf **rxom);
+void ble_gattc_rx_exec_write_rsp(uint16_t conn_handle, int status);
+void ble_gattc_rx_indicate_rsp(uint16_t conn_handle);
+void ble_gattc_rx_find_info_idata(uint16_t conn_handle,
+                                  struct ble_att_find_info_idata *idata);
+void ble_gattc_rx_find_info_complete(uint16_t conn_handle, int status);
+void ble_gattc_connection_txable(uint16_t conn_handle);
+void ble_gattc_connection_broken(uint16_t conn_handle);
+int32_t ble_gattc_timer(void);
+
+int ble_gattc_any_jobs(void);
+int ble_gattc_init(void);
+
+/*** @server. */
+#define BLE_GATTS_CLT_CFG_F_NOTIFY              0x0001
+#define BLE_GATTS_CLT_CFG_F_INDICATE            0x0002
+#define BLE_GATTS_CLT_CFG_F_MODIFIED            0x0080 /* Internal only. */
+#define BLE_GATTS_CLT_CFG_F_RESERVED            0xfffc
+
+#define BLE_GATTS_INC_SVC_LEN_NO_UUID           4
+#define BLE_GATTS_INC_SVC_LEN_UUID              6
+
+/**
+ * Contains counts of resources required by the GATT server.  The contents of
+ * this struct are generally used to populate a configuration struct before
+ * the host is initialized.
+ */
+struct ble_gatt_resources {
+    /** Number of services. */
+    uint16_t svcs;
+
+    /** Number of included services. */
+    uint16_t incs;
+
+    /** Number of characteristics. */
+    uint16_t chrs;
+
+    /** Number of descriptors. */
+    uint16_t dscs;
+
+    /**
+     * Number of client characteristic configuration descriptors.  Each of
+     * these also contributes to the total descriptor count.
+     */
+    uint16_t cccds;
+
+    /** Total number of ATT attributes. */
+    uint16_t attrs;
+};
+
+int ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle);
+int ble_gatts_send_next_indicate(uint16_t conn_handle);
+void ble_gatts_tx_notifications(void);
+void ble_gatts_bonding_established(uint16_t conn_handle);
+void ble_gatts_bonding_restored(uint16_t conn_handle);
+void ble_gatts_connection_broken(uint16_t conn_handle);
+void ble_gatts_lcl_svc_foreach(ble_gatt_svc_foreach_fn cb, void *arg);
+int ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
+                            ble_gatt_register_fn *register_cb,
+                            void *cb_arg);
+int ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle,
+                             uint8_t op, uint16_t offset, struct os_mbuf **om,
+                             void *arg);
+
+/*** @misc. */
+int ble_gatts_conn_can_alloc(void);
+int ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn);
+int ble_gatts_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_gattc.c b/src/libs/mynewt-nimble/nimble/host/src/ble_gattc.c
new file mode 100644
index 00000000..a6e114cc
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_gattc.c
@@ -0,0 +1,4806 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/**
+ * GATT client - Generic Attribute Profile; client operations.
+ *
+ * Design overview:
+ *
+ * GATT client procedures are initiated by the application via function calls.
+ * Such functions return when either of the following happens:
+ *
+ * (1) The procedure completes (success or failure).
+ * (2) The procedure cannot proceed until a BLE peer responds.
+ *
+ * For (1), the result of the procedure if fully indicated by the function
+ * return code.
+ * For (2), the procedure result is indicated by an application-configured
+ * callback.  The callback is executed when the procedure completes.
+ *
+ * Notes on thread-safety:
+ * 1. The ble_hs mutex must never be locked when an application callback is
+ *    executed.  A callback is free to initiate additional host procedures.
+ * 2. The only resource protected by the mutex is the list of active procedures
+ *    (ble_gattc_procs).  Thread-safety is achieved by locking the mutex during
+ *    removal and insertion operations.  Procedure objects are only modified
+ *    while they are not in the list.  This is sufficient, as the host parent
+ *    task is the only task which inspects or modifies individual procedure
+ *    entries.  Tasks have the following permissions regarding procedure
+ *    entries:
+ *
+ *                | insert  | remove    | inspect   | modify
+ *    ------------+---------+-----------|-----------|---------
+ *    parent task | X       | X         | X         | X
+ *    other tasks | X       |           |           |
+ */
+
+#include <stddef.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+#include "os/os_mempool.h"
+#include "nimble/ble.h"
+#include "host/ble_uuid.h"
+#include "host/ble_gap.h"
+#include "ble_hs_priv.h"
+
+/*****************************************************************************
+ * $definitions / declarations                                               *
+ *****************************************************************************/
+
+/**
+ * The maximum time to wait for a single ATT response.  The spec defines this
+ * as the ATT transaction time (Vol. 3, Part F, 3.3.3)
+ */
+#define BLE_GATTC_UNRESPONSIVE_TIMEOUT_MS       30000 /* ms */
+
+#define BLE_GATT_OP_NONE                        UINT8_MAX
+#define BLE_GATT_OP_MTU                         0
+#define BLE_GATT_OP_DISC_ALL_SVCS               1
+#define BLE_GATT_OP_DISC_SVC_UUID               2
+#define BLE_GATT_OP_FIND_INC_SVCS               3
+#define BLE_GATT_OP_DISC_ALL_CHRS               4
+#define BLE_GATT_OP_DISC_CHR_UUID               5
+#define BLE_GATT_OP_DISC_ALL_DSCS               6
+#define BLE_GATT_OP_READ                        7
+#define BLE_GATT_OP_READ_UUID                   8
+#define BLE_GATT_OP_READ_LONG                   9
+#define BLE_GATT_OP_READ_MULT                   10
+#define BLE_GATT_OP_WRITE                       11
+#define BLE_GATT_OP_WRITE_LONG                  12
+#define BLE_GATT_OP_WRITE_RELIABLE              13
+#define BLE_GATT_OP_INDICATE                    14
+#define BLE_GATT_OP_CNT                         15
+
+/** Procedure stalled due to resource exhaustion. */
+#define BLE_GATTC_PROC_F_STALLED                0x01
+
+/** Represents an in-progress GATT procedure. */
+struct ble_gattc_proc {
+    STAILQ_ENTRY(ble_gattc_proc) next;
+
+    uint32_t exp_os_ticks;
+    uint16_t conn_handle;
+    uint8_t op;
+    uint8_t flags;
+
+    union {
+        struct {
+            ble_gatt_mtu_fn *cb;
+            void *cb_arg;
+        } mtu;
+
+        struct {
+            uint16_t prev_handle;
+            ble_gatt_disc_svc_fn *cb;
+            void *cb_arg;
+        } disc_all_svcs;
+
+        struct {
+            ble_uuid_any_t service_uuid;
+            uint16_t prev_handle;
+            ble_gatt_disc_svc_fn *cb;
+            void *cb_arg;
+        } disc_svc_uuid;
+
+        struct {
+            uint16_t prev_handle;
+            uint16_t end_handle;
+
+            uint16_t cur_start;
+            uint16_t cur_end;
+
+            ble_gatt_disc_svc_fn *cb;
+            void *cb_arg;
+        } find_inc_svcs;
+
+        struct {
+            uint16_t prev_handle;
+            uint16_t end_handle;
+            ble_gatt_chr_fn *cb;
+            void *cb_arg;
+        } disc_all_chrs;
+
+        struct {
+            ble_uuid_any_t chr_uuid;
+            uint16_t prev_handle;
+            uint16_t end_handle;
+            ble_gatt_chr_fn *cb;
+            void *cb_arg;
+        } disc_chr_uuid;
+
+        struct {
+            uint16_t chr_val_handle;
+            uint16_t prev_handle;
+            uint16_t end_handle;
+            ble_gatt_dsc_fn *cb;
+            void *cb_arg;
+        } disc_all_dscs;
+
+        struct {
+            uint16_t handle;
+            ble_gatt_attr_fn *cb;
+            void *cb_arg;
+        } read;
+
+        struct {
+            ble_uuid_any_t chr_uuid;
+            uint16_t start_handle;
+            uint16_t end_handle;
+            ble_gatt_attr_fn *cb;
+            void *cb_arg;
+        } read_uuid;
+
+        struct {
+            uint16_t handle;
+            uint16_t offset;
+            ble_gatt_attr_fn *cb;
+            void *cb_arg;
+        } read_long;
+
+        struct {
+            uint16_t handles[MYNEWT_VAL(BLE_GATT_READ_MAX_ATTRS)];
+            uint8_t num_handles;
+            ble_gatt_attr_fn *cb;
+            void *cb_arg;
+        } read_mult;
+
+        struct {
+            uint16_t att_handle;
+            ble_gatt_attr_fn *cb;
+            void *cb_arg;
+        } write;
+
+        struct {
+            struct ble_gatt_attr attr;
+            uint16_t length;
+            ble_gatt_attr_fn *cb;
+            void *cb_arg;
+        } write_long;
+
+        struct {
+            struct ble_gatt_attr attrs[MYNEWT_VAL(BLE_GATT_WRITE_MAX_ATTRS)];
+            uint8_t num_attrs;
+            uint8_t cur_attr;
+            uint16_t length;
+            ble_gatt_reliable_attr_fn *cb;
+            void *cb_arg;
+        } write_reliable;
+
+        struct {
+            uint16_t chr_val_handle;
+        } indicate;
+    };
+};
+
+STAILQ_HEAD(ble_gattc_proc_list, ble_gattc_proc);
+
+/**
+ * Error functions - these handle an incoming ATT error response and apply it
+ * to the appropriate active GATT procedure.
+ */
+typedef void ble_gattc_err_fn(struct ble_gattc_proc *proc, int status,
+                              uint16_t att_handle);
+static ble_gattc_err_fn ble_gattc_mtu_err;
+static ble_gattc_err_fn ble_gattc_disc_all_svcs_err;
+static ble_gattc_err_fn ble_gattc_disc_svc_uuid_err;
+static ble_gattc_err_fn ble_gattc_find_inc_svcs_err;
+static ble_gattc_err_fn ble_gattc_disc_all_chrs_err;
+static ble_gattc_err_fn ble_gattc_disc_chr_uuid_err;
+static ble_gattc_err_fn ble_gattc_disc_all_dscs_err;
+static ble_gattc_err_fn ble_gattc_read_err;
+static ble_gattc_err_fn ble_gattc_read_uuid_err;
+static ble_gattc_err_fn ble_gattc_read_long_err;
+static ble_gattc_err_fn ble_gattc_read_mult_err;
+static ble_gattc_err_fn ble_gattc_write_err;
+static ble_gattc_err_fn ble_gattc_write_long_err;
+static ble_gattc_err_fn ble_gattc_write_reliable_err;
+static ble_gattc_err_fn ble_gattc_indicate_err;
+
+static ble_gattc_err_fn * const ble_gattc_err_dispatch[BLE_GATT_OP_CNT] = {
+    [BLE_GATT_OP_MTU]               = ble_gattc_mtu_err,
+    [BLE_GATT_OP_DISC_ALL_SVCS]     = ble_gattc_disc_all_svcs_err,
+    [BLE_GATT_OP_DISC_SVC_UUID]     = ble_gattc_disc_svc_uuid_err,
+    [BLE_GATT_OP_FIND_INC_SVCS]     = ble_gattc_find_inc_svcs_err,
+    [BLE_GATT_OP_DISC_ALL_CHRS]     = ble_gattc_disc_all_chrs_err,
+    [BLE_GATT_OP_DISC_CHR_UUID]     = ble_gattc_disc_chr_uuid_err,
+    [BLE_GATT_OP_DISC_ALL_DSCS]     = ble_gattc_disc_all_dscs_err,
+    [BLE_GATT_OP_READ]              = ble_gattc_read_err,
+    [BLE_GATT_OP_READ_UUID]         = ble_gattc_read_uuid_err,
+    [BLE_GATT_OP_READ_LONG]         = ble_gattc_read_long_err,
+    [BLE_GATT_OP_READ_MULT]         = ble_gattc_read_mult_err,
+    [BLE_GATT_OP_WRITE]             = ble_gattc_write_err,
+    [BLE_GATT_OP_WRITE_LONG]        = ble_gattc_write_long_err,
+    [BLE_GATT_OP_WRITE_RELIABLE]    = ble_gattc_write_reliable_err,
+    [BLE_GATT_OP_INDICATE]          = ble_gattc_indicate_err,
+};
+
+/**
+ * Resume functions - these handle periodic retries of procedures that have
+ * stalled due to memory exhaustion.
+ */
+typedef int ble_gattc_resume_fn(struct ble_gattc_proc *proc);
+
+static ble_gattc_resume_fn ble_gattc_disc_all_svcs_resume;
+static ble_gattc_resume_fn ble_gattc_disc_svc_uuid_resume;
+static ble_gattc_resume_fn ble_gattc_find_inc_svcs_resume;
+static ble_gattc_resume_fn ble_gattc_disc_all_chrs_resume;
+static ble_gattc_resume_fn ble_gattc_disc_chr_uuid_resume;
+static ble_gattc_resume_fn ble_gattc_disc_all_dscs_resume;
+static ble_gattc_resume_fn ble_gattc_read_long_resume;
+static ble_gattc_resume_fn ble_gattc_write_long_resume;
+static ble_gattc_resume_fn ble_gattc_write_reliable_resume;
+
+static ble_gattc_resume_fn * const
+ble_gattc_resume_dispatch[BLE_GATT_OP_CNT] = {
+    [BLE_GATT_OP_MTU]               = NULL,
+    [BLE_GATT_OP_DISC_ALL_SVCS]     = ble_gattc_disc_all_svcs_resume,
+    [BLE_GATT_OP_DISC_SVC_UUID]     = ble_gattc_disc_svc_uuid_resume,
+    [BLE_GATT_OP_FIND_INC_SVCS]     = ble_gattc_find_inc_svcs_resume,
+    [BLE_GATT_OP_DISC_ALL_CHRS]     = ble_gattc_disc_all_chrs_resume,
+    [BLE_GATT_OP_DISC_CHR_UUID]     = ble_gattc_disc_chr_uuid_resume,
+    [BLE_GATT_OP_DISC_ALL_DSCS]     = ble_gattc_disc_all_dscs_resume,
+    [BLE_GATT_OP_READ]              = NULL,
+    [BLE_GATT_OP_READ_UUID]         = NULL,
+    [BLE_GATT_OP_READ_LONG]         = ble_gattc_read_long_resume,
+    [BLE_GATT_OP_READ_MULT]         = NULL,
+    [BLE_GATT_OP_WRITE]             = NULL,
+    [BLE_GATT_OP_WRITE_LONG]        = ble_gattc_write_long_resume,
+    [BLE_GATT_OP_WRITE_RELIABLE]    = ble_gattc_write_reliable_resume,
+    [BLE_GATT_OP_INDICATE]          = NULL,
+};
+
+/**
+ * Timeout functions - these notify the application that a GATT procedure has
+ * timed out while waiting for a response.
+ */
+typedef void ble_gattc_tmo_fn(struct ble_gattc_proc *proc);
+
+static ble_gattc_tmo_fn ble_gattc_mtu_tmo;
+static ble_gattc_tmo_fn ble_gattc_disc_all_svcs_tmo;
+static ble_gattc_tmo_fn ble_gattc_disc_svc_uuid_tmo;
+static ble_gattc_tmo_fn ble_gattc_find_inc_svcs_tmo;
+static ble_gattc_tmo_fn ble_gattc_disc_all_chrs_tmo;
+static ble_gattc_tmo_fn ble_gattc_disc_chr_uuid_tmo;
+static ble_gattc_tmo_fn ble_gattc_disc_all_dscs_tmo;
+static ble_gattc_tmo_fn ble_gattc_read_tmo;
+static ble_gattc_tmo_fn ble_gattc_read_uuid_tmo;
+static ble_gattc_tmo_fn ble_gattc_read_long_tmo;
+static ble_gattc_tmo_fn ble_gattc_read_mult_tmo;
+static ble_gattc_tmo_fn ble_gattc_write_tmo;
+static ble_gattc_tmo_fn ble_gattc_write_long_tmo;
+static ble_gattc_tmo_fn ble_gattc_write_reliable_tmo;
+static ble_gattc_tmo_fn ble_gattc_indicate_tmo;
+
+static ble_gattc_tmo_fn * const
+ble_gattc_tmo_dispatch[BLE_GATT_OP_CNT] = {
+    [BLE_GATT_OP_MTU]               = ble_gattc_mtu_tmo,
+    [BLE_GATT_OP_DISC_ALL_SVCS]     = ble_gattc_disc_all_svcs_tmo,
+    [BLE_GATT_OP_DISC_SVC_UUID]     = ble_gattc_disc_svc_uuid_tmo,
+    [BLE_GATT_OP_FIND_INC_SVCS]     = ble_gattc_find_inc_svcs_tmo,
+    [BLE_GATT_OP_DISC_ALL_CHRS]     = ble_gattc_disc_all_chrs_tmo,
+    [BLE_GATT_OP_DISC_CHR_UUID]     = ble_gattc_disc_chr_uuid_tmo,
+    [BLE_GATT_OP_DISC_ALL_DSCS]     = ble_gattc_disc_all_dscs_tmo,
+    [BLE_GATT_OP_READ]              = ble_gattc_read_tmo,
+    [BLE_GATT_OP_READ_UUID]         = ble_gattc_read_uuid_tmo,
+    [BLE_GATT_OP_READ_LONG]         = ble_gattc_read_long_tmo,
+    [BLE_GATT_OP_READ_MULT]         = ble_gattc_read_mult_tmo,
+    [BLE_GATT_OP_WRITE]             = ble_gattc_write_tmo,
+    [BLE_GATT_OP_WRITE_LONG]        = ble_gattc_write_long_tmo,
+    [BLE_GATT_OP_WRITE_RELIABLE]    = ble_gattc_write_reliable_tmo,
+    [BLE_GATT_OP_INDICATE]          = ble_gattc_indicate_tmo,
+};
+
+/**
+ * Receive functions - these handle specific incoming responses and apply them
+ * to the appropriate active GATT procedure.
+ */
+typedef int ble_gattc_rx_adata_fn(struct ble_gattc_proc *proc,
+                                  struct ble_att_read_type_adata *adata);
+
+typedef int ble_gattc_rx_prep_fn(struct ble_gattc_proc *proc, int status,
+                                 uint16_t handle, uint16_t offset,
+                                 struct os_mbuf **om);
+
+typedef int ble_gattc_rx_attr_fn(struct ble_gattc_proc *proc, int status,
+                                 struct os_mbuf **om);
+
+typedef int ble_gattc_rx_complete_fn(struct ble_gattc_proc *proc, int status);
+typedef int ble_gattc_rx_exec_fn(struct ble_gattc_proc *proc, int status);
+
+static ble_gattc_rx_adata_fn ble_gattc_find_inc_svcs_rx_adata;
+static ble_gattc_rx_complete_fn ble_gattc_find_inc_svcs_rx_complete;
+static ble_gattc_rx_attr_fn ble_gattc_find_inc_svcs_rx_read_rsp;
+static ble_gattc_rx_adata_fn ble_gattc_disc_all_chrs_rx_adata;
+static ble_gattc_rx_complete_fn ble_gattc_disc_all_chrs_rx_complete;
+static ble_gattc_rx_adata_fn ble_gattc_disc_chr_uuid_rx_adata;
+static ble_gattc_rx_complete_fn ble_gattc_disc_chr_uuid_rx_complete;
+static ble_gattc_rx_attr_fn ble_gattc_read_rx_read_rsp;
+static ble_gattc_rx_attr_fn ble_gattc_read_long_rx_read_rsp;
+static ble_gattc_rx_adata_fn ble_gattc_read_uuid_rx_adata;
+static ble_gattc_rx_complete_fn ble_gattc_read_uuid_rx_complete;
+static ble_gattc_rx_prep_fn ble_gattc_write_long_rx_prep;
+static ble_gattc_rx_exec_fn ble_gattc_write_long_rx_exec;
+static ble_gattc_rx_prep_fn ble_gattc_write_reliable_rx_prep;
+static ble_gattc_rx_exec_fn ble_gattc_write_reliable_rx_exec;
+
+static const struct ble_gattc_rx_adata_entry {
+    uint8_t op;
+    ble_gattc_rx_adata_fn *cb;
+} ble_gattc_rx_read_type_elem_entries[] = {
+    { BLE_GATT_OP_FIND_INC_SVCS,    ble_gattc_find_inc_svcs_rx_adata },
+    { BLE_GATT_OP_DISC_ALL_CHRS,    ble_gattc_disc_all_chrs_rx_adata },
+    { BLE_GATT_OP_DISC_CHR_UUID,    ble_gattc_disc_chr_uuid_rx_adata },
+    { BLE_GATT_OP_READ_UUID,        ble_gattc_read_uuid_rx_adata },
+};
+
+static const struct ble_gattc_rx_complete_entry {
+    uint8_t op;
+    ble_gattc_rx_complete_fn *cb;
+} ble_gattc_rx_read_type_complete_entries[] = {
+    { BLE_GATT_OP_FIND_INC_SVCS,    ble_gattc_find_inc_svcs_rx_complete },
+    { BLE_GATT_OP_DISC_ALL_CHRS,    ble_gattc_disc_all_chrs_rx_complete },
+    { BLE_GATT_OP_DISC_CHR_UUID,    ble_gattc_disc_chr_uuid_rx_complete },
+    { BLE_GATT_OP_READ_UUID,        ble_gattc_read_uuid_rx_complete },
+};
+
+static const struct ble_gattc_rx_attr_entry {
+    uint8_t op;
+    ble_gattc_rx_attr_fn *cb;
+} ble_gattc_rx_read_rsp_entries[] = {
+    { BLE_GATT_OP_READ,             ble_gattc_read_rx_read_rsp },
+    { BLE_GATT_OP_READ_LONG,        ble_gattc_read_long_rx_read_rsp },
+    { BLE_GATT_OP_FIND_INC_SVCS,    ble_gattc_find_inc_svcs_rx_read_rsp },
+};
+
+static const struct ble_gattc_rx_prep_entry {
+    uint8_t op;
+    ble_gattc_rx_prep_fn *cb;
+} ble_gattc_rx_prep_entries[] = {
+    { BLE_GATT_OP_WRITE_LONG,       ble_gattc_write_long_rx_prep },
+    { BLE_GATT_OP_WRITE_RELIABLE,   ble_gattc_write_reliable_rx_prep },
+};
+
+static const struct ble_gattc_rx_exec_entry {
+    uint8_t op;
+    ble_gattc_rx_exec_fn *cb;
+} ble_gattc_rx_exec_entries[] = {
+    { BLE_GATT_OP_WRITE_LONG,       ble_gattc_write_long_rx_exec },
+    { BLE_GATT_OP_WRITE_RELIABLE,   ble_gattc_write_reliable_rx_exec },
+};
+
+static os_membuf_t ble_gattc_proc_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_GATT_MAX_PROCS),
+                    sizeof (struct ble_gattc_proc))
+];
+
+static struct os_mempool ble_gattc_proc_pool;
+
+/* The list of active GATT client procedures. */
+static struct ble_gattc_proc_list ble_gattc_procs;
+
+/* The time when we should attempt to resume stalled procedures, in OS ticks.
+ * A value of 0 indicates no stalled procedures.
+ */
+static ble_npl_time_t ble_gattc_resume_at;
+
+/* Statistics. */
+STATS_SECT_DECL(ble_gattc_stats) ble_gattc_stats;
+STATS_NAME_START(ble_gattc_stats)
+    STATS_NAME(ble_gattc_stats, mtu)
+    STATS_NAME(ble_gattc_stats, mtu_fail)
+    STATS_NAME(ble_gattc_stats, disc_all_svcs)
+    STATS_NAME(ble_gattc_stats, disc_all_svcs_fail)
+    STATS_NAME(ble_gattc_stats, disc_svc_uuid)
+    STATS_NAME(ble_gattc_stats, disc_svc_uuid_fail)
+    STATS_NAME(ble_gattc_stats, find_inc_svcs)
+    STATS_NAME(ble_gattc_stats, find_inc_svcs_fail)
+    STATS_NAME(ble_gattc_stats, disc_all_chrs)
+    STATS_NAME(ble_gattc_stats, disc_all_chrs_fail)
+    STATS_NAME(ble_gattc_stats, disc_chrs_uuid)
+    STATS_NAME(ble_gattc_stats, disc_chrs_uuid_fail)
+    STATS_NAME(ble_gattc_stats, disc_all_dscs)
+    STATS_NAME(ble_gattc_stats, disc_all_dscs_fail)
+    STATS_NAME(ble_gattc_stats, read)
+    STATS_NAME(ble_gattc_stats, read_fail)
+    STATS_NAME(ble_gattc_stats, read_uuid)
+    STATS_NAME(ble_gattc_stats, read_uuid_fail)
+    STATS_NAME(ble_gattc_stats, read_long)
+    STATS_NAME(ble_gattc_stats, read_long_fail)
+    STATS_NAME(ble_gattc_stats, read_mult)
+    STATS_NAME(ble_gattc_stats, read_mult_fail)
+    STATS_NAME(ble_gattc_stats, write_no_rsp)
+    STATS_NAME(ble_gattc_stats, write_no_rsp_fail)
+    STATS_NAME(ble_gattc_stats, write)
+    STATS_NAME(ble_gattc_stats, write_fail)
+    STATS_NAME(ble_gattc_stats, write_long)
+    STATS_NAME(ble_gattc_stats, write_long_fail)
+    STATS_NAME(ble_gattc_stats, write_reliable)
+    STATS_NAME(ble_gattc_stats, write_reliable_fail)
+    STATS_NAME(ble_gattc_stats, notify)
+    STATS_NAME(ble_gattc_stats, notify_fail)
+    STATS_NAME(ble_gattc_stats, indicate)
+    STATS_NAME(ble_gattc_stats, indicate_fail)
+    STATS_NAME(ble_gattc_stats, proc_timeout)
+STATS_NAME_END(ble_gattc_stats)
+
+/*****************************************************************************
+ * $debug                                                                    *
+ *****************************************************************************/
+
+static void
+ble_gattc_dbg_assert_proc_not_inserted(struct ble_gattc_proc *proc)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    struct ble_gattc_proc *cur;
+
+    ble_hs_lock();
+
+    STAILQ_FOREACH(cur, &ble_gattc_procs, next) {
+        BLE_HS_DBG_ASSERT(cur != proc);
+    }
+
+    ble_hs_unlock();
+#endif
+}
+
+/*****************************************************************************
+ * $log                                                                      *
+ *****************************************************************************/
+
+static void
+ble_gattc_log_proc_init(const char *name)
+{
+    BLE_HS_LOG(INFO, "GATT procedure initiated: %s", name);
+}
+
+static void
+ble_gattc_log_uuid(const ble_uuid_t *uuid)
+{
+    char buf[BLE_UUID_STR_LEN];
+
+    ble_uuid_to_str(uuid, buf);
+
+    BLE_HS_LOG(INFO, "%s", buf);
+}
+
+static void
+ble_gattc_log_disc_svc_uuid(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("discover service by uuid; uuid=");
+    ble_gattc_log_uuid(&proc->disc_svc_uuid.service_uuid.u);
+    BLE_HS_LOG(INFO, "\n");
+}
+
+static void
+ble_gattc_log_find_inc_svcs(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("find included services; ");
+    BLE_HS_LOG(INFO, "start_handle=%d end_handle=%d\n",
+               proc->find_inc_svcs.prev_handle + 1,
+               proc->find_inc_svcs.end_handle);
+}
+
+static void
+ble_gattc_log_disc_all_chrs(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("discover all characteristics; ");
+    BLE_HS_LOG(INFO, "start_handle=%d end_handle=%d\n",
+               proc->disc_all_chrs.prev_handle + 1,
+               proc->disc_all_chrs.end_handle);
+}
+
+static void
+ble_gattc_log_disc_chr_uuid(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("discover characteristics by uuid; ");
+    BLE_HS_LOG(INFO, "start_handle=%d end_handle=%d uuid=",
+               proc->disc_chr_uuid.prev_handle + 1,
+               proc->disc_chr_uuid.end_handle);
+    ble_gattc_log_uuid(&proc->disc_chr_uuid.chr_uuid.u);
+    BLE_HS_LOG(INFO, "\n");
+}
+
+static void
+ble_gattc_log_disc_all_dscs(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("discover all descriptors; ");
+    BLE_HS_LOG(INFO, "chr_val_handle=%d end_handle=%d\n",
+               proc->disc_all_dscs.chr_val_handle,
+               proc->disc_all_dscs.end_handle);
+}
+
+static void
+ble_gattc_log_read(uint16_t att_handle)
+{
+    ble_gattc_log_proc_init("read; ");
+    BLE_HS_LOG(INFO, "att_handle=%d\n", att_handle);
+}
+
+static void
+ble_gattc_log_read_uuid(uint16_t start_handle, uint16_t end_handle,
+                        const ble_uuid_t *uuid)
+{
+    ble_gattc_log_proc_init("read by uuid; ");
+    BLE_HS_LOG(INFO, "start_handle=%d end_handle=%d uuid=",
+               start_handle, end_handle);
+    ble_gattc_log_uuid(uuid);
+    BLE_HS_LOG(INFO, "\n");
+}
+
+static void
+ble_gattc_log_read_long(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("read long; ");
+    BLE_HS_LOG(INFO, "att_handle=%d\n", proc->read_long.handle);
+}
+
+static void
+ble_gattc_log_read_mult(const uint16_t *handles, uint8_t num_handles)
+{
+    int i;
+
+    ble_gattc_log_proc_init("read multiple; ");
+    BLE_HS_LOG(INFO, "att_handles=");
+    for (i = 0; i < num_handles; i++) {
+        BLE_HS_LOG(INFO, "%s%d", i != 0 ? "," : "", handles[i]);
+    }
+    BLE_HS_LOG(INFO, "\n");
+}
+
+static void
+ble_gattc_log_write(uint16_t att_handle, uint16_t len, int expecting_rsp)
+{
+    const char *name;
+
+    if (expecting_rsp) {
+        name = "write; ";
+    } else {
+        name = "write no rsp; ";
+    }
+
+    ble_gattc_log_proc_init(name);
+    BLE_HS_LOG(INFO, "att_handle=%d len=%d\n", att_handle, len);
+}
+
+static void
+ble_gattc_log_write_long(struct ble_gattc_proc *proc)
+{
+    ble_gattc_log_proc_init("write long; ");
+    BLE_HS_LOG(INFO, "att_handle=%d len=%d\n",
+               proc->write_long.attr.handle,
+               OS_MBUF_PKTLEN(proc->write_long.attr.om));
+}
+
+static void
+ble_gattc_log_write_reliable(struct ble_gattc_proc *proc)
+{
+    int i;
+
+    ble_gattc_log_proc_init("write reliable; ");
+    BLE_HS_LOG(INFO, "att_handles=");
+    for (i = 0; i < proc->write_reliable.num_attrs; i++) {
+        BLE_HS_LOG(INFO, "%s%d", i != 0 ? "," : "",
+                   proc->write_reliable.attrs[i].handle);
+    }
+    BLE_HS_LOG(INFO, "\n");
+}
+
+static void
+ble_gattc_log_notify(uint16_t att_handle)
+{
+    ble_gattc_log_proc_init("notify; ");
+    BLE_HS_LOG(INFO, "att_handle=%d\n", att_handle);
+}
+
+static void
+ble_gattc_log_indicate(uint16_t att_handle)
+{
+    ble_gattc_log_proc_init("indicate; ");
+    BLE_HS_LOG(INFO, "att_handle=%d\n", att_handle);
+}
+
+/*****************************************************************************
+ * $rx entry                                                                 *
+ *****************************************************************************/
+
+static const void *
+ble_gattc_rx_entry_find(uint8_t op, const void *rx_entries, int num_entries)
+{
+    struct gen_entry {
+        uint8_t op;
+        void (*cb)(void);
+    };
+
+    const struct gen_entry *entries;
+    int i;
+
+    entries = rx_entries;
+    for (i = 0; i < num_entries; i++) {
+        if (entries[i].op == op) {
+            return entries + i;
+        }
+    }
+
+    return NULL;
+}
+
+/*****************************************************************************
+ * $proc                                                                    *
+ *****************************************************************************/
+
+/**
+ * Allocates a proc entry.
+ *
+ * @return                      An entry on success; null on failure.
+ */
+static struct ble_gattc_proc *
+ble_gattc_proc_alloc(void)
+{
+    struct ble_gattc_proc *proc;
+
+    proc = os_memblock_get(&ble_gattc_proc_pool);
+    if (proc != NULL) {
+        memset(proc, 0, sizeof *proc);
+    }
+
+    return proc;
+}
+
+/**
+ * Frees the specified proc entry.  No-op if passed a null pointer.
+ */
+static void
+ble_gattc_proc_free(struct ble_gattc_proc *proc)
+{
+    int rc;
+    int i;
+
+    if (proc != NULL) {
+        ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+        switch (proc->op) {
+        case BLE_GATT_OP_WRITE_LONG:
+            os_mbuf_free_chain(proc->write_long.attr.om);
+            break;
+
+        case BLE_GATT_OP_WRITE_RELIABLE:
+            for (i = 0; i < proc->write_reliable.num_attrs; i++) {
+                os_mbuf_free_chain(proc->write_reliable.attrs[i].om);
+            }
+            break;
+
+        default:
+            break;
+        }
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        memset(proc, 0xff, sizeof *proc);
+#endif
+        rc = os_memblock_put(&ble_gattc_proc_pool, proc);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static void
+ble_gattc_proc_insert(struct ble_gattc_proc *proc)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_hs_lock();
+    STAILQ_INSERT_TAIL(&ble_gattc_procs, proc, next);
+    ble_hs_unlock();
+}
+
+static void
+ble_gattc_proc_set_exp_timer(struct ble_gattc_proc *proc)
+{
+    proc->exp_os_ticks = ble_npl_time_get() +
+                         ble_npl_time_ms_to_ticks32(BLE_GATTC_UNRESPONSIVE_TIMEOUT_MS);
+}
+
+static void
+ble_gattc_proc_set_resume_timer(struct ble_gattc_proc *proc)
+{
+    proc->flags |= BLE_GATTC_PROC_F_STALLED;
+
+    /* Don't overwrite resume time if it is already set; piggyback on it
+     * instead.
+     */
+    if (ble_gattc_resume_at == 0) {
+        ble_gattc_resume_at = ble_npl_time_get() +
+                              ble_npl_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE));
+
+        /* A value of 0 indicates the timer is unset.  Disambiguate this. */
+        if (ble_gattc_resume_at == 0) {
+            ble_gattc_resume_at++;
+        }
+    }
+}
+
+static void
+ble_gattc_process_status(struct ble_gattc_proc *proc, int status)
+{
+    switch (status) {
+    case 0:
+        if (!(proc->flags & BLE_GATTC_PROC_F_STALLED)) {
+            ble_gattc_proc_set_exp_timer(proc);
+        }
+
+        ble_gattc_proc_insert(proc);
+        ble_hs_timer_resched();
+        break;
+
+    default:
+        ble_gattc_proc_free(proc);
+        break;
+    }
+}
+
+/**
+ * Processes the return code that results from an attempt to resume a
+ * procedure.  If the resume attempt failed due to memory exhaustion at a lower
+ * layer, the procedure is marked as stalled but still in progress.  Otherwise,
+ * the resume error code is unmodified.
+ */
+static int
+ble_gattc_process_resume_status(struct ble_gattc_proc *proc, int status)
+{
+    switch (status) {
+    case 0:
+        return 0;
+
+    case BLE_HS_ENOMEM:
+        ble_gattc_proc_set_resume_timer(proc);
+        return 0;
+
+    default:
+        return status;
+    }
+}
+
+/*****************************************************************************
+ * $util                                                                     *
+ *****************************************************************************/
+
+/**
+ * Retrieves the error dispatch entry with the specified op code.
+ */
+static ble_gattc_err_fn *
+ble_gattc_err_dispatch_get(uint8_t op)
+{
+    BLE_HS_DBG_ASSERT(op < BLE_GATT_OP_CNT);
+    return ble_gattc_err_dispatch[op];
+}
+
+/**
+ * Retrieves the error dispatch entry with the specified op code.
+ */
+static ble_gattc_resume_fn *
+ble_gattc_resume_dispatch_get(uint8_t op)
+{
+    BLE_HS_DBG_ASSERT(op < BLE_GATT_OP_CNT);
+    return ble_gattc_resume_dispatch[op];
+}
+
+static ble_gattc_tmo_fn *
+ble_gattc_tmo_dispatch_get(uint8_t op)
+{
+    BLE_HS_DBG_ASSERT(op < BLE_GATT_OP_CNT);
+    return ble_gattc_tmo_dispatch[op];
+}
+
+typedef int ble_gattc_match_fn(struct ble_gattc_proc *proc, void *arg);
+
+struct ble_gattc_criteria_conn_op {
+    uint16_t conn_handle;
+    uint8_t op;
+};
+
+/**
+ * Tests if a proc entry fits the specified criteria.
+ *
+ * @param proc                  The procedure to test.
+ * @param conn_handle           The connection handle to match against.
+ * @param op                    The op code to match against, or
+ *                                  BLE_GATT_OP_NONE to ignore this criterion.
+ *
+ * @return                      1 if the proc matches; 0 otherwise.
+ */
+static int
+ble_gattc_proc_matches_conn_op(struct ble_gattc_proc *proc, void *arg)
+{
+    const struct ble_gattc_criteria_conn_op *criteria;
+
+    criteria = arg;
+
+    if (criteria->conn_handle != proc->conn_handle) {
+        return 0;
+    }
+
+    if (criteria->op != proc->op && criteria->op != BLE_GATT_OP_NONE) {
+        return 0;
+    }
+
+    return 1;
+}
+
+struct ble_gattc_criteria_exp {
+    ble_npl_time_t now;
+    int32_t next_exp_in;
+};
+
+static int
+ble_gattc_proc_matches_expired(struct ble_gattc_proc *proc, void *arg)
+{
+    struct ble_gattc_criteria_exp *criteria;
+    int32_t time_diff;
+
+    criteria = arg;
+
+    time_diff = proc->exp_os_ticks - criteria->now;
+
+    if (time_diff <= 0) {
+        /* Procedure is expired. */
+        return 1;
+    }
+
+    /* Procedure isn't expired; determine if it is the next to expire. */
+    if (time_diff < criteria->next_exp_in) {
+        criteria->next_exp_in = time_diff;
+    }
+    return 0;
+}
+
+struct ble_gattc_criteria_conn_rx_entry {
+    uint16_t conn_handle;
+    const void *rx_entries;
+    int num_rx_entries;
+    const void *matching_rx_entry;
+};
+
+static int
+ble_gattc_proc_matches_conn_rx_entry(struct ble_gattc_proc *proc, void *arg)
+{
+    struct ble_gattc_criteria_conn_rx_entry *criteria;
+
+    criteria = arg;
+
+    if (criteria->conn_handle != BLE_HS_CONN_HANDLE_NONE &&
+        criteria->conn_handle != proc->conn_handle) {
+
+        return 0;
+    }
+
+    /* Entry matches; indicate corresponding rx entry. */
+    criteria->matching_rx_entry = ble_gattc_rx_entry_find(
+        proc->op, criteria->rx_entries, criteria->num_rx_entries);
+
+    return (criteria->matching_rx_entry != NULL);
+}
+
+static void
+ble_gattc_extract(ble_gattc_match_fn *cb, void *arg, int max_procs,
+                  struct ble_gattc_proc_list *dst_list)
+{
+    struct ble_gattc_proc *proc;
+    struct ble_gattc_proc *prev;
+    struct ble_gattc_proc *next;
+    int num_extracted;
+
+    /* Only the parent task is allowed to remove entries from the list. */
+    BLE_HS_DBG_ASSERT(ble_hs_is_parent_task());
+
+    STAILQ_INIT(dst_list);
+    num_extracted = 0;
+
+    ble_hs_lock();
+
+    prev = NULL;
+    proc = STAILQ_FIRST(&ble_gattc_procs);
+    while (proc != NULL) {
+        next = STAILQ_NEXT(proc, next);
+
+        if (cb(proc, arg)) {
+            if (prev == NULL) {
+                STAILQ_REMOVE_HEAD(&ble_gattc_procs, next);
+            } else {
+                STAILQ_REMOVE_AFTER(&ble_gattc_procs, prev, next);
+            }
+            STAILQ_INSERT_TAIL(dst_list, proc, next);
+
+            if (max_procs > 0) {
+                num_extracted++;
+                if (num_extracted >= max_procs) {
+                    break;
+                }
+            }
+        } else {
+            prev = proc;
+        }
+
+        proc = next;
+    }
+
+    ble_hs_unlock();
+}
+
+static struct ble_gattc_proc *
+ble_gattc_extract_one(ble_gattc_match_fn *cb, void *arg)
+{
+    struct ble_gattc_proc_list dst_list;
+
+    ble_gattc_extract(cb, arg, 1, &dst_list);
+    return STAILQ_FIRST(&dst_list);
+}
+
+static void
+ble_gattc_extract_by_conn_op(uint16_t conn_handle, uint8_t op, int max_procs,
+                             struct ble_gattc_proc_list *dst_list)
+{
+    struct ble_gattc_criteria_conn_op criteria;
+
+    criteria.conn_handle = conn_handle;
+    criteria.op = op;
+
+    ble_gattc_extract(ble_gattc_proc_matches_conn_op, &criteria, max_procs, dst_list);
+}
+
+static struct ble_gattc_proc *
+ble_gattc_extract_first_by_conn_op(uint16_t conn_handle, uint8_t op)
+{
+    struct ble_gattc_proc_list dst_list;
+
+    ble_gattc_extract_by_conn_op(conn_handle, op, 1, &dst_list);
+    return STAILQ_FIRST(&dst_list);
+}
+
+static int
+ble_gattc_proc_matches_stalled(struct ble_gattc_proc *proc, void *unused)
+{
+    return proc->flags & BLE_GATTC_PROC_F_STALLED;
+}
+
+static void
+ble_gattc_extract_stalled(struct ble_gattc_proc_list *dst_list)
+{
+    ble_gattc_extract(ble_gattc_proc_matches_stalled, NULL, 0, dst_list);
+}
+
+/**
+ * @return                      The number of ticks until the next expiration
+ *                                  occurs.
+ */
+static int32_t
+ble_gattc_extract_expired(struct ble_gattc_proc_list *dst_list)
+{
+    struct ble_gattc_criteria_exp criteria;
+
+    criteria.now = ble_npl_time_get();
+    criteria.next_exp_in = BLE_HS_FOREVER;
+
+    STAILQ_INIT(dst_list);
+    ble_gattc_extract(ble_gattc_proc_matches_expired, &criteria, 0, dst_list);
+
+    return criteria.next_exp_in;
+}
+
+static struct ble_gattc_proc *
+ble_gattc_extract_with_rx_entry(uint16_t conn_handle,
+                                const void *rx_entries, int num_rx_entries,
+                                const void **out_rx_entry)
+{
+    struct ble_gattc_criteria_conn_rx_entry criteria;
+    struct ble_gattc_proc *proc;
+
+    criteria.conn_handle = conn_handle;
+    criteria.rx_entries = rx_entries;
+    criteria.num_rx_entries = num_rx_entries;
+    criteria.matching_rx_entry = NULL;
+
+    proc = ble_gattc_extract_one(ble_gattc_proc_matches_conn_rx_entry,
+                                 &criteria);
+    *out_rx_entry = criteria.matching_rx_entry;
+
+    return proc;
+}
+
+/**
+ * Searches the main proc list for an entry whose connection handle and op code
+ * match those specified.  If a matching entry is found, it is removed from the
+ * list and returned.
+ *
+ * @param conn_handle           The connection handle to match against.
+ * @param rx_entries            The array of rx entries corresponding to the
+ *                                  op code of the incoming response.
+ * @param out_rx_entry          On success, the address of the matching rx
+ *                                  entry is written to this pointer.
+ *
+ * @return                      The matching proc entry on success;
+ *                                  null on failure.
+ */
+#define BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle, rx_entries, out_rx_entry)  \
+    ble_gattc_extract_with_rx_entry(                                          \
+        (conn_handle), (rx_entries),                                          \
+        sizeof (rx_entries) / sizeof (rx_entries)[0],                         \
+        (const void **)(out_rx_entry))
+
+
+/**
+ * Causes all GATT procedures matching the specified criteria to fail with the
+ * specified status code.
+ */
+static void
+ble_gattc_fail_procs(uint16_t conn_handle, uint8_t op, int status)
+{
+    struct ble_gattc_proc_list temp_list;
+    struct ble_gattc_proc *proc;
+    ble_gattc_err_fn *err_cb;
+
+    /* Remove all procs with the specified conn handle-op-pair and insert them
+     * into the temporary list.
+     */
+    ble_gattc_extract_by_conn_op(conn_handle, op, 0, &temp_list);
+
+    /* Notify application of failed procedures and free the corresponding proc
+     * entries.
+     */
+    while ((proc = STAILQ_FIRST(&temp_list)) != NULL) {
+        err_cb = ble_gattc_err_dispatch_get(proc->op);
+        err_cb(proc, status, 0);
+
+        STAILQ_REMOVE_HEAD(&temp_list, next);
+        ble_gattc_proc_free(proc);
+    }
+}
+
+static void
+ble_gattc_resume_procs(void)
+{
+    struct ble_gattc_proc_list stall_list;
+    struct ble_gattc_proc *proc;
+    ble_gattc_resume_fn *resume_cb;
+    int rc;
+
+    /* Cancel resume timer since it is being serviced. */
+    ble_gattc_resume_at = 0;
+
+    ble_gattc_extract_stalled(&stall_list);
+
+    STAILQ_FOREACH(proc, &stall_list, next) {
+        resume_cb = ble_gattc_resume_dispatch_get(proc->op);
+        BLE_HS_DBG_ASSERT(resume_cb != NULL);
+
+        proc->flags &= ~BLE_GATTC_PROC_F_STALLED;
+        rc = resume_cb(proc);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+static int32_t
+ble_gattc_ticks_until_resume(void)
+{
+    ble_npl_time_t now;
+    int32_t diff;
+
+    /* Resume timer not set. */
+    if (ble_gattc_resume_at == 0) {
+        return BLE_HS_FOREVER;
+    }
+
+    now = ble_npl_time_get();
+    diff = ble_gattc_resume_at - now;
+    if (diff <= 0) {
+        /* Timer already expired; resume immediately. */
+        return 0;
+    }
+
+    return diff;
+}
+
+static void
+ble_gattc_proc_timeout(struct ble_gattc_proc *proc)
+{
+    ble_gattc_tmo_fn *cb;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    cb = ble_gattc_tmo_dispatch_get(proc->op);
+    if (cb != NULL) {
+        cb(proc);
+    }
+}
+
+/**
+ * Times out expired GATT client procedures.
+ *
+ * @return                      The number of ticks until this function should
+ *                                  be called again.
+ */
+int32_t
+ble_gattc_timer(void)
+{
+    struct ble_gattc_proc_list exp_list;
+    struct ble_gattc_proc *proc;
+    int32_t ticks_until_resume;
+    int32_t ticks_until_exp;
+
+    /* Remove timed-out procedures from the main list and insert them into a
+     * temporary list.  This function also calculates the number of ticks until
+     * the next expiration will occur.
+     */
+    ticks_until_exp = ble_gattc_extract_expired(&exp_list);
+
+    /* Terminate the connection associated with each timed-out procedure. */
+    while ((proc = STAILQ_FIRST(&exp_list)) != NULL) {
+        STATS_INC(ble_gattc_stats, proc_timeout);
+
+        ble_gattc_proc_timeout(proc);
+
+        ble_gap_terminate(proc->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+
+        STAILQ_REMOVE_HEAD(&exp_list, next);
+        ble_gattc_proc_free(proc);
+    }
+
+    /* If there are stalled procedures, the GATT client will need to wake up to
+     * resume them.
+     */
+    ticks_until_resume = ble_gattc_ticks_until_resume();
+    if (ticks_until_resume == 0) {
+        ble_gattc_resume_procs();
+        ticks_until_resume = ble_gattc_ticks_until_resume();
+    }
+
+    return min(ticks_until_exp, ticks_until_resume);
+}
+
+/**
+ * Returns a pointer to a GATT error object with the specified fields.  The
+ * returned object is statically allocated, so this function is not reentrant.
+ * This function should only ever be called by the ble_hs task.
+ */
+static struct ble_gatt_error *
+ble_gattc_error(int status, uint16_t att_handle)
+{
+    static struct ble_gatt_error error;
+
+    /* For consistency, always indicate a handle of 0 on success. */
+    if (status == 0 || status == BLE_HS_EDONE) {
+        att_handle = 0;
+    }
+
+    error.status = status;
+    error.att_handle = att_handle;
+    return &error;
+}
+
+/*****************************************************************************
+ * $mtu                                                                      *
+ *****************************************************************************/
+
+/**
+ * Calls an mtu-exchange proc's callback with the specified parameters.  If the
+ * proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_mtu_cb(struct ble_gattc_proc *proc, int status, uint16_t att_handle,
+                 uint16_t mtu)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, mtu_fail);
+    }
+
+    if (proc->mtu.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->mtu.cb(proc->conn_handle,
+                          ble_gattc_error(status, att_handle),
+                          mtu, proc->mtu.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_mtu_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_mtu_cb(proc, BLE_HS_ETIMEOUT, 0, 0);
+}
+
+/**
+ * Handles an incoming ATT error response for the specified mtu-exchange proc.
+ */
+static void
+ble_gattc_mtu_err(struct ble_gattc_proc *proc, int status, uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_mtu_cb(proc, status, att_handle, 0);
+}
+
+static int
+ble_gattc_mtu_tx(struct ble_gattc_proc *proc)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    uint16_t mtu;
+    int rc;
+
+    ble_hs_lock();
+    rc = ble_att_conn_chan_find(proc->conn_handle, &conn, &chan);
+    if (rc == 0) {
+        mtu = chan->my_mtu;
+    }
+    ble_hs_unlock();
+
+    if (rc == 0) {
+        rc = ble_att_clt_tx_mtu(proc->conn_handle, mtu);
+    }
+
+    return rc;
+}
+
+int
+ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *cb_arg)
+{
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, mtu);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_MTU;
+    proc->conn_handle = conn_handle;
+    proc->mtu.cb = cb;
+    proc->mtu.cb_arg = cb_arg;
+
+    ble_gattc_log_proc_init("exchange mtu\n");
+
+    rc = ble_gattc_mtu_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, mtu_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $discover all services                                                    *
+ *****************************************************************************/
+
+/**
+ * Calls a discover-all-services proc's callback with the specified parameters.
+ * If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_disc_all_svcs_cb(struct ble_gattc_proc *proc,
+                           uint16_t status, uint16_t att_handle,
+                           struct ble_gatt_svc *service)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(service != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, disc_all_svcs_fail);
+    }
+
+    if (proc->disc_all_svcs.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->disc_all_svcs.cb(proc->conn_handle,
+                                    ble_gattc_error(status, att_handle),
+                                    service, proc->disc_all_svcs.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_disc_all_svcs_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_disc_all_svcs_cb(proc, BLE_HS_ETIMEOUT, 0, 0);
+}
+
+/**
+ * Triggers a pending transmit for the specified discover-all-services proc.
+ */
+static int
+ble_gattc_disc_all_svcs_tx(struct ble_gattc_proc *proc)
+{
+    ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_PRIMARY_SERVICE);
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    rc = ble_att_clt_tx_read_group_type(proc->conn_handle,
+                                        proc->disc_all_svcs.prev_handle + 1,
+                                        0xffff, &uuid.u);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_disc_all_svcs_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_disc_all_svcs_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_disc_all_svcs_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * discover-all-services proc.
+ */
+static void
+ble_gattc_disc_all_svcs_err(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
+        /* Discovery is complete. */
+        status = BLE_HS_EDONE;
+    }
+
+    ble_gattc_disc_all_svcs_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming attribute data entry from a read-group-type response for
+ * the specified discover-all-services proc.
+ */
+static int
+ble_gattc_disc_all_svcs_rx_adata(struct ble_gattc_proc *proc,
+                                 struct ble_att_read_group_type_adata *adata)
+{
+    struct ble_gatt_svc service;
+    int cbrc;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    switch (adata->value_len) {
+    case 2:
+    case 16:
+        rc = ble_uuid_init_from_att_buf(&service.uuid, adata->value,
+                                        adata->value_len);
+        if (rc != 0) {
+            rc = BLE_HS_EBADDATA;
+            goto done;
+        }
+        break;
+
+    default:
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    if (adata->end_group_handle <= proc->disc_all_svcs.prev_handle) {
+        /* Peer sent services out of order; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    proc->disc_all_svcs.prev_handle = adata->end_group_handle;
+
+    service.start_handle = adata->att_handle;
+    service.end_handle = adata->end_group_handle;
+
+    rc = 0;
+
+done:
+    cbrc = ble_gattc_disc_all_svcs_cb(proc, rc, 0, &service);
+    if (rc != 0 || cbrc != 0) {
+        return BLE_HS_EDONE;
+    } else {
+        return 0;
+    }
+}
+
+/**
+ * Handles a notification that an incoming read-group-type response has been
+ * fully processed.
+ */
+static int
+ble_gattc_disc_all_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_disc_all_svcs_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    if (proc->disc_all_svcs.prev_handle == 0xffff) {
+        /* Service discovery complete. */
+        ble_gattc_disc_all_svcs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_all_svcs_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
+}
+
+int
+ble_gattc_disc_all_svcs(uint16_t conn_handle, ble_gatt_disc_svc_fn *cb,
+                        void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_DISC_ALL_SVCS)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, disc_all_svcs);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_DISC_ALL_SVCS;
+    proc->conn_handle = conn_handle;
+    proc->disc_all_svcs.prev_handle = 0x0000;
+    proc->disc_all_svcs.cb = cb;
+    proc->disc_all_svcs.cb_arg = cb_arg;
+
+    ble_gattc_log_proc_init("discover all services\n");
+
+    rc = ble_gattc_disc_all_svcs_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, disc_all_svcs_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $discover service by uuid                                                 *
+ *****************************************************************************/
+
+/**
+ * Calls a discover-service-by-uuid proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_disc_svc_uuid_cb(struct ble_gattc_proc *proc, int status,
+                           uint16_t att_handle,
+                           struct ble_gatt_svc *service)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(service != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, disc_svc_uuid_fail);
+    }
+
+    if (proc->disc_svc_uuid.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->disc_svc_uuid.cb(proc->conn_handle,
+                                    ble_gattc_error(status, att_handle),
+                                    service, proc->disc_svc_uuid.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_disc_svc_uuid_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_disc_svc_uuid_cb(proc, BLE_HS_ETIMEOUT, 0, 0);
+}
+
+/**
+ * Triggers a pending transmit for the specified discover-service-by-uuid proc.
+ */
+static int
+ble_gattc_disc_svc_uuid_tx(struct ble_gattc_proc *proc)
+{
+    uint8_t val[16];
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_uuid_flat(&proc->disc_svc_uuid.service_uuid.u, val);
+    rc = ble_att_clt_tx_find_type_value(proc->conn_handle,
+                                        proc->disc_svc_uuid.prev_handle + 1,
+                                        0xffff, BLE_ATT_UUID_PRIMARY_SERVICE,
+                                        val,
+                                        ble_uuid_length(&proc->disc_svc_uuid.service_uuid.u));
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_disc_svc_uuid_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_disc_svc_uuid_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_disc_svc_uuid_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * discover-service-by-uuid proc.
+ */
+static void
+ble_gattc_disc_svc_uuid_err(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
+        /* Discovery is complete. */
+        status = BLE_HS_EDONE;
+    }
+
+    ble_gattc_disc_svc_uuid_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming "handles info" entry from a find-type-value response for
+ * the specified discover-service-by-uuid proc.
+ */
+static int
+ble_gattc_disc_svc_uuid_rx_hinfo(struct ble_gattc_proc *proc,
+                                 struct ble_att_find_type_value_hinfo *hinfo)
+{
+    struct ble_gatt_svc service;
+    int cbrc;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (hinfo->group_end_handle <= proc->disc_svc_uuid.prev_handle) {
+        /* Peer sent services out of order; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    proc->disc_svc_uuid.prev_handle = hinfo->group_end_handle;
+
+    service.start_handle = hinfo->attr_handle;
+    service.end_handle = hinfo->group_end_handle;
+    service.uuid = proc->disc_svc_uuid.service_uuid;
+
+    rc = 0;
+
+done:
+    cbrc = ble_gattc_disc_svc_uuid_cb(proc, rc, 0, &service);
+    if (rc != 0 || cbrc != 0) {
+        return BLE_HS_EDONE;
+    } else {
+        return 0;
+    }
+}
+
+/**
+ * Handles a notification that a find-type-value response has been fully
+ * processed for the specified discover-service-by-uuid proc.
+ */
+static int
+ble_gattc_disc_svc_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_disc_svc_uuid_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    if (proc->disc_svc_uuid.prev_handle == 0xffff) {
+        /* Service discovery complete. */
+        ble_gattc_disc_svc_uuid_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_svc_uuid_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
+}
+
+int
+ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, const ble_uuid_t *uuid,
+                           ble_gatt_disc_svc_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_DISC_SVC_UUID)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, disc_svc_uuid);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_DISC_SVC_UUID;
+    proc->conn_handle = conn_handle;
+    ble_uuid_to_any(uuid, &proc->disc_svc_uuid.service_uuid);
+    proc->disc_svc_uuid.prev_handle = 0x0000;
+    proc->disc_svc_uuid.cb = cb;
+    proc->disc_svc_uuid.cb_arg = cb_arg;
+
+    ble_gattc_log_disc_svc_uuid(proc);
+
+    rc = ble_gattc_disc_svc_uuid_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, disc_svc_uuid_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $find included svcs                                                       *
+ *****************************************************************************/
+
+/**
+ * Calls a find-included-services proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_find_inc_svcs_cb(struct ble_gattc_proc *proc, int status,
+                           uint16_t att_handle,
+                           struct ble_gatt_svc *service)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(service != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, find_inc_svcs_fail);
+    }
+
+    if (proc->find_inc_svcs.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->find_inc_svcs.cb(proc->conn_handle,
+                                    ble_gattc_error(status, att_handle),
+                                    service, proc->find_inc_svcs.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_find_inc_svcs_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_find_inc_svcs_cb(proc, BLE_HS_ETIMEOUT, 0, 0);
+}
+
+/**
+ * Triggers a pending transmit for the specified find-included-services proc.
+ */
+static int
+ble_gattc_find_inc_svcs_tx(struct ble_gattc_proc *proc)
+{
+    ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_INCLUDE);
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (proc->find_inc_svcs.cur_start == 0) {
+        /* Find the next included service. */
+        rc = ble_att_clt_tx_read_type(proc->conn_handle,
+                                      proc->find_inc_svcs.prev_handle + 1,
+                                      proc->find_inc_svcs.end_handle, &uuid.u);
+        if (rc != 0) {
+            return rc;
+        }
+    } else {
+        /* Read the UUID of the previously found service. */
+        rc = ble_att_clt_tx_read(proc->conn_handle,
+                                 proc->find_inc_svcs.cur_start);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_find_inc_svcs_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_find_inc_svcs_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_find_inc_svcs_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * find-included-services proc.
+ */
+static void
+ble_gattc_find_inc_svcs_err(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (proc->find_inc_svcs.cur_start == 0 &&
+        status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
+
+        /* Discovery is complete. */
+        status = BLE_HS_EDONE;
+    }
+
+    ble_gattc_find_inc_svcs_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming read-response for the specified find-included-services
+ * proc.
+ */
+static int
+ble_gattc_find_inc_svcs_rx_read_rsp(struct ble_gattc_proc *proc, int status,
+                                    struct os_mbuf **om)
+{
+    struct ble_gatt_svc service;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    rc = ble_uuid_init_from_att_mbuf(&service.uuid, *om, 0, 16);
+    os_mbuf_free_chain(*om);
+    *om = NULL;
+
+    if (rc != 0) {
+        /* Invalid UUID. */
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
+    if (proc->find_inc_svcs.cur_start == 0) {
+        /* Unexpected read response; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
+    if (status != 0) {
+        rc = status;
+        goto err;
+    }
+
+    /* Report discovered service to application. */
+    service.start_handle = proc->find_inc_svcs.cur_start;
+    service.end_handle = proc->find_inc_svcs.cur_end;
+    rc = ble_gattc_find_inc_svcs_cb(proc, 0, 0, &service);
+    if (rc != 0) {
+        /* Application has indicated that the procedure should be aborted. */
+        return BLE_HS_EDONE;
+    }
+
+    /* Proceed to the next service. */
+    proc->find_inc_svcs.cur_start = 0;
+    proc->find_inc_svcs.cur_end = 0;
+    rc = ble_gattc_find_inc_svcs_resume(proc);
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    ble_gattc_find_inc_svcs_cb(proc, rc, 0, NULL);
+    return BLE_HS_EDONE;
+}
+
+/**
+ * Handles an incoming "attribute data" entry from a read-by-type response for
+ * the specified find-included-services proc.
+ */
+static int
+ble_gattc_find_inc_svcs_rx_adata(struct ble_gattc_proc *proc,
+                                 struct ble_att_read_type_adata *adata)
+{
+    struct ble_gatt_svc service;
+    int call_cb;
+    int cbrc;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (proc->find_inc_svcs.cur_start != 0) {
+        /* We only read one 128-bit UUID service at a time.  Ignore the
+         * additional services in the response.
+         */
+        return 0;
+    }
+
+    call_cb = 1;
+
+    if (adata->att_handle <= proc->find_inc_svcs.prev_handle) {
+        /* Peer sent services out of order; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    proc->find_inc_svcs.prev_handle = adata->att_handle;
+
+    rc = 0;
+
+    switch (adata->value_len) {
+    case BLE_GATTS_INC_SVC_LEN_NO_UUID:
+        proc->find_inc_svcs.cur_start = get_le16(adata->value + 0);
+        proc->find_inc_svcs.cur_end = get_le16(adata->value + 2);
+        call_cb = 0;
+        break;
+
+    case BLE_GATTS_INC_SVC_LEN_UUID:
+        service.start_handle = get_le16(adata->value + 0);
+        service.end_handle = get_le16(adata->value + 2);
+        rc = ble_uuid_init_from_att_buf(&service.uuid, adata->value + 4, 2);
+        if (rc != 0) {
+            rc = BLE_HS_EBADDATA;
+        }
+        break;
+
+    default:
+        rc = BLE_HS_EBADDATA;
+        break;
+    }
+
+done:
+    if (call_cb) {
+        cbrc = ble_gattc_find_inc_svcs_cb(proc, 0, 0, &service);
+        if (rc != 0) {
+            rc = cbrc;
+        }
+    } else {
+        cbrc = 0;
+    }
+
+    if (rc != 0 || cbrc != 0) {
+        return BLE_HS_EDONE;
+    } else {
+        return 0;
+    }
+}
+
+/**
+ * Handles a notification that a read-by-type response has been fully
+ * processed for the specified find-included-services proc.
+ */
+static int
+ble_gattc_find_inc_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_find_inc_svcs_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    if (proc->find_inc_svcs.prev_handle == 0xffff) {
+        /* Procedure complete. */
+        ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_find_inc_svcs_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+    return 0;
+}
+
+int
+ble_gattc_find_inc_svcs(uint16_t conn_handle, uint16_t start_handle,
+                        uint16_t end_handle,
+                        ble_gatt_disc_svc_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_FIND_INC_SVCS)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, find_inc_svcs);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_FIND_INC_SVCS;
+    proc->conn_handle = conn_handle;
+    proc->find_inc_svcs.prev_handle = start_handle - 1;
+    proc->find_inc_svcs.end_handle = end_handle;
+    proc->find_inc_svcs.cb = cb;
+    proc->find_inc_svcs.cb_arg = cb_arg;
+
+    ble_gattc_log_find_inc_svcs(proc);
+
+    rc = ble_gattc_find_inc_svcs_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, find_inc_svcs_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $discover all characteristics                                             *
+ *****************************************************************************/
+
+/**
+ * Calls a discover-all-characteristics proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_disc_all_chrs_cb(struct ble_gattc_proc *proc, int status,
+                           uint16_t att_handle, struct ble_gatt_chr *chr)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(chr != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, disc_all_chrs_fail);
+    }
+
+    if (proc->disc_all_chrs.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->disc_all_chrs.cb(proc->conn_handle,
+                                    ble_gattc_error(status, att_handle), chr,
+                                    proc->disc_all_chrs.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_disc_all_chrs_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_disc_all_chrs_cb(proc, BLE_HS_ETIMEOUT, 0, NULL);
+}
+
+/**
+ * Triggers a pending transmit for the specified discover-all-characteristics
+ * proc.
+ */
+static int
+ble_gattc_disc_all_chrs_tx(struct ble_gattc_proc *proc)
+{
+    ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    rc = ble_att_clt_tx_read_type(proc->conn_handle,
+                                  proc->disc_all_chrs.prev_handle + 1,
+                                  proc->disc_all_chrs.end_handle, &uuid.u);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_disc_all_chrs_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_disc_all_chrs_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_disc_all_chrs_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * discover-all-characteristics proc.
+ */
+static void
+ble_gattc_disc_all_chrs_err(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
+        /* Discovery is complete. */
+        status = BLE_HS_EDONE;
+    }
+
+    ble_gattc_disc_all_chrs_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming "attribute data" entry from a read-by-type response for
+ * the specified discover-all-characteristics proc.
+ */
+static int
+ble_gattc_disc_all_chrs_rx_adata(struct ble_gattc_proc *proc,
+                                 struct ble_att_read_type_adata *adata)
+{
+    struct ble_gatt_chr chr;
+    int cbrc;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    memset(&chr, 0, sizeof chr);
+    chr.def_handle = adata->att_handle;
+
+    switch (adata->value_len) {
+    case BLE_GATT_CHR_DECL_SZ_16:
+    case BLE_GATT_CHR_DECL_SZ_128:
+        rc = ble_uuid_init_from_att_buf(&chr.uuid, adata->value + 3,
+                                        adata->value_len - 3);
+        if (rc != 0) {
+            rc = BLE_HS_EBADDATA;
+            goto done;
+        }
+        break;
+
+    default:
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    chr.properties = adata->value[0];
+    chr.val_handle = get_le16(adata->value + 1);
+
+    if (adata->att_handle <= proc->disc_all_chrs.prev_handle) {
+        /* Peer sent characteristics out of order; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+    proc->disc_all_chrs.prev_handle = adata->att_handle;
+
+    rc = 0;
+
+done:
+    cbrc = ble_gattc_disc_all_chrs_cb(proc, rc, 0, &chr);
+    if (rc != 0 || cbrc != 0) {
+        return BLE_HS_EDONE;
+    } else {
+        return 0;
+    }
+}
+
+/**
+ * Handles a notification that a read-by-type response has been fully
+ * processed for the specified discover-all-characteristics proc.
+ */
+static int
+ble_gattc_disc_all_chrs_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_disc_all_chrs_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    if (proc->disc_all_chrs.prev_handle == proc->disc_all_chrs.end_handle) {
+        /* Characteristic discovery complete. */
+        ble_gattc_disc_all_chrs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_all_chrs_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+    return 0;
+}
+
+int
+ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle,
+                        uint16_t end_handle, ble_gatt_chr_fn *cb,
+                        void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_DISC_ALL_CHRS)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, disc_all_chrs);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_DISC_ALL_CHRS;
+    proc->conn_handle = conn_handle;
+    proc->disc_all_chrs.prev_handle = start_handle - 1;
+    proc->disc_all_chrs.end_handle = end_handle;
+    proc->disc_all_chrs.cb = cb;
+    proc->disc_all_chrs.cb_arg = cb_arg;
+
+    ble_gattc_log_disc_all_chrs(proc);
+
+    rc = ble_gattc_disc_all_chrs_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, disc_all_chrs_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $discover characteristic by uuid                                          *
+ *****************************************************************************/
+
+/**
+ * Calls a discover-characteristic-by-uuid proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_disc_chr_uuid_cb(struct ble_gattc_proc *proc, int status,
+                           uint16_t att_handle, struct ble_gatt_chr *chr)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(chr != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, disc_chrs_uuid_fail);
+    }
+
+    if (proc->disc_chr_uuid.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->disc_chr_uuid.cb(proc->conn_handle,
+                                    ble_gattc_error(status, att_handle), chr,
+                                    proc->disc_chr_uuid.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_disc_chr_uuid_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_disc_chr_uuid_cb(proc, BLE_HS_ETIMEOUT, 0, NULL);
+}
+
+/**
+ * Triggers a pending transmit for the specified
+ * discover-characteristic-by-uuid proc.
+ */
+static int
+ble_gattc_disc_chr_uuid_tx(struct ble_gattc_proc *proc)
+{
+    ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    rc = ble_att_clt_tx_read_type(proc->conn_handle,
+                                  proc->disc_chr_uuid.prev_handle + 1,
+                                  proc->disc_chr_uuid.end_handle, &uuid.u);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_disc_chr_uuid_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_disc_chr_uuid_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_disc_chr_uuid_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * discover-characteristic-by-uuid proc.
+ */
+static void
+ble_gattc_disc_chr_uuid_err(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
+        /* Discovery is complete. */
+        status = BLE_HS_EDONE;
+    }
+
+    ble_gattc_disc_chr_uuid_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming "attribute data" entry from a read-by-type response for
+ * the specified discover-characteristics-by-uuid proc.
+ */
+static int
+ble_gattc_disc_chr_uuid_rx_adata(struct ble_gattc_proc *proc,
+                                 struct ble_att_read_type_adata *adata)
+{
+    struct ble_gatt_chr chr;
+    int cbrc;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    memset(&chr, 0, sizeof chr);
+    chr.def_handle = adata->att_handle;
+
+    switch (adata->value_len) {
+    case BLE_GATT_CHR_DECL_SZ_16:
+    case BLE_GATT_CHR_DECL_SZ_128:
+        rc = ble_uuid_init_from_att_buf(&chr.uuid, adata->value + 3,
+                                        adata->value_len - 3);
+        if (rc != 0) {
+            rc = BLE_HS_EBADDATA;
+            goto done;
+        }
+        break;
+
+    default:
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    chr.properties = adata->value[0];
+    chr.val_handle = get_le16(adata->value + 1);
+
+    if (adata->att_handle <= proc->disc_chr_uuid.prev_handle) {
+        /* Peer sent characteristics out of order; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+
+    proc->disc_chr_uuid.prev_handle = adata->att_handle;
+
+    rc = 0;
+
+done:
+    if (rc != 0) {
+        /* Failure. */
+        cbrc = ble_gattc_disc_chr_uuid_cb(proc, rc, 0, NULL);
+    } else if (ble_uuid_cmp(&chr.uuid.u, &proc->disc_chr_uuid.chr_uuid.u) == 0) {
+        /* Requested characteristic discovered. */
+        cbrc = ble_gattc_disc_chr_uuid_cb(proc, 0, 0, &chr);
+    } else {
+        /* Uninteresting characteristic; ignore. */
+        cbrc = 0;
+    }
+
+    if (rc != 0 || cbrc != 0) {
+        return BLE_HS_EDONE;
+    } else {
+        return 0;
+    }
+}
+
+/**
+ * Handles a notification that a read-by-type response has been fully
+ * processed for the specified discover-characteristics-by-uuid proc.
+ */
+static int
+ble_gattc_disc_chr_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_disc_chr_uuid_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    if (proc->disc_chr_uuid.prev_handle == proc->disc_chr_uuid.end_handle) {
+        /* Characteristic discovery complete. */
+        ble_gattc_disc_chr_uuid_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_chr_uuid_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+    return 0;
+}
+
+int
+ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle,
+                            uint16_t end_handle, const ble_uuid_t *uuid,
+                            ble_gatt_chr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_DISC_CHR_UUID)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, disc_chrs_uuid);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_DISC_CHR_UUID;
+    proc->conn_handle = conn_handle;
+    ble_uuid_to_any(uuid, &proc->disc_chr_uuid.chr_uuid);
+    proc->disc_chr_uuid.prev_handle = start_handle - 1;
+    proc->disc_chr_uuid.end_handle = end_handle;
+    proc->disc_chr_uuid.cb = cb;
+    proc->disc_chr_uuid.cb_arg = cb_arg;
+
+    ble_gattc_log_disc_chr_uuid(proc);
+
+    rc = ble_gattc_disc_chr_uuid_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, disc_chrs_uuid_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $discover all characteristic descriptors                                  *
+ *****************************************************************************/
+
+/**
+ * Calls a discover-all-descriptors proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_disc_all_dscs_cb(struct ble_gattc_proc *proc, int status,
+                           uint16_t att_handle, struct ble_gatt_dsc *dsc)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(dsc != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, disc_all_dscs_fail);
+    }
+
+    if (proc->disc_all_dscs.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->disc_all_dscs.cb(proc->conn_handle,
+                                    ble_gattc_error(status, att_handle),
+                                    proc->disc_all_dscs.chr_val_handle,
+                                    dsc, proc->disc_all_dscs.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_disc_all_dscs_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_disc_all_dscs_cb(proc, BLE_HS_ETIMEOUT, 0, NULL);
+}
+
+/**
+ * Triggers a pending transmit for the specified discover-all-descriptors proc.
+ */
+static int
+ble_gattc_disc_all_dscs_tx(struct ble_gattc_proc *proc)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    rc = ble_att_clt_tx_find_info(proc->conn_handle,
+                                  proc->disc_all_dscs.prev_handle + 1,
+                                  proc->disc_all_dscs.end_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_disc_all_dscs_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_disc_all_dscs_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_disc_all_dscs_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * discover-all-descriptors proc.
+ */
+static void
+ble_gattc_disc_all_dscs_err(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
+        /* Discovery is complete. */
+        status = BLE_HS_EDONE;
+    }
+
+    ble_gattc_disc_all_dscs_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming "information data" entry from a find-information
+ * response for the specified discover-all-descriptors proc.
+ */
+static int
+ble_gattc_disc_all_dscs_rx_idata(struct ble_gattc_proc *proc,
+                                 struct ble_att_find_info_idata *idata)
+{
+    struct ble_gatt_dsc dsc;
+    int cbrc;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (idata->attr_handle <= proc->disc_all_dscs.prev_handle) {
+        /* Peer sent descriptors out of order; terminate procedure. */
+        rc = BLE_HS_EBADDATA;
+        goto done;
+    }
+    proc->disc_all_dscs.prev_handle = idata->attr_handle;
+
+    rc = 0;
+
+done:
+    dsc.handle = idata->attr_handle;
+    dsc.uuid = idata->uuid;
+
+    cbrc = ble_gattc_disc_all_dscs_cb(proc, rc, 0, &dsc);
+    if (rc != 0 || cbrc != 0) {
+        return BLE_HS_EDONE;
+    } else {
+        return 0;
+    }
+}
+
+/**
+ * Handles a notification that a find-information response has been fully
+ * processed for the specified discover-all-descriptors proc.
+ */
+static int
+ble_gattc_disc_all_dscs_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_disc_all_dscs_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    if (proc->disc_all_dscs.prev_handle == proc->disc_all_dscs.end_handle) {
+        /* All descriptors discovered. */
+        ble_gattc_disc_all_dscs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_all_dscs_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
+}
+
+int
+ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t start_handle,
+                        uint16_t end_handle,
+                        ble_gatt_dsc_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_DISC_ALL_DSCS)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, disc_all_dscs);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_DISC_ALL_DSCS;
+    proc->conn_handle = conn_handle;
+    proc->disc_all_dscs.chr_val_handle = start_handle;
+    proc->disc_all_dscs.prev_handle = start_handle;
+    proc->disc_all_dscs.end_handle = end_handle;
+    proc->disc_all_dscs.cb = cb;
+    proc->disc_all_dscs.cb_arg = cb_arg;
+
+    ble_gattc_log_disc_all_dscs(proc);
+
+    rc = ble_gattc_disc_all_dscs_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, disc_all_dscs_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $read                                                                     *
+ *****************************************************************************/
+
+/**
+ * Calls a read-characteristic proc's callback with the specified parameters.
+ * If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_read_cb(struct ble_gattc_proc *proc, int status,
+                  uint16_t att_handle, struct ble_gatt_attr *attr)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(attr != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, read_fail);
+    }
+
+    if (proc->read.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->read.cb(proc->conn_handle,
+                           ble_gattc_error(status, att_handle), attr,
+                           proc->read.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_read_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_read_cb(proc, BLE_HS_ETIMEOUT, 0, NULL);
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * read-characteristic-value proc.
+ */
+static void
+ble_gattc_read_err(struct ble_gattc_proc *proc, int status,
+                   uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_read_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming read-response for the specified
+ * read-characteristic-value proc.
+ */
+static int
+ble_gattc_read_rx_read_rsp(struct ble_gattc_proc *proc, int status,
+                           struct os_mbuf **om)
+{
+    struct ble_gatt_attr attr;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    attr.handle = proc->read.handle;
+    attr.offset = 0;
+    attr.om = *om;
+
+    ble_gattc_read_cb(proc, status, 0, &attr);
+
+    /* Indicate to the caller whether the application consumed the mbuf. */
+    *om = attr.om;
+
+    /* The read operation only has a single request / response exchange. */
+    return BLE_HS_EDONE;
+}
+
+static int
+ble_gattc_read_tx(struct ble_gattc_proc *proc)
+{
+    int rc;
+
+    rc = ble_att_clt_tx_read(proc->conn_handle, proc->read.handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_gattc_read(uint16_t conn_handle, uint16_t attr_handle,
+               ble_gatt_attr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_READ)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, read);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_READ;
+    proc->conn_handle = conn_handle;
+    proc->read.handle = attr_handle;
+    proc->read.cb = cb;
+    proc->read.cb_arg = cb_arg;
+
+    ble_gattc_log_read(attr_handle);
+    rc = ble_gattc_read_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, read_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $read by uuid                                                             *
+ *****************************************************************************/
+
+/**
+ * Calls a read-using-characteristic-uuid proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_read_uuid_cb(struct ble_gattc_proc *proc, int status,
+                       uint16_t att_handle, struct ble_gatt_attr *attr)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(attr != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, read_uuid_fail);
+    }
+
+    if (proc->read_uuid.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->read_uuid.cb(proc->conn_handle,
+                                ble_gattc_error(status, att_handle), attr,
+                                proc->read_uuid.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_read_uuid_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_read_uuid_cb(proc, BLE_HS_ETIMEOUT, 0, NULL);
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * read-using-characteristic-uuid proc.
+ */
+static void
+ble_gattc_read_uuid_err(struct ble_gattc_proc *proc, int status,
+                        uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_read_uuid_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming "attribute data" entry from a read-by-type response for
+ * the specified read-using-characteristic-uuid proc.
+ */
+static int
+ble_gattc_read_uuid_rx_adata(struct ble_gattc_proc *proc,
+                             struct ble_att_read_type_adata *adata)
+{
+    struct ble_gatt_attr attr;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    attr.handle = adata->att_handle;
+    attr.offset = 0;
+    attr.om = ble_hs_mbuf_from_flat(adata->value, adata->value_len);
+    if (attr.om == NULL) {
+        rc = BLE_HS_ENOMEM;
+    } else {
+        rc = 0;
+    }
+    rc = ble_gattc_read_uuid_cb(proc, rc, 0, &attr);
+
+    /* Free the attribute mbuf if the application has not consumed it. */
+    os_mbuf_free_chain(attr.om);
+
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles a notification that a read-by-type response has been fully
+ * processed for the specified read-using-characteristic-uuid proc.
+ */
+static int
+ble_gattc_read_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0) {
+        ble_gattc_read_uuid_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* XXX: We may need to send a follow-up request to address the possibility
+     * of multiple characteristics with identical UUIDs.
+     */
+    ble_gattc_read_uuid_cb(proc, BLE_HS_EDONE, 0, NULL);
+    return BLE_HS_EDONE;
+}
+
+static int
+ble_gattc_read_uuid_tx(struct ble_gattc_proc *proc)
+{
+    return ble_att_clt_tx_read_type(proc->conn_handle,
+                                    proc->read_uuid.start_handle,
+                                    proc->read_uuid.end_handle,
+                                    &proc->read_uuid.chr_uuid.u);
+}
+
+int
+ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle,
+                       uint16_t end_handle, const ble_uuid_t *uuid,
+                       ble_gatt_attr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_READ_UUID)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, read_uuid);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_READ_UUID;
+    proc->conn_handle = conn_handle;
+    ble_uuid_to_any(uuid, &proc->read_uuid.chr_uuid);
+    proc->read_uuid.start_handle = start_handle;
+    proc->read_uuid.end_handle = end_handle;
+    proc->read_uuid.cb = cb;
+    proc->read_uuid.cb_arg = cb_arg;
+
+    ble_gattc_log_read_uuid(start_handle, end_handle, uuid);
+    rc = ble_gattc_read_uuid_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, read_uuid_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $read long                                                                *
+ *****************************************************************************/
+
+/**
+ * Calls a read-long-characteristic proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_read_long_cb(struct ble_gattc_proc *proc, int status,
+                       uint16_t att_handle, struct ble_gatt_attr *attr)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(attr != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, read_long_fail);
+    }
+
+    if (proc->read_long.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->read_long.cb(proc->conn_handle,
+                                ble_gattc_error(status, att_handle), attr,
+                                proc->read_long.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_read_long_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_read_long_cb(proc, BLE_HS_ETIMEOUT, 0, NULL);
+}
+
+/**
+ * Triggers a pending transmit for the specified read-long-characteristic proc.
+ */
+static int
+ble_gattc_read_long_tx(struct ble_gattc_proc *proc)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (proc->read_long.offset == 0) {
+        rc = ble_att_clt_tx_read(proc->conn_handle, proc->read_long.handle);
+        if (rc != 0) {
+            return rc;
+        }
+    } else {
+        rc = ble_att_clt_tx_read_blob(proc->conn_handle,
+                                      proc->read_long.handle,
+                                      proc->read_long.offset);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_gattc_read_long_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_read_long_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_read_long_cb(proc, rc, 0, NULL);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * read-long-characteristic proc.
+ */
+static void
+ble_gattc_read_long_err(struct ble_gattc_proc *proc, int status,
+                        uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_read_long_cb(proc, status, att_handle, NULL);
+}
+
+/**
+ * Handles an incoming read-response for the specified
+ * read-long-characteristic-values proc.
+ */
+static int
+ble_gattc_read_long_rx_read_rsp(struct ble_gattc_proc *proc, int status,
+                                struct os_mbuf **om)
+{
+    struct ble_gatt_attr attr;
+    uint16_t data_len;
+    uint16_t mtu;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    data_len = OS_MBUF_PKTLEN(*om);
+
+    attr.handle = proc->read_long.handle;
+    attr.offset = proc->read_long.offset;
+    attr.om = *om;
+
+    /* Report partial payload to application. */
+    rc = ble_gattc_read_long_cb(proc, status, 0, &attr);
+
+    /* Indicate to the caller whether the application consumed the mbuf. */
+    *om = attr.om;
+
+    if (rc != 0 || status != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    /* Determine if this is the end of the attribute value. */
+    mtu = ble_att_mtu(proc->conn_handle);
+    if (mtu == 0) {
+        /* No longer connected. */
+        return BLE_HS_EDONE;
+    }
+
+    if (data_len < mtu - 1) {
+        /* Response shorter than maximum allowed; read complete. */
+        ble_gattc_read_long_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    proc->read_long.offset += data_len;
+    rc = ble_gattc_read_long_resume(proc);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
+}
+
+int
+ble_gattc_read_long(uint16_t conn_handle, uint16_t handle, uint16_t offset,
+                    ble_gatt_attr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_READ_LONG)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, read_long);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_READ_LONG;
+    proc->conn_handle = conn_handle;
+    proc->read_long.handle = handle;
+    proc->read_long.offset = offset;
+    proc->read_long.cb = cb;
+    proc->read_long.cb_arg = cb_arg;
+
+    ble_gattc_log_read_long(proc);
+
+    rc = ble_gattc_read_long_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, read_long_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $read multiple                                                            *
+ *****************************************************************************/
+
+/**
+ * Calls a read-multiple-characteristics proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_read_mult_cb(struct ble_gattc_proc *proc, int status,
+                       uint16_t att_handle, struct os_mbuf **om)
+{
+    struct ble_gatt_attr attr;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(om != NULL || status != 0);
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, read_mult_fail);
+    }
+
+    attr.handle = 0;
+    attr.offset = 0;
+    if (om == NULL) {
+        attr.om = NULL;
+    } else {
+        attr.om = *om;
+    }
+
+    if (proc->read_mult.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->read_mult.cb(proc->conn_handle,
+                                ble_gattc_error(status, att_handle), &attr,
+                                proc->read_mult.cb_arg);
+    }
+
+    /* Indicate to the caller whether the application consumed the mbuf. */
+    if (om != NULL) {
+        *om = attr.om;
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_read_mult_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_read_mult_cb(proc, BLE_HS_ETIMEOUT, 0, 0);
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * read-multiple-characteristics proc.
+ */
+static void
+ble_gattc_read_mult_err(struct ble_gattc_proc *proc, int status,
+                        uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_read_mult_cb(proc, status, att_handle, NULL);
+}
+
+static int
+ble_gattc_read_mult_tx(struct ble_gattc_proc *proc)
+{
+    int rc;
+
+    rc = ble_att_clt_tx_read_mult(proc->conn_handle, proc->read_mult.handles,
+                                  proc->read_mult.num_handles);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+
+int
+ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles,
+                    uint8_t num_handles, ble_gatt_attr_fn *cb,
+                    void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_READ_MULT)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = NULL;
+
+    STATS_INC(ble_gattc_stats, read_mult);
+
+    if (num_handles > MYNEWT_VAL(BLE_GATT_READ_MAX_ATTRS)) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_READ_MULT;
+    proc->conn_handle = conn_handle;
+    memcpy(proc->read_mult.handles, handles, num_handles * sizeof *handles);
+    proc->read_mult.num_handles = num_handles;
+    proc->read_mult.cb = cb;
+    proc->read_mult.cb_arg = cb_arg;
+
+    ble_gattc_log_read_mult(handles, num_handles);
+    rc = ble_gattc_read_mult_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, read_mult_fail);
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $write no response                                                        *
+ *****************************************************************************/
+
+int
+ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
+                       struct os_mbuf *txom)
+{
+#if !MYNEWT_VAL(BLE_GATT_WRITE_NO_RSP)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    int rc;
+
+    STATS_INC(ble_gattc_stats, write_no_rsp);
+
+    ble_gattc_log_write(attr_handle, OS_MBUF_PKTLEN(txom), 0);
+
+    rc = ble_att_clt_tx_write_cmd(conn_handle, attr_handle, txom);
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, write);
+    }
+
+    return rc;
+}
+
+int
+ble_gattc_write_no_rsp_flat(uint16_t conn_handle, uint16_t attr_handle,
+                            const void *data, uint16_t data_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_from_flat(data, data_len);
+    if (om == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $write                                                                    *
+ *****************************************************************************/
+
+/**
+ * Calls a write-characteristic-value proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_write_cb(struct ble_gattc_proc *proc, int status,
+                   uint16_t att_handle)
+{
+    struct ble_gatt_attr attr;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, write_fail);
+    }
+
+    if (proc->write.cb == NULL) {
+        rc = 0;
+    } else {
+        memset(&attr, 0, sizeof attr);
+        attr.handle = proc->write.att_handle;
+        rc = proc->write.cb(proc->conn_handle,
+                            ble_gattc_error(status, att_handle),
+                            &attr, proc->write.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_write_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_write_cb(proc, BLE_HS_ETIMEOUT, 0);
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * write-characteristic-value proc.
+ */
+static void
+ble_gattc_write_err(struct ble_gattc_proc *proc, int status,
+                    uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_write_cb(proc, status, att_handle);
+}
+
+int
+ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle,
+                struct os_mbuf *txom, ble_gatt_attr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_WRITE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, write);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_WRITE;
+    proc->conn_handle = conn_handle;
+    proc->write.att_handle = attr_handle;
+    proc->write.cb = cb;
+    proc->write.cb_arg = cb_arg;
+
+    ble_gattc_log_write(attr_handle, OS_MBUF_PKTLEN(txom), 1);
+
+    rc = ble_att_clt_tx_write_req(conn_handle, attr_handle, txom);
+    txom = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, write_fail);
+    }
+
+    /* Free the mbuf in case the send failed. */
+    os_mbuf_free_chain(txom);
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+int
+ble_gattc_write_flat(uint16_t conn_handle, uint16_t attr_handle,
+                     const void *data, uint16_t data_len,
+                     ble_gatt_attr_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_from_flat(data, data_len);
+    if (om == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    rc = ble_gattc_write(conn_handle, attr_handle, om, cb, cb_arg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $write long                                                               *
+ *****************************************************************************/
+
+/**
+ * Calls a write-long-characteristic-value proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_write_long_cb(struct ble_gattc_proc *proc, int status,
+                        uint16_t att_handle)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, write_long_fail);
+    }
+
+    if (proc->write_long.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->write_long.cb(proc->conn_handle,
+                                 ble_gattc_error(status, att_handle),
+                                 &proc->write_long.attr,
+                                 proc->write_long.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_write_long_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_write_long_cb(proc, BLE_HS_ETIMEOUT, 0);
+}
+
+/**
+ * Triggers a pending transmit for the specified
+ * write-long-characteristic-value proc.
+ */
+static int
+ble_gattc_write_long_tx(struct ble_gattc_proc *proc)
+{
+    struct os_mbuf *om;
+    int write_len;
+    int max_sz;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    om = NULL;
+
+    max_sz = ble_att_mtu(proc->conn_handle) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+    if (max_sz <= 0) {
+        /* Not connected. */
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
+    write_len = min(max_sz,
+                    OS_MBUF_PKTLEN(proc->write_long.attr.om) -
+                        proc->write_long.attr.offset);
+
+    if (write_len <= 0) {
+        rc = ble_att_clt_tx_exec_write(proc->conn_handle,
+                                       BLE_ATT_EXEC_WRITE_F_EXECUTE);
+        goto done;
+    }
+
+    proc->write_long.length = write_len;
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = os_mbuf_appendfrom(om, proc->write_long.attr.om,
+                            proc->write_long.attr.offset,
+                            proc->write_long.length);
+    if (rc != 0) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_clt_tx_prep_write(proc->conn_handle,
+                                   proc->write_long.attr.handle,
+                                   proc->write_long.attr.offset, om);
+    om = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
+static int
+ble_gattc_write_long_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_write_long_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_write_long_cb(proc, rc, 0);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * write-long-characteristic-value proc.
+ */
+static void
+ble_gattc_write_long_err(struct ble_gattc_proc *proc, int status,
+                         uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    /* If we have successfully queued any data, and the failure occurred before
+     * we could send the execute write command, then erase all queued data.
+     */
+    if (proc->write_long.attr.offset > 0 &&
+        proc->write_long.attr.offset <
+            OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
+        ble_att_clt_tx_exec_write(proc->conn_handle,
+                                  BLE_ATT_EXEC_WRITE_F_CANCEL);
+    }
+
+    /* Report failure. */
+    ble_gattc_write_long_cb(proc, status, att_handle);
+}
+
+/**
+ * Handles an incoming prepare-write-response for the specified
+ * write-long-cahracteristic-values proc.
+ */
+static int
+ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc,
+                             int status,
+                             uint16_t handle, uint16_t offset,
+                             struct os_mbuf **rxom)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    /* Let the caller free the mbuf. */
+    om = *rxom;
+
+    if (status != 0) {
+        rc = status;
+        goto err;
+    }
+
+    /* Verify the response. */
+    if (proc->write_long.attr.offset >=
+        OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
+        /* Expecting a prepare write response, not an execute write
+         * response.
+         */
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (handle != proc->write_long.attr.handle) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (offset != proc->write_long.attr.offset) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (offset + OS_MBUF_PKTLEN(om) >
+        OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (OS_MBUF_PKTLEN(om) != proc->write_long.length) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (os_mbuf_cmpm(om, 0,
+                     proc->write_long.attr.om, offset,
+                     proc->write_long.length) != 0) {
+
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
+    /* Send follow-up request. */
+    proc->write_long.attr.offset += OS_MBUF_PKTLEN(om);
+    rc = ble_gattc_write_long_resume(proc);
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    /* XXX: Might need to cancel pending writes. */
+    ble_gattc_write_long_cb(proc, rc, 0);
+    return BLE_HS_EDONE;
+}
+
+/**
+ * Handles an incoming execute-write-response for the specified
+ * write-long-characteristic-values proc.
+ */
+static int
+ble_gattc_write_long_rx_exec(struct ble_gattc_proc *proc, int status)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (proc->write_long.attr.offset <
+        OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
+        /* Expecting an execute write response, not a prepare write
+         * response.
+         */
+        return BLE_HS_EBADDATA;
+    }
+
+    ble_gattc_write_long_cb(proc, status, 0);
+    return BLE_HS_EDONE;
+}
+
+int
+ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle,
+                     uint16_t offset, struct os_mbuf *txom,
+                     ble_gatt_attr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_WRITE_LONG)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, write_long);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_WRITE_LONG;
+    proc->conn_handle = conn_handle;
+    proc->write_long.attr.handle = attr_handle;
+    proc->write_long.attr.offset = offset;
+    proc->write_long.attr.om = txom;
+    proc->write_long.cb = cb;
+    proc->write_long.cb_arg = cb_arg;
+
+    /* The mbuf is consumed by the procedure. */
+    txom = NULL;
+
+    ble_gattc_log_write_long(proc);
+
+    rc = ble_gattc_write_long_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, write_long_fail);
+    }
+
+    /* Free the mbuf in case of failure. */
+    os_mbuf_free_chain(txom);
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $write reliable                                                           *
+ *****************************************************************************/
+
+/**
+ * Calls a write-long-characteristic-value proc's callback with the specified
+ * parameters.  If the proc has no callback, this function is a no-op.
+ *
+ * @return                      The return code of the callback (or 0 if there
+ *                                  is no callback).
+ */
+static int
+ble_gattc_write_reliable_cb(struct ble_gattc_proc *proc, int status,
+                            uint16_t att_handle)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != 0 && status != BLE_HS_EDONE) {
+        STATS_INC(ble_gattc_stats, write_reliable_fail);
+    }
+
+    if (proc->write_reliable.cb == NULL) {
+        rc = 0;
+    } else {
+        rc = proc->write_reliable.cb(proc->conn_handle,
+                                     ble_gattc_error(status, att_handle),
+                                     proc->write_reliable.attrs,
+                                     proc->write_reliable.num_attrs,
+                                     proc->write_reliable.cb_arg);
+    }
+
+    return rc;
+}
+
+static void
+ble_gattc_write_reliable_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gattc_write_reliable_cb(proc, BLE_HS_ETIMEOUT, 0);
+}
+
+/**
+ * Triggers a pending transmit for the specified
+ * write-reliable-characteristic-value proc.
+ */
+static int
+ble_gattc_write_reliable_tx(struct ble_gattc_proc *proc)
+{
+    struct ble_gatt_attr *attr;
+    struct os_mbuf *om;
+    uint16_t max_sz;
+    int attr_idx;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    om = NULL;
+
+    attr_idx = proc->write_reliable.cur_attr;
+
+    if (attr_idx >= proc->write_reliable.num_attrs) {
+        rc = ble_att_clt_tx_exec_write(proc->conn_handle,
+                                       BLE_ATT_EXEC_WRITE_F_EXECUTE);
+        goto done;
+    }
+
+    attr = proc->write_reliable.attrs + attr_idx;
+
+    max_sz = ble_att_mtu(proc->conn_handle) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+    if (max_sz <= 0) {
+        /* Not connected. */
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
+    proc->write_reliable.length =
+        min(max_sz, OS_MBUF_PKTLEN(attr->om) - attr->offset);
+
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = os_mbuf_appendfrom(om, attr->om, attr->offset,
+                            proc->write_reliable.length);
+    if (rc != 0) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_clt_tx_prep_write(proc->conn_handle, attr->handle,
+                                   attr->offset, om);
+    om = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
+static int
+ble_gattc_write_reliable_resume(struct ble_gattc_proc *proc)
+{
+    int status;
+    int rc;
+
+    status = ble_gattc_write_reliable_tx(proc);
+    rc = ble_gattc_process_resume_status(proc, status);
+    if (rc != 0) {
+        ble_gattc_write_reliable_cb(proc, rc, 0);
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Handles an incoming ATT error response for the specified
+ * write-reliable-characteristic-value proc.
+ */
+static void
+ble_gattc_write_reliable_err(struct ble_gattc_proc *proc, int status,
+                             uint16_t att_handle)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_write_reliable_cb(proc, status, att_handle);
+
+    /* If we have successfully queued any data, and the failure occurred before
+     * we could send the execute write command, then erase all queued data.
+     */
+    if (proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) {
+
+        ble_att_clt_tx_exec_write(proc->conn_handle,
+                                  BLE_ATT_EXEC_WRITE_F_CANCEL);
+    }
+}
+
+/**
+ * Handles an incoming prepare-write-response for the specified
+ * write-reliable-cahracteristic-values proc.
+ */
+static int
+ble_gattc_write_reliable_rx_prep(struct ble_gattc_proc *proc,
+                                 int status,
+                                 uint16_t handle, uint16_t offset,
+                                 struct os_mbuf **rxom)
+{
+    struct ble_gatt_attr *attr;
+    struct os_mbuf *om;
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    /* Let the caller free the mbuf. */
+    om = *rxom;
+
+    if (status != 0) {
+        rc = status;
+        goto err;
+    }
+
+    if (proc->write_reliable.cur_attr >= proc->write_reliable.num_attrs) {
+        /* Expecting an execute write response, not a prepare write
+         * response.
+         */
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    attr = proc->write_reliable.attrs + proc->write_reliable.cur_attr;
+
+    /* Verify the response. */
+    if (handle != attr->handle) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (offset != attr->offset) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+    if (os_mbuf_cmpm(attr->om, offset, om, 0,
+                     proc->write_reliable.length) != 0) {
+
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
+    /* Send follow-up request. */
+    attr->offset += proc->write_reliable.length;
+    if (attr->offset >= OS_MBUF_PKTLEN(attr->om)) {
+        attr->offset = 0;
+        proc->write_reliable.cur_attr++;
+    }
+    rc = ble_gattc_write_reliable_resume(proc);
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    ble_gattc_write_reliable_err(proc, rc, 0);
+    return BLE_HS_EDONE;
+}
+
+/**
+ * Handles an incoming execute-write-response for the specified
+ * write-reliable-characteristic-values proc.
+ */
+static int
+ble_gattc_write_reliable_rx_exec(struct ble_gattc_proc *proc, int status)
+{
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+    ble_gattc_write_reliable_cb(proc, status, 0);
+    return BLE_HS_EDONE;
+}
+
+int
+ble_gattc_write_reliable(uint16_t conn_handle,
+                         struct ble_gatt_attr *attrs,
+                         int num_attrs,
+                         ble_gatt_reliable_attr_fn *cb, void *cb_arg)
+{
+#if !MYNEWT_VAL(BLE_GATT_WRITE_RELIABLE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+    int i;
+
+    proc = NULL;
+
+    STATS_INC(ble_gattc_stats, write_reliable);
+
+    if (num_attrs > MYNEWT_VAL(BLE_GATT_WRITE_MAX_ATTRS)) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_WRITE_RELIABLE;
+    proc->conn_handle = conn_handle;
+    proc->write_reliable.num_attrs = num_attrs;
+    proc->write_reliable.cur_attr = 0;
+    proc->write_reliable.cb = cb;
+    proc->write_reliable.cb_arg = cb_arg;
+
+    for (i = 0; i < num_attrs; i++) {
+        proc->write_reliable.attrs[i] = attrs[i];
+        proc->write_reliable.attrs[i].offset = 0;
+
+        /* Consume mbuf from caller. */
+        attrs[i].om = NULL;
+    }
+
+    ble_gattc_log_write_reliable(proc);
+    rc = ble_gattc_write_reliable_tx(proc);
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, write_reliable_fail);
+    }
+
+    /* Free supplied mbufs in case something failed. */
+    for (i = 0; i < num_attrs; i++) {
+        os_mbuf_free_chain(attrs[i].om);
+        attrs[i].om = NULL;
+    }
+
+    ble_gattc_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $notify                                                                   *
+ *****************************************************************************/
+
+int
+ble_gattc_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle,
+                        struct os_mbuf *txom)
+{
+#if !MYNEWT_VAL(BLE_GATT_NOTIFY)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    int rc;
+
+    STATS_INC(ble_gattc_stats, notify);
+
+    ble_gattc_log_notify(chr_val_handle);
+
+    if (txom == NULL) {
+        /* No custom attribute data; read the value from the specified
+         * attribute.
+         */
+        txom = ble_hs_mbuf_att_pkt();
+        if (txom == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto done;
+        }
+        rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE,
+                                     chr_val_handle, 0, txom, NULL);
+        if (rc != 0) {
+            /* Fatal error; application disallowed attribute read. */
+            rc = BLE_HS_EAPP;
+            goto done;
+        }
+    }
+
+    rc = ble_att_clt_tx_notify(conn_handle, chr_val_handle, txom);
+    txom = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, notify_fail);
+    }
+
+    /* Tell the application that a notification transmission was attempted. */
+    ble_gap_notify_tx_event(rc, conn_handle, chr_val_handle, 0);
+
+    os_mbuf_free_chain(txom);
+
+    return rc;
+}
+
+int
+ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle)
+{
+#if !MYNEWT_VAL(BLE_GATT_NOTIFY)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    int rc;
+
+    rc = ble_gattc_notify_custom(conn_handle, chr_val_handle, NULL);
+
+    return rc;
+}
+
+/*****************************************************************************
+ * $indicate                                                                 *
+ *****************************************************************************/
+
+/**
+ * Handles an incoming ATT error response for the specified indication proc.
+ * A device should never send an error in response to an indication.  If this
+ * happens, we treat it like a confirmation (indication ack), but report the
+ * error status to the application.
+ */
+static void
+ble_gattc_indicate_err(struct ble_gattc_proc *proc, int status,
+                       uint16_t att_handle)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (status != BLE_HS_ENOTCONN) {
+        rc = ble_gatts_rx_indicate_ack(proc->conn_handle,
+                                       proc->indicate.chr_val_handle);
+        if (rc != 0) {
+            return;
+        }
+    }
+
+    /* Tell the application about the received acknowledgment. */
+    ble_gap_notify_tx_event(status, proc->conn_handle,
+                            proc->indicate.chr_val_handle, 1);
+
+    /* Send the next indication if one is pending. */
+    ble_gatts_send_next_indicate(proc->conn_handle);
+}
+
+static void
+ble_gattc_indicate_tmo(struct ble_gattc_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    ble_gap_notify_tx_event(BLE_HS_ETIMEOUT, proc->conn_handle,
+                            proc->indicate.chr_val_handle, 1);
+}
+
+/**
+ * Handles an incoming handle-value-confirmation for the specified indication
+ * proc.
+ */
+static void
+ble_gattc_indicate_rx_rsp(struct ble_gattc_proc *proc)
+{
+    int rc;
+
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    rc = ble_gatts_rx_indicate_ack(proc->conn_handle,
+                                   proc->indicate.chr_val_handle);
+    if (rc != 0) {
+        return;
+    }
+
+    /* Tell the application about the received acknowledgment. */
+    ble_gap_notify_tx_event(BLE_HS_EDONE, proc->conn_handle,
+                            proc->indicate.chr_val_handle, 1);
+
+    /* Send the next indication if one is pending. */
+    ble_gatts_send_next_indicate(proc->conn_handle);
+}
+
+/**
+ * Causes the indication in progress for the specified connection (if any) to
+ * fail with a status code of BLE_HS_ENOTCONN;
+ */
+void
+ble_gatts_indicate_fail_notconn(uint16_t conn_handle)
+{
+    ble_gattc_fail_procs(conn_handle, BLE_GATT_OP_INDICATE, BLE_HS_ENOTCONN);
+}
+
+int
+ble_gattc_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle,
+                          struct os_mbuf *txom)
+{
+#if !MYNEWT_VAL(BLE_GATT_INDICATE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_gattc_proc *proc;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    STATS_INC(ble_gattc_stats, indicate);
+
+    proc = ble_gattc_proc_alloc();
+    if (proc == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_GATT_OP_INDICATE;
+    proc->conn_handle = conn_handle;
+    proc->indicate.chr_val_handle = chr_val_handle;
+
+    ble_gattc_log_indicate(chr_val_handle);
+
+    if (txom == NULL) {
+        /* No custom attribute data; read the value from the specified
+         * attribute.
+         */
+        txom = ble_hs_mbuf_att_pkt();
+        if (txom == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto done;
+        }
+
+        rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, chr_val_handle,
+                                     0, txom, NULL);
+        if (rc != 0) {
+            /* Fatal error; application disallowed attribute read. */
+            BLE_HS_DBG_ASSERT(0);
+            rc = BLE_HS_EAPP;
+            goto done;
+        }
+    }
+
+    rc = ble_att_clt_tx_indicate(conn_handle, chr_val_handle, txom);
+    txom = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        BLE_HS_DBG_ASSERT(conn->bhc_gatt_svr.indicate_val_handle == 0);
+        conn->bhc_gatt_svr.indicate_val_handle = chr_val_handle;
+    }
+    ble_hs_unlock();
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, indicate_fail);
+    }
+
+    /* Tell the application that an indication transmission was attempted. */
+    ble_gap_notify_tx_event(rc, conn_handle, chr_val_handle, 1);
+
+    ble_gattc_process_status(proc, rc);
+    os_mbuf_free_chain(txom);
+    return rc;
+}
+
+int
+ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle)
+{
+    return ble_gattc_indicate_custom(conn_handle, chr_val_handle, NULL);
+}
+
+/*****************************************************************************
+ * $rx                                                                       *
+ *****************************************************************************/
+
+/**
+ * Dispatches an incoming ATT error-response to the appropriate active GATT
+ * procedure.
+ */
+void
+ble_gattc_rx_err(uint16_t conn_handle, uint16_t handle, uint16_t status)
+{
+    struct ble_gattc_proc *proc;
+    ble_gattc_err_fn *err_cb;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle, BLE_GATT_OP_NONE);
+    if (proc != NULL) {
+        err_cb = ble_gattc_err_dispatch_get(proc->op);
+        if (err_cb != NULL) {
+            err_cb(proc, BLE_HS_ERR_ATT_BASE + status, handle);
+        }
+        ble_gattc_proc_free(proc);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT exchange-mtu-response to the appropriate active
+ * GATT procedure.
+ */
+void
+ble_gattc_rx_mtu(uint16_t conn_handle, int status, uint16_t chan_mtu)
+{
+    struct ble_gattc_proc *proc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle, BLE_GATT_OP_MTU);
+    if (proc != NULL) {
+        ble_gattc_mtu_cb(proc, status, 0, chan_mtu);
+        ble_gattc_process_status(proc, BLE_HS_EDONE);
+    }
+}
+
+/**
+ * Dispatches an incoming "information data" entry from a
+ * find-information-response to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_find_info_idata(uint16_t conn_handle,
+                             struct ble_att_find_info_idata *idata)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_INFO
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_DISC_ALL_DSCS);
+    if (proc != NULL) {
+        rc = ble_gattc_disc_all_dscs_rx_idata(proc, idata);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming notification of the end of a
+ * find-information-response to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_find_info_complete(uint16_t conn_handle, int status)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_INFO
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_DISC_ALL_DSCS);
+    if (proc != NULL) {
+        rc = ble_gattc_disc_all_dscs_rx_complete(proc, status);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming "handles info" entry from a
+ * find-by-type-value-response to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_find_type_value_hinfo(uint16_t conn_handle,
+                                   struct ble_att_find_type_value_hinfo *hinfo)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_TYPE
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_DISC_SVC_UUID);
+    if (proc != NULL) {
+        rc = ble_gattc_disc_svc_uuid_rx_hinfo(proc, hinfo);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming notification of the end of a
+ * find-by-type-value-response to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_find_type_value_complete(uint16_t conn_handle, int status)
+{
+#if !NIMBLE_BLE_ATT_CLT_FIND_TYPE
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_DISC_SVC_UUID);
+    if (proc != NULL) {
+        rc = ble_gattc_disc_svc_uuid_rx_complete(proc, status);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming "attribute data" entry from a read-by-type-response
+ * to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_read_type_adata(uint16_t conn_handle,
+                             struct ble_att_read_type_adata *adata)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_TYPE
+    return;
+#endif
+
+    const struct ble_gattc_rx_adata_entry *rx_entry;
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle,
+                                         ble_gattc_rx_read_type_elem_entries,
+                                         &rx_entry);
+    if (proc != NULL) {
+        rc = rx_entry->cb(proc, adata);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming notification of the end of a read-by-type-response to
+ * the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_read_type_complete(uint16_t conn_handle, int status)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_TYPE
+    return;
+#endif
+
+    const struct ble_gattc_rx_complete_entry *rx_entry;
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = BLE_GATTC_RX_EXTRACT_RX_ENTRY(
+        conn_handle, ble_gattc_rx_read_type_complete_entries,
+        &rx_entry);
+    if (proc != NULL) {
+        rc = rx_entry->cb(proc, status);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming "attribute data" entry from a
+ * read-by-group-type-response to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_read_group_type_adata(uint16_t conn_handle,
+                                   struct ble_att_read_group_type_adata *adata)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_GROUP_TYPE
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_DISC_ALL_SVCS);
+    if (proc != NULL) {
+        rc = ble_gattc_disc_all_svcs_rx_adata(proc, adata);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming notification of the end of a
+ * read-by-group-type-response to the appropriate active GATT procedure.
+ */
+void
+ble_gattc_rx_read_group_type_complete(uint16_t conn_handle, int status)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_GROUP_TYPE
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_DISC_ALL_SVCS);
+    if (proc != NULL) {
+        rc = ble_gattc_disc_all_svcs_rx_complete(proc, status);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT read-response to the appropriate active GATT
+ * procedure.
+ */
+void
+ble_gattc_rx_read_rsp(uint16_t conn_handle, int status, struct os_mbuf **om)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ
+    return;
+#endif
+
+    const struct ble_gattc_rx_attr_entry *rx_entry;
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle,
+                                         ble_gattc_rx_read_rsp_entries,
+                                         &rx_entry);
+    if (proc != NULL) {
+        rc = rx_entry->cb(proc, status, om);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT read-blob-response to the appropriate active GATT
+ * procedure.
+ */
+void
+ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, int status,
+                           struct os_mbuf **om)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_BLOB
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_READ_LONG);
+    if (proc != NULL) {
+        rc = ble_gattc_read_long_rx_read_rsp(proc, status, om);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT read-multiple-response to the appropriate active
+ * GATT procedure.
+ */
+void
+ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, int status,
+                           struct os_mbuf **om)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_MULT
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_READ_MULT);
+    if (proc != NULL) {
+        ble_gattc_read_mult_cb(proc, status, 0, om);
+        ble_gattc_process_status(proc, BLE_HS_EDONE);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT write-response to the appropriate active GATT
+ * procedure.
+ */
+void
+ble_gattc_rx_write_rsp(uint16_t conn_handle)
+{
+#if !NIMBLE_BLE_ATT_CLT_WRITE
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_WRITE);
+    if (proc != NULL) {
+        ble_gattc_write_cb(proc, 0, 0);
+        ble_gattc_process_status(proc, BLE_HS_EDONE);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT prepare-write-response to the appropriate active
+ * GATT procedure.
+ */
+void
+ble_gattc_rx_prep_write_rsp(uint16_t conn_handle, int status,
+                            uint16_t handle, uint16_t offset,
+                            struct os_mbuf **om)
+{
+#if !NIMBLE_BLE_ATT_CLT_PREP_WRITE
+    return;
+#endif
+
+    const struct ble_gattc_rx_prep_entry *rx_entry;
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle,
+                                         ble_gattc_rx_prep_entries,
+                                         &rx_entry);
+    if (proc != NULL) {
+        rc = rx_entry->cb(proc, status, handle, offset, om);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT execute-write-response to the appropriate active
+ * GATT procedure.
+ */
+void
+ble_gattc_rx_exec_write_rsp(uint16_t conn_handle, int status)
+{
+#if !NIMBLE_BLE_ATT_CLT_EXEC_WRITE
+    return;
+#endif
+
+    const struct ble_gattc_rx_exec_entry *rx_entry;
+    struct ble_gattc_proc *proc;
+    int rc;
+
+    proc = BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle,
+                                         ble_gattc_rx_exec_entries, &rx_entry);
+    if (proc != NULL) {
+        rc = rx_entry->cb(proc, status);
+        ble_gattc_process_status(proc, rc);
+    }
+}
+
+/**
+ * Dispatches an incoming ATT handle-value-confirmation to the appropriate
+ * active GATT procedure.
+ */
+void
+ble_gattc_rx_indicate_rsp(uint16_t conn_handle)
+{
+#if !NIMBLE_BLE_ATT_CLT_INDICATE
+    return;
+#endif
+
+    struct ble_gattc_proc *proc;
+
+    proc = ble_gattc_extract_first_by_conn_op(conn_handle,
+                                              BLE_GATT_OP_INDICATE);
+    if (proc != NULL) {
+        ble_gattc_indicate_rx_rsp(proc);
+        ble_gattc_process_status(proc, BLE_HS_EDONE);
+    }
+}
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+/**
+ * Called when a BLE connection ends.  Frees all GATT resources associated with
+ * the connection and cancels all relevant pending and in-progress GATT
+ * procedures.
+ *
+ * @param conn_handle           The handle of the connection that was
+ *                                  terminated.
+ */
+void
+ble_gattc_connection_broken(uint16_t conn_handle)
+{
+    ble_gattc_fail_procs(conn_handle, BLE_GATT_OP_NONE, BLE_HS_ENOTCONN);
+}
+
+/**
+ * Indicates whether there are currently any active GATT client procedures.
+ */
+int
+ble_gattc_any_jobs(void)
+{
+    return !STAILQ_EMPTY(&ble_gattc_procs);
+}
+
+int
+ble_gattc_init(void)
+{
+    int rc;
+
+    STAILQ_INIT(&ble_gattc_procs);
+
+    if (MYNEWT_VAL(BLE_GATT_MAX_PROCS) > 0) {
+        rc = os_mempool_init(&ble_gattc_proc_pool,
+                             MYNEWT_VAL(BLE_GATT_MAX_PROCS),
+                             sizeof (struct ble_gattc_proc),
+                             ble_gattc_proc_mem,
+                             "ble_gattc_proc_pool");
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    rc = stats_init_and_reg(
+        STATS_HDR(ble_gattc_stats), STATS_SIZE_INIT_PARMS(ble_gattc_stats,
+        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_gattc_stats), "ble_gattc");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_gatts.c b/src/libs/mynewt-nimble/nimble/host/src/ble_gatts.c
new file mode 100644
index 00000000..a635f2d7
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_gatts.c
@@ -0,0 +1,2184 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "nimble/ble.h"
+#include "host/ble_uuid.h"
+#include "host/ble_store.h"
+#include "ble_hs_priv.h"
+
+#define BLE_GATTS_INCLUDE_SZ    6
+#define BLE_GATTS_CHR_MAX_SZ    19
+
+static const ble_uuid_t *uuid_pri =
+    BLE_UUID16_DECLARE(BLE_ATT_UUID_PRIMARY_SERVICE);
+static const ble_uuid_t *uuid_sec =
+    BLE_UUID16_DECLARE(BLE_ATT_UUID_SECONDARY_SERVICE);
+static const ble_uuid_t *uuid_inc =
+    BLE_UUID16_DECLARE(BLE_ATT_UUID_INCLUDE);
+static const ble_uuid_t *uuid_chr =
+    BLE_UUID16_DECLARE(BLE_ATT_UUID_CHARACTERISTIC);
+static const ble_uuid_t *uuid_ccc =
+    BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16);
+
+static const struct ble_gatt_svc_def **ble_gatts_svc_defs;
+static int ble_gatts_num_svc_defs;
+
+struct ble_gatts_svc_entry {
+    const struct ble_gatt_svc_def *svc;
+    uint16_t handle;            /* 0 means unregistered. */
+    uint16_t end_group_handle;  /* 0xffff means unset. */
+};
+
+static struct ble_gatts_svc_entry *ble_gatts_svc_entries;
+static uint16_t ble_gatts_num_svc_entries;
+
+static os_membuf_t *ble_gatts_clt_cfg_mem;
+static struct os_mempool ble_gatts_clt_cfg_pool;
+
+struct ble_gatts_clt_cfg {
+    uint16_t chr_val_handle;
+    uint8_t flags;
+    uint8_t allowed;
+};
+
+/** A cached array of handles for the configurable characteristics. */
+static struct ble_gatts_clt_cfg *ble_gatts_clt_cfgs;
+static int ble_gatts_num_cfgable_chrs;
+
+STATS_SECT_DECL(ble_gatts_stats) ble_gatts_stats;
+STATS_NAME_START(ble_gatts_stats)
+    STATS_NAME(ble_gatts_stats, svcs)
+    STATS_NAME(ble_gatts_stats, chrs)
+    STATS_NAME(ble_gatts_stats, dscs)
+    STATS_NAME(ble_gatts_stats, svc_def_reads)
+    STATS_NAME(ble_gatts_stats, svc_inc_reads)
+    STATS_NAME(ble_gatts_stats, chr_def_reads)
+    STATS_NAME(ble_gatts_stats, chr_val_reads)
+    STATS_NAME(ble_gatts_stats, chr_val_writes)
+    STATS_NAME(ble_gatts_stats, dsc_reads)
+    STATS_NAME(ble_gatts_stats, dsc_writes)
+STATS_NAME_END(ble_gatts_stats)
+
+static int
+ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle,
+                     uint8_t op, uint16_t offset, struct os_mbuf **om,
+                     void *arg)
+{
+    const struct ble_gatt_svc_def *svc;
+    uint8_t *buf;
+
+    STATS_INC(ble_gatts_stats, svc_def_reads);
+
+    BLE_HS_DBG_ASSERT(op == BLE_ATT_ACCESS_OP_READ);
+
+    svc = arg;
+
+    buf = os_mbuf_extend(*om, ble_uuid_length(svc->uuid));
+    if (buf == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
+    ble_uuid_flat(svc->uuid, buf);
+
+    return 0;
+}
+
+static int
+ble_gatts_inc_access(uint16_t conn_handle, uint16_t attr_handle,
+                     uint8_t op, uint16_t offset, struct os_mbuf **om,
+                     void *arg)
+{
+    const struct ble_gatts_svc_entry *entry;
+    uint16_t uuid16;
+    uint8_t *buf;
+
+    STATS_INC(ble_gatts_stats, svc_inc_reads);
+
+    BLE_HS_DBG_ASSERT(op == BLE_ATT_ACCESS_OP_READ);
+
+    entry = arg;
+
+    buf = os_mbuf_extend(*om, 4);
+    if (buf == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+    put_le16(buf + 0, entry->handle);
+    put_le16(buf + 2, entry->end_group_handle);
+
+    /* Only include the service UUID if it has a 16-bit representation. */
+    uuid16 = ble_uuid_u16(entry->svc->uuid);
+    if (uuid16 != 0) {
+        buf = os_mbuf_extend(*om, 2);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        put_le16(buf, uuid16);
+    }
+
+    return 0;
+}
+
+static uint16_t
+ble_gatts_chr_clt_cfg_allowed(const struct ble_gatt_chr_def *chr)
+{
+    uint16_t flags;
+
+    flags = 0;
+    if (chr->flags & BLE_GATT_CHR_F_NOTIFY) {
+        flags |= BLE_GATTS_CLT_CFG_F_NOTIFY;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_INDICATE) {
+        flags |= BLE_GATTS_CLT_CFG_F_INDICATE;
+    }
+
+    return flags;
+}
+
+static uint8_t
+ble_gatts_att_flags_from_chr_flags(ble_gatt_chr_flags chr_flags)
+{
+    uint8_t att_flags;
+
+    att_flags = 0;
+    if (chr_flags & BLE_GATT_CHR_F_READ) {
+        att_flags |= BLE_ATT_F_READ;
+    }
+    if (chr_flags & (BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_WRITE)) {
+        att_flags |= BLE_ATT_F_WRITE;
+    }
+    if (chr_flags & BLE_GATT_CHR_F_READ_ENC) {
+        att_flags |= BLE_ATT_F_READ_ENC;
+    }
+    if (chr_flags & BLE_GATT_CHR_F_READ_AUTHEN) {
+        att_flags |= BLE_ATT_F_READ_AUTHEN;
+    }
+    if (chr_flags & BLE_GATT_CHR_F_READ_AUTHOR) {
+        att_flags |= BLE_ATT_F_READ_AUTHOR;
+    }
+    if (chr_flags & BLE_GATT_CHR_F_WRITE_ENC) {
+        att_flags |= BLE_ATT_F_WRITE_ENC;
+    }
+    if (chr_flags & BLE_GATT_CHR_F_WRITE_AUTHEN) {
+        att_flags |= BLE_ATT_F_WRITE_AUTHEN;
+    }
+    if (chr_flags & BLE_GATT_CHR_F_WRITE_AUTHOR) {
+        att_flags |= BLE_ATT_F_WRITE_AUTHOR;
+    }
+
+    return att_flags;
+}
+
+static uint8_t
+ble_gatts_chr_properties(const struct ble_gatt_chr_def *chr)
+{
+    uint8_t properties;
+
+    properties = 0;
+
+    if (chr->flags & BLE_GATT_CHR_F_BROADCAST) {
+        properties |= BLE_GATT_CHR_PROP_BROADCAST;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_READ) {
+        properties |= BLE_GATT_CHR_PROP_READ;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_WRITE_NO_RSP) {
+        properties |= BLE_GATT_CHR_PROP_WRITE_NO_RSP;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_WRITE) {
+        properties |= BLE_GATT_CHR_PROP_WRITE;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_NOTIFY) {
+        properties |= BLE_GATT_CHR_PROP_NOTIFY;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_INDICATE) {
+        properties |= BLE_GATT_CHR_PROP_INDICATE;
+    }
+    if (chr->flags & BLE_GATT_CHR_F_AUTH_SIGN_WRITE) {
+        properties |= BLE_GATT_CHR_PROP_AUTH_SIGN_WRITE;
+    }
+    if (chr->flags &
+        (BLE_GATT_CHR_F_RELIABLE_WRITE | BLE_GATT_CHR_F_AUX_WRITE)) {
+
+        properties |= BLE_GATT_CHR_PROP_EXTENDED;
+    }
+
+    return properties;
+}
+
+static int
+ble_gatts_chr_def_access(uint16_t conn_handle, uint16_t attr_handle,
+                         uint8_t op, uint16_t offset, struct os_mbuf **om,
+                         void *arg)
+{
+    const struct ble_gatt_chr_def *chr;
+    uint8_t *buf;
+
+    STATS_INC(ble_gatts_stats, chr_def_reads);
+
+    BLE_HS_DBG_ASSERT(op == BLE_ATT_ACCESS_OP_READ);
+
+    chr = arg;
+
+    buf = os_mbuf_extend(*om, 3);
+    if (buf == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
+    buf[0] = ble_gatts_chr_properties(chr);
+
+    /* The value attribute is always immediately after the declaration. */
+    put_le16(buf + 1, attr_handle + 1);
+
+    buf = os_mbuf_extend(*om, ble_uuid_length(chr->uuid));
+    if (buf == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
+    ble_uuid_flat(chr->uuid, buf);
+
+    return 0;
+}
+
+static int
+ble_gatts_chr_is_sane(const struct ble_gatt_chr_def *chr)
+{
+    if (chr->uuid == NULL) {
+        return 0;
+    }
+
+    if (chr->access_cb == NULL) {
+        return 0;
+    }
+
+    /* XXX: Check properties. */
+
+    return 1;
+}
+
+static uint8_t
+ble_gatts_chr_op(uint8_t att_op)
+{
+    switch (att_op) {
+    case BLE_ATT_ACCESS_OP_READ:
+        return BLE_GATT_ACCESS_OP_READ_CHR;
+
+    case BLE_ATT_ACCESS_OP_WRITE:
+        return BLE_GATT_ACCESS_OP_WRITE_CHR;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_GATT_ACCESS_OP_READ_CHR;
+    }
+}
+
+static void
+ble_gatts_chr_inc_val_stat(uint8_t gatt_op)
+{
+    switch (gatt_op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+        STATS_INC(ble_gatts_stats, chr_val_reads);
+        break;
+
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+        STATS_INC(ble_gatts_stats, chr_val_writes);
+        break;
+
+    default:
+        break;
+    }
+}
+
+/**
+ * Indicates whether the set of registered services can be modified.  The
+ * service set is mutable if:
+ *     o No peers are connected, and
+ *     o No GAP operations are active (advertise, discover, or connect).
+ *
+ * @return                      true if the GATT service set can be modified;
+ *                              false otherwise.
+ */
+static bool
+ble_gatts_mutable(void)
+{
+    /* Ensure no active GAP procedures. */
+    if (ble_gap_adv_active() ||
+        ble_gap_disc_active() ||
+        ble_gap_conn_active()) {
+
+        return false;
+    }
+
+    /* Ensure no established connections. */
+    if (ble_hs_conn_first() != NULL) {
+        return false;
+    }
+
+    return true;
+}
+
+static int
+ble_gatts_val_access(uint16_t conn_handle, uint16_t attr_handle,
+                     uint16_t offset, struct ble_gatt_access_ctxt *gatt_ctxt,
+                     struct os_mbuf **om, ble_gatt_access_fn *access_cb,
+                     void *cb_arg)
+{
+    uint16_t initial_len;
+    int attr_len;
+    int new_om;
+    int rc;
+
+    switch (gatt_ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+    case BLE_GATT_ACCESS_OP_READ_DSC:
+        /* A characteristic value is being read.
+         *
+         * If the read specifies an offset of 0:
+         *     just append the characteristic value directly onto the response
+         *     mbuf.
+         *
+         * Else:
+         *     allocate a new mbuf to hold the characteristic data, then append
+         *     the requested portion onto the response mbuf.
+         */
+        if (offset == 0) {
+            new_om = 0;
+            gatt_ctxt->om = *om;
+        } else {
+            new_om = 1;
+            gatt_ctxt->om = os_msys_get_pkthdr(0, 0);
+            if (gatt_ctxt->om == NULL) {
+                return BLE_ATT_ERR_INSUFFICIENT_RES;
+            }
+        }
+
+        initial_len = OS_MBUF_PKTLEN(gatt_ctxt->om);
+        rc = access_cb(conn_handle, attr_handle, gatt_ctxt, cb_arg);
+        if (rc == 0) {
+            attr_len = OS_MBUF_PKTLEN(gatt_ctxt->om) - initial_len - offset;
+            if (attr_len >= 0) {
+                if (new_om) {
+                    os_mbuf_appendfrom(*om, gatt_ctxt->om, offset, attr_len);
+                }
+            } else {
+                rc = BLE_ATT_ERR_INVALID_OFFSET;
+            }
+        }
+
+        if (new_om) {
+            os_mbuf_free_chain(gatt_ctxt->om);
+        }
+        return rc;
+
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+    case BLE_GATT_ACCESS_OP_WRITE_DSC:
+        gatt_ctxt->om = *om;
+        rc = access_cb(conn_handle, attr_handle, gatt_ctxt, cb_arg);
+        *om = gatt_ctxt->om;
+        return rc;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+static int
+ble_gatts_chr_val_access(uint16_t conn_handle, uint16_t attr_handle,
+                         uint8_t att_op, uint16_t offset,
+                         struct os_mbuf **om, void *arg)
+{
+    const struct ble_gatt_chr_def *chr_def;
+    struct ble_gatt_access_ctxt gatt_ctxt;
+    int rc;
+
+    chr_def = arg;
+    BLE_HS_DBG_ASSERT(chr_def != NULL && chr_def->access_cb != NULL);
+
+    gatt_ctxt.op = ble_gatts_chr_op(att_op);
+    gatt_ctxt.chr = chr_def;
+
+    ble_gatts_chr_inc_val_stat(gatt_ctxt.op);
+    rc = ble_gatts_val_access(conn_handle, attr_handle, offset, &gatt_ctxt, om,
+                              chr_def->access_cb, chr_def->arg);
+
+    return rc;
+}
+
+static int
+ble_gatts_find_svc_entry_idx(const struct ble_gatt_svc_def *svc)
+{
+    int i;
+
+    for (i = 0; i < ble_gatts_num_svc_entries; i++) {
+        if (ble_gatts_svc_entries[i].svc == svc) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+static int
+ble_gatts_svc_incs_satisfied(const struct ble_gatt_svc_def *svc)
+{
+    int idx;
+    int i;
+
+    if (svc->includes == NULL) {
+        /* No included services. */
+        return 1;
+    }
+
+    for (i = 0; svc->includes[i] != NULL; i++) {
+        idx = ble_gatts_find_svc_entry_idx(svc->includes[i]);
+        if (idx == -1 || ble_gatts_svc_entries[idx].handle == 0) {
+            return 0;
+        }
+    }
+
+    return 1;
+}
+
+static int
+ble_gatts_register_inc(struct ble_gatts_svc_entry *entry)
+{
+    uint16_t handle;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(entry->handle != 0);
+    BLE_HS_DBG_ASSERT(entry->end_group_handle != 0xffff);
+
+    rc = ble_att_svr_register(uuid_inc, BLE_ATT_F_READ, 0, &handle,
+                              ble_gatts_inc_access, entry);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static uint8_t
+ble_gatts_dsc_op(uint8_t att_op)
+{
+    switch (att_op) {
+    case BLE_ATT_ACCESS_OP_READ:
+        return BLE_GATT_ACCESS_OP_READ_DSC;
+
+    case BLE_ATT_ACCESS_OP_WRITE:
+        return BLE_GATT_ACCESS_OP_WRITE_DSC;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_GATT_ACCESS_OP_READ_DSC;
+    }
+}
+
+static void
+ble_gatts_dsc_inc_stat(uint8_t gatt_op)
+{
+    switch (gatt_op) {
+    case BLE_GATT_ACCESS_OP_READ_DSC:
+        STATS_INC(ble_gatts_stats, dsc_reads);
+        break;
+
+    case BLE_GATT_ACCESS_OP_WRITE_DSC:
+        STATS_INC(ble_gatts_stats, dsc_writes);
+        break;
+
+    default:
+        break;
+    }
+}
+
+static int
+ble_gatts_dsc_access(uint16_t conn_handle, uint16_t attr_handle,
+                     uint8_t att_op, uint16_t offset, struct os_mbuf **om,
+                     void *arg)
+{
+    const struct ble_gatt_dsc_def *dsc_def;
+    struct ble_gatt_access_ctxt gatt_ctxt;
+    int rc;
+
+    dsc_def = arg;
+    BLE_HS_DBG_ASSERT(dsc_def != NULL && dsc_def->access_cb != NULL);
+
+    gatt_ctxt.op = ble_gatts_dsc_op(att_op);
+    gatt_ctxt.dsc = dsc_def;
+
+    ble_gatts_dsc_inc_stat(gatt_ctxt.op);
+    rc = ble_gatts_val_access(conn_handle, attr_handle, offset, &gatt_ctxt, om,
+                              dsc_def->access_cb, dsc_def->arg);
+
+    return rc;
+}
+
+static int
+ble_gatts_dsc_is_sane(const struct ble_gatt_dsc_def *dsc)
+{
+    if (dsc->uuid == NULL) {
+        return 0;
+    }
+
+    if (dsc->access_cb == NULL) {
+        return 0;
+    }
+
+    return 1;
+}
+
+static int
+ble_gatts_register_dsc(const struct ble_gatt_svc_def *svc,
+                       const struct ble_gatt_chr_def *chr,
+                       const struct ble_gatt_dsc_def *dsc,
+                       uint16_t chr_def_handle,
+                       ble_gatt_register_fn *register_cb, void *cb_arg)
+{
+    struct ble_gatt_register_ctxt register_ctxt;
+    uint16_t dsc_handle;
+    int rc;
+
+    if (!ble_gatts_dsc_is_sane(dsc)) {
+        return BLE_HS_EINVAL;
+    }
+
+    rc = ble_att_svr_register(dsc->uuid, dsc->att_flags, dsc->min_key_size,
+                              &dsc_handle, ble_gatts_dsc_access, (void *)dsc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (register_cb != NULL) {
+        register_ctxt.op = BLE_GATT_REGISTER_OP_DSC;
+        register_ctxt.dsc.handle = dsc_handle;
+        register_ctxt.dsc.svc_def = svc;
+        register_ctxt.dsc.chr_def = chr;
+        register_ctxt.dsc.dsc_def = dsc;
+        register_cb(&register_ctxt, cb_arg);
+    }
+
+    STATS_INC(ble_gatts_stats, dscs);
+
+    return 0;
+
+}
+
+static int
+ble_gatts_clt_cfg_find_idx(struct ble_gatts_clt_cfg *cfgs,
+                           uint16_t chr_val_handle)
+{
+    struct ble_gatts_clt_cfg *cfg;
+    int i;
+
+    for (i = 0; i < ble_gatts_num_cfgable_chrs; i++) {
+        cfg = cfgs + i;
+        if (cfg->chr_val_handle == chr_val_handle) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+static struct ble_gatts_clt_cfg *
+ble_gatts_clt_cfg_find(struct ble_gatts_clt_cfg *cfgs,
+                       uint16_t chr_val_handle)
+{
+    int idx;
+
+    idx = ble_gatts_clt_cfg_find_idx(cfgs, chr_val_handle);
+    if (idx == -1) {
+        return NULL;
+    } else {
+        return cfgs + idx;
+    }
+}
+
+static void
+ble_gatts_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
+                          uint8_t reason,
+                          uint8_t prev_flags, uint8_t cur_flags)
+{
+    if ((prev_flags ^ cur_flags) & ~BLE_GATTS_CLT_CFG_F_RESERVED) {
+        ble_gap_subscribe_event(conn_handle,
+                                attr_handle,
+                                reason,
+                                prev_flags  & BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                cur_flags   & BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                prev_flags  & BLE_GATTS_CLT_CFG_F_INDICATE,
+                                cur_flags   & BLE_GATTS_CLT_CFG_F_INDICATE);
+    }
+}
+
+/**
+ * Performs a read or write access on a client characteritic configuration
+ * descriptor (CCCD).
+ *
+ * @param conn                  The connection of the peer doing the accessing.
+ * @apram attr_handle           The handle of the CCCD.
+ * @param att_op                The ATT operation being performed (read or
+ *                                  write).
+ * @param ctxt                  Communication channel between this function and
+ *                                  the caller within the nimble stack.
+ *                                  Semantics depends on the operation being
+ *                                  performed.
+ * @param out_cccd              If the CCCD should be persisted as a result of
+ *                                  the access, the data-to-be-persisted gets
+ *                                  written here.  If no persistence is
+ *                                  necessary, out_cccd->chr_val_handle is set
+ *                                  to 0.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+static int
+ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle,
+                                uint8_t att_op, uint16_t offset,
+                                struct os_mbuf *om,
+                                struct ble_store_value_cccd *out_cccd,
+                                uint8_t *out_prev_clt_cfg_flags,
+                                uint8_t *out_cur_clt_cfg_flags)
+{
+    struct ble_gatts_clt_cfg *clt_cfg;
+    uint16_t chr_val_handle;
+    uint16_t flags;
+    uint8_t gatt_op;
+    uint8_t *buf;
+
+    /* Assume nothing needs to be persisted. */
+    out_cccd->chr_val_handle = 0;
+
+    /* We always register the client characteristics descriptor with handle
+     * (chr_val + 1).
+     */
+    chr_val_handle = attr_handle - 1;
+    if (chr_val_handle > attr_handle) {
+        /* Attribute handle wrapped somehow. */
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    clt_cfg = ble_gatts_clt_cfg_find(conn->bhc_gatt_svr.clt_cfgs,
+                                     chr_val_handle);
+    if (clt_cfg == NULL) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    /* Assume no change in flags. */
+    *out_prev_clt_cfg_flags = clt_cfg->flags;
+    *out_cur_clt_cfg_flags = clt_cfg->flags;
+
+    gatt_op = ble_gatts_dsc_op(att_op);
+    ble_gatts_dsc_inc_stat(gatt_op);
+
+    switch (gatt_op) {
+    case BLE_GATT_ACCESS_OP_READ_DSC:
+        STATS_INC(ble_gatts_stats, dsc_reads);
+        buf = os_mbuf_extend(om, 2);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        put_le16(buf, clt_cfg->flags & ~BLE_GATTS_CLT_CFG_F_RESERVED);
+        break;
+
+    case BLE_GATT_ACCESS_OP_WRITE_DSC:
+        STATS_INC(ble_gatts_stats, dsc_writes);
+        if (OS_MBUF_PKTLEN(om) != 2) {
+            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+        }
+
+        om = os_mbuf_pullup(om, 2);
+        BLE_HS_DBG_ASSERT(om != NULL);
+
+        flags = get_le16(om->om_data);
+        if ((flags & ~clt_cfg->allowed) != 0) {
+            return BLE_ATT_ERR_REQ_NOT_SUPPORTED;
+        }
+
+        if (clt_cfg->flags != flags) {
+            clt_cfg->flags = flags;
+            *out_cur_clt_cfg_flags = flags;
+
+            /* Successful writes get persisted for bonded connections. */
+            if (conn->bhc_sec_state.bonded) {
+                out_cccd->peer_addr = conn->bhc_peer_addr;
+                out_cccd->peer_addr.type =
+                    ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type);
+                out_cccd->chr_val_handle = chr_val_handle;
+                out_cccd->flags = clt_cfg->flags;
+                out_cccd->value_changed = 0;
+            }
+        }
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+int
+ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle,
+                         uint8_t op, uint16_t offset, struct os_mbuf **om,
+                         void *arg)
+{
+    struct ble_store_value_cccd cccd_value;
+    struct ble_store_key_cccd cccd_key;
+    struct ble_hs_conn *conn;
+    uint16_t chr_val_handle;
+    uint8_t prev_flags;
+    uint8_t cur_flags;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        rc = BLE_ATT_ERR_UNLIKELY;
+    } else {
+        rc = ble_gatts_clt_cfg_access_locked(conn, attr_handle, op, offset,
+                                             *om, &cccd_value, &prev_flags,
+                                             &cur_flags);
+    }
+
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* The value attribute is always immediately after the declaration. */
+    chr_val_handle = attr_handle - 1;
+
+    /* Tell the application if the peer changed its subscription state. */
+    ble_gatts_subscribe_event(conn_handle, chr_val_handle,
+                              BLE_GAP_SUBSCRIBE_REASON_WRITE,
+                              prev_flags, cur_flags);
+
+    /* Persist the CCCD if required. */
+    if (cccd_value.chr_val_handle != 0) {
+        if (cccd_value.flags == 0) {
+            ble_store_key_from_value_cccd(&cccd_key, &cccd_value);
+            rc = ble_store_delete_cccd(&cccd_key);
+        } else {
+            rc = ble_store_write_cccd(&cccd_value);
+        }
+    }
+
+    return rc;
+}
+
+static int
+ble_gatts_register_clt_cfg_dsc(uint16_t *att_handle)
+{
+    int rc;
+
+    rc = ble_att_svr_register(uuid_ccc, BLE_ATT_F_READ | BLE_ATT_F_WRITE, 0,
+                              att_handle, ble_gatts_clt_cfg_access, NULL);
+    if (rc != 0) {
+        return rc;
+    }
+
+    STATS_INC(ble_gatts_stats, dscs);
+
+    return 0;
+}
+
+static int
+ble_gatts_register_chr(const struct ble_gatt_svc_def *svc,
+                       const struct ble_gatt_chr_def *chr,
+                       ble_gatt_register_fn *register_cb, void *cb_arg)
+{
+    struct ble_gatt_register_ctxt register_ctxt;
+    struct ble_gatt_dsc_def *dsc;
+    uint16_t def_handle;
+    uint16_t val_handle;
+    uint16_t dsc_handle;
+    uint8_t att_flags;
+    int rc;
+
+    if (!ble_gatts_chr_is_sane(chr)) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (ble_gatts_chr_clt_cfg_allowed(chr) != 0) {
+        if (ble_gatts_num_cfgable_chrs > ble_hs_max_client_configs) {
+            return BLE_HS_ENOMEM;
+        }
+        ble_gatts_num_cfgable_chrs++;
+    }
+
+    /* Register characteristic definition attribute (cast away const on
+     * callback arg).
+     */
+    rc = ble_att_svr_register(uuid_chr, BLE_ATT_F_READ, 0, &def_handle,
+                              ble_gatts_chr_def_access, (void *)chr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Register characteristic value attribute (cast away const on callback
+     * arg).
+     */
+    att_flags = ble_gatts_att_flags_from_chr_flags(chr->flags);
+    rc = ble_att_svr_register(chr->uuid, att_flags, chr->min_key_size,
+                              &val_handle, ble_gatts_chr_val_access,
+                              (void *)chr);
+    if (rc != 0) {
+        return rc;
+    }
+    BLE_HS_DBG_ASSERT(val_handle == def_handle + 1);
+
+    if (chr->val_handle != NULL) {
+        *chr->val_handle = val_handle;
+    }
+
+    if (register_cb != NULL) {
+        register_ctxt.op = BLE_GATT_REGISTER_OP_CHR;
+        register_ctxt.chr.def_handle = def_handle;
+        register_ctxt.chr.val_handle = val_handle;
+        register_ctxt.chr.svc_def = svc;
+        register_ctxt.chr.chr_def = chr;
+        register_cb(&register_ctxt, cb_arg);
+    }
+
+    if (ble_gatts_chr_clt_cfg_allowed(chr) != 0) {
+        rc = ble_gatts_register_clt_cfg_dsc(&dsc_handle);
+        if (rc != 0) {
+            return rc;
+        }
+        BLE_HS_DBG_ASSERT(dsc_handle == def_handle + 2);
+    }
+
+    /* Register each descriptor. */
+    if (chr->descriptors != NULL) {
+        for (dsc = chr->descriptors; dsc->uuid != NULL; dsc++) {
+            rc = ble_gatts_register_dsc(svc, chr, dsc, def_handle, register_cb,
+                                        cb_arg);
+            if (rc != 0) {
+                return rc;
+            }
+        }
+    }
+
+    STATS_INC(ble_gatts_stats, chrs);
+
+    return 0;
+}
+
+static int
+ble_gatts_svc_type_to_uuid(uint8_t svc_type, const ble_uuid_t **uuid)
+{
+    switch (svc_type) {
+    case BLE_GATT_SVC_TYPE_PRIMARY:
+        *uuid = uuid_pri;
+        return 0;
+
+    case BLE_GATT_SVC_TYPE_SECONDARY:
+        *uuid = uuid_sec;
+        return 0;
+
+    default:
+        return BLE_HS_EINVAL;
+    }
+}
+
+static int
+ble_gatts_svc_is_sane(const struct ble_gatt_svc_def *svc)
+{
+    if (svc->type != BLE_GATT_SVC_TYPE_PRIMARY &&
+        svc->type != BLE_GATT_SVC_TYPE_SECONDARY) {
+
+        return 0;
+    }
+
+    if (svc->uuid == NULL) {
+        return 0;
+    }
+
+    return 1;
+}
+
+static int
+ble_gatts_register_svc(const struct ble_gatt_svc_def *svc,
+                       uint16_t *out_handle,
+                       ble_gatt_register_fn *register_cb, void *cb_arg)
+{
+    const struct ble_gatt_chr_def *chr;
+    struct ble_gatt_register_ctxt register_ctxt;
+    const ble_uuid_t *uuid;
+    int idx;
+    int rc;
+    int i;
+
+    if (!ble_gatts_svc_incs_satisfied(svc)) {
+        return BLE_HS_EAGAIN;
+    }
+
+    if (!ble_gatts_svc_is_sane(svc)) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* Prevent spurious maybe-uninitialized gcc warning. */
+    uuid = NULL;
+
+    rc = ble_gatts_svc_type_to_uuid(svc->type, &uuid);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    /* Register service definition attribute (cast away const on callback
+     * arg).
+     */
+    rc = ble_att_svr_register(uuid, BLE_ATT_F_READ, 0, out_handle,
+                              ble_gatts_svc_access, (void *)svc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (register_cb != NULL) {
+        register_ctxt.op = BLE_GATT_REGISTER_OP_SVC;
+        register_ctxt.svc.handle = *out_handle;
+        register_ctxt.svc.svc_def = svc;
+        register_cb(&register_ctxt, cb_arg);
+    }
+
+    /* Register each include. */
+    if (svc->includes != NULL) {
+        for (i = 0; svc->includes[i] != NULL; i++) {
+            idx = ble_gatts_find_svc_entry_idx(svc->includes[i]);
+            BLE_HS_DBG_ASSERT_EVAL(idx != -1);
+
+            rc = ble_gatts_register_inc(ble_gatts_svc_entries + idx);
+            if (rc != 0) {
+                return rc;
+            }
+        }
+    }
+
+    /* Register each characteristic. */
+    if (svc->characteristics != NULL) {
+        for (chr = svc->characteristics; chr->uuid != NULL; chr++) {
+            rc = ble_gatts_register_chr(svc, chr, register_cb, cb_arg);
+            if (rc != 0) {
+                return rc;
+            }
+        }
+    }
+
+    STATS_INC(ble_gatts_stats, svcs);
+
+    return 0;
+}
+
+static int
+ble_gatts_register_round(int *out_num_registered, ble_gatt_register_fn *cb,
+                         void *cb_arg)
+{
+    struct ble_gatts_svc_entry *entry;
+    uint16_t handle;
+    int rc;
+    int i;
+
+    *out_num_registered = 0;
+    for (i = 0; i < ble_gatts_num_svc_entries; i++) {
+        entry = ble_gatts_svc_entries + i;
+
+        if (entry->handle == 0) {
+            rc = ble_gatts_register_svc(entry->svc, &handle, cb, cb_arg);
+            switch (rc) {
+            case 0:
+                /* Service successfully registered. */
+                entry->handle = handle;
+                entry->end_group_handle = ble_att_svr_prev_handle();
+                (*out_num_registered)++;
+                break;
+
+            case BLE_HS_EAGAIN:
+                /* Service could not be registered due to unsatisfied includes.
+                 * Try again on the next iteration.
+                 */
+                break;
+
+            default:
+                return rc;
+            }
+        }
+    }
+
+    if (*out_num_registered == 0) {
+        /* There is a circular dependency. */
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+/**
+ * Registers a set of services, characteristics, and descriptors to be accessed
+ * by GATT clients.
+ *
+ * @param svcs                  A table of the service definitions to be
+ *                                  registered.
+ * @param cb                    The function to call for each service,
+ *                                  characteristic, and descriptor that gets
+ *                                  registered.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOMEM if registration failed due to
+ *                                  resource exhaustion;
+ *                              BLE_HS_EINVAL if the service definition table
+ *                                  contains an invalid element.
+ */
+int
+ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
+                        ble_gatt_register_fn *cb, void *cb_arg)
+{
+    int total_registered;
+    int cur_registered;
+    int num_svcs;
+    int idx;
+    int rc;
+    int i;
+
+    for (i = 0; svcs[i].type != BLE_GATT_SVC_TYPE_END; i++) {
+        idx = ble_gatts_num_svc_entries + i;
+        if (idx >= ble_hs_max_services) {
+            return BLE_HS_ENOMEM;
+        }
+
+        ble_gatts_svc_entries[idx].svc = svcs + i;
+        ble_gatts_svc_entries[idx].handle = 0;
+        ble_gatts_svc_entries[idx].end_group_handle = 0xffff;
+    }
+    num_svcs = i;
+    ble_gatts_num_svc_entries += num_svcs;
+
+    total_registered = 0;
+    while (total_registered < num_svcs) {
+        rc = ble_gatts_register_round(&cur_registered, cb, cb_arg);
+        if (rc != 0) {
+            return rc;
+        }
+        total_registered += cur_registered;
+    }
+
+    return 0;
+}
+
+static int
+ble_gatts_clt_cfg_size(void)
+{
+    return ble_gatts_num_cfgable_chrs * sizeof (struct ble_gatts_clt_cfg);
+}
+
+/**
+ * Handles GATT server clean up for a terminated connection:
+ *     o Informs the application that the peer is no longer subscribed to any
+ *       characteristic updates.
+ *     o Frees GATT server resources consumed by the connection (CCCDs).
+ */
+void
+ble_gatts_connection_broken(uint16_t conn_handle)
+{
+    struct ble_gatts_clt_cfg *clt_cfgs;
+    struct ble_hs_conn *conn;
+    int num_clt_cfgs;
+    int rc;
+    int i;
+
+    /* Find the specified connection and extract its CCCD entries.  Extracting
+     * the clt_cfg pointer and setting the original to null is done for two
+     * reasons:
+     *     1. So that the CCCD entries can be safely processed after unlocking
+     *        the mutex.
+     *     2. To ensure a subsequent indicate procedure for this peer is not
+     *        attempted, as the connection is about to be terminated.  This
+     *        avoids a spurious notify-tx GAP event callback to the
+     *        application.  By setting the clt_cfg pointer to null, it is
+     *        assured that the connection has no pending indications to send.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        clt_cfgs = conn->bhc_gatt_svr.clt_cfgs;
+        num_clt_cfgs = conn->bhc_gatt_svr.num_clt_cfgs;
+
+        conn->bhc_gatt_svr.clt_cfgs = NULL;
+        conn->bhc_gatt_svr.num_clt_cfgs = 0;
+    }
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return;
+    }
+
+    /* If there is an indicate procedure in progress for this connection,
+     * inform the application that it has failed.
+     */
+    ble_gatts_indicate_fail_notconn(conn_handle);
+
+    /* Now that the mutex is unlocked, inform the application that the peer is
+     * no longer subscribed to any characteristic updates.
+     */
+    if (clt_cfgs != NULL) {
+        for (i = 0; i < num_clt_cfgs; i++) {
+            ble_gatts_subscribe_event(conn_handle, clt_cfgs[i].chr_val_handle,
+                                      BLE_GAP_SUBSCRIBE_REASON_TERM,
+                                      clt_cfgs[i].flags, 0);
+        }
+
+        rc = os_memblock_put(&ble_gatts_clt_cfg_pool, clt_cfgs);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static void
+ble_gatts_free_svc_defs(void)
+{
+    free(ble_gatts_svc_defs);
+    ble_gatts_svc_defs = NULL;
+    ble_gatts_num_svc_defs = 0;
+}
+
+static void
+ble_gatts_free_mem(void)
+{
+    free(ble_gatts_clt_cfg_mem);
+    ble_gatts_clt_cfg_mem = NULL;
+
+    free(ble_gatts_svc_entries);
+    ble_gatts_svc_entries = NULL;
+}
+
+int
+ble_gatts_start(void)
+{
+    struct ble_att_svr_entry *ha;
+    struct ble_gatt_chr_def *chr;
+    uint16_t allowed_flags;
+    ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
+    int num_elems;
+    int idx;
+    int rc;
+    int i;
+
+    ble_hs_lock();
+    if (!ble_gatts_mutable()) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
+
+    ble_gatts_free_mem();
+
+    rc = ble_att_svr_start();
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (ble_hs_max_client_configs > 0) {
+        ble_gatts_clt_cfg_mem = malloc(
+            OS_MEMPOOL_BYTES(ble_hs_max_client_configs,
+                             sizeof (struct ble_gatts_clt_cfg)));
+        if (ble_gatts_clt_cfg_mem == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto done;
+        }
+    }
+
+    if (ble_hs_max_services > 0) {
+        ble_gatts_svc_entries =
+            malloc(ble_hs_max_services * sizeof *ble_gatts_svc_entries);
+        if (ble_gatts_svc_entries == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto done;
+        }
+    }
+
+
+    ble_gatts_num_svc_entries = 0;
+    for (i = 0; i < ble_gatts_num_svc_defs; i++) {
+        rc = ble_gatts_register_svcs(ble_gatts_svc_defs[i],
+                                     ble_hs_cfg.gatts_register_cb,
+                                     ble_hs_cfg.gatts_register_arg);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+    ble_gatts_free_svc_defs();
+
+    if (ble_gatts_num_cfgable_chrs == 0) {
+        rc = 0;
+        goto done;
+    }
+
+    /* Initialize client-configuration memory pool. */
+    num_elems = ble_hs_max_client_configs / ble_gatts_num_cfgable_chrs;
+    rc = os_mempool_init(&ble_gatts_clt_cfg_pool, num_elems,
+                         ble_gatts_clt_cfg_size(), ble_gatts_clt_cfg_mem,
+                         "ble_gatts_clt_cfg_pool");
+    if (rc != 0) {
+        rc = BLE_HS_EOS;
+        goto done;
+    }
+
+    /* Allocate the cached array of handles for the configuration
+     * characteristics.
+     */
+    ble_gatts_clt_cfgs = os_memblock_get(&ble_gatts_clt_cfg_pool);
+    if (ble_gatts_clt_cfgs == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    /* Fill the cache. */
+    idx = 0;
+    ha = NULL;
+    while ((ha = ble_att_svr_find_by_uuid(ha, &uuid.u, 0xffff)) != NULL) {
+        chr = ha->ha_cb_arg;
+        allowed_flags = ble_gatts_chr_clt_cfg_allowed(chr);
+        if (allowed_flags != 0) {
+            BLE_HS_DBG_ASSERT_EVAL(idx < ble_gatts_num_cfgable_chrs);
+
+            ble_gatts_clt_cfgs[idx].chr_val_handle = ha->ha_handle_id + 1;
+            ble_gatts_clt_cfgs[idx].allowed = allowed_flags;
+            ble_gatts_clt_cfgs[idx].flags = 0;
+            idx++;
+        }
+    }
+
+done:
+    if (rc != 0) {
+        ble_gatts_free_mem();
+        ble_gatts_free_svc_defs();
+    }
+
+    ble_hs_unlock();
+    return rc;
+}
+
+int
+ble_gatts_conn_can_alloc(void)
+{
+    return ble_gatts_num_cfgable_chrs == 0 ||
+           ble_gatts_clt_cfg_pool.mp_num_free > 0;
+}
+
+int
+ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn)
+{
+    if (ble_gatts_num_cfgable_chrs > 0) {
+        gatts_conn->clt_cfgs = os_memblock_get(&ble_gatts_clt_cfg_pool);
+        if (gatts_conn->clt_cfgs == NULL) {
+            return BLE_HS_ENOMEM;
+        }
+
+        /* Initialize the client configuration with a copy of the cache. */
+        memcpy(gatts_conn->clt_cfgs, ble_gatts_clt_cfgs,
+               ble_gatts_clt_cfg_size());
+        gatts_conn->num_clt_cfgs = ble_gatts_num_cfgable_chrs;
+    } else {
+        gatts_conn->clt_cfgs = NULL;
+        gatts_conn->num_clt_cfgs = 0;
+    }
+
+    return 0;
+}
+
+
+/**
+ * Schedules a notification or indication for the specified peer-CCCD pair.  If
+ * the update should be sent immediately, it is indicated in the return code.
+ *
+ * @param conn                  The connection to schedule the update for.
+ * @param clt_cfg               The client config entry corresponding to the
+ *                                  peer and affected characteristic.
+ *
+ * @return                      The att_op of the update to send immediately,
+ *                                  if any.  0 if nothing should get sent.
+ */
+static uint8_t
+ble_gatts_schedule_update(struct ble_hs_conn *conn,
+                          struct ble_gatts_clt_cfg *clt_cfg)
+{
+    uint8_t att_op;
+
+    if (!(clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED)) {
+        /* Characteristic not modified.  Nothing to send. */
+        att_op = 0;
+    } else if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_NOTIFY) {
+        /* Notifications always get sent immediately. */
+        att_op = BLE_ATT_OP_NOTIFY_REQ;
+    } else if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_INDICATE) {
+        /* Only one outstanding indication per peer is allowed.  If we
+         * are still awaiting an ack, mark this CCCD as updated so that
+         * we know to send the indication upon receiving the expected ack.
+         * If there isn't an outstanding indication, send this one now.
+         */
+        if (conn->bhc_gatt_svr.indicate_val_handle != 0) {
+            att_op = 0;
+        } else {
+            att_op = BLE_ATT_OP_INDICATE_REQ;
+        }
+    } else {
+        /* Peer isn't subscribed to notifications or indications.  Nothing to
+         * send.
+         */
+        att_op = 0;
+    }
+
+    /* If we will be sending an update, clear the modified flag so that we
+     * don't double-send.
+     */
+    if (att_op != 0) {
+        clt_cfg->flags &= ~BLE_GATTS_CLT_CFG_F_MODIFIED;
+    }
+
+    return att_op;
+}
+
+int
+ble_gatts_send_next_indicate(uint16_t conn_handle)
+{
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_hs_conn *conn;
+    uint16_t chr_val_handle;
+    int rc;
+    int i;
+
+    /* Assume no pending indications. */
+    chr_val_handle = 0;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        for (i = 0; i < conn->bhc_gatt_svr.num_clt_cfgs; i++) {
+            clt_cfg = conn->bhc_gatt_svr.clt_cfgs + i;
+            if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED) {
+                BLE_HS_DBG_ASSERT(clt_cfg->flags &
+                                  BLE_GATTS_CLT_CFG_F_INDICATE);
+
+                chr_val_handle = clt_cfg->chr_val_handle;
+
+                /* Clear pending flag in anticipation of indication tx. */
+                clt_cfg->flags &= ~BLE_GATTS_CLT_CFG_F_MODIFIED;
+                break;
+            }
+        }
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    if (chr_val_handle == 0) {
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_gattc_indicate(conn_handle, chr_val_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle)
+{
+    struct ble_store_value_cccd cccd_value;
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_hs_conn *conn;
+    int clt_cfg_idx;
+    int persist;
+    int rc;
+
+    clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs,
+                                             chr_val_handle);
+    if (clt_cfg_idx == -1) {
+        /* This characteristic does not have a CCCD. */
+        return BLE_HS_ENOENT;
+    }
+
+    clt_cfg = ble_gatts_clt_cfgs + clt_cfg_idx;
+    if (!(clt_cfg->allowed & BLE_GATTS_CLT_CFG_F_INDICATE)) {
+        /* This characteristic does not allow indications. */
+        return BLE_HS_ENOENT;
+    }
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+    if (conn->bhc_gatt_svr.indicate_val_handle == chr_val_handle) {
+        /* This acknowledgement is expected. */
+        rc = 0;
+
+        /* Mark that there is no longer an outstanding txed indicate. */
+        conn->bhc_gatt_svr.indicate_val_handle = 0;
+
+        /* Determine if we need to persist that there is no pending indication
+         * for this peer-characteristic pair.  If the characteristic has not
+         * been modified since we sent the indication, there is no indication
+         * pending.
+         */
+        BLE_HS_DBG_ASSERT(conn->bhc_gatt_svr.num_clt_cfgs > clt_cfg_idx);
+        clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx;
+        BLE_HS_DBG_ASSERT(clt_cfg->chr_val_handle == chr_val_handle);
+
+        persist = conn->bhc_sec_state.bonded &&
+                  !(clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED);
+        if (persist) {
+            cccd_value.peer_addr = conn->bhc_peer_addr;
+            cccd_value.peer_addr.type =
+                ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type);
+            cccd_value.chr_val_handle = chr_val_handle;
+            cccd_value.flags = clt_cfg->flags;
+            cccd_value.value_changed = 0;
+        }
+    } else {
+        /* This acknowledgement doesn't correspond to the outstanding
+         * indication; ignore it.
+         */
+        rc = BLE_HS_ENOENT;
+    }
+
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (persist) {
+        rc = ble_store_write_cccd(&cccd_value);
+        if (rc != 0) {
+            /* XXX: How should this error get reported? */
+        }
+    }
+
+    return 0;
+}
+
+void
+ble_gatts_chr_updated(uint16_t chr_val_handle)
+{
+    struct ble_store_value_cccd cccd_value;
+    struct ble_store_key_cccd cccd_key;
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_hs_conn *conn;
+    int new_notifications = 0;
+    int clt_cfg_idx;
+    int persist;
+    int rc;
+    int i;
+
+    /* Determine if notifications or indications are allowed for this
+     * characteristic.  If not, return immediately.
+     */
+    clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs,
+                                             chr_val_handle);
+    if (clt_cfg_idx == -1) {
+        return;
+    }
+
+    /*** Send notifications and indications to connected devices. */
+
+    ble_hs_lock();
+    for (i = 0; ; i++) {
+        /* XXX: This is inefficient when there are a lot of connections.
+         * Consider using a "foreach" function to walk the connection list.
+         */
+        conn = ble_hs_conn_find_by_idx(i);
+        if (conn == NULL) {
+            break;
+        }
+
+        BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
+                               clt_cfg_idx);
+        clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx;
+        BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle);
+
+        /* Mark the CCCD entry as modified. */
+        clt_cfg->flags |= BLE_GATTS_CLT_CFG_F_MODIFIED;
+        new_notifications = 1;
+    }
+    ble_hs_unlock();
+
+    if (new_notifications) {
+        ble_hs_notifications_sched();
+    }
+
+    /*** Persist updated flag for unconnected and not-yet-bonded devices. */
+
+    /* Retrieve each record corresponding to the modified characteristic. */
+    cccd_key.peer_addr = *BLE_ADDR_ANY;
+    cccd_key.chr_val_handle = chr_val_handle;
+    cccd_key.idx = 0;
+
+    while (1) {
+        rc = ble_store_read_cccd(&cccd_key, &cccd_value);
+        if (rc != 0) {
+            /* Read error or no more CCCD records. */
+            break;
+        }
+
+        /* Determine if this record needs to be rewritten. */
+        ble_hs_lock();
+        conn = ble_hs_conn_find_by_addr(&cccd_key.peer_addr);
+
+        if (conn == NULL) {
+            /* Device isn't connected; persist the changed flag so that an
+             * update can be sent when the device reconnects and rebonds.
+             */
+            persist = 1;
+        } else if (cccd_value.flags & BLE_GATTS_CLT_CFG_F_INDICATE) {
+            /* Indication for a connected device; record that the
+             * characteristic has changed until we receive the ack.
+             */
+            persist = 1;
+        } else {
+            /* Notification for a connected device; we already sent it so there
+             * is no need to persist.
+             */
+            persist = 0;
+        }
+
+        ble_hs_unlock();
+
+        /* Only persist if the value changed flag wasn't already sent (i.e.,
+         * don't overwrite with identical data).
+         */
+        if (persist && !cccd_value.value_changed) {
+            cccd_value.value_changed = 1;
+            ble_store_write_cccd(&cccd_value);
+        }
+
+        /* Read the next matching record. */
+        cccd_key.idx++;
+    }
+}
+
+/**
+ * Sends notifications or indications for the specified characteristic to all
+ * connected devices.  The bluetooth spec does not allow more than one
+ * concurrent indication for a single peer, so this function will hold off on
+ * sending such indications.
+ */
+static void
+ble_gatts_tx_notifications_one_chr(uint16_t chr_val_handle)
+{
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+    uint8_t att_op;
+    int clt_cfg_idx;
+    int i;
+
+    /* Determine if notifications / indications are enabled for this
+     * characteristic.
+     */
+    clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs,
+                                             chr_val_handle);
+    if (clt_cfg_idx == -1) {
+        return;
+    }
+
+    for (i = 0; ; i++) {
+        ble_hs_lock();
+
+        conn = ble_hs_conn_find_by_idx(i);
+        if (conn != NULL) {
+            BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
+                                   clt_cfg_idx);
+            clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx;
+            BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle);
+
+            /* Determine what type of command should get sent, if any. */
+            att_op = ble_gatts_schedule_update(conn, clt_cfg);
+            conn_handle = conn->bhc_handle;
+        } else {
+            /* Silence some spurious gcc warnings. */
+            att_op = 0;
+            conn_handle = BLE_HS_CONN_HANDLE_NONE;
+        }
+        ble_hs_unlock();
+
+        if (conn == NULL) {
+            /* No more connected devices. */
+            break;
+        }
+
+        switch (att_op) {
+        case 0:
+            break;
+
+        case BLE_ATT_OP_NOTIFY_REQ:
+            ble_gattc_notify(conn_handle, chr_val_handle);
+            break;
+
+        case BLE_ATT_OP_INDICATE_REQ:
+            ble_gattc_indicate(conn_handle, chr_val_handle);
+            break;
+
+        default:
+            BLE_HS_DBG_ASSERT(0);
+            break;
+        }
+    }
+}
+
+/**
+ * Sends all pending notifications and indications.  The bluetooth spec does
+ * not allow more than one concurrent indication for a single peer, so this
+ * function will hold off on sending such indications.
+ */
+void
+ble_gatts_tx_notifications(void)
+{
+    uint16_t chr_val_handle;
+    int i;
+
+    for (i = 0; i < ble_gatts_num_cfgable_chrs; i++) {
+        chr_val_handle = ble_gatts_clt_cfgs[i].chr_val_handle;
+        ble_gatts_tx_notifications_one_chr(chr_val_handle);
+    }
+}
+
+void
+ble_gatts_bonding_established(uint16_t conn_handle)
+{
+    struct ble_store_value_cccd cccd_value;
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_gatts_conn *gatt_srv;
+    struct ble_hs_conn *conn;
+    int i;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+    BLE_HS_DBG_ASSERT(conn->bhc_sec_state.bonded);
+
+    cccd_value.peer_addr = conn->bhc_peer_addr;
+    cccd_value.peer_addr.type =
+        ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type);
+    gatt_srv = &conn->bhc_gatt_svr;
+
+    for (i = 0; i < gatt_srv->num_clt_cfgs; ++i) {
+        clt_cfg = &gatt_srv->clt_cfgs[i];
+
+        if (clt_cfg->flags != 0) {
+            cccd_value.chr_val_handle = clt_cfg->chr_val_handle;
+            cccd_value.flags = clt_cfg->flags;
+            cccd_value.value_changed = 0;
+
+            /* Store write use ble_hs_lock */
+            ble_hs_unlock();
+            ble_store_write_cccd(&cccd_value);
+            ble_hs_lock();
+
+            conn = ble_hs_conn_find(conn_handle);
+            BLE_HS_DBG_ASSERT(conn != NULL);
+        }
+    }
+
+    ble_hs_unlock();
+}
+
+/**
+ * Called when bonding has been restored via the encryption procedure.  This
+ * function:
+ *     o Restores persisted CCCD entries for the connected peer.
+ *     o Sends all pending notifications to the connected peer.
+ *     o Sends up to one pending indication to the connected peer; schedules
+ *       any remaining pending indications.
+ */
+void
+ble_gatts_bonding_restored(uint16_t conn_handle)
+{
+    struct ble_store_value_cccd cccd_value;
+    struct ble_store_key_cccd cccd_key;
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_hs_conn *conn;
+    uint8_t att_op;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+    BLE_HS_DBG_ASSERT(conn->bhc_sec_state.bonded);
+
+    cccd_key.peer_addr = conn->bhc_peer_addr;
+    cccd_key.peer_addr.type =
+        ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type);
+    cccd_key.chr_val_handle = 0;
+    cccd_key.idx = 0;
+
+    ble_hs_unlock();
+
+    while (1) {
+        rc = ble_store_read_cccd(&cccd_key, &cccd_value);
+        if (rc != 0) {
+            break;
+        }
+
+        /* Assume no notification or indication will get sent. */
+        att_op = 0;
+
+        ble_hs_lock();
+
+        conn = ble_hs_conn_find(conn_handle);
+        BLE_HS_DBG_ASSERT(conn != NULL);
+
+        clt_cfg = ble_gatts_clt_cfg_find(conn->bhc_gatt_svr.clt_cfgs,
+                                         cccd_value.chr_val_handle);
+        if (clt_cfg != NULL) {
+            clt_cfg->flags = cccd_value.flags;
+
+            if (cccd_value.value_changed) {
+                /* The characteristic's value changed while the device was
+                 * disconnected or unbonded.  Schedule the notification or
+                 * indication now.
+                 */
+                clt_cfg->flags |= BLE_GATTS_CLT_CFG_F_MODIFIED;
+                att_op = ble_gatts_schedule_update(conn, clt_cfg);
+            }
+        }
+
+        ble_hs_unlock();
+
+        /* Tell the application if the peer changed its subscription state
+         * when it was restored from persistence.
+         */
+        ble_gatts_subscribe_event(conn_handle, cccd_value.chr_val_handle,
+                                  BLE_GAP_SUBSCRIBE_REASON_RESTORE,
+                                  0, cccd_value.flags);
+
+        switch (att_op) {
+        case 0:
+            break;
+
+        case BLE_ATT_OP_NOTIFY_REQ:
+            rc = ble_gattc_notify(conn_handle, cccd_value.chr_val_handle);
+            if (rc == 0) {
+                cccd_value.value_changed = 0;
+                ble_store_write_cccd(&cccd_value);
+            }
+            break;
+
+        case BLE_ATT_OP_INDICATE_REQ:
+            ble_gattc_indicate(conn_handle, cccd_value.chr_val_handle);
+            break;
+
+        default:
+            BLE_HS_DBG_ASSERT(0);
+            break;
+        }
+
+        cccd_key.idx++;
+    }
+}
+
+static struct ble_gatts_svc_entry *
+ble_gatts_find_svc_entry(const ble_uuid_t *uuid)
+{
+    struct ble_gatts_svc_entry *entry;
+    int i;
+
+    for (i = 0; i < ble_gatts_num_svc_entries; i++) {
+        entry = ble_gatts_svc_entries + i;
+        if (ble_uuid_cmp(uuid, entry->svc->uuid) == 0) {
+            return entry;
+        }
+    }
+
+    return NULL;
+}
+
+static int
+ble_gatts_find_svc_chr_attr(const ble_uuid_t *svc_uuid,
+                            const ble_uuid_t *chr_uuid,
+                            struct ble_gatts_svc_entry **out_svc_entry,
+                            struct ble_att_svr_entry **out_att_chr)
+{
+    struct ble_gatts_svc_entry *svc_entry;
+    struct ble_att_svr_entry *att_svc;
+    struct ble_att_svr_entry *next;
+    struct ble_att_svr_entry *cur;
+
+    svc_entry = ble_gatts_find_svc_entry(svc_uuid);
+    if (svc_entry == NULL) {
+        return BLE_HS_ENOENT;
+    }
+
+    att_svc = ble_att_svr_find_by_handle(svc_entry->handle);
+    if (att_svc == NULL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    cur = STAILQ_NEXT(att_svc, ha_next);
+    while (1) {
+        if (cur == NULL) {
+            /* Reached end of attribute list without a match. */
+            return BLE_HS_ENOENT;
+        }
+        next = STAILQ_NEXT(cur, ha_next);
+
+        if (cur->ha_handle_id == svc_entry->end_group_handle) {
+            /* Reached end of service without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        if (ble_uuid_u16(cur->ha_uuid) == BLE_ATT_UUID_CHARACTERISTIC &&
+            next != NULL &&
+            ble_uuid_cmp(next->ha_uuid, chr_uuid) == 0) {
+
+            if (out_svc_entry != NULL) {
+                *out_svc_entry = svc_entry;
+            }
+            if (out_att_chr != NULL) {
+                *out_att_chr = next;
+            }
+            return 0;
+        }
+
+        cur = next;
+    }
+}
+
+int
+ble_gatts_find_svc(const ble_uuid_t *uuid, uint16_t *out_handle)
+{
+    struct ble_gatts_svc_entry *entry;
+
+    entry = ble_gatts_find_svc_entry(uuid);
+    if (entry == NULL) {
+        return BLE_HS_ENOENT;
+    }
+
+    if (out_handle != NULL) {
+        *out_handle = entry->handle;
+    }
+    return 0;
+}
+
+int
+ble_gatts_find_chr(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
+                   uint16_t *out_def_handle, uint16_t *out_val_handle)
+{
+    struct ble_att_svr_entry *att_chr;
+    int rc;
+
+    rc = ble_gatts_find_svc_chr_attr(svc_uuid, chr_uuid, NULL, &att_chr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (out_def_handle) {
+        *out_def_handle = att_chr->ha_handle_id - 1;
+    }
+    if (out_val_handle) {
+        *out_val_handle = att_chr->ha_handle_id;
+    }
+    return 0;
+}
+
+int
+ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
+                   const ble_uuid_t *dsc_uuid, uint16_t *out_handle)
+{
+    struct ble_gatts_svc_entry *svc_entry;
+    struct ble_att_svr_entry *att_chr;
+    struct ble_att_svr_entry *cur;
+    uint16_t uuid16;
+    int rc;
+
+    rc = ble_gatts_find_svc_chr_attr(svc_uuid, chr_uuid, &svc_entry,
+                                     &att_chr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    cur = STAILQ_NEXT(att_chr, ha_next);
+    while (1) {
+        if (cur == NULL) {
+            /* Reached end of attribute list without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        if (cur->ha_handle_id > svc_entry->end_group_handle) {
+            /* Reached end of service without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        uuid16 = ble_uuid_u16(cur->ha_uuid);
+        if (uuid16 == BLE_ATT_UUID_CHARACTERISTIC) {
+            /* Reached end of characteristic without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        if (ble_uuid_cmp(cur->ha_uuid, dsc_uuid) == 0) {
+            if (out_handle != NULL) {
+                *out_handle = cur->ha_handle_id;
+                return 0;
+            }
+        }
+        cur = STAILQ_NEXT(cur, ha_next);
+    }
+}
+
+int
+ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs)
+{
+    void *p;
+    int rc;
+
+    ble_hs_lock();
+    if (!ble_gatts_mutable()) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
+
+    p = realloc(ble_gatts_svc_defs,
+                (ble_gatts_num_svc_defs + 1) * sizeof *ble_gatts_svc_defs);
+    if (p == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    ble_gatts_svc_defs = p;
+    ble_gatts_svc_defs[ble_gatts_num_svc_defs] = svcs;
+    ble_gatts_num_svc_defs++;
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+    return rc;
+}
+
+int
+ble_gatts_svc_set_visibility(uint16_t handle, int visible)
+{
+    int i;
+
+    for (i = 0; i < ble_gatts_num_svc_entries; i++) {
+        struct ble_gatts_svc_entry *entry = &ble_gatts_svc_entries[i];
+
+        if (entry->handle == handle) {
+            if (visible) {
+                ble_att_svr_restore_range(entry->handle, entry->end_group_handle);
+            } else {
+                ble_att_svr_hide_range(entry->handle, entry->end_group_handle);
+            }
+            return 0;
+        }
+    }
+
+    return BLE_HS_ENOENT;
+}
+
+/**
+ * Accumulates counts of each resource type required by the specified service
+ * definition array.  This function is generally used to calculate some host
+ * configuration values prior to initialization.  This function adds the counts
+ * to the appropriate fields in the supplied ble_gatt_resources object without
+ * clearing them first, so it can be called repeatedly with different inputs to
+ * calculate totals.  Be sure to zero the resource struct prior to the first
+ * call to this function.
+ *
+ * @param svcs                  The service array containing the resource
+ *                                  definitions to be counted.
+ * @param res                   The resource counts are accumulated in this
+ *                                  struct.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the svcs array contains an
+ *                                  invalid resource definition.
+ */
+static int
+ble_gatts_count_resources(const struct ble_gatt_svc_def *svcs,
+                          struct ble_gatt_resources *res)
+{
+    const struct ble_gatt_svc_def *svc;
+    const struct ble_gatt_chr_def *chr;
+    int s;
+    int i;
+    int c;
+    int d;
+
+    for (s = 0; svcs[s].type != BLE_GATT_SVC_TYPE_END; s++) {
+        svc = svcs + s;
+
+        if (!ble_gatts_svc_is_sane(svc)) {
+            BLE_HS_DBG_ASSERT(0);
+            return BLE_HS_EINVAL;
+        }
+
+        /* Each service requires:
+         *     o 1 service
+         *     o 1 attribute
+         */
+        res->svcs++;
+        res->attrs++;
+
+        if (svc->includes != NULL) {
+            for (i = 0; svc->includes[i] != NULL; i++) {
+                /* Each include requires:
+                 *     o 1 include
+                 *     o 1 attribute
+                 */
+                res->incs++;
+                res->attrs++;
+            }
+        }
+
+        if (svc->characteristics != NULL) {
+            for (c = 0; svc->characteristics[c].uuid != NULL; c++) {
+                chr = svc->characteristics + c;
+
+                if (!ble_gatts_chr_is_sane(chr)) {
+                    BLE_HS_DBG_ASSERT(0);
+                    return BLE_HS_EINVAL;
+                }
+
+                /* Each characteristic requires:
+                 *     o 1 characteristic
+                 *     o 2 attributes
+                 */
+                res->chrs++;
+                res->attrs += 2;
+
+                /* If the characteristic permits notifications or indications,
+                 * it has a CCCD.
+                 */
+                if (chr->flags & BLE_GATT_CHR_F_NOTIFY ||
+                    chr->flags & BLE_GATT_CHR_F_INDICATE) {
+
+                    /* Each CCCD requires:
+                     *     o 1 descriptor
+                     *     o 1 CCCD
+                     *     o 1 attribute
+                     */
+                    res->dscs++;
+                    res->cccds++;
+                    res->attrs++;
+                }
+
+                if (chr->descriptors != NULL) {
+                    for (d = 0; chr->descriptors[d].uuid != NULL; d++) {
+                        if (!ble_gatts_dsc_is_sane(chr->descriptors + d)) {
+                            BLE_HS_DBG_ASSERT(0);
+                            return BLE_HS_EINVAL;
+                        }
+
+                        /* Each descriptor requires:
+                         *     o 1 descriptor
+                         *     o 1 attribute
+                         */
+                        res->dscs++;
+                        res->attrs++;
+                    }
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+int
+ble_gatts_count_cfg(const struct ble_gatt_svc_def *defs)
+{
+    struct ble_gatt_resources res = { 0 };
+    int rc;
+
+    rc = ble_gatts_count_resources(defs, &res);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_max_services += res.svcs;
+    ble_hs_max_attrs += res.attrs;
+
+    /* Reserve an extra CCCD for the cache. */
+    ble_hs_max_client_configs +=
+        res.cccds * (MYNEWT_VAL(BLE_MAX_CONNECTIONS) + 1);
+
+    return 0;
+}
+
+void
+ble_gatts_lcl_svc_foreach(ble_gatt_svc_foreach_fn cb, void *arg)
+{
+    int i;
+
+    for (i = 0; i < ble_gatts_num_svc_entries; i++) {
+        cb(ble_gatts_svc_entries[i].svc,
+           ble_gatts_svc_entries[i].handle,
+           ble_gatts_svc_entries[i].end_group_handle, arg);
+    }
+}
+
+int
+ble_gatts_reset(void)
+{
+    int rc;
+
+    ble_hs_lock();
+
+    if (!ble_gatts_mutable()) {
+        rc = BLE_HS_EBUSY;
+    } else {
+        /* Unregister all ATT attributes. */
+        ble_att_svr_reset();
+        ble_gatts_num_cfgable_chrs = 0;
+        rc = 0;
+
+        /* Note: gatts memory gets freed on next call to ble_gatts_start(). */
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_gatts_init(void)
+{
+    int rc;
+
+    ble_gatts_num_cfgable_chrs = 0;
+    ble_gatts_clt_cfgs = NULL;
+
+    rc = stats_init_and_reg(
+        STATS_HDR(ble_gatts_stats), STATS_SIZE_INIT_PARMS(ble_gatts_stats,
+        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_gatts_stats), "ble_gatts");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    return 0;
+
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_gatts_lcl.c b/src/libs/mynewt-nimble/nimble/host/src/ble_gatts_lcl.c
new file mode 100644
index 00000000..a45f397b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_gatts_lcl.c
@@ -0,0 +1,211 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include "host/ble_uuid.h"
+#include "console/console.h"
+#include "nimble/ble.h"
+#include "ble_hs_priv.h"
+
+static const ble_uuid_t *uuid_ccc =
+        BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16);
+
+static const char * const ble_gatt_chr_f_names[] = {
+    "BROADCAST",
+    "READ",
+    "WRITE_NO_RSP",
+    "WRITE",
+    "NOTIFY",
+    "INDICATE",
+    "AUTH_SIGN_WRITE",
+    "RELIABLE_WRITE",
+    "AUX_WRITE",
+    "READ_ENC",
+    "READ_AUTHEN",
+    "READ_AUTHOR",
+    "WRITE_ENC",
+    "WRITE_AUTHEN",
+    "WRITE_AUTHOR",
+    NULL
+};
+
+static const char * const ble_gatt_dsc_f_names[] = {
+    "READ",
+    "WRITE",
+    "READ_ENC",
+    "READ_AUTHEN",
+    "READ_AUTHOR",
+    "WRITE_ENC",
+    "WRITE_AUTHEN",
+    "WRITE_AUTHOR",
+    NULL
+};
+
+#define BLE_CHR_FLAGS_STR_LEN 180
+
+static char *
+ble_gatts_flags_to_str(uint16_t flags, char *buf,
+                       const char * const *names)
+{
+    int bit;
+    bool non_empty = false;
+    size_t length = 0;
+
+    buf[0] = '\0';
+    strcpy(buf, "[");
+    length += 1;
+    for (bit = 0; names[bit]; ++bit) {
+        if (flags & (1 << bit)) {
+            length += strlen(names[bit]);
+            if (length + 1 >= BLE_CHR_FLAGS_STR_LEN) {
+                return buf;
+            }
+            if (non_empty) {
+                strcat(buf, "|");
+                length += 1;
+            }
+            strcat(buf, names[bit]);
+            non_empty = true;
+        }
+    }
+    strcat(buf, "]");
+    return buf;
+}
+
+
+#define STRINGIFY(X) #X
+#define FIELD_NAME_LEN STRINGIFY(12)
+#define FIELD_INDENT STRINGIFY(2)
+
+static void
+ble_gatt_show_local_chr(const struct ble_gatt_svc_def *svc,
+                        uint16_t handle, char *uuid_buf, char *flags_buf)
+{
+    const struct ble_gatt_chr_def *chr;
+    const struct ble_gatt_dsc_def *dsc;
+
+    for (chr = svc->characteristics; chr && chr->uuid; ++chr) {
+        console_printf("characteristic\n");
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%s\n", " ", "uuid",
+                       ble_uuid_to_str(chr->uuid, uuid_buf));
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%d\n", " ", "def_handle", handle);
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%d\n", " ", "val_handle", handle+1);
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%d\n", " ", "min_key_size", chr->min_key_size);
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%s\n", " ", "flags",
+                       ble_gatts_flags_to_str(chr->flags,
+                       flags_buf, ble_gatt_chr_f_names));
+        handle += 2;
+
+        if ((chr->flags & BLE_GATT_CHR_F_NOTIFY) ||
+            (chr->flags & BLE_GATT_CHR_F_INDICATE)) {
+            console_printf("ccc descriptor\n");
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%s\n", " ", "uuid",
+                           ble_uuid_to_str(uuid_ccc, uuid_buf));
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%d\n", " ", "handle", handle);
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%d\n", " ", "min_key_size", 0);
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%s\n", " ", "flags",
+                           ble_gatts_flags_to_str(BLE_ATT_F_READ | BLE_ATT_F_WRITE,
+                                                  flags_buf, ble_gatt_dsc_f_names));
+            handle++;
+        }
+
+        for (dsc = chr->descriptors; dsc && dsc->uuid; ++dsc) {
+            console_printf("descriptor\n");
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%s\n", " ", "uuid",
+                           ble_uuid_to_str(dsc->uuid, uuid_buf));
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%d\n", " ", "handle", handle);
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%d\n", " ", "min_key_size", dsc->min_key_size);
+            console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                           "%s\n", " ", "flags",
+                           ble_gatts_flags_to_str(dsc->att_flags,
+                           flags_buf, ble_gatt_dsc_f_names));
+            handle++;
+        }
+    }
+}
+
+static int
+ble_gatt_show_local_inc_svc(const struct ble_gatt_svc_def *svc,
+                            uint16_t handle, char *uuid_buf)
+{
+    const struct ble_gatt_svc_def **includes;
+    int num = 0;
+
+    for (includes = &svc->includes[0]; *includes != NULL; ++includes) {
+        console_printf("included service\n");
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%s\n", " ", "uuid",
+                       ble_uuid_to_str((*includes)->uuid, uuid_buf));
+        console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                       "%d\n", " ", "attr handle", handle);
+        ++num;
+    }
+
+    return num;
+}
+
+static void
+ble_gatt_show_local_svc(const struct ble_gatt_svc_def *svc,
+                        uint16_t handle, uint16_t end_group_handle,
+                        void *arg)
+{
+    char uuid_buf[BLE_UUID_STR_LEN];
+    char flags_buf[BLE_CHR_FLAGS_STR_LEN];
+
+    console_printf("%s service\n",
+                   svc->type == BLE_GATT_SVC_TYPE_PRIMARY ?
+                           "primary" : "secondary");
+    console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                   "%s\n", " ", "uuid",
+                   ble_uuid_to_str(svc->uuid, uuid_buf));
+    console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                   "%d\n", " ", "handle",
+                   handle);
+    console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s "
+                   "%d\n", " ", "end_handle",
+                   end_group_handle);
+    handle++;
+
+    if (svc->includes) {
+        handle += ble_gatt_show_local_inc_svc(svc, handle, uuid_buf);
+    }
+
+    ble_gatt_show_local_chr(svc, handle,
+                            uuid_buf, flags_buf);
+}
+
+void
+ble_gatts_show_local(void)
+{
+    ble_gatts_lcl_svc_foreach(ble_gatt_show_local_svc, NULL);
+}
+
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs.c
new file mode 100644
index 00000000..b41064fe
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs.c
@@ -0,0 +1,808 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "stats/stats.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble_hs_priv.h"
+#include "ble_monitor_priv.h"
+#include "nimble/nimble_npl.h"
+#ifndef MYNEWT
+#include "nimble/nimble_port.h"
+#endif
+
+#define BLE_HS_HCI_EVT_COUNT                    \
+    (MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT) +     \
+     MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT))
+
+static void ble_hs_event_rx_hci_ev(struct ble_npl_event *ev);
+static void ble_hs_event_tx_notify(struct ble_npl_event *ev);
+static void ble_hs_event_reset(struct ble_npl_event *ev);
+static void ble_hs_event_start_stage1(struct ble_npl_event *ev);
+static void ble_hs_event_start_stage2(struct ble_npl_event *ev);
+static void ble_hs_timer_sched(int32_t ticks_from_now);
+
+struct os_mempool ble_hs_hci_ev_pool;
+static os_membuf_t ble_hs_hci_os_event_buf[
+    OS_MEMPOOL_SIZE(BLE_HS_HCI_EVT_COUNT, sizeof (struct ble_npl_event))
+];
+
+/** OS event - triggers tx of pending notifications and indications. */
+static struct ble_npl_event ble_hs_ev_tx_notifications;
+
+/** OS event - triggers a full reset. */
+static struct ble_npl_event ble_hs_ev_reset;
+
+static struct ble_npl_event ble_hs_ev_start_stage1;
+static struct ble_npl_event ble_hs_ev_start_stage2;
+
+uint8_t ble_hs_sync_state;
+uint8_t ble_hs_enabled_state;
+static int ble_hs_reset_reason;
+
+#define BLE_HS_SYNC_RETRY_TIMEOUT_MS    100 /* ms */
+
+static void *ble_hs_parent_task;
+
+/**
+ * Handles unresponsive timeouts and periodic retries in case of resource
+ * shortage.
+ */
+static struct ble_npl_callout ble_hs_timer;
+
+/* Shared queue that the host uses for work items. */
+static struct ble_npl_eventq *ble_hs_evq;
+
+static struct ble_mqueue ble_hs_rx_q;
+
+static struct ble_npl_mutex ble_hs_mutex;
+
+/** These values keep track of required ATT and GATT resources counts.  They
+ * increase as services are added, and are read when the ATT server and GATT
+ * server are started.
+ */
+uint16_t ble_hs_max_attrs;
+uint16_t ble_hs_max_services;
+uint16_t ble_hs_max_client_configs;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+static uint8_t ble_hs_dbg_mutex_locked;
+#endif
+
+STATS_SECT_DECL(ble_hs_stats) ble_hs_stats;
+STATS_NAME_START(ble_hs_stats)
+    STATS_NAME(ble_hs_stats, conn_create)
+    STATS_NAME(ble_hs_stats, conn_delete)
+    STATS_NAME(ble_hs_stats, hci_cmd)
+    STATS_NAME(ble_hs_stats, hci_event)
+    STATS_NAME(ble_hs_stats, hci_invalid_ack)
+    STATS_NAME(ble_hs_stats, hci_unknown_event)
+    STATS_NAME(ble_hs_stats, hci_timeout)
+    STATS_NAME(ble_hs_stats, reset)
+    STATS_NAME(ble_hs_stats, sync)
+    STATS_NAME(ble_hs_stats, pvcy_add_entry)
+    STATS_NAME(ble_hs_stats, pvcy_add_entry_fail)
+STATS_NAME_END(ble_hs_stats)
+
+struct ble_npl_eventq *
+ble_hs_evq_get(void)
+{
+    return ble_hs_evq;
+}
+
+void
+ble_hs_evq_set(struct ble_npl_eventq *evq)
+{
+    ble_hs_evq = evq;
+}
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+int
+ble_hs_locked_by_cur_task(void)
+{
+#if MYNEWT
+    struct os_task *owner;
+
+    if (!ble_npl_os_started()) {
+        return ble_hs_dbg_mutex_locked;
+    }
+
+    owner = ble_hs_mutex.mu.mu_owner;
+    return owner != NULL && owner == os_sched_get_current_task();
+#else
+    return 1;
+#endif
+}
+#endif
+
+/**
+ * Indicates whether the host's parent task is currently running.
+ */
+int
+ble_hs_is_parent_task(void)
+{
+    return !ble_npl_os_started() ||
+           ble_npl_get_current_task_id() == ble_hs_parent_task;
+}
+
+/**
+ * Locks the BLE host mutex.  Nested locks allowed.
+ */
+void
+ble_hs_lock_nested(void)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (!ble_npl_os_started()) {
+        ble_hs_dbg_mutex_locked = 1;
+        return;
+    }
+#endif
+
+    rc = ble_npl_mutex_pend(&ble_hs_mutex, 0xffffffff);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+}
+
+/**
+ * Unlocks the BLE host mutex.  Nested locks allowed.
+ */
+void
+ble_hs_unlock_nested(void)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (!ble_npl_os_started()) {
+        ble_hs_dbg_mutex_locked = 0;
+        return;
+    }
+#endif
+
+    rc = ble_npl_mutex_release(&ble_hs_mutex);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+}
+
+/**
+ * Locks the BLE host mutex.  Nested locks not allowed.
+ */
+void
+ble_hs_lock(void)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (!ble_npl_os_started()) {
+        BLE_HS_DBG_ASSERT(!ble_hs_dbg_mutex_locked);
+    }
+#endif
+
+    ble_hs_lock_nested();
+}
+
+/**
+ * Unlocks the BLE host mutex.  Nested locks not allowed.
+ */
+void
+ble_hs_unlock(void)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (!ble_npl_os_started()) {
+        BLE_HS_DBG_ASSERT(ble_hs_dbg_mutex_locked);
+    }
+#endif
+
+    ble_hs_unlock_nested();
+}
+
+void
+ble_hs_process_rx_data_queue(void)
+{
+    struct os_mbuf *om;
+
+    while ((om = ble_mqueue_get(&ble_hs_rx_q)) != NULL) {
+#if BLE_MONITOR
+        ble_monitor_send_om(BLE_MONITOR_OPCODE_ACL_RX_PKT, om);
+#endif
+
+        ble_hs_hci_evt_acl_process(om);
+    }
+}
+
+static int
+ble_hs_wakeup_tx_conn(struct ble_hs_conn *conn)
+{
+    struct os_mbuf_pkthdr *omp;
+    struct os_mbuf *om;
+    int rc;
+
+    while ((omp = STAILQ_FIRST(&conn->bhc_tx_q)) != NULL) {
+        STAILQ_REMOVE_HEAD(&conn->bhc_tx_q, omp_next);
+
+        om = OS_MBUF_PKTHDR_TO_MBUF(omp);
+        rc = ble_hs_hci_acl_tx_now(conn, &om);
+        if (rc == BLE_HS_EAGAIN) {
+            /* Controller is at capacity.  This packet will be the first to
+             * get transmitted next time around.
+             */
+            STAILQ_INSERT_HEAD(&conn->bhc_tx_q, OS_MBUF_PKTHDR(om), omp_next);
+            return BLE_HS_EAGAIN;
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Schedules the transmission of all queued ACL data packets to the controller.
+ */
+void
+ble_hs_wakeup_tx(void)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_lock();
+
+    /* If there is a connection with a partially transmitted packet, it has to
+     * be serviced first.  The controller is waiting for the remainder so it
+     * can reassemble it.
+     */
+    for (conn = ble_hs_conn_first();
+         conn != NULL;
+         conn = SLIST_NEXT(conn, bhc_next)) {
+
+        if (conn->bhc_flags & BLE_HS_CONN_F_TX_FRAG) {
+            rc = ble_hs_wakeup_tx_conn(conn);
+            if (rc != 0) {
+                goto done;
+            }
+            break;
+        }
+    }
+
+    /* For each connection, transmit queued packets until there are no more
+     * packets to send or the controller's buffers are exhausted.
+     */
+    for (conn = ble_hs_conn_first();
+         conn != NULL;
+         conn = SLIST_NEXT(conn, bhc_next)) {
+
+        rc = ble_hs_wakeup_tx_conn(conn);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+
+done:
+    ble_hs_unlock();
+}
+
+static void
+ble_hs_clear_rx_queue(void)
+{
+    struct os_mbuf *om;
+
+    while ((om = ble_mqueue_get(&ble_hs_rx_q)) != NULL) {
+        os_mbuf_free_chain(om);
+    }
+}
+
+int
+ble_hs_is_enabled(void)
+{
+    return ble_hs_enabled_state == BLE_HS_ENABLED_STATE_ON;
+}
+
+int
+ble_hs_synced(void)
+{
+    return ble_hs_sync_state == BLE_HS_SYNC_STATE_GOOD;
+}
+
+static int
+ble_hs_sync(void)
+{
+    ble_npl_time_t retry_tmo_ticks;
+    int rc;
+
+    /* Set the sync state to "bringup."  This allows the parent task to send
+     * the startup sequence to the controller.  No other tasks are allowed to
+     * send any commands.
+     */
+    ble_hs_sync_state = BLE_HS_SYNC_STATE_BRINGUP;
+
+    rc = ble_hs_startup_go();
+    if (rc == 0) {
+        ble_hs_sync_state = BLE_HS_SYNC_STATE_GOOD;
+    } else {
+        ble_hs_sync_state = BLE_HS_SYNC_STATE_BAD;
+    }
+
+    retry_tmo_ticks = ble_npl_time_ms_to_ticks32(BLE_HS_SYNC_RETRY_TIMEOUT_MS);
+    ble_hs_timer_sched(retry_tmo_ticks);
+
+    if (rc == 0) {
+        rc = ble_hs_misc_restore_irks();
+        if (rc != 0) {
+            BLE_HS_LOG(INFO, "Failed to restore IRKs from store; status=%d\n",
+                       rc);
+        }
+
+        if (ble_hs_cfg.sync_cb != NULL) {
+            ble_hs_cfg.sync_cb();
+        }
+
+        STATS_INC(ble_hs_stats, sync);
+    }
+
+    return rc;
+}
+
+static int
+ble_hs_reset(void)
+{
+    int rc;
+
+    STATS_INC(ble_hs_stats, reset);
+
+    ble_hs_sync_state = 0;
+
+    /* Reset transport.  Assume success; there is nothing we can do in case of
+     * failure.  If the transport failed to reset, the host will reset itself
+     * again when it fails to sync with the controller.
+     */
+    (void)ble_hci_trans_reset();
+
+    ble_hs_clear_rx_queue();
+
+    /* Clear adverising and scanning states. */
+    ble_gap_reset_state(ble_hs_reset_reason);
+
+    /* Clear configured addresses. */
+    ble_hs_id_reset();
+
+    if (ble_hs_cfg.reset_cb != NULL && ble_hs_reset_reason != 0) {
+        ble_hs_cfg.reset_cb(ble_hs_reset_reason);
+    }
+    ble_hs_reset_reason = 0;
+
+    rc = ble_hs_sync();
+    return rc;
+}
+
+/**
+ * Called when the host timer expires.  Handles unresponsive timeouts and
+ * periodic retries in case of resource shortage.
+ */
+static void
+ble_hs_timer_exp(struct ble_npl_event *ev)
+{
+    int32_t ticks_until_next;
+
+    switch (ble_hs_sync_state) {
+    case BLE_HS_SYNC_STATE_GOOD:
+        ticks_until_next = ble_gattc_timer();
+        ble_hs_timer_sched(ticks_until_next);
+
+        ticks_until_next = ble_gap_timer();
+        ble_hs_timer_sched(ticks_until_next);
+
+        ticks_until_next = ble_l2cap_sig_timer();
+        ble_hs_timer_sched(ticks_until_next);
+
+        ticks_until_next = ble_sm_timer();
+        ble_hs_timer_sched(ticks_until_next);
+
+        ticks_until_next = ble_hs_conn_timer();
+        ble_hs_timer_sched(ticks_until_next);
+        break;
+
+    case BLE_HS_SYNC_STATE_BAD:
+        ble_hs_reset();
+        break;
+
+    case BLE_HS_SYNC_STATE_BRINGUP:
+    default:
+        /* The timer should not be set in this state. */
+        assert(0);
+        break;
+    }
+
+}
+
+static void
+ble_hs_timer_reset(uint32_t ticks)
+{
+    int rc;
+
+    if (!ble_hs_is_enabled()) {
+        ble_npl_callout_stop(&ble_hs_timer);
+    } else {
+        rc = ble_npl_callout_reset(&ble_hs_timer, ticks);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static void
+ble_hs_timer_sched(int32_t ticks_from_now)
+{
+    ble_npl_time_t abs_time;
+
+    if (ticks_from_now == BLE_HS_FOREVER) {
+        return;
+    }
+
+    /* Reset timer if it is not currently scheduled or if the specified time is
+     * sooner than the previous expiration time.
+     */
+    abs_time = ble_npl_time_get() + ticks_from_now;
+    if (!ble_npl_callout_is_active(&ble_hs_timer) ||
+            ((ble_npl_stime_t)(abs_time -
+                               ble_npl_callout_get_ticks(&ble_hs_timer))) < 0) {
+        ble_hs_timer_reset(ticks_from_now);
+    }
+}
+
+void
+ble_hs_timer_resched(void)
+{
+    /* Reschedule the timer to run immediately.  The timer callback will query
+     * each module for an up-to-date expiration time.
+     */
+    ble_hs_timer_reset(0);
+}
+
+static void
+ble_hs_sched_start_stage2(void)
+{
+    ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(),
+                       &ble_hs_ev_start_stage2);
+}
+
+void
+ble_hs_sched_start(void)
+{
+#ifdef MYNEWT
+    ble_npl_eventq_put((struct ble_npl_eventq *)os_eventq_dflt_get(),
+                       &ble_hs_ev_start_stage1);
+#else
+    ble_npl_eventq_put(nimble_port_get_dflt_eventq(), &ble_hs_ev_start_stage1);
+#endif
+}
+
+static void
+ble_hs_event_rx_hci_ev(struct ble_npl_event *ev)
+{
+    const struct ble_hci_ev *hci_ev;
+    int rc;
+
+    hci_ev = ble_npl_event_get_arg(ev);
+
+    rc = os_memblock_put(&ble_hs_hci_ev_pool, ev);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+#if BLE_MONITOR
+    ble_monitor_send(BLE_MONITOR_OPCODE_EVENT_PKT, hci_ev,
+                     hci_ev->length + sizeof(*hci_ev));
+#endif
+
+    ble_hs_hci_evt_process(hci_ev);
+}
+
+static void
+ble_hs_event_tx_notify(struct ble_npl_event *ev)
+{
+    ble_gatts_tx_notifications();
+}
+
+static void
+ble_hs_event_rx_data(struct ble_npl_event *ev)
+{
+    ble_hs_process_rx_data_queue();
+}
+
+static void
+ble_hs_event_reset(struct ble_npl_event *ev)
+{
+    ble_hs_reset();
+}
+
+/**
+ * Implements the first half of the start process.  This just enqueues another
+ * event on the host parent task's event queue.
+ *
+ * Starting is done in two stages to allow the application time to configure
+ * the event queue to use after system initialization but before the host
+ * starts.
+ */
+static void
+ble_hs_event_start_stage1(struct ble_npl_event *ev)
+{
+    ble_hs_sched_start_stage2();
+}
+
+/**
+ * Implements the second half of the start process.  This actually starts the
+ * host.
+ *
+ * Starting is done in two stages to allow the application time to configure
+ * the event queue to use after system initialization but before the host
+ * starts.
+ */
+static void
+ble_hs_event_start_stage2(struct ble_npl_event *ev)
+{
+    int rc;
+
+    rc = ble_hs_start();
+    assert(rc == 0);
+}
+
+void
+ble_hs_enqueue_hci_event(uint8_t *hci_evt)
+{
+    struct ble_npl_event *ev;
+
+    ev = os_memblock_get(&ble_hs_hci_ev_pool);
+    if (ev == NULL) {
+        ble_hci_trans_buf_free(hci_evt);
+    } else {
+        ble_npl_event_init(ev, ble_hs_event_rx_hci_ev, hci_evt);
+        ble_npl_eventq_put(ble_hs_evq, ev);
+    }
+}
+
+/**
+ * Schedules for all pending notifications and indications to be sent in the
+ * host parent task.
+ */
+void
+ble_hs_notifications_sched(void)
+{
+#if !MYNEWT_VAL(BLE_HS_REQUIRE_OS)
+    if (!ble_npl_os_started()) {
+        ble_gatts_tx_notifications();
+        return;
+    }
+#endif
+
+    ble_npl_eventq_put(ble_hs_evq, &ble_hs_ev_tx_notifications);
+}
+
+void
+ble_hs_sched_reset(int reason)
+{
+    BLE_HS_DBG_ASSERT(ble_hs_reset_reason == 0);
+
+    ble_hs_reset_reason = reason;
+    ble_npl_eventq_put(ble_hs_evq, &ble_hs_ev_reset);
+}
+
+void
+ble_hs_hw_error(uint8_t hw_code)
+{
+    ble_hs_sched_reset(BLE_HS_HW_ERR(hw_code));
+}
+
+int
+ble_hs_start(void)
+{
+    int rc;
+
+    ble_hs_lock();
+    switch (ble_hs_enabled_state) {
+    case BLE_HS_ENABLED_STATE_ON:
+        rc = BLE_HS_EALREADY;
+        break;
+
+    case BLE_HS_ENABLED_STATE_STOPPING:
+        rc = BLE_HS_EBUSY;
+        break;
+
+    case BLE_HS_ENABLED_STATE_OFF:
+        ble_hs_enabled_state = BLE_HS_ENABLED_STATE_ON;
+        rc = 0;
+        break;
+
+    default:
+        assert(0);
+        rc = BLE_HS_EUNKNOWN;
+        break;
+    }
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_parent_task = ble_npl_get_current_task_id();
+
+#if MYNEWT_VAL(SELFTEST)
+    /* Stop the timer just in case the host was already running (e.g., unit
+     * tests).
+     */
+    ble_npl_callout_stop(&ble_hs_timer);
+#endif
+
+    ble_npl_callout_init(&ble_hs_timer, ble_hs_evq, ble_hs_timer_exp, NULL);
+
+    rc = ble_gatts_start();
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_sync();
+
+    return 0;
+}
+
+/**
+ * Called when a data packet is received from the controller.  This function
+ * consumes the supplied mbuf, regardless of the outcome.
+ *
+ * @param om                    The incoming data packet, beginning with the
+ *                                  HCI ACL data header.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+static int
+ble_hs_rx_data(struct os_mbuf *om, void *arg)
+{
+    int rc;
+
+    /* If flow control is enabled, mark this packet with its corresponding
+     * connection handle.
+     */
+    ble_hs_flow_fill_acl_usrhdr(om);
+
+    rc = ble_mqueue_put(&ble_hs_rx_q, ble_hs_evq, om);
+    if (rc != 0) {
+        os_mbuf_free_chain(om);
+        return BLE_HS_EOS;
+    }
+
+    return 0;
+}
+
+/**
+ * Enqueues an ACL data packet for transmission.  This function consumes the
+ * supplied mbuf, regardless of the outcome.
+ *
+ * @param om                    The outgoing data packet, beginning with the
+ *                                  HCI ACL data header.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_hs_tx_data(struct os_mbuf *om)
+{
+#if BLE_MONITOR
+    ble_monitor_send_om(BLE_MONITOR_OPCODE_ACL_TX_PKT, om);
+#endif
+
+    return ble_hci_trans_hs_acl_tx(om);
+}
+
+void
+ble_hs_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    /* Create memory pool of OS events */
+    rc = os_mempool_init(&ble_hs_hci_ev_pool, BLE_HS_HCI_EVT_COUNT,
+                         sizeof (struct ble_npl_event), ble_hs_hci_os_event_buf,
+                         "ble_hs_hci_ev_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    /* These get initialized here to allow unit tests to run without a zeroed
+     * bss.
+     */
+    ble_hs_reset_reason = 0;
+    ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
+
+    ble_npl_event_init(&ble_hs_ev_tx_notifications, ble_hs_event_tx_notify,
+                       NULL);
+    ble_npl_event_init(&ble_hs_ev_reset, ble_hs_event_reset, NULL);
+    ble_npl_event_init(&ble_hs_ev_start_stage1, ble_hs_event_start_stage1,
+                       NULL);
+    ble_npl_event_init(&ble_hs_ev_start_stage2, ble_hs_event_start_stage2,
+                       NULL);
+
+    ble_hs_hci_init();
+
+    rc = ble_hs_conn_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    rc = ble_hs_periodic_sync_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+#endif
+
+    rc = ble_l2cap_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_att_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_att_svr_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gap_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gattc_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    ble_hs_stop_init();
+
+    ble_mqueue_init(&ble_hs_rx_q, ble_hs_event_rx_data, NULL);
+
+    rc = stats_init_and_reg(
+        STATS_HDR(ble_hs_stats), STATS_SIZE_INIT_PARMS(ble_hs_stats,
+        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_hs_stats), "ble_hs");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_npl_mutex_init(&ble_hs_mutex);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    ble_hs_dbg_mutex_locked = 0;
+#endif
+
+#ifdef MYNEWT
+    ble_hs_evq_set((struct ble_npl_eventq *)os_eventq_dflt_get());
+#else
+    ble_hs_evq_set(nimble_port_get_dflt_eventq());
+#endif
+
+    /* Configure the HCI transport to communicate with a host. */
+    ble_hci_trans_cfg_hs(ble_hs_hci_rx_evt, NULL, ble_hs_rx_data, NULL);
+
+#if BLE_MONITOR
+    rc = ble_monitor_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
+#endif
+
+    /* Enqueue the start event to the default event queue.  Using the default
+     * queue ensures the event won't run until the end of main().  This allows
+     * the application to configure this package in the meantime.
+     */
+#if MYNEWT_VAL(BLE_HS_AUTO_START)
+#ifdef MYNEWT
+    ble_npl_eventq_put((struct ble_npl_eventq *)os_eventq_dflt_get(),
+                       &ble_hs_ev_start_stage1);
+#else
+    ble_npl_eventq_put(nimble_port_get_dflt_eventq(), &ble_hs_ev_start_stage1);
+#endif
+#endif
+
+#if BLE_MONITOR
+    ble_monitor_new_index(0, (uint8_t[6]){ }, "nimble0");
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c
new file mode 100644
index 00000000..1d938b95
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c
@@ -0,0 +1,803 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "nimble/ble.h"
+#include "host/ble_hs_adv.h"
+#include "ble_hs_priv.h"
+
+struct find_field_data {
+    uint8_t type;
+    const struct ble_hs_adv_field *field;
+};
+
+static ble_uuid16_t ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2];
+static ble_uuid32_t ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4];
+static ble_uuid128_t ble_hs_adv_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16];
+
+static int
+ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len,
+                   uint8_t *dst, uint8_t *dst_len, struct os_mbuf *om)
+{
+    int rc;
+
+    if (om ) {
+        data_len++;
+        rc = os_mbuf_append(om, &data_len, sizeof(data_len));
+        if (rc) {
+            return rc;
+        }
+
+        return os_mbuf_append(om, &type, sizeof(type));
+    }
+
+
+    if (*dst_len + 2 + data_len > max_len) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    dst[*dst_len] = data_len + 1;
+    dst[*dst_len + 1] = type;
+
+    *dst_len += 2;
+
+    return 0;
+}
+
+static int
+ble_hs_adv_set_flat_mbuf(uint8_t type, int data_len, const void *data,
+                         uint8_t *dst, uint8_t *dst_len, uint8_t max_len,
+                         struct os_mbuf *om)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(data_len > 0);
+
+    rc = ble_hs_adv_set_hdr(type, data_len, max_len, dst, dst_len, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (om) {
+        return os_mbuf_append(om, data, data_len);
+    }
+
+    memcpy(dst + *dst_len, data, data_len);
+    *dst_len += data_len;
+
+    return 0;
+}
+
+int
+ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data,
+                    uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
+{
+#if !NIMBLE_BLE_ADVERTISE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    return ble_hs_adv_set_flat_mbuf(type, data_len, data, dst, dst_len, max_len,
+                                    NULL);
+}
+
+static int
+ble_hs_adv_set_array_uuid16(uint8_t type, uint8_t num_elems,
+                            const ble_uuid16_t *elems, uint8_t *dst,
+                            uint8_t *dst_len, uint8_t max_len,
+                            struct os_mbuf *om)
+{
+    int rc;
+    int i;
+
+    rc = ble_hs_adv_set_hdr(type, num_elems * 2, max_len, dst,
+                            dst_len, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    for (i = 0; i < num_elems; i++) {
+        if (om) {
+            rc = ble_uuid_to_mbuf(&elems[i].u, om);
+            if (rc) {
+                return rc;
+            }
+        } else {
+            ble_uuid_flat(&elems[i].u, dst + *dst_len);
+            *dst_len += 2;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_set_array_uuid32(uint8_t type, uint8_t num_elems,
+                            const ble_uuid32_t *elems, uint8_t *dst,
+                            uint8_t *dst_len, uint8_t max_len,
+                            struct os_mbuf *om)
+{
+    uint32_t uuid_le;
+    int rc;
+    int i;
+
+    rc = ble_hs_adv_set_hdr(type, num_elems * 4, max_len, dst,
+                            dst_len, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    for (i = 0; i < num_elems; i++) {
+        /* We cannot use ble_uuid_flat here since it converts 32-bit UUIDs to
+         * 128-bit as ATT requires. In AD, 32-bit UUID shall be written as an
+         * actual 32-bit value.
+         */
+        if (om) {
+            uuid_le = htole32(elems[i].value);
+            rc = os_mbuf_append(om, &uuid_le, sizeof(uuid_le));
+            if (rc) {
+                return rc;
+            }
+        } else {
+            put_le32(dst + *dst_len, elems[i].value);
+            *dst_len += 4;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_set_array_uuid128(uint8_t type, uint8_t num_elems,
+                             const ble_uuid128_t *elems, uint8_t *dst,
+                             uint8_t *dst_len, uint8_t max_len,
+                             struct os_mbuf *om)
+{
+    int rc;
+    int i;
+
+    rc = ble_hs_adv_set_hdr(type, num_elems * 16, max_len, dst,
+                            dst_len, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    for (i = 0; i < num_elems; i++) {
+        if (om) {
+            rc = ble_uuid_to_mbuf(&elems[i].u, om);
+            if (rc) {
+                return rc;
+            }
+        } else {
+            ble_uuid_flat(&elems[i].u, dst + *dst_len);
+            *dst_len += 16;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, const uint16_t *elems,
+                       uint8_t *dst, uint8_t *dst_len, uint8_t max_len,
+                       struct os_mbuf *om)
+{
+    uint16_t tmp;
+    int rc;
+    int i;
+
+    rc = ble_hs_adv_set_hdr(type, num_elems * sizeof *elems, max_len, dst,
+                            dst_len, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    for (i = 0; i < num_elems; i++) {
+        if (om) {
+            tmp = htole16(elems[i]);
+            rc = os_mbuf_append(om, &tmp, sizeof(tmp));
+            if (rc) {
+                return rc;
+            }
+        } else {
+            put_le16(dst + *dst_len, elems[i]);
+            *dst_len += sizeof elems[i];
+        }
+    }
+
+    return 0;
+}
+
+static int
+adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                      uint8_t *dst, uint8_t *dst_len, uint8_t max_len,
+                      struct os_mbuf *om)
+{
+#if !NIMBLE_BLE_ADVERTISE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    uint8_t type;
+    int8_t tx_pwr_lvl;
+    uint8_t dst_len_local;
+    int rc;
+
+    dst_len_local = 0;
+
+    /*** 0x01 - Flags. */
+    /* The application has two options concerning the flags field:
+     * 1. Don't include it in advertisements (flags == 0).
+     * 2. Explicitly specify the value (flags != 0).
+     *
+     * Note: The CSS prohibits advertising a flags value of 0, so this method
+     * of specifying option 1 vs. 2 is sound.
+     */
+    if (adv_fields->flags != 0) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_FLAGS, 1,
+                                      &adv_fields->flags, dst, &dst_len_local,
+                                      max_len, om);
+
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x02,0x03 - 16-bit service class UUIDs. */
+    if (adv_fields->uuids16 != NULL && adv_fields->num_uuids16) {
+        if (adv_fields->uuids16_is_complete) {
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS16;
+        } else {
+            type = BLE_HS_ADV_TYPE_INCOMP_UUIDS16;
+        }
+
+        rc = ble_hs_adv_set_array_uuid16(type, adv_fields->num_uuids16,
+                                         adv_fields->uuids16, dst, &dst_len_local,
+                                         max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x04,0x05 - 32-bit service class UUIDs. */
+    if (adv_fields->uuids32 != NULL && adv_fields->num_uuids32) {
+        if (adv_fields->uuids32_is_complete) {
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS32;
+        } else {
+            type = BLE_HS_ADV_TYPE_INCOMP_UUIDS32;
+        }
+
+        rc = ble_hs_adv_set_array_uuid32(type, adv_fields->num_uuids32,
+                                         adv_fields->uuids32, dst, &dst_len_local,
+                                         max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x06,0x07 - 128-bit service class UUIDs. */
+    if (adv_fields->uuids128 != NULL && adv_fields->num_uuids128 > 0) {
+        if (adv_fields->uuids128_is_complete) {
+            type = BLE_HS_ADV_TYPE_COMP_UUIDS128;
+        } else {
+            type = BLE_HS_ADV_TYPE_INCOMP_UUIDS128;
+        }
+
+        rc = ble_hs_adv_set_array_uuid128(type, adv_fields->num_uuids128,
+                                          adv_fields->uuids128, dst, &dst_len_local,
+                                          max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x08,0x09 - Local name. */
+    if (adv_fields->name != NULL && adv_fields->name_len > 0) {
+        if (adv_fields->name_is_complete) {
+            type = BLE_HS_ADV_TYPE_COMP_NAME;
+        } else {
+            type = BLE_HS_ADV_TYPE_INCOMP_NAME;
+        }
+
+        rc = ble_hs_adv_set_flat_mbuf(type, adv_fields->name_len,
+                                      adv_fields->name, dst, &dst_len_local, max_len,
+                                      om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x0a - Tx power level. */
+    if (adv_fields->tx_pwr_lvl_is_present) {
+        /* Read the power level from the controller if requested; otherwise use
+         * the explicitly specified value.
+         */
+        if (adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO) {
+            rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_pwr_lvl);
+            if (rc != 0) {
+                return rc;
+            }
+        } else {
+            tx_pwr_lvl = adv_fields->tx_pwr_lvl;
+        }
+
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_TX_PWR_LVL, 1,
+                                      &tx_pwr_lvl, dst, &dst_len_local, max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x12 - Slave connection interval range. */
+    if (adv_fields->slave_itvl_range != NULL) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE,
+                                      BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN,
+                                      adv_fields->slave_itvl_range, dst,
+                                      &dst_len_local, max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x16 - Service data - 16-bit UUID. */
+    if (adv_fields->svc_data_uuid16 != NULL && adv_fields->svc_data_uuid16_len) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID16,
+                                      adv_fields->svc_data_uuid16_len,
+                                      adv_fields->svc_data_uuid16, dst, &dst_len_local,
+                                      max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x17 - Public target address. */
+    if (adv_fields->public_tgt_addr != NULL &&
+        adv_fields->num_public_tgt_addrs != 0) {
+
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR,
+                                 BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN *
+                                     adv_fields->num_public_tgt_addrs,
+                                 adv_fields->public_tgt_addr, dst, &dst_len_local,
+                                 max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x19 - Appearance. */
+    if (adv_fields->appearance_is_present) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_APPEARANCE,
+                                      BLE_HS_ADV_APPEARANCE_LEN,
+                                      &adv_fields->appearance, dst, &dst_len_local,
+                                      max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x1a - Advertising interval. */
+    if (adv_fields->adv_itvl_is_present) {
+        rc = ble_hs_adv_set_array16(BLE_HS_ADV_TYPE_ADV_ITVL, 1,
+                                    &adv_fields->adv_itvl, dst, &dst_len_local,
+                                    max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x20 - Service data - 32-bit UUID. */
+    if (adv_fields->svc_data_uuid32 != NULL && adv_fields->svc_data_uuid32_len) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID32,
+                                     adv_fields->svc_data_uuid32_len,
+                                     adv_fields->svc_data_uuid32, dst, &dst_len_local,
+                                     max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x21 - Service data - 128-bit UUID. */
+    if (adv_fields->svc_data_uuid128 != NULL && adv_fields->svc_data_uuid128_len) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID128,
+                                      adv_fields->svc_data_uuid128_len,
+                                      adv_fields->svc_data_uuid128, dst,
+                                      &dst_len_local, max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0x24 - URI. */
+    if (adv_fields->uri != NULL && adv_fields->uri_len) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_URI, adv_fields->uri_len,
+                                      adv_fields->uri, dst, &dst_len_local, max_len,
+                                      om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    /*** 0xff - Manufacturer specific data. */
+    if ((adv_fields->mfg_data != NULL) && (adv_fields->mfg_data_len >= 2)) {
+        rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_MFG_DATA,
+                                      adv_fields->mfg_data_len,
+                                      adv_fields->mfg_data,
+                                      dst, &dst_len_local, max_len, om);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    if (dst_len) {
+        *dst_len = dst_len_local;
+    }
+
+    return 0;
+}
+
+/**
+ * Converts a high-level set of fields to a byte buffer.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                      uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
+{
+#if !NIMBLE_BLE_ADVERTISE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    return adv_set_fields(adv_fields, dst, dst_len, max_len, NULL);
+}
+
+int
+ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields,
+                           struct os_mbuf *om)
+{
+#if !NIMBLE_BLE_ADVERTISE
+    return BLE_HS_ENOTSUP;
+#endif
+    return adv_set_fields(adv_fields, NULL, NULL, 0, om);
+}
+
+static int
+ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields *adv_fields,
+                         const uint8_t *data, uint8_t data_len)
+{
+    ble_uuid_any_t uuid;
+    int i;
+
+    if (data_len % 2 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    adv_fields->uuids16 = ble_hs_adv_uuids16;
+    adv_fields->num_uuids16 = data_len / 2;
+
+    for (i = 0; i < adv_fields->num_uuids16; i++) {
+        ble_uuid_init_from_buf(&uuid, data + i * 2, 2);
+        ble_hs_adv_uuids16[i] = uuid.u16;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields *adv_fields,
+                         const uint8_t *data, uint8_t data_len)
+{
+    ble_uuid_any_t uuid;
+    int i;
+
+    if (data_len % 4 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    adv_fields->uuids32 = ble_hs_adv_uuids32;
+    adv_fields->num_uuids32 = data_len / 4;
+
+    for (i = 0; i < adv_fields->num_uuids32; i++) {
+        ble_uuid_init_from_buf(&uuid, data + i * 4, 4);
+        ble_hs_adv_uuids32[i] = uuid.u32;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_parse_uuids128(struct ble_hs_adv_fields *adv_fields,
+                          const uint8_t *data, uint8_t data_len)
+{
+    ble_uuid_any_t uuid;
+    int i;
+
+    if (data_len % 16 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    adv_fields->uuids128 = ble_hs_adv_uuids128;
+    adv_fields->num_uuids128 = data_len / 16;
+
+    for (i = 0; i < adv_fields->num_uuids128; i++) {
+        ble_uuid_init_from_buf(&uuid, data + i * 16, 16);
+        ble_hs_adv_uuids128[i] = uuid.u128;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
+                           uint8_t *total_len, const uint8_t *src,
+                           uint8_t src_len)
+{
+    uint8_t data_len;
+    uint8_t type;
+    const uint8_t *data;
+    int rc;
+
+    if (src_len < 1) {
+        return BLE_HS_EMSGSIZE;
+    }
+    *total_len = src[0] + 1;
+
+    if (src_len < *total_len) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    type = src[1];
+    data = src + 2;
+    data_len = *total_len - 2;
+
+    if (data_len > BLE_HS_ADV_MAX_FIELD_SZ) {
+        return BLE_HS_EBADDATA;
+    }
+
+    switch (type) {
+    case BLE_HS_ADV_TYPE_FLAGS:
+        if (data_len != BLE_HS_ADV_FLAGS_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->flags = *data;
+        break;
+
+    case BLE_HS_ADV_TYPE_INCOMP_UUIDS16:
+        rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
+        }
+        adv_fields->uuids16_is_complete = 0;
+        break;
+
+    case BLE_HS_ADV_TYPE_COMP_UUIDS16:
+        rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
+        }
+        adv_fields->uuids16_is_complete = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_INCOMP_UUIDS32:
+        rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
+        }
+        adv_fields->uuids32_is_complete = 0;
+        break;
+
+    case BLE_HS_ADV_TYPE_COMP_UUIDS32:
+        rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
+        }
+        adv_fields->uuids32_is_complete = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_INCOMP_UUIDS128:
+        rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
+        }
+        adv_fields->uuids128_is_complete = 0;
+        break;
+
+    case BLE_HS_ADV_TYPE_COMP_UUIDS128:
+        rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
+        }
+        adv_fields->uuids128_is_complete = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_INCOMP_NAME:
+        adv_fields->name = data;
+        adv_fields->name_len = data_len;
+        adv_fields->name_is_complete = 0;
+        break;
+
+    case BLE_HS_ADV_TYPE_COMP_NAME:
+        adv_fields->name = data;
+        adv_fields->name_len = data_len;
+        adv_fields->name_is_complete = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_TX_PWR_LVL:
+        if (data_len != BLE_HS_ADV_TX_PWR_LVL_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->tx_pwr_lvl = *data;
+        adv_fields->tx_pwr_lvl_is_present = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE:
+        if (data_len != BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->slave_itvl_range = data;
+        break;
+
+    case BLE_HS_ADV_TYPE_SVC_DATA_UUID16:
+        if (data_len < BLE_HS_ADV_SVC_DATA_UUID16_MIN_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->svc_data_uuid16 = data;
+        adv_fields->svc_data_uuid16_len = data_len;
+        break;
+
+    case BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR:
+        if (data_len % BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN != 0) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->public_tgt_addr = data;
+        adv_fields->num_public_tgt_addrs =
+            data_len / BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN;
+        break;
+
+    case BLE_HS_ADV_TYPE_APPEARANCE:
+        if (data_len != BLE_HS_ADV_APPEARANCE_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->appearance = get_le16(data);
+        adv_fields->appearance_is_present = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_ADV_ITVL:
+        if (data_len != BLE_HS_ADV_ADV_ITVL_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->adv_itvl = get_le16(data);
+        adv_fields->adv_itvl_is_present = 1;
+        break;
+
+    case BLE_HS_ADV_TYPE_SVC_DATA_UUID32:
+        if (data_len < BLE_HS_ADV_SVC_DATA_UUID32_MIN_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->svc_data_uuid32 = data;
+        adv_fields->svc_data_uuid32_len = data_len;
+        break;
+
+    case BLE_HS_ADV_TYPE_SVC_DATA_UUID128:
+        if (data_len < BLE_HS_ADV_SVC_DATA_UUID128_MIN_LEN) {
+            return BLE_HS_EBADDATA;
+        }
+        adv_fields->svc_data_uuid128 = data;
+        adv_fields->svc_data_uuid128_len = data_len;
+        break;
+
+    case BLE_HS_ADV_TYPE_URI:
+        adv_fields->uri = data;
+        adv_fields->uri_len = data_len;
+        break;
+
+    case BLE_HS_ADV_TYPE_MFG_DATA:
+        adv_fields->mfg_data = data;
+        adv_fields->mfg_data_len = data_len;
+        break;
+
+    default:
+        break;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields,
+                        const uint8_t *src, uint8_t src_len)
+{
+    uint8_t field_len;
+    int rc;
+
+    memset(adv_fields, 0, sizeof *adv_fields);
+
+    while (src_len > 0) {
+        rc = ble_hs_adv_parse_one_field(adv_fields, &field_len, src, src_len);
+        if (rc != 0) {
+            return rc;
+        }
+
+        src += field_len;
+        src_len -= field_len;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_adv_parse(const uint8_t *data, uint8_t length,
+                 ble_hs_adv_parse_func_t func, void *user_data)
+{
+    const struct ble_hs_adv_field *field;
+
+    while (length > 1) {
+        field = (const void *) data;
+
+        if (field->length >= length) {
+            return BLE_HS_EBADDATA;
+        }
+
+        if (func(field, user_data) == 0) {
+            return 0;
+        }
+
+        length -= 1 + field->length;
+        data += 1 + field->length;
+    }
+
+    return 0;
+}
+
+static int
+find_field_func(const struct ble_hs_adv_field *field, void *user_data)
+{
+    struct find_field_data *ffd = user_data;
+
+    if (field->type != ffd->type) {
+        return BLE_HS_EAGAIN;
+    }
+
+    ffd->field = field;
+
+    return 0;
+}
+
+int
+ble_hs_adv_find_field(uint8_t type, const uint8_t *data, uint8_t length,
+                      const struct ble_hs_adv_field **out)
+{
+    int rc;
+    struct find_field_data ffd = {
+            .type = type,
+            .field = NULL,
+    };
+
+    rc = ble_hs_adv_parse(data, length, find_field_func, &ffd);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (!ffd.field) {
+        return BLE_HS_ENOENT;
+    }
+
+    *out = ffd.field;
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv_priv.h
new file mode 100644
index 00000000..5c8a6ecc
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv_priv.h
@@ -0,0 +1,36 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_ADV_PRIV_
+#define H_BLE_HS_ADV_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data,
+                        uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
+int ble_hs_adv_find_field(uint8_t type, const uint8_t *data, uint8_t length,
+                          const struct ble_hs_adv_field **out);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_atomic.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_atomic.c
new file mode 100644
index 00000000..f26ba7ac
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_atomic.c
@@ -0,0 +1,120 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "ble_hs_priv.h"
+
+int
+ble_hs_atomic_conn_delete(uint16_t conn_handle)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        ble_hs_conn_remove(conn);
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+        if (conn->psync) {
+            ble_hs_periodic_sync_free(conn->psync);
+        }
+#endif
+        ble_hs_conn_free(conn);
+
+    }
+    ble_hs_unlock();
+
+    return conn != NULL ? 0 : BLE_HS_ENOTCONN;
+}
+
+void
+ble_hs_atomic_conn_insert(struct ble_hs_conn *conn)
+{
+    ble_hs_lock();
+    ble_hs_conn_insert(conn);
+    ble_hs_unlock();
+}
+
+int
+ble_hs_atomic_conn_flags(uint16_t conn_handle, ble_hs_conn_flags_t *out_flags)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+    } else {
+        rc = 0;
+        if (out_flags != NULL) {
+            *out_flags = conn->bhc_flags;
+        }
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_hs_atomic_conn_set_flags(uint16_t conn_handle, ble_hs_conn_flags_t flags,
+                             int on)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+    } else {
+        rc = 0;
+
+        if (on) {
+            conn->bhc_flags |= flags;
+        } else {
+            conn->bhc_flags &= ~flags;
+        }
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+uint16_t
+ble_hs_atomic_first_conn_handle(void)
+{
+    const struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_first();
+    if (conn != NULL) {
+        conn_handle = conn->bhc_handle;
+    } else {
+        conn_handle = BLE_HS_CONN_HANDLE_NONE;
+    }
+
+    ble_hs_unlock();
+
+    return conn_handle;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_atomic_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_atomic_priv.h
new file mode 100644
index 00000000..9f7d8d16
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_atomic_priv.h
@@ -0,0 +1,41 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_ATOMIC_
+#define H_BLE_HS_ATOMIC_
+
+#include "ble_hs_conn_priv.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_hs_atomic_conn_delete(uint16_t conn_handle);
+void ble_hs_atomic_conn_insert(struct ble_hs_conn *conn);
+int ble_hs_atomic_conn_flags(uint16_t conn_handle,
+                             ble_hs_conn_flags_t *out_flags);
+int ble_hs_atomic_conn_set_flags(uint16_t conn_handle,
+                                 ble_hs_conn_flags_t flags, int on);
+uint16_t ble_hs_atomic_first_conn_handle(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_cfg.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_cfg.c
new file mode 100644
index 00000000..a46a604a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_cfg.c
@@ -0,0 +1,33 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+#include "host/ble_hs.h"
+
+struct ble_hs_cfg ble_hs_cfg = {
+    /** Security manager settings. */
+    .sm_io_cap = MYNEWT_VAL(BLE_SM_IO_CAP),
+    .sm_oob_data_flag = MYNEWT_VAL(BLE_SM_OOB_DATA_FLAG),
+    .sm_bonding = MYNEWT_VAL(BLE_SM_BONDING),
+    .sm_mitm = MYNEWT_VAL(BLE_SM_MITM),
+    .sm_sc = MYNEWT_VAL(BLE_SM_SC),
+    .sm_keypress = MYNEWT_VAL(BLE_SM_KEYPRESS),
+    .sm_our_key_dist = MYNEWT_VAL(BLE_SM_OUR_KEY_DIST),
+    .sm_their_key_dist = MYNEWT_VAL(BLE_SM_THEIR_KEY_DIST),
+};
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_conn.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_conn.c
new file mode 100644
index 00000000..70695fa6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_conn.c
@@ -0,0 +1,576 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "host/ble_hs_id.h"
+#include "ble_hs_priv.h"
+
+/** At least three channels required per connection (sig, att, sm). */
+#define BLE_HS_CONN_MIN_CHANS       3
+
+static SLIST_HEAD(, ble_hs_conn) ble_hs_conns;
+static struct os_mempool ble_hs_conn_pool;
+
+static os_membuf_t ble_hs_conn_elem_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_CONNECTIONS),
+                    sizeof (struct ble_hs_conn))
+];
+
+static const uint8_t ble_hs_conn_null_addr[6];
+
+int
+ble_hs_conn_can_alloc(void)
+{
+#if !NIMBLE_BLE_CONNECT
+    return 0;
+#endif
+
+    return ble_hs_conn_pool.mp_num_free >= 1 &&
+           ble_l2cap_chan_pool.mp_num_free >= BLE_HS_CONN_MIN_CHANS &&
+           ble_gatts_conn_can_alloc();
+}
+
+struct ble_l2cap_chan *
+ble_hs_conn_chan_find_by_scid(struct ble_hs_conn *conn, uint16_t cid)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_l2cap_chan *chan;
+
+    SLIST_FOREACH(chan, &conn->bhc_channels, next) {
+        if (chan->scid == cid) {
+            return chan;
+        }
+        if (chan->scid > cid) {
+            return NULL;
+        }
+    }
+
+    return NULL;
+}
+
+struct ble_l2cap_chan *
+ble_hs_conn_chan_find_by_dcid(struct ble_hs_conn *conn, uint16_t cid)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_l2cap_chan *chan;
+
+    SLIST_FOREACH(chan, &conn->bhc_channels, next) {
+        if (chan->dcid == cid) {
+            return chan;
+        }
+    }
+
+    return NULL;
+}
+
+bool
+ble_hs_conn_chan_exist(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_l2cap_chan *tmp;
+
+    SLIST_FOREACH(tmp, &conn->bhc_channels, next) {
+        if (chan == tmp) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+int
+ble_hs_conn_chan_insert(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
+{
+#if !NIMBLE_BLE_CONNECT
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_l2cap_chan *prev;
+    struct ble_l2cap_chan *cur;
+
+    prev = NULL;
+    SLIST_FOREACH(cur, &conn->bhc_channels, next) {
+        if (cur->scid == chan->scid) {
+            return BLE_HS_EALREADY;
+        }
+        if (cur->scid > chan->scid) {
+            break;
+        }
+
+        prev = cur;
+    }
+
+    if (prev == NULL) {
+        SLIST_INSERT_HEAD(&conn->bhc_channels, chan, next);
+    } else {
+        SLIST_INSERT_AFTER(prev, chan, next);
+    }
+
+    return 0;
+}
+
+struct ble_hs_conn *
+ble_hs_conn_alloc(uint16_t conn_handle)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    conn = os_memblock_get(&ble_hs_conn_pool);
+    if (conn == NULL) {
+        goto err;
+    }
+    memset(conn, 0, sizeof *conn);
+    conn->bhc_handle = conn_handle;
+
+    SLIST_INIT(&conn->bhc_channels);
+
+    chan = ble_att_create_chan(conn_handle);
+    if (chan == NULL) {
+        goto err;
+    }
+    rc = ble_hs_conn_chan_insert(conn, chan);
+    if (rc != 0) {
+        goto err;
+    }
+
+    chan = ble_l2cap_sig_create_chan(conn_handle);
+    if (chan == NULL) {
+        goto err;
+    }
+    rc = ble_hs_conn_chan_insert(conn, chan);
+    if (rc != 0) {
+        goto err;
+    }
+
+    /* Create the SM channel even if not configured. We need it to reject SM
+     * messages.
+     */
+    chan = ble_sm_create_chan(conn_handle);
+    if (chan == NULL) {
+        goto err;
+    }
+    rc = ble_hs_conn_chan_insert(conn, chan);
+    if (rc != 0) {
+        goto err;
+    }
+
+    rc = ble_gatts_conn_init(&conn->bhc_gatt_svr);
+    if (rc != 0) {
+        goto err;
+    }
+
+    STAILQ_INIT(&conn->bhc_tx_q);
+
+    STATS_INC(ble_hs_stats, conn_create);
+
+    return conn;
+
+err:
+    ble_hs_conn_free(conn);
+    return NULL;
+}
+
+void
+ble_hs_conn_delete_chan(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
+{
+    if (conn->bhc_rx_chan == chan) {
+        conn->bhc_rx_chan = NULL;
+    }
+
+    SLIST_REMOVE(&conn->bhc_channels, chan, ble_l2cap_chan, next);
+    ble_l2cap_chan_free(conn, chan);
+}
+
+void
+ble_hs_conn_foreach(ble_hs_conn_foreach_fn *cb, void *arg)
+{
+    struct ble_hs_conn *conn;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
+        if (cb(conn, arg) != 0) {
+            return;
+        }
+    }
+}
+
+void
+ble_hs_conn_free(struct ble_hs_conn *conn)
+{
+#if !NIMBLE_BLE_CONNECT
+    return;
+#endif
+
+    struct ble_l2cap_chan *chan;
+    struct os_mbuf_pkthdr *omp;
+    int rc;
+
+    if (conn == NULL) {
+        return;
+    }
+
+    ble_att_svr_prep_clear(&conn->bhc_att_svr.basc_prep_list);
+
+    while ((chan = SLIST_FIRST(&conn->bhc_channels)) != NULL) {
+        ble_hs_conn_delete_chan(conn, chan);
+    }
+
+    while ((omp = STAILQ_FIRST(&conn->bhc_tx_q)) != NULL) {
+        STAILQ_REMOVE_HEAD(&conn->bhc_tx_q, omp_next);
+        os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp));
+    }
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    memset(conn, 0xff, sizeof *conn);
+#endif
+    rc = os_memblock_put(&ble_hs_conn_pool, conn);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    STATS_INC(ble_hs_stats, conn_delete);
+}
+
+void
+ble_hs_conn_insert(struct ble_hs_conn *conn)
+{
+#if !NIMBLE_BLE_CONNECT
+    return;
+#endif
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    BLE_HS_DBG_ASSERT_EVAL(ble_hs_conn_find(conn->bhc_handle) == NULL);
+    SLIST_INSERT_HEAD(&ble_hs_conns, conn, bhc_next);
+}
+
+void
+ble_hs_conn_remove(struct ble_hs_conn *conn)
+{
+#if !NIMBLE_BLE_CONNECT
+    return;
+#endif
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    SLIST_REMOVE(&ble_hs_conns, conn, ble_hs_conn, bhc_next);
+}
+
+struct ble_hs_conn *
+ble_hs_conn_find(uint16_t conn_handle)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_hs_conn *conn;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
+        if (conn->bhc_handle == conn_handle) {
+            return conn;
+        }
+    }
+
+    return NULL;
+}
+
+struct ble_hs_conn *
+ble_hs_conn_find_assert(uint16_t conn_handle)
+{
+    struct ble_hs_conn *conn;
+
+    conn = ble_hs_conn_find(conn_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+
+    return conn;
+}
+
+struct ble_hs_conn *
+ble_hs_conn_find_by_addr(const ble_addr_t *addr)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_hs_conn *conn;
+    struct ble_hs_conn_addrs addrs;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    if (!addr) {
+        return NULL;
+    }
+
+    SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
+        if (BLE_ADDR_IS_RPA(addr)) {
+            if (ble_addr_cmp(&conn->bhc_peer_rpa_addr, addr) == 0) {
+                return conn;
+            }
+        } else {
+            if (ble_addr_cmp(&conn->bhc_peer_addr, addr) == 0) {
+                return conn;
+            }
+            if (conn->bhc_peer_addr.type < BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT) {
+                continue;
+            }
+            /*If type 0x02 or 0x03 is used, let's double check if address is good */
+            ble_hs_conn_addrs(conn, &addrs);
+            if (ble_addr_cmp(&addrs.peer_id_addr, addr) == 0) {
+                return conn;
+            }
+        }
+    }
+
+    return NULL;
+}
+
+struct ble_hs_conn *
+ble_hs_conn_find_by_idx(int idx)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    struct ble_hs_conn *conn;
+    int i;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    i = 0;
+    SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
+        if (i == idx) {
+            return conn;
+        }
+
+        i++;
+    }
+
+    return NULL;
+}
+
+int
+ble_hs_conn_exists(uint16_t conn_handle)
+{
+#if !NIMBLE_BLE_CONNECT
+    return 0;
+#endif
+    return ble_hs_conn_find(conn_handle) != NULL;
+}
+
+/**
+ * Retrieves the first connection in the list.
+ */
+struct ble_hs_conn *
+ble_hs_conn_first(void)
+{
+#if !NIMBLE_BLE_CONNECT
+    return NULL;
+#endif
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+    return SLIST_FIRST(&ble_hs_conns);
+}
+
+void
+ble_hs_conn_addrs(const struct ble_hs_conn *conn,
+                  struct ble_hs_conn_addrs *addrs)
+{
+    const uint8_t *our_id_addr_val;
+    int rc;
+
+    /* Determine our address information. */
+    addrs->our_id_addr.type =
+        ble_hs_misc_own_addr_type_to_id(conn->bhc_our_addr_type);
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    /* With EA enabled random address for slave connection is per advertising
+     * instance and requires special handling here.
+     */
+
+    if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER) &&
+            addrs->our_id_addr.type == BLE_ADDR_RANDOM) {
+        our_id_addr_val = conn->bhc_our_rnd_addr;
+    } else {
+        rc = ble_hs_id_addr(addrs->our_id_addr.type, &our_id_addr_val, NULL);
+        assert(rc == 0);
+    }
+#else
+    rc = ble_hs_id_addr(addrs->our_id_addr.type, &our_id_addr_val, NULL);
+    assert(rc == 0);
+#endif
+
+    memcpy(addrs->our_id_addr.val, our_id_addr_val, 6);
+
+    if (memcmp(conn->bhc_our_rpa_addr.val, ble_hs_conn_null_addr, 6) == 0) {
+        addrs->our_ota_addr = addrs->our_id_addr;
+    } else {
+        addrs->our_ota_addr = conn->bhc_our_rpa_addr;
+    }
+
+    /* Determine peer address information. */
+    addrs->peer_id_addr = conn->bhc_peer_addr;
+    addrs->peer_ota_addr = conn->bhc_peer_addr;
+    switch (conn->bhc_peer_addr.type) {
+    case BLE_ADDR_PUBLIC:
+    case BLE_ADDR_RANDOM:
+        break;
+
+    case BLE_ADDR_PUBLIC_ID:
+        addrs->peer_id_addr.type = BLE_ADDR_PUBLIC;
+        addrs->peer_ota_addr = conn->bhc_peer_rpa_addr;
+        break;
+
+    case BLE_ADDR_RANDOM_ID:
+        addrs->peer_id_addr.type = BLE_ADDR_RANDOM;
+        addrs->peer_ota_addr = conn->bhc_peer_rpa_addr;
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+}
+
+int32_t
+ble_hs_conn_timer(void)
+{
+    /* If there are no timeouts configured, then there is nothing to check. */
+#if MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) == 0 && \
+    BLE_HS_ATT_SVR_QUEUED_WRITE_TMO == 0
+
+    return BLE_HS_FOREVER;
+#endif
+
+    struct ble_hs_conn *conn;
+    ble_npl_time_t now;
+    int32_t next_exp_in;
+    int32_t time_diff;
+    uint16_t conn_handle;
+
+    conn_handle = BLE_HS_CONN_HANDLE_NONE;
+    next_exp_in = BLE_HS_FOREVER;
+    now = ble_npl_time_get();
+
+    ble_hs_lock();
+
+    /* This loop performs one of two tasks:
+     * 1. Determine if any connections need to be terminated due to timeout.
+     *    If so, break out of the loop and terminate the connection.  This
+     *    function will need to be executed again.
+     * 2. Otherwise, determine when the next timeout will occur.
+     */
+    SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
+        if (!(conn->bhc_flags & BLE_HS_CONN_F_TERMINATING)) {
+
+#if MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) != 0
+            /* Check each connection's rx fragment timer.  If too much time
+             * passes after a partial packet is received, the connection is
+             * terminated.
+             */
+            if (conn->bhc_rx_chan != NULL) {
+                time_diff = conn->bhc_rx_timeout - now;
+
+                if (time_diff <= 0) {
+                    /* ACL reassembly has timed out.  Remember the connection
+                     * handle so it can be terminated after the mutex is
+                     * unlocked.
+                     */
+                    conn_handle = conn->bhc_handle;
+                    break;
+                }
+
+                /* Determine if this connection is the soonest to time out. */
+                if (time_diff < next_exp_in) {
+                    next_exp_in = time_diff;
+                }
+            }
+#endif
+
+#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO
+            /* Check each connection's rx queued write timer.  If too much
+             * time passes after a prep write is received, the queue is
+             * cleared.
+             */
+            time_diff = ble_att_svr_ticks_until_tmo(&conn->bhc_att_svr, now);
+            if (time_diff <= 0) {
+                /* ACL reassembly has timed out.  Remember the connection
+                 * handle so it can be terminated after the mutex is
+                 * unlocked.
+                 */
+                conn_handle = conn->bhc_handle;
+                break;
+            }
+
+            /* Determine if this connection is the soonest to time out. */
+            if (time_diff < next_exp_in) {
+                next_exp_in = time_diff;
+            }
+#endif
+        }
+    }
+
+    ble_hs_unlock();
+
+    /* If a connection has timed out, terminate it.  We need to recursively
+     * call this function again to determine when the next timeout is.  This
+     * is a tail-recursive call, so it should be optimized to execute in the
+     * same stack frame.
+     */
+    if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+        ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+        return ble_hs_conn_timer();
+    }
+
+    return next_exp_in;
+}
+
+int
+ble_hs_conn_init(void)
+{
+    int rc;
+
+    rc = os_mempool_init(&ble_hs_conn_pool, MYNEWT_VAL(BLE_MAX_CONNECTIONS),
+                         sizeof (struct ble_hs_conn),
+                         ble_hs_conn_elem_mem, "ble_hs_conn_pool");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    SLIST_INIT(&ble_hs_conns);
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_conn_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_conn_priv.h
new file mode 100644
index 00000000..0e451194
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_conn_priv.h
@@ -0,0 +1,148 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_CONN_
+#define H_BLE_HS_CONN_
+
+#include <inttypes.h>
+#include "ble_l2cap_priv.h"
+#include "ble_gatt_priv.h"
+#include "ble_att_priv.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct hci_le_conn_complete;
+struct hci_create_conn;
+struct ble_l2cap_chan;
+
+typedef uint8_t ble_hs_conn_flags_t;
+
+#define BLE_HS_CONN_F_MASTER        0x01
+#define BLE_HS_CONN_F_TERMINATING   0x02
+#define BLE_HS_CONN_F_TX_FRAG       0x04 /* Cur ACL packet partially txed. */
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+#define BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN_REM \
+                      ((MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) % (8 * sizeof(uint32_t))) ? 1 : 0)
+
+#define BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN  \
+                      (MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) / (8 * sizeof(uint32_t)) + \
+                       BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN_REM)
+#endif
+
+struct ble_hs_conn {
+    SLIST_ENTRY(ble_hs_conn) bhc_next;
+    uint16_t bhc_handle;
+    uint8_t bhc_our_addr_type;
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    uint8_t bhc_our_rnd_addr[6];
+#endif
+    ble_addr_t bhc_peer_addr;
+    ble_addr_t bhc_our_rpa_addr;
+    ble_addr_t bhc_peer_rpa_addr;
+
+    uint16_t bhc_itvl;
+    uint16_t bhc_latency;
+    uint16_t bhc_supervision_timeout;
+    uint8_t bhc_master_clock_accuracy;
+
+    uint32_t supported_feat;
+
+    ble_hs_conn_flags_t bhc_flags;
+
+    struct ble_l2cap_chan_list bhc_channels;
+    struct ble_l2cap_chan *bhc_rx_chan; /* Channel rxing current packet. */
+    ble_npl_time_t bhc_rx_timeout;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+    uint32_t l2cap_coc_cid_mask[BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN];
+#endif
+
+    /**
+     * Count of packets sent over this connection that the controller has not
+     * transmitted or flushed yet.
+     */
+    uint16_t bhc_outstanding_pkts;
+
+#if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
+    /**
+     * Count of packets received over this connection that have been processed
+     * and freed.
+     */
+    uint16_t bhc_completed_pkts;
+#endif
+
+    /** Queue of outgoing packets that could not be sent. */
+    STAILQ_HEAD(, os_mbuf_pkthdr) bhc_tx_q;
+
+    struct ble_att_svr_conn bhc_att_svr;
+    struct ble_gatts_conn bhc_gatt_svr;
+
+    struct ble_gap_sec_state bhc_sec_state;
+
+    ble_gap_event_fn *bhc_cb;
+    void *bhc_cb_arg;
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    struct ble_hs_periodic_sync *psync;
+#endif
+};
+
+struct ble_hs_conn_addrs {
+    ble_addr_t our_id_addr;
+    ble_addr_t peer_id_addr;
+    ble_addr_t our_ota_addr;
+    ble_addr_t peer_ota_addr;
+};
+
+int ble_hs_conn_can_alloc(void);
+struct ble_hs_conn *ble_hs_conn_alloc(uint16_t conn_handle);
+void ble_hs_conn_free(struct ble_hs_conn *conn);
+void ble_hs_conn_insert(struct ble_hs_conn *conn);
+void ble_hs_conn_remove(struct ble_hs_conn *conn);
+struct ble_hs_conn *ble_hs_conn_find(uint16_t conn_handle);
+struct ble_hs_conn *ble_hs_conn_find_assert(uint16_t conn_handle);
+struct ble_hs_conn *ble_hs_conn_find_by_addr(const ble_addr_t *addr);
+struct ble_hs_conn *ble_hs_conn_find_by_idx(int idx);
+int ble_hs_conn_exists(uint16_t conn_handle);
+struct ble_hs_conn *ble_hs_conn_first(void);
+struct ble_l2cap_chan *ble_hs_conn_chan_find_by_scid(struct ble_hs_conn *conn,
+                                             uint16_t cid);
+struct ble_l2cap_chan *ble_hs_conn_chan_find_by_dcid(struct ble_hs_conn *conn,
+                                             uint16_t cid);
+bool ble_hs_conn_chan_exist(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan);
+int ble_hs_conn_chan_insert(struct ble_hs_conn *conn,
+                            struct ble_l2cap_chan *chan);
+void ble_hs_conn_delete_chan(struct ble_hs_conn *conn,
+                             struct ble_l2cap_chan *chan);
+
+void ble_hs_conn_addrs(const struct ble_hs_conn *conn,
+                       struct ble_hs_conn_addrs *addrs);
+int32_t ble_hs_conn_timer(void);
+
+typedef int ble_hs_conn_foreach_fn(struct ble_hs_conn *conn, void *arg);
+void ble_hs_conn_foreach(ble_hs_conn_foreach_fn *cb, void *arg);
+
+int ble_hs_conn_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_flow.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_flow.c
new file mode 100644
index 00000000..d224e6ee
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_flow.c
@@ -0,0 +1,267 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble_hs_priv.h"
+
+#if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
+
+#define BLE_HS_FLOW_ITVL_TICKS  \
+    ble_npl_time_ms_to_ticks32(MYNEWT_VAL(BLE_HS_FLOW_CTRL_ITVL))
+
+/**
+ * The number of freed buffers since the most-recent
+ * number-of-completed-packets event was sent.  This is used to determine if an
+ * immediate event transmission is required.
+ */
+static uint16_t ble_hs_flow_num_completed_pkts;
+
+/** Periodically sends number-of-completed-packets events.  */
+static struct ble_npl_callout ble_hs_flow_timer;
+
+static ble_npl_event_fn ble_hs_flow_event_cb;
+
+static struct ble_npl_event ble_hs_flow_ev;
+
+static int
+ble_hs_flow_tx_num_comp_pkts(void)
+{
+    uint8_t buf[
+        sizeof(struct ble_hci_cb_host_num_comp_pkts_cp) +
+        sizeof(struct ble_hci_cb_host_num_comp_pkts_entry)
+    ];
+    struct ble_hci_cb_host_num_comp_pkts_cp *cmd = (void *) buf;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    /* For each connection with completed packets, send a separate
+     * host-number-of-completed-packets command.
+     */
+    for (conn = ble_hs_conn_first();
+         conn != NULL;
+         conn = SLIST_NEXT(conn, bhc_next)) {
+
+        if (conn->bhc_completed_pkts > 0) {
+            /* Only specify one connection per command. */
+            /* TODO could combine this in single HCI command */
+            cmd->handles = 1;
+
+            /* Append entry for this connection. */
+            cmd->h[0].handle = htole16(conn->bhc_handle);
+            cmd->h[0].count = htole16(conn->bhc_completed_pkts);
+
+            conn->bhc_completed_pkts = 0;
+
+            /* The host-number-of-completed-packets command does not elicit a
+             * response from the controller, so don't use the normal blocking
+             * HCI API when sending it.
+             */
+            rc = ble_hs_hci_cmd_send_buf(
+                BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                           BLE_HCI_OCF_CB_HOST_NUM_COMP_PKTS),
+                buf, sizeof(buf));
+            if (rc != 0) {
+                return rc;
+            }
+        }
+    }
+
+    return 0;
+}
+
+static void
+ble_hs_flow_event_cb(struct ble_npl_event *ev)
+{
+    int rc;
+
+    ble_hs_lock();
+
+    if (ble_hs_flow_num_completed_pkts > 0) {
+        rc = ble_hs_flow_tx_num_comp_pkts();
+        if (rc != 0) {
+            ble_hs_sched_reset(rc);
+        }
+
+        ble_hs_flow_num_completed_pkts = 0;
+    }
+
+    ble_hs_unlock();
+}
+
+static void
+ble_hs_flow_inc_completed_pkts(struct ble_hs_conn *conn)
+{
+    uint16_t num_free;
+
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    conn->bhc_completed_pkts++;
+    ble_hs_flow_num_completed_pkts++;
+
+    if (ble_hs_flow_num_completed_pkts > MYNEWT_VAL(BLE_ACL_BUF_COUNT)) {
+        ble_hs_sched_reset(BLE_HS_ECONTROLLER);
+        return;
+    }
+
+    /* If the number of free buffers is at or below the configured threshold,
+     * send an immediate number-of-copmleted-packets event.
+     */
+    num_free = MYNEWT_VAL(BLE_ACL_BUF_COUNT) - ble_hs_flow_num_completed_pkts;
+    if (num_free <= MYNEWT_VAL(BLE_HS_FLOW_CTRL_THRESH)) {
+        ble_npl_eventq_put(ble_hs_evq_get(), &ble_hs_flow_ev);
+        ble_npl_callout_stop(&ble_hs_flow_timer);
+    } else if (ble_hs_flow_num_completed_pkts == 1) {
+        rc = ble_npl_callout_reset(&ble_hs_flow_timer, BLE_HS_FLOW_ITVL_TICKS);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static os_error_t
+ble_hs_flow_acl_free(struct os_mempool_ext *mpe, void *data, void *arg)
+{
+    struct ble_hs_conn *conn;
+    const struct os_mbuf *om;
+    uint16_t conn_handle;
+    int rc;
+
+    om = data;
+
+    /* An ACL data packet must be a single mbuf, and it must contain the
+     * corresponding connection handle in its user header.
+     */
+    assert(OS_MBUF_IS_PKTHDR(om));
+    assert(OS_MBUF_USRHDR_LEN(om) >= sizeof conn_handle);
+
+    /* Copy the connection handle out of the mbuf. */
+    memcpy(&conn_handle, OS_MBUF_USRHDR(om), sizeof conn_handle);
+
+    /* Free the mbuf back to its pool. */
+    rc = os_memblock_put_from_cb(&mpe->mpe_mp, data);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Allow nested locks - there are too many places where acl buffers can get
+     * freed.
+     */
+    ble_hs_lock_nested();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        ble_hs_flow_inc_completed_pkts(conn);
+    }
+
+    ble_hs_unlock_nested();
+
+    return 0;
+}
+#endif /* MYNEWT_VAL(BLE_HS_FLOW_CTRL) */
+
+void
+ble_hs_flow_connection_broken(uint16_t conn_handle)
+{
+#if MYNEWT_VAL(BLE_HS_FLOW_CTRL) &&                 \
+    MYNEWT_VAL(BLE_HS_FLOW_CTRL_TX_ON_DISCONNECT)
+    ble_hs_lock();
+    ble_hs_flow_tx_num_comp_pkts();
+    ble_hs_unlock();
+#endif
+}
+
+/**
+ * Fills the user header of an incoming data packet.  On function return, the
+ * header contains the connection handle associated with the sender.
+ *
+ * If flow control is disabled, this function is a no-op.
+ */
+void
+ble_hs_flow_fill_acl_usrhdr(struct os_mbuf *om)
+{
+#if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
+    const struct hci_data_hdr *hdr;
+    uint16_t *conn_handle;
+
+    BLE_HS_DBG_ASSERT(OS_MBUF_USRHDR_LEN(om) >= sizeof *conn_handle);
+    conn_handle = OS_MBUF_USRHDR(om);
+
+    hdr = (void *)om->om_data;
+    *conn_handle = BLE_HCI_DATA_HANDLE(hdr->hdh_handle_pb_bc);
+#endif
+}
+
+/**
+ * Sends the HCI commands to the controller required for enabling host flow
+ * control.
+ *
+ * If flow control is disabled, this function is a no-op.
+ */
+int
+ble_hs_flow_startup(void)
+{
+#if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
+    struct ble_hci_cb_ctlr_to_host_fc_cp enable_cmd;
+    struct ble_hci_cb_host_buf_size_cp buf_size_cmd = {
+            .acl_data_len = htole16(MYNEWT_VAL(BLE_ACL_BUF_SIZE)),
+            .acl_num = htole16(MYNEWT_VAL(BLE_ACL_BUF_COUNT)),
+    };
+    int rc;
+
+    ble_npl_event_init(&ble_hs_flow_ev, ble_hs_flow_event_cb, NULL);
+
+    /* Assume failure. */
+    ble_hci_trans_set_acl_free_cb(NULL, NULL);
+
+#if MYNEWT_VAL(SELFTEST)
+    ble_npl_callout_stop(&ble_hs_flow_timer);
+#endif
+
+    enable_cmd.enable = BLE_HCI_CTLR_TO_HOST_FC_ACL;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                                      BLE_HCI_OCF_CB_SET_CTLR_TO_HOST_FC),
+                           &enable_cmd, sizeof(enable_cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                                      BLE_HCI_OCF_CB_HOST_BUF_SIZE),
+                           &buf_size_cmd, sizeof(buf_size_cmd), NULL, 0);
+    if (rc != 0) {
+        enable_cmd.enable = BLE_HCI_CTLR_TO_HOST_FC_OFF;
+        ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                                     BLE_HCI_OCF_CB_SET_CTLR_TO_HOST_FC),
+                          &enable_cmd, sizeof(enable_cmd), NULL, 0);
+        return rc;
+    }
+
+    /* Flow control successfully enabled. */
+    ble_hs_flow_num_completed_pkts = 0;
+    ble_hci_trans_set_acl_free_cb(ble_hs_flow_acl_free, NULL);
+    ble_npl_callout_init(&ble_hs_flow_timer, ble_hs_evq_get(),
+                         ble_hs_flow_event_cb, NULL);
+#endif
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_flow_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_flow_priv.h
new file mode 100644
index 00000000..b1aa8c2f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_flow_priv.h
@@ -0,0 +1,36 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_FLOW_PRIV_
+#define H_BLE_HS_FLOW_PRIV_
+
+#include <inttypes.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_hs_flow_connection_broken(uint16_t conn_handle);
+void ble_hs_flow_fill_acl_usrhdr(struct os_mbuf *om);
+int ble_hs_flow_startup(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci.c
new file mode 100644
index 00000000..a334a747
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci.c
@@ -0,0 +1,622 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include "os/os.h"
+#include "mem/mem.h"
+#include "nimble/ble_hci_trans.h"
+#include "host/ble_monitor.h"
+#include "ble_hs_priv.h"
+#include "ble_monitor_priv.h"
+
+#define BLE_HCI_CMD_TIMEOUT_MS  2000
+
+static struct ble_npl_mutex ble_hs_hci_mutex;
+static struct ble_npl_sem ble_hs_hci_sem;
+
+static struct ble_hci_ev *ble_hs_hci_ack;
+static uint16_t ble_hs_hci_buf_sz;
+static uint8_t ble_hs_hci_max_pkts;
+
+/* For now 32-bits of features is enough */
+static uint32_t ble_hs_hci_sup_feat;
+
+static uint8_t ble_hs_hci_version;
+
+#define BLE_HS_HCI_FRAG_DATABUF_SIZE    \
+    (BLE_ACL_MAX_PKT_SIZE +             \
+     BLE_HCI_DATA_HDR_SZ +              \
+     sizeof (struct os_mbuf_pkthdr) +   \
+     sizeof (struct os_mbuf))
+
+#define BLE_HS_HCI_FRAG_MEMBLOCK_SIZE   \
+    (OS_ALIGN(BLE_HS_HCI_FRAG_DATABUF_SIZE, 4))
+
+#define BLE_HS_HCI_FRAG_MEMPOOL_SIZE    \
+    OS_MEMPOOL_SIZE(1, BLE_HS_HCI_FRAG_MEMBLOCK_SIZE)
+
+/**
+ *  A one-element mbuf pool dedicated to holding outgoing ACL data packets.
+ *  This dedicated pool prevents a deadlock caused by mbuf exhaustion.  Without
+ *  this pool, all msys mbufs could be permanently allocated, preventing us
+ *  from fragmenting outgoing packets and sending them (and ultimately freeing
+ *  them).
+ */
+static os_membuf_t ble_hs_hci_frag_data[BLE_HS_HCI_FRAG_MEMPOOL_SIZE];
+static struct os_mbuf_pool ble_hs_hci_frag_mbuf_pool;
+static struct os_mempool ble_hs_hci_frag_mempool;
+
+/**
+ * The number of available ACL transmit buffers on the controller.  This
+ * variable must only be accessed while the host mutex is locked.
+ */
+uint16_t ble_hs_hci_avail_pkts;
+
+#if MYNEWT_VAL(BLE_HS_PHONY_HCI_ACKS)
+static ble_hs_hci_phony_ack_fn *ble_hs_hci_phony_ack_cb;
+#endif
+
+#if MYNEWT_VAL(BLE_HS_PHONY_HCI_ACKS)
+void
+ble_hs_hci_set_phony_ack_cb(ble_hs_hci_phony_ack_fn *cb)
+{
+    ble_hs_hci_phony_ack_cb = cb;
+}
+#endif
+
+static void
+ble_hs_hci_lock(void)
+{
+    int rc;
+
+    rc = ble_npl_mutex_pend(&ble_hs_hci_mutex, BLE_NPL_TIME_FOREVER);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+}
+
+static void
+ble_hs_hci_unlock(void)
+{
+    int rc;
+
+    rc = ble_npl_mutex_release(&ble_hs_hci_mutex);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+}
+
+int
+ble_hs_hci_set_buf_sz(uint16_t pktlen, uint16_t max_pkts)
+{
+    if (pktlen == 0 || max_pkts == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_hci_buf_sz = pktlen;
+    ble_hs_hci_max_pkts = max_pkts;
+    ble_hs_hci_avail_pkts = max_pkts;
+
+    return 0;
+}
+
+/**
+ * Increases the count of available controller ACL buffers.
+ */
+void
+ble_hs_hci_add_avail_pkts(uint16_t delta)
+{
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    if (ble_hs_hci_avail_pkts + delta > UINT16_MAX) {
+        ble_hs_sched_reset(BLE_HS_ECONTROLLER);
+    } else {
+        ble_hs_hci_avail_pkts += delta;
+    }
+}
+
+static int
+ble_hs_hci_rx_cmd_complete(const void *data, int len,
+                           struct ble_hs_hci_ack *out_ack)
+{
+    const struct ble_hci_ev_command_complete *ev = data;
+    const struct ble_hci_ev_command_complete_nop *nop = data;
+    uint16_t opcode;
+
+    if (len < sizeof(*ev)) {
+        if (len < sizeof(*nop)) {
+            return BLE_HS_ECONTROLLER;
+        }
+
+        /* nop is special as it doesn't have status and response */
+
+        opcode = le16toh(nop->opcode);
+        if (opcode != BLE_HCI_OPCODE_NOP) {
+            return BLE_HS_ECONTROLLER;
+        }
+
+        /* TODO Process num_pkts field. */
+
+        out_ack->bha_status = 0;
+        out_ack->bha_params = NULL;
+        out_ack->bha_params_len = 0;
+        return 0;
+    }
+
+    opcode = le16toh(ev->opcode);
+
+    /* TODO Process num_pkts field. */
+
+    out_ack->bha_opcode = opcode;
+
+    out_ack->bha_status = BLE_HS_HCI_ERR(ev->status);
+    out_ack->bha_params_len = len - sizeof(*ev);
+    if (out_ack->bha_params_len) {
+        out_ack->bha_params = ev->return_params;
+    } else {
+        out_ack->bha_params = NULL;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_hci_rx_cmd_status(const void *data, int len,
+                         struct ble_hs_hci_ack *out_ack)
+{
+    const struct ble_hci_ev_command_status *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    /* XXX: Process num_pkts field. */
+
+    out_ack->bha_opcode = le16toh(ev->opcode);
+    out_ack->bha_params = NULL;
+    out_ack->bha_params_len = 0;
+    out_ack->bha_status = BLE_HS_HCI_ERR(ev->status);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_process_ack(uint16_t expected_opcode,
+                       uint8_t *params_buf, uint8_t params_buf_len,
+                       struct ble_hs_hci_ack *out_ack)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_hci_ack != NULL);
+
+    /* Count events received */
+    STATS_INC(ble_hs_stats, hci_event);
+
+
+    /* Clear ack fields up front to silence spurious gcc warnings. */
+    memset(out_ack, 0, sizeof *out_ack);
+
+    switch (ble_hs_hci_ack->opcode) {
+    case BLE_HCI_EVCODE_COMMAND_COMPLETE:
+        rc = ble_hs_hci_rx_cmd_complete(ble_hs_hci_ack->data,
+                                        ble_hs_hci_ack->length, out_ack);
+        break;
+
+    case BLE_HCI_EVCODE_COMMAND_STATUS:
+        rc = ble_hs_hci_rx_cmd_status(ble_hs_hci_ack->data,
+                                      ble_hs_hci_ack->length, out_ack);
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        rc = BLE_HS_EUNKNOWN;
+        break;
+    }
+
+    if (rc == 0) {
+        if (params_buf == NULL || out_ack->bha_params == NULL) {
+            out_ack->bha_params_len = 0;
+        } else {
+            if (out_ack->bha_params_len > params_buf_len) {
+                out_ack->bha_params_len = params_buf_len;
+                rc = BLE_HS_ECONTROLLER;
+            }
+            memcpy(params_buf, out_ack->bha_params, out_ack->bha_params_len);
+        }
+        out_ack->bha_params = params_buf;
+
+        if (out_ack->bha_opcode != expected_opcode) {
+            rc = BLE_HS_ECONTROLLER;
+        }
+    }
+
+    if (rc != 0) {
+        STATS_INC(ble_hs_stats, hci_invalid_ack);
+    }
+
+    return rc;
+}
+
+static int
+ble_hs_hci_wait_for_ack(void)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_PHONY_HCI_ACKS)
+    if (ble_hs_hci_phony_ack_cb == NULL) {
+        rc = BLE_HS_ETIMEOUT_HCI;
+    } else {
+        ble_hs_hci_ack =
+            (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+        BLE_HS_DBG_ASSERT(ble_hs_hci_ack != NULL);
+        rc = ble_hs_hci_phony_ack_cb((void *)ble_hs_hci_ack, 260);
+    }
+#else
+    rc = ble_npl_sem_pend(&ble_hs_hci_sem,
+                          ble_npl_time_ms_to_ticks32(BLE_HCI_CMD_TIMEOUT_MS));
+    switch (rc) {
+    case 0:
+        BLE_HS_DBG_ASSERT(ble_hs_hci_ack != NULL);
+
+#if BLE_MONITOR
+        ble_monitor_send(BLE_MONITOR_OPCODE_EVENT_PKT, (void *) ble_hs_hci_ack,
+                         sizeof(*ble_hs_hci_ack) + ble_hs_hci_ack->length);
+#endif
+
+        break;
+    case OS_TIMEOUT:
+        rc = BLE_HS_ETIMEOUT_HCI;
+        STATS_INC(ble_hs_stats, hci_timeout);
+        break;
+    default:
+        rc = BLE_HS_EOS;
+        break;
+    }
+#endif
+
+    return rc;
+}
+
+int
+ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len,
+                  void *rsp, uint8_t rsp_len)
+{
+    struct ble_hs_hci_ack ack;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_hci_ack == NULL);
+    ble_hs_hci_lock();
+
+    rc = ble_hs_hci_cmd_send_buf(opcode, cmd, cmd_len);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = ble_hs_hci_wait_for_ack();
+    if (rc != 0) {
+        ble_hs_sched_reset(rc);
+        goto done;
+    }
+
+    rc = ble_hs_hci_process_ack(opcode, rsp, rsp_len, &ack);
+    if (rc != 0) {
+        ble_hs_sched_reset(rc);
+        goto done;
+    }
+
+    rc = ack.bha_status;
+
+    /* on success we should always get full response */
+    if (!rc && (ack.bha_params_len != rsp_len)) {
+        ble_hs_sched_reset(rc);
+        goto done;
+    }
+
+done:
+    if (ble_hs_hci_ack != NULL) {
+        ble_hci_trans_buf_free((uint8_t *) ble_hs_hci_ack);
+        ble_hs_hci_ack = NULL;
+    }
+
+    ble_hs_hci_unlock();
+    return rc;
+}
+
+static void
+ble_hs_hci_rx_ack(uint8_t *ack_ev)
+{
+    if (ble_npl_sem_get_count(&ble_hs_hci_sem) > 0) {
+        /* This ack is unexpected; ignore it. */
+        ble_hci_trans_buf_free(ack_ev);
+        return;
+    }
+    BLE_HS_DBG_ASSERT(ble_hs_hci_ack == NULL);
+
+    /* Unblock the application now that the HCI command buffer is populated
+     * with the acknowledgement.
+     */
+    ble_hs_hci_ack = (struct ble_hci_ev *) ack_ev;
+    ble_npl_sem_release(&ble_hs_hci_sem);
+}
+
+int
+ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg)
+{
+    struct ble_hci_ev *ev = (void *) hci_ev;
+    struct ble_hci_ev_command_complete *cmd_complete = (void *) ev->data;
+    struct ble_hci_ev_command_status *cmd_status = (void *) ev->data;
+    int enqueue;
+
+    BLE_HS_DBG_ASSERT(hci_ev != NULL);
+
+    switch (ev->opcode) {
+    case BLE_HCI_EVCODE_COMMAND_COMPLETE:
+        enqueue = (cmd_complete->opcode == BLE_HCI_OPCODE_NOP);
+        break;
+    case BLE_HCI_EVCODE_COMMAND_STATUS:
+        enqueue = (cmd_status->opcode == BLE_HCI_OPCODE_NOP);
+        break;
+    default:
+        enqueue = 1;
+        break;
+    }
+
+    if (enqueue) {
+        ble_hs_enqueue_hci_event(hci_ev);
+    } else {
+        ble_hs_hci_rx_ack(hci_ev);
+    }
+
+    return 0;
+}
+
+/**
+ * Calculates the largest ACL payload that the controller can accept.
+ */
+static uint16_t
+ble_hs_hci_max_acl_payload_sz(void)
+{
+    /* As per BLE 5.1 Standard, Vol. 2, Part E, section 7.8.2:
+     * The LE_Read_Buffer_Size command is used to read the maximum size of the
+     * data portion of HCI LE ACL Data Packets sent from the Host to the
+     * Controller.
+     */
+    return ble_hs_hci_buf_sz;
+}
+
+/**
+ * Allocates an mbuf to contain an outgoing ACL data fragment.
+ */
+static struct os_mbuf *
+ble_hs_hci_frag_alloc(uint16_t frag_size, void *arg)
+{
+    struct os_mbuf *om;
+
+    /* Prefer the dedicated one-element fragment pool. */
+    om = os_mbuf_get_pkthdr(&ble_hs_hci_frag_mbuf_pool, 0);
+    if (om != NULL) {
+        om->om_data += BLE_HCI_DATA_HDR_SZ;
+        return om;
+    }
+
+    /* Otherwise, fall back to msys. */
+    om = ble_hs_mbuf_acl_pkt();
+    if (om != NULL) {
+        return om;
+    }
+
+    return NULL;
+}
+
+/**
+ * Retrieves the total capacity of the ACL fragment pool (always 1).
+ */
+int
+ble_hs_hci_frag_num_mbufs(void)
+{
+    return ble_hs_hci_frag_mempool.mp_num_blocks;
+}
+
+/**
+ * Retrieves the the count of free buffers in the ACL fragment pool.
+ */
+int
+ble_hs_hci_frag_num_mbufs_free(void)
+{
+    return ble_hs_hci_frag_mempool.mp_num_free;
+}
+
+static struct os_mbuf *
+ble_hs_hci_acl_hdr_prepend(struct os_mbuf *om, uint16_t handle,
+                           uint8_t pb_flag)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om2;
+
+    hci_hdr.hdh_handle_pb_bc =
+        ble_hs_hci_util_handle_pb_bc_join(handle, pb_flag, 0);
+    put_le16(&hci_hdr.hdh_len, OS_MBUF_PKTHDR(om)->omp_len);
+
+    om2 = os_mbuf_prepend(om, sizeof hci_hdr);
+    if (om2 == NULL) {
+        return NULL;
+    }
+
+    om = om2;
+    om = os_mbuf_pullup(om, sizeof hci_hdr);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    memcpy(om->om_data, &hci_hdr, sizeof hci_hdr);
+
+#if !BLE_MONITOR
+    BLE_HS_LOG(DEBUG, "host tx hci data; handle=%d length=%d\n", handle,
+               get_le16(&hci_hdr.hdh_len));
+#endif
+
+    return om;
+}
+
+int
+ble_hs_hci_acl_tx_now(struct ble_hs_conn *conn, struct os_mbuf **om)
+{
+    struct os_mbuf *txom;
+    struct os_mbuf *frag;
+    uint8_t pb;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    txom = *om;
+    *om = NULL;
+
+    if (!(conn->bhc_flags & BLE_HS_CONN_F_TX_FRAG)) {
+        /* The first fragment uses the first-non-flush packet boundary value.
+         * After sending the first fragment, pb gets set appropriately for all
+         * subsequent fragments in this packet.
+         */
+        pb = BLE_HCI_PB_FIRST_NON_FLUSH;
+    } else {
+        pb = BLE_HCI_PB_MIDDLE;
+    }
+
+    /* Send fragments until the entire packet has been sent. */
+    while (txom != NULL && ble_hs_hci_avail_pkts > 0) {
+        frag = mem_split_frag(&txom, ble_hs_hci_max_acl_payload_sz(),
+                              ble_hs_hci_frag_alloc, NULL);
+        if (frag == NULL) {
+            *om = txom;
+            return BLE_HS_EAGAIN;
+        }
+
+        frag = ble_hs_hci_acl_hdr_prepend(frag, conn->bhc_handle, pb);
+        if (frag == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+#if !BLE_MONITOR
+        BLE_HS_LOG(DEBUG, "ble_hs_hci_acl_tx(): ");
+        ble_hs_log_mbuf(frag);
+        BLE_HS_LOG(DEBUG, "\n");
+#endif
+
+        rc = ble_hs_tx_data(frag);
+        if (rc != 0) {
+            goto err;
+        }
+
+        /* If any fragments remain, they should be marked as 'middle'
+         * fragments.
+         */
+        conn->bhc_flags |= BLE_HS_CONN_F_TX_FRAG;
+        pb = BLE_HCI_PB_MIDDLE;
+
+        /* Account for the controller buf that will hold the txed fragment. */
+        conn->bhc_outstanding_pkts++;
+        ble_hs_hci_avail_pkts--;
+    }
+
+    if (txom != NULL) {
+        /* The controller couldn't accommodate some or all of the packet. */
+        *om = txom;
+        return BLE_HS_EAGAIN;
+    }
+
+    /* The entire packet was transmitted. */
+    conn->bhc_flags &= ~BLE_HS_CONN_F_TX_FRAG;
+
+    return 0;
+
+err:
+    BLE_HS_DBG_ASSERT(rc != 0);
+
+    conn->bhc_flags &= ~BLE_HS_CONN_F_TX_FRAG;
+    os_mbuf_free_chain(txom);
+    return rc;
+}
+
+/**
+ * Transmits an HCI ACL data packet.  This function consumes the supplied mbuf,
+ * regardless of the outcome.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EAGAIN if the packet could not be sent
+ *                                  in its entirety due to controller buffer
+ *                                  exhaustion.  The unsent data is pointed to
+ *                                  by the `om` parameter.
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ *
+ */
+int
+ble_hs_hci_acl_tx(struct ble_hs_conn *conn, struct os_mbuf **om)
+{
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    /* If this conn is already backed up, don't even try to send. */
+    if (STAILQ_FIRST(&conn->bhc_tx_q) != NULL) {
+        return BLE_HS_EAGAIN;
+    }
+
+    return ble_hs_hci_acl_tx_now(conn, om);
+}
+
+void
+ble_hs_hci_set_le_supported_feat(uint32_t feat)
+{
+    ble_hs_hci_sup_feat = feat;
+}
+
+uint32_t
+ble_hs_hci_get_le_supported_feat(void)
+{
+    return ble_hs_hci_sup_feat;
+}
+
+void
+ble_hs_hci_set_hci_version(uint8_t hci_version)
+{
+    ble_hs_hci_version = hci_version;
+}
+
+uint8_t
+ble_hs_hci_get_hci_version(void)
+{
+    return ble_hs_hci_version;
+}
+
+void
+ble_hs_hci_init(void)
+{
+    int rc;
+
+    rc = ble_npl_sem_init(&ble_hs_hci_sem, 0);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    rc = ble_npl_mutex_init(&ble_hs_hci_mutex);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    rc = mem_init_mbuf_pool(ble_hs_hci_frag_data,
+                            &ble_hs_hci_frag_mempool,
+                            &ble_hs_hci_frag_mbuf_pool,
+                            1,
+                            BLE_HS_HCI_FRAG_MEMBLOCK_SIZE,
+                            "ble_hs_hci_frag");
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_cmd.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_cmd.c
new file mode 100644
index 00000000..a0fd1cea
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_cmd.c
@@ -0,0 +1,102 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+#include <stdint.h>
+#include <assert.h>
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include "os/os.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "host/ble_monitor.h"
+#include "ble_hs_priv.h"
+#include "ble_monitor_priv.h"
+
+static int
+ble_hs_hci_cmd_transport(struct ble_hci_cmd *cmd)
+{
+    int rc;
+
+#if BLE_MONITOR
+    ble_monitor_send(BLE_MONITOR_OPCODE_COMMAND_PKT, cmd,
+                     cmd->length + sizeof(*cmd));
+#endif
+
+    rc = ble_hci_trans_hs_cmd_tx((uint8_t *) cmd);
+    switch (rc) {
+    case 0:
+        return 0;
+
+    case BLE_ERR_MEM_CAPACITY:
+        return BLE_HS_ENOMEM_EVT;
+
+    default:
+        return BLE_HS_EUNKNOWN;
+    }
+}
+
+static int
+ble_hs_hci_cmd_send(uint16_t opcode, uint8_t len, const void *cmddata)
+{
+    struct ble_hci_cmd *cmd;
+    int rc;
+
+    cmd = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+    BLE_HS_DBG_ASSERT(cmd != NULL);
+
+    cmd->opcode = htole16(opcode);
+    cmd->length = len;
+    if (len != 0) {
+        memcpy(cmd->data, cmddata, len);
+    }
+
+    rc = ble_hs_hci_cmd_transport(cmd);
+
+    if (rc == 0) {
+        STATS_INC(ble_hs_stats, hci_cmd);
+    } else {
+        BLE_HS_LOG(DEBUG, "ble_hs_hci_cmd_send failure; rc=%d\n", rc);
+    }
+
+    return rc;
+}
+
+int
+ble_hs_hci_cmd_send_buf(uint16_t opcode, const void *buf, uint8_t buf_len)
+{
+    switch (ble_hs_sync_state) {
+    case BLE_HS_SYNC_STATE_BAD:
+        return BLE_HS_ENOTSYNCED;
+
+    case BLE_HS_SYNC_STATE_BRINGUP:
+        if (!ble_hs_is_parent_task()) {
+            return BLE_HS_ENOTSYNCED;
+        }
+        break;
+
+    case BLE_HS_SYNC_STATE_GOOD:
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HS_EUNKNOWN;
+    }
+
+    return ble_hs_hci_cmd_send(opcode, buf_len, buf);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_evt.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_evt.c
new file mode 100644
index 00000000..e8ba7119
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_evt.c
@@ -0,0 +1,884 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include "os/os.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "host/ble_gap.h"
+#include "host/ble_monitor.h"
+#include "ble_hs_priv.h"
+
+_Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ,
+               "struct hci_data_hdr must be 4 bytes");
+
+typedef int ble_hs_hci_evt_fn(uint8_t event_code, const void *data,
+                              unsigned int len);
+static ble_hs_hci_evt_fn ble_hs_hci_evt_disconn_complete;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_encrypt_change;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_hw_error;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_num_completed_pkts;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_enc_key_refresh;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_le_meta;
+
+typedef int ble_hs_hci_evt_le_fn(uint8_t subevent, const void *data,
+                                 unsigned int len);
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_conn_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_adv_rpt;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_conn_upd_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_lt_key_req;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_conn_parm_req;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_dir_adv_rpt;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_phy_update_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_ext_adv_rpt;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_rd_rem_used_feat_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_scan_timeout;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_adv_set_terminated;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_periodic_adv_sync_estab;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_periodic_adv_rpt;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_periodic_adv_sync_lost;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_scan_req_rcvd;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_enh_conn_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_periodic_adv_sync_transfer;
+
+/* Statistics */
+struct host_hci_stats
+{
+    uint32_t events_rxd;
+    uint32_t good_acks_rxd;
+    uint32_t bad_acks_rxd;
+    uint32_t unknown_events_rxd;
+};
+
+#define BLE_HS_HCI_EVT_TIMEOUT        50      /* Milliseconds. */
+
+/** Dispatch table for incoming HCI events.  Sorted by event code field. */
+struct ble_hs_hci_evt_dispatch_entry {
+    uint8_t event_code;
+    ble_hs_hci_evt_fn *cb;
+};
+
+static const struct ble_hs_hci_evt_dispatch_entry ble_hs_hci_evt_dispatch[] = {
+    { BLE_HCI_EVCODE_LE_META, ble_hs_hci_evt_le_meta },
+    { BLE_HCI_EVCODE_NUM_COMP_PKTS, ble_hs_hci_evt_num_completed_pkts },
+    { BLE_HCI_EVCODE_DISCONN_CMP, ble_hs_hci_evt_disconn_complete },
+    { BLE_HCI_EVCODE_ENCRYPT_CHG, ble_hs_hci_evt_encrypt_change },
+    { BLE_HCI_EVCODE_ENC_KEY_REFRESH, ble_hs_hci_evt_enc_key_refresh },
+    { BLE_HCI_EVCODE_HW_ERROR, ble_hs_hci_evt_hw_error },
+};
+
+#define BLE_HS_HCI_EVT_DISPATCH_SZ \
+    (sizeof ble_hs_hci_evt_dispatch / sizeof ble_hs_hci_evt_dispatch[0])
+
+static ble_hs_hci_evt_le_fn * const ble_hs_hci_evt_le_dispatch[] = {
+    [BLE_HCI_LE_SUBEV_CONN_COMPLETE] = ble_hs_hci_evt_le_conn_complete,
+    [BLE_HCI_LE_SUBEV_ADV_RPT] = ble_hs_hci_evt_le_adv_rpt,
+    [BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE] = ble_hs_hci_evt_le_conn_upd_complete,
+    [BLE_HCI_LE_SUBEV_LT_KEY_REQ] = ble_hs_hci_evt_le_lt_key_req,
+    [BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ] = ble_hs_hci_evt_le_conn_parm_req,
+    [BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE] = ble_hs_hci_evt_le_enh_conn_complete,
+    [BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT] = ble_hs_hci_evt_le_dir_adv_rpt,
+    [BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE] = ble_hs_hci_evt_le_phy_update_complete,
+    [BLE_HCI_LE_SUBEV_EXT_ADV_RPT] = ble_hs_hci_evt_le_ext_adv_rpt,
+    [BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB] = ble_hs_hci_evt_le_periodic_adv_sync_estab,
+    [BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT] = ble_hs_hci_evt_le_periodic_adv_rpt,
+    [BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST] = ble_hs_hci_evt_le_periodic_adv_sync_lost,
+    [BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT] = ble_hs_hci_evt_le_rd_rem_used_feat_complete,
+    [BLE_HCI_LE_SUBEV_SCAN_TIMEOUT] = ble_hs_hci_evt_le_scan_timeout,
+    [BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED] = ble_hs_hci_evt_le_adv_set_terminated,
+    [BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD] = ble_hs_hci_evt_le_scan_req_rcvd,
+    [BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER] = ble_hs_hci_evt_le_periodic_adv_sync_transfer,
+};
+
+#define BLE_HS_HCI_EVT_LE_DISPATCH_SZ \
+    (sizeof ble_hs_hci_evt_le_dispatch / sizeof ble_hs_hci_evt_le_dispatch[0])
+
+static const struct ble_hs_hci_evt_dispatch_entry *
+ble_hs_hci_evt_dispatch_find(uint8_t event_code)
+{
+    const struct ble_hs_hci_evt_dispatch_entry *entry;
+    int i;
+
+    for (i = 0; i < BLE_HS_HCI_EVT_DISPATCH_SZ; i++) {
+        entry = ble_hs_hci_evt_dispatch + i;
+        if (entry->event_code == event_code) {
+            return entry;
+        }
+    }
+
+    return NULL;
+}
+
+static ble_hs_hci_evt_le_fn *
+ble_hs_hci_evt_le_dispatch_find(uint8_t event_code)
+{
+    if (event_code >= BLE_HS_HCI_EVT_LE_DISPATCH_SZ) {
+        return NULL;
+    }
+
+    return ble_hs_hci_evt_le_dispatch[event_code];
+}
+
+static int
+ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data,
+                                unsigned int len)
+{
+    const struct ble_hci_ev_disconn_cmp *ev = data;
+    const struct ble_hs_conn *conn;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(le16toh(ev->conn_handle));
+    if (conn != NULL) {
+        ble_hs_hci_add_avail_pkts(conn->bhc_outstanding_pkts);
+    }
+    ble_hs_unlock();
+
+    ble_gap_rx_disconn_complete(ev);
+
+    /* The connection termination may have freed up some capacity in the
+     * controller for additional ACL data packets.  Wake up any stalled
+     * connections.
+     */
+    ble_hs_wakeup_tx();
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_encrypt_change(uint8_t event_code, const void *data,
+                              unsigned int len)
+{
+    const struct ble_hci_ev_enrypt_chg *ev = data;
+
+    if (len != sizeof (*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_sm_enc_change_rx(ev);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_hw_error(uint8_t event_code, const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_hw_error *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_hs_hw_error(ev->hw_code);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_enc_key_refresh(uint8_t event_code, const void *data,
+                               unsigned int len)
+{
+    const struct ble_hci_ev_enc_key_refresh *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_sm_enc_key_refresh_rx(ev);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_num_completed_pkts(uint8_t event_code, const void *data,
+                                  unsigned int len)
+{
+    const struct ble_hci_ev_num_comp_pkts *ev = data;
+    struct ble_hs_conn *conn;
+    uint16_t num_pkts;
+    int i;
+
+    if (len != sizeof(*ev) + (ev->count * sizeof(ev->completed[0]))) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    for (i = 0; i < ev->count; i++) {
+        num_pkts = le16toh(ev->completed[i].packets);
+
+        if (num_pkts > 0) {
+            ble_hs_lock();
+            conn = ble_hs_conn_find(le16toh(ev->completed[i].handle));
+            if (conn != NULL) {
+                if (conn->bhc_outstanding_pkts < num_pkts) {
+                    ble_hs_sched_reset(BLE_HS_ECONTROLLER);
+                } else {
+                    conn->bhc_outstanding_pkts -= num_pkts;
+                }
+
+                ble_hs_hci_add_avail_pkts(num_pkts);
+            }
+            ble_hs_unlock();
+        }
+    }
+
+    /* If any transmissions have stalled, wake them up now. */
+    ble_hs_wakeup_tx();
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_meta(uint8_t event_code, const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_le_meta *ev = data;
+    ble_hs_hci_evt_le_fn *fn;
+
+    if (len < sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    fn = ble_hs_hci_evt_le_dispatch_find(ev->subevent);
+    if (fn) {
+        return fn(ev->subevent, data, len);
+    }
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+static struct ble_gap_conn_complete pend_conn_complete;
+#endif
+
+static int
+ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data,
+                                    unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_enh_conn_complete *ev = data;
+    struct ble_gap_conn_complete evt;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    memset(&evt, 0, sizeof(evt));
+
+    evt.status = ev->status;
+
+    if (evt.status == BLE_ERR_SUCCESS) {
+        evt.connection_handle = le16toh(ev->conn_handle);
+        evt.role = ev->role;
+        evt.peer_addr_type = ev->peer_addr_type;
+        memcpy(evt.peer_addr, ev->peer_addr, BLE_DEV_ADDR_LEN);
+        memcpy(evt.local_rpa, ev->local_rpa, BLE_DEV_ADDR_LEN);
+        memcpy(evt.peer_rpa,ev->peer_rpa, BLE_DEV_ADDR_LEN);
+        evt.conn_itvl = le16toh(ev->conn_itvl);
+        evt.conn_latency = le16toh(ev->conn_latency);
+        evt.supervision_timeout = le16toh(ev->supervision_timeout);
+        evt.master_clk_acc = ev->mca;
+    } else {
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        evt.connection_handle = BLE_HS_CONN_HANDLE_NONE;
+#endif
+    }
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    if (evt.status == BLE_ERR_DIR_ADV_TMO ||
+                            evt.role == BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE) {
+    /* store this until we get set terminated event with adv handle */
+        memcpy(&pend_conn_complete, &evt, sizeof(evt));
+        return 0;
+    }
+#endif
+    return ble_gap_rx_conn_complete(&evt, 0);
+
+}
+
+static int
+ble_hs_hci_evt_le_conn_complete(uint8_t subevent, const void *data,
+                                unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_conn_complete *ev = data;
+    struct ble_gap_conn_complete evt;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    memset(&evt, 0, sizeof(evt));
+
+    evt.status = ev->status;
+
+    if (evt.status == BLE_ERR_SUCCESS) {
+        evt.connection_handle = le16toh(ev->conn_handle);
+        evt.role = ev->role;
+        evt.peer_addr_type = ev->peer_addr_type;
+        memcpy(evt.peer_addr, ev->peer_addr, BLE_DEV_ADDR_LEN);
+
+        evt.conn_itvl = le16toh(ev->conn_itvl);
+        evt.conn_latency = le16toh(ev->conn_latency);
+        evt.supervision_timeout = le16toh(ev->supervision_timeout);
+        evt.master_clk_acc = ev->mca;
+    } else {
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        evt.connection_handle = BLE_HS_CONN_HANDLE_NONE;
+#endif
+    }
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    if (evt.status == BLE_ERR_DIR_ADV_TMO ||
+                            evt.role == BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE) {
+    /* store this until we get set terminated event with adv handle */
+        memcpy(&pend_conn_complete, &evt, sizeof(evt));
+        return 0;
+    }
+#endif
+    return ble_gap_rx_conn_complete(&evt, 0);
+}
+
+static int
+ble_hs_hci_evt_le_adv_rpt_first_pass(const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_adv_rpt *ev = data;
+    const struct adv_report *rpt;
+    int i;
+
+    if (len < sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    len -= sizeof(*ev);
+    data += sizeof(*ev);
+
+    if (ev->num_reports < BLE_HCI_LE_ADV_RPT_NUM_RPTS_MIN ||
+        ev->num_reports > BLE_HCI_LE_ADV_RPT_NUM_RPTS_MAX) {
+        return BLE_HS_EBADDATA;
+    }
+
+    for (i = 0; i < ev->num_reports; i++) {
+        /* extra byte for RSSI after adv data */
+        if (len < sizeof(*rpt) + 1) {
+            return BLE_HS_ECONTROLLER;
+        }
+
+        rpt = data;
+
+        len -= sizeof(*rpt) + 1;
+        data += sizeof(rpt) + 1;
+
+        if (rpt->data_len > len) {
+            return BLE_HS_ECONTROLLER;
+        }
+
+        len -= rpt->data_len;
+        data += rpt->data_len;
+    }
+
+    /* Make sure length was correct */
+    if (len) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_adv_rpt *ev = data;
+    struct ble_gap_disc_desc desc = {0};
+    const struct adv_report *rpt;
+    int rc;
+    int i;
+
+    /* Validate the event is formatted correctly */
+    rc = ble_hs_hci_evt_le_adv_rpt_first_pass(data, len);
+    if (rc != 0) {
+        return rc;
+    }
+
+    data += sizeof(*ev);
+
+    desc.direct_addr = *BLE_ADDR_ANY;
+
+    for (i = 0; i < ev->num_reports; i++) {
+        rpt = data;
+
+        data += sizeof(rpt) + rpt->data_len + 1;
+
+        desc.event_type = rpt->type;
+        desc.addr.type = rpt->addr_type;
+        memcpy(desc.addr.val, rpt->addr, BLE_DEV_ADDR_LEN);
+        desc.length_data = rpt->data_len;
+        desc.data = rpt->data;
+        desc.rssi = rpt->data[rpt->data_len];
+
+        ble_gap_rx_adv_report(&desc);
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_dir_adv_rpt(uint8_t subevent, const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_direct_adv_rpt *ev = data;
+    struct ble_gap_disc_desc desc = {0};
+    int i;
+
+    if (len < sizeof(*ev) || len != ev->num_reports * sizeof(ev->reports[0])) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    /* Data fields not present in a direct advertising report. */
+    desc.data = NULL;
+    desc.length_data = 0;
+
+    for (i = 0; i < ev->num_reports; i++) {
+        desc.event_type = ev->reports[i].type;
+        desc.addr.type = ev->reports[i].addr_type;
+        memcpy(desc.addr.val, ev->reports[i].addr, BLE_DEV_ADDR_LEN);
+        desc.direct_addr.type = ev->reports[i].dir_addr_type;
+        memcpy(desc.direct_addr.val, ev->reports[i].dir_addr, BLE_DEV_ADDR_LEN);
+        desc.rssi = ev->reports[i].rssi;
+
+        ble_gap_rx_adv_report(&desc);
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_rd_rem_used_feat_complete(uint8_t subevent, const void *data,
+                                            unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_rd_rem_used_feat *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_gap_rx_rd_rem_sup_feat_complete(ev);
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN
+static int
+ble_hs_hci_decode_legacy_type(uint16_t evt_type)
+{
+     switch (evt_type) {
+     case BLE_HCI_LEGACY_ADV_EVTYPE_ADV_IND:
+         return BLE_HCI_ADV_RPT_EVTYPE_ADV_IND;
+     case BLE_HCI_LEGACY_ADV_EVTYPE_ADV_DIRECT_IND:
+         return BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
+     case BLE_HCI_LEGACY_ADV_EVTYPE_ADV_SCAN_IND:
+         return BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND;
+     case BLE_HCI_LEGACY_ADV_EVTYPE_ADV_NONCON_IND:
+         return BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND;
+     case BLE_HCI_LEGACY_ADV_EVTYPE_SCAN_RSP_ADV_IND:
+         return BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP;
+     default:
+         return -1;
+     }
+}
+#endif
+
+static int
+ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data,
+                              unsigned int len)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN
+    const struct ble_hci_ev_le_subev_ext_adv_rpt *ev = data;
+    const struct ext_adv_report *report;
+    struct ble_gap_ext_disc_desc desc;
+    int i;
+    int legacy_event_type;
+
+    if (len < sizeof(*ev)) {
+        return BLE_HS_EBADDATA;
+    }
+
+    if (ev->num_reports < BLE_HCI_LE_ADV_RPT_NUM_RPTS_MIN ||
+        ev->num_reports > BLE_HCI_LE_ADV_RPT_NUM_RPTS_MAX) {
+        return BLE_HS_EBADDATA;
+    }
+
+    /* TODO properly validate len of the event */
+
+    report = &ev->reports[0];
+    for (i = 0; i < ev->num_reports; i++) {
+        memset(&desc, 0, sizeof(desc));
+
+        desc.props = (report->evt_type) & 0x1F;
+        if (desc.props & BLE_HCI_ADV_LEGACY_MASK) {
+            legacy_event_type = ble_hs_hci_decode_legacy_type(report->evt_type);
+            if (legacy_event_type < 0) {
+                report = (const void *) &report->data[report->data_len];
+                continue;
+            }
+            desc.legacy_event_type = legacy_event_type;
+            desc.data_status = BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE;
+        } else {
+            switch(report->evt_type & BLE_HCI_ADV_DATA_STATUS_MASK) {
+            case BLE_HCI_ADV_DATA_STATUS_COMPLETE:
+                desc.data_status = BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE;
+                break;
+            case BLE_HCI_ADV_DATA_STATUS_INCOMPLETE:
+                desc.data_status = BLE_GAP_EXT_ADV_DATA_STATUS_INCOMPLETE;
+                break;
+            case BLE_HCI_ADV_DATA_STATUS_TRUNCATED:
+                desc.data_status = BLE_GAP_EXT_ADV_DATA_STATUS_TRUNCATED;
+                break;
+            default:
+                assert(false);
+            }
+        }
+        desc.addr.type = report->addr_type;
+        memcpy(desc.addr.val, report->addr, 6);
+        desc.length_data = report->data_len;
+        desc.data = report->data;
+        desc.rssi = report->rssi;
+        desc.tx_power = report->tx_power;
+        memcpy(desc.direct_addr.val, report->dir_addr, 6);
+        desc.direct_addr.type = report->dir_addr_type;
+        desc.sid = report->sid;
+        desc.prim_phy = report->pri_phy;
+        desc.sec_phy = report->sec_phy;
+        desc.periodic_adv_itvl = report->periodic_itvl;
+
+        ble_gap_rx_ext_adv_report(&desc);
+
+        report = (const void *) &report->data[report->data_len];
+    }
+#endif
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_periodic_adv_sync_estab(uint8_t subevent, const void *data,
+                                          unsigned int len)
+{
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_gap_rx_peroidic_adv_sync_estab(ev);
+#endif
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_periodic_adv_rpt(uint8_t subevent, const void *data,
+                                   unsigned int len)
+{
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    const struct ble_hci_ev_le_subev_periodic_adv_rpt *ev = data;
+
+    if (len < sizeof(*ev) || len != (sizeof(*ev) + ev->data_len)) {
+        return BLE_HS_EBADDATA;
+    }
+
+    ble_gap_rx_periodic_adv_rpt(ev);
+#endif
+
+return 0;
+}
+
+static int
+ble_hs_hci_evt_le_periodic_adv_sync_lost(uint8_t subevent, const void *data,
+                                         unsigned int len)
+{
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    const struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_EBADDATA;
+    }
+
+    ble_gap_rx_periodic_adv_sync_lost(ev);
+
+#endif
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_periodic_adv_sync_transfer(uint8_t subevent, const void *data,
+                                             unsigned int len)
+{
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+    const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_EBADDATA;
+    }
+
+    ble_gap_rx_periodic_adv_sync_transfer(ev);
+
+#endif
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_scan_timeout(uint8_t subevent, const void *data,
+                               unsigned int len)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN
+    const struct ble_hci_ev_le_subev_scan_timeout *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_EBADDATA;
+    }
+
+    ble_gap_rx_le_scan_timeout();
+#endif
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_adv_set_terminated(uint8_t subevent, const void *data,
+                                     unsigned int len)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    const struct ble_hci_ev_le_subev_adv_set_terminated *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    if (ev->status == 0) {
+        /* ignore return code as we need to terminate advertising set anyway */
+        ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle);
+    }
+    ble_gap_rx_adv_set_terminated(ev);
+#endif
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_scan_req_rcvd(uint8_t subevent, const void *data,
+                                unsigned int len)
+{
+#if MYNEWT_VAL(BLE_EXT_ADV)
+    const struct ble_hci_ev_le_subev_scan_req_rcvd *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_gap_rx_scan_req_rcvd(ev);
+#endif
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_conn_upd_complete(uint8_t subevent, const void *data,
+                                    unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_conn_upd_complete *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    if (ev->status == 0) {
+        BLE_HS_DBG_ASSERT(le16toh(ev->conn_itvl) >= BLE_HCI_CONN_ITVL_MIN);
+        BLE_HS_DBG_ASSERT(le16toh(ev->conn_itvl) <= BLE_HCI_CONN_ITVL_MAX);
+
+        BLE_HS_DBG_ASSERT(le16toh(ev->conn_latency) >= BLE_HCI_CONN_LATENCY_MIN);
+        BLE_HS_DBG_ASSERT(le16toh(ev->conn_latency) <= BLE_HCI_CONN_LATENCY_MAX);
+
+        BLE_HS_DBG_ASSERT(le16toh(ev->supervision_timeout) >= BLE_HCI_CONN_SPVN_TIMEOUT_MIN);
+        BLE_HS_DBG_ASSERT(le16toh(ev->supervision_timeout) <= BLE_HCI_CONN_SPVN_TIMEOUT_MAX);
+    }
+
+    ble_gap_rx_update_complete(ev);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_lt_key_req(uint8_t subevent, const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_lt_key_req *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_sm_ltk_req_rx(ev);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_conn_parm_req(uint8_t subevent, const void *data, unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_rem_conn_param_req *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    BLE_HS_DBG_ASSERT(le16toh(ev->min_interval) >= BLE_HCI_CONN_ITVL_MIN);
+    BLE_HS_DBG_ASSERT(le16toh(ev->max_interval) <= BLE_HCI_CONN_ITVL_MAX);
+    BLE_HS_DBG_ASSERT(le16toh(ev->max_interval) >= le16toh(ev->min_interval));
+
+    BLE_HS_DBG_ASSERT(le16toh(ev->latency) >= BLE_HCI_CONN_LATENCY_MIN);
+    BLE_HS_DBG_ASSERT(le16toh(ev->latency) <= BLE_HCI_CONN_LATENCY_MAX);
+
+    BLE_HS_DBG_ASSERT(le16toh(ev->timeout) >= BLE_HCI_CONN_SPVN_TIMEOUT_MIN);
+    BLE_HS_DBG_ASSERT(le16toh(ev->timeout) <= BLE_HCI_CONN_SPVN_TIMEOUT_MAX);
+
+    ble_gap_rx_param_req(ev);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_phy_update_complete(uint8_t subevent, const void *data,
+                                      unsigned int len)
+{
+    const struct ble_hci_ev_le_subev_phy_update_complete *ev = data;
+
+    if (len != sizeof(*ev)) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_gap_rx_phy_update_complete(ev);
+
+    return 0;
+}
+
+int
+ble_hs_hci_evt_process(const struct ble_hci_ev *ev)
+{
+    const struct ble_hs_hci_evt_dispatch_entry *entry;
+    int rc;
+
+    /* Count events received */
+    STATS_INC(ble_hs_stats, hci_event);
+
+
+    entry = ble_hs_hci_evt_dispatch_find(ev->opcode);
+    if (entry == NULL) {
+        STATS_INC(ble_hs_stats, hci_unknown_event);
+        rc = BLE_HS_ENOTSUP;
+    } else {
+        rc = entry->cb(ev->opcode, ev->data, ev->length);
+    }
+
+    ble_hci_trans_buf_free((uint8_t *) ev);
+
+    return rc;
+}
+
+/**
+ * Called when a data packet is received from the controller.  This function
+ * consumes the supplied mbuf, regardless of the outcome.
+ *
+ * @param om                    The incoming data packet, beginning with the
+ *                                  HCI ACL data header.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_hs_hci_evt_acl_process(struct os_mbuf *om)
+{
+    struct hci_data_hdr hci_hdr;
+    struct ble_hs_conn *conn;
+    ble_l2cap_rx_fn *rx_cb;
+    uint16_t conn_handle;
+    int reject_cid;
+    int rc;
+
+    rc = ble_hs_hci_util_data_hdr_strip(om, &hci_hdr);
+    if (rc != 0) {
+        goto err;
+    }
+
+#if (BLETEST_THROUGHPUT_TEST == 0)
+#if !BLE_MONITOR
+    BLE_HS_LOG(DEBUG, "ble_hs_hci_evt_acl_process(): conn_handle=%u pb=%x "
+                      "len=%u data=",
+               BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc),
+               BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc),
+               hci_hdr.hdh_len);
+    ble_hs_log_mbuf(om);
+    BLE_HS_LOG(DEBUG, "\n");
+#endif
+#endif
+
+    if (hci_hdr.hdh_len != OS_MBUF_PKTHDR(om)->omp_len) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
+    conn_handle = BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        /* Peer not connected; quietly discard packet. */
+        rc = BLE_HS_ENOTCONN;
+        reject_cid = -1;
+    } else {
+        /* Forward ACL data to L2CAP. */
+        rc = ble_l2cap_rx(conn, &hci_hdr, om, &rx_cb, &reject_cid);
+        om = NULL;
+    }
+
+    ble_hs_unlock();
+
+    switch (rc) {
+    case 0:
+        /* Final fragment received. */
+        BLE_HS_DBG_ASSERT(rx_cb != NULL);
+        rc = rx_cb(conn->bhc_rx_chan);
+        ble_l2cap_remove_rx(conn, conn->bhc_rx_chan);
+        break;
+
+    case BLE_HS_EAGAIN:
+        /* More fragments on the way. */
+        break;
+
+    default:
+        if (reject_cid != -1) {
+            ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, 0, 0, reject_cid);
+        }
+        goto err;
+    }
+
+    return 0;
+
+err:
+    os_mbuf_free_chain(om);
+    return rc;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_priv.h
new file mode 100644
index 00000000..b02d4ab2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_priv.h
@@ -0,0 +1,125 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_HCI_PRIV_
+#define H_BLE_HS_HCI_PRIV_
+
+#include "nimble/hci_common.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_conn;
+struct os_mbuf;
+
+#define BLE_HS_HCI_LE_FEAT_ENCRYPTION                   (0x00000001)
+#define BLE_HS_HCI_LE_FEAT_CONN_PARAM_REQUEST           (0x00000002)
+#define BLE_HS_HCI_LE_FEAT_EXT_REJECT                   (0x00000004)
+#define BLE_HS_HCI_LE_FEAT_SLAVE_FEAT_EXCHANGE          (0x00000008)
+#define BLE_HS_HCI_LE_FEAT_PING                         (0x00000010)
+#define BLE_HS_HCI_LE_FEAT_DATA_PACKET_LENGTH_EXT       (0x00000020)
+#define BLE_HS_HCI_LE_FEAT_LL_PRIVACY                   (0x00000040)
+#define BLE_HS_HCI_LE_FEAT_EXT_SCANNER_FILTER_POLICIES  (0x00000080)
+#define BLE_HS_HCI_LE_FEAT_2M_PHY                       (0x00000100)
+#define BLE_HS_HCI_LE_FEAT_STABLE_MOD_INDEX_TX          (0x00000200)
+#define BLE_HS_HCI_LE_FEAT_STABLE_MOD_INDEX_RX          (0x00000400)
+#define BLE_HS_HCI_LE_FEAT_CODED_PHY                    (0x00000800)
+#define BLE_HS_HCI_LE_FEAT_EXT_ADV                      (0x00001000)
+#define BLE_HS_HCI_LE_FEAT_PERIODIC_ADV                 (0x00002000)
+#define BLE_HS_HCI_LE_FEAT_CSA2                         (0x00004000)
+#define BLE_HS_HCI_LE_FEAT_POWER_CLASS_1                (0x00008000)
+#define BLE_HS_HCI_LE_FEAT_MIN_NUM_USED_CHAN            (0x00010000)
+
+struct ble_hs_hci_ack {
+    int bha_status;         /* A BLE_HS_E<...> error; NOT a naked HCI code. */
+    const uint8_t *bha_params;
+    int bha_params_len;
+    uint16_t bha_opcode;
+    uint8_t bha_hci_handle;
+};
+
+#if MYNEWT_VAL(BLE_EXT_ADV)
+struct ble_hs_hci_ext_scan_param {
+    uint8_t scan_type;
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+};
+
+struct ble_hs_hci_ext_conn_params {
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    uint16_t conn_itvl;
+    uint16_t conn_windows;
+};
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+/* Periodic Advertising Parameters */
+struct hci_periodic_adv_params
+{
+    uint16_t min_interval;
+    uint16_t max_interval;
+    uint16_t properties;
+};
+#endif
+#endif
+
+extern uint16_t ble_hs_hci_avail_pkts;
+
+int ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len,
+                      void *rsp, uint8_t rsp_len);
+void ble_hs_hci_init(void);
+
+void ble_hs_hci_set_le_supported_feat(uint32_t feat);
+uint32_t ble_hs_hci_get_le_supported_feat(void);
+void ble_hs_hci_set_hci_version(uint8_t hci_version);
+uint8_t ble_hs_hci_get_hci_version(void);
+
+#if MYNEWT_VAL(BLE_HS_PHONY_HCI_ACKS)
+typedef int ble_hs_hci_phony_ack_fn(uint8_t *ack, int ack_buf_len);
+void ble_hs_hci_set_phony_ack_cb(ble_hs_hci_phony_ack_fn *cb);
+#endif
+
+int ble_hs_hci_util_read_adv_tx_pwr(int8_t *out_pwr);
+int ble_hs_hci_util_rand(void *dst, int len);
+int ble_hs_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi);
+int ble_hs_hci_util_set_random_addr(const uint8_t *addr);
+int ble_hs_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
+                                 uint16_t tx_time);
+int ble_hs_hci_util_data_hdr_strip(struct os_mbuf *om,
+                                   struct hci_data_hdr *out_hdr);
+int ble_hs_hci_evt_process(const struct ble_hci_ev *ev);
+
+int ble_hs_hci_cmd_send_buf(uint16_t opcode, const void *buf, uint8_t buf_len);
+int ble_hs_hci_set_buf_sz(uint16_t pktlen, uint16_t max_pkts);
+void ble_hs_hci_add_avail_pkts(uint16_t delta);
+
+uint16_t ble_hs_hci_util_handle_pb_bc_join(uint16_t handle, uint8_t pb,
+                                           uint8_t bc);
+
+int ble_hs_hci_acl_tx_now(struct ble_hs_conn *conn, struct os_mbuf **om);
+int ble_hs_hci_acl_tx(struct ble_hs_conn *conn, struct os_mbuf **om);
+
+int ble_hs_hci_frag_num_mbufs(void);
+int ble_hs_hci_frag_num_mbufs_free(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_util.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_util.c
new file mode 100644
index 00000000..996e0fc1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_hci_util.c
@@ -0,0 +1,215 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include "nimble/hci_common.h"
+#include "host/ble_hs_hci.h"
+#include "ble_hs_priv.h"
+
+uint16_t
+ble_hs_hci_util_handle_pb_bc_join(uint16_t handle, uint8_t pb, uint8_t bc)
+{
+    BLE_HS_DBG_ASSERT(handle <= 0x0fff);
+    BLE_HS_DBG_ASSERT(pb <= 0x03);
+    BLE_HS_DBG_ASSERT(bc <= 0x03);
+
+    return (handle  << 0)   |
+           (pb      << 12)  |
+           (bc      << 14);
+}
+
+int
+ble_hs_hci_util_read_adv_tx_pwr(int8_t *out_tx_pwr)
+{
+    struct ble_hci_le_rd_adv_chan_txpwr_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
+                           NULL, 0, &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    *out_tx_pwr = rsp.power_level;
+
+    if (*out_tx_pwr < BLE_HCI_ADV_CHAN_TXPWR_MIN ||
+        *out_tx_pwr > BLE_HCI_ADV_CHAN_TXPWR_MAX) {
+        BLE_HS_LOG(WARN, "advertiser txpwr out of range\n");
+    }
+
+    return 0;
+}
+
+int
+ble_hs_hci_util_rand(void *dst, int len)
+{
+    struct ble_hci_le_rand_rp rsp;
+    uint8_t *u8ptr;
+    int chunk_sz;
+    int rc;
+
+    u8ptr = dst;
+    while (len > 0) {
+        rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RAND),
+                               NULL, 0, &rsp, sizeof(rsp));
+        if (rc != 0) {
+            return rc;
+        }
+
+        chunk_sz = min(len, sizeof(rsp));
+        memcpy(u8ptr, &rsp.random_number, chunk_sz);
+
+        len -= chunk_sz;
+        u8ptr += chunk_sz;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi)
+{
+    struct ble_hci_rd_rssi_cp cmd;
+    struct ble_hci_rd_rssi_rp rsp;
+
+    int rc;
+
+    cmd.handle = htole16(conn_handle);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_STATUS_PARAMS,
+                                      BLE_HCI_OCF_RD_RSSI), &cmd, sizeof(cmd),
+                           &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (le16toh(rsp.handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    *out_rssi = rsp.rssi;
+
+    return 0;
+}
+
+int
+ble_hs_hci_util_set_random_addr(const uint8_t *addr)
+{
+    struct ble_hci_le_set_rand_addr_cp cmd;
+
+    memcpy(cmd.addr, addr, BLE_DEV_ADDR_LEN);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_RAND_ADDR),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_hs_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
+                             uint16_t tx_time)
+{
+    struct ble_hci_le_set_data_len_cp cmd;
+    struct ble_hci_le_set_data_len_rp rsp;
+    int rc;
+
+    if (tx_octets < BLE_HCI_SET_DATALEN_TX_OCTETS_MIN ||
+        tx_octets > BLE_HCI_SET_DATALEN_TX_OCTETS_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (tx_time < BLE_HCI_SET_DATALEN_TX_TIME_MIN ||
+        tx_time > BLE_HCI_SET_DATALEN_TX_TIME_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    cmd.conn_handle = htole16(conn_handle);
+    cmd.tx_octets = htole16(tx_octets);
+    cmd.tx_time = htole16(tx_time);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_SET_DATA_LEN),
+                           &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (le16toh(rsp.conn_handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_hci_util_data_hdr_strip(struct os_mbuf *om,
+                               struct hci_data_hdr *out_hdr)
+{
+    int rc;
+
+    rc = os_mbuf_copydata(om, 0, BLE_HCI_DATA_HDR_SZ, out_hdr);
+    if (rc != 0) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    /* Strip HCI ACL data header from the front of the packet. */
+    os_mbuf_adj(om, BLE_HCI_DATA_HDR_SZ);
+
+    out_hdr->hdh_handle_pb_bc = get_le16(&out_hdr->hdh_handle_pb_bc);
+    out_hdr->hdh_len = get_le16(&out_hdr->hdh_len);
+
+    return 0;
+}
+
+int
+ble_hs_hci_read_chan_map(uint16_t conn_handle, uint8_t *out_chan_map)
+{
+    struct ble_hci_le_rd_chan_map_cp cmd;
+    struct ble_hci_le_rd_chan_map_rp rsp;
+    int rc;
+
+    cmd.conn_handle = htole16(conn_handle);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_RD_CHAN_MAP),
+                           &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (le16toh(rsp.conn_handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    memcpy(out_chan_map, rsp.chan_map, 5);
+
+    return 0;
+}
+
+int
+ble_hs_hci_set_chan_class(const uint8_t *chan_map)
+{
+    struct ble_hci_le_set_host_chan_class_cp cmd;
+
+    memcpy(cmd.chan_map, chan_map, sizeof(cmd.chan_map));
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_HOST_CHAN_CLASS),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_id.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_id.c
new file mode 100644
index 00000000..e8b6c8b6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_id.c
@@ -0,0 +1,306 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include "host/ble_hs_id.h"
+#include "ble_hs_priv.h"
+
+static uint8_t ble_hs_id_pub[6];
+static uint8_t ble_hs_id_rnd[6];
+
+void
+ble_hs_id_set_pub(const uint8_t *pub_addr)
+{
+    ble_hs_lock();
+    memcpy(ble_hs_id_pub, pub_addr, 6);
+    ble_hs_unlock();
+}
+
+int
+ble_hs_id_gen_rnd(int nrpa, ble_addr_t *out_addr)
+{
+    int rc;
+
+    out_addr->type = BLE_ADDR_RANDOM;
+
+    rc = ble_hs_hci_util_rand(out_addr->val, 6);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (nrpa) {
+        out_addr->val[5] &= ~0xc0;
+    } else {
+        out_addr->val[5] |= 0xc0;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_id_set_rnd(const uint8_t *rnd_addr)
+{
+    uint8_t addr_type_byte;
+    int rc;
+    int ones;
+
+    ble_hs_lock();
+
+    /* Make sure random part of rnd_addr is not all ones or zeros. Reference:
+     * Core v5.0, Vol 6, Part B, section 1.3.2.1 */
+    addr_type_byte = rnd_addr[5] & 0xc0;
+
+    /* count bits set to 1 in random part of address */
+    ones = __builtin_popcount(rnd_addr[0]);
+    ones += __builtin_popcount(rnd_addr[1]);
+    ones += __builtin_popcount(rnd_addr[2]);
+    ones += __builtin_popcount(rnd_addr[3]);
+    ones += __builtin_popcount(rnd_addr[4]);
+    ones += __builtin_popcount(rnd_addr[5] & 0x3f);
+
+    if ((addr_type_byte != 0x00 && addr_type_byte != 0xc0) ||
+            (ones == 0 || ones == 46)) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    rc = ble_hs_hci_util_set_random_addr(rnd_addr);
+    if (rc != 0) {
+        goto done;
+    }
+
+    memcpy(ble_hs_id_rnd, rnd_addr, 6);
+
+done:
+    ble_hs_unlock();
+    return rc;
+}
+
+/**
+ * Retrieves one of the device's identity addresses.  The device can have two
+ * identity addresses: one public and one random.  The id_addr_type argument
+ * specifies which of these two addresses to retrieve.
+ *
+ * @param id_addr_type          The type of identity address to retrieve.
+ *                                  Valid values are:
+ *                                      o BLE_ADDR_PUBLIC
+ *                                      o BLE_ADDR_RANDOM
+ * @param out_id_addr           On success, this is reseated to point to the
+ *                                  retrieved 6-byte identity address.  Pass
+ *                                  NULL if you do not require this
+ *                                  information.
+
+ * @param out_is_nrpa           On success, the pointed-to value indicates
+ *                                  whether the retrieved address is a
+ *                                  non-resolvable private address.  Pass NULL
+ *                                  if you do not require this information.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if an invalid address type was
+ *                                  specified;
+ *                              BLE_HS_ENOADDR if the device does not have an
+ *                                  identity address of the requested type;
+ *                              Other BLE host core code on error.
+ */
+int
+ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr,
+               int *out_is_nrpa)
+{
+    const uint8_t *id_addr;
+    int nrpa;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    switch (id_addr_type) {
+    case BLE_ADDR_PUBLIC:
+        id_addr = ble_hs_id_pub;
+        nrpa = 0;
+        break;
+
+    case BLE_ADDR_RANDOM:
+        id_addr = ble_hs_id_rnd;
+        nrpa = (ble_hs_id_rnd[5] & 0xc0) == 0;
+        break;
+
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    if (memcmp(id_addr, ble_hs_misc_null_addr, 6) == 0) {
+        return BLE_HS_ENOADDR;
+    }
+
+    if (out_id_addr != NULL) {
+        *out_id_addr = id_addr;
+    }
+    if (out_is_nrpa != NULL) {
+        *out_is_nrpa = nrpa;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_id_copy_addr(uint8_t id_addr_type, uint8_t *out_id_addr,
+                    int *out_is_nrpa)
+{
+    const uint8_t *addr;
+    int rc;
+
+    ble_hs_lock();
+
+    rc = ble_hs_id_addr(id_addr_type, &addr, out_is_nrpa);
+    if (rc == 0 && out_id_addr != NULL) {
+        memcpy(out_id_addr, addr, 6);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+static int
+ble_hs_id_addr_type_usable(uint8_t own_addr_type)
+{
+    uint8_t id_addr_type;
+    int nrpa;
+    int rc;
+
+    switch (own_addr_type) {
+    case BLE_OWN_ADDR_PUBLIC:
+    case BLE_OWN_ADDR_RANDOM:
+        rc = ble_hs_id_addr(own_addr_type, NULL, NULL);
+        if (rc != 0) {
+            return rc;
+        }
+        break;
+
+    case BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT:
+    case BLE_OWN_ADDR_RPA_RANDOM_DEFAULT:
+        id_addr_type = ble_hs_misc_own_addr_type_to_id(own_addr_type);
+        rc = ble_hs_id_addr(id_addr_type, NULL, &nrpa);
+        if (rc != 0) {
+            return rc;
+        }
+        if (nrpa) {
+            return BLE_HS_ENOADDR;
+        }
+        break;
+
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_id_use_addr(uint8_t own_addr_type)
+{
+    int rc;
+
+    rc = ble_hs_id_addr_type_usable(own_addr_type);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* If privacy is being used, make sure RPA rotation is in effect. */
+    if (own_addr_type == BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT ||
+        own_addr_type == BLE_OWN_ADDR_RPA_RANDOM_DEFAULT) {
+
+        rc = ble_hs_pvcy_ensure_started();
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+int
+ble_hs_id_infer_auto(int privacy, uint8_t *out_addr_type)
+{
+    static const uint8_t pub_addr_types[] = {
+        BLE_OWN_ADDR_RANDOM,
+        BLE_OWN_ADDR_PUBLIC,
+    };
+    static const uint8_t priv_addr_types[] = {
+        BLE_OWN_ADDR_RPA_RANDOM_DEFAULT,
+        BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+    };
+    const uint8_t *addr_types;
+    uint8_t addr_type;
+    int num_addr_types;
+    int rc;
+    int i;
+
+    ble_hs_lock();
+
+    if (privacy) {
+        addr_types = priv_addr_types;
+        num_addr_types = sizeof priv_addr_types / sizeof priv_addr_types[0];
+    } else {
+        addr_types = pub_addr_types;
+        num_addr_types = sizeof pub_addr_types / sizeof pub_addr_types[0];
+    }
+
+    for (i = 0; i < num_addr_types; i++) {
+        addr_type = addr_types[i];
+
+        rc = ble_hs_id_addr_type_usable(addr_type);
+        switch (rc) {
+        case 0:
+            *out_addr_type = addr_type;
+            goto done;
+
+        case BLE_HS_ENOADDR:
+            break;
+
+        default:
+            goto done;
+        }
+    }
+
+    rc = BLE_HS_ENOADDR;
+
+done:
+    ble_hs_unlock();
+    return rc;
+}
+
+/**
+ * Clears both the public and random addresses.  This function is necessary
+ * when the controller loses its random address (e.g., on a stack reset).
+ */
+void
+ble_hs_id_reset(void)
+{
+    memset(ble_hs_id_pub, 0, sizeof ble_hs_id_pub);
+    memset(ble_hs_id_rnd, 0, sizeof ble_hs_id_pub);
+}
+
+/**
+ * Clears random address. This function is necessary when the host wants to
+ * clear random address.
+ */
+ void
+ ble_hs_id_rnd_reset(void)
+ {
+    memset(ble_hs_id_rnd, 0, sizeof ble_hs_id_rnd);
+ }
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_id_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_id_priv.h
new file mode 100644
index 00000000..aa2827d4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_id_priv.h
@@ -0,0 +1,40 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_ID_PRIV_
+#define H_BLE_HS_ID_PRIV_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_hs_id_set_pub(const uint8_t *pub_addr);
+int ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr,
+                   int *out_is_nrpa);
+int ble_hs_id_use_addr(uint8_t addr_type);
+void ble_hs_id_reset(void);
+void ble_hs_id_rnd_reset(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_log.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_log.c
new file mode 100644
index 00000000..7ec69469
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_log.c
@@ -0,0 +1,47 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "os/os.h"
+#include "host/ble_hs.h"
+
+struct log ble_hs_log;
+
+void
+ble_hs_log_mbuf(const struct os_mbuf *om)
+{
+    uint8_t u8;
+    int i;
+
+    for (i = 0; i < OS_MBUF_PKTLEN(om); i++) {
+        os_mbuf_copydata(om, i, 1, &u8);
+        BLE_HS_LOG(DEBUG, "0x%02x ", u8);
+    }
+}
+
+void
+ble_hs_log_flat_buf(const void *data, int len)
+{
+    const uint8_t *u8ptr;
+    int i;
+
+    u8ptr = data;
+    for (i = 0; i < len; i++) {
+        BLE_HS_LOG(DEBUG, "0x%02x ", u8ptr[i]);
+    }
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mbuf.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mbuf.c
new file mode 100644
index 00000000..d938d348
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mbuf.c
@@ -0,0 +1,161 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "host/ble_hs.h"
+#include "ble_hs_priv.h"
+
+/**
+ * Allocates an mbuf for use by the nimble host.
+ */
+static struct os_mbuf *
+ble_hs_mbuf_gen_pkt(uint16_t leading_space)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = os_msys_get_pkthdr(0, 0);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    if (om->om_omp->omp_databuf_len < leading_space) {
+        rc = os_mbuf_free_chain(om);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+        return NULL;
+    }
+
+    om->om_data += leading_space;
+
+    return om;
+}
+
+/**
+ * Allocates an mbuf with no leading space.
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_bare_pkt(void)
+{
+    return ble_hs_mbuf_gen_pkt(0);
+}
+
+/**
+ * Allocates an mbuf suitable for an HCI ACL data packet.
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_acl_pkt(void)
+{
+    return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ);
+}
+
+/**
+ * Allocates an mbuf suitable for an L2CAP data packet.  The resulting packet
+ * has sufficient leading space for:
+ *     o ACL data header
+ *     o L2CAP B-frame header
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_l2cap_pkt(void)
+{
+    return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + BLE_L2CAP_HDR_SZ);
+}
+
+struct os_mbuf *
+ble_hs_mbuf_att_pkt(void)
+{
+    /* Prepare write request and response are the larget ATT commands which
+     * contain attribute data.
+     */
+     return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ +
+                                BLE_L2CAP_HDR_SZ +
+                                BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+}
+
+struct os_mbuf *
+ble_hs_mbuf_from_flat(const void *buf, uint16_t len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        return NULL;
+    }
+
+    rc = os_mbuf_copyinto(om, 0, buf, len);
+    if (rc != 0) {
+        os_mbuf_free_chain(om);
+        return NULL;
+    }
+
+    return om;
+}
+
+int
+ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len,
+                    uint16_t *out_copy_len)
+{
+    uint16_t copy_len;
+    int rc;
+
+    if (OS_MBUF_PKTLEN(om) <= max_len) {
+        copy_len = OS_MBUF_PKTLEN(om);
+    } else {
+        copy_len = max_len;
+    }
+
+    rc = os_mbuf_copydata(om, 0, copy_len, flat);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    if (copy_len > max_len) {
+        rc = BLE_HS_EMSGSIZE;
+    } else {
+        rc = 0;
+    }
+
+    if (out_copy_len != NULL) {
+        *out_copy_len = copy_len;
+    }
+    return rc;
+}
+
+int
+ble_hs_mbuf_pullup_base(struct os_mbuf **om, int base_len)
+{
+    if (OS_MBUF_PKTLEN(*om) < base_len) {
+        return BLE_HS_EBADDATA;
+    }
+
+    *om = os_mbuf_pullup(*om, base_len);
+    if (*om == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mbuf_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mbuf_priv.h
new file mode 100644
index 00000000..8923678a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mbuf_priv.h
@@ -0,0 +1,38 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_MBUF_PRIV_
+#define H_BLE_HS_MBUF_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+struct os_mbuf *ble_hs_mbuf_bare_pkt(void);
+struct os_mbuf *ble_hs_mbuf_acl_pkt(void);
+struct os_mbuf *ble_hs_mbuf_l2cap_pkt(void);
+int ble_hs_mbuf_pullup_base(struct os_mbuf **om, int base_len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_misc.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_misc.c
new file mode 100644
index 00000000..6c6da467
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_misc.c
@@ -0,0 +1,145 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include <stdlib.h>
+#include "os/os.h"
+#include "ble_hs_priv.h"
+
+const uint8_t ble_hs_misc_null_addr[6];
+
+int
+ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
+                           struct ble_hs_conn **out_conn,
+                           struct ble_l2cap_chan **out_chan)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        chan = NULL;
+        rc = BLE_HS_ENOTCONN;
+    } else {
+        chan = ble_hs_conn_chan_find_by_scid(conn, cid);
+        if (chan == NULL) {
+            rc = BLE_HS_ENOTCONN;
+        } else {
+            rc = 0;
+        }
+    }
+
+    if (out_conn != NULL) {
+        *out_conn = conn;
+    }
+    if (out_chan != NULL) {
+        *out_chan = chan;
+    }
+
+    return rc;
+}
+
+void
+ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
+                                struct ble_hs_conn **out_conn,
+                                struct ble_l2cap_chan **out_chan)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    rc = ble_hs_misc_conn_chan_find(conn_handle, cid, &conn, &chan);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    if (out_conn != NULL) {
+        *out_conn = conn;
+    }
+    if (out_chan != NULL) {
+        *out_chan = chan;
+    }
+}
+
+uint8_t
+ble_hs_misc_own_addr_type_to_id(uint8_t own_addr_type)
+{
+    switch (own_addr_type) {
+    case BLE_OWN_ADDR_PUBLIC:
+    case BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT:
+         return BLE_ADDR_PUBLIC;
+
+    case BLE_OWN_ADDR_RANDOM:
+    case BLE_OWN_ADDR_RPA_RANDOM_DEFAULT:
+         return BLE_ADDR_RANDOM;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_ADDR_PUBLIC;
+    }
+}
+
+uint8_t
+ble_hs_misc_peer_addr_type_to_id(uint8_t peer_addr_type)
+{
+    switch (peer_addr_type) {
+        case BLE_ADDR_PUBLIC:
+        case BLE_ADDR_PUBLIC_ID:
+            return BLE_ADDR_PUBLIC;
+
+        case BLE_ADDR_RANDOM:
+        case BLE_ADDR_RANDOM_ID:
+            return BLE_ADDR_RANDOM;
+
+        default:
+            BLE_HS_DBG_ASSERT(0);
+            return BLE_ADDR_PUBLIC;
+    }
+}
+
+static int
+ble_hs_misc_restore_one_irk(int obj_type, union ble_store_value *val,
+                            void *cookie)
+{
+    const struct ble_store_value_sec *sec;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(obj_type == BLE_STORE_OBJ_TYPE_PEER_SEC);
+
+    sec = &val->sec;
+    if (sec->irk_present) {
+        rc = ble_hs_pvcy_add_entry(sec->peer_addr.val, sec->peer_addr.type,
+                                   sec->irk);
+        if (rc != 0) {
+            BLE_HS_LOG(ERROR, "failed to configure restored IRK\n");
+        }
+    }
+
+    return 0;
+}
+
+int
+ble_hs_misc_restore_irks(void)
+{
+    int rc;
+
+    rc = ble_store_iterate(BLE_STORE_OBJ_TYPE_PEER_SEC,
+                           ble_hs_misc_restore_one_irk,
+                           NULL);
+    return rc;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mqueue.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mqueue.c
new file mode 100644
index 00000000..2e08efc1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_mqueue.c
@@ -0,0 +1,82 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "ble_hs_priv.h"
+
+int
+ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg)
+{
+    STAILQ_INIT(&mq->head);
+
+    ble_npl_event_init(&mq->ev, ev_fn, ev_arg);
+
+    return (0);
+}
+
+struct os_mbuf *
+ble_mqueue_get(struct ble_mqueue *mq)
+{
+    struct os_mbuf_pkthdr *mp;
+    struct os_mbuf *om;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    mp = STAILQ_FIRST(&mq->head);
+    if (mp) {
+        STAILQ_REMOVE_HEAD(&mq->head, omp_next);
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    if (mp) {
+        om = OS_MBUF_PKTHDR_TO_MBUF(mp);
+    } else {
+        om = NULL;
+    }
+
+    return (om);
+}
+
+int
+ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om)
+{
+    struct os_mbuf_pkthdr *mp;
+    os_sr_t sr;
+    int rc;
+
+    /* Can only place the head of a chained mbuf on the queue. */
+    if (!OS_MBUF_IS_PKTHDR(om)) {
+        rc = OS_EINVAL;
+        goto err;
+    }
+
+    mp = OS_MBUF_PKTHDR(om);
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&mq->head, mp, omp_next);
+    OS_EXIT_CRITICAL(sr);
+
+    /* Only post an event to the queue if its specified */
+    if (evq) {
+        ble_npl_eventq_put(evq, &mq->ev);
+    }
+
+    return (0);
+err:
+    return (rc);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_periodic_sync.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_periodic_sync.c
new file mode 100644
index 00000000..dad53513
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_periodic_sync.c
@@ -0,0 +1,154 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "host/ble_hs_id.h"
+#include "ble_hs_priv.h"
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+static SLIST_HEAD(, ble_hs_periodic_sync) g_ble_hs_periodic_sync_handles;
+static struct os_mempool ble_hs_periodic_sync_pool;
+
+static os_membuf_t ble_hs_psync_elem_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS),
+                    sizeof (struct ble_hs_periodic_sync))
+];
+
+struct ble_hs_periodic_sync *
+ble_hs_periodic_sync_alloc(void)
+{
+    struct ble_hs_periodic_sync *psync;
+
+    psync = os_memblock_get(&ble_hs_periodic_sync_pool);
+    if (psync) {
+        memset(psync, 0, sizeof(*psync));
+    }
+
+    return psync;
+}
+
+void
+ble_hs_periodic_sync_free(struct ble_hs_periodic_sync *psync)
+{
+    int rc;
+
+    if (psync == NULL) {
+        return;
+    }
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    memset(psync, 0xff, sizeof *psync);
+#endif
+    rc = os_memblock_put(&ble_hs_periodic_sync_pool, psync);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+}
+
+void
+ble_hs_periodic_sync_insert(struct ble_hs_periodic_sync *psync)
+{
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    BLE_HS_DBG_ASSERT_EVAL(
+                       ble_hs_periodic_sync_find_by_handle(psync->sync_handle) == NULL);
+
+    SLIST_INSERT_HEAD(&g_ble_hs_periodic_sync_handles, psync, next);
+}
+
+void
+ble_hs_periodic_sync_remove(struct ble_hs_periodic_sync *psync)
+{
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    SLIST_REMOVE(&g_ble_hs_periodic_sync_handles, psync, ble_hs_periodic_sync,
+                 next);
+}
+
+struct ble_hs_periodic_sync *
+ble_hs_periodic_sync_find_by_handle(uint16_t sync_handle)
+{
+    struct ble_hs_periodic_sync *psync;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    SLIST_FOREACH(psync, &g_ble_hs_periodic_sync_handles, next) {
+        if (psync->sync_handle == sync_handle) {
+            return psync;
+        }
+    }
+
+    return NULL;
+}
+
+struct ble_hs_periodic_sync *
+ble_hs_periodic_sync_find(const ble_addr_t *addr, uint8_t sid)
+{
+    struct ble_hs_periodic_sync *psync;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    if (!addr) {
+        return NULL;
+    }
+
+    SLIST_FOREACH(psync, &g_ble_hs_periodic_sync_handles, next) {
+        if ((ble_addr_cmp(&psync->advertiser_addr, addr) == 0) &&
+                (psync->adv_sid == sid)) {
+            return psync;
+        }
+    }
+
+    return NULL;
+}
+
+/**
+ * Retrieves the first periodic discovery handle in the list.
+ */
+struct ble_hs_periodic_sync *
+ble_hs_periodic_sync_first(void)
+{
+    struct ble_hs_periodic_sync *psync;
+
+    ble_hs_lock();
+    psync = SLIST_FIRST(&g_ble_hs_periodic_sync_handles);
+    ble_hs_unlock();
+
+    return psync;
+}
+
+int
+ble_hs_periodic_sync_init(void)
+{
+    int rc;
+
+    rc = os_mempool_init(&ble_hs_periodic_sync_pool,
+                         MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS),
+                         sizeof (struct ble_hs_periodic_sync),
+                         ble_hs_psync_elem_mem, "ble_hs_periodic_disc_pool");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    SLIST_INIT(&g_ble_hs_periodic_sync_handles);
+
+    return 0;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_periodic_sync_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_periodic_sync_priv.h
new file mode 100644
index 00000000..c82ea790
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_periodic_sync_priv.h
@@ -0,0 +1,55 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_PERIODIC_SYNC_
+#define H_BLE_HS_PERIODIC_SYNC_
+
+#include <inttypes.h>
+#include "os/queue.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_periodic_sync {
+    SLIST_ENTRY(ble_hs_periodic_sync) next;
+    uint16_t   sync_handle;
+    ble_addr_t advertiser_addr;
+    uint8_t    adv_sid;
+
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
+    struct ble_npl_event lost_ev;
+};
+
+struct ble_hs_periodic_sync *ble_hs_periodic_sync_alloc(void);
+void ble_hs_periodic_sync_free(struct ble_hs_periodic_sync *psync);
+void ble_hs_periodic_sync_insert(struct ble_hs_periodic_sync *psync);
+void ble_hs_periodic_sync_remove(struct ble_hs_periodic_sync *psync);
+struct ble_hs_periodic_sync *ble_hs_periodic_sync_find_by_handle(uint16_t sync_handle);
+struct ble_hs_periodic_sync *ble_hs_periodic_sync_find(const ble_addr_t *addr,
+                                                       uint8_t sid);
+struct ble_hs_periodic_sync *ble_hs_periodic_sync_first(void);
+int ble_hs_periodic_sync_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_priv.h
new file mode 100644
index 00000000..2cad6ef1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_priv.h
@@ -0,0 +1,157 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_PRIV_
+#define H_BLE_HS_PRIV_
+
+#include <assert.h>
+#include <inttypes.h>
+#include "ble_att_cmd_priv.h"
+#include "ble_att_priv.h"
+#include "ble_gap_priv.h"
+#include "ble_gatt_priv.h"
+#include "ble_hs_hci_priv.h"
+#include "ble_hs_atomic_priv.h"
+#include "ble_hs_conn_priv.h"
+#include "ble_hs_atomic_priv.h"
+#include "ble_hs_mbuf_priv.h"
+#include "ble_hs_startup_priv.h"
+#include "ble_l2cap_priv.h"
+#include "ble_l2cap_sig_priv.h"
+#include "ble_l2cap_coc_priv.h"
+#include "ble_sm_priv.h"
+#include "ble_hs_adv_priv.h"
+#include "ble_hs_flow_priv.h"
+#include "ble_hs_pvcy_priv.h"
+#include "ble_hs_id_priv.h"
+#include "ble_hs_periodic_sync_priv.h"
+#include "ble_uuid_priv.h"
+#include "host/ble_hs.h"
+#include "host/ble_monitor.h"
+#include "nimble/nimble_opt.h"
+#include "stats/stats.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_conn;
+struct ble_l2cap_chan;
+struct os_mbuf;
+struct os_mempool;
+struct os_event;
+
+#define BLE_HS_SYNC_STATE_BAD           0
+#define BLE_HS_SYNC_STATE_BRINGUP       1
+#define BLE_HS_SYNC_STATE_GOOD          2
+
+#define BLE_HS_ENABLED_STATE_OFF        0
+#define BLE_HS_ENABLED_STATE_STOPPING   1
+#define BLE_HS_ENABLED_STATE_ON         2
+
+#if NIMBLE_BLE_CONNECT
+#define BLE_HS_MAX_CONNECTIONS MYNEWT_VAL(BLE_MAX_CONNECTIONS)
+#else
+#define BLE_HS_MAX_CONNECTIONS 0
+#endif
+
+#if !MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE)
+#define BLE_HS_ATT_SVR_QUEUED_WRITE_TMO 0
+#else
+#define BLE_HS_ATT_SVR_QUEUED_WRITE_TMO \
+    MYNEWT_VAL(BLE_ATT_SVR_QUEUED_WRITE_TMO)
+#endif
+
+STATS_SECT_START(ble_hs_stats)
+    STATS_SECT_ENTRY(conn_create)
+    STATS_SECT_ENTRY(conn_delete)
+    STATS_SECT_ENTRY(hci_cmd)
+    STATS_SECT_ENTRY(hci_event)
+    STATS_SECT_ENTRY(hci_invalid_ack)
+    STATS_SECT_ENTRY(hci_unknown_event)
+    STATS_SECT_ENTRY(hci_timeout)
+    STATS_SECT_ENTRY(reset)
+    STATS_SECT_ENTRY(sync)
+    STATS_SECT_ENTRY(pvcy_add_entry)
+    STATS_SECT_ENTRY(pvcy_add_entry_fail)
+STATS_SECT_END
+extern STATS_SECT_DECL(ble_hs_stats) ble_hs_stats;
+
+extern struct os_mbuf_pool ble_hs_mbuf_pool;
+extern uint8_t ble_hs_sync_state;
+extern uint8_t ble_hs_enabled_state;
+
+extern const uint8_t ble_hs_misc_null_addr[6];
+
+extern uint16_t ble_hs_max_attrs;
+extern uint16_t ble_hs_max_services;
+extern uint16_t ble_hs_max_client_configs;
+
+void ble_hs_process_rx_data_queue(void);
+int ble_hs_tx_data(struct os_mbuf *om);
+void ble_hs_wakeup_tx(void);
+void ble_hs_enqueue_hci_event(uint8_t *hci_evt);
+void ble_hs_event_enqueue(struct os_event *ev);
+
+int ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg);
+int ble_hs_hci_evt_acl_process(struct os_mbuf *om);
+
+int ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
+                               struct ble_hs_conn **out_conn,
+                               struct ble_l2cap_chan **out_chan);
+void ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
+                                     struct ble_hs_conn **out_conn,
+                                     struct ble_l2cap_chan **out_chan);
+uint8_t ble_hs_misc_own_addr_type_to_id(uint8_t addr_type);
+uint8_t ble_hs_misc_peer_addr_type_to_id(uint8_t addr_type);
+int ble_hs_misc_restore_irks(void);
+
+int ble_hs_locked_by_cur_task(void);
+int ble_hs_is_parent_task(void);
+void ble_hs_lock_nested(void);
+void ble_hs_unlock_nested(void);
+void ble_hs_lock(void);
+void ble_hs_unlock(void);
+void ble_hs_hw_error(uint8_t hw_code);
+void ble_hs_timer_resched(void);
+void ble_hs_notifications_sched(void);
+struct ble_npl_eventq *ble_hs_evq_get(void);
+void ble_hs_stop_init(void);
+
+struct ble_mqueue {
+    STAILQ_HEAD(, os_mbuf_pkthdr) head;
+    struct ble_npl_event ev;
+};
+
+int ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg);
+struct os_mbuf *ble_mqueue_get(struct ble_mqueue *mq);
+int ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om);
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    #define BLE_HS_DBG_ASSERT(x) assert(x)
+    #define BLE_HS_DBG_ASSERT_EVAL(x) assert(x)
+#else
+    #define BLE_HS_DBG_ASSERT(x)
+    #define BLE_HS_DBG_ASSERT_EVAL(x) ((void)(x))
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_pvcy.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_pvcy.c
new file mode 100644
index 00000000..dc09b51b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_pvcy.c
@@ -0,0 +1,248 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include "stats/stats.h"
+#include "ble_hs_priv.h"
+
+static uint8_t ble_hs_pvcy_started;
+static uint8_t ble_hs_pvcy_irk[16];
+
+/** Use this as a default IRK if none gets set. */
+const uint8_t ble_hs_pvcy_default_irk[16] = {
+    0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+    0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+};
+
+static int
+ble_hs_pvcy_set_addr_timeout(uint16_t timeout)
+{
+    struct ble_hci_le_set_rpa_tmo_cp cmd;
+
+    if (timeout == 0 || timeout > 0xA1B8) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd.rpa_timeout = htole16(timeout);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_RPA_TMO),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_hs_pvcy_set_resolve_enabled(int enable)
+{
+    struct ble_hci_le_set_addr_res_en_cp cmd;
+
+    cmd.enable = enable;
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr)
+{
+    struct ble_hci_le_rmv_resolve_list_cp cmd;
+
+    if (addr_type > BLE_ADDR_RANDOM) {
+        addr_type = addr_type % 2;
+    }
+
+    cmd.peer_addr_type = addr_type;
+    memcpy(cmd.peer_id_addr, addr, BLE_DEV_ADDR_LEN);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_RMV_RESOLV_LIST),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+static int
+ble_hs_pvcy_clear_entries(void)
+{
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
+                             NULL, 0, NULL, 0);
+}
+
+static int
+ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type,
+                          const uint8_t *irk)
+{
+    struct ble_hci_le_add_resolv_list_cp cmd;
+    ble_addr_t peer_addr;
+    int rc;
+
+    if (addr_type > BLE_ADDR_RANDOM) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd.peer_addr_type = addr_type;
+    memcpy(cmd.peer_id_addr, addr, 6);
+    memcpy(cmd.local_irk, ble_hs_pvcy_irk, 16);
+    memcpy(cmd.peer_irk, irk, 16);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
+                           &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* FIXME Controller is BT5.0 and default privacy mode is network which
+     * can cause problems for apps which are not aware of it. We need to
+     * sort it out somehow. For now we set device mode for all of the peer
+     * devices and application should change it to network if needed
+     */
+    peer_addr.type = addr_type;
+    memcpy(peer_addr.val, addr, sizeof peer_addr.val);
+    rc = ble_hs_pvcy_set_mode(&peer_addr, BLE_GAP_PRIVATE_MODE_DEVICE);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type,
+                      const uint8_t *irk)
+{
+    int rc;
+
+    STATS_INC(ble_hs_stats, pvcy_add_entry);
+
+    /* No GAP procedures can be active when adding an entry to the resolving
+     * list (Vol 2, Part E, 7.8.38).  Stop all GAP procedures and temporarily
+     * prevent any new ones from being started.
+     */
+    ble_gap_preempt();
+
+    /* Try to add the entry now that GAP is halted. */
+    rc = ble_hs_pvcy_add_entry_hci(addr, addr_type, irk);
+
+    /* Allow GAP procedures to be started again. */
+    ble_gap_preempt_done();
+
+    if (rc != 0) {
+        STATS_INC(ble_hs_stats, pvcy_add_entry_fail);
+    }
+
+    return rc;
+}
+
+int
+ble_hs_pvcy_ensure_started(void)
+{
+    int rc;
+
+    if (ble_hs_pvcy_started) {
+        return 0;
+    }
+
+    /* Set up the periodic change of our RPA. */
+    rc = ble_hs_pvcy_set_addr_timeout(MYNEWT_VAL(BLE_RPA_TIMEOUT));
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_pvcy_started = 1;
+
+    return 0;
+}
+
+int
+ble_hs_pvcy_set_our_irk(const uint8_t *irk)
+{
+    uint8_t tmp_addr[6];
+    uint8_t new_irk[16];
+    int rc;
+
+    if (irk != NULL) {
+        memcpy(new_irk, irk, 16);
+    } else {
+        memcpy(new_irk, ble_hs_pvcy_default_irk, 16);
+    }
+
+    /* Clear the resolving list if this is a new IRK. */
+    if (memcmp(ble_hs_pvcy_irk, new_irk, 16) != 0) {
+        memcpy(ble_hs_pvcy_irk, new_irk, 16);
+
+        rc = ble_hs_pvcy_set_resolve_enabled(0);
+        if (rc != 0) {
+            return rc;
+        }
+
+        rc = ble_hs_pvcy_clear_entries();
+        if (rc != 0) {
+            return rc;
+        }
+
+        rc = ble_hs_pvcy_set_resolve_enabled(1);
+        if (rc != 0) {
+            return rc;
+        }
+
+        /*
+         * Add local IRK entry with 00:00:00:00:00:00 address. This entry will
+         * be used to generate RPA for non-directed advertising if own_addr_type
+         * is set to rpa_pub since we use all-zero address as peer addres in
+         * such case. Peer IRK should be left all-zero since this is not for an
+         * actual peer.
+         */
+        memset(tmp_addr, 0, 6);
+        memset(new_irk, 0, 16);
+        rc = ble_hs_pvcy_add_entry(tmp_addr, 0, new_irk);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+int
+ble_hs_pvcy_our_irk(const uint8_t **out_irk)
+{
+    /* XXX: Return error if privacy not supported. */
+
+    *out_irk = ble_hs_pvcy_irk;
+    return 0;
+}
+
+int
+ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode)
+{
+    struct ble_hci_le_set_privacy_mode_cp cmd;
+
+    if (addr->type > BLE_ADDR_RANDOM) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    cmd.mode = priv_mode;
+    cmd.peer_id_addr_type = addr->type;
+    memcpy(cmd.peer_id_addr, addr->val, BLE_DEV_ADDR_LEN);
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_pvcy_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_pvcy_priv.h
new file mode 100644
index 00000000..7f0aa4b9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_pvcy_priv.h
@@ -0,0 +1,43 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_PVCY_PRIV_
+#define H_BLE_HS_PVCY_PRIV_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern const uint8_t ble_hs_pvcy_default_irk[16];
+
+int ble_hs_pvcy_set_our_irk(const uint8_t *irk);
+int ble_hs_pvcy_our_irk(const uint8_t **out_irk);
+int ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr);
+int ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addrtype,
+                          const uint8_t *irk);
+int ble_hs_pvcy_ensure_started(void);
+int ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_shutdown.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_shutdown.c
new file mode 100644
index 00000000..f29d4a66
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_shutdown.c
@@ -0,0 +1,69 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#if MYNEWT
+
+#include "os/mynewt.h"
+#include "ble_hs_priv.h"
+
+static struct ble_hs_stop_listener ble_hs_shutdown_stop_listener;
+
+/**
+ * Called when the host stop procedure has completed.
+ */
+static void
+ble_hs_shutdown_stop_cb(int status, void *arg)
+{
+    SYSDOWN_ASSERT_ACTIVE();
+
+    /* Indicate to sysdown that the host is fully shut down. */
+    sysdown_release();
+}
+
+int
+ble_hs_shutdown(int reason)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysdown. */
+    SYSDOWN_ASSERT_ACTIVE();
+
+    /* Initiate a host stop procedure. */
+    rc = ble_hs_stop(&ble_hs_shutdown_stop_listener, ble_hs_shutdown_stop_cb,
+                     NULL);
+    switch (rc) {
+    case 0:
+        /* Stop initiated.  Wait for result to be reported asynchronously. */
+        return SYSDOWN_IN_PROGRESS;
+
+    case BLE_HS_EBUSY:
+        /* Already stopping.  Wait for result to be reported asynchronously. */
+        return SYSDOWN_IN_PROGRESS;
+
+    case BLE_HS_EALREADY:
+        /* Already stopped.  Shutdown complete. */
+        return SYSDOWN_COMPLETE;
+
+    default:
+        BLE_HS_LOG(ERROR, "ble_hs_shutdown: failed to stop host; rc=%d\n", rc);
+        return SYSDOWN_COMPLETE;
+    }
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_startup.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_startup.c
new file mode 100644
index 00000000..83026ac1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_startup.c
@@ -0,0 +1,367 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include "host/ble_hs_hci.h"
+#include "ble_hs_priv.h"
+
+#if !MYNEWT_VAL(BLE_CONTROLLER)
+static int
+ble_hs_startup_read_sup_f_tx(void)
+{
+    struct ble_hci_ip_rd_loc_supp_feat_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_INFO_PARAMS,
+                                      BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT),
+                           NULL, 0, &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* for now we don't use it outside of init sequence so check this here
+     * LE Supported (Controller) byte 4, bit 6
+     */
+    if (!(rsp.features & 0x0000006000000000)) {
+        BLE_HS_LOG(ERROR, "Controller doesn't support LE\n");
+        return BLE_HS_ECONTROLLER;
+    }
+
+    return 0;
+}
+#endif
+
+static int
+ble_hs_startup_read_local_ver_tx(void)
+{
+    struct ble_hci_ip_rd_local_ver_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_INFO_PARAMS,
+                                      BLE_HCI_OCF_IP_RD_LOCAL_VER),
+                           NULL, 0, &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* For now we are interested only in HCI Version */
+    ble_hs_hci_set_hci_version(rsp.hci_ver);
+
+    return 0;
+}
+
+static int
+ble_hs_startup_le_read_sup_f_tx(void)
+{
+    struct ble_hci_le_rd_loc_supp_feat_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT),
+                           NULL,0, &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_hci_set_le_supported_feat(le64toh(rsp.features));
+
+    return 0;
+}
+
+static int
+ble_hs_startup_le_read_buf_sz_tx(uint16_t *out_pktlen, uint8_t *out_max_pkts)
+{
+    struct ble_hci_le_rd_buf_size_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_RD_BUF_SIZE), NULL, 0,
+                           &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    *out_pktlen = le16toh(rsp.data_len);
+    *out_max_pkts = rsp.data_packets;
+
+    return 0;
+}
+
+static int
+ble_hs_startup_read_buf_sz_tx(uint16_t *out_pktlen, uint16_t *out_max_pkts)
+{
+    struct ble_hci_ip_rd_buf_size_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_INFO_PARAMS,
+                                      BLE_HCI_OCF_IP_RD_BUF_SIZE), NULL, 0,
+                           &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    *out_pktlen = le16toh(rsp.acl_data_len);
+    *out_max_pkts = le16toh(rsp.acl_num);
+
+    return 0;
+}
+
+static int
+ble_hs_startup_read_buf_sz(void)
+{
+    uint16_t le_pktlen = 0;
+    uint16_t max_pkts = 0;
+    uint16_t pktlen = 0;
+    uint8_t le_max_pkts = 0;
+    int rc;
+
+    rc = ble_hs_startup_le_read_buf_sz_tx(&le_pktlen, &le_max_pkts);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (le_pktlen != 0) {
+        pktlen = le_pktlen;
+        max_pkts = le_max_pkts;
+    } else {
+        rc = ble_hs_startup_read_buf_sz_tx(&pktlen, &max_pkts);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    rc = ble_hs_hci_set_buf_sz(pktlen, max_pkts);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_startup_read_bd_addr(void)
+{
+    struct ble_hci_ip_rd_bd_addr_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_INFO_PARAMS,
+                                      BLE_HCI_OCF_IP_RD_BD_ADDR),
+                           NULL, 0, &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_id_set_pub(rsp.addr);
+    return 0;
+}
+
+static int
+ble_hs_startup_le_set_evmask_tx(void)
+{
+    struct ble_hci_le_set_event_mask_cp cmd;
+    uint8_t version;
+    uint64_t mask;
+    int rc;
+
+    version = ble_hs_hci_get_hci_version();
+
+    /* TODO should we also check for supported commands when setting this? */
+
+    /**
+     * Enable the following LE events:
+     *     0x0000000000000001 LE Connection Complete Event
+     *     0x0000000000000002 LE Advertising Report Event
+     *     0x0000000000000004 LE Connection Update Complete Event
+     *     0x0000000000000008 LE Read Remote Used Features Complete Event
+     *     0x0000000000000010 LE Long Term Key Request Event
+     */
+    mask = 0x000000000000001f;
+
+    if (version >= BLE_HCI_VER_BCS_4_1) {
+        /**
+         * Enable the following LE events:
+         *   0x0000000000000020 LE Remote Connection Parameter Request Event */
+        mask |= 0x0000000000000020;
+    }
+
+    if (version >= BLE_HCI_VER_BCS_4_2) {
+        /**
+         * Enable the following LE events:
+         *   0x0000000000000040 LE Data Length Change Event
+         *   0x0000000000000200 LE Enhanced Connection Complete Event
+         *   0x0000000000000400 LE Directed Advertising Report Event
+         */
+        mask |= 0x0000000000000640;
+    }
+
+    if (version >= BLE_HCI_VER_BCS_5_0) {
+        /**
+         * Enable the following LE events:
+         *   0x0000000000000800 LE PHY Update Complete Event
+         *   0x0000000000001000 LE Extended Advertising Report Event
+         *   0x0000000000002000 LE Periodic Advertising Sync Established Event
+         *   0x0000000000004000 LE Periodic Advertising Report Event
+         *   0x0000000000008000 LE Periodic Advertising Sync Lost Event
+         *   0x0000000000010000 LE Extended Scan Timeout Event
+         *   0x0000000000020000 LE Extended Advertising Set Terminated Event
+         *   0x0000000000040000 LE Scan Request Received Event
+         *   0x0000000000080000 LE Channel Selection Algorithm Event
+         */
+        mask |= 0x00000000000ff800;
+    }
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+    if (version >= BLE_HCI_VER_BCS_5_1) {
+        /**
+         * Enable the following LE events:
+         * 0x0000000000800000 LE Periodic Advertising Sync Transfer Received event
+         */
+        mask |= 0x0000000000800000;
+    }
+#endif
+
+    cmd.event_mask = htole64(mask);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_SET_EVENT_MASK),
+                           &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_startup_set_evmask_tx(void)
+{
+    struct ble_hci_cb_set_event_mask_cp cmd;
+    struct ble_hci_cb_set_event_mask2_cp cmd2;
+    uint8_t version;
+    int rc;
+
+    version = ble_hs_hci_get_hci_version();
+
+    /**
+     * Enable the following events:
+     *     0x0000000000000010 Disconnection Complete Event
+     *     0x0000000000000080 Encryption Change Event
+     *     0x0000000000008000 Hardware Error Event
+     *     0x0000000002000000 Data Buffer Overflow Event
+     *     0x0000800000000000 Encryption Key Refresh Complete Event
+     *     0x2000000000000000 LE Meta-Event
+     */
+    cmd.event_mask = htole64(0x2000800002008090);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                                      BLE_HCI_OCF_CB_SET_EVENT_MASK),
+                           &cmd, sizeof(cmd), NULL, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (version >= BLE_HCI_VER_BCS_4_1) {
+        /**
+         * Enable the following events:
+         *     0x0000000000800000 Authenticated Payload Timeout Event
+         */
+        cmd2.event_mask2 = htole64(0x0000000000800000);
+        rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                                          BLE_HCI_OCF_CB_SET_EVENT_MASK2),
+                               &cmd2, sizeof(cmd2), NULL, 0);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_startup_reset_tx(void)
+{
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND,
+                                        BLE_HCI_OCF_CB_RESET),
+                             NULL, 0, NULL, 0);
+}
+
+int
+ble_hs_startup_go(void)
+{
+    int rc;
+
+    rc = ble_hs_startup_reset_tx();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_startup_read_local_ver_tx();
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* XXX: Read local supported commands. */
+
+    /* we need to check this only if using external controller */
+#if !MYNEWT_VAL(BLE_CONTROLLER)
+    if (ble_hs_hci_get_hci_version() < BLE_HCI_VER_BCS_4_0) {
+        BLE_HS_LOG(ERROR, "Required controller version is 4.0 (6)\n");
+        return BLE_HS_ECONTROLLER;
+    }
+
+    rc = ble_hs_startup_read_sup_f_tx();
+    if (rc != 0) {
+        return rc;
+    }
+#endif
+
+    rc = ble_hs_startup_set_evmask_tx();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_startup_le_set_evmask_tx();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_startup_read_buf_sz();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_startup_le_read_sup_f_tx();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_startup_read_bd_addr();
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_pvcy_set_our_irk(NULL);
+
+    /* If flow control is enabled, configure the controller to use it. */
+    ble_hs_flow_startup();
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_startup_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_startup_priv.h
new file mode 100644
index 00000000..a2d32875
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_startup_priv.h
@@ -0,0 +1,33 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_STARTUP_
+#define H_BLE_HS_STARTUP_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_hs_startup_go(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_stop.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_stop.c
new file mode 100644
index 00000000..b90d3ec6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_stop.c
@@ -0,0 +1,283 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "ble_hs_priv.h"
+#include "nimble/nimble_npl.h"
+#ifndef MYNEWT
+#include "nimble/nimble_port.h"
+#endif
+
+#define BLE_HOST_STOP_TIMEOUT_MS MYNEWT_VAL(BLE_HS_STOP_ON_SHUTDOWN_TIMEOUT)
+
+static struct ble_gap_event_listener ble_hs_stop_gap_listener;
+
+/**
+ * List of stop listeners.  These are notified when a stop procedure completes.
+ */
+SLIST_HEAD(ble_hs_stop_listener_slist, ble_hs_stop_listener);
+static struct ble_hs_stop_listener_slist ble_hs_stop_listeners;
+
+/* Track number of connections */
+static uint8_t ble_hs_stop_conn_cnt;
+
+static struct ble_npl_callout ble_hs_stop_terminate_tmo;
+
+/**
+ * Called when a stop procedure has completed.
+ */
+static void
+ble_hs_stop_done(int status)
+{
+    struct ble_hs_stop_listener_slist slist;
+    struct ble_hs_stop_listener *listener;
+
+    ble_npl_callout_stop(&ble_hs_stop_terminate_tmo);
+
+    ble_hs_lock();
+
+    ble_gap_event_listener_unregister(&ble_hs_stop_gap_listener);
+
+    slist = ble_hs_stop_listeners;
+    SLIST_INIT(&ble_hs_stop_listeners);
+
+    ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
+
+    ble_hs_unlock();
+
+    SLIST_FOREACH(listener, &slist, link) {
+        listener->fn(status, listener->arg);
+    }
+}
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+/**
+ * Terminates all active periodic sync handles
+ *
+ * If there are no active periodic sync handles, signals completion of the
+ * close procedure.
+ */
+static int
+ble_hs_stop_terminate_all_periodic_sync(void)
+{
+    int rc = 0;
+    struct ble_hs_periodic_sync *psync;
+    uint16_t sync_handle;
+
+    while((psync = ble_hs_periodic_sync_first())){
+        /* Terminate sync command waits a command complete event, so there
+         * is no need to wait for GAP event, as the calling thread will be
+         * blocked on the hci semaphore until the command complete is received.
+         *
+         * Also, once the sync is terminated, the psync will be freed and
+         * removed from the list such that the next call to
+         * ble_hs_periodic_sync_first yields the next psync handle
+         */
+        sync_handle = psync->sync_handle;
+        rc = ble_gap_periodic_adv_sync_terminate(sync_handle);
+        if (rc != 0 && rc != BLE_HS_ENOTCONN) {
+            BLE_HS_LOG(ERROR, "failed to terminate periodic sync=0x%04x, rc=%d\n",
+                       sync_handle, rc);
+            return rc;
+        }
+    }
+
+    return 0;
+}
+#endif
+
+/**
+ * Terminates connection.
+ */
+static int
+ble_hs_stop_terminate_conn(struct ble_hs_conn *conn, void *arg)
+{
+    int rc;
+
+    rc = ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM);
+    if (rc == 0) {
+        /* Terminate procedure successfully initiated.  Let the GAP event
+         * handler deal with the result.
+         */
+        ble_hs_stop_conn_cnt++;
+    } else {
+        /* If failed, just make sure we are not going to wait for connection complete event,
+         * just count it as already disconnected
+         */
+        BLE_HS_LOG(ERROR, "ble_hs_stop: failed to terminate connection; rc=%d\n", rc);
+    }
+
+    return 0;
+}
+
+/**
+ * This is called when host graceful disconnect timeout fires. That means some devices
+ * are out of range and disconnection completed did no happen yet.
+ */
+static void
+ble_hs_stop_terminate_timeout_cb(struct ble_npl_event *ev)
+{
+    BLE_HS_LOG(ERROR, "ble_hs_stop_terminate_timeout_cb,"
+                      "%d connection(s) still up \n", ble_hs_stop_conn_cnt);
+
+    /* TODO: Shall we send error here? */
+    ble_hs_stop_done(0);
+}
+
+/**
+ * GAP event callback.  Listens for connection termination and then terminates
+ * the next one.
+ *
+ * If there are no connections, signals completion of the stop procedure.
+ */
+static int
+ble_hs_stop_gap_event(struct ble_gap_event *event, void *arg)
+{
+    /* Only process connection termination events. */
+    if (event->type == BLE_GAP_EVENT_DISCONNECT ||
+        event->type == BLE_GAP_EVENT_TERM_FAILURE) {
+
+        ble_hs_stop_conn_cnt--;
+
+        if (ble_hs_stop_conn_cnt == 0) {
+            ble_hs_stop_done(0);
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Registers a listener to listen for completion of the current stop procedure.
+ */
+static void
+ble_hs_stop_register_listener(struct ble_hs_stop_listener *listener,
+                              ble_hs_stop_fn *fn, void *arg)
+{
+    BLE_HS_DBG_ASSERT(fn != NULL);
+
+    listener->fn = fn;
+    listener->arg = arg;
+    SLIST_INSERT_HEAD(&ble_hs_stop_listeners, listener, link);
+}
+
+static int
+ble_hs_stop_begin(struct ble_hs_stop_listener *listener,
+                   ble_hs_stop_fn *fn, void *arg)
+{
+    switch (ble_hs_enabled_state) {
+    case BLE_HS_ENABLED_STATE_ON:
+        /* Host is enabled; proceed with the stop procedure. */
+        ble_hs_enabled_state = BLE_HS_ENABLED_STATE_STOPPING;
+        if (listener != NULL) {
+            ble_hs_stop_register_listener(listener, fn, arg);
+        }
+
+        /* Put the host in the "stopping" state and ensure the host timer is
+         * not running.
+         */
+        ble_hs_timer_resched();
+        return 0;
+
+    case BLE_HS_ENABLED_STATE_STOPPING:
+        /* A stop procedure is already in progress.  Just listen for the
+         * procedure's completion.
+         */
+        if (listener != NULL) {
+            ble_hs_stop_register_listener(listener, fn, arg);
+        }
+        return BLE_HS_EBUSY;
+
+    case BLE_HS_ENABLED_STATE_OFF:
+        /* Host already stopped. */
+        return BLE_HS_EALREADY;
+
+    default:
+        assert(0);
+        return BLE_HS_EUNKNOWN;
+    }
+}
+
+int
+ble_hs_stop(struct ble_hs_stop_listener *listener, 
+            ble_hs_stop_fn *fn, void *arg)
+{
+    int rc;
+
+    ble_hs_lock();
+    rc = ble_hs_stop_begin(listener, fn, arg);
+    ble_hs_unlock();
+
+    switch (rc) {
+    case 0:
+        break;
+
+    case BLE_HS_EBUSY:
+        return 0;
+
+    default:
+        return rc;
+    }
+
+    /* Abort all active GAP procedures. */
+    ble_gap_preempt();
+    ble_gap_preempt_done();
+
+#if MYNEWT_VAL(BLE_PERIODIC_ADV)
+    /* Check for active periodic sync first and terminate it all */
+    rc = ble_hs_stop_terminate_all_periodic_sync();
+    if (rc != 0) {
+        return rc;
+    }
+#endif
+
+    rc = ble_gap_event_listener_register(&ble_hs_stop_gap_listener,
+                                         ble_hs_stop_gap_event, NULL);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_lock();
+    ble_hs_conn_foreach(ble_hs_stop_terminate_conn, NULL);
+    ble_hs_unlock();
+
+    if (ble_hs_stop_conn_cnt > 0) {
+        ble_npl_callout_reset(&ble_hs_stop_terminate_tmo,
+                              ble_npl_time_ms_to_ticks32(BLE_HOST_STOP_TIMEOUT_MS));
+    } else {
+        /* No connections, stop is completed */
+        ble_hs_stop_done(0);
+    }
+
+    return 0;
+}
+
+void
+ble_hs_stop_init(void)
+{
+#ifdef MYNEWT
+    ble_npl_callout_init(&ble_hs_stop_terminate_tmo, ble_npl_eventq_dflt_get(),
+                         ble_hs_stop_terminate_timeout_cb, NULL);
+#else
+    ble_npl_callout_init(&ble_hs_stop_terminate_tmo, nimble_port_get_dflt_eventq(),
+                         ble_hs_stop_terminate_timeout_cb, NULL);
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_ibeacon.c b/src/libs/mynewt-nimble/nimble/host/src/ble_ibeacon.c
new file mode 100644
index 00000000..0c6ef99d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_ibeacon.c
@@ -0,0 +1,82 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include "host/ble_hs_adv.h"
+#include "ble_hs_priv.h"
+
+#define BLE_IBEACON_MFG_DATA_SIZE       25
+
+/**
+ * Configures the device to advertise iBeacons.
+ *
+ * @param uuid                  The 128-bit UUID to advertise.
+ * @param major                 The major version number to include in
+ *                                  iBeacons.
+ * @param minor                 The minor version number to include in
+ *                                  iBeacons.
+ * @param measured_power        The Measured Power (RSSI value at 1 Meter).
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              Other nonzero on failure.
+ */
+int
+ble_ibeacon_set_adv_data(void *uuid128, uint16_t major,
+                         uint16_t minor, int8_t measured_power)
+{
+    struct ble_hs_adv_fields fields;
+    uint8_t buf[BLE_IBEACON_MFG_DATA_SIZE];
+    int rc;
+
+    /** Company identifier (Apple). */
+    buf[0] = 0x4c;
+    buf[1] = 0x00;
+
+    /** iBeacon indicator. */
+    buf[2] = 0x02;
+    buf[3] = 0x15;
+
+    /** UUID. */
+    memcpy(buf + 4, uuid128, 16);
+
+    /** Version number. */
+    put_be16(buf + 20, major);
+    put_be16(buf + 22, minor);
+
+    /* Measured Power ranging data (Calibrated tx power at 1 meters). */
+    if (measured_power < -126 || measured_power > 20) {
+        return BLE_HS_EINVAL;
+    }
+    buf[24] = measured_power;
+
+    memset(&fields, 0, sizeof fields);
+    fields.mfg_data = buf;
+    fields.mfg_data_len = sizeof buf;
+
+    /* Advertise two flags:
+     *     o Discoverability in forthcoming advertisement (general)
+     *     o BLE-only (BR/EDR unsupported).
+     */
+    fields.flags = BLE_HS_ADV_F_DISC_GEN |
+                   BLE_HS_ADV_F_BREDR_UNSUP;
+
+    rc = ble_gap_adv_set_fields(&fields);
+    return rc;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap.c b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap.c
new file mode 100644
index 00000000..0d9f082d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap.c
@@ -0,0 +1,506 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "ble_hs_priv.h"
+#include "ble_l2cap_coc_priv.h"
+
+_Static_assert(sizeof (struct ble_l2cap_hdr) == BLE_L2CAP_HDR_SZ,
+               "struct ble_l2cap_hdr must be 4 bytes");
+
+struct os_mempool ble_l2cap_chan_pool;
+
+static os_membuf_t ble_l2cap_chan_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_MAX_CHANS) +
+                    MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
+                    sizeof (struct ble_l2cap_chan))
+];
+
+STATS_SECT_DECL(ble_l2cap_stats) ble_l2cap_stats;
+STATS_NAME_START(ble_l2cap_stats)
+    STATS_NAME(ble_l2cap_stats, chan_create)
+    STATS_NAME(ble_l2cap_stats, chan_delete)
+    STATS_NAME(ble_l2cap_stats, update_init)
+    STATS_NAME(ble_l2cap_stats, update_rx)
+    STATS_NAME(ble_l2cap_stats, update_fail)
+    STATS_NAME(ble_l2cap_stats, proc_timeout)
+    STATS_NAME(ble_l2cap_stats, sig_tx)
+    STATS_NAME(ble_l2cap_stats, sig_rx)
+    STATS_NAME(ble_l2cap_stats, sm_tx)
+    STATS_NAME(ble_l2cap_stats, sm_rx)
+STATS_NAME_END(ble_l2cap_stats)
+
+struct ble_l2cap_chan *
+ble_l2cap_chan_alloc(uint16_t conn_handle)
+{
+    struct ble_l2cap_chan *chan;
+
+    chan = os_memblock_get(&ble_l2cap_chan_pool);
+    if (chan == NULL) {
+        return NULL;
+    }
+
+    memset(chan, 0, sizeof *chan);
+    chan->conn_handle = conn_handle;
+
+    STATS_INC(ble_l2cap_stats, chan_create);
+
+    return chan;
+}
+
+void
+ble_l2cap_chan_free(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
+{
+    int rc;
+
+    if (chan == NULL) {
+        return;
+    }
+
+    os_mbuf_free_chain(chan->rx_buf);
+    ble_l2cap_coc_cleanup_chan(conn, chan);
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    memset(chan, 0xff, sizeof *chan);
+#endif
+    rc = os_memblock_put(&ble_l2cap_chan_pool, chan);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    STATS_INC(ble_l2cap_stats, chan_delete);
+}
+
+bool
+ble_l2cap_is_mtu_req_sent(const struct ble_l2cap_chan *chan)
+{
+    return (chan->flags & BLE_L2CAP_CHAN_F_TXED_MTU);
+}
+
+int
+ble_l2cap_parse_hdr(struct os_mbuf *om, int off,
+                    struct ble_l2cap_hdr *l2cap_hdr)
+{
+    int rc;
+
+    rc = os_mbuf_copydata(om, off, sizeof *l2cap_hdr, l2cap_hdr);
+    if (rc != 0) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    l2cap_hdr->len = get_le16(&l2cap_hdr->len);
+    l2cap_hdr->cid = get_le16(&l2cap_hdr->cid);
+
+    return 0;
+}
+
+struct os_mbuf *
+ble_l2cap_prepend_hdr(struct os_mbuf *om, uint16_t cid, uint16_t len)
+{
+    struct ble_l2cap_hdr hdr;
+
+    put_le16(&hdr.len, len);
+    put_le16(&hdr.cid, cid);
+
+    om = os_mbuf_prepend_pullup(om, sizeof hdr);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    memcpy(om->om_data, &hdr, sizeof hdr);
+
+    return om;
+}
+
+uint16_t
+ble_l2cap_get_conn_handle(struct ble_l2cap_chan *chan)
+{
+    if (!chan) {
+        return BLE_HS_CONN_HANDLE_NONE;
+    }
+
+    return chan->conn_handle;
+}
+
+int
+ble_l2cap_create_server(uint16_t psm, uint16_t mtu,
+                        ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    return ble_l2cap_coc_create_server(psm, mtu, cb, cb_arg);
+}
+
+int
+ble_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
+                  struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    return ble_l2cap_sig_coc_connect(conn_handle, psm, mtu, sdu_rx, cb, cb_arg);
+}
+
+int
+ble_l2cap_get_chan_info(struct ble_l2cap_chan *chan, struct ble_l2cap_chan_info *chan_info)
+{
+    if (!chan || !chan_info) {
+        return BLE_HS_EINVAL;
+    }
+
+    memset(chan_info, 0, sizeof(*chan_info));
+    chan_info->dcid = chan->dcid;
+    chan_info->scid = chan->scid;
+    chan_info->our_l2cap_mtu = chan->my_mtu;
+    chan_info->peer_l2cap_mtu = chan->peer_mtu;
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+    chan_info->psm = chan->psm;
+    chan_info->our_coc_mtu = chan->coc_rx.mtu;
+    chan_info->peer_coc_mtu = chan->coc_tx.mtu;
+#endif
+
+    return 0;
+}
+
+int
+ble_l2cap_enhanced_connect(uint16_t conn_handle,
+                               uint16_t psm, uint16_t mtu,
+                               uint8_t num, struct os_mbuf *sdu_rx[],
+                               ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    return ble_l2cap_sig_ecoc_connect(conn_handle, psm, mtu,
+                                      num, sdu_rx, cb, cb_arg);
+}
+
+int
+ble_l2cap_reconfig(struct ble_l2cap_chan *chans[], uint8_t num, uint16_t new_mtu)
+{
+    int i;
+    uint16_t conn_handle;
+
+    if (num == 0 || !chans) {
+        return BLE_HS_EINVAL;
+    }
+
+    conn_handle = chans[0]->conn_handle;
+
+    for (i = 1; i < num; i++) {
+        if (conn_handle != chans[i]->conn_handle) {
+            BLE_HS_LOG(ERROR, "All channels should have same conn handle\n");
+            return BLE_HS_EINVAL;
+        }
+    }
+
+    return ble_l2cap_sig_coc_reconfig(conn_handle, chans, num, new_mtu);
+}
+
+int
+ble_l2cap_disconnect(struct ble_l2cap_chan *chan)
+{
+    return ble_l2cap_sig_disconnect(chan);
+}
+
+/**
+ * Transmits a packet over an L2CAP channel.  This function only consumes the
+ * supplied mbuf on success.
+ */
+int
+ble_l2cap_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu)
+{
+    return ble_l2cap_coc_send(chan, sdu);
+}
+
+int
+ble_l2cap_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx)
+{
+    return ble_l2cap_coc_recv_ready(chan, sdu_rx);
+}
+
+void
+ble_l2cap_remove_rx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
+{
+    conn->bhc_rx_chan = NULL;
+    os_mbuf_free_chain(chan->rx_buf);
+    chan->rx_buf = NULL;
+    chan->rx_len = 0;
+}
+
+static void
+ble_l2cap_append_rx(struct ble_l2cap_chan *chan, struct os_mbuf *frag)
+{
+#if MYNEWT_VAL(BLE_L2CAP_JOIN_RX_FRAGS)
+    struct os_mbuf *m;
+
+    /* Copy the data from the incoming fragment into the packet in progress. */
+    m = os_mbuf_pack_chains(chan->rx_buf, frag);
+    assert(m);
+#else
+    /* Join disabled or append failed due to mbuf shortage.  Just attach the
+     * mbuf to the end of the packet.
+     */
+    os_mbuf_concat(chan->rx_buf, frag);
+#endif
+}
+
+static int
+ble_l2cap_rx_payload(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
+                     struct os_mbuf *om,
+                     ble_l2cap_rx_fn **out_rx_cb)
+{
+    int len_diff;
+    int rc;
+
+    if (chan->rx_buf == NULL) {
+        /* First fragment in packet. */
+        chan->rx_buf = om;
+    } else {
+        /* Continuation of packet in progress. */
+        ble_l2cap_append_rx(chan, om);
+    }
+
+    /* Determine if packet is fully reassembled. */
+    len_diff = OS_MBUF_PKTLEN(chan->rx_buf) - chan->rx_len;
+    if (len_diff > 0) {
+        /* More data than expected; data corruption. */
+        ble_l2cap_remove_rx(conn, chan);
+        rc = BLE_HS_EBADDATA;
+    } else if (len_diff == 0) {
+        /* All fragments received. */
+        *out_rx_cb = chan->rx_fn;
+        rc = 0;
+    } else {
+        /* More fragments remain. */
+#if MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) != 0
+        conn->bhc_rx_timeout =
+            ble_npl_time_get() + MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT);
+
+        ble_hs_timer_resched();
+#endif
+        rc = BLE_HS_EAGAIN;
+    }
+
+    return rc;
+}
+
+static uint16_t
+ble_l2cap_get_mtu(struct ble_l2cap_chan *chan)
+{
+    if (chan->scid == BLE_L2CAP_CID_ATT) {
+        /* In case of ATT chan->my_mtu keeps preferred MTU which is later
+         * used during exchange MTU procedure. Helper below will gives us actual
+         * MTU on the channel, which is 23 or higher if exchange MTU has been
+         * done
+         */
+        return ble_att_chan_mtu(chan);
+    }
+
+    return chan->my_mtu;
+}
+
+/**
+ * Processes an incoming L2CAP fragment.
+ *
+ * @param conn                  The connection the L2CAP fragment was sent
+ *                                  over.
+ * @param hci_hdr               The ACL data header that was at the start of
+ *                                  the L2CAP fragment.  This header has been
+ *                                  stripped from the mbuf parameter.
+ * @param om                    An mbuf containing the L2CAP data.  If this is
+ *                                  the first fragment, the L2CAP header is at
+ *                                  the start of the mbuf.  For subsequent
+ *                                  fragments, the mbuf starts with L2CAP
+ *                                  payload data.
+ * @param out_rx_cb             If a full L2CAP packet has been received, a
+ *                                  pointer to the appropriate handler gets
+ *                                  written here.  The caller should pass the
+ *                                  receive buffer to this callback.
+ * @param out_rx_buf            If a full L2CAP packet has been received, this
+ *                                  will point to the entire L2CAP packet.  To
+ *                                  process the packet, pass this buffer to the
+ *                                  receive handler (out_rx_cb).
+ * @param out_reject_cid        Indicates whether an L2CAP Command Reject
+ *                                  command should be sent.  If this equals -1,
+ *                                  no reject should get sent.  Otherwise, the
+ *                                  value indicates the CID that the outgoing
+ *                                  reject should specify.
+ *
+ * @return                      0 if a complete L2CAP packet has been received.
+ *                              BLE_HS_EAGAIN if a partial L2CAP packet has
+ *                                  been received; more fragments are expected.
+ *                              Other value on error.
+ */
+int
+ble_l2cap_rx(struct ble_hs_conn *conn,
+             struct hci_data_hdr *hci_hdr,
+             struct os_mbuf *om,
+             ble_l2cap_rx_fn **out_rx_cb,
+             int *out_reject_cid)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_l2cap_hdr l2cap_hdr;
+    uint8_t pb;
+    int rc;
+
+    *out_reject_cid = -1;
+
+    pb = BLE_HCI_DATA_PB(hci_hdr->hdh_handle_pb_bc);
+    switch (pb) {
+    case BLE_HCI_PB_FIRST_FLUSH:
+        /* First fragment. */
+        rc = ble_l2cap_parse_hdr(om, 0, &l2cap_hdr);
+        if (rc != 0) {
+            goto err;
+        }
+
+        /* Strip L2CAP header from the front of the mbuf. */
+        os_mbuf_adj(om, BLE_L2CAP_HDR_SZ);
+
+        chan = ble_hs_conn_chan_find_by_scid(conn, l2cap_hdr.cid);
+        if (chan == NULL) {
+            rc = BLE_HS_ENOENT;
+
+            /* Unsupported channel. If the target CID is the black hole
+             * channel, quietly drop the packet.  Otherwise, send an invalid
+             * CID response.
+             */
+            if (l2cap_hdr.cid != BLE_L2CAP_CID_BLACK_HOLE) {
+                BLE_HS_LOG(DEBUG, "rx on unknown L2CAP channel: %d\n",
+                           l2cap_hdr.cid);
+                *out_reject_cid = l2cap_hdr.cid;
+            }
+            goto err;
+        }
+
+        if (chan->rx_buf != NULL) {
+            /* Previous data packet never completed.  Discard old packet. */
+            ble_l2cap_remove_rx(conn, chan);
+        }
+
+        if (l2cap_hdr.len > ble_l2cap_get_mtu(chan)) {
+            /* More data then we expected on the channel */
+            rc = BLE_HS_EBADDATA;
+            goto err;
+        }
+
+        /* Remember channel and length of L2CAP data for reassembly. */
+        conn->bhc_rx_chan = chan;
+        chan->rx_len = l2cap_hdr.len;
+        break;
+
+    case BLE_HCI_PB_MIDDLE:
+        chan = conn->bhc_rx_chan;
+        if (chan == NULL || chan->rx_buf == NULL) {
+            /* Middle fragment without the start.  Discard new packet. */
+            rc = BLE_HS_EBADDATA;
+            goto err;
+        }
+        break;
+
+    default:
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
+    rc = ble_l2cap_rx_payload(conn, chan, om, out_rx_cb);
+    om = NULL;
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
+/**
+ * Transmits the L2CAP payload contained in the specified mbuf.  The supplied
+ * mbuf is consumed, regardless of the outcome of the function call.
+ *
+ * @param chan                  The L2CAP channel to transmit over.
+ * @param txom                  The data to transmit.
+ *
+ * @return                      0 on success; nonzero on error.
+ */
+int
+ble_l2cap_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
+             struct os_mbuf *txom)
+{
+    int rc;
+
+    txom = ble_l2cap_prepend_hdr(txom, chan->dcid, OS_MBUF_PKTLEN(txom));
+    if (txom == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    rc = ble_hs_hci_acl_tx(conn, &txom);
+    switch (rc) {
+    case 0:
+        /* Success. */
+        return 0;
+
+    case BLE_HS_EAGAIN:
+        /* Controller could not accommodate full packet.  Enqueue remainder. */
+        STAILQ_INSERT_TAIL(&conn->bhc_tx_q, OS_MBUF_PKTHDR(txom), omp_next);
+        return 0;
+
+    default:
+        /* Error. */
+        return rc;
+    }
+}
+
+int
+ble_l2cap_init(void)
+{
+    int rc;
+
+    rc = os_mempool_init(&ble_l2cap_chan_pool,
+                         MYNEWT_VAL(BLE_L2CAP_MAX_CHANS) +
+                         MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
+                         sizeof (struct ble_l2cap_chan),
+                         ble_l2cap_chan_mem, "ble_l2cap_chan_pool");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    rc = ble_l2cap_sig_init();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_l2cap_coc_init();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_sm_init();
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = stats_init_and_reg(
+        STATS_HDR(ble_l2cap_stats), STATS_SIZE_INIT_PARMS(ble_l2cap_stats,
+        STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_l2cap_stats), "ble_l2cap");
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_coc.c b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_coc.c
new file mode 100644
index 00000000..b15646f6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_coc.c
@@ -0,0 +1,616 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "nimble/ble.h"
+#include "ble_hs_priv.h"
+#include "ble_l2cap_priv.h"
+#include "ble_l2cap_coc_priv.h"
+#include "ble_l2cap_sig_priv.h"
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+
+#define BLE_L2CAP_SDU_SIZE              2
+
+STAILQ_HEAD(ble_l2cap_coc_srv_list, ble_l2cap_coc_srv);
+
+static struct ble_l2cap_coc_srv_list ble_l2cap_coc_srvs;
+
+static os_membuf_t ble_l2cap_coc_srv_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
+                    sizeof (struct ble_l2cap_coc_srv))
+];
+
+static struct os_mempool ble_l2cap_coc_srv_pool;
+
+static void
+ble_l2cap_coc_dbg_assert_srv_not_inserted(struct ble_l2cap_coc_srv *srv)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    struct ble_l2cap_coc_srv *cur;
+
+    STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) {
+        BLE_HS_DBG_ASSERT(cur != srv);
+    }
+#endif
+}
+
+static struct ble_l2cap_coc_srv *
+ble_l2cap_coc_srv_alloc(void)
+{
+    struct ble_l2cap_coc_srv *srv;
+
+    srv = os_memblock_get(&ble_l2cap_coc_srv_pool);
+    if (srv != NULL) {
+        memset(srv, 0, sizeof(*srv));
+    }
+
+    return srv;
+}
+
+int
+ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu,
+                                        ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    struct ble_l2cap_coc_srv * srv;
+
+    srv = ble_l2cap_coc_srv_alloc();
+    if (!srv) {
+        return BLE_HS_ENOMEM;
+    }
+
+    srv->psm = psm;
+    srv->mtu = mtu;
+    srv->cb = cb;
+    srv->cb_arg = cb_arg;
+
+    ble_l2cap_coc_dbg_assert_srv_not_inserted(srv);
+
+    STAILQ_INSERT_HEAD(&ble_l2cap_coc_srvs, srv, next);
+
+    return 0;
+}
+
+static inline void
+ble_l2cap_set_used_cid(uint32_t *cid_mask, int bit)
+{
+    cid_mask[bit / 32] |= (1 << (bit % 32));
+}
+
+static inline void
+ble_l2cap_clear_used_cid(uint32_t *cid_mask, int bit)
+{
+    cid_mask[bit / 32] &= ~(1 << (bit % 32));
+}
+
+static inline int
+ble_l2cap_get_first_available_bit(uint32_t *cid_mask)
+{
+    int i;
+    int bit = 0;
+
+    for (i = 0; i < BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN; i++) {
+        /* Find first available index by finding first available bit
+         * in the mask.
+         * Note:
+         * a) If bit == 0 means all the bits are used
+         * b) this function returns 1 + index
+         */
+        bit = __builtin_ffs(~(unsigned int)(cid_mask[i]));
+        if (bit != 0) {
+            break;
+        }
+    }
+
+    if (i == BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN) {
+        return -1;
+    }
+
+    return (i * 32 + bit - 1);
+}
+
+static int
+ble_l2cap_coc_get_cid(uint32_t *cid_mask)
+{
+    int bit;
+
+    bit = ble_l2cap_get_first_available_bit(cid_mask);
+    if (bit < 0) {
+        return -1;
+    }
+
+    ble_l2cap_set_used_cid(cid_mask, bit);
+    return BLE_L2CAP_COC_CID_START + bit;
+}
+
+static struct ble_l2cap_coc_srv *
+ble_l2cap_coc_srv_find(uint16_t psm)
+{
+    struct ble_l2cap_coc_srv *cur, *srv;
+
+    srv = NULL;
+    STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) {
+        if (cur->psm == psm) {
+                srv = cur;
+                break;
+        }
+    }
+
+    return srv;
+}
+
+static void
+ble_l2cap_event_coc_received_data(struct ble_l2cap_chan *chan,
+                                  struct os_mbuf *om)
+{
+    struct ble_l2cap_event event;
+
+    event.type = BLE_L2CAP_EVENT_COC_DATA_RECEIVED;
+    event.receive.conn_handle = chan->conn_handle;
+    event.receive.chan = chan;
+    event.receive.sdu_rx = om;
+
+    chan->cb(&event, chan->cb_arg);
+}
+
+static int
+ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan)
+{
+    int rc;
+    struct os_mbuf **om;
+    struct ble_l2cap_coc_endpoint *rx;
+    uint16_t om_total;
+
+    /* Create a shortcut to rx_buf */
+    om = &chan->rx_buf;
+    BLE_HS_DBG_ASSERT(*om != NULL);
+
+    /* Create a shortcut to rx endpoint */
+    rx = &chan->coc_rx;
+    BLE_HS_DBG_ASSERT(rx != NULL);
+
+    om_total = OS_MBUF_PKTLEN(*om);
+
+    /* First LE frame */
+    if (OS_MBUF_PKTLEN(rx->sdu) == 0) {
+        uint16_t sdu_len;
+
+        rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SDU_SIZE);
+        if (rc != 0) {
+            return rc;
+        }
+
+        sdu_len = get_le16((*om)->om_data);
+        if (sdu_len > rx->mtu) {
+            BLE_HS_LOG(INFO, "error: sdu_len > rx->mtu (%d>%d)\n",
+                       sdu_len, rx->mtu);
+
+            /* Disconnect peer with invalid behaviour */
+            ble_l2cap_disconnect(chan);
+            return BLE_HS_EBADDATA;
+        }
+
+        BLE_HS_LOG(DEBUG, "sdu_len=%d, received LE frame=%d, credits=%d\n",
+                   sdu_len, om_total, rx->credits);
+
+        os_mbuf_adj(*om , BLE_L2CAP_SDU_SIZE);
+
+        rc = os_mbuf_appendfrom(rx->sdu, *om, 0, om_total - BLE_L2CAP_SDU_SIZE);
+        if (rc != 0) {
+            /* FIXME: User shall give us big enough buffer.
+             * need to handle it better
+             */
+            BLE_HS_LOG(INFO, "Could not append data rc=%d\n", rc);
+            assert(0);
+        }
+
+        /* In RX case data_offset keeps incoming SDU len */
+        rx->data_offset = sdu_len;
+
+    } else {
+        BLE_HS_LOG(DEBUG, "Continuation...received %d\n", (*om)->om_len);
+
+        rc  = os_mbuf_appendfrom(rx->sdu, *om, 0, om_total);
+        if (rc != 0) {
+            /* FIXME: need to handle it better */
+            BLE_HS_LOG(DEBUG, "Could not append data rc=%d\n", rc);
+            assert(0);
+        }
+    }
+
+    rx->credits--;
+
+    if (OS_MBUF_PKTLEN(rx->sdu) == rx->data_offset) {
+        struct os_mbuf *sdu_rx = rx->sdu;
+
+        BLE_HS_LOG(DEBUG, "Received sdu_len=%d, credits left=%d\n",
+                   OS_MBUF_PKTLEN(rx->sdu), rx->credits);
+
+        /* Lets get back control to os_mbuf to application.
+         * Since it this callback application might want to set new sdu
+         * we need to prepare space for this. Therefore we need sdu_rx
+         */
+        rx->sdu = NULL;
+        rx->data_offset = 0;
+
+        ble_l2cap_event_coc_received_data(chan, sdu_rx);
+
+        return 0;
+    }
+
+    /* If we did not received full SDU and credits are 0 it means
+     * that remote was sending us not fully filled up LE frames.
+     * However, we still have buffer to for next LE Frame so lets give one more
+     * credit to peer so it can send us full SDU
+     */
+    if (rx->credits == 0) {
+        /* Remote did not send full SDU. Lets give him one more credits to do
+         * so since we have still buffer to handle it
+         */
+        rx->credits = 1;
+        ble_l2cap_sig_le_credits(chan->conn_handle, chan->scid, rx->credits);
+    }
+
+    BLE_HS_LOG(DEBUG, "Received partial sdu_len=%d, credits left=%d\n",
+               OS_MBUF_PKTLEN(rx->sdu), rx->credits);
+
+    return 0;
+}
+
+void
+ble_l2cap_coc_set_new_mtu_mps(struct ble_l2cap_chan *chan, uint16_t mtu, uint16_t mps)
+{
+    chan->my_coc_mps = mps;
+    chan->coc_rx.mtu = mtu;
+    chan->initial_credits = mtu / chan->my_coc_mps;
+    if (mtu % chan->my_coc_mps) {
+        chan->initial_credits++;
+    }
+}
+
+struct ble_l2cap_chan *
+ble_l2cap_coc_chan_alloc(struct ble_hs_conn *conn, uint16_t psm, uint16_t mtu,
+                         struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb,
+                         void *cb_arg)
+{
+    struct ble_l2cap_chan *chan;
+
+    chan = ble_l2cap_chan_alloc(conn->bhc_handle);
+    if (!chan) {
+        return NULL;
+    }
+
+    chan->psm = psm;
+    chan->cb = cb;
+    chan->cb_arg = cb_arg;
+    chan->scid = ble_l2cap_coc_get_cid(conn->l2cap_coc_cid_mask);
+    chan->my_coc_mps = MYNEWT_VAL(BLE_L2CAP_COC_MPS);
+    chan->rx_fn = ble_l2cap_coc_rx_fn;
+    chan->coc_rx.mtu = mtu;
+    chan->coc_rx.sdu = sdu_rx;
+
+    /* Number of credits should allow to send full SDU with on given
+     * L2CAP MTU
+     */
+    chan->coc_rx.credits = mtu / chan->my_coc_mps;
+    if (mtu % chan->my_coc_mps) {
+        chan->coc_rx.credits++;
+    }
+
+    chan->initial_credits = chan->coc_rx.credits;
+    return chan;
+}
+
+int
+ble_l2cap_coc_create_srv_chan(struct ble_hs_conn *conn, uint16_t psm,
+                              struct ble_l2cap_chan **chan)
+{
+    struct ble_l2cap_coc_srv *srv;
+
+    /* Check if there is server registered on this PSM */
+    srv = ble_l2cap_coc_srv_find(psm);
+    if (!srv) {
+        return BLE_HS_ENOTSUP;
+    }
+
+    *chan = ble_l2cap_coc_chan_alloc(conn, psm, srv->mtu, NULL, srv->cb,
+                                     srv->cb_arg);
+    if (!*chan) {
+        return BLE_HS_ENOMEM;
+    }
+
+    return 0;
+}
+
+static void
+ble_l2cap_event_coc_disconnected(struct ble_l2cap_chan *chan)
+{
+    struct ble_l2cap_event event = { };
+
+    /* FIXME */
+    if (!chan->cb) {
+        return;
+    }
+
+    event.type = BLE_L2CAP_EVENT_COC_DISCONNECTED;
+    event.disconnect.conn_handle = chan->conn_handle;
+    event.disconnect.chan = chan;
+
+    chan->cb(&event, chan->cb_arg);
+}
+
+void
+ble_l2cap_coc_cleanup_chan(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
+{
+    /* PSM 0 is used for fixed channels. */
+    if (chan->psm == 0) {
+            return;
+    }
+
+    ble_l2cap_event_coc_disconnected(chan);
+
+    if (conn && chan->scid) {
+        ble_l2cap_clear_used_cid(conn->l2cap_coc_cid_mask,
+                                 chan->scid - BLE_L2CAP_COC_CID_START);
+    }
+
+    os_mbuf_free_chain(chan->coc_rx.sdu);
+    os_mbuf_free_chain(chan->coc_tx.sdu);
+}
+
+static void
+ble_l2cap_event_coc_unstalled(struct ble_l2cap_chan *chan, int status)
+{
+    struct ble_l2cap_event event = { };
+
+    if (!chan->cb) {
+        return;
+    }
+
+    event.type = BLE_L2CAP_EVENT_COC_TX_UNSTALLED;
+    event.tx_unstalled.conn_handle = chan->conn_handle;
+    event.tx_unstalled.chan = chan;
+    event.tx_unstalled.status = status;
+
+    chan->cb(&event, chan->cb_arg);
+}
+
+static int
+ble_l2cap_coc_continue_tx(struct ble_l2cap_chan *chan)
+{
+    struct ble_l2cap_coc_endpoint *tx;
+    uint16_t len;
+    uint16_t left_to_send;
+    struct os_mbuf *txom;
+    struct ble_hs_conn *conn;
+    uint16_t sdu_size_offset;
+    int rc;
+
+    /* If there is no data to send, just return success */
+    tx = &chan->coc_tx;
+    if (!tx->sdu) {
+        return 0;
+    }
+
+    while (tx->credits) {
+        sdu_size_offset = 0;
+
+        BLE_HS_LOG(DEBUG, "Available credits %d\n", tx->credits);
+
+        /* lets calculate data we are going to send */
+        left_to_send = OS_MBUF_PKTLEN(tx->sdu) - tx->data_offset;
+
+        if (tx->data_offset == 0) {
+            sdu_size_offset = BLE_L2CAP_SDU_SIZE;
+            left_to_send += sdu_size_offset;
+        }
+
+        /* Take into account peer MTU */
+        len = min(left_to_send, chan->peer_coc_mps);
+
+        /* Prepare packet */
+        txom = ble_hs_mbuf_l2cap_pkt();
+        if (!txom) {
+            BLE_HS_LOG(DEBUG, "Could not prepare l2cap packet len %d", len);
+            rc = BLE_HS_ENOMEM;
+            goto failed;
+        }
+
+        if (tx->data_offset == 0) {
+            /* First packet needs SDU len first. Left to send */
+            uint16_t l = htole16(OS_MBUF_PKTLEN(tx->sdu));
+
+            BLE_HS_LOG(DEBUG, "Sending SDU len=%d\n", OS_MBUF_PKTLEN(tx->sdu));
+            rc = os_mbuf_append(txom, &l, sizeof(uint16_t));
+            if (rc) {
+                rc = BLE_HS_ENOMEM;
+                BLE_HS_LOG(DEBUG, "Could not append data rc=%d", rc);
+                goto failed;
+            }
+        }
+
+        /* In data_offset we keep track on what we already sent. Need to remember
+         * that for first packet we need to decrease data size by 2 bytes for sdu
+         * size
+         */
+        rc = os_mbuf_appendfrom(txom, tx->sdu, tx->data_offset,
+                                len - sdu_size_offset);
+        if (rc) {
+            rc = BLE_HS_ENOMEM;
+            BLE_HS_LOG(DEBUG, "Could not append data rc=%d", rc);
+            goto failed;
+        }
+
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(chan->conn_handle);
+        rc = ble_l2cap_tx(conn, chan, txom);
+        ble_hs_unlock();
+
+        if (rc) {
+          /* txom is consumed by l2cap */
+          txom = NULL;
+          goto failed;
+        } else {
+            tx->credits --;
+            tx->data_offset += len - sdu_size_offset;
+        }
+
+        BLE_HS_LOG(DEBUG, "Sent %d bytes, credits=%d, to send %d bytes \n",
+                  len, tx->credits, OS_MBUF_PKTLEN(tx->sdu)- tx->data_offset );
+
+        if (tx->data_offset == OS_MBUF_PKTLEN(tx->sdu)) {
+                BLE_HS_LOG(DEBUG, "Complete package sent\n");
+                os_mbuf_free_chain(tx->sdu);
+                tx->sdu = 0;
+                tx->data_offset = 0;
+                if (tx->flags & BLE_L2CAP_COC_FLAG_STALLED) {
+                    ble_l2cap_event_coc_unstalled(chan, 0);
+                    tx->flags &= ~BLE_L2CAP_COC_FLAG_STALLED;
+                }
+                break;
+        }
+    }
+
+    if (tx->sdu) {
+        /* Not complete SDU sent, wait for credits */
+        tx->flags |= BLE_L2CAP_COC_FLAG_STALLED;
+        return BLE_HS_ESTALLED;
+    }
+
+    return 0;
+
+failed:
+    os_mbuf_free_chain(tx->sdu);
+    tx->sdu = NULL;
+    os_mbuf_free_chain(txom);
+    if (tx->flags & BLE_L2CAP_COC_FLAG_STALLED) {
+        ble_l2cap_event_coc_unstalled(chan, rc);
+        tx->flags &= ~BLE_L2CAP_COC_FLAG_STALLED;
+    }
+
+    return rc;
+}
+
+void
+ble_l2cap_coc_le_credits_update(uint16_t conn_handle, uint16_t dcid,
+                                uint16_t credits)
+{
+    struct ble_hs_conn *conn;
+    struct ble_l2cap_chan *chan;
+
+    /* remote updated its credits */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        return;
+    }
+
+    chan = ble_hs_conn_chan_find_by_dcid(conn, dcid);
+    if (!chan) {
+        ble_hs_unlock();
+        return;
+    }
+
+    if (chan->coc_tx.credits + credits > 0xFFFF) {
+        BLE_HS_LOG(INFO, "LE CoC credits overflow...disconnecting\n");
+        ble_hs_unlock();
+        ble_l2cap_sig_disconnect(chan);
+        return;
+    }
+
+    chan->coc_tx.credits += credits;
+    ble_hs_unlock();
+    ble_l2cap_coc_continue_tx(chan);
+}
+
+int
+ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx)
+{
+    struct ble_hs_conn *conn;
+    struct ble_l2cap_chan *c;
+
+    if (!sdu_rx) {
+        return BLE_HS_EINVAL;
+    }
+
+    chan->coc_rx.sdu = sdu_rx;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find_assert(chan->conn_handle);
+    c = ble_hs_conn_chan_find_by_scid(conn, chan->scid);
+    if (!c) {
+        ble_hs_unlock();
+        return BLE_HS_ENOENT;
+    }
+
+    /* We want to back only that much credits which remote side is missing
+     * to be able to send complete SDU.
+     */
+    if (chan->coc_rx.credits < c->initial_credits) {
+        ble_hs_unlock();
+        ble_l2cap_sig_le_credits(chan->conn_handle, chan->scid,
+                                 c->initial_credits - chan->coc_rx.credits);
+        ble_hs_lock();
+        chan->coc_rx.credits = c->initial_credits;
+    }
+
+    ble_hs_unlock();
+
+    return 0;
+}
+
+/**
+ * Transmits a packet over a connection-oriented channel.  This function only
+ * consumes the supplied mbuf on success.
+ */
+int
+ble_l2cap_coc_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx)
+{
+    struct ble_l2cap_coc_endpoint *tx;
+
+    tx = &chan->coc_tx;
+
+    if (tx->sdu) {
+        return BLE_HS_EBUSY;
+    }
+
+    if (OS_MBUF_PKTLEN(sdu_tx) > tx->mtu) {
+        return BLE_HS_EBADDATA;
+    }
+
+    tx->sdu = sdu_tx;
+
+    return ble_l2cap_coc_continue_tx(chan);
+}
+
+int
+ble_l2cap_coc_init(void)
+{
+    STAILQ_INIT(&ble_l2cap_coc_srvs);
+
+    return os_mempool_init(&ble_l2cap_coc_srv_pool,
+                         MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
+                         sizeof (struct ble_l2cap_coc_srv),
+                         ble_l2cap_coc_srv_mem,
+                         "ble_l2cap_coc_srv_pool");
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_coc_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_coc_priv.h
new file mode 100644
index 00000000..5ebdaa05
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_coc_priv.h
@@ -0,0 +1,106 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_L2CAP_COC_PRIV_
+#define H_L2CAP_COC_PRIV_
+
+#include <inttypes.h>
+#include "syscfg/syscfg.h"
+#include "os/queue.h"
+#include "os/os_mbuf.h"
+#include "host/ble_l2cap.h"
+#include "ble_l2cap_sig_priv.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_L2CAP_COC_CID_START                 0x0040
+#define BLE_L2CAP_COC_CID_END                   0x007F
+
+struct ble_l2cap_chan;
+
+#define BLE_L2CAP_COC_FLAG_STALLED              0x01
+
+struct ble_l2cap_coc_endpoint {
+    struct os_mbuf *sdu;
+    uint16_t mtu;
+    uint16_t credits;
+    uint16_t data_offset;
+    uint8_t flags;
+};
+
+struct ble_l2cap_coc_srv {
+    STAILQ_ENTRY(ble_l2cap_coc_srv) next;
+    uint16_t psm;
+    uint16_t mtu;
+    ble_l2cap_event_fn *cb;
+    void *cb_arg;
+};
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+int ble_l2cap_coc_init(void);
+int ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu,
+                                ble_l2cap_event_fn *cb, void *cb_arg);
+int ble_l2cap_coc_create_srv_chan(struct ble_hs_conn *conn, uint16_t psm,
+                                  struct ble_l2cap_chan **chan);
+struct ble_l2cap_chan * ble_l2cap_coc_chan_alloc(struct ble_hs_conn *conn,
+                                                 uint16_t psm, uint16_t mtu,
+                                                 struct os_mbuf *sdu_rx,
+                                                 ble_l2cap_event_fn *cb,
+                                                 void *cb_arg);
+void ble_l2cap_coc_cleanup_chan(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan);
+void ble_l2cap_coc_le_credits_update(uint16_t conn_handle, uint16_t dcid,
+                                    uint16_t credits);
+int ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan,
+                             struct os_mbuf *sdu_rx);
+int ble_l2cap_coc_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx);
+void ble_l2cap_coc_set_new_mtu_mps(struct ble_l2cap_chan *chan, uint16_t mtu, uint16_t mps);
+#else
+static inline int
+ble_l2cap_coc_init(void) {
+    return 0;
+}
+
+static inline int
+ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu,
+                            ble_l2cap_event_fn *cb, void *cb_arg) {
+    return BLE_HS_ENOTSUP;
+}
+
+static inline int
+ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan,
+                         struct os_mbuf *sdu_rx) {
+    return BLE_HS_ENOTSUP;
+}
+
+static inline void
+ble_l2cap_coc_cleanup_chan(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan) {
+}
+
+static inline int
+ble_l2cap_coc_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx) {
+    return BLE_HS_ENOTSUP;
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_L2CAP_COC_PRIV_ */
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_priv.h
new file mode 100644
index 00000000..e3409743
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_priv.h
@@ -0,0 +1,144 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_L2CAP_PRIV_
+#define H_L2CAP_PRIV_
+
+#include "ble_l2cap_coc_priv.h"
+#include "host/ble_l2cap.h"
+#include <inttypes.h>
+#include "stats/stats.h"
+#include "os/queue.h"
+#include "os/os_mbuf.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_hs_conn;
+struct hci_data_hdr;
+
+STATS_SECT_START(ble_l2cap_stats)
+    STATS_SECT_ENTRY(chan_create)
+    STATS_SECT_ENTRY(chan_delete)
+    STATS_SECT_ENTRY(update_init)
+    STATS_SECT_ENTRY(update_rx)
+    STATS_SECT_ENTRY(update_fail)
+    STATS_SECT_ENTRY(proc_timeout)
+    STATS_SECT_ENTRY(sig_tx)
+    STATS_SECT_ENTRY(sig_rx)
+    STATS_SECT_ENTRY(sm_tx)
+    STATS_SECT_ENTRY(sm_rx)
+STATS_SECT_END
+extern STATS_SECT_DECL(ble_l2cap_stats) ble_l2cap_stats;
+
+extern struct os_mempool ble_l2cap_chan_pool;
+
+/* This is nimble specific; packets sent to the black hole CID do not elicit
+ * an "invalid CID" response.
+ */
+#define BLE_L2CAP_CID_BLACK_HOLE    0xffff
+
+#define BLE_L2CAP_HDR_SZ    4
+
+typedef uint8_t ble_l2cap_chan_flags;
+
+typedef int ble_l2cap_rx_fn(struct ble_l2cap_chan *chan);
+
+struct ble_l2cap_chan {
+    SLIST_ENTRY(ble_l2cap_chan) next;
+    uint16_t conn_handle;
+    uint16_t dcid;
+    uint16_t scid;
+
+    /* Unions just to avoid confusion on MPS/MTU.
+     * In CoC context, L2CAP MTU is MPS
+     */
+    union {
+        uint16_t my_mtu;
+        uint16_t my_coc_mps;
+    };
+
+    union {
+        uint16_t peer_mtu;
+        uint16_t peer_coc_mps;
+    };
+
+    ble_l2cap_chan_flags flags;
+
+    struct os_mbuf *rx_buf;
+    uint16_t rx_len;        /* Length of current reassembled rx packet. */
+
+    ble_l2cap_rx_fn *rx_fn;
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+    uint16_t psm;
+    struct ble_l2cap_coc_endpoint coc_rx;
+    struct ble_l2cap_coc_endpoint coc_tx;
+    uint16_t initial_credits;
+    ble_l2cap_event_fn *cb;
+    void *cb_arg;
+#endif
+};
+
+struct ble_l2cap_hdr {
+    uint16_t len;
+    uint16_t cid;
+};
+
+typedef int ble_l2cap_tx_fn(struct ble_hs_conn *conn,
+                            struct ble_l2cap_chan *chan);
+
+#define BLE_L2CAP_CHAN_F_TXED_MTU       0x01    /* We have sent our MTU. */
+
+SLIST_HEAD(ble_l2cap_chan_list, ble_l2cap_chan);
+
+int ble_l2cap_parse_hdr(struct os_mbuf *om, int off,
+                        struct ble_l2cap_hdr *l2cap_hdr);
+struct os_mbuf *ble_l2cap_prepend_hdr(struct os_mbuf *om, uint16_t cid,
+                                      uint16_t len);
+
+struct ble_l2cap_chan *ble_l2cap_chan_alloc(uint16_t conn_handle);
+void ble_l2cap_chan_free(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan);
+
+bool ble_l2cap_is_mtu_req_sent(const struct ble_l2cap_chan *chan);
+
+int ble_l2cap_rx(struct ble_hs_conn *conn,
+                 struct hci_data_hdr *hci_hdr,
+                 struct os_mbuf *om,
+                 ble_l2cap_rx_fn **out_rx_cb,
+                 int *out_reject_cid);
+int ble_l2cap_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
+                 struct os_mbuf *txom);
+
+void ble_l2cap_remove_rx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan);
+
+int ble_l2cap_init(void);
+
+/* Below experimental API is available when BLE_VERSION >= 52 */
+int ble_l2cap_enhanced_connect(uint16_t conn_handle,
+                               uint16_t psm, uint16_t mtu,
+                               uint8_t num, struct os_mbuf *sdu_rx[],
+                               ble_l2cap_event_fn *cb, void *cb_arg);
+int ble_l2cap_reconfig(struct ble_l2cap_chan *chans[], uint8_t num, uint16_t new_mtu);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig.c b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig.c
new file mode 100644
index 00000000..bb4d8a5a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig.c
@@ -0,0 +1,1941 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/**
+ * L2CAP Signaling (channel ID = 5).
+ *
+ * Design overview:
+ *
+ * L2CAP sig procedures are initiated by the application via function calls.
+ * Such functions return when either of the following happens:
+ *
+ * (1) The procedure completes (success or failure).
+ * (2) The procedure cannot proceed until a BLE peer responds.
+ *
+ * For (1), the result of the procedure if fully indicated by the function
+ * return code.
+ * For (2), the procedure result is indicated by an application-configured
+ * callback.  The callback is executed when the procedure completes.
+ *
+ * Notes on thread-safety:
+ * 1. The ble_hs mutex must never be locked when an application callback is
+ *    executed.  A callback is free to initiate additional host procedures.
+ * 2. The only resource protected by the mutex is the list of active procedures
+ *    (ble_l2cap_sig_procs).  Thread-safety is achieved by locking the mutex
+ *    during removal and insertion operations.  Procedure objects are only
+ *    modified while they are not in the list.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "nimble/ble.h"
+#include "host/ble_monitor.h"
+#include "ble_hs_priv.h"
+
+/*****************************************************************************
+ * $definitions / declarations                                               *
+ *****************************************************************************/
+
+#define BLE_L2CAP_SIG_UNRESPONSIVE_TIMEOUT      30000   /* Milliseconds. */
+
+#define BLE_L2CAP_SIG_PROC_OP_UPDATE            0
+#define BLE_L2CAP_SIG_PROC_OP_CONNECT           1
+#define BLE_L2CAP_SIG_PROC_OP_RECONFIG          2
+#define BLE_L2CAP_SIG_PROC_OP_DISCONNECT        3
+#define BLE_L2CAP_SIG_PROC_OP_MAX               4
+
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+#define BLE_L2CAP_ECOC_MIN_MTU  (64)
+
+#define BLE_L2CAP_MAX_COC_CONN_REQ  (5)
+#else
+#define BLE_L2CAP_MAX_COC_CONN_REQ  (1)
+#endif
+
+struct ble_l2cap_sig_proc {
+    STAILQ_ENTRY(ble_l2cap_sig_proc) next;
+
+    ble_npl_time_t exp_os_ticks;
+    uint16_t conn_handle;
+    uint8_t op;
+    uint8_t id;
+
+    union {
+        struct {
+            ble_l2cap_sig_update_fn *cb;
+            void *cb_arg;
+        } update;
+        struct {
+            uint8_t chan_cnt;
+            struct ble_l2cap_chan *chan[BLE_L2CAP_MAX_COC_CONN_REQ];
+        } connect;
+        struct {
+            struct ble_l2cap_chan *chan;
+        } disconnect;
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+        struct {
+            uint8_t cid_cnt;
+            uint16_t cids[BLE_L2CAP_MAX_COC_CONN_REQ];
+            uint16_t new_mps;
+            uint16_t new_mtu;
+        } reconfig;
+#endif
+    };
+};
+
+STAILQ_HEAD(ble_l2cap_sig_proc_list, ble_l2cap_sig_proc);
+
+static struct ble_l2cap_sig_proc_list ble_l2cap_sig_procs;
+
+typedef int ble_l2cap_sig_rx_fn(uint16_t conn_handle,
+                                struct ble_l2cap_sig_hdr *hdr,
+                                struct os_mbuf **om);
+
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_rx_noop;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_update_req_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_update_rsp_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_rx_reject;
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_coc_req_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_coc_rsp_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_disc_rsp_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_disc_req_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_le_credits_rx;
+#else
+#define ble_l2cap_sig_coc_req_rx    ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_coc_rsp_rx    ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_disc_rsp_rx   ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_disc_req_rx   ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_le_credits_rx                  ble_l2cap_sig_rx_noop
+#endif
+
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_credit_base_con_req_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_credit_base_con_rsp_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_credit_base_reconfig_req_rx;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_credit_base_reconfig_rsp_rx;
+#else
+#define ble_l2cap_sig_credit_base_con_req_rx      ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_credit_base_con_rsp_rx      ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_credit_base_reconfig_req_rx ble_l2cap_sig_rx_noop
+#define ble_l2cap_sig_credit_base_reconfig_rsp_rx ble_l2cap_sig_rx_noop
+#endif
+
+static ble_l2cap_sig_rx_fn * const ble_l2cap_sig_dispatch[] = {
+    [BLE_L2CAP_SIG_OP_REJECT]               = ble_l2cap_sig_rx_reject,
+    [BLE_L2CAP_SIG_OP_CONNECT_RSP]          = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_CONFIG_RSP]           = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_DISCONN_REQ]          = ble_l2cap_sig_disc_req_rx,
+    [BLE_L2CAP_SIG_OP_DISCONN_RSP]          = ble_l2cap_sig_disc_rsp_rx,
+    [BLE_L2CAP_SIG_OP_ECHO_RSP]             = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_INFO_RSP]             = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_CREATE_CHAN_RSP]      = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_MOVE_CHAN_RSP]        = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_MOVE_CHAN_CONF_RSP]   = ble_l2cap_sig_rx_noop,
+    [BLE_L2CAP_SIG_OP_UPDATE_REQ]           = ble_l2cap_sig_update_req_rx,
+    [BLE_L2CAP_SIG_OP_UPDATE_RSP]           = ble_l2cap_sig_update_rsp_rx,
+    [BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ]   = ble_l2cap_sig_coc_req_rx,
+    [BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_RSP]   = ble_l2cap_sig_coc_rsp_rx,
+    [BLE_L2CAP_SIG_OP_FLOW_CTRL_CREDIT]     = ble_l2cap_sig_le_credits_rx,
+    [BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ]   = ble_l2cap_sig_credit_base_con_req_rx,
+    [BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP]   = ble_l2cap_sig_credit_base_con_rsp_rx,
+    [BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_REQ]  = ble_l2cap_sig_credit_base_reconfig_req_rx,
+    [BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_RSP]  = ble_l2cap_sig_credit_base_reconfig_rsp_rx,
+};
+
+static uint8_t ble_l2cap_sig_cur_id;
+
+static os_membuf_t ble_l2cap_sig_proc_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_SIG_MAX_PROCS),
+                    sizeof (struct ble_l2cap_sig_proc))
+];
+
+static struct os_mempool ble_l2cap_sig_proc_pool;
+
+/*****************************************************************************
+ * $debug                                                                    *
+ *****************************************************************************/
+
+static void
+ble_l2cap_sig_dbg_assert_proc_not_inserted(struct ble_l2cap_sig_proc *proc)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    struct ble_l2cap_sig_proc *cur;
+
+    STAILQ_FOREACH(cur, &ble_l2cap_sig_procs, next) {
+        BLE_HS_DBG_ASSERT(cur != proc);
+    }
+#endif
+}
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+static uint8_t
+ble_l2cap_sig_next_id(void)
+{
+    ble_l2cap_sig_cur_id++;
+    if (ble_l2cap_sig_cur_id == 0) {
+        /* An ID of 0 is illegal. */
+        ble_l2cap_sig_cur_id = 1;
+    }
+
+    return ble_l2cap_sig_cur_id;
+}
+
+static ble_l2cap_sig_rx_fn *
+ble_l2cap_sig_dispatch_get(uint8_t op)
+{
+    if (op >= BLE_L2CAP_SIG_OP_MAX) {
+        return NULL;
+    }
+
+    return ble_l2cap_sig_dispatch[op];
+}
+
+/**
+ * Allocates a proc entry.
+ *
+ * @return                      An entry on success; null on failure.
+ */
+static struct ble_l2cap_sig_proc *
+ble_l2cap_sig_proc_alloc(void)
+{
+    struct ble_l2cap_sig_proc *proc;
+
+    proc = os_memblock_get(&ble_l2cap_sig_proc_pool);
+    if (proc != NULL) {
+        memset(proc, 0, sizeof *proc);
+    }
+
+    return proc;
+}
+
+/**
+ * Frees the specified proc entry.  No-op if passed a null pointer.
+ */
+static void
+ble_l2cap_sig_proc_free(struct ble_l2cap_sig_proc *proc)
+{
+    int rc;
+
+    if (proc != NULL) {
+        ble_l2cap_sig_dbg_assert_proc_not_inserted(proc);
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        memset(proc, 0xff, sizeof *proc);
+#endif
+        rc = os_memblock_put(&ble_l2cap_sig_proc_pool, proc);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static void
+ble_l2cap_sig_proc_insert(struct ble_l2cap_sig_proc *proc)
+{
+    ble_l2cap_sig_dbg_assert_proc_not_inserted(proc);
+
+    ble_hs_lock();
+    STAILQ_INSERT_HEAD(&ble_l2cap_sig_procs, proc, next);
+    ble_hs_unlock();
+}
+
+/**
+ * Tests if a proc entry fits the specified criteria.
+ *
+ * @param proc                  The procedure to test.
+ * @param conn_handle           The connection handle to match against.
+ * @param op                    The op code to match against/
+ * @param id                    The identifier to match against.
+ *                                  0=Ignore this criterion.
+ *
+ * @return                      1 if the proc matches; 0 otherwise.
+ */
+static int
+ble_l2cap_sig_proc_matches(struct ble_l2cap_sig_proc *proc,
+                           uint16_t conn_handle, uint8_t op, uint8_t id)
+{
+    if (conn_handle != proc->conn_handle) {
+        return 0;
+    }
+
+    if (op != proc->op) {
+        return 0;
+    }
+
+    if (id != 0 && id != proc->id) {
+        return 0;
+    }
+
+    return 1;
+}
+
+/**
+ * Searches the main proc list for an "expecting" entry whose connection handle
+ * and op code match those specified.  If a matching entry is found, it is
+ * removed from the list and returned.
+ *
+ * @param conn_handle           The connection handle to match against.
+ * @param op                    The op code to match against.
+ * @param identifier            The identifier to match against;
+ *                                  0=ignore this criterion.
+ *
+ * @return                      The matching proc entry on success;
+ *                                  null on failure.
+ */
+static struct ble_l2cap_sig_proc *
+ble_l2cap_sig_proc_extract(uint16_t conn_handle, uint8_t op,
+                           uint8_t identifier)
+{
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_sig_proc *prev;
+
+    ble_hs_lock();
+
+    prev = NULL;
+    STAILQ_FOREACH(proc, &ble_l2cap_sig_procs, next) {
+        if (ble_l2cap_sig_proc_matches(proc, conn_handle, op, identifier)) {
+            if (prev == NULL) {
+                STAILQ_REMOVE_HEAD(&ble_l2cap_sig_procs, next);
+            } else {
+                STAILQ_REMOVE_AFTER(&ble_l2cap_sig_procs, prev, next);
+            }
+            break;
+        }
+        prev = proc;
+    }
+
+    ble_hs_unlock();
+
+    return proc;
+}
+
+static int
+ble_l2cap_sig_rx_noop(uint16_t conn_handle,
+                      struct ble_l2cap_sig_hdr *hdr,
+                      struct os_mbuf **om)
+{
+    return BLE_HS_ENOTSUP;
+}
+
+static void
+ble_l2cap_sig_proc_set_timer(struct ble_l2cap_sig_proc *proc)
+{
+    proc->exp_os_ticks = ble_npl_time_get() +
+                         ble_npl_time_ms_to_ticks32(BLE_L2CAP_SIG_UNRESPONSIVE_TIMEOUT);
+    ble_hs_timer_resched();
+}
+
+static void
+ble_l2cap_sig_process_status(struct ble_l2cap_sig_proc *proc, int status)
+{
+    if (status == 0) {
+        ble_l2cap_sig_proc_set_timer(proc);
+        ble_l2cap_sig_proc_insert(proc);
+    } else {
+        ble_l2cap_sig_proc_free(proc);
+    }
+}
+
+/*****************************************************************************
+ * $update                                                                   *
+ *****************************************************************************/
+
+static void
+ble_l2cap_sig_update_call_cb(struct ble_l2cap_sig_proc *proc, int status)
+{
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+
+    if (status != 0) {
+        STATS_INC(ble_l2cap_stats, update_fail);
+    }
+
+    if (proc->update.cb != NULL) {
+        proc->update.cb(proc->conn_handle, status, proc->update.cb_arg);
+    }
+}
+
+int
+ble_l2cap_sig_update_req_rx(uint16_t conn_handle,
+                            struct ble_l2cap_sig_hdr *hdr,
+                            struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_update_req *req;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_update_rsp *rsp;
+    struct ble_gap_upd_params params;
+    ble_hs_conn_flags_t conn_flags;
+    uint16_t l2cap_result;
+    int sig_err;
+    int rc;
+
+    l2cap_result = 0; /* Silence spurious gcc warning. */
+
+    rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SIG_UPDATE_REQ_SZ);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_hs_atomic_conn_flags(conn_handle, &conn_flags);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Only a master can process an update request. */
+    sig_err = !(conn_flags & BLE_HS_CONN_F_MASTER);
+    if (sig_err) {
+        return BLE_HS_EREJECT;
+    }
+
+    req = (struct ble_l2cap_sig_update_req *)(*om)->om_data;
+
+    params.itvl_min = le16toh(req->itvl_min);
+    params.itvl_max = le16toh(req->itvl_max);
+    params.latency = le16toh(req->slave_latency);
+    params.supervision_timeout = le16toh(req->timeout_multiplier);
+    params.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
+    params.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
+
+    /* Ask application if slave's connection parameters are acceptable. */
+    rc = ble_gap_rx_l2cap_update_req(conn_handle, &params);
+    if (rc == 0) {
+        /* Application agrees to accept parameters; schedule update. */
+        rc = ble_gap_update_params(conn_handle, &params);
+    }
+
+    if (rc == 0) {
+        l2cap_result = BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT;
+    } else {
+        l2cap_result = BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT;
+    }
+
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_UPDATE_RSP, hdr->identifier,
+                                sizeof(*rsp), &txom);
+    if (!rsp) {
+        /* No memory for response, lest allow to timeout on remote side */
+        return 0;
+    }
+
+    rsp->result = htole16(l2cap_result);
+
+    /* Send L2CAP response. */
+    ble_l2cap_sig_tx(conn_handle, txom);
+
+    return 0;
+}
+
+static int
+ble_l2cap_sig_update_rsp_rx(uint16_t conn_handle,
+                            struct ble_l2cap_sig_hdr *hdr,
+                            struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_update_rsp *rsp;
+    struct ble_l2cap_sig_proc *proc;
+    int cb_status;
+    int rc;
+
+    proc = ble_l2cap_sig_proc_extract(conn_handle,
+                                      BLE_L2CAP_SIG_PROC_OP_UPDATE,
+                                      hdr->identifier);
+    if (proc == NULL) {
+        return 0;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SIG_UPDATE_RSP_SZ);
+    if (rc != 0) {
+        cb_status = rc;
+        goto done;
+    }
+
+    rsp = (struct ble_l2cap_sig_update_rsp *)(*om)->om_data;
+
+    switch (le16toh(rsp->result)) {
+    case BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT:
+        cb_status = 0;
+        rc = 0;
+        break;
+
+    case BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT:
+        cb_status = BLE_HS_EREJECT;
+        rc = 0;
+        break;
+
+    default:
+        cb_status = BLE_HS_EBADDATA;
+        rc = 0;
+        break;
+    }
+
+done:
+    ble_l2cap_sig_update_call_cb(proc, cb_status);
+    ble_l2cap_sig_proc_free(proc);
+    return rc;
+}
+
+int
+ble_l2cap_sig_update(uint16_t conn_handle,
+                     struct ble_l2cap_sig_update_params *params,
+                     ble_l2cap_sig_update_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_update_req *req;
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int master;
+    int rc;
+
+    proc = NULL;
+
+    STATS_INC(ble_l2cap_stats, update_init);
+
+    ble_hs_lock();
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                    &conn, &chan);
+    master = conn->bhc_flags & BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    if (master) {
+        /* Only the slave can initiate the L2CAP connection update
+         * procedure.
+         */
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    proc = ble_l2cap_sig_proc_alloc();
+    if (proc == NULL) {
+        STATS_INC(ble_l2cap_stats, update_fail);
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    proc->op = BLE_L2CAP_SIG_PROC_OP_UPDATE;
+    proc->id = ble_l2cap_sig_next_id();
+    proc->conn_handle = conn_handle;
+    proc->update.cb = cb;
+    proc->update.cb_arg = cb_arg;
+
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_UPDATE_REQ, proc->id,
+                                sizeof(*req), &txom);
+    if (!req) {
+        STATS_INC(ble_l2cap_stats, update_fail);
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    req->itvl_min = htole16(params->itvl_min);
+    req->itvl_max = htole16(params->itvl_max);
+    req->slave_latency = htole16(params->slave_latency);
+    req->timeout_multiplier = htole16(params->timeout_multiplier);
+
+    rc = ble_l2cap_sig_tx(conn_handle, txom);
+
+done:
+    ble_l2cap_sig_process_status(proc, rc);
+    return rc;
+}
+
+/*****************************************************************************
+ * $connect                                                                  *
+ *****************************************************************************/
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+
+static int
+ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err)
+{
+    switch (l2cap_coc_err) {
+    case BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS:
+        return 0;
+    case BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM:
+        return BLE_HS_ENOTSUP;
+    case BLE_L2CAP_COC_ERR_NO_RESOURCES:
+        return BLE_HS_ENOMEM;
+    case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN:
+        return BLE_HS_EAUTHEN;
+    case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR:
+        return BLE_HS_EAUTHOR;
+    case BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ:
+        return BLE_HS_EENCRYPT_KEY_SZ;
+    case BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC:
+        return BLE_HS_EENCRYPT;
+    case BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID:
+        return BLE_HS_EREJECT;
+    case BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED:
+        return BLE_HS_EALREADY;
+    case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS:
+        return BLE_HS_EINVAL;
+    default:
+        return BLE_HS_EUNKNOWN;
+    }
+}
+
+static int
+ble_l2cap_sig_ble_hs_err2coc_err(uint16_t ble_hs_err)
+{
+    switch (ble_hs_err) {
+    case BLE_HS_ENOTSUP:
+        return BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM;
+    case BLE_HS_ENOMEM:
+        return BLE_L2CAP_COC_ERR_NO_RESOURCES;
+    case BLE_HS_EAUTHEN:
+        return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN;
+    case BLE_HS_EAUTHOR:
+        return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR;
+    case BLE_HS_EENCRYPT:
+        return BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC;
+    case BLE_HS_EENCRYPT_KEY_SZ:
+        return BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ;
+    case BLE_HS_EINVAL:
+        return BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS;
+    default:
+        return BLE_L2CAP_COC_ERR_NO_RESOURCES;
+    }
+}
+
+static void
+ble_l2cap_event_coc_connected(struct ble_l2cap_chan *chan, uint16_t status)
+{
+    struct ble_l2cap_event event = { };
+
+    event.type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    event.connect.conn_handle = chan->conn_handle;
+    event.connect.chan = chan;
+    event.connect.status = status;
+
+    chan->cb(&event, chan->cb_arg);
+}
+
+static int
+ble_l2cap_event_coc_accept(struct ble_l2cap_chan *chan, uint16_t peer_sdu_size)
+{
+    struct ble_l2cap_event event = { };
+
+    event.type = BLE_L2CAP_EVENT_COC_ACCEPT;
+    event.accept.chan = chan;
+    event.accept.conn_handle = chan->conn_handle;
+    event.accept.peer_sdu_size = peer_sdu_size;
+
+    return chan->cb(&event, chan->cb_arg);
+}
+
+static void
+ble_l2cap_sig_coc_connect_cb(struct ble_l2cap_sig_proc *proc, int status)
+{
+    struct ble_hs_conn *conn;
+    struct ble_l2cap_chan *chan;
+    int i;
+    bool some_not_connected = false;
+
+    if (!proc) {
+            return;
+    }
+
+    for (i = 0; i < proc->connect.chan_cnt; i++) {
+        chan = proc->connect.chan[i];
+        if (!chan || !chan->cb) {
+            continue;
+        }
+
+        if ((status == 0) && (chan->dcid != 0)) {
+            ble_l2cap_event_coc_connected(chan, status);
+            /* Let's forget about connected channel now.
+             * Not connected will be freed later on.
+             */
+            proc->connect.chan[i] = NULL;
+            continue;
+        }
+        some_not_connected = true;
+        ble_l2cap_event_coc_connected(chan, status ? status : BLE_HS_EREJECT);
+    }
+
+    if (!some_not_connected) {
+        return;
+    }
+
+    /* Free not connected channels*/
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(chan->conn_handle);
+    for (i = 0; i < proc->connect.chan_cnt; i++) {
+        chan = proc->connect.chan[i];
+        if (chan) {
+            /* Normally in channel free we send disconnected event to application.
+             * However in case on error during creation connection we send connected
+             * event with error status. To avoid additional disconnected event lets
+             * clear callbacks since we don't needed it anymore.
+             */
+            chan->cb = NULL;
+            ble_l2cap_chan_free(conn, chan);
+        }
+    }
+    ble_hs_unlock();
+}
+
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+static void
+ble_l2cap_event_coc_reconfigured(uint16_t conn_handle, uint16_t status,
+                                 struct ble_l2cap_chan *chan, bool peer)
+{
+    struct ble_l2cap_event event = { };
+
+    if (peer) {
+        event.type = BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED;
+    } else {
+        event.type = BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED;
+    }
+    event.reconfigured.conn_handle = conn_handle;
+    event.reconfigured.chan = chan;
+    event.reconfigured.status = status;
+
+    chan->cb(&event, chan->cb_arg);
+}
+
+static int
+ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle,
+                                     struct ble_l2cap_sig_hdr *hdr,
+                                     struct os_mbuf **om)
+{
+    struct ble_l2cap_chan *chan[BLE_L2CAP_MAX_COC_CONN_REQ] = {0};
+    struct ble_l2cap_sig_credit_base_reconfig_req *req;
+    struct ble_l2cap_sig_credit_base_reconfig_rsp *rsp;
+    struct ble_hs_conn *conn;
+    struct os_mbuf *txom;
+    int i;
+    int rc;
+    uint8_t cid_cnt;
+    uint8_t reduction_mps = 0;
+
+    rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        return 0;
+    }
+
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_RSP,
+                                    hdr->identifier, sizeof(*rsp) , &txom);
+    if (!rsp) {
+        /* TODO: Reuse request buffer for the response. For now in such a case
+         * remote will timeout.
+         */
+        BLE_HS_LOG(ERROR, "No memory for the response\n");
+        ble_hs_unlock();
+        return 0;
+    }
+
+    if (hdr->length <= sizeof(*req)) {
+        rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM);
+        goto failed;
+    }
+
+    req = (struct ble_l2cap_sig_credit_base_reconfig_req *)(*om)->om_data;
+
+    if ((req->mps < BLE_L2CAP_ECOC_MIN_MTU) || (req->mtu < BLE_L2CAP_ECOC_MIN_MTU)) {
+        rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM);
+        goto failed;
+    }
+
+    /* Assume request will succeed. If not, result will be updated */
+    rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_SUCCEED);
+
+    cid_cnt = (hdr->length - sizeof(*req)) / sizeof(uint16_t);
+    if (cid_cnt > BLE_L2CAP_MAX_COC_CONN_REQ) {
+        rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM);
+        goto failed;
+    }
+
+    for (i = 0; i < cid_cnt; i++) {
+        chan[i] = ble_hs_conn_chan_find_by_dcid(conn, req->dcids[i]);
+        if (!chan[i]) {
+             rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_INVALID_DCID);
+             goto failed;
+        }
+
+        if (chan[i]->peer_coc_mps > req->mps) {
+            reduction_mps++;
+            if (reduction_mps > 1) {
+                rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_REDUCTION_MPS_NOT_ALLOWED);
+                goto failed;
+            }
+        }
+
+        if (chan[i]->coc_tx.mtu > req->mtu) {
+            rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED);
+            goto failed;
+        }
+    }
+
+    ble_hs_unlock();
+
+    for (i = 0; i < cid_cnt; i++) {
+        chan[i]->coc_tx.mtu = req->mtu;
+        chan[i]->peer_coc_mps = req->mps;
+        ble_l2cap_event_coc_reconfigured(conn_handle, 0, chan[i], true);
+    }
+
+    ble_l2cap_sig_tx(conn_handle, txom);
+    return 0;
+
+failed:
+    ble_hs_unlock();
+    ble_l2cap_sig_tx(conn_handle, txom);
+    return 0;
+}
+
+static void
+ble_l2cap_sig_coc_reconfig_cb(struct ble_l2cap_sig_proc *proc, int status)
+{
+    int i;
+    struct ble_l2cap_chan *chan[BLE_L2CAP_MAX_COC_CONN_REQ] = {0};
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(proc->conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        return;
+    }
+
+    for (i = 0; i< proc->reconfig.cid_cnt; i++) {
+        chan[i] = ble_hs_conn_chan_find_by_scid(conn, proc->reconfig.cids[i]);
+        if (status == 0) {
+            ble_l2cap_coc_set_new_mtu_mps(chan[i], proc->reconfig.new_mtu, proc->reconfig.new_mps);
+        }
+    }
+
+    ble_hs_unlock();
+
+    for (i = 0; i < proc->reconfig.cid_cnt; i++) {
+        ble_l2cap_event_coc_reconfigured(proc->conn_handle, status, chan[i], false);
+    }
+}
+
+static int
+ble_l2cap_sig_credit_base_reconfig_rsp_rx(uint16_t conn_handle,
+                                     struct ble_l2cap_sig_hdr *hdr,
+                                     struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_sig_credit_base_reconfig_rsp *rsp;
+    int rc;
+
+    proc = ble_l2cap_sig_proc_extract(conn_handle,
+                                      BLE_L2CAP_SIG_PROC_OP_RECONFIG,
+                                      hdr->identifier);
+    if (!proc) {
+        return 0;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rsp = (struct ble_l2cap_sig_credit_base_reconfig_rsp *)(*om)->om_data;
+    ble_l2cap_sig_coc_reconfig_cb(proc, (rsp->result > 0) ? BLE_HS_EREJECT : 0);
+
+    return 0;
+}
+
+static int
+ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle,
+                                     struct ble_l2cap_sig_hdr *hdr,
+                                     struct os_mbuf **om)
+{
+    int rc;
+    struct ble_l2cap_sig_credit_base_connect_req *req;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_credit_base_connect_rsp *rsp;
+    struct ble_l2cap_chan *chans[5] = { 0 };
+    struct ble_hs_conn *conn;
+    uint16_t scid;
+    uint8_t num_of_scids;
+    uint8_t chan_created = 0;
+    int i;
+    uint8_t len;
+
+    rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+    if (rc != 0) {
+        return rc;
+    }
+
+    len = (hdr->length > sizeof(*req)) ? hdr->length : sizeof(*req);
+
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP,
+                                hdr->identifier, len , &txom);
+    if (!rsp) {
+        /* Well, nothing smart we can do if there is no memory for response.
+         * Remote will timeout.
+         */
+        return 0;
+    }
+
+    ble_hs_lock();
+
+    memset(rsp, 0, len);
+
+    /* Initial dummy values in case of error, just to satisfy PTS */
+    rsp->credits = htole16(1);
+    rsp->mps = htole16(BLE_L2CAP_ECOC_MIN_MTU);
+    rsp->mtu = htole16(BLE_L2CAP_ECOC_MIN_MTU);
+
+    if (hdr->length <= sizeof(*req)) {
+        rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS);
+        goto failed;
+    }
+
+    req = (struct ble_l2cap_sig_credit_base_connect_req *)(*om)->om_data;
+
+    num_of_scids = (hdr->length - sizeof(*req)) / sizeof(uint16_t);
+    if (num_of_scids > 5) {
+        rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS);
+        goto failed;
+    }
+
+    if ((req->mtu < BLE_L2CAP_ECOC_MIN_MTU) || (req->mps < BLE_L2CAP_ECOC_MIN_MTU)) {
+        rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS);
+        goto failed;
+    }
+
+    conn = ble_hs_conn_find_assert(conn_handle);
+
+    /* First verify that provided SCIDs are good */
+    for (i = 0; i < num_of_scids; i++) {
+        scid = le16toh(req->scids[i]);
+        if (scid < BLE_L2CAP_COC_CID_START || scid > BLE_L2CAP_COC_CID_END) {
+            rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID);
+            goto failed;
+        }
+    }
+
+    /* Let us try to connect channels */
+    for (i = 0; i < num_of_scids; i++) {
+        /* Verify CID. Note, scid in the request is dcid for out local channel */
+        scid = le16toh(req->scids[i]);
+        chans[i] = ble_hs_conn_chan_find_by_dcid(conn, scid);
+        if (chans[i]) {
+            rsp->result = htole16(BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED);
+            rsp->dcids[i] = htole16(chans[i]->scid);
+            continue;
+        }
+
+        rc = ble_l2cap_coc_create_srv_chan(conn, le16toh(req->psm), &chans[i]);
+        if (rc != 0) {
+            if (i == 0) {
+                /* In case it is very first channel we cannot create it means PSM is incorrect
+                 * or we are out of resources. Just send a response now.
+                 */
+                rsp->result = htole16(ble_l2cap_sig_ble_hs_err2coc_err(rc));
+                goto failed;
+            } else {
+                /* We cannot create number of channels req by peer due to limited resources. */
+                rsp->result = htole16(BLE_L2CAP_COC_ERR_NO_RESOURCES);
+                goto done;
+            }
+        }
+
+        /* Fill up remote configuration. Note MPS is the L2CAP MTU*/
+        chans[i]->dcid = scid;
+        chans[i]->peer_coc_mps = le16toh(req->mps);
+        chans[i]->coc_tx.credits = le16toh(req->credits);
+        chans[i]->coc_tx.mtu = le16toh(req->mtu);
+
+        ble_hs_conn_chan_insert(conn, chans[i]);
+        /* Sending event to the app. Unlock hs */
+        ble_hs_unlock();
+
+        rc = ble_l2cap_event_coc_accept(chans[i], le16toh(req->mtu));
+        if (rc == 0) {
+            rsp->dcids[i] = htole16(chans[i]->scid);
+            chan_created++;
+            if (chan_created == 1) {
+                /* We need to set it once as there are same initial parameters
+                 * for all the channels
+                 */
+                rsp->credits = htole16(chans[i]->coc_rx.credits);
+                rsp->mps = htole16(chans[i]->my_mtu);
+                rsp->mtu = htole16(chans[i]->coc_rx.mtu);
+            }
+        } else {
+            /* Make sure we do not send disconnect event when removing channel */
+            chans[i]->cb = NULL;
+
+            ble_hs_lock();
+            conn = ble_hs_conn_find_assert(conn_handle);
+            ble_hs_conn_delete_chan(conn, chans[i]);
+            chans[i] = NULL;
+            rsp->result = htole16(ble_l2cap_sig_ble_hs_err2coc_err(rc));
+            rc = 0;
+            ble_hs_unlock();
+        }
+
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(conn_handle);
+    }
+
+done:
+    ble_hs_unlock();
+    rc = ble_l2cap_sig_tx(conn_handle, txom);
+    if (rc != 0) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(conn_handle);
+        for (i = 0; i < num_of_scids; i++) {
+            if (chans[i]) {
+                ble_hs_conn_delete_chan(conn, chans[i]);
+            }
+        }
+        ble_hs_unlock();
+        return 0;
+    }
+
+    /* Notify user about connection status */
+    for (i = 0; i < num_of_scids; i++) {
+        if (chans[i]) {
+            ble_l2cap_event_coc_connected(chans[i], rc);
+        }
+    }
+
+    return 0;
+
+failed:
+    ble_hs_unlock();
+    ble_l2cap_sig_tx(conn_handle, txom);
+    return 0;
+}
+
+static int
+ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle,
+                                     struct ble_l2cap_sig_hdr *hdr,
+                                     struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_sig_credit_base_connect_rsp *rsp;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+    int i;
+
+#if !BLE_MONITOR
+    BLE_HS_LOG(DEBUG, "L2CAP LE COC connection response received\n");
+#endif
+
+    proc = ble_l2cap_sig_proc_extract(conn_handle,
+                                      BLE_L2CAP_SIG_PROC_OP_CONNECT,
+                                      hdr->identifier);
+    if (!proc) {
+        return 0;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rsp = (struct ble_l2cap_sig_credit_base_connect_rsp *)(*om)->om_data;
+
+    if (rsp->result) {
+        rc = ble_l2cap_sig_coc_err2ble_hs_err(le16toh(rsp->result));
+        goto done;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    assert(conn != NULL);
+
+    for (i = 0; i < proc->connect.chan_cnt; i++) {
+        chan = proc->connect.chan[i];
+        if (rsp->dcids[i] == 0) {
+            /* Channel rejected, dont put it on the list.
+             * User will get notified later in that function
+             */
+            chan->dcid = 0;
+            continue;
+        }
+        chan->peer_coc_mps = le16toh(rsp->mps);
+        chan->dcid = le16toh(rsp->dcids[i]);
+        chan->coc_tx.mtu = le16toh(rsp->mtu);
+        chan->coc_tx.credits = le16toh(rsp->credits);
+
+        ble_hs_conn_chan_insert(conn, chan);
+    }
+
+    ble_hs_unlock();
+
+done:
+    ble_l2cap_sig_coc_connect_cb(proc, rc);
+    ble_l2cap_sig_proc_free(proc);
+
+    /* Silently ignore errors as this is response signal */
+    return 0;
+}
+#endif
+
+static int
+ble_l2cap_sig_coc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                         struct os_mbuf **om)
+{
+    int rc;
+    struct ble_l2cap_sig_le_con_req *req;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_le_con_rsp *rsp;
+    struct ble_l2cap_chan *chan = NULL;
+    struct ble_hs_conn *conn;
+    uint16_t scid;
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(req));
+    if (rc != 0) {
+        return rc;
+    }
+
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_RSP,
+                                hdr->identifier, sizeof(*rsp), &txom);
+    if (!rsp) {
+        /* Well, nothing smart we can do if there is no memory for response.
+         * Remote will timeout.
+         */
+        return 0;
+    }
+
+    memset(rsp, 0, sizeof(*rsp));
+
+    req = (struct ble_l2cap_sig_le_con_req *)(*om)->om_data;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find_assert(conn_handle);
+
+    /* Verify CID. Note, scid in the request is dcid for out local channel */
+    scid = le16toh(req->scid);
+    if (scid < BLE_L2CAP_COC_CID_START || scid > BLE_L2CAP_COC_CID_END) {
+        rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID);
+        ble_hs_unlock();
+        goto failed;
+    }
+
+    chan = ble_hs_conn_chan_find_by_dcid(conn, scid);
+    if (chan) {
+        rsp->result = htole16(BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED);
+        ble_hs_unlock();
+        goto failed;
+    }
+
+    rc = ble_l2cap_coc_create_srv_chan(conn, le16toh(req->psm), &chan);
+    if (rc != 0) {
+        uint16_t coc_err = ble_l2cap_sig_ble_hs_err2coc_err(rc);
+        rsp->result = htole16(coc_err);
+        ble_hs_unlock();
+        goto failed;
+    }
+
+    /* Fill up remote configuration. Note MPS is the L2CAP MTU*/
+    chan->dcid = scid;
+    chan->peer_coc_mps = le16toh(req->mps);
+    chan->coc_tx.credits = le16toh(req->credits);
+    chan->coc_tx.mtu = le16toh(req->mtu);
+
+    ble_hs_conn_chan_insert(conn, chan);
+    ble_hs_unlock();
+
+    rc = ble_l2cap_event_coc_accept(chan, le16toh(req->mtu));
+    if (rc != 0) {
+        uint16_t coc_err = ble_l2cap_sig_ble_hs_err2coc_err(rc);
+
+        /* Make sure we do not send disconnect event when removing channel */
+        chan->cb = NULL;
+
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(conn_handle);
+        ble_hs_conn_delete_chan(conn, chan);
+        ble_hs_unlock();
+        rsp->result = htole16(coc_err);
+        goto failed;
+    }
+
+    rsp->dcid = htole16(chan->scid);
+    rsp->credits = htole16(chan->coc_rx.credits);
+    rsp->mps = htole16(chan->my_coc_mps);
+    rsp->mtu = htole16(chan->coc_rx.mtu);
+    rsp->result = htole16(BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS);
+
+    rc = ble_l2cap_sig_tx(conn_handle, txom);
+    if (rc != 0) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(conn_handle);
+        ble_hs_conn_delete_chan(conn, chan);
+        ble_hs_unlock();
+        return 0;
+    }
+
+    /* Notify user about connection status */
+    ble_l2cap_event_coc_connected(chan, rc);
+
+    return 0;
+
+failed:
+    ble_l2cap_sig_tx(conn_handle, txom);
+    return 0;
+}
+
+static int
+ble_l2cap_sig_coc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                          struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_sig_le_con_rsp *rsp;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+#if !BLE_MONITOR
+    BLE_HS_LOG(DEBUG, "L2CAP LE COC connection response received\n");
+#endif
+
+    proc = ble_l2cap_sig_proc_extract(conn_handle,
+                                      BLE_L2CAP_SIG_PROC_OP_CONNECT,
+                                      hdr->identifier);
+    if (!proc) {
+        return 0;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(*rsp));
+    if (rc != 0) {
+        goto done;
+    }
+
+    rsp = (struct ble_l2cap_sig_le_con_rsp *)(*om)->om_data;
+
+    chan = proc->connect.chan[0];
+
+    if (rsp->result) {
+        rc = ble_l2cap_sig_coc_err2ble_hs_err(le16toh(rsp->result));
+        goto done;
+    }
+
+    /* Fill up remote configuration
+     * Note MPS is the L2CAP MTU
+     */
+    chan->peer_coc_mps = le16toh(rsp->mps);
+    chan->dcid = le16toh(rsp->dcid);
+    chan->coc_tx.mtu = le16toh(rsp->mtu);
+    chan->coc_tx.credits = le16toh(rsp->credits);
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    assert(conn != NULL);
+    ble_hs_conn_chan_insert(conn, chan);
+    ble_hs_unlock();
+
+    rc = 0;
+
+done:
+    ble_l2cap_sig_coc_connect_cb(proc, rc);
+    ble_l2cap_sig_proc_free(proc);
+
+    /* Silently ignore errors as this is response signal */
+    return 0;
+}
+
+int
+ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
+                          struct os_mbuf *sdu_rx,
+                          ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    struct ble_hs_conn *conn;
+    struct ble_l2cap_sig_proc *proc;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_le_con_req *req;
+    struct ble_l2cap_chan *chan = NULL;
+    int rc;
+
+    if (!sdu_rx || !cb) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+
+    if (!conn) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    chan = ble_l2cap_coc_chan_alloc(conn, psm, mtu, sdu_rx, cb, cb_arg);
+    if (!chan) {
+        ble_hs_unlock();
+        return BLE_HS_ENOMEM;
+    }
+
+    proc = ble_l2cap_sig_proc_alloc();
+    if (!proc) {
+        ble_l2cap_chan_free(conn, chan);
+        ble_hs_unlock();
+        return BLE_HS_ENOMEM;
+    }
+
+    proc->op = BLE_L2CAP_SIG_PROC_OP_CONNECT;
+    proc->id = ble_l2cap_sig_next_id();
+    proc->conn_handle = conn_handle;
+    proc->connect.chan[0] = chan;
+    proc->connect.chan_cnt = 1;
+
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ, proc->id,
+                                sizeof(*req), &txom);
+    if (!req) {
+        ble_l2cap_chan_free(conn, chan);
+        ble_hs_unlock();
+        rc = BLE_HS_ENOMEM;
+        /* Goto done to clear proc */
+        goto done;
+    }
+
+    req->psm = htole16(psm);
+    req->scid = htole16(chan->scid);
+    req->mtu = htole16(chan->coc_rx.mtu);
+    req->mps = htole16(chan->my_coc_mps);
+    req->credits = htole16(chan->coc_rx.credits);
+
+    ble_hs_unlock();
+
+    rc = ble_l2cap_sig_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find_assert(conn_handle);
+        ble_l2cap_chan_free(conn, chan);
+        ble_hs_unlock();
+    }
+
+done:
+    ble_l2cap_sig_process_status(proc, rc);
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+int
+ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
+                           uint8_t num, struct os_mbuf *sdu_rx[],
+                           ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    struct ble_hs_conn *conn;
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_chan *chan = NULL;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_credit_base_connect_req *req;
+    int rc;
+    int i;
+    int j;
+
+    if (!sdu_rx || !cb) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+
+    if (!conn) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    proc = ble_l2cap_sig_proc_alloc();
+    if (!proc) {
+        ble_hs_unlock();
+        return BLE_HS_ENOMEM;
+    }
+
+    proc->op = BLE_L2CAP_SIG_PROC_OP_CONNECT;
+    proc->id = ble_l2cap_sig_next_id();
+    proc->conn_handle = conn_handle;
+
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ, proc->id,
+                                sizeof(*req) + num * sizeof(uint16_t), &txom);
+    if (!req) {
+        ble_hs_unlock();
+        rc = BLE_HS_ENOMEM;
+        /* Goto done to clear proc */
+        goto done;
+    }
+
+    for (i = 0; i < num; i++) {
+        chan = ble_l2cap_coc_chan_alloc(conn, psm, mtu, sdu_rx[i], cb, cb_arg);
+        if (!chan) {
+            /* Clear request buffer */
+            os_mbuf_free_chain(txom);
+
+            for (j = 0; j < i; j++) {
+                /* Clear callback to make sure "Disconnected event" to the user */
+                chan[j].cb = NULL;
+                ble_l2cap_chan_free(conn, proc->connect.chan[j]);
+            }
+            ble_hs_unlock();
+            rc = BLE_HS_ENOMEM;
+            goto done;
+        }
+        proc->connect.chan[i] = chan;
+    }
+    proc->connect.chan_cnt = num;
+
+    req->psm = htole16(psm);
+    req->mtu = htole16(chan->coc_rx.mtu);
+    req->mps = htole16(chan->my_mtu);
+    req->credits = htole16(chan->coc_rx.credits);
+    for (i = 0; i < num; i++) {
+        req->scids[i] = htole16(proc->connect.chan[i]->scid);
+    }
+
+    ble_hs_unlock();
+
+    rc = ble_l2cap_sig_tx(proc->conn_handle, txom);
+
+done:
+    ble_l2cap_sig_process_status(proc, rc);
+
+    return rc;
+}
+
+int
+ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
+                           uint8_t num, uint16_t new_mtu)
+{
+    struct ble_hs_conn *conn;
+    struct ble_l2cap_sig_proc *proc;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_credit_base_reconfig_req *req;
+    int rc;
+    int i;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+
+    if (!conn) {
+        ble_hs_unlock();
+        return BLE_HS_ENOTCONN;
+    }
+
+    proc = ble_l2cap_sig_proc_alloc();
+    if (!proc) {
+        ble_hs_unlock();
+        return BLE_HS_ENOMEM;
+    }
+
+    for (i = 0; i < num; i++) {
+        if (ble_hs_conn_chan_exist(conn, chans[i])) {
+            proc->reconfig.cids[i] = chans[i]->scid;
+        } else {
+            ble_hs_unlock();
+            rc = BLE_HS_ENOMEM;
+            goto done;
+        }
+    }
+
+    proc->op = BLE_L2CAP_SIG_PROC_OP_RECONFIG;
+    proc->reconfig.cid_cnt = num;
+    proc->reconfig.new_mtu = new_mtu;
+    proc->reconfig.new_mps = MYNEWT_VAL(BLE_L2CAP_COC_MPS);
+    proc->id = ble_l2cap_sig_next_id();
+    proc->conn_handle = conn_handle;
+
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_REQ, proc->id,
+                                sizeof(*req) + num * sizeof(uint16_t), &txom);
+    if (!req) {
+        ble_hs_unlock();
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    /* For now we allow to change CoC MTU only.*/
+    req->mtu = htole16(proc->reconfig.new_mtu);
+    req->mps = htole16(proc->reconfig.new_mps);
+
+    for (i = 0; i < num; i++) {
+        req->dcids[i] = htole16(proc->reconfig.cids[i]);
+    }
+
+    ble_hs_unlock();
+
+    rc = ble_l2cap_sig_tx(proc->conn_handle, txom);
+
+done:
+    ble_l2cap_sig_process_status(proc, rc);
+
+    return rc;
+}
+#endif
+
+/*****************************************************************************
+ * $disconnect                                                               *
+ *****************************************************************************/
+
+static int
+ble_l2cap_sig_disc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                          struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_disc_req *req;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_disc_rsp *rsp;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(*req));
+    if (rc != 0) {
+        return rc;
+    }
+
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_DISCONN_RSP, hdr->identifier,
+                                sizeof(*rsp), &txom);
+    if (!rsp) {
+        /* Well, nothing smart we can do if there is no memory for response.
+         * Remote will timeout.
+         */
+        return 0;
+    }
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find_assert(conn_handle);
+
+    req = (struct ble_l2cap_sig_disc_req *) (*om)->om_data;
+
+    /* Let's find matching channel. Note that destination CID in the request
+     * is from peer perspective. It is source CID from nimble perspective
+     */
+    chan = ble_hs_conn_chan_find_by_scid(conn, le16toh(req->dcid));
+    if (!chan || (le16toh(req->scid) != chan->dcid)) {
+        os_mbuf_free_chain(txom);
+        ble_hs_unlock();
+        return 0;
+    }
+
+    /* Note that in the response destination CID is form peer perspective and
+     * it is source CID from nimble perspective.
+     */
+    rsp->dcid = htole16(chan->scid);
+    rsp->scid = htole16(chan->dcid);
+
+    ble_hs_conn_delete_chan(conn, chan);
+    ble_hs_unlock();
+
+    ble_l2cap_sig_tx(conn_handle, txom);
+    return 0;
+}
+
+static void
+ble_l2cap_sig_coc_disconnect_cb(struct ble_l2cap_sig_proc *proc, int status)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_l2cap_event event;
+    struct ble_hs_conn *conn;
+
+    if (!proc) {
+        return;
+    }
+
+    memset(&event, 0, sizeof(event));
+    chan = proc->disconnect.chan;
+
+    if (!chan) {
+        return;
+    }
+
+    if (!chan->cb) {
+        goto done;
+    }
+
+done:
+    ble_hs_lock();
+    conn = ble_hs_conn_find_assert(chan->conn_handle);
+    if (conn) {
+        ble_hs_conn_delete_chan(conn, chan);
+    } else {
+        ble_l2cap_chan_free(NULL, chan);
+    }
+    ble_hs_unlock();
+}
+
+static int
+ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                           struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_disc_rsp *rsp;
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_chan *chan;
+    int rc;
+
+    proc = ble_l2cap_sig_proc_extract(conn_handle,
+                                      BLE_L2CAP_SIG_PROC_OP_DISCONNECT,
+                                      hdr->identifier);
+    if (!proc) {
+        return 0;
+    }
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(*rsp));
+    if (rc != 0) {
+        goto done;
+    }
+
+    chan = proc->disconnect.chan;
+    if (!chan) {
+        goto done;
+    }
+
+    rsp = (struct ble_l2cap_sig_disc_rsp *)(*om)->om_data;
+    if (chan->dcid != le16toh(rsp->dcid) || chan->scid != le16toh(rsp->scid)) {
+        /* This response is incorrect, lets wait for timeout */
+        ble_l2cap_sig_process_status(proc, 0);
+        return 0;
+    }
+
+    ble_l2cap_sig_coc_disconnect_cb(proc, rc);
+
+done:
+    ble_l2cap_sig_proc_free(proc);
+    return 0;
+}
+
+int
+ble_l2cap_sig_disconnect(struct ble_l2cap_chan *chan)
+{
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_disc_req *req;
+    struct ble_l2cap_sig_proc *proc;
+    int rc;
+
+    proc = ble_l2cap_sig_proc_alloc();
+    if (proc == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    proc->op = BLE_L2CAP_SIG_PROC_OP_DISCONNECT;
+    proc->id = ble_l2cap_sig_next_id();
+    proc->conn_handle = chan->conn_handle;
+    proc->disconnect.chan = chan;
+
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_DISCONN_REQ, proc->id,
+                                sizeof(*req), &txom);
+    if (!req) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    req->dcid = htole16(chan->dcid);
+    req->scid = htole16(chan->scid);
+
+    rc = ble_l2cap_sig_tx(proc->conn_handle, txom);
+
+done:
+    ble_l2cap_sig_process_status(proc, rc);
+
+    return rc;
+}
+
+static int
+ble_l2cap_sig_le_credits_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                            struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_le_credits *req;
+    int rc;
+
+    rc = ble_hs_mbuf_pullup_base(om, sizeof(*req));
+    if (rc != 0) {
+        return 0;
+    }
+
+    req = (struct ble_l2cap_sig_le_credits *) (*om)->om_data;
+
+    /* Ignore when peer sends zero credits */
+    if (req->credits == 0) {
+            return 0;
+    }
+
+    ble_l2cap_coc_le_credits_update(conn_handle, le16toh(req->scid),
+                                    le16toh(req->credits));
+
+    return 0;
+}
+
+int
+ble_l2cap_sig_le_credits(uint16_t conn_handle, uint16_t scid, uint16_t credits)
+{
+    struct ble_l2cap_sig_le_credits *cmd;
+    struct os_mbuf *txom;
+
+    cmd = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_FLOW_CTRL_CREDIT,
+                                ble_l2cap_sig_next_id(), sizeof(*cmd), &txom);
+
+    if (!cmd) {
+        return BLE_HS_ENOMEM;
+    }
+
+    cmd->scid = htole16(scid);
+    cmd->credits = htole16(credits);
+
+    return ble_l2cap_sig_tx(conn_handle, txom);
+}
+#endif
+
+static int
+ble_l2cap_sig_rx_reject(uint16_t conn_handle,
+                        struct ble_l2cap_sig_hdr *hdr,
+                        struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_proc *proc;
+    proc = ble_l2cap_sig_proc_extract(conn_handle,
+                                         BLE_L2CAP_SIG_PROC_OP_CONNECT,
+                                         hdr->identifier);
+   if (!proc) {
+       return 0;
+   }
+
+   switch (proc->id) {
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+       case BLE_L2CAP_SIG_PROC_OP_CONNECT:
+           ble_l2cap_sig_coc_connect_cb(proc, BLE_HS_EREJECT);
+           break;
+#endif
+       default:
+           break;
+   }
+
+   ble_l2cap_sig_proc_free(proc);
+   return 0;
+}
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+static int
+ble_l2cap_sig_rx(struct ble_l2cap_chan *chan)
+{
+    struct ble_l2cap_sig_hdr hdr;
+    ble_l2cap_sig_rx_fn *rx_cb;
+    uint16_t conn_handle;
+    struct os_mbuf **om;
+    int rc;
+
+    conn_handle = chan->conn_handle;
+    om = &chan->rx_buf;
+
+    STATS_INC(ble_l2cap_stats, sig_rx);
+
+#if !BLE_MONITOR
+    BLE_HS_LOG(DEBUG, "L2CAP - rxed signalling msg: ");
+    ble_hs_log_mbuf(*om);
+    BLE_HS_LOG(DEBUG, "\n");
+#endif
+
+    rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SIG_HDR_SZ);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_l2cap_sig_hdr_parse((*om)->om_data, (*om)->om_len, &hdr);
+
+    /* Strip L2CAP sig header from the front of the mbuf. */
+    os_mbuf_adj(*om, BLE_L2CAP_SIG_HDR_SZ);
+
+    if (OS_MBUF_PKTLEN(*om) != hdr.length) {
+        return BLE_HS_EBADDATA;
+    }
+
+    rx_cb = ble_l2cap_sig_dispatch_get(hdr.op);
+    if (rx_cb == NULL) {
+        rc = BLE_HS_EREJECT;
+    } else {
+        rc = rx_cb(conn_handle, &hdr, om);
+    }
+
+    if (rc) {
+        ble_l2cap_sig_reject_tx(conn_handle, hdr.identifier,
+                                        BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
+                                        NULL, 0);
+    }
+
+    return rc;
+}
+
+struct ble_l2cap_chan *
+ble_l2cap_sig_create_chan(uint16_t conn_handle)
+{
+    struct ble_l2cap_chan *chan;
+
+    chan = ble_l2cap_chan_alloc(conn_handle);
+    if (chan == NULL) {
+        return NULL;
+    }
+
+    chan->scid = BLE_L2CAP_CID_SIG;
+    chan->dcid = BLE_L2CAP_CID_SIG;
+    chan->my_mtu = BLE_L2CAP_SIG_MTU;
+    chan->rx_fn = ble_l2cap_sig_rx;
+
+    return chan;
+}
+
+/**
+ * @return                      The number of ticks until the next expiration
+ *                                  occurs.
+ */
+static int32_t
+ble_l2cap_sig_extract_expired(struct ble_l2cap_sig_proc_list *dst_list)
+{
+    struct ble_l2cap_sig_proc *proc;
+    struct ble_l2cap_sig_proc *prev;
+    struct ble_l2cap_sig_proc *next;
+    ble_npl_time_t now;
+    ble_npl_stime_t next_exp_in;
+    ble_npl_stime_t time_diff;
+
+    now = ble_npl_time_get();
+    STAILQ_INIT(dst_list);
+
+    /* Assume each event is either expired or has infinite duration. */
+    next_exp_in = BLE_HS_FOREVER;
+
+    ble_hs_lock();
+
+    prev = NULL;
+    proc = STAILQ_FIRST(&ble_l2cap_sig_procs);
+    while (proc != NULL) {
+        next = STAILQ_NEXT(proc, next);
+
+        time_diff = proc->exp_os_ticks - now;
+        if (time_diff <= 0) {
+            /* Procedure has expired; move it to the destination list. */
+            if (prev == NULL) {
+                STAILQ_REMOVE_HEAD(&ble_l2cap_sig_procs, next);
+            } else {
+                STAILQ_REMOVE_AFTER(&ble_l2cap_sig_procs, prev, next);
+            }
+            STAILQ_INSERT_TAIL(dst_list, proc, next);
+        } else {
+            if (time_diff < next_exp_in) {
+                next_exp_in = time_diff;
+            }
+        }
+
+        proc = next;
+    }
+
+    ble_hs_unlock();
+
+    return next_exp_in;
+}
+
+void
+ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason)
+{
+    struct ble_l2cap_sig_proc *proc;
+
+    /* Report a failure for each timed out procedure. */
+    while ((proc = STAILQ_FIRST(&ble_l2cap_sig_procs)) != NULL) {
+        switch(proc->op) {
+            case BLE_L2CAP_SIG_PROC_OP_UPDATE:
+                ble_l2cap_sig_update_call_cb(proc, reason);
+                break;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+            case BLE_L2CAP_SIG_PROC_OP_CONNECT:
+                ble_l2cap_sig_coc_connect_cb(proc, reason);
+            break;
+            case BLE_L2CAP_SIG_PROC_OP_DISCONNECT:
+                ble_l2cap_sig_coc_disconnect_cb(proc, reason);
+            break;
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+            case BLE_L2CAP_SIG_PROC_OP_RECONFIG:
+                ble_l2cap_sig_coc_reconfig_cb(proc, reason);
+            break;
+#endif
+#endif
+            }
+
+            STAILQ_REMOVE_HEAD(&ble_l2cap_sig_procs, next);
+            ble_l2cap_sig_proc_free(proc);
+    }
+
+}
+
+/**
+ * Terminates expired procedures.
+ *
+ * @return                      The number of ticks until this function should
+ *                                  be called again.
+ */
+int32_t
+ble_l2cap_sig_timer(void)
+{
+    struct ble_l2cap_sig_proc_list temp_list;
+    struct ble_l2cap_sig_proc *proc;
+    int32_t ticks_until_exp;
+
+    /* Remove timed-out procedures from the main list and insert them into a
+     * temporary list.  This function also calculates the number of ticks until
+     * the next expiration will occur.
+     */
+    ticks_until_exp = ble_l2cap_sig_extract_expired(&temp_list);
+
+    /* Report a failure for each timed out procedure. */
+    while ((proc = STAILQ_FIRST(&temp_list)) != NULL) {
+        STATS_INC(ble_l2cap_stats, proc_timeout);
+        switch(proc->op) {
+            case BLE_L2CAP_SIG_PROC_OP_UPDATE:
+                ble_l2cap_sig_update_call_cb(proc, BLE_HS_ETIMEOUT);
+                break;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+            case BLE_L2CAP_SIG_PROC_OP_CONNECT:
+                ble_l2cap_sig_coc_connect_cb(proc, BLE_HS_ETIMEOUT);
+            break;
+            case BLE_L2CAP_SIG_PROC_OP_DISCONNECT:
+                ble_l2cap_sig_coc_disconnect_cb(proc, BLE_HS_ETIMEOUT);
+            break;
+#endif
+        }
+
+        STAILQ_REMOVE_HEAD(&temp_list, next);
+        ble_l2cap_sig_proc_free(proc);
+    }
+
+    return ticks_until_exp;
+}
+
+int
+ble_l2cap_sig_init(void)
+{
+    int rc;
+
+    STAILQ_INIT(&ble_l2cap_sig_procs);
+
+    rc = os_mempool_init(&ble_l2cap_sig_proc_pool,
+                         MYNEWT_VAL(BLE_L2CAP_SIG_MAX_PROCS),
+                         sizeof (struct ble_l2cap_sig_proc),
+                         ble_l2cap_sig_proc_mem,
+                         "ble_l2cap_sig_proc_pool");
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig_cmd.c b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig_cmd.c
new file mode 100644
index 00000000..366dde62
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig_cmd.c
@@ -0,0 +1,112 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include "ble_hs_priv.h"
+
+int
+ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_lock();
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                    &conn, &chan);
+    rc = ble_l2cap_tx(conn, chan, txom);
+    ble_hs_unlock();
+
+    return rc;
+}
+
+void
+ble_l2cap_sig_hdr_parse(void *payload, uint16_t len,
+                        struct ble_l2cap_sig_hdr *dst)
+{
+    struct ble_l2cap_sig_hdr *src = payload;
+
+    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_HDR_SZ);
+
+    dst->op = src->op;
+    dst->identifier = src->identifier;
+    dst->length = le16toh(src->length);
+}
+
+int
+ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason,
+                        void *data, int data_len)
+{
+    struct ble_l2cap_sig_reject *cmd;
+    struct os_mbuf *txom;
+
+    cmd = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_REJECT, id,
+                           sizeof(*cmd) + data_len, &txom);
+    if (!cmd) {
+        return BLE_HS_ENOMEM;
+    }
+
+    cmd->reason = htole16(reason);
+    memcpy(cmd->data, data, data_len);
+
+    STATS_INC(ble_l2cap_stats, sig_rx);
+    return ble_l2cap_sig_tx(conn_handle, txom);
+}
+
+int
+ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id,
+                                    uint16_t src_cid, uint16_t dst_cid)
+{
+    struct {
+        uint16_t local_cid;
+        uint16_t remote_cid;
+    } data = {
+        .local_cid = dst_cid,
+        .remote_cid = src_cid,
+    };
+
+    return ble_l2cap_sig_reject_tx(conn_handle, id,
+                                 BLE_L2CAP_SIG_ERR_INVALID_CID,
+                                 &data, sizeof data);
+}
+
+void *
+ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len,
+                      struct os_mbuf **txom)
+{
+    struct ble_l2cap_sig_hdr *hdr;
+
+    *txom = ble_hs_mbuf_l2cap_pkt();
+    if (*txom == NULL) {
+        return NULL;
+    }
+
+    if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) {
+        os_mbuf_free_chain(*txom);
+        return NULL;
+    }
+
+    hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data;
+
+    hdr->op = opcode;
+    hdr->identifier = id;
+    hdr->length = htole16(len);
+
+    return hdr->data;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig_priv.h
new file mode 100644
index 00000000..a698cd0d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_l2cap_sig_priv.h
@@ -0,0 +1,184 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_L2CAP_SIG_
+#define H_BLE_L2CAP_SIG_
+
+#include "syscfg/syscfg.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_L2CAP_SIG_MTU           100  /* This is our own default. */
+
+#define BLE_L2CAP_SIG_HDR_SZ                4
+struct ble_l2cap_sig_hdr {
+    uint8_t op;
+    uint8_t identifier;
+    uint16_t length;
+    uint8_t data[0];
+} __attribute__((packed));
+
+#define BLE_L2CAP_SIG_REJECT_MIN_SZ         2
+struct ble_l2cap_sig_reject {
+    uint16_t reason;
+    uint8_t data[0];
+} __attribute__((packed));
+
+#define BLE_L2CAP_SIG_UPDATE_REQ_SZ         8
+struct ble_l2cap_sig_update_req {
+    uint16_t itvl_min;
+    uint16_t itvl_max;
+    uint16_t slave_latency;
+    uint16_t timeout_multiplier;
+} __attribute__((packed));
+
+#define BLE_L2CAP_SIG_UPDATE_RSP_SZ         2
+struct ble_l2cap_sig_update_rsp {
+    uint16_t result;
+} __attribute__((packed));
+
+#define BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT  0x0000
+#define BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT  0x0001
+
+struct ble_l2cap_sig_le_con_req {
+    uint16_t psm;
+    uint16_t scid;
+    uint16_t mtu;
+    uint16_t mps;
+    uint16_t credits;
+} __attribute__((packed));
+
+struct ble_l2cap_sig_le_con_rsp {
+    uint16_t dcid;
+    uint16_t mtu;
+    uint16_t mps;
+    uint16_t credits;
+    uint16_t result;
+} __attribute__((packed));
+
+struct ble_l2cap_sig_credit_base_connect_req {
+    uint16_t psm;
+    uint16_t mtu;
+    uint16_t mps;
+    uint16_t credits;
+    uint16_t scids[0];
+} __attribute__((packed));
+
+struct ble_l2cap_sig_credit_base_connect_rsp {
+    uint16_t mtu;
+    uint16_t mps;
+    uint16_t credits;
+    uint16_t result;
+    uint16_t dcids[0];
+} __attribute__((packed));
+
+struct ble_l2cap_sig_credit_base_reconfig_req {
+    uint16_t mtu;
+    uint16_t mps;
+    uint16_t dcids[0];
+} __attribute__((packed));
+
+struct ble_l2cap_sig_credit_base_reconfig_rsp {
+    uint16_t result;
+} __attribute__((packed));
+
+struct ble_l2cap_sig_disc_req {
+    uint16_t dcid;
+    uint16_t scid;
+} __attribute__((packed));
+
+struct ble_l2cap_sig_disc_rsp {
+    uint16_t dcid;
+    uint16_t scid;
+} __attribute__((packed));
+
+struct ble_l2cap_sig_le_credits {
+    uint16_t scid;
+    uint16_t credits;
+} __attribute__((packed));
+
+void ble_l2cap_sig_hdr_parse(void *payload, uint16_t len,
+                             struct ble_l2cap_sig_hdr *hdr);
+int ble_l2cap_sig_reject_tx(uint16_t conn_handle,
+                            uint8_t id, uint16_t reason,
+                            void *data, int data_len);
+int ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id,
+                                        uint16_t src_cid, uint16_t dst_cid);
+int ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom);
+void *ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len,
+                            struct os_mbuf **txom);
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+int ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
+                              struct os_mbuf *sdu_rx,
+                              ble_l2cap_event_fn *cb, void *cb_arg);
+int ble_l2cap_sig_disconnect(struct ble_l2cap_chan *chan);
+int ble_l2cap_sig_le_credits(uint16_t conn_handle, uint16_t scid,
+                             uint16_t credits);
+#else
+static inline int
+ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
+                          struct os_mbuf *sdu_rx,
+                          ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    return BLE_HS_ENOTSUP;
+}
+
+static inline int
+ble_l2cap_sig_disconnect(struct ble_l2cap_chan *chan)
+{
+    return BLE_HS_ENOTSUP;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+int ble_l2cap_sig_ecoc_connect(uint16_t conn_handle,
+                                       uint16_t psm, uint16_t mtu,
+                                       uint8_t num, struct os_mbuf *sdu_rx[],
+                                       ble_l2cap_event_fn *cb, void *cb_arg);
+int ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
+                               uint8_t num, uint16_t new_mtu);
+#else
+static inline int
+ble_l2cap_sig_ecoc_connect(uint16_t conn_handle,
+                           uint16_t psm, uint16_t mtu,
+                           uint8_t num, struct os_mbuf *sdu_rx[],
+                           ble_l2cap_event_fn *cb, void *cb_arg)
+{
+    return BLE_HS_ENOTSUP;
+}
+static inline int
+ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
+                           uint8_t num, uint16_t new_mtu)
+{
+    return BLE_HS_ENOTSUP;
+}
+#endif
+
+void ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason);
+int32_t ble_l2cap_sig_timer(void);
+struct ble_l2cap_chan *ble_l2cap_sig_create_chan(uint16_t conn_handle);
+int ble_l2cap_sig_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_monitor.c b/src/libs/mynewt-nimble/nimble/host/src/ble_monitor.c
new file mode 100644
index 00000000..e6db48b8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_monitor.c
@@ -0,0 +1,473 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "host/ble_monitor.h"
+
+#if BLE_MONITOR
+
+#if MYNEWT_VAL(BLE_MONITOR_UART) && MYNEWT_VAL(BLE_MONITOR_RTT)
+#error "Cannot enable monitor over UART and RTT at the same time!"
+#endif
+
+#include <stdarg.h>
+#include <stdio.h>
+#include <inttypes.h>
+#include "os/os.h"
+#include "log/log.h"
+#if MYNEWT_VAL(BLE_MONITOR_UART)
+#include "uart/uart.h"
+#endif
+#if MYNEWT_VAL(BLE_MONITOR_RTT)
+#include "rtt/SEGGER_RTT.h"
+#endif
+#include "ble_hs_priv.h"
+#include "ble_monitor_priv.h"
+
+struct ble_npl_mutex lock;
+
+#if MYNEWT_VAL(BLE_MONITOR_UART)
+struct uart_dev *uart;
+
+static uint8_t tx_ringbuf[MYNEWT_VAL(BLE_MONITOR_UART_BUFFER_SIZE)];
+static uint8_t tx_ringbuf_head;
+static uint8_t tx_ringbuf_tail;
+#endif
+
+#if MYNEWT_VAL(BLE_MONITOR_RTT)
+static uint8_t rtt_buf[MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_SIZE)];
+static int rtt_index;
+#if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+static uint8_t rtt_pktbuf[MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_SIZE)];
+static size_t rtt_pktbuf_pos;
+static struct {
+    bool dropped;
+    struct ble_npl_callout tmo;
+    struct ble_monitor_drops_hdr drops_hdr;
+} rtt_drops;
+
+#endif
+#endif
+
+#if MYNEWT_VAL(BLE_MONITOR_UART)
+static inline int
+inc_and_wrap(int i, int max)
+{
+    return (i + 1) & (max - 1);
+}
+
+static int
+monitor_uart_rx_discard(void *arg, uint8_t ch)
+{
+    return 0;
+}
+
+static int
+monitor_uart_tx_char(void *arg)
+{
+    uint8_t ch;
+
+    /* No more data */
+    if (tx_ringbuf_head == tx_ringbuf_tail) {
+        return -1;
+    }
+
+    ch = tx_ringbuf[tx_ringbuf_tail];
+    tx_ringbuf_tail = inc_and_wrap(tx_ringbuf_tail, sizeof(tx_ringbuf));
+
+    return ch;
+}
+
+static void
+monitor_uart_queue_char(uint8_t ch)
+{
+    int sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    /* We need to try flush some data from ringbuffer if full */
+    while (inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf)) ==
+            tx_ringbuf_tail) {
+        uart_start_tx(uart);
+        OS_EXIT_CRITICAL(sr);
+        if (os_started()) {
+            os_time_delay(1);
+        }
+        OS_ENTER_CRITICAL(sr);
+    }
+
+    tx_ringbuf[tx_ringbuf_head] = ch;
+    tx_ringbuf_head = inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf));
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+static void
+monitor_write(const void *buf, size_t len)
+{
+    const uint8_t *ch = buf;
+
+    while (len--) {
+        monitor_uart_queue_char(*ch++);
+    }
+
+    uart_start_tx(uart);
+}
+#endif
+
+#if MYNEWT_VAL(BLE_MONITOR_RTT)
+
+#if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+static void
+update_drop_counters(struct ble_monitor_hdr *failed_hdr)
+{
+    uint8_t *cnt;
+
+    rtt_drops.dropped = true;
+
+    switch (failed_hdr->opcode) {
+    case BLE_MONITOR_OPCODE_COMMAND_PKT:
+        cnt = &rtt_drops.drops_hdr.cmd;
+        break;
+    case BLE_MONITOR_OPCODE_EVENT_PKT:
+        cnt = &rtt_drops.drops_hdr.evt;
+        break;
+    case BLE_MONITOR_OPCODE_ACL_TX_PKT:
+        cnt = &rtt_drops.drops_hdr.acl_tx;
+        break;
+    case BLE_MONITOR_OPCODE_ACL_RX_PKT:
+        cnt = &rtt_drops.drops_hdr.acl_rx;
+        break;
+    default:
+        cnt = &rtt_drops.drops_hdr.other;
+        break;
+    }
+
+    if (*cnt < UINT8_MAX) {
+        (*cnt)++;
+        ble_npl_callout_reset(&rtt_drops.tmo, OS_TICKS_PER_SEC);
+    }
+}
+
+static void
+reset_drop_counters(void)
+{
+    rtt_drops.dropped = false;
+    rtt_drops.drops_hdr.cmd = 0;
+    rtt_drops.drops_hdr.evt = 0;
+    rtt_drops.drops_hdr.acl_tx = 0;
+    rtt_drops.drops_hdr.acl_rx = 0;
+    rtt_drops.drops_hdr.other = 0;
+
+    ble_npl_callout_stop(&rtt_drops.tmo);
+}
+#endif
+
+static void
+monitor_write(const void *buf, size_t len)
+{
+#if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+    struct ble_monitor_hdr *hdr = (struct ble_monitor_hdr *) rtt_pktbuf;
+    bool discard;
+    unsigned ret = 0;
+
+    /* We will discard any packet which exceeds length of intermediate buffer */
+    discard = rtt_pktbuf_pos + len > sizeof(rtt_pktbuf);
+
+    if (!discard) {
+        memcpy(rtt_pktbuf + rtt_pktbuf_pos, buf, len);
+    }
+
+    rtt_pktbuf_pos += len;
+    if (rtt_pktbuf_pos < sizeof(hdr->data_len) + hdr->data_len) {
+        return;
+    }
+
+    if (!discard) {
+        ret = SEGGER_RTT_WriteNoLock(rtt_index, rtt_pktbuf, rtt_pktbuf_pos);
+    }
+
+    if (ret > 0) {
+        reset_drop_counters();
+    } else {
+        update_drop_counters(hdr);
+    }
+
+    rtt_pktbuf_pos = 0;
+#else
+    SEGGER_RTT_WriteNoLock(rtt_index, buf, len);
+#endif
+}
+#endif
+
+static void
+monitor_write_header(uint16_t opcode, uint16_t len)
+{
+    struct ble_monitor_hdr hdr;
+    struct ble_monitor_ts_hdr ts_hdr;
+    uint8_t hdr_len;
+    int64_t ts;
+
+    hdr_len = sizeof(ts_hdr);
+#if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+    if (rtt_drops.dropped) {
+        hdr_len += sizeof(rtt_drops.drops_hdr);
+    }
+#endif
+
+    hdr.data_len = htole16(4 + hdr_len + len);
+    hdr.hdr_len  = hdr_len;
+    hdr.opcode   = htole16(opcode);
+    hdr.flags    = 0;
+
+    /* Use uptime for timestamp */
+    ts = os_get_uptime_usec();
+
+    /*
+     * btsnoop specification states that fields of extended header must be
+     * sorted in increasing order so we will send drops (if any) headers before
+     * timestamp header.
+     */
+
+    monitor_write(&hdr, sizeof(hdr));
+
+#if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+    if (rtt_drops.dropped) {
+        monitor_write(&rtt_drops.drops_hdr, sizeof(rtt_drops.drops_hdr));
+    }
+#endif
+
+    ts_hdr.type = BLE_MONITOR_EXTHDR_TS32;
+    ts_hdr.ts32 = htole32(ts / 100);
+
+    monitor_write(&ts_hdr, sizeof(ts_hdr));
+}
+
+static size_t
+btmon_write(FILE *instance, const char *bp, size_t n)
+{
+    monitor_write(bp, n);
+
+    return n;
+}
+
+static FILE *btmon = (FILE *) &(struct File) {
+    .vmt = &(struct File_methods) {
+        .write = btmon_write,
+    },
+};
+
+#if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+static void
+drops_tmp_cb(struct ble_npl_event *ev)
+{
+    ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
+
+    /*
+     * There's no "nop" in btsnoop protocol so we just send empty system note
+     * to indicate drops.
+     */
+
+    monitor_write_header(BLE_MONITOR_OPCODE_SYSTEM_NOTE, 1);
+    monitor_write("", 1);
+
+    ble_npl_mutex_release(&lock);
+}
+#endif
+
+int
+ble_monitor_init(void)
+{
+#if MYNEWT_VAL(BLE_MONITOR_UART)
+    struct uart_conf uc = {
+        .uc_speed = MYNEWT_VAL(BLE_MONITOR_UART_BAUDRATE),
+        .uc_databits = 8,
+        .uc_stopbits = 1,
+        .uc_parity = UART_PARITY_NONE,
+        .uc_flow_ctl = UART_FLOW_CTL_NONE,
+        .uc_tx_char = monitor_uart_tx_char,
+        .uc_rx_char = monitor_uart_rx_discard,
+        .uc_cb_arg = NULL,
+    };
+
+    uart = (struct uart_dev *)os_dev_open(MYNEWT_VAL(BLE_MONITOR_UART_DEV),
+                                          OS_TIMEOUT_NEVER, &uc);
+    if (!uart) {
+        return -1;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_MONITOR_RTT)
+#if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+    ble_npl_callout_init(&rtt_drops.tmo, ble_hs_evq_get(), drops_tmp_cb, NULL);
+
+    /* Initialize types in header (we won't touch them later) */
+    rtt_drops.drops_hdr.type_cmd = BLE_MONITOR_EXTHDR_COMMAND_DROPS;
+    rtt_drops.drops_hdr.type_evt = BLE_MONITOR_EXTHDR_EVENT_DROPS;
+    rtt_drops.drops_hdr.type_acl_tx = BLE_MONITOR_EXTHDR_ACL_TX_DROPS;
+    rtt_drops.drops_hdr.type_acl_rx = BLE_MONITOR_EXTHDR_ACL_RX_DROPS;
+    rtt_drops.drops_hdr.type_other = BLE_MONITOR_EXTHDR_OTHER_DROPS;
+
+    rtt_index = SEGGER_RTT_AllocUpBuffer(MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_NAME),
+                                         rtt_buf, sizeof(rtt_buf),
+                                         SEGGER_RTT_MODE_NO_BLOCK_SKIP);
+#else
+    rtt_index = SEGGER_RTT_AllocUpBuffer(MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_NAME),
+                                         rtt_buf, sizeof(rtt_buf),
+                                         SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL);
+#endif
+
+    if (rtt_index < 0) {
+        return -1;
+    }
+#endif
+
+    ble_npl_mutex_init(&lock);
+
+    return 0;
+}
+
+int
+ble_monitor_send(uint16_t opcode, const void *data, size_t len)
+{
+    ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
+
+    monitor_write_header(opcode, len);
+    monitor_write(data, len);
+
+    ble_npl_mutex_release(&lock);
+
+    return 0;
+}
+
+int
+ble_monitor_send_om(uint16_t opcode, const struct os_mbuf *om)
+{
+    const struct os_mbuf *om_tmp;
+    uint16_t length = 0;
+
+    om_tmp = om;
+    while (om_tmp) {
+        length += om_tmp->om_len;
+        om_tmp = SLIST_NEXT(om_tmp, om_next);
+    }
+
+    ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
+
+    monitor_write_header(opcode, length);
+
+    while (om) {
+        monitor_write(om->om_data, om->om_len);
+        om = SLIST_NEXT(om, om_next);
+    }
+
+    ble_npl_mutex_release(&lock);
+
+    return 0;
+}
+
+int
+ble_monitor_new_index(uint8_t bus, uint8_t *addr, const char *name)
+{
+    struct ble_monitor_new_index pkt;
+
+    pkt.type = 0; /* Primary controller, we don't support other */
+    pkt.bus = bus;
+    memcpy(pkt.bdaddr, addr, 6);
+    strncpy(pkt.name, name, sizeof(pkt.name) - 1);
+    pkt.name[sizeof(pkt.name) - 1] = '\0';
+
+    ble_monitor_send(BLE_MONITOR_OPCODE_NEW_INDEX, &pkt, sizeof(pkt));
+
+    return 0;
+}
+
+int
+ble_monitor_log(int level, const char *fmt, ...)
+{
+    static const char id[] = "nimble";
+    struct ble_monitor_user_logging ulog;
+    va_list va;
+    int len;
+
+    va_start(va, fmt);
+    len = vsnprintf(NULL, 0, fmt, va);
+    va_end(va);
+
+    switch (level) {
+    case LOG_LEVEL_ERROR:
+        ulog.priority = 3;
+        break;
+    case LOG_LEVEL_WARN:
+        ulog.priority = 4;
+        break;
+    case LOG_LEVEL_INFO:
+        ulog.priority = 6;
+        break;
+    case LOG_LEVEL_DEBUG:
+        ulog.priority = 7;
+        break;
+    default:
+        ulog.priority = 8;
+        break;
+    }
+
+    ulog.ident_len = sizeof(id);
+
+    ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
+
+    monitor_write_header(BLE_MONITOR_OPCODE_USER_LOGGING,
+                         sizeof(ulog) + sizeof(id) + len + 1);
+    monitor_write(&ulog, sizeof(ulog));
+    monitor_write(id, sizeof(id));
+
+    va_start(va, fmt);
+    vfprintf(btmon, fmt, va);
+    va_end(va);
+
+    /* null-terminate string */
+    monitor_write("", 1);
+
+    ble_npl_mutex_release(&lock);
+
+    return 0;
+}
+
+int
+ble_monitor_out(int c)
+{
+    static char buf[MYNEWT_VAL(BLE_MONITOR_CONSOLE_BUFFER_SIZE)];
+    static size_t len;
+
+    if (c != '\n') {
+        buf[len++] = c;
+
+        if (len < sizeof(buf) - 1) {
+            return c;
+        }
+    }
+
+    buf[len++] = '\0';
+
+    ble_monitor_send(BLE_MONITOR_OPCODE_SYSTEM_NOTE, buf, len);
+    len = 0;
+
+    return c;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_monitor_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_monitor_priv.h
new file mode 100644
index 00000000..93578704
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_monitor_priv.h
@@ -0,0 +1,100 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_MONITOR_PRIV_
+#define H_BLE_MONITOR_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_MONITOR_OPCODE_NEW_INDEX    0
+#define BLE_MONITOR_OPCODE_DEL_INDEX    1
+#define BLE_MONITOR_OPCODE_COMMAND_PKT  2
+#define BLE_MONITOR_OPCODE_EVENT_PKT    3
+#define BLE_MONITOR_OPCODE_ACL_TX_PKT   4
+#define BLE_MONITOR_OPCODE_ACL_RX_PKT   5
+#define BLE_MONITOR_OPCODE_SCO_TX_PKT   6
+#define BLE_MONITOR_OPCODE_SCO_RX_PKT   7
+#define BLE_MONITOR_OPCODE_OPEN_INDEX   8
+#define BLE_MONITOR_OPCODE_CLOSE_INDEX  9
+#define BLE_MONITOR_OPCODE_INDEX_INFO   10
+#define BLE_MONITOR_OPCODE_VENDOR_DIAG  11
+#define BLE_MONITOR_OPCODE_SYSTEM_NOTE  12
+#define BLE_MONITOR_OPCODE_USER_LOGGING 13
+
+#define BLE_MONITOR_EXTHDR_COMMAND_DROPS    1
+#define BLE_MONITOR_EXTHDR_EVENT_DROPS      2
+#define BLE_MONITOR_EXTHDR_ACL_RX_DROPS     3
+#define BLE_MONITOR_EXTHDR_ACL_TX_DROPS     4
+#define BLE_MONITOR_EXTHDR_SCO_RX_DROPS     5
+#define BLE_MONITOR_EXTHDR_SCO_TX_DROPS     6
+#define BLE_MONITOR_EXTHDR_OTHER_DROPS      7
+#define BLE_MONITOR_EXTHDR_TS32             8
+
+struct ble_monitor_hdr {
+    uint16_t  data_len;
+    uint16_t  opcode;
+    uint8_t   flags;
+    uint8_t   hdr_len;
+} __attribute__((packed));
+
+struct ble_monitor_drops_hdr {
+    uint8_t type_cmd;
+    uint8_t cmd;
+    uint8_t type_evt;
+    uint8_t evt;
+    uint8_t type_acl_tx;
+    uint8_t acl_tx;
+    uint8_t type_acl_rx;
+    uint8_t acl_rx;
+    uint8_t type_other;
+    uint8_t other;
+} __attribute__((packed));
+
+struct ble_monitor_ts_hdr {
+    uint8_t   type;
+    uint32_t  ts32;
+} __attribute__((packed));
+
+struct ble_monitor_new_index {
+    uint8_t  type;
+    uint8_t  bus;
+    uint8_t  bdaddr[6];
+    char     name[8];
+} __attribute__((packed));
+
+struct ble_monitor_user_logging {
+    uint8_t  priority;
+    uint8_t  ident_len;
+} __attribute__((packed));
+
+int ble_monitor_init(void);
+
+int ble_monitor_send(uint16_t opcode, const void *data, size_t len);
+
+int ble_monitor_send_om(uint16_t opcode, const struct os_mbuf *om);
+
+int ble_monitor_new_index(uint8_t bus, uint8_t *addr, const char *name);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_sm.c b/src/libs/mynewt-nimble/nimble/host/src/ble_sm.c
new file mode 100644
index 00000000..cfd80fcb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_sm.c
@@ -0,0 +1,2813 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/**
+ * L2CAP Security Manager (channel ID = 6).
+ *
+ * Design overview:
+ *
+ * L2CAP sm procedures are initiated by the application via function calls.
+ * Such functions return when either of the following happens:
+ *
+ * (1) The procedure completes (success or failure).
+ * (2) The procedure cannot proceed until a BLE peer responds.
+ *
+ * For (1), the result of the procedure if fully indicated by the function
+ * return code.
+ * For (2), the procedure result is indicated by an application-configured
+ * callback.  The callback is executed when the procedure completes.
+ *
+ * Notes on thread-safety:
+ * 1. The ble_hs mutex must never be locked when an application callback is
+ *    executed.  A callback is free to initiate additional host procedures.
+ * 2. Keep the host mutex locked whenever:
+ *      o A proc entry is read from or written to.
+ *      o The proc list is read or modified.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_priv.h"
+
+#if NIMBLE_BLE_SM
+
+/** Procedure timeout; 30 seconds. */
+#define BLE_SM_TIMEOUT_MS             (30000)
+
+STAILQ_HEAD(ble_sm_proc_list, ble_sm_proc);
+
+typedef void ble_sm_rx_fn(uint16_t conn_handle, struct os_mbuf **om,
+                          struct ble_sm_result *res);
+
+static ble_sm_rx_fn ble_sm_rx_noop;
+static ble_sm_rx_fn ble_sm_pair_req_rx;
+static ble_sm_rx_fn ble_sm_pair_rsp_rx;
+static ble_sm_rx_fn ble_sm_confirm_rx;
+static ble_sm_rx_fn ble_sm_random_rx;
+static ble_sm_rx_fn ble_sm_fail_rx;
+static ble_sm_rx_fn ble_sm_enc_info_rx;
+static ble_sm_rx_fn ble_sm_master_id_rx;
+static ble_sm_rx_fn ble_sm_id_info_rx;
+static ble_sm_rx_fn ble_sm_id_addr_info_rx;
+static ble_sm_rx_fn ble_sm_sign_info_rx;
+static ble_sm_rx_fn ble_sm_sec_req_rx;
+
+static ble_sm_rx_fn * const ble_sm_dispatch[] = {
+   [BLE_SM_OP_PAIR_REQ] = ble_sm_pair_req_rx,
+   [BLE_SM_OP_PAIR_RSP] = ble_sm_pair_rsp_rx,
+   [BLE_SM_OP_PAIR_CONFIRM] = ble_sm_confirm_rx,
+   [BLE_SM_OP_PAIR_RANDOM] = ble_sm_random_rx,
+   [BLE_SM_OP_PAIR_FAIL] = ble_sm_fail_rx,
+   [BLE_SM_OP_ENC_INFO] = ble_sm_enc_info_rx,
+   [BLE_SM_OP_MASTER_ID] = ble_sm_master_id_rx,
+   [BLE_SM_OP_IDENTITY_INFO] = ble_sm_id_info_rx,
+   [BLE_SM_OP_IDENTITY_ADDR_INFO] = ble_sm_id_addr_info_rx,
+   [BLE_SM_OP_SIGN_INFO] = ble_sm_sign_info_rx,
+   [BLE_SM_OP_SEC_REQ] = ble_sm_sec_req_rx,
+   [BLE_SM_OP_PAIR_KEYPRESS_NOTIFY] = ble_sm_rx_noop,
+#if MYNEWT_VAL(BLE_SM_SC)
+   [BLE_SM_OP_PAIR_PUBLIC_KEY] = ble_sm_sc_public_key_rx,
+   [BLE_SM_OP_PAIR_DHKEY_CHECK] = ble_sm_sc_dhkey_check_rx,
+#else
+   [BLE_SM_OP_PAIR_PUBLIC_KEY] = ble_sm_rx_noop,
+   [BLE_SM_OP_PAIR_DHKEY_CHECK] = ble_sm_rx_noop,
+#endif
+};
+
+struct hci_start_encrypt
+{
+    uint16_t connection_handle;
+    uint16_t encrypted_diversifier;
+    uint64_t random_number;
+    uint8_t long_term_key[16];
+};
+
+typedef void ble_sm_state_fn(struct ble_sm_proc *proc,
+                             struct ble_sm_result *res, void *arg);
+
+static ble_sm_state_fn ble_sm_pair_exec;
+static ble_sm_state_fn ble_sm_confirm_exec;
+static ble_sm_state_fn ble_sm_random_exec;
+static ble_sm_state_fn ble_sm_ltk_start_exec;
+static ble_sm_state_fn ble_sm_ltk_restore_exec;
+static ble_sm_state_fn ble_sm_enc_start_exec;
+static ble_sm_state_fn ble_sm_enc_restore_exec;
+static ble_sm_state_fn ble_sm_key_exch_exec;
+static ble_sm_state_fn ble_sm_sec_req_exec;
+
+static ble_sm_state_fn * const
+ble_sm_state_dispatch[BLE_SM_PROC_STATE_CNT] = {
+    [BLE_SM_PROC_STATE_PAIR]          = ble_sm_pair_exec,
+    [BLE_SM_PROC_STATE_CONFIRM]       = ble_sm_confirm_exec,
+    [BLE_SM_PROC_STATE_RANDOM]        = ble_sm_random_exec,
+    [BLE_SM_PROC_STATE_LTK_START]     = ble_sm_ltk_start_exec,
+    [BLE_SM_PROC_STATE_LTK_RESTORE]   = ble_sm_ltk_restore_exec,
+    [BLE_SM_PROC_STATE_ENC_START]     = ble_sm_enc_start_exec,
+    [BLE_SM_PROC_STATE_ENC_RESTORE]   = ble_sm_enc_restore_exec,
+    [BLE_SM_PROC_STATE_KEY_EXCH]      = ble_sm_key_exch_exec,
+    [BLE_SM_PROC_STATE_SEC_REQ]       = ble_sm_sec_req_exec,
+#if MYNEWT_VAL(BLE_SM_SC)
+    [BLE_SM_PROC_STATE_PUBLIC_KEY]    = ble_sm_sc_public_key_exec,
+    [BLE_SM_PROC_STATE_DHKEY_CHECK]   = ble_sm_sc_dhkey_check_exec,
+#else
+    [BLE_SM_PROC_STATE_PUBLIC_KEY]    = NULL,
+    [BLE_SM_PROC_STATE_DHKEY_CHECK]   = NULL,
+#endif
+};
+
+static os_membuf_t ble_sm_proc_mem[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_SM_MAX_PROCS),
+                    sizeof (struct ble_sm_proc))
+];
+
+static struct os_mempool ble_sm_proc_pool;
+
+/* Maintains the list of active security manager procedures. */
+static struct ble_sm_proc_list ble_sm_procs;
+
+static void ble_sm_pair_cfg(struct ble_sm_proc *proc);
+
+
+/*****************************************************************************
+ * $debug                                                                    *
+ *****************************************************************************/
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+
+static uint8_t ble_sm_dbg_next_pair_rand[16];
+static uint8_t ble_sm_dbg_next_pair_rand_set;
+static uint16_t ble_sm_dbg_next_ediv;
+static uint8_t ble_sm_dbg_next_ediv_set;
+static uint64_t ble_sm_dbg_next_master_id_rand;
+static uint8_t ble_sm_dbg_next_master_id_rand_set;
+static uint8_t ble_sm_dbg_next_ltk[16];
+static uint8_t ble_sm_dbg_next_ltk_set;
+static uint8_t ble_sm_dbg_next_csrk[16];
+static uint8_t ble_sm_dbg_next_csrk_set;
+
+void
+ble_sm_dbg_set_next_pair_rand(uint8_t *next_pair_rand)
+{
+    memcpy(ble_sm_dbg_next_pair_rand, next_pair_rand,
+           sizeof ble_sm_dbg_next_pair_rand);
+    ble_sm_dbg_next_pair_rand_set = 1;
+}
+
+void
+ble_sm_dbg_set_next_ediv(uint16_t next_ediv)
+{
+    ble_sm_dbg_next_ediv = next_ediv;
+    ble_sm_dbg_next_ediv_set = 1;
+}
+
+void
+ble_sm_dbg_set_next_master_id_rand(uint64_t next_master_id_rand)
+{
+    ble_sm_dbg_next_master_id_rand = next_master_id_rand;
+    ble_sm_dbg_next_master_id_rand_set = 1;
+}
+
+void
+ble_sm_dbg_set_next_ltk(uint8_t *next_ltk)
+{
+    memcpy(ble_sm_dbg_next_ltk, next_ltk,
+           sizeof ble_sm_dbg_next_ltk);
+    ble_sm_dbg_next_ltk_set = 1;
+}
+
+void
+ble_sm_dbg_set_next_csrk(uint8_t *next_csrk)
+{
+    memcpy(ble_sm_dbg_next_csrk, next_csrk,
+           sizeof ble_sm_dbg_next_csrk);
+    ble_sm_dbg_next_csrk_set = 1;
+}
+
+#endif
+
+static void
+ble_sm_dbg_assert_no_cycles(void)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    ble_sm_num_procs();
+#endif
+}
+
+static void
+ble_sm_dbg_assert_not_inserted(struct ble_sm_proc *proc)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    struct ble_sm_proc *cur;
+
+    STAILQ_FOREACH(cur, &ble_sm_procs, next) {
+        BLE_HS_DBG_ASSERT(cur != proc);
+    }
+#endif
+}
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+/**
+ * Calculates the number of active SM procedures.
+ */
+int
+ble_sm_num_procs(void)
+{
+    struct ble_sm_proc *proc;
+    int cnt;
+
+    cnt = 0;
+    STAILQ_FOREACH(proc, &ble_sm_procs, next) {
+        BLE_HS_DBG_ASSERT(cnt < MYNEWT_VAL(BLE_SM_MAX_PROCS));
+        cnt++;
+    }
+
+    return cnt;
+}
+
+int
+ble_sm_gen_pair_rand(uint8_t *pair_rand)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (ble_sm_dbg_next_pair_rand_set) {
+        ble_sm_dbg_next_pair_rand_set = 0;
+        memcpy(pair_rand, ble_sm_dbg_next_pair_rand,
+               sizeof ble_sm_dbg_next_pair_rand);
+        return 0;
+    }
+#endif
+
+    rc = ble_hs_hci_util_rand(pair_rand, 16);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_gen_ediv(struct ble_sm_master_id *master_id)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (ble_sm_dbg_next_ediv_set) {
+        ble_sm_dbg_next_ediv_set = 0;
+        master_id->ediv = ble_sm_dbg_next_ediv;
+        return 0;
+    }
+#endif
+
+    rc = ble_hs_hci_util_rand(&master_id->ediv, sizeof master_id->ediv);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_gen_master_id_rand(struct ble_sm_master_id *master_id)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (ble_sm_dbg_next_master_id_rand_set) {
+        ble_sm_dbg_next_master_id_rand_set = 0;
+        master_id->rand_val = ble_sm_dbg_next_master_id_rand;
+        return 0;
+    }
+#endif
+
+    rc = ble_hs_hci_util_rand(&master_id->rand_val, sizeof master_id->rand_val);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_gen_ltk(struct ble_sm_proc *proc, uint8_t *ltk)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (ble_sm_dbg_next_ltk_set) {
+        ble_sm_dbg_next_ltk_set = 0;
+        memcpy(ltk, ble_sm_dbg_next_ltk,
+               sizeof ble_sm_dbg_next_ltk);
+        return 0;
+    }
+#endif
+
+    rc = ble_hs_hci_util_rand(ltk, proc->key_size);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Ensure proper key size */
+    memset(ltk + proc->key_size, 0, sizeof proc->ltk - proc->key_size);
+
+    return 0;
+}
+
+static int
+ble_sm_gen_csrk(struct ble_sm_proc *proc, uint8_t *csrk)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (ble_sm_dbg_next_csrk_set) {
+        ble_sm_dbg_next_csrk_set = 0;
+        memcpy(csrk, ble_sm_dbg_next_csrk,
+               sizeof ble_sm_dbg_next_csrk);
+        return 0;
+    }
+#endif
+
+    rc = ble_hs_hci_util_rand(csrk, 16);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static void
+ble_sm_proc_set_timer(struct ble_sm_proc *proc)
+{
+    proc->exp_os_ticks = ble_npl_time_get() +
+                         ble_npl_time_ms_to_ticks32(BLE_SM_TIMEOUT_MS);
+    ble_hs_timer_resched();
+}
+
+static ble_sm_rx_fn *
+ble_sm_dispatch_get(uint8_t op)
+{
+    if (op >= sizeof ble_sm_dispatch / sizeof ble_sm_dispatch[0]) {
+        return NULL;
+    }
+
+    return ble_sm_dispatch[op];
+}
+
+/**
+ * Allocates a proc entry.
+ *
+ * @return                      An entry on success; null on failure.
+ */
+static struct ble_sm_proc *
+ble_sm_proc_alloc(void)
+{
+    struct ble_sm_proc *proc;
+
+    proc = os_memblock_get(&ble_sm_proc_pool);
+    if (proc != NULL) {
+        memset(proc, 0, sizeof *proc);
+    }
+
+    return proc;
+}
+
+/**
+ * Frees the specified proc entry.  No-state if passed a null pointer.
+ */
+static void
+ble_sm_proc_free(struct ble_sm_proc *proc)
+{
+    int rc;
+
+    if (proc != NULL) {
+        ble_sm_dbg_assert_not_inserted(proc);
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+        memset(proc, 0xff, sizeof *proc);
+#endif
+        rc = os_memblock_put(&ble_sm_proc_pool, proc);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
+}
+
+static void
+ble_sm_proc_remove(struct ble_sm_proc *proc,
+                         struct ble_sm_proc *prev)
+{
+    if (prev == NULL) {
+        BLE_HS_DBG_ASSERT(STAILQ_FIRST(&ble_sm_procs) == proc);
+        STAILQ_REMOVE_HEAD(&ble_sm_procs, next);
+    } else {
+        BLE_HS_DBG_ASSERT(STAILQ_NEXT(prev, next) == proc);
+        STAILQ_REMOVE_AFTER(&ble_sm_procs, prev, next);
+    }
+
+    ble_sm_dbg_assert_no_cycles();
+}
+
+static void
+ble_sm_update_sec_state(uint16_t conn_handle, int encrypted,
+                        int authenticated, int bonded, int key_size)
+{
+    struct ble_hs_conn *conn;
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        conn->bhc_sec_state.encrypted = encrypted;
+
+        /* Authentication and bonding are never revoked from a secure link */
+        if (authenticated) {
+            conn->bhc_sec_state.authenticated = 1;
+        }
+        if (bonded) {
+            conn->bhc_sec_state.bonded = 1;
+        }
+
+        if (key_size) {
+            conn->bhc_sec_state.key_size = key_size;
+        }
+    }
+}
+
+static void
+ble_sm_fill_store_value(const ble_addr_t *peer_addr,
+                        int authenticated,
+                        int sc,
+                        struct ble_sm_keys *keys,
+                        struct ble_store_value_sec *value_sec)
+{
+    memset(value_sec, 0, sizeof *value_sec);
+
+    value_sec->peer_addr = *peer_addr;
+
+    if (keys->ediv_rand_valid && keys->ltk_valid) {
+        value_sec->key_size = keys->key_size;
+        value_sec->ediv = keys->ediv;
+        value_sec->rand_num = keys->rand_val;
+
+        memcpy(value_sec->ltk, keys->ltk, sizeof value_sec->ltk);
+        value_sec->ltk_present = 1;
+
+        value_sec->authenticated = !!authenticated;
+        value_sec->sc = !!sc;
+    }
+
+    if (keys->irk_valid) {
+        memcpy(value_sec->irk, keys->irk, sizeof value_sec->irk);
+        value_sec->irk_present = 1;
+    }
+
+    if (keys->csrk_valid) {
+        memcpy(value_sec->csrk, keys->csrk, sizeof value_sec->csrk);
+        value_sec->csrk_present = 1;
+    }
+}
+
+void
+ble_sm_ia_ra(struct ble_sm_proc *proc,
+             uint8_t *out_iat, uint8_t *out_ia,
+             uint8_t *out_rat, uint8_t *out_ra)
+{
+    struct ble_hs_conn_addrs addrs;
+    struct ble_hs_conn *conn;
+
+    conn = ble_hs_conn_find_assert(proc->conn_handle);
+
+    ble_hs_conn_addrs(conn, &addrs);
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        *out_iat = addrs.our_ota_addr.type;
+        memcpy(out_ia, addrs.our_ota_addr.val, 6);
+
+        *out_rat = addrs.peer_ota_addr.type;
+        memcpy(out_ra, addrs.peer_ota_addr.val, 6);
+    } else {
+        *out_iat = addrs.peer_ota_addr.type;
+        memcpy(out_ia, addrs.peer_ota_addr.val, 6);
+
+        *out_rat = addrs.our_ota_addr.type;
+        memcpy(out_ra, addrs.our_ota_addr.val, 6);
+    }
+}
+
+static void
+ble_sm_persist_keys(struct ble_sm_proc *proc)
+{
+    struct ble_store_value_sec value_sec;
+    struct ble_hs_conn *conn;
+    ble_addr_t peer_addr;
+    int authenticated;
+    int identity_ev = 0;
+    int sc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(proc->conn_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+
+    /* If we got an identity address, use that for key storage. */
+    if (proc->peer_keys.addr_valid) {
+        peer_addr.type = proc->peer_keys.addr_type;
+        memcpy(peer_addr.val, proc->peer_keys.addr, sizeof peer_addr.val);
+
+        conn->bhc_peer_addr = peer_addr;
+
+        /* Update identity address in conn.
+         * If peer's rpa address is set then it means that the peer's address
+         * is an identity address. The peer's address type has to be
+         * set as 'ID' to allow resolve 'id' and 'ota' addresses properly in
+         * conn info.
+         */
+        if (memcmp(BLE_ADDR_ANY->val, &conn->bhc_peer_rpa_addr.val, 6) != 0) {
+            switch (peer_addr.type) {
+            case BLE_ADDR_PUBLIC:
+            case BLE_ADDR_PUBLIC_ID:
+                conn->bhc_peer_addr.type = BLE_ADDR_PUBLIC_ID;
+                break;
+
+            case BLE_ADDR_RANDOM:
+            case BLE_ADDR_RANDOM_ID:
+                conn->bhc_peer_addr.type = BLE_ADDR_RANDOM_ID;
+                break;
+            }
+
+            identity_ev = 1;
+        }
+    } else {
+        peer_addr = conn->bhc_peer_addr;
+        peer_addr.type =
+            ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type);
+    }
+
+    ble_hs_unlock();
+
+    if (identity_ev) {
+        ble_gap_identity_event(proc->conn_handle);
+    }
+
+    authenticated = proc->flags & BLE_SM_PROC_F_AUTHENTICATED;
+    sc = proc->flags & BLE_SM_PROC_F_SC;
+
+    ble_sm_fill_store_value(&peer_addr, authenticated, sc, &proc->our_keys,
+                            &value_sec);
+    ble_store_write_our_sec(&value_sec);
+
+    ble_sm_fill_store_value(&peer_addr, authenticated, sc, &proc->peer_keys,
+                            &value_sec);
+    ble_store_write_peer_sec(&value_sec);
+}
+
+static int
+ble_sm_proc_matches(struct ble_sm_proc *proc, uint16_t conn_handle,
+                    uint8_t state, int is_initiator)
+{
+    int proc_is_initiator;
+
+    if (conn_handle != proc->conn_handle) {
+        return 0;
+    }
+
+    if (state != BLE_SM_PROC_STATE_NONE && state != proc->state) {
+        return 0;
+    }
+
+    proc_is_initiator = !!(proc->flags & BLE_SM_PROC_F_INITIATOR);
+    if (is_initiator != -1 && is_initiator != proc_is_initiator) {
+        return 0;
+    }
+
+    return 1;
+}
+
+/**
+ * Searches the main proc list for an entry whose connection handle and state
+ * code match those specified.
+ *
+ * @param conn_handle           The connection handle to match against.
+ * @param state                 The state code to match against.
+ * @param is_initiator          Matches on the proc's initiator flag:
+ *                                   0=non-initiator only
+ *                                   1=initiator only
+ *                                  -1=don't care
+ * @param out_prev              On success, the entry previous to the result is
+ *                                  written here.
+ *
+ * @return                      The matching proc entry on success;
+ *                                  null on failure.
+ */
+struct ble_sm_proc *
+ble_sm_proc_find(uint16_t conn_handle, uint8_t state, int is_initiator,
+                 struct ble_sm_proc **out_prev)
+{
+    struct ble_sm_proc *proc;
+    struct ble_sm_proc *prev;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    prev = NULL;
+    STAILQ_FOREACH(proc, &ble_sm_procs, next) {
+        if (ble_sm_proc_matches(proc, conn_handle, state, is_initiator)) {
+            if (out_prev != NULL) {
+                *out_prev = prev;
+            }
+            break;
+        }
+
+        prev = proc;
+    }
+
+    return proc;
+}
+
+static void
+ble_sm_insert(struct ble_sm_proc *proc)
+{
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    struct ble_sm_proc *cur;
+
+    STAILQ_FOREACH(cur, &ble_sm_procs, next) {
+        BLE_HS_DBG_ASSERT(cur != proc);
+    }
+#endif
+
+    STAILQ_INSERT_HEAD(&ble_sm_procs, proc, next);
+}
+
+static int32_t
+ble_sm_extract_expired(struct ble_sm_proc_list *dst_list)
+{
+    struct ble_sm_proc *proc;
+    struct ble_sm_proc *prev;
+    struct ble_sm_proc *next;
+    ble_npl_time_t now;
+    ble_npl_stime_t next_exp_in;
+    ble_npl_stime_t time_diff;
+
+    now = ble_npl_time_get();
+    STAILQ_INIT(dst_list);
+
+    /* Assume each event is either expired or has infinite duration. */
+    next_exp_in = BLE_HS_FOREVER;
+
+    ble_hs_lock();
+
+    prev = NULL;
+    proc = STAILQ_FIRST(&ble_sm_procs);
+    while (proc != NULL) {
+        next = STAILQ_NEXT(proc, next);
+
+        time_diff = proc->exp_os_ticks - now;
+        if (time_diff <= 0) {
+            /* Procedure has expired; move it to the destination list. */
+            if (prev == NULL) {
+                STAILQ_REMOVE_HEAD(&ble_sm_procs, next);
+            } else {
+                STAILQ_REMOVE_AFTER(&ble_sm_procs, prev, next);
+            }
+            STAILQ_INSERT_HEAD(dst_list, proc, next);
+        } else {
+            if (time_diff < next_exp_in) {
+                next_exp_in = time_diff;
+            }
+        }
+
+        prev = proc;
+        proc = next;
+    }
+
+    ble_sm_dbg_assert_no_cycles();
+
+    ble_hs_unlock();
+
+    return next_exp_in;
+}
+
+static void
+ble_sm_rx_noop(uint16_t conn_handle, struct os_mbuf **om,
+               struct ble_sm_result *res)
+{
+    res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CMD_NOT_SUPP);
+    res->sm_err = BLE_SM_ERR_CMD_NOT_SUPP;
+}
+
+static uint8_t
+ble_sm_build_authreq(void)
+{
+    return ble_hs_cfg.sm_bonding << 0  |
+           ble_hs_cfg.sm_mitm << 2     |
+           ble_hs_cfg.sm_sc << 3       |
+           ble_hs_cfg.sm_keypress << 4;
+}
+
+static int
+ble_sm_io_action(struct ble_sm_proc *proc, uint8_t *action)
+{
+    if (proc->flags & BLE_SM_PROC_F_SC) {
+        return ble_sm_sc_io_action(proc, action);
+    } else {
+        return ble_sm_lgcy_io_action(proc, action);
+    }
+}
+
+int
+ble_sm_ioact_state(uint8_t action)
+{
+    switch (action) {
+    case BLE_SM_IOACT_NONE:
+        return BLE_SM_PROC_STATE_NONE;
+
+    case BLE_SM_IOACT_NUMCMP:
+        return BLE_SM_PROC_STATE_DHKEY_CHECK;
+
+    case BLE_SM_IOACT_OOB_SC:
+        return BLE_SM_PROC_STATE_RANDOM;
+
+    case BLE_SM_IOACT_OOB:
+    case BLE_SM_IOACT_INPUT:
+    case BLE_SM_IOACT_DISP:
+        return BLE_SM_PROC_STATE_CONFIRM;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_SM_PROC_STATE_NONE;
+    }
+}
+
+int
+ble_sm_proc_can_advance(struct ble_sm_proc *proc)
+{
+    uint8_t ioact;
+    int rc;
+
+    rc = ble_sm_io_action(proc, &ioact);
+    if (rc != 0) {
+        BLE_HS_DBG_ASSERT(0);
+    }
+
+    if (ble_sm_ioact_state(ioact) != proc->state) {
+        return 1;
+    }
+
+    if (proc->flags & BLE_SM_PROC_F_IO_INJECTED &&
+        proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO) {
+
+        return 1;
+    }
+
+    return 0;
+}
+
+static void
+ble_sm_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, void *arg)
+{
+    ble_sm_state_fn *cb;
+
+    memset(res, 0, sizeof *res);
+
+    if (!ble_hs_conn_exists(proc->conn_handle)) {
+        res->app_status = BLE_HS_ENOTCONN;
+    } else {
+        BLE_HS_DBG_ASSERT(proc->state < BLE_SM_PROC_STATE_CNT);
+        cb = ble_sm_state_dispatch[proc->state];
+        BLE_HS_DBG_ASSERT(cb != NULL);
+        cb(proc, res, arg);
+    }
+}
+
+static void
+ble_sm_pair_fail_tx(uint16_t conn_handle, uint8_t reason)
+{
+    struct ble_sm_pair_fail *cmd;
+    struct os_mbuf *txom;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(reason > 0 && reason < BLE_SM_ERR_MAX_PLUS_1);
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_FAIL, sizeof(*cmd), &txom);
+    if (cmd) {
+        cmd->reason = reason;
+        rc = ble_sm_tx(conn_handle, txom);
+        if (rc) {
+            BLE_HS_LOG(ERROR, "ble_sm_pair_fail_tx failed, rc = %d\n", rc);
+        }
+    }
+}
+
+/**
+ * Reads a bond from storage.
+ */
+static int
+ble_sm_read_bond(uint16_t conn_handle, struct ble_store_value_sec *out_bond)
+{
+    struct ble_store_key_sec key_sec;
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    rc = ble_gap_conn_find(conn_handle, &desc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    memset(&key_sec, 0, sizeof key_sec);
+    key_sec.peer_addr = desc.peer_id_addr;
+
+    rc = ble_store_read_peer_sec(&key_sec, out_bond);
+    return rc;
+}
+
+/**
+ * Checks if the specified peer is already bonded.  If it is, the application
+ * is queried about how to proceed: retry or ignore.  The application should
+ * only indicate a retry if it deleted the old bond.
+ *
+ * @param conn_handle           The handle of the connection over which the
+ *                                  pairing request was received.
+ * @param proc_flags            The security flags associated with the
+ *                                  conflicting SM procedure.
+ * @param key_size              The key size of the conflicting SM procedure.
+ *
+ * @return                      0 if the procedure should continue;
+ *                              nonzero if the request should be ignored.
+ */
+static int
+ble_sm_chk_repeat_pairing(uint16_t conn_handle,
+                          ble_sm_proc_flags proc_flags,
+                          uint8_t key_size)
+{
+    struct ble_gap_repeat_pairing rp;
+    struct ble_store_value_sec bond;
+    int rc;
+
+    do {
+        /* If the peer isn't bonded, indicate that the pairing procedure should
+         * continue.
+         */
+        rc = ble_sm_read_bond(conn_handle, &bond);
+        switch (rc) {
+        case 0:
+            break;
+        case BLE_HS_ENOENT:
+            return 0;
+        default:
+            return rc;
+        }
+
+        /* Peer is already bonded.  Ask the application what to do about it. */
+        rp.conn_handle = conn_handle;
+        rp.cur_key_size = bond.key_size;
+        rp.cur_authenticated = bond.authenticated;
+        rp.cur_sc = bond.sc;
+
+        rp.new_key_size = key_size;
+        rp.new_authenticated = !!(proc_flags & BLE_SM_PROC_F_AUTHENTICATED);
+        rp.new_sc = !!(proc_flags & BLE_SM_PROC_F_SC);
+        rp.new_bonding = !!(proc_flags & BLE_SM_PROC_F_BONDING);
+
+        rc = ble_gap_repeat_pairing_event(&rp);
+    } while (rc == BLE_GAP_REPEAT_PAIRING_RETRY);
+
+    BLE_HS_LOG(DEBUG, "silently ignoring pair request from bonded peer");
+
+    return BLE_HS_EALREADY;
+}
+
+void
+ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res)
+{
+    struct ble_sm_proc *prev;
+    struct ble_sm_proc *proc;
+    int rm;
+
+    rm = 0;
+
+    while (1) {
+        ble_hs_lock();
+        proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1,
+                                &prev);
+
+        if (proc != NULL) {
+            if (res->execute) {
+                ble_sm_exec(proc, res, res->state_arg);
+            }
+
+            if (res->app_status != 0) {
+                rm = 1;
+            }
+
+            if (proc->state == BLE_SM_PROC_STATE_NONE) {
+                rm = 1;
+            }
+
+            if (rm) {
+                ble_sm_proc_remove(proc, prev);
+            } else {
+                ble_sm_proc_set_timer(proc);
+            }
+        }
+
+        if (res->sm_err != 0) {
+            ble_sm_pair_fail_tx(conn_handle, res->sm_err);
+        }
+
+        ble_hs_unlock();
+
+        if (proc == NULL) {
+            break;
+        }
+
+        if (res->enc_cb) {
+            BLE_HS_DBG_ASSERT(proc == NULL || rm);
+            ble_gap_enc_event(conn_handle, res->app_status, res->restore);
+        }
+
+        if (res->app_status == 0 &&
+            res->passkey_params.action != BLE_SM_IOACT_NONE) {
+
+            ble_gap_passkey_event(conn_handle, &res->passkey_params);
+        }
+
+        /* Persist keys if bonding has successfully completed. */
+        if (res->app_status == 0    &&
+            rm                      &&
+            proc->flags & BLE_SM_PROC_F_BONDING) {
+
+            ble_sm_persist_keys(proc);
+        }
+
+        if (rm) {
+            ble_sm_proc_free(proc);
+            break;
+        }
+
+        if (!res->execute) {
+            break;
+        }
+
+        memset(res, 0, sizeof *res);
+        res->execute = 1;
+    }
+}
+
+static void
+ble_sm_key_dist(struct ble_sm_proc *proc,
+                uint8_t *out_init_key_dist, uint8_t *out_resp_key_dist)
+{
+    struct ble_sm_pair_cmd *pair_rsp;
+
+    pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+
+    *out_init_key_dist = pair_rsp->init_key_dist;
+    *out_resp_key_dist = pair_rsp->resp_key_dist;
+
+    /* Encryption info and master ID are only sent in legacy pairing. */
+    if (proc->flags & BLE_SM_PROC_F_SC) {
+        *out_init_key_dist &= ~BLE_SM_PAIR_KEY_DIST_ENC;
+        *out_resp_key_dist &= ~BLE_SM_PAIR_KEY_DIST_ENC;
+    }
+}
+
+static int
+ble_sm_chk_store_overflow_by_type(int obj_type, uint16_t conn_handle)
+{
+#if !MYNEWT_VAL(BLE_SM_BONDING)
+    return 0;
+#endif
+
+    int count;
+    int rc;
+
+    rc = ble_store_util_count(obj_type, &count);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Pessimistically assume all active procs will persist bonds. */
+    ble_hs_lock();
+    count += ble_sm_num_procs();
+    ble_hs_unlock();
+
+    if (count < MYNEWT_VAL(BLE_STORE_MAX_BONDS)) {
+        /* There is sufficient capacity for another bond. */
+        return 0;
+    }
+
+    /* No capacity for an additional bond.  Tell the application to make
+     * room.
+     */
+    rc = ble_store_full_event(obj_type, conn_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_chk_store_overflow(uint16_t conn_handle)
+{
+    int rc;
+
+    rc = ble_sm_chk_store_overflow_by_type(BLE_STORE_OBJ_TYPE_PEER_SEC,
+                                           conn_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_sm_chk_store_overflow_by_type(BLE_STORE_OBJ_TYPE_OUR_SEC,
+                                           conn_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $enc                                                                      *
+ *****************************************************************************/
+
+static int
+ble_sm_start_encrypt_tx(struct hci_start_encrypt *params)
+{
+    struct ble_hci_le_start_encrypt_cp cmd;
+
+    cmd.conn_handle = htole16(params->connection_handle);
+    cmd.div = htole16(params->encrypted_diversifier);
+    cmd.rand = htole64(params->random_number);
+    memcpy(cmd.ltk, params->long_term_key, sizeof(cmd.ltk));
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_START_ENCRYPT),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+static void
+ble_sm_enc_start_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                      void *arg)
+{
+    struct hci_start_encrypt cmd;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(proc->flags & BLE_SM_PROC_F_INITIATOR);
+
+    cmd.connection_handle = proc->conn_handle;
+    cmd.encrypted_diversifier = 0;
+    cmd.random_number = 0;
+    memcpy(cmd.long_term_key, proc->ltk, sizeof cmd.long_term_key);
+
+    rc = ble_sm_start_encrypt_tx(&cmd);
+    if (rc != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->app_status = rc;
+        res->enc_cb = 1;
+    }
+}
+
+static void
+ble_sm_enc_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                        void *arg)
+{
+    struct hci_start_encrypt *cmd;
+
+    BLE_HS_DBG_ASSERT(proc->flags & BLE_SM_PROC_F_INITIATOR);
+
+    cmd = arg;
+    BLE_HS_DBG_ASSERT(cmd != NULL);
+
+    res->app_status = ble_sm_start_encrypt_tx(cmd);
+}
+
+static void
+ble_sm_enc_event_rx(uint16_t conn_handle, uint8_t evt_status, int encrypted)
+{
+    struct ble_sm_result res;
+    struct ble_sm_proc *proc;
+    int authenticated;
+    int bonded;
+    int key_size;
+
+    memset(&res, 0, sizeof res);
+
+    /* Assume no change in authenticated and bonded statuses. */
+    authenticated = 0;
+    bonded = 0;
+    key_size = 0;
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL);
+    if (proc != NULL) {
+        switch (proc->state) {
+        case BLE_SM_PROC_STATE_ENC_START:
+            /* We are completing a pairing procedure; keys may need to be
+             * exchanged.
+             */
+            if (evt_status == 0) {
+                /* If the responder has any keys to send, it sends them
+                 * first.
+                 */
+                proc->state = BLE_SM_PROC_STATE_KEY_EXCH;
+                if (!(proc->flags & BLE_SM_PROC_F_INITIATOR) ||
+                    proc->rx_key_flags == 0) {
+
+                    res.execute = 1;
+                }
+
+                key_size = proc->key_size;
+            } else {
+                /* Failure or no keys to exchange; procedure is complete. */
+                proc->state = BLE_SM_PROC_STATE_NONE;
+            }
+            if (proc->flags & BLE_SM_PROC_F_AUTHENTICATED) {
+                authenticated = 1;
+            }
+            break;
+
+        case BLE_SM_PROC_STATE_ENC_RESTORE:
+            /* A secure link is being restored via the encryption
+             * procedure.  Keys were exchanged during pairing; they don't
+             * get exchanged again now.  Procedure is complete.
+             */
+            BLE_HS_DBG_ASSERT(proc->rx_key_flags == 0);
+            proc->state = BLE_SM_PROC_STATE_NONE;
+            if (proc->flags & BLE_SM_PROC_F_AUTHENTICATED) {
+                authenticated = 1;
+            }
+            bonded = 1;
+            res.restore = 1;
+
+            key_size = proc->key_size;
+            break;
+
+        default:
+            /* The encryption change event is unexpected.  We take the
+             * controller at its word that the state has changed and we
+             * terminate the procedure.
+             */
+            proc->state = BLE_SM_PROC_STATE_NONE;
+            res.sm_err = BLE_SM_ERR_UNSPECIFIED;
+            break;
+        }
+    }
+
+    if (evt_status == 0) {
+        /* Set the encrypted state of the connection as indicated in the
+         * event.
+         */
+        ble_sm_update_sec_state(conn_handle, encrypted, authenticated, bonded,
+                                key_size);
+    }
+
+    /* Unless keys need to be exchanged, notify the application of the security
+     * change.  If key exchange is pending, the application callback is
+     * triggered after exchange completes.
+     */
+    if (proc == NULL || proc->state == BLE_SM_PROC_STATE_NONE) {
+        res.enc_cb = 1;
+        res.app_status = BLE_HS_HCI_ERR(evt_status);
+    }
+
+    ble_hs_unlock();
+
+    ble_sm_process_result(conn_handle, &res);
+}
+
+void
+ble_sm_enc_change_rx(const struct ble_hci_ev_enrypt_chg *ev)
+{
+    /* For encrypted state: read LE-encryption bit; ignore BR/EDR and reserved
+     * bits.
+     */
+    ble_sm_enc_event_rx(le16toh(ev->connection_handle), ev->status,
+                        ev->enabled & 0x01);
+}
+
+void
+ble_sm_enc_key_refresh_rx(const struct ble_hci_ev_enc_key_refresh *ev)
+{
+    ble_sm_enc_event_rx(le16toh(ev->conn_handle), ev->status, 1);
+}
+
+/*****************************************************************************
+ * $ltk                                                                      *
+ *****************************************************************************/
+
+static int
+ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type,
+                    uint8_t *peer_addr, struct ble_store_value_sec *value_sec)
+{
+    struct ble_store_key_sec key_sec;
+    int rc;
+
+    /* Tell applicaiton to look up LTK by peer address and ediv/rand pair. */
+    memset(&key_sec, 0, sizeof key_sec);
+    key_sec.peer_addr.type = peer_addr_type;
+    memcpy(key_sec.peer_addr.val, peer_addr, 6);
+    key_sec.ediv = ediv;
+    key_sec.rand_num = rand;
+    key_sec.ediv_rand_present = 1;
+
+    rc = ble_store_read_our_sec(&key_sec, value_sec);
+    return rc;
+}
+
+static int
+ble_sm_ltk_req_reply_tx(uint16_t conn_handle, const uint8_t *ltk)
+{
+    struct ble_hci_le_lt_key_req_reply_cp cmd;
+    struct ble_hci_le_lt_key_req_reply_rp rsp;
+    int rc;
+
+    cmd.conn_handle = htole16(conn_handle);
+    memcpy(cmd.ltk, ltk, 16);
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY),
+                           &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (le16toh(rsp.conn_handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_ltk_req_neg_reply_tx(uint16_t conn_handle)
+{
+    struct ble_hci_le_lt_key_req_neg_reply_cp cmd;
+    struct ble_hci_le_lt_key_req_neg_reply_cp rsp;
+    int rc;
+
+    cmd.conn_handle = htole16(conn_handle);
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY),
+                           &cmd, sizeof(cmd), &rsp, sizeof(rsp));
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (le16toh(rsp.conn_handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    return 0;
+}
+
+static void
+ble_sm_ltk_start_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                      void *arg)
+{
+    BLE_HS_DBG_ASSERT(!(proc->flags & BLE_SM_PROC_F_INITIATOR));
+
+    res->app_status = ble_sm_ltk_req_reply_tx(proc->conn_handle, proc->ltk);
+    if (res->app_status == 0) {
+        proc->state = BLE_SM_PROC_STATE_ENC_START;
+    } else {
+        res->enc_cb = 1;
+    }
+}
+
+static void
+ble_sm_ltk_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                        void *arg)
+{
+    struct ble_store_value_sec *value_sec;
+
+    BLE_HS_DBG_ASSERT(!(proc->flags & BLE_SM_PROC_F_INITIATOR));
+
+    value_sec = arg;
+
+    if (value_sec != NULL) {
+        /* Store provided a key; send it to the controller. */
+        res->app_status = ble_sm_ltk_req_reply_tx(
+            proc->conn_handle, value_sec->ltk);
+
+        if (res->app_status == 0) {
+            proc->key_size = value_sec->key_size;
+            if (value_sec->authenticated) {
+                proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+            }
+        } else {
+            /* Notify the app if it provided a key and the procedure failed. */
+            res->enc_cb = 1;
+        }
+    } else {
+        /* Application does not have the requested key in its database.  Send a
+         * negative reply to the controller.
+         */
+        ble_sm_ltk_req_neg_reply_tx(proc->conn_handle);
+        res->app_status = BLE_HS_ENOENT;
+    }
+
+    if (res->app_status == 0) {
+        proc->state = BLE_SM_PROC_STATE_ENC_RESTORE;
+    }
+}
+
+int
+ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev)
+{
+    struct ble_store_value_sec value_sec;
+    struct ble_hs_conn_addrs addrs;
+    struct ble_sm_result res;
+    struct ble_sm_proc *proc;
+    struct ble_hs_conn *conn;
+    uint8_t peer_id_addr[6];
+    int store_rc;
+    int restore;
+
+    uint16_t conn_handle = le16toh(ev->conn_handle);
+
+    memset(&res, 0, sizeof res);
+
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, 0, NULL);
+    if (proc == NULL) {
+        /* The peer is attempting to restore a encrypted connection via the
+         * encryption procedure.  Create a proc entry to indicate that security
+         * establishment is in progress and execute the procedure after the
+         * mutex gets unlocked.
+         */
+        restore = 1;
+        proc = ble_sm_proc_alloc();
+        if (proc == NULL) {
+            res.app_status = BLE_HS_ENOMEM;
+        } else {
+            proc->conn_handle = conn_handle;
+            proc->state = BLE_SM_PROC_STATE_LTK_RESTORE;
+            ble_sm_insert(proc);
+
+            res.execute = 1;
+        }
+    } else if (proc->state == BLE_SM_PROC_STATE_SEC_REQ) {
+        /* Same as above, except we solicited the encryption procedure by
+         * sending a security request.
+         */
+        restore = 1;
+        proc->state = BLE_SM_PROC_STATE_LTK_RESTORE;
+        res.execute = 1;
+    } else if (proc->state == BLE_SM_PROC_STATE_LTK_START) {
+        /* Legacy pairing just completed.  Send the short term key to the
+         * controller.
+         */
+        restore = 0;
+        res.execute = 1;
+    } else {
+        /* The request is unexpected; nack and forget. */
+        restore = 0;
+        ble_sm_ltk_req_neg_reply_tx(conn_handle);
+        proc = NULL;
+    }
+
+    if (restore) {
+        conn = ble_hs_conn_find_assert(conn_handle);
+        ble_hs_conn_addrs(conn, &addrs);
+        memcpy(peer_id_addr, addrs.peer_id_addr.val, 6);
+    }
+
+    ble_hs_unlock();
+
+    if (proc == NULL) {
+        return res.app_status;
+    }
+
+    if (res.app_status == 0) {
+        if (restore) {
+            store_rc = ble_sm_retrieve_ltk(le16toh(ev->div), le64toh(ev->rand),
+                                            addrs.peer_id_addr.type,
+                                           peer_id_addr, &value_sec);
+            if (store_rc == 0) {
+                /* Send the key to the controller. */
+                res.state_arg = &value_sec;
+            } else {
+                /* Send a nack to the controller. */
+                res.state_arg = NULL;
+            }
+        }
+    }
+
+    ble_sm_process_result(conn_handle, &res);
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $random                                                                   *
+ *****************************************************************************/
+
+uint8_t *
+ble_sm_our_pair_rand(struct ble_sm_proc *proc)
+{
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        return proc->randm;
+    } else {
+        return proc->rands;
+    }
+}
+
+uint8_t *
+ble_sm_peer_pair_rand(struct ble_sm_proc *proc)
+{
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        return proc->rands;
+    } else {
+        return proc->randm;
+    }
+}
+
+static void
+ble_sm_random_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                   void *arg)
+{
+    if (proc->flags & BLE_SM_PROC_F_SC) {
+        ble_sm_sc_random_exec(proc, res);
+    } else {
+        ble_sm_lgcy_random_exec(proc, res);
+    }
+}
+
+static void
+ble_sm_random_rx(uint16_t conn_handle, struct os_mbuf **om,
+                 struct ble_sm_result *res)
+{
+    struct ble_sm_pair_random *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_pair_random *)(*om)->om_data;
+
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_RANDOM, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+    } else {
+        memcpy(ble_sm_peer_pair_rand(proc), cmd->value, 16);
+
+        if (proc->flags & BLE_SM_PROC_F_SC) {
+            ble_sm_sc_random_rx(proc, res);
+        } else {
+            ble_sm_lgcy_random_rx(proc, res);
+        }
+    }
+    ble_hs_unlock();
+}
+
+/*****************************************************************************
+ * $confirm                                                                  *
+ *****************************************************************************/
+
+static void
+ble_sm_confirm_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                    void *arg)
+{
+    if (!(proc->flags & BLE_SM_PROC_F_SC)) {
+        ble_sm_lgcy_confirm_exec(proc, res);
+    } else {
+        ble_sm_sc_confirm_exec(proc, res);
+    }
+}
+
+static void
+ble_sm_confirm_rx(uint16_t conn_handle, struct os_mbuf **om,
+                  struct ble_sm_result *res)
+{
+    struct ble_sm_pair_confirm *cmd;
+    struct ble_sm_proc *proc;
+    uint8_t ioact;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_pair_confirm *)(*om)->om_data;
+
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_CONFIRM, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+    } else {
+        memcpy(proc->confirm_peer, cmd->value, 16);
+
+        if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+            proc->state = BLE_SM_PROC_STATE_RANDOM;
+            res->execute = 1;
+        } else {
+            int rc;
+
+            rc = ble_sm_io_action(proc, &ioact);
+            if (rc != 0) {
+                BLE_HS_DBG_ASSERT(0);
+            }
+
+            if (ble_sm_ioact_state(ioact) == proc->state) {
+                proc->flags |= BLE_SM_PROC_F_ADVANCE_ON_IO;
+            }
+            if (ble_sm_proc_can_advance(proc)) {
+                res->execute = 1;
+            }
+        }
+    }
+    ble_hs_unlock();
+}
+
+/*****************************************************************************
+ * $pair                                                                     *
+ *****************************************************************************/
+
+static uint8_t
+ble_sm_state_after_pair(struct ble_sm_proc *proc)
+{
+    if (proc->flags & BLE_SM_PROC_F_SC) {
+        return BLE_SM_PROC_STATE_PUBLIC_KEY;
+    } else {
+        return BLE_SM_PROC_STATE_CONFIRM;
+    }
+}
+
+static void
+ble_sm_pair_cfg(struct ble_sm_proc *proc)
+{
+    struct ble_sm_pair_cmd *pair_req, *pair_rsp;
+    uint8_t init_key_dist;
+    uint8_t resp_key_dist;
+    uint8_t rx_key_dist;
+    uint8_t ioact;
+    int rc;
+
+    pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
+    pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+
+    if (pair_req->authreq & BLE_SM_PAIR_AUTHREQ_SC &&
+        pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_SC) {
+
+        proc->flags |= BLE_SM_PROC_F_SC;
+    }
+
+    ble_sm_key_dist(proc, &init_key_dist, &resp_key_dist);
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        rx_key_dist = resp_key_dist;
+    } else {
+        rx_key_dist = init_key_dist;
+    }
+
+    if (pair_req->authreq & BLE_SM_PAIR_AUTHREQ_BOND &&
+        pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_BOND) {
+
+        proc->flags |= BLE_SM_PROC_F_BONDING;
+    }
+
+    /* In legacy mode, bonding requires the exchange of keys
+     * at least from one side.  If no key exchange was specified,
+     * pretend bonding is not enabled.
+     */
+    if (!(proc->flags & BLE_SM_PROC_F_SC) &&
+        (init_key_dist == 0 && resp_key_dist == 0)) {
+
+        proc->flags &= ~BLE_SM_PROC_F_BONDING;
+    }
+
+    proc->rx_key_flags = 0;
+    if (rx_key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+        proc->rx_key_flags |= BLE_SM_KE_F_ENC_INFO |
+                              BLE_SM_KE_F_MASTER_ID;
+    }
+    if (rx_key_dist & BLE_SM_PAIR_KEY_DIST_ID) {
+        proc->rx_key_flags |= BLE_SM_KE_F_ID_INFO |
+                              BLE_SM_KE_F_ADDR_INFO;
+    }
+    if (rx_key_dist & BLE_SM_PAIR_KEY_DIST_SIGN) {
+        proc->rx_key_flags |= BLE_SM_KE_F_SIGN_INFO;
+    }
+
+    proc->key_size = min(pair_req->max_enc_key_size,
+                         pair_rsp->max_enc_key_size);
+
+    rc = ble_sm_io_action(proc, &ioact);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+}
+
+static void
+ble_sm_pair_base_fill(struct ble_sm_pair_cmd *cmd)
+{
+    cmd->io_cap = ble_hs_cfg.sm_io_cap;
+    cmd->oob_data_flag = ble_hs_cfg.sm_oob_data_flag;
+    cmd->authreq = ble_sm_build_authreq();
+    cmd->max_enc_key_size = BLE_SM_PAIR_KEY_SZ_MAX;
+}
+
+static void
+ble_sm_pair_req_fill(struct ble_sm_proc *proc)
+{
+    struct ble_sm_pair_cmd *req;
+
+    req = (void *)(proc->pair_req + 1);
+
+    proc->pair_req[0] = BLE_SM_OP_PAIR_REQ;
+    ble_sm_pair_base_fill(req);
+    req->init_key_dist = ble_hs_cfg.sm_our_key_dist;
+    req->resp_key_dist = ble_hs_cfg.sm_their_key_dist;
+}
+
+static void
+ble_sm_pair_rsp_fill(struct ble_sm_proc *proc)
+{
+    const struct ble_sm_pair_cmd *req;
+    struct ble_sm_pair_cmd *rsp;
+
+    req = (void *)(proc->pair_req + 1);
+    rsp = (void *)(proc->pair_rsp + 1);
+
+    proc->pair_rsp[0] = BLE_SM_OP_PAIR_RSP;
+    ble_sm_pair_base_fill(rsp);
+
+    /* The response's key distribution flags field is the intersection of
+     * the peer's preferences and our capabilities.
+     */
+    rsp->init_key_dist = req->init_key_dist &
+                         ble_hs_cfg.sm_their_key_dist;
+    rsp->resp_key_dist = req->resp_key_dist &
+                         ble_hs_cfg.sm_our_key_dist;
+}
+
+static void
+ble_sm_pair_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                 void *arg)
+{
+    struct ble_sm_pair_cmd *cmd;
+    struct os_mbuf *txom;
+    uint8_t ioact;
+    int is_req;
+    int rc;
+
+    is_req = proc->flags & BLE_SM_PROC_F_INITIATOR;
+
+    cmd = ble_sm_cmd_get(is_req ? BLE_SM_OP_PAIR_REQ : BLE_SM_OP_PAIR_RSP,
+                         sizeof(*cmd), &txom);
+    if (cmd == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    if (is_req) {
+        ble_sm_pair_req_fill(proc);
+        memcpy(cmd, proc->pair_req + 1, sizeof(*cmd));
+    } else {
+        /* The response was already generated when we processed the incoming
+         * request.
+         */
+        memcpy(cmd, proc->pair_rsp + 1, sizeof(*cmd));
+
+        proc->state = ble_sm_state_after_pair(proc);
+
+        rc = ble_sm_io_action(proc, &ioact);
+        BLE_HS_DBG_ASSERT(rc == 0);
+
+        if (ble_sm_ioact_state(ioact) == proc->state) {
+            res->passkey_params.action = ioact;
+        }
+    }
+
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        goto err;
+    }
+
+    res->app_status = ble_sm_gen_pair_rand(ble_sm_our_pair_rand(proc));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    return;
+
+err:
+    res->app_status = rc;
+
+    if (!is_req) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    }
+}
+
+static bool
+ble_sm_verify_auth_requirements(uint8_t authreq)
+{
+    /* For now we check only SC only mode. I.e.: when remote indicates
+     * to not support SC pairing, let us make sure legacy pairing is supported
+     * on our side. If not, we can fail right away.
+     */
+    if (!(authreq & BLE_SM_PAIR_AUTHREQ_SC)) {
+        if (MYNEWT_VAL(BLE_SM_LEGACY) == 0) {
+            return false;
+        }
+    }
+    return true;
+}
+
+static void
+ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
+                   struct ble_sm_result *res)
+{
+    struct ble_sm_pair_cmd *req;
+    struct ble_sm_proc *proc;
+    struct ble_sm_proc *prev;
+    struct ble_hs_conn *conn;
+    ble_sm_proc_flags proc_flags;
+    uint8_t key_size;
+    int rc;
+
+    /* Silence spurious unused-variable warnings. */
+    proc_flags = 0;
+    key_size = 0;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*req));
+    if (res->app_status != 0) {
+        return;
+    }
+
+    req = (struct ble_sm_pair_cmd *)(*om)->om_data;
+
+    ble_hs_lock();
+
+    /* XXX: Check connection state; reject if not appropriate. */
+    /* XXX: Ensure enough time has passed since the previous failed pairing
+     * attempt.
+     */
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, &prev);
+    if (proc != NULL) {
+        /* Fail if procedure is in progress unless we sent a slave security
+         * request to peer.
+         */
+        if (proc->state != BLE_SM_PROC_STATE_SEC_REQ) {
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_UNSPECIFIED);
+            ble_hs_unlock();
+            return;
+        }
+
+        /* Remove the procedure because it was allocated when
+         * sending the Slave Security Request and it will be allocated
+         * again later in this method. We should probably refactor this
+         * in the future.
+         */
+        ble_sm_proc_remove(proc, prev);
+        ble_sm_proc_free(proc);
+    }
+
+    ble_hs_unlock();
+
+    /* Check if there is storage capacity for a new bond.  If there isn't, ask
+     * the application to make room.
+     */
+    rc = ble_sm_chk_store_overflow(conn_handle);
+    if (rc != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->app_status = rc;
+        return;
+    }
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_alloc();
+    if (proc != NULL) {
+        proc->conn_handle = conn_handle;
+        proc->state = BLE_SM_PROC_STATE_PAIR;
+        ble_sm_insert(proc);
+
+        proc->pair_req[0] = BLE_SM_OP_PAIR_REQ;
+        memcpy(proc->pair_req + 1, req, sizeof(*req));
+
+        conn = ble_hs_conn_find_assert(proc->conn_handle);
+        if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
+            res->sm_err = BLE_SM_ERR_CMD_NOT_SUPP;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CMD_NOT_SUPP);
+        } else if (req->max_enc_key_size < BLE_SM_PAIR_KEY_SZ_MIN) {
+            res->sm_err = BLE_SM_ERR_ENC_KEY_SZ;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ);
+        } else if (req->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) {
+            res->sm_err = BLE_SM_ERR_INVAL;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
+        } else if (!ble_sm_verify_auth_requirements(req->authreq)) {
+            res->sm_err = BLE_SM_ERR_AUTHREQ;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ);
+        } else {
+            /* The request looks good.  Precalculate our pairing response and
+             * determine some properties of the imminent link.  We need this
+             * information in case this is a repeated pairing attempt (i.e., we
+             * are already bonded to this peer).  In that case, we include the
+             * information in a notification to the app.
+             */
+            ble_sm_pair_rsp_fill(proc);
+            ble_sm_pair_cfg(proc);
+
+            proc_flags = proc->flags;
+            key_size = proc->key_size;
+            res->execute = 1;
+        }
+    }
+
+    ble_hs_unlock();
+
+    /* Check if we are already bonded to this peer.  If so, give the
+     * application an opportunity to delete the old bond.
+     */
+    if (res->app_status == 0) {
+        rc = ble_sm_chk_repeat_pairing(conn_handle, proc_flags, key_size);
+        if (rc != 0) {
+            /* The app indicated that the pairing request should be ignored. */
+            res->app_status = rc;
+            res->execute = 0;
+        }
+    }
+}
+
+static void
+ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om,
+                   struct ble_sm_result *res)
+{
+    struct ble_sm_pair_cmd *rsp;
+    struct ble_sm_proc *proc;
+    uint8_t ioact;
+    int rc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*rsp));
+    if (res->app_status != 0) {
+        res->enc_cb = 1;
+        return;
+    }
+
+    rsp = (struct ble_sm_pair_cmd *)(*om)->om_data;
+
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_PAIR, 1, NULL);
+    if (proc != NULL) {
+        proc->pair_rsp[0] = BLE_SM_OP_PAIR_RSP;
+        memcpy(proc->pair_rsp + 1, rsp, sizeof(*rsp));
+
+        if (rsp->max_enc_key_size < BLE_SM_PAIR_KEY_SZ_MIN) {
+            res->sm_err = BLE_SM_ERR_ENC_KEY_SZ;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ);
+        } else if (rsp->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) {
+            res->sm_err = BLE_SM_ERR_INVAL;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
+        } else {
+            ble_sm_pair_cfg(proc);
+
+            rc = ble_sm_io_action(proc, &ioact);
+            if (rc != 0) {
+                res->sm_err = BLE_SM_ERR_AUTHREQ;
+                res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ);
+                res->enc_cb = 1;
+            } else {
+                proc->state = ble_sm_state_after_pair(proc);
+                if (ble_sm_ioact_state(ioact) == proc->state) {
+                    res->passkey_params.action = ioact;
+                }
+                if (ble_sm_proc_can_advance(proc)) {
+                    res->execute = 1;
+                }
+            }
+        }
+    }
+
+    ble_hs_unlock();
+}
+
+/*****************************************************************************
+ * $security request                                                         *
+ *****************************************************************************/
+
+static void
+ble_sm_sec_req_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                    void *arg)
+{
+    struct ble_sm_sec_req *cmd;
+    struct os_mbuf *txom;
+    int rc;
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_SEC_REQ, sizeof(*cmd), &txom);
+    if (!cmd) {
+        res->app_status = BLE_HS_ENOMEM;
+        return;
+    }
+
+    cmd->authreq = ble_sm_build_authreq();
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        res->app_status = rc;
+        return;
+    }
+}
+
+static void
+ble_sm_sec_req_rx(uint16_t conn_handle, struct os_mbuf **om,
+                  struct ble_sm_result *res)
+{
+    struct ble_store_value_sec value_sec;
+    struct ble_store_key_sec key_sec;
+    struct ble_hs_conn_addrs addrs;
+    struct ble_sm_sec_req *cmd;
+    struct ble_hs_conn *conn;
+    int authreq_mitm;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        return;
+    }
+
+    cmd = (struct ble_sm_sec_req *)(*om)->om_data;
+
+    /* XXX: Reject if:
+     *     o authreq-reserved flags set?
+     */
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find_assert(conn_handle);
+    if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) {
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CMD_NOT_SUPP);
+        res->sm_err = BLE_SM_ERR_CMD_NOT_SUPP;
+    } else {
+        /* We will be querying the SM database for a key corresponding to the
+         * sender; remember the sender's address while the connection list is
+         * locked.
+         */
+        ble_hs_conn_addrs(conn, &addrs);
+        memset(&key_sec, 0, sizeof key_sec);
+        key_sec.peer_addr = addrs.peer_id_addr;
+    }
+
+    ble_hs_unlock();
+
+    if (res->app_status == 0) {
+        /* If the peer is requesting a bonded connection, query database for an
+         * LTK corresponding to the sender.
+         */
+        if (cmd->authreq & BLE_SM_PAIR_AUTHREQ_BOND) {
+            res->app_status = ble_store_read_peer_sec(&key_sec, &value_sec);
+        } else {
+            res->app_status = BLE_HS_ENOENT;
+        }
+        if (res->app_status == 0) {
+            /* Found a key corresponding to this peer.  Make sure it meets the
+             * requested minimum authreq.
+             */
+            authreq_mitm = cmd->authreq & BLE_SM_PAIR_AUTHREQ_MITM;
+            if (authreq_mitm && !value_sec.authenticated) {
+                res->app_status = BLE_HS_EREJECT;
+            }
+        }
+
+        if (res->app_status == 0) {
+            res->app_status = ble_sm_enc_initiate(conn_handle,
+                                                  value_sec.key_size,
+                                                  value_sec.ltk,
+                                                  value_sec.ediv,
+                                                  value_sec.rand_num,
+                                                  value_sec.authenticated);
+        } else {
+            res->app_status = ble_sm_pair_initiate(conn_handle);
+        }
+    }
+}
+
+/*****************************************************************************
+ * $key exchange                                                             *
+ *****************************************************************************/
+
+static void
+ble_sm_key_exch_success(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    /* The procedure is now complete.  Update connection bonded state and
+     * terminate procedure.
+     */
+    ble_sm_update_sec_state(proc->conn_handle, 1,
+                            !!(proc->flags & BLE_SM_PROC_F_AUTHENTICATED),
+                            !!(proc->flags & BLE_SM_PROC_F_BONDING),
+                            proc->key_size);
+    proc->state = BLE_SM_PROC_STATE_NONE;
+
+    res->app_status = 0;
+    res->enc_cb = 1;
+}
+
+static void
+ble_sm_key_exch_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                     void *arg)
+{
+    struct ble_sm_id_addr_info *addr_info;
+    struct ble_hs_conn_addrs addrs;
+    struct ble_sm_sign_info *sign_info;
+    struct ble_sm_master_id *master_id;
+    struct ble_sm_enc_info *enc_info;
+    struct ble_sm_id_info *id_info;
+    struct ble_hs_conn *conn;
+    uint8_t init_key_dist;
+    uint8_t resp_key_dist;
+    uint8_t our_key_dist;
+    struct os_mbuf *txom;
+    const uint8_t *irk;
+    int rc;
+
+    ble_sm_key_dist(proc, &init_key_dist, &resp_key_dist);
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        our_key_dist = init_key_dist;
+    } else {
+        our_key_dist = resp_key_dist;
+    }
+
+    if (our_key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+        /* Send encryption information. */
+        enc_info = ble_sm_cmd_get(BLE_SM_OP_ENC_INFO, sizeof(*enc_info), &txom);
+        if (!enc_info) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        rc = ble_sm_gen_ltk(proc, enc_info->ltk);
+        if (rc != 0) {
+            os_mbuf_free_chain(txom);
+            goto err;
+        }
+
+        /* store LTK before sending since ble_sm_tx consumes tx mbuf */
+        memcpy(proc->our_keys.ltk, enc_info->ltk, 16);
+        proc->our_keys.key_size = proc->key_size;
+        proc->our_keys.ltk_valid = 1;
+
+        rc = ble_sm_tx(proc->conn_handle, txom);
+        if (rc != 0) {
+            goto err;
+        }
+
+        /* Send master identification. */
+        master_id = ble_sm_cmd_get(BLE_SM_OP_MASTER_ID, sizeof(*master_id),
+                                   &txom);
+        if (!master_id) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        rc = ble_sm_gen_ediv(master_id);
+        if (rc != 0) {
+            os_mbuf_free_chain(txom);
+            goto err;
+        }
+        rc = ble_sm_gen_master_id_rand(master_id);
+        if (rc != 0) {
+            os_mbuf_free_chain(txom);
+            goto err;
+        }
+
+        proc->our_keys.ediv_rand_valid = 1;
+        proc->our_keys.rand_val = master_id->rand_val;
+        proc->our_keys.ediv = master_id->ediv;
+
+        rc = ble_sm_tx(proc->conn_handle, txom);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+
+    if (our_key_dist & BLE_SM_PAIR_KEY_DIST_ID) {
+        /* Send identity information. */
+        id_info = ble_sm_cmd_get(BLE_SM_OP_IDENTITY_INFO, sizeof(*id_info),
+                                 &txom);
+        if (!id_info) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        rc = ble_hs_pvcy_our_irk(&irk);
+        if (rc != 0) {
+            os_mbuf_free_chain(txom);
+            goto err;
+        }
+
+        memcpy(id_info->irk, irk, 16);
+        proc->our_keys.irk_valid = 1;
+
+        rc = ble_sm_tx(proc->conn_handle, txom);
+        if (rc != 0) {
+            goto err;
+        }
+
+        /* Send identity address information. */
+        addr_info = ble_sm_cmd_get(BLE_SM_OP_IDENTITY_ADDR_INFO,
+                                   sizeof(*addr_info), &txom);
+        if (!addr_info) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        conn = ble_hs_conn_find_assert(proc->conn_handle);
+        ble_hs_conn_addrs(conn, &addrs);
+
+        addr_info->addr_type = addrs.our_id_addr.type;
+        memcpy(addr_info->bd_addr, addrs.our_id_addr.val, 6);
+
+        proc->our_keys.addr_valid = 1;
+        memcpy(proc->our_keys.irk, irk, 16);
+        proc->our_keys.addr_type = addr_info->addr_type;
+        memcpy(proc->our_keys.addr, addr_info->bd_addr, 6);
+
+        rc = ble_sm_tx(proc->conn_handle, txom);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+
+    if (our_key_dist & BLE_SM_PAIR_KEY_DIST_SIGN) {
+        /* Send signing information. */
+        sign_info = ble_sm_cmd_get(BLE_SM_OP_SIGN_INFO, sizeof(*sign_info),
+                                   &txom);
+        if (!sign_info) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        rc = ble_sm_gen_csrk(proc, sign_info->sig_key);
+        if (rc != 0) {
+            os_mbuf_free_chain(txom);
+            goto err;
+        }
+
+        proc->our_keys.csrk_valid = 1;
+        memcpy(proc->our_keys.csrk, sign_info->sig_key, 16);
+
+        rc = ble_sm_tx(proc->conn_handle, txom);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR || proc->rx_key_flags == 0) {
+        /* The procedure is now complete. */
+        ble_sm_key_exch_success(proc, res);
+    }
+
+    return;
+
+err:
+    res->app_status = rc;
+    res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    res->enc_cb = 1;
+}
+
+static void
+ble_sm_key_rxed(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    BLE_HS_LOG(DEBUG, "rx_key_flags=0x%02x\n", proc->rx_key_flags);
+
+    if (proc->rx_key_flags == 0) {
+        /* The peer is done sending keys.  If we are the initiator, we need to
+         * send ours.  If we are the responder, the procedure is complete.
+         */
+        if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+            res->execute = 1;
+        } else {
+            ble_sm_key_exch_success(proc, res);
+        }
+    }
+}
+
+static void
+ble_sm_enc_info_rx(uint16_t conn_handle, struct os_mbuf **om,
+                   struct ble_sm_result *res)
+{
+    struct ble_sm_enc_info *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_enc_info *)(*om)->om_data;
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
+        proc->rx_key_flags &= ~BLE_SM_KE_F_ENC_INFO;
+        proc->peer_keys.ltk_valid = 1;
+        memcpy(proc->peer_keys.ltk, cmd->ltk, 16);
+        proc->peer_keys.key_size = proc->key_size;
+
+        ble_sm_key_rxed(proc, res);
+    }
+
+    ble_hs_unlock();
+}
+
+static void
+ble_sm_master_id_rx(uint16_t conn_handle, struct os_mbuf **om,
+                    struct ble_sm_result *res)
+{
+    struct ble_sm_master_id *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_master_id *)(*om)->om_data;
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
+        proc->rx_key_flags &= ~BLE_SM_KE_F_MASTER_ID;
+        proc->peer_keys.ediv_rand_valid = 1;
+
+        proc->peer_keys.ediv = le16toh(cmd->ediv);
+        proc->peer_keys.rand_val = le64toh(cmd->rand_val);
+
+        ble_sm_key_rxed(proc, res);
+    }
+
+    ble_hs_unlock();
+}
+
+static void
+ble_sm_id_info_rx(uint16_t conn_handle, struct os_mbuf **om,
+                  struct ble_sm_result *res)
+{
+    struct ble_sm_id_info *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_id_info *)(*om)->om_data;
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
+        proc->rx_key_flags &= ~BLE_SM_KE_F_ID_INFO;
+
+        memcpy(proc->peer_keys.irk, cmd->irk, 16);
+        proc->peer_keys.irk_valid = 1;
+
+        ble_sm_key_rxed(proc, res);
+    }
+
+    ble_hs_unlock();
+}
+
+static void
+ble_sm_id_addr_info_rx(uint16_t conn_handle, struct os_mbuf **om,
+                       struct ble_sm_result *res)
+{
+    struct ble_sm_id_addr_info *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_id_addr_info *)(*om)->om_data;
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
+        proc->rx_key_flags &= ~BLE_SM_KE_F_ADDR_INFO;
+        proc->peer_keys.addr_valid = 1;
+        proc->peer_keys.addr_type = cmd->addr_type;
+        memcpy(proc->peer_keys.addr, cmd->bd_addr, 6);
+
+        ble_sm_key_rxed(proc, res);
+    }
+
+    ble_hs_unlock();
+}
+
+static void
+ble_sm_sign_info_rx(uint16_t conn_handle, struct os_mbuf **om,
+                    struct ble_sm_result *res)
+{
+    struct ble_sm_sign_info *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    cmd = (struct ble_sm_sign_info *)(*om)->om_data;
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
+        proc->rx_key_flags &= ~BLE_SM_KE_F_SIGN_INFO;
+
+        memcpy(proc->peer_keys.csrk, cmd->sig_key, 16);
+        proc->peer_keys.csrk_valid = 1;
+
+        ble_sm_key_rxed(proc, res);
+    }
+
+    ble_hs_unlock();
+}
+
+/*****************************************************************************
+ * $fail                                                                     *
+ *****************************************************************************/
+
+static void
+ble_sm_fail_rx(uint16_t conn_handle, struct os_mbuf **om,
+               struct ble_sm_result *res)
+{
+    struct ble_sm_pair_fail *cmd;
+
+    res->enc_cb = 1;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status == 0) {
+        cmd = (struct ble_sm_pair_fail *)(*om)->om_data;
+
+        res->app_status = BLE_HS_SM_PEER_ERR(cmd->reason);
+    }
+}
+
+/*****************************************************************************
+ * $api                                                                      *
+ *****************************************************************************/
+
+/**
+ * Times out expired SM procedures.
+ *
+ * @return                      The number of ticks until this function should
+ *                                  be called again.
+ */
+int32_t
+ble_sm_timer(void)
+{
+    struct ble_sm_proc_list exp_list;
+    struct ble_sm_proc *proc;
+    int32_t ticks_until_exp;
+
+    /* Remove timed-out procedures from the main list and insert them into a
+     * temporary list.  This function also calculates the number of ticks until
+     * the next expiration will occur.
+     */
+    ticks_until_exp = ble_sm_extract_expired(&exp_list);
+
+    /* Notify application of each failure and free the corresponding procedure
+     * object.
+     * XXX: Mark connection as tainted; don't allow any subsequent SMP
+     * procedures without reconnect.
+     */
+    while ((proc = STAILQ_FIRST(&exp_list)) != NULL) {
+        ble_gap_enc_event(proc->conn_handle, BLE_HS_ETIMEOUT, 0);
+
+        STAILQ_REMOVE_HEAD(&exp_list, next);
+        ble_sm_proc_free(proc);
+    }
+
+    return ticks_until_exp;
+}
+
+/**
+ * Initiates the pairing procedure for the specified connection.
+ */
+int
+ble_sm_pair_initiate(uint16_t conn_handle)
+{
+    struct ble_sm_result res;
+    struct ble_sm_proc *proc;
+    int rc;
+
+    memset(&res, 0, sizeof(res));
+
+    /* Make sure a procedure isn't already in progress for this connection. */
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL);
+    ble_hs_unlock();
+
+    if (proc != NULL) {
+        res.app_status = BLE_HS_EALREADY;
+        return BLE_HS_EALREADY;
+    }
+
+    /* Check if there is storage capacity for a new bond.  If there isn't, ask
+     * the application to make room.
+     */
+    rc = ble_sm_chk_store_overflow(conn_handle);
+    if (rc != 0) {
+        return rc;
+    }
+
+    proc = ble_sm_proc_alloc();
+    if (proc == NULL) {
+        res.app_status = BLE_HS_ENOMEM;
+    } else {
+        proc->conn_handle = conn_handle;
+        proc->state = BLE_SM_PROC_STATE_PAIR;
+        proc->flags |= BLE_SM_PROC_F_INITIATOR;
+
+        ble_hs_lock();
+        ble_sm_insert(proc);
+        ble_hs_unlock();
+
+        res.execute = 1;
+    }
+
+    if (proc != NULL) {
+        ble_sm_process_result(conn_handle, &res);
+    }
+
+    return res.app_status;
+}
+
+int
+ble_sm_slave_initiate(uint16_t conn_handle)
+{
+    struct ble_sm_result res;
+    struct ble_sm_proc *proc;
+
+    memset(&res, 0, sizeof(res));
+
+    ble_hs_lock();
+
+    /* Make sure a procedure isn't already in progress for this connection. */
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL);
+    if (proc != NULL) {
+        res.app_status = BLE_HS_EALREADY;
+
+        /* Set pointer to null so that existing entry doesn't get freed. */
+        proc = NULL;
+    } else {
+        proc = ble_sm_proc_alloc();
+        if (proc == NULL) {
+            res.app_status = BLE_HS_ENOMEM;
+        } else {
+            proc->conn_handle = conn_handle;
+            proc->state = BLE_SM_PROC_STATE_SEC_REQ;
+            ble_sm_insert(proc);
+
+            res.execute = 1;
+        }
+    }
+
+    ble_hs_unlock();
+
+    if (proc != NULL) {
+        ble_sm_process_result(conn_handle, &res);
+    }
+
+    return res.app_status;
+}
+
+/**
+ * Initiates the encryption procedure for the specified connection.
+ */
+int
+ble_sm_enc_initiate(uint16_t conn_handle, uint8_t key_size,
+                    const uint8_t *ltk, uint16_t ediv,
+                    uint64_t rand_val, int auth)
+{
+    struct ble_sm_result res;
+    struct ble_sm_proc *proc;
+    struct hci_start_encrypt cmd;
+
+    memset(&res, 0, sizeof res);
+
+    /* Make sure a procedure isn't already in progress for this connection. */
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL);
+    if (proc != NULL) {
+        res.app_status = BLE_HS_EALREADY;
+
+        /* Set pointer to null so that existing entry doesn't get freed. */
+        proc = NULL;
+    } else {
+        proc = ble_sm_proc_alloc();
+        if (proc == NULL) {
+            res.app_status = BLE_HS_ENOMEM;
+        } else {
+            proc->conn_handle = conn_handle;
+            proc->key_size = key_size;
+            proc->state = BLE_SM_PROC_STATE_ENC_RESTORE;
+            proc->flags |= BLE_SM_PROC_F_INITIATOR;
+            if (auth) {
+                proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+            }
+            ble_sm_insert(proc);
+
+            cmd.connection_handle = conn_handle;
+            cmd.encrypted_diversifier = ediv;
+            cmd.random_number = rand_val;
+            memcpy(cmd.long_term_key, ltk, sizeof cmd.long_term_key);
+
+            res.execute = 1;
+            res.state_arg = &cmd;
+        }
+    }
+
+    ble_hs_unlock();
+
+    ble_sm_process_result(conn_handle, &res);
+
+    return res.app_status;
+}
+
+static int
+ble_sm_rx(struct ble_l2cap_chan *chan)
+{
+    struct ble_sm_result res;
+    ble_sm_rx_fn *rx_cb;
+    uint8_t op;
+    uint16_t conn_handle;
+    struct os_mbuf **om;
+    int rc;
+
+    STATS_INC(ble_l2cap_stats, sm_rx);
+
+    conn_handle = ble_l2cap_get_conn_handle(chan);
+    if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    om = &chan->rx_buf;
+    BLE_HS_DBG_ASSERT(*om != NULL);
+
+    rc = os_mbuf_copydata(*om, 0, 1, &op);
+    if (rc != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    /* Strip L2CAP SM header from the front of the mbuf. */
+    os_mbuf_adj(*om, 1);
+
+    rx_cb = ble_sm_dispatch_get(op);
+    if (rx_cb != NULL) {
+        memset(&res, 0, sizeof res);
+
+        rx_cb(conn_handle, om, &res);
+        ble_sm_process_result(conn_handle, &res);
+        rc = res.app_status;
+    } else {
+        rc = BLE_HS_ENOTSUP;
+    }
+
+    return rc;
+}
+
+int
+ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey)
+{
+    struct ble_sm_result res;
+    struct ble_sm_proc *proc;
+    int rc;
+    uint8_t action;
+
+    memset(&res, 0, sizeof res);
+
+    ble_hs_lock();
+
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL);
+    if (proc == NULL) {
+        rc = BLE_HS_ENOENT;
+    } else if (proc->flags & BLE_SM_PROC_F_IO_INJECTED) {
+        rc = BLE_HS_EALREADY;
+    } else if ((ble_sm_io_action(proc, &action) == 0) && pkey->action != action) {
+        /* Application provided incorrect IO type. */
+        rc = BLE_HS_EINVAL;
+    } else if (ble_sm_ioact_state(pkey->action) != proc->state) {
+        /* Procedure is not ready for user input. */
+        rc = BLE_HS_EINVAL;
+    } else {
+        /* Assume valid input. */
+        rc = 0;
+
+        switch (pkey->action) {
+        case BLE_SM_IOACT_OOB:
+            proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
+            memcpy(proc->tk, pkey->oob, 16);
+            if ((proc->flags & BLE_SM_PROC_F_INITIATOR) ||
+                (proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO)) {
+
+                res.execute = 1;
+            }
+            break;
+
+        case BLE_SM_IOACT_INPUT:
+        case BLE_SM_IOACT_DISP:
+            if (pkey->passkey > 999999) {
+                rc = BLE_HS_EINVAL;
+            } else {
+                proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
+                memset(proc->tk, 0, 16);
+                proc->tk[0] = (pkey->passkey >> 0) & 0xff;
+                proc->tk[1] = (pkey->passkey >> 8) & 0xff;
+                proc->tk[2] = (pkey->passkey >> 16) & 0xff;
+                proc->tk[3] = (pkey->passkey >> 24) & 0xff;
+                if ((proc->flags & BLE_SM_PROC_F_INITIATOR) ||
+                    (proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO)) {
+
+                    res.execute = 1;
+                }
+            }
+            break;
+
+        case BLE_SM_IOACT_NUMCMP:
+            if (!pkey->numcmp_accept) {
+                res.app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_NUMCMP);
+                res.sm_err = BLE_SM_ERR_NUMCMP;
+            } else {
+                proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
+                if (proc->flags & BLE_SM_PROC_F_INITIATOR ||
+                    proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO) {
+
+                    res.execute = 1;
+                }
+            }
+            break;
+
+#if MYNEWT_VAL(BLE_SM_SC)
+        case BLE_SM_IOACT_OOB_SC:
+            if (!ble_sm_sc_oob_data_check(proc,
+                                          (pkey->oob_sc_data.local != NULL),
+                                          (pkey->oob_sc_data.remote != NULL))) {
+                res.app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_OOB);
+                res.sm_err = BLE_SM_ERR_OOB;
+            } else {
+                proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
+                proc->oob_data_local = pkey->oob_sc_data.local;
+                proc->oob_data_remote = pkey->oob_sc_data.remote;
+
+                /* Execute Confirm step */
+                ble_sm_sc_oob_confirm(proc, &res);
+            }
+            break;
+#endif
+
+        default:
+            BLE_HS_DBG_ASSERT(0);
+            rc = BLE_HS_EINVAL;
+            break;
+        }
+    }
+
+    ble_hs_unlock();
+
+    /* If application provided invalid input, return error without modifying
+     * SMP state.
+     */
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_sm_process_result(conn_handle, &res);
+    return res.app_status;
+}
+
+void
+ble_sm_connection_broken(uint16_t conn_handle)
+{
+    struct ble_sm_result res;
+
+    memset(&res, 0, sizeof res);
+    res.app_status = BLE_HS_ENOTCONN;
+    res.enc_cb = 1;
+
+    ble_sm_process_result(conn_handle, &res);
+}
+
+int
+ble_sm_init(void)
+{
+    int rc;
+
+    STAILQ_INIT(&ble_sm_procs);
+
+    rc = os_mempool_init(&ble_sm_proc_pool,
+                         MYNEWT_VAL(BLE_SM_MAX_PROCS),
+                         sizeof (struct ble_sm_proc),
+                         ble_sm_proc_mem,
+                         "ble_sm_proc_pool");
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_sm_sc_init();
+
+    return 0;
+}
+#else
+/* if pairing is not supported it is only needed to reply with Pairing
+ * Failed with 'Pairing not Supported' reason so this function can be very
+ * simple
+ */
+static int
+ble_sm_rx(struct ble_l2cap_chan *chan)
+{
+    struct ble_sm_pair_fail *cmd;
+    struct os_mbuf *txom;
+    uint16_t handle;
+    int rc;
+
+    handle = ble_l2cap_get_conn_handle(chan);
+    if (!handle) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_FAIL, sizeof(*cmd), &txom);
+    if (cmd == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    cmd->reason = BLE_SM_ERR_PAIR_NOT_SUPP;
+
+    ble_hs_lock();
+    rc = ble_sm_tx(handle, txom);
+    ble_hs_unlock();
+
+    return rc;
+}
+#endif
+
+struct ble_l2cap_chan *
+ble_sm_create_chan(uint16_t conn_handle)
+{
+    struct ble_l2cap_chan *chan;
+
+    chan = ble_l2cap_chan_alloc(conn_handle);
+    if (chan == NULL) {
+        return NULL;
+    }
+
+    chan->scid = BLE_L2CAP_CID_SM;
+    chan->dcid = BLE_L2CAP_CID_SM;
+    chan->my_mtu = BLE_SM_MTU;
+    chan->rx_fn = ble_sm_rx;
+
+    return chan;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_sm_alg.c b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_alg.c
new file mode 100644
index 00000000..148995c8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_alg.c
@@ -0,0 +1,530 @@
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/* These functions are adapted from the Intel Zephyr BLE security manager
+ * code.
+ */
+
+#include <inttypes.h>
+#include <string.h>
+#include "syscfg/syscfg.h"
+#include "nimble/nimble_opt.h"
+
+#if NIMBLE_BLE_SM
+
+#include "nimble/ble.h"
+#include "ble_hs_priv.h"
+#include "tinycrypt/aes.h"
+#include "tinycrypt/constants.h"
+#include "tinycrypt/utils.h"
+
+#if MYNEWT_VAL(BLE_SM_SC)
+#include "tinycrypt/cmac_mode.h"
+#include "tinycrypt/ecc_dh.h"
+#if MYNEWT_VAL(TRNG)
+#include "trng/trng.h"
+#endif
+#endif
+
+#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(TRNG)
+static struct trng_dev *g_trng;
+#endif
+
+static void
+ble_sm_alg_xor_128(const uint8_t *p, const uint8_t *q, uint8_t *r)
+{
+    int i;
+
+    for (i = 0; i < 16; i++) {
+        r[i] = p[i] ^ q[i];
+    }
+}
+
+static int
+ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
+                   uint8_t *enc_data)
+{
+    struct tc_aes_key_sched_struct s;
+    uint8_t tmp[16];
+
+    swap_buf(tmp, key, 16);
+
+    if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    swap_buf(tmp, plaintext, 16);
+
+    if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    swap_in_place(enc_data, 16);
+
+    return 0;
+}
+
+int
+ble_sm_alg_s1(const uint8_t *k, const uint8_t *r1, const uint8_t *r2,
+              uint8_t *out)
+{
+    int rc;
+
+    /* The most significant 64-bits of r1 are discarded to generate
+     * r1' and the most significant 64-bits of r2 are discarded to
+     * generate r2'.
+     * r1' is concatenated with r2' to generate r' which is used as
+     * the 128-bit input parameter plaintextData to security function e:
+     *
+     *    r' = r1' || r2'
+     */
+    memcpy(out, r2, 8);
+    memcpy(out + 8, r1, 8);
+
+    /* s1(k, r1 , r2) = e(k, r') */
+    rc = ble_sm_alg_encrypt(k, out, out);
+    if (rc != 0) {
+        return rc;
+    }
+
+    BLE_HS_LOG(DEBUG, "ble_sm_alg_s1()\n    k=");
+    ble_hs_log_flat_buf(k, 16);
+    BLE_HS_LOG(DEBUG, "\n    r1=");
+    ble_hs_log_flat_buf(r1, 16);
+    BLE_HS_LOG(DEBUG, "\n    r2=");
+    ble_hs_log_flat_buf(r2, 16);
+    BLE_HS_LOG(DEBUG, "\n    out=");
+    ble_hs_log_flat_buf(out, 16);
+    BLE_HS_LOG(DEBUG, "\n");
+
+    return 0;
+}
+
+int
+ble_sm_alg_c1(const uint8_t *k, const uint8_t *r,
+              const uint8_t *preq, const uint8_t *pres,
+              uint8_t iat, uint8_t rat,
+              const uint8_t *ia, const uint8_t *ra,
+              uint8_t *out_enc_data)
+{
+    uint8_t p1[16], p2[16];
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "ble_sm_alg_c1()\n    k=");
+    ble_hs_log_flat_buf(k, 16);
+    BLE_HS_LOG(DEBUG, "\n    r=");
+    ble_hs_log_flat_buf(r, 16);
+    BLE_HS_LOG(DEBUG, "\n    iat=%d rat=%d", iat, rat);
+    BLE_HS_LOG(DEBUG, "\n    ia=");
+    ble_hs_log_flat_buf(ia, 6);
+    BLE_HS_LOG(DEBUG, "\n    ra=");
+    ble_hs_log_flat_buf(ra, 6);
+    BLE_HS_LOG(DEBUG, "\n    preq=");
+    ble_hs_log_flat_buf(preq, 7);
+    BLE_HS_LOG(DEBUG, "\n    pres=");
+    ble_hs_log_flat_buf(pres, 7);
+
+    /* pres, preq, rat and iat are concatenated to generate p1 */
+    p1[0] = iat;
+    p1[1] = rat;
+    memcpy(p1 + 2, preq, 7);
+    memcpy(p1 + 9, pres, 7);
+
+    BLE_HS_LOG(DEBUG, "\n    p1=");
+    ble_hs_log_flat_buf(p1, sizeof p1);
+
+    /* c1 = e(k, e(k, r XOR p1) XOR p2) */
+
+    /* Using out_enc_data as temporary output buffer */
+    ble_sm_alg_xor_128(r, p1, out_enc_data);
+
+    rc = ble_sm_alg_encrypt(k, out_enc_data, out_enc_data);
+    if (rc != 0) {
+        rc = BLE_HS_EUNKNOWN;
+        goto done;
+    }
+
+    /* ra is concatenated with ia and padding to generate p2 */
+    memcpy(p2, ra, 6);
+    memcpy(p2 + 6, ia, 6);
+    memset(p2 + 12, 0, 4);
+
+    BLE_HS_LOG(DEBUG, "\n    p2=");
+    ble_hs_log_flat_buf(p2, sizeof p2);
+
+    ble_sm_alg_xor_128(out_enc_data, p2, out_enc_data);
+
+    rc = ble_sm_alg_encrypt(k, out_enc_data, out_enc_data);
+    if (rc != 0) {
+        rc = BLE_HS_EUNKNOWN;
+        goto done;
+    }
+
+    BLE_HS_LOG(DEBUG, "\n    out_enc_data=");
+    ble_hs_log_flat_buf(out_enc_data, 16);
+
+    rc = 0;
+
+done:
+    BLE_HS_LOG(DEBUG, "\n    rc=%d\n", rc);
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_SM_SC)
+
+static void
+ble_sm_alg_log_buf(const char *name, const uint8_t *buf, int len)
+{
+    BLE_HS_LOG(DEBUG, "    %s=", name);
+    ble_hs_log_flat_buf(buf, len);
+    BLE_HS_LOG(DEBUG, "\n");
+}
+
+/**
+ * Cypher based Message Authentication Code (CMAC) with AES 128 bit
+ *
+ * @param key                   128-bit key.
+ * @param in                    Message to be authenticated.
+ * @param len                   Length of the message in octets.
+ * @param out                   Output; message authentication code.
+ */
+static int
+ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len,
+                    uint8_t *out)
+{
+    struct tc_aes_key_sched_struct sched;
+    struct tc_cmac_struct state;
+
+    if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    if (tc_cmac_update(&state, in, len) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    if (tc_cmac_final(out, &state) == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    return 0;
+}
+
+int
+ble_sm_alg_f4(const uint8_t *u, const uint8_t *v, const uint8_t *x,
+              uint8_t z, uint8_t *out_enc_data)
+{
+    uint8_t xs[16];
+    uint8_t m[65];
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "ble_sm_alg_f4()\n    u=");
+    ble_hs_log_flat_buf(u, 32);
+    BLE_HS_LOG(DEBUG, "\n    v=");
+    ble_hs_log_flat_buf(v, 32);
+    BLE_HS_LOG(DEBUG, "\n    x=");
+    ble_hs_log_flat_buf(x, 16);
+    BLE_HS_LOG(DEBUG, "\n    z=0x%02x\n", z);
+
+    /*
+     * U, V and Z are concatenated and used as input m to the function
+     * AES-CMAC and X is used as the key k.
+     *
+     * Core Spec 4.2 Vol 3 Part H 2.2.5
+     *
+     * note:
+     * ble_sm_alg_aes_cmac uses BE data; ble_sm_alg_f4 accepts LE so we swap.
+     */
+    swap_buf(m, u, 32);
+    swap_buf(m + 32, v, 32);
+    m[64] = z;
+
+    swap_buf(xs, x, 16);
+
+    rc = ble_sm_alg_aes_cmac(xs, m, sizeof(m), out_enc_data);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    swap_in_place(out_enc_data, 16);
+
+    BLE_HS_LOG(DEBUG, "    out_enc_data=");
+    ble_hs_log_flat_buf(out_enc_data, 16);
+    BLE_HS_LOG(DEBUG, "\n");
+
+    return 0;
+}
+
+int
+ble_sm_alg_f5(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
+              uint8_t a1t, const uint8_t *a1, uint8_t a2t, const uint8_t *a2,
+              uint8_t *mackey, uint8_t *ltk)
+{
+    static const uint8_t salt[16] = { 0x6c, 0x88, 0x83, 0x91, 0xaa, 0xf5,
+                      0xa5, 0x38, 0x60, 0x37, 0x0b, 0xdb,
+                      0x5a, 0x60, 0x83, 0xbe };
+    uint8_t m[53] = {
+        0x00, /* counter */
+        0x62, 0x74, 0x6c, 0x65, /* keyID */
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /*n1*/
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /*2*/
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* a1 */
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* a2 */
+        0x01, 0x00 /* length */
+    };
+    uint8_t ws[32];
+    uint8_t t[16];
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "ble_sm_alg_f5()\n");
+    ble_sm_alg_log_buf("w", w, 32);
+    ble_sm_alg_log_buf("n1", n1, 16);
+    ble_sm_alg_log_buf("n2", n2, 16);
+
+    swap_buf(ws, w, 32);
+
+    rc = ble_sm_alg_aes_cmac(salt, ws, 32, t);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    ble_sm_alg_log_buf("t", t, 16);
+
+    swap_buf(m + 5, n1, 16);
+    swap_buf(m + 21, n2, 16);
+    m[37] = a1t;
+    swap_buf(m + 38, a1, 6);
+    m[44] = a2t;
+    swap_buf(m + 45, a2, 6);
+
+    rc = ble_sm_alg_aes_cmac(t, m, sizeof(m), mackey);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    ble_sm_alg_log_buf("mackey", mackey, 16);
+
+    swap_in_place(mackey, 16);
+
+    /* Counter for ltk is 1. */
+    m[0] = 0x01;
+
+    rc = ble_sm_alg_aes_cmac(t, m, sizeof(m), ltk);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    ble_sm_alg_log_buf("ltk", ltk, 16);
+
+    swap_in_place(ltk, 16);
+
+    return 0;
+}
+
+int
+ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
+              const uint8_t *r, const uint8_t *iocap, uint8_t a1t,
+              const uint8_t *a1, uint8_t a2t, const uint8_t *a2,
+              uint8_t *check)
+{
+    uint8_t ws[16];
+    uint8_t m[65];
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "ble_sm_alg_f6()\n");
+    ble_sm_alg_log_buf("w", w, 16);
+    ble_sm_alg_log_buf("n1", n1, 16);
+    ble_sm_alg_log_buf("n2", n2, 16);
+    ble_sm_alg_log_buf("r", r, 16);
+    ble_sm_alg_log_buf("iocap", iocap, 3);
+    ble_sm_alg_log_buf("a1t", &a1t, 1);
+    ble_sm_alg_log_buf("a1", a1, 6);
+    ble_sm_alg_log_buf("a2t", &a2t, 1);
+    ble_sm_alg_log_buf("a2", a2, 6);
+
+    swap_buf(m, n1, 16);
+    swap_buf(m + 16, n2, 16);
+    swap_buf(m + 32, r, 16);
+    swap_buf(m + 48, iocap, 3);
+
+    m[51] = a1t;
+    memcpy(m + 52, a1, 6);
+    swap_buf(m + 52, a1, 6);
+
+    m[58] = a2t;
+    memcpy(m + 59, a2, 6);
+    swap_buf(m + 59, a2, 6);
+
+    swap_buf(ws, w, 16);
+
+    rc = ble_sm_alg_aes_cmac(ws, m, sizeof(m), check);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    ble_sm_alg_log_buf("res", check, 16);
+
+    swap_in_place(check, 16);
+
+    return 0;
+}
+
+int
+ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x,
+              const uint8_t *y, uint32_t *passkey)
+{
+    uint8_t m[80], xs[16];
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "ble_sm_alg_g2()\n");
+    ble_sm_alg_log_buf("u", u, 32);
+    ble_sm_alg_log_buf("v", v, 32);
+    ble_sm_alg_log_buf("x", x, 16);
+    ble_sm_alg_log_buf("y", y, 16);
+
+    swap_buf(m, u, 32);
+    swap_buf(m + 32, v, 32);
+    swap_buf(m + 64, y, 16);
+
+    swap_buf(xs, x, 16);
+
+    /* reuse xs (key) as buffer for result */
+    rc = ble_sm_alg_aes_cmac(xs, m, sizeof(m), xs);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    ble_sm_alg_log_buf("res", xs, 16);
+
+    *passkey = get_be32(xs + 12) % 1000000;
+    BLE_HS_LOG(DEBUG, "    passkey=%u\n", *passkey);
+
+    return 0;
+}
+
+int
+ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y,
+                     const uint8_t *our_priv_key, uint8_t *out_dhkey)
+{
+    uint8_t dh[32];
+    uint8_t pk[64];
+    uint8_t priv[32];
+    int rc;
+
+    swap_buf(pk, peer_pub_key_x, 32);
+    swap_buf(&pk[32], peer_pub_key_y, 32);
+    swap_buf(priv, our_priv_key, 32);
+
+    if (uECC_valid_public_key(pk, &curve_secp256r1) < 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    rc = uECC_shared_secret(pk, priv, dh, &curve_secp256r1);
+    if (rc == TC_CRYPTO_FAIL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    swap_buf(out_dhkey, dh, 32);
+
+    return 0;
+}
+
+/* based on Core Specification 4.2 Vol 3. Part H 2.3.5.6.1 */
+static const uint8_t ble_sm_alg_dbg_priv_key[32] = {
+    0x3f, 0x49, 0xf6, 0xd4, 0xa3, 0xc5, 0x5f, 0x38, 0x74, 0xc9, 0xb3, 0xe3,
+    0xd2, 0x10, 0x3f, 0x50, 0x4a, 0xff, 0x60, 0x7b, 0xeb, 0x40, 0xb7, 0x99,
+    0x58, 0x99, 0xb8, 0xa6, 0xcd, 0x3c, 0x1a, 0xbd
+};
+
+#if MYNEWT_VAL(BLE_SM_SC_DEBUG_KEYS)
+static const uint8_t ble_sm_alg_dbg_pub_key[64] = {
+    /* X */
+    0x20, 0xb0, 0x03, 0xd2, 0xf2, 0x97, 0xbe, 0x2c, 0x5e, 0x2c, 0x83, 0xa7,
+    0xe9, 0xf9, 0xa5, 0xb9, 0xef, 0xf4, 0x91, 0x11, 0xac, 0xf4, 0xfd, 0xdb,
+    0xcc, 0x03, 0x01, 0x48, 0x0e, 0x35, 0x9d, 0xe6,
+    /* Y */
+    0xdc, 0x80, 0x9c, 0x49, 0x65, 0x2a, 0xeb, 0x6d, 0x63, 0x32, 0x9a, 0xbf,
+    0x5a, 0x52, 0x15, 0x5c, 0x76, 0x63, 0x45, 0xc2, 0x8f, 0xed, 0x30, 0x24,
+    0x74, 0x1c, 0x8e, 0xd0, 0x15, 0x89, 0xd2, 0x8b,
+};
+#endif
+
+/**
+ * pub: 64 bytes
+ * priv: 32 bytes
+ */
+int
+ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv)
+{
+#if MYNEWT_VAL(BLE_SM_SC_DEBUG_KEYS)
+    swap_buf(pub, ble_sm_alg_dbg_pub_key, 32);
+    swap_buf(&pub[32], &ble_sm_alg_dbg_pub_key[32], 32);
+    swap_buf(priv, ble_sm_alg_dbg_priv_key, 32);
+#else
+    uint8_t pk[64];
+
+    do {
+        if (uECC_make_key(pk, priv, &curve_secp256r1) != TC_CRYPTO_SUCCESS) {
+            return BLE_HS_EUNKNOWN;
+        }
+
+        /* Make sure generated key isn't debug key. */
+    } while (memcmp(priv, ble_sm_alg_dbg_priv_key, 32) == 0);
+
+    swap_buf(pub, pk, 32);
+    swap_buf(&pub[32], &pk[32], 32);
+    swap_in_place(priv, 32);
+#endif
+
+    return 0;
+}
+
+/* used by uECC to get random data */
+static int
+ble_sm_alg_rand(uint8_t *dst, unsigned int size)
+{
+#if MYNEWT_VAL(TRNG)
+    size_t num;
+
+    if (!g_trng) {
+        g_trng = (struct trng_dev *)os_dev_open("trng", OS_WAIT_FOREVER, NULL);
+        assert(g_trng);
+    }
+
+    while (size) {
+        num = trng_read(g_trng, dst, size);
+        dst += num;
+        size -= num;
+    }
+#else
+    if (ble_hs_hci_util_rand(dst, size)) {
+        return 0;
+    }
+#endif
+
+    return 1;
+}
+
+void
+ble_sm_alg_ecc_init(void)
+{
+    uECC_set_rng(ble_sm_alg_rand);
+}
+
+#endif
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_sm_cmd.c b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_cmd.c
new file mode 100644
index 00000000..5eef798d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_cmd.c
@@ -0,0 +1,63 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_priv.h"
+
+void *
+ble_sm_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom)
+{
+    struct ble_sm_hdr *hdr;
+
+    *txom = ble_hs_mbuf_l2cap_pkt();
+    if (*txom == NULL) {
+        return NULL;
+    }
+
+    if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) {
+        os_mbuf_free_chain(*txom);
+        return NULL;
+    }
+
+    hdr = (struct ble_sm_hdr *)(*txom)->om_data;
+
+    hdr->opcode = opcode;
+
+    return hdr->data;
+}
+
+/* this function consumes tx os_mbuf */
+int
+ble_sm_tx(uint16_t conn_handle, struct os_mbuf *txom)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    STATS_INC(ble_l2cap_stats, sm_tx);
+
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SM,
+                                    &conn, &chan);
+    return ble_l2cap_tx(conn, chan, txom);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_sm_lgcy.c b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_lgcy.c
new file mode 100644
index 00000000..bb2d66d5
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_lgcy.c
@@ -0,0 +1,254 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_priv.h"
+
+#if MYNEWT_VAL(BLE_SM_LEGACY)
+
+/**
+ * Create some shortened names for the passkey actions so that the table is
+ * easier to read.
+ */
+#define IOACT_NONE  BLE_SM_IOACT_NONE
+#define IOACT_OOB   BLE_SM_IOACT_OOB
+#define IOACT_INPUT BLE_SM_IOACT_INPUT
+#define IOACT_DISP  BLE_SM_IOACT_DISP
+
+/* This is the initiator passkey action action dpeneding on the io
+ * capabilties of both parties
+ */
+static const uint8_t ble_sm_lgcy_init_ioa[5 /*resp*/ ][5 /*init*/ ] =
+{
+    {IOACT_NONE,    IOACT_NONE,   IOACT_INPUT, IOACT_NONE, IOACT_INPUT},
+    {IOACT_NONE,    IOACT_NONE,   IOACT_INPUT, IOACT_NONE, IOACT_INPUT},
+    {IOACT_DISP,    IOACT_DISP,   IOACT_INPUT, IOACT_NONE, IOACT_DISP},
+    {IOACT_NONE,    IOACT_NONE,   IOACT_NONE,  IOACT_NONE, IOACT_NONE},
+    {IOACT_DISP,    IOACT_DISP,   IOACT_INPUT, IOACT_NONE, IOACT_DISP},
+};
+
+/* This is the responder passkey action action depending on the io
+ * capabilities of both parties
+ */
+static const uint8_t ble_sm_lgcy_resp_ioa[5 /*resp*/ ][5 /*init*/ ] =
+{
+    {IOACT_NONE,    IOACT_NONE,   IOACT_DISP,  IOACT_NONE, IOACT_DISP},
+    {IOACT_NONE,    IOACT_NONE,   IOACT_DISP,  IOACT_NONE, IOACT_DISP},
+    {IOACT_INPUT,   IOACT_INPUT,  IOACT_INPUT, IOACT_NONE, IOACT_INPUT},
+    {IOACT_NONE,    IOACT_NONE,   IOACT_NONE,  IOACT_NONE, IOACT_NONE},
+    {IOACT_INPUT,   IOACT_INPUT,  IOACT_DISP,  IOACT_NONE, IOACT_INPUT},
+};
+
+int
+ble_sm_lgcy_io_action(struct ble_sm_proc *proc, uint8_t *action)
+{
+    struct ble_sm_pair_cmd *pair_req, *pair_rsp;
+
+    pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
+    pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+
+    if (pair_req->oob_data_flag == BLE_SM_PAIR_OOB_YES &&
+        pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES) {
+        *action = BLE_SM_IOACT_OOB;
+    } else if (!(pair_req->authreq & BLE_SM_PAIR_AUTHREQ_MITM) &&
+               !(pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_MITM)) {
+
+        *action = BLE_SM_IOACT_NONE;
+    } else if (pair_req->io_cap >= BLE_SM_IO_CAP_RESERVED ||
+               pair_rsp->io_cap >= BLE_SM_IO_CAP_RESERVED) {
+        *action = BLE_SM_IOACT_NONE;
+    } else if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        *action = ble_sm_lgcy_init_ioa[pair_rsp->io_cap][pair_req->io_cap];
+    } else {
+        *action = ble_sm_lgcy_resp_ioa[pair_rsp->io_cap][pair_req->io_cap];
+    }
+
+    switch (*action) {
+    case BLE_SM_IOACT_NONE:
+        proc->pair_alg = BLE_SM_PAIR_ALG_JW;
+        break;
+
+    case BLE_SM_IOACT_OOB:
+        proc->pair_alg = BLE_SM_PAIR_ALG_OOB;
+        proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+        break;
+
+    case BLE_SM_IOACT_INPUT:
+    case BLE_SM_IOACT_DISP:
+        proc->pair_alg = BLE_SM_PAIR_ALG_PASSKEY;
+        proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+void
+ble_sm_lgcy_confirm_exec(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    struct ble_sm_pair_confirm *cmd;
+    struct os_mbuf *txom;
+    uint8_t ia[6];
+    uint8_t ra[6];
+    uint8_t iat;
+    uint8_t rat;
+    int rc;
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_CONFIRM, sizeof(*cmd), &txom);
+    if (cmd == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    ble_sm_ia_ra(proc, &iat, ia, &rat, ra);
+
+    rc = ble_sm_alg_c1(proc->tk, ble_sm_our_pair_rand(proc), proc->pair_req,
+                       proc->pair_rsp, iat, rat, ia, ra, cmd->value);
+    if (rc != 0) {
+        goto err;
+    }
+
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        goto err;
+    }
+
+    if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
+        proc->state = BLE_SM_PROC_STATE_RANDOM;
+    }
+
+    return;
+
+err:
+    if (txom) {
+        os_mbuf_free_chain(txom);
+    }
+
+    res->app_status = rc;
+    res->enc_cb = 1;
+    res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+}
+
+static int
+ble_sm_gen_stk(struct ble_sm_proc *proc)
+{
+    uint8_t key[16];
+    int rc;
+
+    rc = ble_sm_alg_s1(proc->tk, proc->rands, proc->randm, key);
+    if (rc != 0) {
+        return rc;
+    }
+
+    memcpy(proc->ltk, key, proc->key_size);
+
+    /* Ensure proper key size */
+    memset(proc->ltk + proc->key_size, 0, sizeof key - proc->key_size);
+
+    return 0;
+}
+
+void
+ble_sm_lgcy_random_exec(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    struct ble_sm_pair_random *cmd;
+    struct os_mbuf *txom;
+    int rc;
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_RANDOM, sizeof(*cmd), &txom);
+    if (cmd == NULL) {
+        res->app_status = BLE_HS_ENOMEM;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    memcpy(cmd->value, ble_sm_our_pair_rand(proc), 16);
+
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
+        proc->state = BLE_SM_PROC_STATE_LTK_START;
+    }
+}
+
+void
+ble_sm_lgcy_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    uint8_t confirm_val[16];
+    uint8_t ia[6];
+    uint8_t ra[6];
+    uint8_t iat;
+    uint8_t rat;
+    int rc;
+
+    ble_sm_ia_ra(proc, &iat, ia, &rat, ra);
+
+    rc = ble_sm_alg_c1(proc->tk, ble_sm_peer_pair_rand(proc), proc->pair_req,
+                       proc->pair_rsp, iat, rat, ia, ra, confirm_val);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    if (memcmp(proc->confirm_peer, confirm_val, 16) != 0) {
+        /* Random number mismatch. */
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH);
+        res->sm_err = BLE_SM_ERR_CONFIRM_MISMATCH;
+        res->enc_cb = 1;
+        return;
+    }
+
+    /* Generate the key. */
+    rc = ble_sm_gen_stk(proc);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        /* Send the start-encrypt HCI command to the controller.   For
+         * short-term key generation, we always set ediv and rand to 0.
+         * (Vol. 3, part H, 2.4.4.1).
+         */
+        proc->state = BLE_SM_PROC_STATE_ENC_START;
+    }
+
+    res->execute = 1;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_sm_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_priv.h
new file mode 100644
index 00000000..6d5601bf
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_priv.h
@@ -0,0 +1,423 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SM_PRIV_
+#define H_BLE_SM_PRIV_
+
+#include <inttypes.h>
+#include "syscfg/syscfg.h"
+#include "os/queue.h"
+#include "nimble/nimble_opt.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_gap_sec_state;
+struct hci_le_lt_key_req;
+struct hci_encrypt_change;
+
+#define BLE_SM_MTU                  65
+
+#define BLE_SM_OP_PAIR_REQ                      0x01
+#define BLE_SM_OP_PAIR_RSP                      0x02
+#define BLE_SM_OP_PAIR_CONFIRM                  0x03
+#define BLE_SM_OP_PAIR_RANDOM                   0x04
+#define BLE_SM_OP_PAIR_FAIL                     0x05
+#define BLE_SM_OP_ENC_INFO                      0x06
+#define BLE_SM_OP_MASTER_ID                     0x07
+#define BLE_SM_OP_IDENTITY_INFO                 0x08
+#define BLE_SM_OP_IDENTITY_ADDR_INFO            0x09
+#define BLE_SM_OP_SIGN_INFO                     0x0a
+#define BLE_SM_OP_SEC_REQ                       0x0b
+#define BLE_SM_OP_PAIR_PUBLIC_KEY               0x0c
+#define BLE_SM_OP_PAIR_DHKEY_CHECK              0x0d
+#define BLE_SM_OP_PAIR_KEYPRESS_NOTIFY          0x0e
+
+struct ble_sm_hdr {
+    uint8_t opcode;
+    uint8_t data[0];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x01/0x02 [req/rsp])         | 1                 |
+ * | IO Capability                      | 1                 |
+ * | OOB data flag                      | 1                 |
+ * | AuthReq                            | 1                 |
+ * | Maximum Encryption Key Size        | 1                 |
+ * | Initiator Key Distribution         | 1                 |
+ * | Responder Key Distribution         | 1                 |
+ */
+
+struct ble_sm_pair_cmd {
+    uint8_t io_cap;
+    uint8_t oob_data_flag;
+    uint8_t authreq;
+    uint8_t max_enc_key_size;
+    uint8_t init_key_dist;
+    uint8_t resp_key_dist;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x03)                        | 1                 |
+ * | Confirm Value                      | 16                |
+ */
+
+struct ble_sm_pair_confirm {
+    uint8_t value[16];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x04)                        | 1                 |
+ * | Random Value                       | 16                |
+ */
+struct ble_sm_pair_random {
+    uint8_t value[16];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x05)                        | 1                 |
+ * | Reason                             | 1                 |
+ */
+struct ble_sm_pair_fail {
+    uint8_t reason;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x06)                        | 1                 |
+ * | ltk                                | 16                |
+ */
+struct ble_sm_enc_info {
+    uint8_t ltk[16];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x07)                        | 1                 |
+ * | EDIV                               | 2                 |
+ * | RAND                               | 8                 |
+ */
+struct ble_sm_master_id {
+    uint16_t ediv;
+    uint64_t rand_val;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x08)                        | 1                 |
+ * | irk                                | 16                |
+ */
+struct ble_sm_id_info {
+    uint8_t irk[16];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x09)                        | 1                 |
+ * | addr_type                          | 1                 |
+ * | address                            | 6                 |
+ */
+struct ble_sm_id_addr_info {
+    uint8_t addr_type;
+    uint8_t bd_addr[6];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x0A)                        | 1                 |
+ * | csrk                               | 16                |
+ */
+struct ble_sm_sign_info {
+    uint8_t sig_key[16];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x0B)                        | 1                 |
+ * | authreq                            | 1                 |
+ */
+struct ble_sm_sec_req {
+    uint8_t authreq;
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x0c)                        | 1                 |
+ * | Public Key X                       | 32                |
+ * | Public Key Y                       | 32                |
+ */
+struct ble_sm_public_key {
+    uint8_t x[32];
+    uint8_t y[32];
+} __attribute__((packed));
+
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x0d)                        | 1                 |
+ * | DHKey Check                        | 16                |
+ */
+struct ble_sm_dhkey_check {
+    uint8_t value[16];
+} __attribute__((packed));
+
+#if NIMBLE_BLE_SM
+
+#define BLE_SM_PROC_STATE_NONE              ((uint8_t)-1)
+
+#define BLE_SM_PROC_STATE_PAIR              0
+#define BLE_SM_PROC_STATE_CONFIRM           1
+#define BLE_SM_PROC_STATE_RANDOM            2
+#define BLE_SM_PROC_STATE_LTK_START         3
+#define BLE_SM_PROC_STATE_LTK_RESTORE       4
+#define BLE_SM_PROC_STATE_ENC_START         5
+#define BLE_SM_PROC_STATE_ENC_RESTORE       6
+#define BLE_SM_PROC_STATE_KEY_EXCH          7
+#define BLE_SM_PROC_STATE_SEC_REQ           8
+#define BLE_SM_PROC_STATE_PUBLIC_KEY        9
+#define BLE_SM_PROC_STATE_DHKEY_CHECK       10
+#define BLE_SM_PROC_STATE_CNT               11
+
+#define BLE_SM_PROC_F_INITIATOR             0x01
+#define BLE_SM_PROC_F_IO_INJECTED           0x02
+#define BLE_SM_PROC_F_ADVANCE_ON_IO         0x04
+#define BLE_SM_PROC_F_AUTHENTICATED         0x08
+#define BLE_SM_PROC_F_SC                    0x10
+#define BLE_SM_PROC_F_BONDING               0x20
+
+#define BLE_SM_KE_F_ENC_INFO                0x01
+#define BLE_SM_KE_F_MASTER_ID               0x02
+#define BLE_SM_KE_F_ID_INFO                 0x04
+#define BLE_SM_KE_F_ADDR_INFO               0x08
+#define BLE_SM_KE_F_SIGN_INFO               0x10
+
+typedef uint8_t ble_sm_proc_flags;
+
+struct ble_sm_keys {
+    unsigned ltk_valid:1;
+    unsigned ediv_rand_valid:1;
+    unsigned irk_valid:1;
+    unsigned csrk_valid:1;
+    unsigned addr_valid:1;
+    uint16_t ediv;
+    uint64_t rand_val;
+    uint8_t addr_type;
+    uint8_t key_size;
+    uint8_t ltk[16];    /* Little endian. */
+    uint8_t irk[16];    /* Little endian. */
+    uint8_t csrk[16];   /* Little endian. */
+    uint8_t addr[6];    /* Little endian. */
+};
+
+struct ble_sm_proc {
+    STAILQ_ENTRY(ble_sm_proc) next;
+
+    ble_npl_time_t exp_os_ticks;
+    ble_sm_proc_flags flags;
+    uint16_t conn_handle;
+    uint8_t pair_alg;
+    uint8_t state;
+    uint8_t rx_key_flags;
+    uint8_t key_size;
+
+    uint8_t pair_req[sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_cmd)];
+    uint8_t pair_rsp[sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_cmd)];
+    uint8_t tk[16];
+    uint8_t confirm_peer[16];
+    uint8_t randm[16];
+    uint8_t rands[16];
+    uint8_t ltk[16]; /* Little endian. */
+    struct ble_sm_keys our_keys;
+    struct ble_sm_keys peer_keys;
+
+#if MYNEWT_VAL(BLE_SM_SC)
+    /* Secure connections. */
+    uint8_t passkey_bits_exchanged;
+    uint8_t ri;
+    struct ble_sm_public_key pub_key_peer;
+    uint8_t mackey[16];
+    uint8_t dhkey[32];
+    const struct ble_sm_sc_oob_data *oob_data_local;
+    const struct ble_sm_sc_oob_data *oob_data_remote;
+#endif
+};
+
+struct ble_sm_result {
+    int app_status;
+    uint8_t sm_err;
+    struct ble_gap_passkey_params passkey_params;
+    void *state_arg;
+    unsigned execute:1;
+    unsigned enc_cb:1;
+    unsigned persist_keys:1;
+    unsigned restore:1;
+};
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+void ble_sm_dbg_set_next_pair_rand(uint8_t *next_pair_rand);
+void ble_sm_dbg_set_next_ediv(uint16_t next_ediv);
+void ble_sm_dbg_set_next_master_id_rand(uint64_t next_master_id_rand);
+void ble_sm_dbg_set_next_ltk(uint8_t *next_ltk);
+void ble_sm_dbg_set_next_csrk(uint8_t *next_csrk);
+void ble_sm_dbg_set_sc_keys(uint8_t *pubkey, uint8_t *privkey);
+#endif
+
+int ble_sm_num_procs(void);
+
+int ble_sm_alg_s1(const uint8_t *k, const uint8_t *r1, const uint8_t *r2,
+                  uint8_t *out);
+int ble_sm_alg_c1(const uint8_t *k, const uint8_t *r,
+                  const uint8_t *preq, const uint8_t *pres,
+                  uint8_t iat, uint8_t rat,
+                  const uint8_t *ia, const uint8_t *ra,
+                  uint8_t *out_enc_data);
+int ble_sm_alg_f4(const uint8_t *u, const uint8_t *v, const uint8_t *x,
+                  uint8_t z, uint8_t *out_enc_data);
+int ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x,
+                  const uint8_t *y, uint32_t *passkey);
+int ble_sm_alg_f5(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
+                  uint8_t a1t, const uint8_t *a1, uint8_t a2t,
+                  const uint8_t *a2, uint8_t *mackey, uint8_t *ltk);
+int ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
+                  const uint8_t *r, const uint8_t *iocap, uint8_t a1t,
+                  const uint8_t *a1, uint8_t a2t, const uint8_t *a2,
+                  uint8_t *check);
+int ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x,
+                         const uint8_t *peer_pub_key_y,
+                         const uint8_t *our_priv_key, uint8_t *out_dhkey);
+int ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv);
+void ble_sm_alg_ecc_init(void);
+
+void ble_sm_enc_change_rx(const struct ble_hci_ev_enrypt_chg *ev);
+void ble_sm_enc_key_refresh_rx(const struct ble_hci_ev_enc_key_refresh *ev);
+int ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev);
+
+#if MYNEWT_VAL(BLE_SM_LEGACY)
+int ble_sm_lgcy_io_action(struct ble_sm_proc *proc, uint8_t *action);
+void ble_sm_lgcy_confirm_exec(struct ble_sm_proc *proc,
+                              struct ble_sm_result *res);
+void ble_sm_lgcy_random_exec(struct ble_sm_proc *proc,
+                             struct ble_sm_result *res);
+void ble_sm_lgcy_random_rx(struct ble_sm_proc *proc,
+                           struct ble_sm_result *res);
+#else
+#define ble_sm_lgcy_io_action(proc, action) (BLE_HS_ENOTSUP)
+#define ble_sm_lgcy_confirm_exec(proc, res)
+#define ble_sm_lgcy_random_exec(proc, res)
+#define ble_sm_lgcy_random_rx(proc, res)
+#endif
+
+#if MYNEWT_VAL(BLE_SM_SC)
+int ble_sm_sc_io_action(struct ble_sm_proc *proc, uint8_t *action);
+void ble_sm_sc_confirm_exec(struct ble_sm_proc *proc,
+                            struct ble_sm_result *res);
+void ble_sm_sc_random_exec(struct ble_sm_proc *proc,
+                           struct ble_sm_result *res);
+void ble_sm_sc_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res);
+void ble_sm_sc_public_key_exec(struct ble_sm_proc *proc,
+                               struct ble_sm_result *res,
+                               void *arg);
+void ble_sm_sc_public_key_rx(uint16_t conn_handle, struct os_mbuf **rxom,
+                             struct ble_sm_result *res);
+void ble_sm_sc_dhkey_check_exec(struct ble_sm_proc *proc,
+                                struct ble_sm_result *res, void *arg);
+void ble_sm_sc_dhkey_check_rx(uint16_t conn_handle, struct os_mbuf **rxom,
+                              struct ble_sm_result *res);
+bool ble_sm_sc_oob_data_check(struct ble_sm_proc *proc,
+                              bool oob_data_local_present,
+                              bool oob_data_remote_present);
+void ble_sm_sc_oob_confirm(struct ble_sm_proc *proc, struct ble_sm_result *res);
+void ble_sm_sc_init(void);
+#else
+#define ble_sm_sc_io_action(proc, action) (BLE_HS_ENOTSUP)
+#define ble_sm_sc_confirm_exec(proc, res)
+#define ble_sm_sc_random_exec(proc, res)
+#define ble_sm_sc_random_rx(proc, res)
+#define ble_sm_sc_public_key_exec(proc, res, arg)
+#define ble_sm_sc_public_key_rx(conn_handle, op, om, res)
+#define ble_sm_sc_dhkey_check_exec(proc, res, arg)
+#define ble_sm_sc_dhkey_check_rx(conn_handle, op, om, res)
+#define ble_sm_sc_init()
+
+#endif
+
+struct ble_sm_proc *ble_sm_proc_find(uint16_t conn_handle, uint8_t state,
+                                     int is_initiator,
+                                     struct ble_sm_proc **out_prev);
+int ble_sm_gen_pair_rand(uint8_t *pair_rand);
+uint8_t *ble_sm_our_pair_rand(struct ble_sm_proc *proc);
+uint8_t *ble_sm_peer_pair_rand(struct ble_sm_proc *proc);
+int ble_sm_ioact_state(uint8_t action);
+int ble_sm_proc_can_advance(struct ble_sm_proc *proc);
+void ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res);
+void ble_sm_confirm_advance(struct ble_sm_proc *proc);
+void ble_sm_ia_ra(struct ble_sm_proc *proc,
+                  uint8_t *out_iat, uint8_t *out_ia,
+                  uint8_t *out_rat, uint8_t *out_ra);
+
+int32_t ble_sm_timer(void);
+void ble_sm_connection_broken(uint16_t conn_handle);
+int ble_sm_pair_initiate(uint16_t conn_handle);
+int ble_sm_slave_initiate(uint16_t conn_handle);
+int ble_sm_enc_initiate(uint16_t conn_handle, uint8_t key_size,
+                        const uint8_t *ltk, uint16_t ediv,
+                        uint64_t rand_val, int auth);
+int ble_sm_init(void);
+#else
+
+#define ble_sm_enc_change_rx(evt) ((void)(evt))
+#define ble_sm_ltk_req_rx(evt) ((void)(evt))
+#define ble_sm_enc_key_refresh_rx(evt) ((void)(evt))
+
+#define ble_sm_timer() BLE_HS_FOREVER
+#define ble_sm_connection_broken(conn_handle)
+#define ble_sm_pair_initiate(conn_handle)   BLE_HS_ENOTSUP
+#define ble_sm_slave_initiate(conn_handle)  BLE_HS_ENOTSUP
+#define ble_sm_enc_initiate(conn_handle, keysize, ltk, ediv, rand_val, auth) \
+        BLE_HS_ENOTSUP
+
+#define ble_sm_init() 0
+
+#endif
+
+struct ble_l2cap_chan *ble_sm_create_chan(uint16_t handle);
+void *ble_sm_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom);
+int ble_sm_tx(uint16_t conn_handle, struct os_mbuf *txom);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_sm_sc.c b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_sc.c
new file mode 100644
index 00000000..562f33b5
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_sm_sc.c
@@ -0,0 +1,909 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_priv.h"
+#include "ble_sm_priv.h"
+
+#if MYNEWT_VAL(BLE_SM_SC)
+
+#define BLE_SM_SC_PASSKEY_BYTES     4
+#define BLE_SM_SC_PASSKEY_BITS      20
+
+static uint8_t ble_sm_sc_pub_key[64];
+static uint8_t ble_sm_sc_priv_key[32];
+
+/**
+ * Whether our public-private key pair has been generated.  We generate it on
+ * startup for now until we have a non-volatile storage mechanism.
+ */
+static uint8_t ble_sm_sc_keys_generated;
+
+/**
+ * Create some shortened names for the passkey actions so that the table is
+ * easier to read.
+ */
+#define IOACT_NONE      BLE_SM_IOACT_NONE
+#define IOACT_OOB       BLE_SM_IOACT_OOB
+#define IOACT_INPUT     BLE_SM_IOACT_INPUT
+#define IOACT_DISP      BLE_SM_IOACT_DISP
+#define IOACT_NUMCMP    BLE_SM_IOACT_NUMCMP
+
+/**
+ * This table expresses the required initiator IO action.  Inputs are:
+ *     o Responder IO capabilities (from pair response).
+ *     o Initiator IO capabilities (from pair request).
+ */
+static const uint8_t ble_sm_sc_init_ioa[5 /*resp*/ ][5 /*init*/ ] =
+{
+      /* init */
+/*r*/ {IOACT_NONE,    IOACT_NONE,   IOACT_INPUT, IOACT_NONE, IOACT_INPUT},
+/*e*/ {IOACT_NONE,    IOACT_NUMCMP, IOACT_INPUT, IOACT_NONE, IOACT_NUMCMP},
+/*s*/ {IOACT_DISP,    IOACT_DISP,   IOACT_INPUT, IOACT_NONE, IOACT_DISP},
+/*p*/ {IOACT_NONE,    IOACT_NONE,   IOACT_NONE,  IOACT_NONE, IOACT_NONE},
+      {IOACT_DISP,    IOACT_NUMCMP, IOACT_INPUT, IOACT_NONE, IOACT_NUMCMP},
+};
+
+/**
+ * This table expresses the required responder IO action.  Inputs are:
+ *     o Responder IO capabilities (from pair response).
+ *     o Initiator IO capabilities (from pair request).
+ */
+static const uint8_t ble_sm_sc_resp_ioa[5 /*resp*/ ][5 /*init*/ ] =
+{
+      /* init */
+/*r*/ {IOACT_NONE,    IOACT_NONE,   IOACT_DISP,  IOACT_NONE, IOACT_DISP},
+/*e*/ {IOACT_NONE,    IOACT_NUMCMP, IOACT_DISP,  IOACT_NONE, IOACT_NUMCMP},
+/*s*/ {IOACT_INPUT,   IOACT_INPUT,  IOACT_INPUT, IOACT_NONE, IOACT_INPUT},
+/*p*/ {IOACT_NONE,    IOACT_NONE,   IOACT_NONE,  IOACT_NONE, IOACT_NONE},
+      {IOACT_INPUT,   IOACT_NUMCMP, IOACT_DISP,  IOACT_NONE, IOACT_NUMCMP},
+};
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+
+static uint8_t ble_sm_dbg_sc_pub_key[64];
+static uint8_t ble_sm_dbg_sc_priv_key[32];
+static uint8_t ble_sm_dbg_sc_keys_set;
+
+void
+ble_sm_dbg_set_sc_keys(uint8_t *pubkey, uint8_t *privkey)
+{
+    memcpy(ble_sm_dbg_sc_pub_key, pubkey,
+           sizeof ble_sm_dbg_sc_pub_key);
+    memcpy(ble_sm_dbg_sc_priv_key, privkey,
+           sizeof ble_sm_dbg_sc_priv_key);
+    ble_sm_dbg_sc_keys_set = 1;
+}
+
+#endif
+
+int
+ble_sm_sc_io_action(struct ble_sm_proc *proc, uint8_t *action)
+{
+    struct ble_sm_pair_cmd *pair_req, *pair_rsp;
+
+    pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
+    pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+
+    if (pair_req->oob_data_flag == BLE_SM_PAIR_OOB_YES ||
+        pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES) {
+        *action = BLE_SM_IOACT_OOB_SC;
+    } else if (!(pair_req->authreq & BLE_SM_PAIR_AUTHREQ_MITM) &&
+               !(pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_MITM)) {
+
+        *action = BLE_SM_IOACT_NONE;
+    } else if (pair_req->io_cap >= BLE_SM_IO_CAP_RESERVED ||
+               pair_rsp->io_cap >= BLE_SM_IO_CAP_RESERVED) {
+        *action = BLE_SM_IOACT_NONE;
+    } else if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        *action = ble_sm_sc_init_ioa[pair_rsp->io_cap][pair_req->io_cap];
+    } else {
+        *action = ble_sm_sc_resp_ioa[pair_rsp->io_cap][pair_req->io_cap];
+    }
+
+    switch (*action) {
+    case BLE_SM_IOACT_NONE:
+        proc->pair_alg = BLE_SM_PAIR_ALG_JW;
+        break;
+
+    case BLE_SM_IOACT_OOB_SC:
+        proc->pair_alg = BLE_SM_PAIR_ALG_OOB;
+        proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+        break;
+
+    case BLE_SM_IOACT_INPUT:
+    case BLE_SM_IOACT_DISP:
+        proc->pair_alg = BLE_SM_PAIR_ALG_PASSKEY;
+        proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+        break;
+
+    case BLE_SM_IOACT_NUMCMP:
+        proc->pair_alg = BLE_SM_PAIR_ALG_NUMCMP;
+        proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_gen_pub_priv(uint8_t *pub, uint8_t *priv)
+{
+    int rc;
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+    if (ble_sm_dbg_sc_keys_set) {
+        ble_sm_dbg_sc_keys_set = 0;
+        memcpy(pub, ble_sm_dbg_sc_pub_key, sizeof ble_sm_dbg_sc_pub_key);
+        memcpy(priv, ble_sm_dbg_sc_priv_key, sizeof ble_sm_dbg_sc_priv_key);
+        return 0;
+    }
+#endif
+
+    rc = ble_sm_alg_gen_key_pair(pub, priv);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_sm_sc_ensure_keys_generated(void)
+{
+    int rc;
+
+    if (!ble_sm_sc_keys_generated) {
+        rc = ble_sm_gen_pub_priv(ble_sm_sc_pub_key, ble_sm_sc_priv_key);
+        if (rc != 0) {
+            return rc;
+        }
+
+        ble_sm_sc_keys_generated = 1;
+    }
+
+    BLE_HS_LOG(DEBUG, "our pubkey=");
+    ble_hs_log_flat_buf(&ble_sm_sc_pub_key, 64);
+    BLE_HS_LOG(DEBUG, "\n");
+    BLE_HS_LOG(DEBUG, "our privkey=");
+    ble_hs_log_flat_buf(&ble_sm_sc_priv_key, 32);
+    BLE_HS_LOG(DEBUG, "\n");
+
+    return 0;
+}
+
+/* Initiator does not send a confirm when pairing algorithm is any of:
+ *     o just works
+ *     o numeric comparison
+ * (vol. 3, part H, 2.3.5.6.2)
+ */
+static int
+ble_sm_sc_initiator_txes_confirm(struct ble_sm_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(proc->flags & BLE_SM_PROC_F_SC);
+
+    return proc->pair_alg != BLE_SM_PAIR_ALG_JW &&
+           proc->pair_alg != BLE_SM_PAIR_ALG_NUMCMP;
+}
+
+/* Responder does not verify the initiator's random number when pairing
+ * algorithm is any of:
+ *     o just works
+ *     o numeric comparison
+ * (vol. 3, part H, 2.3.5.6.2)
+ */
+static int
+ble_sm_sc_responder_verifies_random(struct ble_sm_proc *proc)
+{
+    BLE_HS_DBG_ASSERT(proc->flags & BLE_SM_PROC_F_SC);
+
+    return proc->pair_alg != BLE_SM_PAIR_ALG_JW &&
+           proc->pair_alg != BLE_SM_PAIR_ALG_NUMCMP;
+}
+
+/**
+ * Generates the Ri byte used in the confirm message.  On success, the byte is
+ * written to the supplied procedure object.
+ */
+static int
+ble_sm_sc_gen_ri(struct ble_sm_proc *proc)
+{
+    int byte;
+    int bit;
+
+    switch (proc->pair_alg) {
+    case BLE_SM_PAIR_ALG_JW:
+    case BLE_SM_PAIR_ALG_NUMCMP:
+    case BLE_SM_PAIR_ALG_OOB:
+        proc->ri = 0;
+        return 0;
+
+    case BLE_SM_PAIR_ALG_PASSKEY:
+        BLE_HS_DBG_ASSERT(proc->passkey_bits_exchanged <
+                          BLE_SM_SC_PASSKEY_BITS);
+
+        byte = proc->passkey_bits_exchanged / 8;
+        bit = proc->passkey_bits_exchanged % 8;
+        proc->ri = 0x80 | !!(proc->tk[byte] & (1 << bit));
+
+        proc->passkey_bits_exchanged++;
+
+        return 0;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HS_EUNKNOWN;
+    }
+}
+
+void
+ble_sm_sc_oob_confirm(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    int err;
+    bool match;
+    uint8_t c[16];
+
+    /* Authentication stage 1: Step 5 */
+    if (proc->oob_data_remote) {
+        err = ble_sm_alg_f4(proc->pub_key_peer.x, proc->pub_key_peer.x,
+                            proc->oob_data_remote->r, 0, c);
+        if (err) {
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_UNSPECIFIED);
+            res->enc_cb = 1;
+            return;
+        }
+
+        match = (memcmp(c, proc->oob_data_remote->c, sizeof(c)) == 0);
+        if (!match) {
+            /* Random number mismatch. */
+            res->sm_err = BLE_SM_ERR_CONFIRM_MISMATCH;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH);
+            res->enc_cb = 1;
+            return;
+        }
+    }
+
+    if ((proc->flags & BLE_SM_PROC_F_INITIATOR) ||
+        (proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO)) {
+        /* If is initiator or was waiting on
+         * IO then execute step 6: send Random
+         */
+        res->execute = 1;
+    }
+}
+
+void
+ble_sm_sc_confirm_exec(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    struct ble_sm_pair_confirm *cmd;
+    struct os_mbuf *txom;
+    int rc;
+
+    rc = ble_sm_sc_gen_ri(proc);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_CONFIRM, sizeof(*cmd), &txom);
+    if (cmd == NULL) {
+        rc = BLE_HS_ENOMEM;
+        res->app_status = rc;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    rc = ble_sm_alg_f4(ble_sm_sc_pub_key, proc->pub_key_peer.x,
+                       ble_sm_our_pair_rand(proc), proc->ri, cmd->value);
+    if (rc != 0) {
+        os_mbuf_free_chain(txom);
+        res->app_status = rc;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
+        proc->state = BLE_SM_PROC_STATE_RANDOM;
+    }
+}
+
+static void
+ble_sm_sc_gen_numcmp(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    uint8_t *pka;
+    uint8_t *pkb;
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        pka = ble_sm_sc_pub_key;
+        pkb = proc->pub_key_peer.x;
+    } else {
+        pka = proc->pub_key_peer.x;
+        pkb = ble_sm_sc_pub_key;
+    }
+    res->app_status = ble_sm_alg_g2(pka, pkb, proc->randm, proc->rands,
+                                    &res->passkey_params.numcmp);
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+    }
+}
+
+/**
+ * Advances the supplied procedure object to the next state after it has
+ * completed the random state.
+ */
+static int
+ble_sm_sc_random_advance(struct ble_sm_proc *proc)
+{
+    int rc;
+
+    if (proc->pair_alg != BLE_SM_PAIR_ALG_PASSKEY ||
+        proc->passkey_bits_exchanged >= BLE_SM_SC_PASSKEY_BITS) {
+
+        proc->state = BLE_SM_PROC_STATE_DHKEY_CHECK;
+    } else {
+        proc->state = BLE_SM_PROC_STATE_CONFIRM;
+        rc = ble_sm_gen_pair_rand(ble_sm_our_pair_rand(proc));
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+void
+ble_sm_sc_random_exec(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    struct ble_sm_pair_random *cmd;
+    struct os_mbuf *txom;
+    uint8_t ioact;
+    int rc;
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_RANDOM, sizeof(*cmd), &txom);
+    if (cmd == NULL) {
+        rc = BLE_HS_ENOMEM;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    memcpy(cmd->value, ble_sm_our_pair_rand(proc), 16);
+
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
+        rc = ble_sm_sc_random_advance(proc);
+        if (rc != 0) {
+            res->app_status = rc;
+            res->enc_cb = 1;
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+            return;
+        }
+
+        rc = ble_sm_sc_io_action(proc, &ioact);
+        BLE_HS_DBG_ASSERT(rc == 0);
+
+        if (ble_sm_ioact_state(ioact) == proc->state &&
+            !(proc->flags & BLE_SM_PROC_F_IO_INJECTED)) {
+
+            res->passkey_params.action = ioact;
+            BLE_HS_DBG_ASSERT(ioact == BLE_SM_IOACT_NUMCMP);
+            ble_sm_sc_gen_numcmp(proc, res);
+        }
+    }
+}
+
+void
+ble_sm_sc_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res)
+{
+    uint8_t confirm_val[16];
+    uint8_t ia[6];
+    uint8_t ra[6];
+    uint8_t ioact;
+    uint8_t iat;
+    uint8_t rat;
+    int rc;
+
+    if (proc->pair_alg != BLE_SM_PAIR_ALG_OOB && (
+        proc->flags & BLE_SM_PROC_F_INITIATOR ||
+        ble_sm_sc_responder_verifies_random(proc))) {
+
+        BLE_HS_LOG(DEBUG, "tk=");
+        ble_hs_log_flat_buf(proc->tk, 16);
+        BLE_HS_LOG(DEBUG, "\n");
+
+        rc = ble_sm_alg_f4(proc->pub_key_peer.x, ble_sm_sc_pub_key,
+                           ble_sm_peer_pair_rand(proc), proc->ri,
+                           confirm_val);
+        if (rc != 0) {
+            res->app_status = rc;
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+            res->enc_cb = 1;
+            return;
+        }
+
+        if (memcmp(proc->confirm_peer, confirm_val, 16) != 0) {
+            /* Random number mismatch. */
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH);
+            res->sm_err = BLE_SM_ERR_CONFIRM_MISMATCH;
+            res->enc_cb = 1;
+            return;
+        }
+    }
+
+    /* Calculate the mac key and ltk. */
+    ble_sm_ia_ra(proc, &iat, ia, &rat, ra);
+    rc = ble_sm_alg_f5(proc->dhkey, proc->randm, proc->rands,
+                       iat, ia, rat, ra, proc->mackey, proc->ltk);
+    if (rc != 0) {
+        res->app_status = rc;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    /* Ensure proper key size */
+    memset(proc->ltk + proc->key_size, 0, sizeof proc->ltk - proc->key_size);
+
+    /* Ensure the ltk gets persisted when the pairing procedure succeeds. */
+    memcpy(proc->our_keys.ltk, proc->ltk, sizeof proc->our_keys.ltk);
+    proc->our_keys.ltk_valid = 1;
+    proc->our_keys.ediv = 0;
+    proc->our_keys.rand_val = 0;
+    proc->our_keys.ediv_rand_valid = 1;
+    proc->our_keys.key_size = proc->key_size;
+
+    memcpy(proc->peer_keys.ltk, proc->ltk, sizeof proc->peer_keys.ltk);
+    proc->peer_keys.ltk_valid = 1;
+    proc->peer_keys.ediv = 0;
+    proc->peer_keys.rand_val = 0;
+    proc->peer_keys.ediv_rand_valid = 1;
+    proc->peer_keys.key_size = proc->key_size;
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        ble_sm_sc_random_advance(proc);
+
+        rc = ble_sm_sc_io_action(proc, &ioact);
+        if (rc != 0) {
+            BLE_HS_DBG_ASSERT(0);
+        }
+
+        if (ble_sm_ioact_state(ioact) == proc->state &&
+            !(proc->flags & BLE_SM_PROC_F_IO_INJECTED)) {
+
+            res->passkey_params.action = ioact;
+            BLE_HS_DBG_ASSERT(ioact == BLE_SM_IOACT_NUMCMP);
+            ble_sm_sc_gen_numcmp(proc, res);
+        } else {
+            res->execute = 1;
+        }
+    } else {
+        if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB &&
+            !(proc->flags & BLE_SM_PROC_F_IO_INJECTED)) {
+            proc->flags |= BLE_SM_PROC_F_ADVANCE_ON_IO;
+        } else {
+            res->execute = 1;
+        }
+    }
+}
+
+void
+ble_sm_sc_public_key_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                          void *arg)
+{
+    struct ble_sm_public_key *cmd;
+    struct os_mbuf *txom;
+    uint8_t ioact;
+    int rc;
+
+    res->app_status = ble_sm_sc_ensure_keys_generated();
+    if (res->app_status != 0) {
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_PUBLIC_KEY, sizeof(*cmd), &txom);
+    if (!cmd) {
+        res->app_status = BLE_HS_ENOMEM;
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    memcpy(cmd->x, ble_sm_sc_pub_key + 0, 32);
+    memcpy(cmd->y, ble_sm_sc_pub_key + 32, 32);
+
+    res->app_status = ble_sm_tx(proc->conn_handle, txom);
+    if (res->app_status != 0) {
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
+        if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) {
+            proc->state = BLE_SM_PROC_STATE_RANDOM;
+        } else {
+            proc->state = BLE_SM_PROC_STATE_CONFIRM;
+        }
+
+        rc = ble_sm_sc_io_action(proc, &ioact);
+        if (rc != 0) {
+            BLE_HS_DBG_ASSERT(0);
+        }
+
+        if (ble_sm_ioact_state(ioact) == proc->state) {
+            res->passkey_params.action = ioact;
+        }
+
+        if (ble_sm_proc_can_advance(proc) &&
+            !ble_sm_sc_initiator_txes_confirm(proc)) {
+
+            res->execute = 1;
+        }
+    }
+}
+
+void
+ble_sm_sc_public_key_rx(uint16_t conn_handle, struct os_mbuf **om,
+                        struct ble_sm_result *res)
+{
+    struct ble_sm_public_key *cmd;
+    struct ble_sm_proc *proc;
+    uint8_t ioact;
+    int rc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->enc_cb = 1;
+        return;
+    }
+
+    res->app_status = ble_sm_sc_ensure_keys_generated();
+    if (res->app_status != 0) {
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    cmd = (struct ble_sm_public_key *)(*om)->om_data;
+
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_PUBLIC_KEY, -1,
+                            NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
+        memcpy(&proc->pub_key_peer, cmd, sizeof(*cmd));
+        rc = ble_sm_alg_gen_dhkey(proc->pub_key_peer.x,
+                                  proc->pub_key_peer.y,
+                                  ble_sm_sc_priv_key,
+                                  proc->dhkey);
+        if (rc != 0) {
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_DHKEY);
+            res->sm_err = BLE_SM_ERR_DHKEY;
+            res->enc_cb = 1;
+        } else {
+            if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+                if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) {
+                    proc->state = BLE_SM_PROC_STATE_RANDOM;
+                } else {
+                    proc->state = BLE_SM_PROC_STATE_CONFIRM;
+                }
+
+                rc = ble_sm_sc_io_action(proc, &ioact);
+                if (rc != 0) {
+                        BLE_HS_DBG_ASSERT(0);
+                }
+
+                if (ble_sm_ioact_state(ioact) == proc->state) {
+                    res->passkey_params.action = ioact;
+                }
+
+                if (ble_sm_proc_can_advance(proc) &&
+                    ble_sm_sc_initiator_txes_confirm(proc)) {
+
+                    res->execute = 1;
+                }
+            } else {
+                res->execute = 1;
+            }
+        }
+    }
+    ble_hs_unlock();
+}
+
+static void
+ble_sm_sc_dhkey_addrs(struct ble_sm_proc *proc, ble_addr_t *our_addr,
+                      ble_addr_t *peer_addr)
+{
+    struct ble_hs_conn_addrs addrs;
+    struct ble_hs_conn *conn;
+
+    conn = ble_hs_conn_find_assert(proc->conn_handle);
+
+    ble_hs_conn_addrs(conn, &addrs);
+
+    *our_addr = addrs.our_ota_addr;
+    *peer_addr = addrs.peer_ota_addr;
+}
+
+void
+ble_sm_sc_dhkey_check_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
+                           void *arg)
+{
+    struct ble_sm_dhkey_check *cmd;
+    ble_addr_t our_addr;
+    ble_addr_t peer_addr;
+    struct os_mbuf *txom;
+    uint8_t *iocap;
+    int rc;
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        struct ble_sm_pair_cmd *pair_req;
+
+        pair_req  = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
+        iocap = &pair_req->io_cap;
+    } else {
+        struct ble_sm_pair_cmd *pair_rsp;
+
+        pair_rsp  = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+        iocap = &pair_rsp->io_cap;
+    }
+
+    if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) {
+        if (proc->oob_data_remote) {
+            memcpy(proc->tk, proc->oob_data_remote->r, 16);
+        } else {
+            memset(proc->tk, 0, 16);
+        }
+    }
+
+    ble_sm_sc_dhkey_addrs(proc, &our_addr, &peer_addr);
+
+    cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_DHKEY_CHECK, sizeof(*cmd), &txom);
+    if (!cmd) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    rc = ble_sm_alg_f6(proc->mackey, ble_sm_our_pair_rand(proc),
+                       ble_sm_peer_pair_rand(proc), proc->tk, iocap,
+                       our_addr.type, our_addr.val, peer_addr.type,
+                       peer_addr.val, cmd->value);
+    if (rc != 0) {
+        os_mbuf_free_chain(txom);
+        goto err;
+    }
+
+    rc = ble_sm_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        goto err;
+    }
+
+    if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
+        proc->state = BLE_SM_PROC_STATE_LTK_START;
+    }
+
+    return;
+
+err:
+    res->app_status = rc;
+    res->enc_cb = 1;
+    res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+}
+
+static void
+ble_sm_dhkey_check_process(struct ble_sm_proc *proc,
+                           struct ble_sm_dhkey_check *cmd,
+                           struct ble_sm_result *res)
+{
+    uint8_t exp_value[16];
+    ble_addr_t our_addr;
+    ble_addr_t peer_addr;
+    uint8_t *iocap;
+    uint8_t ioact;
+    int rc;
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        struct ble_sm_pair_cmd *pair_rsp;
+
+        pair_rsp  = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+        iocap = &pair_rsp->io_cap;
+
+        if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) {
+            if (pair_rsp->oob_data_flag) {
+                memcpy(proc->tk, proc->oob_data_local->r, 16);
+            } else {
+                memset(proc->tk, 0, 16);
+            }
+        }
+    } else {
+        struct ble_sm_pair_cmd *pair_req;
+
+        pair_req  = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
+        iocap = &pair_req->io_cap;
+
+        if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) {
+            if (pair_req->oob_data_flag) {
+                memcpy(proc->tk, proc->oob_data_local->r, 16);
+            } else {
+                memset(proc->tk, 0, 16);
+            }
+        }
+    }
+
+    ble_sm_sc_dhkey_addrs(proc, &our_addr, &peer_addr);
+    BLE_HS_LOG(DEBUG, "tk=");
+    ble_hs_log_flat_buf(proc->tk, 16);
+    BLE_HS_LOG(DEBUG, "\n");
+
+    res->app_status = ble_sm_alg_f6(proc->mackey,
+                                    ble_sm_peer_pair_rand(proc),
+                                    ble_sm_our_pair_rand(proc),
+                                    proc->tk, iocap,
+                                    peer_addr.type, peer_addr.val,
+                                    our_addr.type, our_addr.val,
+                                    exp_value);
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+        return;
+    }
+
+    if (memcmp(cmd->value, exp_value, 16) != 0) {
+        /* Random number mismatch. */
+        res->sm_err = BLE_SM_ERR_DHKEY;
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_DHKEY);
+        res->enc_cb = 1;
+        return;
+    }
+
+    rc = ble_sm_sc_io_action(proc, &ioact);
+    if (rc != 0) {
+        BLE_HS_DBG_ASSERT(0);
+    }
+
+    if (ble_sm_ioact_state(ioact) == proc->state) {
+        proc->flags |= BLE_SM_PROC_F_ADVANCE_ON_IO;
+    }
+
+    if (ble_sm_proc_can_advance(proc)) {
+        if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+            proc->state = BLE_SM_PROC_STATE_ENC_START;
+        }
+
+        res->execute = 1;
+    }
+}
+
+void
+ble_sm_sc_dhkey_check_rx(uint16_t conn_handle, struct os_mbuf **om,
+                         struct ble_sm_result *res)
+{
+    struct ble_sm_dhkey_check *cmd;
+    struct ble_sm_proc *proc;
+
+    res->app_status = ble_hs_mbuf_pullup_base(om, sizeof(*cmd));
+    if (res->app_status != 0) {
+        res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        return;
+    }
+
+    cmd = (struct ble_sm_dhkey_check *)(*om)->om_data;
+
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_DHKEY_CHECK, -1,
+                            NULL);
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOENT;
+    } else {
+        ble_sm_dhkey_check_process(proc, cmd, res);
+    }
+    ble_hs_unlock();
+}
+
+bool
+ble_sm_sc_oob_data_check(struct ble_sm_proc *proc,
+                         bool oob_data_local_present,
+                         bool oob_data_remote_present)
+{
+    struct ble_sm_pair_cmd *pair_req;
+    struct ble_sm_pair_cmd *pair_rsp;
+    bool req_oob_present;
+    bool rsp_oob_present;
+
+    pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
+    pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1];
+    req_oob_present = pair_req->oob_data_flag == BLE_SM_PAIR_OOB_YES;
+    rsp_oob_present = pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES;
+
+    if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
+        return req_oob_present == oob_data_remote_present;
+    } else {
+        return rsp_oob_present == oob_data_remote_present;
+    }
+}
+
+int
+ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data)
+{
+    int rc;
+
+#if !MYNEWT_VAL(BLE_SM_SC)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    rc = ble_sm_sc_ensure_keys_generated();
+    if (rc) {
+        return rc;
+    }
+
+    rc = ble_hs_hci_util_rand(oob_data->r, 16);
+    if (rc) {
+        return rc;
+    }
+
+    rc = ble_sm_alg_f4(ble_sm_sc_pub_key, ble_sm_sc_pub_key, oob_data->r, 0,
+                       oob_data->c);
+    if (rc) {
+        return rc;
+    }
+
+    return 0;
+}
+
+void
+ble_sm_sc_init(void)
+{
+    ble_sm_alg_ecc_init();
+    ble_sm_sc_keys_generated = 0;
+}
+
+#endif  /* MYNEWT_VAL(BLE_SM_SC) */
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_store.c b/src/libs/mynewt-nimble/nimble/host/src/ble_store.c
new file mode 100644
index 00000000..22e60894
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_store.c
@@ -0,0 +1,420 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+
+#include "host/ble_store.h"
+#include "ble_hs_priv.h"
+
+int
+ble_store_read(int obj_type, const union ble_store_key *key,
+               union ble_store_value *val)
+{
+    int rc;
+
+    ble_hs_lock();
+
+    if (ble_hs_cfg.store_read_cb == NULL) {
+        rc = BLE_HS_ENOTSUP;
+    } else {
+        rc = ble_hs_cfg.store_read_cb(obj_type, key, val);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_store_write(int obj_type, const union ble_store_value *val)
+{
+    int rc;
+
+    if (ble_hs_cfg.store_write_cb == NULL) {
+        return BLE_HS_ENOTSUP;
+    }
+
+    while (1) {
+        ble_hs_lock();
+        rc = ble_hs_cfg.store_write_cb(obj_type, val);
+        ble_hs_unlock();
+
+        switch (rc) {
+        case 0:
+            return 0;
+        case BLE_HS_ESTORE_CAP:
+            /* Record didn't fit.  Give the application the opportunity to free
+             * up some space.
+             */
+            rc = ble_store_overflow_event(obj_type, val);
+            if (rc != 0) {
+                return rc;
+            }
+
+            /* Application made room for the record; try again. */
+            break;
+
+        default:
+            return rc;
+        }
+    }
+}
+
+int
+ble_store_delete(int obj_type, const union ble_store_key *key)
+{
+    int rc;
+
+    ble_hs_lock();
+
+    if (ble_hs_cfg.store_delete_cb == NULL) {
+        rc = BLE_HS_ENOTSUP;
+    } else {
+        rc = ble_hs_cfg.store_delete_cb(obj_type, key);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+static int
+ble_store_status(struct ble_store_status_event *event)
+{
+    int rc;
+
+    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+
+    if (ble_hs_cfg.store_status_cb == NULL) {
+        rc = BLE_HS_ENOTSUP;
+    } else {
+        rc = ble_hs_cfg.store_status_cb(event, ble_hs_cfg.store_status_arg);
+    }
+
+    return rc;
+}
+
+int
+ble_store_overflow_event(int obj_type, const union ble_store_value *value)
+{
+    struct ble_store_status_event event;
+
+    event.event_code = BLE_STORE_EVENT_OVERFLOW;
+    event.overflow.obj_type = obj_type;
+    event.overflow.value = value;
+
+    return ble_store_status(&event);
+}
+
+int
+ble_store_full_event(int obj_type, uint16_t conn_handle)
+{
+    struct ble_store_status_event event;
+
+    event.event_code = BLE_STORE_EVENT_FULL;
+    event.full.obj_type = obj_type;
+    event.full.conn_handle = conn_handle;
+
+    return ble_store_status(&event);
+}
+
+int
+ble_store_read_our_sec(const struct ble_store_key_sec *key_sec,
+                       struct ble_store_value_sec *value_sec)
+{
+    const union ble_store_key *store_key;
+    union ble_store_value *store_value;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(key_sec->peer_addr.type == BLE_ADDR_PUBLIC ||
+                      key_sec->peer_addr.type == BLE_ADDR_RANDOM ||
+                      ble_addr_cmp(&key_sec->peer_addr, BLE_ADDR_ANY) == 0);
+
+    store_key = (void *)key_sec;
+    store_value = (void *)value_sec;
+    rc = ble_store_read(BLE_STORE_OBJ_TYPE_OUR_SEC, store_key, store_value);
+    return rc;
+}
+
+static int
+ble_store_persist_sec(int obj_type,
+                      const struct ble_store_value_sec *value_sec)
+{
+    union ble_store_value *store_value;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(value_sec->peer_addr.type == BLE_ADDR_PUBLIC ||
+                      value_sec->peer_addr.type == BLE_ADDR_RANDOM);
+    BLE_HS_DBG_ASSERT(value_sec->ltk_present ||
+                      value_sec->irk_present ||
+                      value_sec->csrk_present);
+
+    store_value = (void *)value_sec;
+    rc = ble_store_write(obj_type, store_value);
+    return rc;
+}
+
+int
+ble_store_write_our_sec(const struct ble_store_value_sec *value_sec)
+{
+    int rc;
+
+    rc = ble_store_persist_sec(BLE_STORE_OBJ_TYPE_OUR_SEC, value_sec);
+    return rc;
+}
+
+int
+ble_store_delete_our_sec(const struct ble_store_key_sec *key_sec)
+{
+    union ble_store_key *store_key;
+    int rc;
+
+    store_key = (void *)key_sec;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, store_key);
+    return rc;
+}
+
+int
+ble_store_delete_peer_sec(const struct ble_store_key_sec *key_sec)
+{
+    union ble_store_key *store_key;
+    int rc;
+
+    store_key = (void *)key_sec;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_PEER_SEC, store_key);
+    return rc;
+}
+
+int
+ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec,
+                        struct ble_store_value_sec *value_sec)
+{
+    union ble_store_value *store_value;
+    union ble_store_key *store_key;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(key_sec->peer_addr.type == BLE_ADDR_PUBLIC ||
+                      key_sec->peer_addr.type == BLE_ADDR_RANDOM);
+
+    store_key = (void *)key_sec;
+    store_value = (void *)value_sec;
+    rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_SEC, store_key, store_value);
+
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_store_write_peer_sec(const struct ble_store_value_sec *value_sec)
+{
+    int rc;
+
+    rc = ble_store_persist_sec(BLE_STORE_OBJ_TYPE_PEER_SEC, value_sec);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) &&
+        value_sec->irk_present) {
+
+        /* Write the peer IRK to the controller keycache
+         * There is not much to do here if it fails */
+        rc = ble_hs_pvcy_add_entry(value_sec->peer_addr.val,
+                                          value_sec->peer_addr.type,
+                                          value_sec->irk);
+        if (rc != 0) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
+
+int
+ble_store_read_cccd(const struct ble_store_key_cccd *key,
+                    struct ble_store_value_cccd *out_value)
+{
+    union ble_store_value *store_value;
+    union ble_store_key *store_key;
+    int rc;
+
+    store_key = (void *)key;
+    store_value = (void *)out_value;
+    rc = ble_store_read(BLE_STORE_OBJ_TYPE_CCCD, store_key, store_value);
+    return rc;
+}
+
+int
+ble_store_write_cccd(const struct ble_store_value_cccd *value)
+{
+    union ble_store_value *store_value;
+    int rc;
+
+    store_value = (void *)value;
+    rc = ble_store_write(BLE_STORE_OBJ_TYPE_CCCD, store_value);
+    return rc;
+}
+
+int
+ble_store_delete_cccd(const struct ble_store_key_cccd *key)
+{
+    union ble_store_key *store_key;
+    int rc;
+
+    store_key = (void *)key;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_CCCD, store_key);
+    return rc;
+}
+
+void
+ble_store_key_from_value_cccd(struct ble_store_key_cccd *out_key,
+                              const struct ble_store_value_cccd *value)
+{
+    out_key->peer_addr = value->peer_addr;
+    out_key->chr_val_handle = value->chr_val_handle;
+    out_key->idx = 0;
+}
+
+void
+ble_store_key_from_value_sec(struct ble_store_key_sec *out_key,
+                             const struct ble_store_value_sec *value)
+{
+    out_key->peer_addr = value->peer_addr;
+
+    out_key->ediv = value->ediv;
+    out_key->rand_num = value->rand_num;
+    out_key->ediv_rand_present = 1;
+    out_key->idx = 0;
+}
+
+void
+ble_store_key_from_value(int obj_type,
+                         union ble_store_key *out_key,
+                         const union ble_store_value *value)
+{
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        ble_store_key_from_value_sec(&out_key->sec, &value->sec);
+        break;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        ble_store_key_from_value_cccd(&out_key->cccd, &value->cccd);
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+}
+
+int
+ble_store_iterate(int obj_type,
+                  ble_store_iterator_fn *callback,
+                  void *cookie)
+{
+    union ble_store_key key;
+    union ble_store_value value;
+    int idx = 0;
+    uint8_t *pidx;
+    int rc;
+
+    /* a magic value to retrieve anything */
+    memset(&key, 0, sizeof(key));
+    switch(obj_type) {
+        case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        case BLE_STORE_OBJ_TYPE_OUR_SEC:
+            key.sec.peer_addr = *BLE_ADDR_ANY;
+            pidx = &key.sec.idx;
+            break;
+        case BLE_STORE_OBJ_TYPE_CCCD:
+            key.cccd.peer_addr = *BLE_ADDR_ANY;
+            pidx = &key.cccd.idx;
+            break;
+        default:
+            BLE_HS_DBG_ASSERT(0);
+            return BLE_HS_EINVAL;
+    }
+
+    while (1) {
+        *pidx = idx;
+        rc = ble_store_read(obj_type, &key, &value);
+        switch (rc) {
+        case 0:
+            if (callback != NULL) {
+                rc = callback(obj_type, &value, cookie);
+                if (rc != 0) {
+                    /* User function indicates to stop iterating. */
+                    return 0;
+                }
+            }
+            break;
+
+        case BLE_HS_ENOENT:
+            /* No more entries. */
+            return 0;
+
+        default:
+            /* Read error. */
+            return rc;
+        }
+
+        idx++;
+    }
+}
+
+/**
+ * Deletes all objects from the BLE host store.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_store_clear(void)
+{
+    const uint8_t obj_types[] = {
+        BLE_STORE_OBJ_TYPE_OUR_SEC,
+        BLE_STORE_OBJ_TYPE_PEER_SEC,
+        BLE_STORE_OBJ_TYPE_CCCD,
+    };
+    union ble_store_key key;
+    int obj_type;
+    int rc;
+    int i;
+
+    /* A zeroed key will always retrieve the first value. */
+    memset(&key, 0, sizeof key);
+
+    for (i = 0; i < sizeof obj_types / sizeof obj_types[0]; i++) {
+        obj_type = obj_types[i];
+
+        do {
+            rc = ble_store_delete(obj_type, &key);
+        } while (rc == 0);
+
+        /* BLE_HS_ENOENT means we deleted everything. */
+        if (rc != BLE_HS_ENOENT) {
+            return rc;
+        }
+    }
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_store_util.c b/src/libs/mynewt-nimble/nimble/host/src/ble_store_util.c
new file mode 100644
index 00000000..7de48272
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_store_util.c
@@ -0,0 +1,256 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "host/ble_store.h"
+#include "ble_hs_priv.h"
+
+struct ble_store_util_peer_set {
+    ble_addr_t *peer_id_addrs;
+    int num_peers;
+    int max_peers;
+    int status;
+};
+
+static int
+ble_store_util_iter_unique_peer(int obj_type,
+                                union ble_store_value *val,
+                                void *arg)
+{
+    struct ble_store_util_peer_set *set;
+    int i;
+
+    BLE_HS_DBG_ASSERT(obj_type == BLE_STORE_OBJ_TYPE_OUR_SEC ||
+                      obj_type == BLE_STORE_OBJ_TYPE_PEER_SEC);
+
+    set = arg;
+
+    /* Do nothing if this peer is a duplicate. */
+    for (i = 0; i < set->num_peers; i++) {
+        if (ble_addr_cmp(set->peer_id_addrs + i, &val->sec.peer_addr) == 0) {
+            return 0;
+        }
+    }
+
+    if (set->num_peers >= set->max_peers) {
+        /* Overflow; abort the iterate procedure. */
+        set->status = BLE_HS_ENOMEM;
+        return 1;
+    }
+
+    set->peer_id_addrs[set->num_peers] = val->sec.peer_addr;
+    set->num_peers++;
+
+    return 0;
+}
+
+/**
+ * Retrieves the set of peer addresses for which a bond has been established.
+ *
+ * @param out_peer_id_addrs     On success, the set of bonded peer addresses
+ *                                  gets written here.
+ * @param out_num_peers         On success, the number of bonds gets written
+ *                                  here.
+ * @param max_peers             The capacity of the destination buffer.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOMEM if the destination buffer is too
+ *                                  small;
+ *                              Other nonzero on error.
+ */
+int
+ble_store_util_bonded_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers,
+                            int max_peers)
+{
+    struct ble_store_util_peer_set set = {
+        .peer_id_addrs = out_peer_id_addrs,
+        .num_peers = 0,
+        .max_peers = max_peers,
+        .status = 0,
+    };
+    int rc;
+
+    rc = ble_store_iterate(BLE_STORE_OBJ_TYPE_OUR_SEC,
+                           ble_store_util_iter_unique_peer,
+                           &set);
+    if (rc != 0) {
+        return rc;
+    }
+    if (set.status != 0) {
+        return set.status;
+    }
+
+    *out_num_peers = set.num_peers;
+    return 0;
+}
+
+/**
+ * Deletes all entries from the store that are attached to the specified peer
+ * address.  This function deletes security entries and CCCD records.
+ *
+ * @param peer_id_addr          Entries with this peer address get deleted.
+ *
+ * @return                      0 on success;
+ *                              Other nonzero on error.
+ */
+int
+ble_store_util_delete_peer(const ble_addr_t *peer_id_addr)
+{
+    union ble_store_key key;
+    int rc;
+
+    memset(&key, 0, sizeof key);
+    key.sec.peer_addr = *peer_id_addr;
+
+    rc = ble_store_util_delete_all(BLE_STORE_OBJ_TYPE_OUR_SEC, &key);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_store_util_delete_all(BLE_STORE_OBJ_TYPE_PEER_SEC, &key);
+    if (rc != 0) {
+        return rc;
+    }
+
+    memset(&key, 0, sizeof key);
+    key.cccd.peer_addr = *peer_id_addr;
+
+    rc = ble_store_util_delete_all(BLE_STORE_OBJ_TYPE_CCCD, &key);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Deletes all entries from the store that match the specified key.
+ *
+ * @param type                  The type of store entry to delete.
+ * @param key                   Entries matching this key get deleted.
+ *
+ * @return                      0 on success;
+ *                              Other nonzero on error.
+ */
+int
+ble_store_util_delete_all(int type, const union ble_store_key *key)
+{
+    int rc;
+
+    do {
+        rc = ble_store_delete(type, key);
+    } while (rc == 0);
+
+    if (rc != BLE_HS_ENOENT) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_util_iter_count(int obj_type,
+                          union ble_store_value *val,
+                          void *arg)
+{
+    int *count;
+
+    count = arg;
+    (*count)++;
+
+    return 0;
+}
+
+int
+ble_store_util_count(int type, int *out_count)
+{
+    int rc;
+
+    *out_count = 0;
+    rc = ble_store_iterate(type,
+                           ble_store_util_iter_count,
+                           out_count);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_store_util_delete_oldest_peer(void)
+{
+    ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+    int num_peers;
+    int rc;
+
+    rc = ble_store_util_bonded_peers(
+            peer_id_addrs, &num_peers,
+            sizeof peer_id_addrs / sizeof peer_id_addrs[0]);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (num_peers == 0) {
+        return 0;
+    }
+
+    rc = ble_store_util_delete_peer(&peer_id_addrs[0]);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Round-robin status callback.  If a there is insufficient storage capacity
+ * for a new record, delete the oldest bond and proceed with the persist
+ * operation.
+ *
+ * Note: This is not the best behavior for an actual product because
+ * uninteresting peers could cause important bonds to be deleted.  This is
+ * useful for demonstrations and sample apps.
+ */
+int
+ble_store_util_status_rr(struct ble_store_status_event *event, void *arg)
+{
+    switch (event->event_code) {
+    case BLE_STORE_EVENT_OVERFLOW:
+        switch (event->overflow.obj_type) {
+        case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        case BLE_STORE_OBJ_TYPE_PEER_SEC:
+            return ble_gap_unpair_oldest_peer();
+        case BLE_STORE_OBJ_TYPE_CCCD:
+            /* Try unpairing oldest peer except current peer */
+            return ble_gap_unpair_oldest_except(&event->overflow.value->cccd.peer_addr);
+
+        default:
+            return BLE_HS_EUNKNOWN;
+        }
+
+    case BLE_STORE_EVENT_FULL:
+        /* Just proceed with the operation.  If it results in an overflow,
+         * we'll delete a record when the overflow occurs.
+         */
+        return 0;
+
+    default:
+        return BLE_HS_EUNKNOWN;
+    }
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_uuid.c b/src/libs/mynewt-nimble/nimble/host/src/ble_uuid.c
new file mode 100644
index 00000000..16352cf6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_uuid.c
@@ -0,0 +1,260 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include "os/os_mbuf.h"
+#include "nimble/ble.h"
+#include "ble_hs_priv.h"
+#include "host/ble_uuid.h"
+
+static uint8_t ble_uuid_base[16] = {
+    0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
+    0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+#if MYNEWT_VAL(BLE_HS_DEBUG)
+#define VERIFY_UUID(uuid)                       \
+    assert((uuid->type == BLE_UUID_TYPE_16) ||  \
+           (uuid->type == BLE_UUID_TYPE_32) ||  \
+           (uuid->type == BLE_UUID_TYPE_128))
+#else
+#define VERIFY_UUID(uuid)
+#endif
+
+int
+ble_uuid_init_from_buf(ble_uuid_any_t *uuid, const void *buf, size_t len)
+{
+    switch (len) {
+    case 2:
+        uuid->u.type = BLE_UUID_TYPE_16;
+        uuid->u16.value = get_le16(buf);
+        return 0;
+    case 4:
+        uuid->u.type = BLE_UUID_TYPE_32;
+        uuid->u32.value = get_le32(buf);
+        return 0;
+    case 16:
+        uuid->u.type = BLE_UUID_TYPE_128;
+        memcpy(uuid->u128.value, buf, 16);
+        return 0;
+    }
+
+    return BLE_HS_EINVAL;
+}
+
+int
+ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2)
+{
+    VERIFY_UUID(uuid1);
+    VERIFY_UUID(uuid2);
+
+    if (uuid1->type != uuid2->type) {
+      return uuid1->type - uuid2->type;
+    }
+
+    switch (uuid1->type) {
+    case BLE_UUID_TYPE_16:
+        return (int) BLE_UUID16(uuid1)->value - (int) BLE_UUID16(uuid2)->value;
+    case BLE_UUID_TYPE_32:
+        return (int) BLE_UUID32(uuid1)->value - (int) BLE_UUID32(uuid2)->value;
+    case BLE_UUID_TYPE_128:
+        return memcmp(BLE_UUID128(uuid1)->value, BLE_UUID128(uuid2)->value, 16);
+    }
+
+    BLE_HS_DBG_ASSERT(0);
+
+    return -1;
+}
+
+void
+ble_uuid_copy(ble_uuid_any_t *dst, const ble_uuid_t *src)
+{
+    VERIFY_UUID(src);
+
+    switch (src->type) {
+    case BLE_UUID_TYPE_16:
+        dst->u16 = *(const ble_uuid16_t *)src;
+        break;
+    case BLE_UUID_TYPE_32:
+        dst->u32 = *(const ble_uuid32_t *)src;
+        break;
+    case BLE_UUID_TYPE_128:
+        dst->u128 = *(const ble_uuid128_t *)src;
+        break;
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+}
+
+char *
+ble_uuid_to_str(const ble_uuid_t *uuid, char *dst)
+{
+    const uint8_t *u8p;
+
+    switch (uuid->type) {
+    case BLE_UUID_TYPE_16:
+        sprintf(dst, "0x%04" PRIx16, BLE_UUID16(uuid)->value);
+        break;
+    case BLE_UUID_TYPE_32:
+        sprintf(dst, "0x%08" PRIx32, BLE_UUID32(uuid)->value);
+        break;
+    case BLE_UUID_TYPE_128:
+        u8p = BLE_UUID128(uuid)->value;
+
+        sprintf(dst, "%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-"
+                     "%02x%02x%02x%02x%02x%02x",
+                u8p[15], u8p[14], u8p[13], u8p[12],
+                u8p[11], u8p[10],  u8p[9],  u8p[8],
+                 u8p[7],  u8p[6],  u8p[5],  u8p[4],
+                 u8p[3],  u8p[2],  u8p[1],  u8p[0]);
+        break;
+    default:
+        dst[0] = '\0';
+        break;
+    }
+
+    return dst;
+}
+
+uint16_t
+ble_uuid_u16(const ble_uuid_t *uuid)
+{
+    VERIFY_UUID(uuid);
+
+    return uuid->type == BLE_UUID_TYPE_16 ? BLE_UUID16(uuid)->value : 0;
+}
+
+/* APIs below are private (ble_uuid_priv.h) */
+
+int
+ble_uuid_init_from_att_mbuf(ble_uuid_any_t *uuid, struct os_mbuf *om, int off,
+                            int len)
+{
+    uint8_t val[16];
+    int rc;
+
+    rc = os_mbuf_copydata(om, off, len, val);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_uuid_init_from_att_buf(uuid, val, len);
+
+    return rc;
+}
+
+int
+ble_uuid_init_from_att_buf(ble_uuid_any_t *uuid, const void *buf, size_t len)
+{
+    int rc = 0;
+
+    if (len == 2) {
+        uuid->u.type = BLE_UUID_TYPE_16;
+        uuid->u16.value = get_le16(buf);
+    } else if (len == 16) {
+        uuid->u.type = BLE_UUID_TYPE_128;
+        memcpy(uuid->u128.value, buf, 16);
+    } else {
+        rc = BLE_HS_EINVAL;
+    }
+
+    return rc;
+}
+
+int
+ble_uuid_to_any(const ble_uuid_t *uuid, ble_uuid_any_t *uuid_any)
+{
+    VERIFY_UUID(uuid);
+
+    uuid_any->u.type = uuid->type;
+
+    switch (uuid->type) {
+    case BLE_UUID_TYPE_16:
+        uuid_any->u16.value = BLE_UUID16(uuid)->value;
+        break;
+
+    case BLE_UUID_TYPE_32:
+        uuid_any->u32.value = BLE_UUID32(uuid)->value;
+        break;
+
+    case BLE_UUID_TYPE_128:
+        memcpy(uuid_any->u128.value, BLE_UUID128(uuid)->value, 16);
+        break;
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+int
+ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om)
+{
+    int len;
+    void *buf;
+
+    VERIFY_UUID(uuid);
+
+    len = ble_uuid_length(uuid);
+
+    buf = os_mbuf_extend(om, len);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    ble_uuid_flat(uuid, buf);
+
+    return 0;
+}
+
+int
+ble_uuid_flat(const ble_uuid_t *uuid, void *dst)
+{
+    VERIFY_UUID(uuid);
+
+    switch (uuid->type) {
+    case BLE_UUID_TYPE_16:
+        put_le16(dst, BLE_UUID16(uuid)->value);
+        break;
+    case BLE_UUID_TYPE_32:
+        memcpy(dst, ble_uuid_base, 16);
+        put_le32(dst + 12, BLE_UUID32(uuid)->value);
+        break;
+    case BLE_UUID_TYPE_128:
+        memcpy(dst, BLE_UUID128(uuid)->value, 16);
+        break;
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
+
+int
+ble_uuid_length(const ble_uuid_t *uuid)
+{
+    VERIFY_UUID(uuid);
+
+    return uuid->type >> 3;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_uuid_priv.h b/src/libs/mynewt-nimble/nimble/host/src/ble_uuid_priv.h
new file mode 100644
index 00000000..3dbcc6b8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/src/ble_uuid_priv.h
@@ -0,0 +1,45 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_UUID_PRIV_
+#define H_BLE_UUID_PRIV_
+
+#include "host/ble_uuid.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+int ble_uuid_init_from_att_mbuf(ble_uuid_any_t *uuid, struct os_mbuf *om,
+                                int off, int len);
+int ble_uuid_init_from_att_buf(ble_uuid_any_t *uuid, const void *buf,
+                               size_t len);
+
+int ble_uuid_to_any(const ble_uuid_t *uuid, ble_uuid_any_t *uuid_any);
+int ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om);
+int ble_uuid_flat(const ble_uuid_t *uuid, void *dst);
+int ble_uuid_length(const ble_uuid_t *uuid);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/store/config/include/store/config/ble_store_config.h b/src/libs/mynewt-nimble/nimble/host/store/config/include/store/config/ble_store_config.h
new file mode 100644
index 00000000..1b3f8e6c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/config/include/store/config/ble_store_config.h
@@ -0,0 +1,39 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_STORE_CONFIG_
+#define H_BLE_STORE_CONFIG_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+union ble_store_key;
+union ble_store_value;
+
+int ble_store_config_read(int obj_type, const union ble_store_key *key,
+                          union ble_store_value *value);
+int ble_store_config_write(int obj_type, const union ble_store_value *val);
+int ble_store_config_delete(int obj_type, const union ble_store_key *key);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/store/config/pkg.yml b/src/libs/mynewt-nimble/nimble/host/store/config/pkg.yml
new file mode 100644
index 00000000..db80d1df
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/config/pkg.yml
@@ -0,0 +1,38 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/store/config
+pkg.description: sys/config-based persistence layer for the NimBLE host.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - persistence
+
+pkg.deps:
+    - "@apache-mynewt-core/encoding/base64"
+    - nimble/host
+
+pkg.deps.BLE_STORE_CONFIG_PERSIST:
+    - "@apache-mynewt-core/sys/config"
+
+pkg.init:
+    ble_store_config_init: 'MYNEWT_VAL(BLE_STORE_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config.c b/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config.c
new file mode 100644
index 00000000..ce60d1fb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config.c
@@ -0,0 +1,533 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <inttypes.h>
+#include <string.h>
+
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "host/ble_hs.h"
+#include "config/config.h"
+#include "base64/base64.h"
+#include "store/config/ble_store_config.h"
+#include "ble_store_config_priv.h"
+
+struct ble_store_value_sec
+    ble_store_config_our_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+int ble_store_config_num_our_secs;
+
+struct ble_store_value_sec
+    ble_store_config_peer_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+int ble_store_config_num_peer_secs;
+
+struct ble_store_value_cccd
+    ble_store_config_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)];
+int ble_store_config_num_cccds;
+
+/*****************************************************************************
+ * $sec                                                                      *
+ *****************************************************************************/
+
+static void
+ble_store_config_print_value_sec(const struct ble_store_value_sec *sec)
+{
+    if (sec->ltk_present) {
+        BLE_HS_LOG(DEBUG, "ediv=%u rand=%llu authenticated=%d ltk=",
+                       sec->ediv, sec->rand_num, sec->authenticated);
+        ble_hs_log_flat_buf(sec->ltk, 16);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+    if (sec->irk_present) {
+        BLE_HS_LOG(DEBUG, "irk=");
+        ble_hs_log_flat_buf(sec->irk, 16);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+    if (sec->csrk_present) {
+        BLE_HS_LOG(DEBUG, "csrk=");
+        ble_hs_log_flat_buf(sec->csrk, 16);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+
+    BLE_HS_LOG(DEBUG, "\n");
+}
+
+static void
+ble_store_config_print_key_sec(const struct ble_store_key_sec *key_sec)
+{
+    if (ble_addr_cmp(&key_sec->peer_addr, BLE_ADDR_ANY)) {
+        BLE_HS_LOG(DEBUG, "peer_addr_type=%d peer_addr=",
+                       key_sec->peer_addr.type);
+        ble_hs_log_flat_buf(key_sec->peer_addr.val, 6);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+    if (key_sec->ediv_rand_present) {
+        BLE_HS_LOG(DEBUG, "ediv=0x%02x rand=0x%llx ",
+                       key_sec->ediv, key_sec->rand_num);
+    }
+}
+
+static int
+ble_store_config_find_sec(const struct ble_store_key_sec *key_sec,
+                          const struct ble_store_value_sec *value_secs,
+                          int num_value_secs)
+{
+    const struct ble_store_value_sec *cur;
+    int skipped;
+    int i;
+
+    skipped = 0;
+
+    for (i = 0; i < num_value_secs; i++) {
+        cur = value_secs + i;
+
+        if (ble_addr_cmp(&key_sec->peer_addr, BLE_ADDR_ANY)) {
+            if (ble_addr_cmp(&cur->peer_addr, &key_sec->peer_addr)) {
+                continue;
+            }
+        }
+
+        if (key_sec->ediv_rand_present) {
+            if (cur->ediv != key_sec->ediv) {
+                continue;
+            }
+
+            if (cur->rand_num != key_sec->rand_num) {
+                continue;
+            }
+        }
+
+        if (key_sec->idx > skipped) {
+            skipped++;
+            continue;
+        }
+
+        return i;
+    }
+
+    return -1;
+}
+
+static int
+ble_store_config_read_our_sec(const struct ble_store_key_sec *key_sec,
+                              struct ble_store_value_sec *value_sec)
+{
+    int idx;
+
+    idx = ble_store_config_find_sec(key_sec, ble_store_config_our_secs,
+                                    ble_store_config_num_our_secs);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    *value_sec = ble_store_config_our_secs[idx];
+    return 0;
+}
+
+
+static int
+ble_store_config_write_our_sec(const struct ble_store_value_sec *value_sec)
+{
+    struct ble_store_key_sec key_sec;
+    int idx;
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "persisting our sec; ");
+    ble_store_config_print_value_sec(value_sec);
+
+    ble_store_key_from_value_sec(&key_sec, value_sec);
+    idx = ble_store_config_find_sec(&key_sec, ble_store_config_our_secs,
+                                    ble_store_config_num_our_secs);
+    if (idx == -1) {
+        if (ble_store_config_num_our_secs >= MYNEWT_VAL(BLE_STORE_MAX_BONDS)) {
+            BLE_HS_LOG(DEBUG, "error persisting our sec; too many entries "
+                              "(%d)\n", ble_store_config_num_our_secs);
+            return BLE_HS_ESTORE_CAP;
+        }
+
+        idx = ble_store_config_num_our_secs;
+        ble_store_config_num_our_secs++;
+    }
+
+    ble_store_config_our_secs[idx] = *value_sec;
+
+    rc = ble_store_config_persist_our_secs();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_config_delete_obj(void *values, int value_size, int idx,
+                            int *num_values)
+{
+    uint8_t *dst;
+    uint8_t *src;
+    int move_count;
+
+    (*num_values)--;
+    if (idx < *num_values) {
+        dst = values;
+        dst += idx * value_size;
+        src = dst + value_size;
+
+        move_count = *num_values - idx;
+        memmove(dst, src, move_count * value_size);
+    }
+
+    return 0;
+}
+
+static int
+ble_store_config_delete_sec(const struct ble_store_key_sec *key_sec,
+                            struct ble_store_value_sec *value_secs,
+                            int *num_value_secs)
+{
+    int idx;
+    int rc;
+
+    idx = ble_store_config_find_sec(key_sec, value_secs, *num_value_secs);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_store_config_delete_obj(value_secs, sizeof *value_secs, idx,
+                                  num_value_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_config_delete_our_sec(const struct ble_store_key_sec *key_sec)
+{
+    int rc;
+
+    rc = ble_store_config_delete_sec(key_sec, ble_store_config_our_secs,
+                                     &ble_store_config_num_our_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_store_config_persist_our_secs();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_config_delete_peer_sec(const struct ble_store_key_sec *key_sec)
+{
+    int rc;
+
+    rc = ble_store_config_delete_sec(key_sec, ble_store_config_peer_secs,
+                                  &ble_store_config_num_peer_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_store_config_persist_peer_secs();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_config_read_peer_sec(const struct ble_store_key_sec *key_sec,
+                               struct ble_store_value_sec *value_sec)
+{
+    int idx;
+
+    idx = ble_store_config_find_sec(key_sec, ble_store_config_peer_secs,
+                             ble_store_config_num_peer_secs);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    *value_sec = ble_store_config_peer_secs[idx];
+    return 0;
+}
+
+static int
+ble_store_config_write_peer_sec(const struct ble_store_value_sec *value_sec)
+{
+    struct ble_store_key_sec key_sec;
+    int idx;
+    int rc;
+
+    BLE_HS_LOG(DEBUG, "persisting peer sec; ");
+    ble_store_config_print_value_sec(value_sec);
+
+    ble_store_key_from_value_sec(&key_sec, value_sec);
+    idx = ble_store_config_find_sec(&key_sec, ble_store_config_peer_secs,
+                                 ble_store_config_num_peer_secs);
+    if (idx == -1) {
+        if (ble_store_config_num_peer_secs >= MYNEWT_VAL(BLE_STORE_MAX_BONDS)) {
+            BLE_HS_LOG(DEBUG, "error persisting peer sec; too many entries "
+                             "(%d)\n", ble_store_config_num_peer_secs);
+            return BLE_HS_ESTORE_CAP;
+        }
+
+        idx = ble_store_config_num_peer_secs;
+        ble_store_config_num_peer_secs++;
+    }
+
+    ble_store_config_peer_secs[idx] = *value_sec;
+
+    rc = ble_store_config_persist_peer_secs();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $cccd                                                                     *
+ *****************************************************************************/
+
+static int
+ble_store_config_find_cccd(const struct ble_store_key_cccd *key)
+{
+    struct ble_store_value_cccd *cccd;
+    int skipped;
+    int i;
+
+    skipped = 0;
+    for (i = 0; i < ble_store_config_num_cccds; i++) {
+        cccd = ble_store_config_cccds + i;
+
+        if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) {
+            if (ble_addr_cmp(&cccd->peer_addr, &key->peer_addr)) {
+                continue;
+            }
+        }
+
+        if (key->chr_val_handle != 0) {
+            if (cccd->chr_val_handle != key->chr_val_handle) {
+                continue;
+            }
+        }
+
+        if (key->idx > skipped) {
+            skipped++;
+            continue;
+        }
+
+        return i;
+    }
+
+    return -1;
+}
+
+static int
+ble_store_config_delete_cccd(const struct ble_store_key_cccd *key_cccd)
+{
+    int idx;
+    int rc;
+
+    idx = ble_store_config_find_cccd(key_cccd);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_store_config_delete_obj(ble_store_config_cccds,
+                                     sizeof *ble_store_config_cccds,
+                                     idx,
+                                     &ble_store_config_num_cccds);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_store_config_persist_cccds();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_config_read_cccd(const struct ble_store_key_cccd *key_cccd,
+                           struct ble_store_value_cccd *value_cccd)
+{
+    int idx;
+
+    idx = ble_store_config_find_cccd(key_cccd);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    *value_cccd = ble_store_config_cccds[idx];
+    return 0;
+}
+
+static int
+ble_store_config_write_cccd(const struct ble_store_value_cccd *value_cccd)
+{
+    struct ble_store_key_cccd key_cccd;
+    int idx;
+    int rc;
+
+    ble_store_key_from_value_cccd(&key_cccd, value_cccd);
+    idx = ble_store_config_find_cccd(&key_cccd);
+    if (idx == -1) {
+        if (ble_store_config_num_cccds >= MYNEWT_VAL(BLE_STORE_MAX_CCCDS)) {
+            BLE_HS_LOG(DEBUG, "error persisting cccd; too many entries (%d)\n",
+                       ble_store_config_num_cccds);
+            return BLE_HS_ESTORE_CAP;
+        }
+
+        idx = ble_store_config_num_cccds;
+        ble_store_config_num_cccds++;
+    }
+
+    ble_store_config_cccds[idx] = *value_cccd;
+
+    rc = ble_store_config_persist_cccds();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************
+ * $api                                                                      *
+ *****************************************************************************/
+
+/**
+ * Searches the database for an object matching the specified criteria.
+ *
+ * @return                      0 if a key was found; else BLE_HS_ENOENT.
+ */
+int
+ble_store_config_read(int obj_type, const union ble_store_key *key,
+                      union ble_store_value *value)
+{
+    int rc;
+
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        /* An encryption procedure (bonding) is being attempted.  The nimble
+         * stack is asking us to look in our key database for a long-term key
+         * corresponding to the specified ediv and random number.
+         *
+         * Perform a key lookup and populate the context object with the
+         * result.  The nimble stack will use this key if this function returns
+         * success.
+         */
+        BLE_HS_LOG(DEBUG, "looking up peer sec; ");
+        ble_store_config_print_key_sec(&key->sec);
+        BLE_HS_LOG(DEBUG, "\n");
+        rc = ble_store_config_read_peer_sec(&key->sec, &value->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        BLE_HS_LOG(DEBUG, "looking up our sec; ");
+        ble_store_config_print_key_sec(&key->sec);
+        BLE_HS_LOG(DEBUG, "\n");
+        rc = ble_store_config_read_our_sec(&key->sec, &value->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        rc = ble_store_config_read_cccd(&key->cccd, &value->cccd);
+        return rc;
+
+    default:
+        return BLE_HS_ENOTSUP;
+    }
+}
+
+/**
+ * Adds the specified object to the database.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ESTORE_CAP if the database is full.
+ */
+int
+ble_store_config_write(int obj_type, const union ble_store_value *val)
+{
+    int rc;
+
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        rc = ble_store_config_write_peer_sec(&val->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        rc = ble_store_config_write_our_sec(&val->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        rc = ble_store_config_write_cccd(&val->cccd);
+        return rc;
+
+    default:
+        return BLE_HS_ENOTSUP;
+    }
+}
+
+int
+ble_store_config_delete(int obj_type, const union ble_store_key *key)
+{
+    int rc;
+
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        rc = ble_store_config_delete_peer_sec(&key->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        rc = ble_store_config_delete_our_sec(&key->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        rc = ble_store_config_delete_cccd(&key->cccd);
+        return rc;
+
+    default:
+        return BLE_HS_ENOTSUP;
+    }
+}
+
+void
+ble_store_config_init(void)
+{
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    ble_hs_cfg.store_read_cb = ble_store_config_read;
+    ble_hs_cfg.store_write_cb = ble_store_config_write;
+    ble_hs_cfg.store_delete_cb = ble_store_config_delete;
+
+    /* Re-initialize BSS values in case of unit tests. */
+    ble_store_config_num_our_secs = 0;
+    ble_store_config_num_peer_secs = 0;
+    ble_store_config_num_cccds = 0;
+
+    ble_store_config_conf_init();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config_conf.c b/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config_conf.c
new file mode 100644
index 00000000..e74127ae
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config_conf.c
@@ -0,0 +1,231 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST)
+
+#include <inttypes.h>
+#include <string.h>
+
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "config/config.h"
+#include "base64/base64.h"
+#include "store/config/ble_store_config.h"
+#include "ble_store_config_priv.h"
+
+static int
+ble_store_config_conf_set(int argc, char **argv, char *val);
+static int
+ble_store_config_conf_export(void (*func)(char *name, char *val),
+                             enum conf_export_tgt tgt);
+
+static struct conf_handler ble_store_config_conf_handler = {
+    .ch_name = "ble_hs",
+    .ch_get = NULL,
+    .ch_set = ble_store_config_conf_set,
+    .ch_commit = NULL,
+    .ch_export = ble_store_config_conf_export
+};
+
+#define BLE_STORE_CONFIG_SEC_ENCODE_SZ      \
+    BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_sec))
+
+#define BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ  \
+    (MYNEWT_VAL(BLE_STORE_MAX_BONDS) * BLE_STORE_CONFIG_SEC_ENCODE_SZ + 1)
+
+#define BLE_STORE_CONFIG_CCCD_ENCODE_SZ     \
+    BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_cccd))
+
+#define BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ \
+    (MYNEWT_VAL(BLE_STORE_MAX_CCCDS) * BLE_STORE_CONFIG_CCCD_ENCODE_SZ + 1)
+
+static void
+ble_store_config_serialize_arr(const void *arr, int obj_sz, int num_objs,
+                               char *out_buf, int buf_sz)
+{
+    int arr_size;
+
+    arr_size = obj_sz * num_objs;
+    assert(arr_size <= buf_sz);
+
+    base64_encode(arr, arr_size, out_buf, 1);
+}
+
+static int
+ble_store_config_deserialize_arr(const char *enc,
+                                 void *out_arr,
+                                 int obj_sz,
+                                 int *out_num_objs)
+{
+    int len;
+
+    len = base64_decode(enc, out_arr);
+    if (len < 0) {
+        return OS_EINVAL;
+    }
+
+    *out_num_objs = len / obj_sz;
+    return 0;
+}
+
+static int
+ble_store_config_conf_set(int argc, char **argv, char *val)
+{
+    int rc;
+
+    if (argc == 1) {
+        if (strcmp(argv[0], "our_sec") == 0) {
+            rc = ble_store_config_deserialize_arr(
+                    val,
+                    ble_store_config_our_secs,
+                    sizeof *ble_store_config_our_secs,
+                    &ble_store_config_num_our_secs);
+            return rc;
+        } else if (strcmp(argv[0], "peer_sec") == 0) {
+            rc = ble_store_config_deserialize_arr(
+                    val,
+                    ble_store_config_peer_secs,
+                    sizeof *ble_store_config_peer_secs,
+                    &ble_store_config_num_peer_secs);
+            return rc;
+        } else if (strcmp(argv[0], "cccd") == 0) {
+            rc = ble_store_config_deserialize_arr(
+                    val,
+                    ble_store_config_cccds,
+                    sizeof *ble_store_config_cccds,
+                    &ble_store_config_num_cccds);
+            return rc;
+        }
+    }
+    return OS_ENOENT;
+}
+
+static int
+ble_store_config_conf_export(void (*func)(char *name, char *val),
+                             enum conf_export_tgt tgt)
+{
+    union {
+        char sec[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ];
+        char cccd[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ];
+    } buf;
+
+    ble_store_config_serialize_arr(ble_store_config_our_secs,
+                                   sizeof *ble_store_config_our_secs,
+                                   ble_store_config_num_our_secs,
+                                   buf.sec,
+                                   sizeof buf.sec);
+    func("ble_hs/our_sec", buf.sec);
+
+    ble_store_config_serialize_arr(ble_store_config_peer_secs,
+                                   sizeof *ble_store_config_peer_secs,
+                                   ble_store_config_num_peer_secs,
+                                   buf.sec,
+                                   sizeof buf.sec);
+    func("ble_hs/peer_sec", buf.sec);
+
+    ble_store_config_serialize_arr(ble_store_config_cccds,
+                                   sizeof *ble_store_config_cccds,
+                                   ble_store_config_num_cccds,
+                                   buf.cccd,
+                                   sizeof buf.cccd);
+    func("ble_hs/cccd", buf.cccd);
+
+    return 0;
+}
+
+static int
+ble_store_config_persist_sec_set(const char *setting_name,
+                                 const struct ble_store_value_sec *secs,
+                                 int num_secs)
+{
+    char buf[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ];
+    int rc;
+
+    ble_store_config_serialize_arr(secs, sizeof *secs, num_secs,
+                                   buf, sizeof buf);
+    rc = conf_save_one(setting_name, buf);
+    if (rc != 0) {
+        return BLE_HS_ESTORE_FAIL;
+    }
+
+    return 0;
+}
+
+int
+ble_store_config_persist_our_secs(void)
+{
+    int rc;
+
+    rc = ble_store_config_persist_sec_set("ble_hs/our_sec",
+                                          ble_store_config_our_secs,
+                                          ble_store_config_num_our_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_store_config_persist_peer_secs(void)
+{
+    int rc;
+
+    rc = ble_store_config_persist_sec_set("ble_hs/peer_sec",
+                                          ble_store_config_peer_secs,
+                                          ble_store_config_num_peer_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_store_config_persist_cccds(void)
+{
+    char buf[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ];
+    int rc;
+
+    ble_store_config_serialize_arr(ble_store_config_cccds,
+                                   sizeof *ble_store_config_cccds,
+                                   ble_store_config_num_cccds,
+                                   buf,
+                                   sizeof buf);
+    rc = conf_save_one("ble_hs/cccd", buf);
+    if (rc != 0) {
+        return BLE_HS_ESTORE_FAIL;
+    }
+
+    return 0;
+}
+
+void
+ble_store_config_conf_init(void)
+{
+    int rc;
+
+    rc = conf_register(&ble_store_config_conf_handler);
+    SYSINIT_PANIC_ASSERT_MSG(rc == 0,
+                             "Failed to register ble_store_config conf");
+}
+
+#endif /* MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) */
diff --git a/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config_priv.h b/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config_priv.h
new file mode 100644
index 00000000..bae90e97
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/config/src/ble_store_config_priv.h
@@ -0,0 +1,59 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_STORE_CONFIG_PRIV_
+#define H_BLE_STORE_CONFIG_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern struct ble_store_value_sec
+    ble_store_config_our_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+extern int ble_store_config_num_our_secs;
+
+extern struct ble_store_value_sec
+    ble_store_config_peer_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+extern int ble_store_config_num_peer_secs;
+
+extern struct ble_store_value_cccd
+    ble_store_config_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)];
+extern int ble_store_config_num_cccds;
+
+#if MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST)
+
+int ble_store_config_persist_our_secs(void);
+int ble_store_config_persist_peer_secs(void);
+int ble_store_config_persist_cccds(void);
+void ble_store_config_conf_init(void);
+
+#else
+
+static inline int ble_store_config_persist_our_secs(void)   { return 0; }
+static inline int ble_store_config_persist_peer_secs(void)  { return 0; }
+static inline int ble_store_config_persist_cccds(void)      { return 0; }
+static inline void ble_store_config_conf_init(void)         { }
+
+#endif /* MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/store/config/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/store/config/syscfg.yml
new file mode 100644
index 00000000..ff0689c6
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/config/syscfg.yml
@@ -0,0 +1,27 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_STORE_CONFIG_PERSIST:
+        description: >
+            Whether to save data to sys/config, or just keep it in RAM.
+        value: 1
+    BLE_STORE_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for BLE host store.
+        value: 500
diff --git a/src/libs/mynewt-nimble/nimble/host/store/ram/include/store/ram/ble_store_ram.h b/src/libs/mynewt-nimble/nimble/host/store/ram/include/store/ram/ble_store_ram.h
new file mode 100644
index 00000000..842fb5f3
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/ram/include/store/ram/ble_store_ram.h
@@ -0,0 +1,39 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_STORE_RAM_
+#define H_BLE_STORE_RAM_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+union ble_store_key;
+union ble_store_value;
+
+int ble_store_ram_read(int obj_type, const union ble_store_key *key,
+                       union ble_store_value *value);
+int ble_store_ram_write(int obj_type, const union ble_store_value *val);
+int ble_store_ram_delete(int obj_type, const union ble_store_key *key);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/store/ram/pkg.yml b/src/libs/mynewt-nimble/nimble/host/store/ram/pkg.yml
new file mode 100644
index 00000000..68765fdd
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/ram/pkg.yml
@@ -0,0 +1,37 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/store/ram
+pkg.description: >
+    DEPRECATED; for a RAM-only BLE store, use store/config and set
+    BLE_STORE_CONFIG_PERSIST to 0.  RAM-based persistence layer for the NimBLE
+    host.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - persistence
+
+pkg.deps:
+    - nimble/host
+
+pkg.init:
+    ble_store_ram_init: 'MYNEWT_VAL(BLE_STORE_RAM_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/host/store/ram/src/ble_store_ram.c b/src/libs/mynewt-nimble/nimble/host/store/ram/src/ble_store_ram.c
new file mode 100644
index 00000000..ab5cdb9f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/ram/src/ble_store_ram.c
@@ -0,0 +1,497 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/**
+ * This file implements a simple in-RAM key database for BLE host security
+ * material and CCCDs.  As this database is only ble_store_ramd in RAM, its
+ * contents are lost when the application terminates.
+ */
+
+#include <inttypes.h>
+#include <string.h>
+
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "host/ble_hs.h"
+#include "store/ram/ble_store_ram.h"
+
+static struct ble_store_value_sec
+    ble_store_ram_our_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+static int ble_store_ram_num_our_secs;
+
+static struct ble_store_value_sec
+    ble_store_ram_peer_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
+static int ble_store_ram_num_peer_secs;
+
+static struct ble_store_value_cccd
+    ble_store_ram_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)];
+static int ble_store_ram_num_cccds;
+
+/*****************************************************************************
+ * $sec                                                                      *
+ *****************************************************************************/
+
+static void
+ble_store_ram_print_value_sec(const struct ble_store_value_sec *sec)
+{
+    if (sec->ltk_present) {
+        BLE_HS_LOG(DEBUG, "ediv=%u rand=%llu authenticated=%d ltk=",
+                       sec->ediv, sec->rand_num, sec->authenticated);
+        ble_hs_log_flat_buf(sec->ltk, 16);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+    if (sec->irk_present) {
+        BLE_HS_LOG(DEBUG, "irk=");
+        ble_hs_log_flat_buf(sec->irk, 16);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+    if (sec->csrk_present) {
+        BLE_HS_LOG(DEBUG, "csrk=");
+        ble_hs_log_flat_buf(sec->csrk, 16);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+
+    BLE_HS_LOG(DEBUG, "\n");
+}
+
+static void
+ble_store_ram_print_key_sec(const struct ble_store_key_sec *key_sec)
+{
+    if (ble_addr_cmp(&key_sec->peer_addr, BLE_ADDR_ANY)) {
+        BLE_HS_LOG(DEBUG, "peer_addr_type=%d peer_addr=",
+                       key_sec->peer_addr.type);
+        ble_hs_log_flat_buf(key_sec->peer_addr.val, 6);
+        BLE_HS_LOG(DEBUG, " ");
+    }
+    if (key_sec->ediv_rand_present) {
+        BLE_HS_LOG(DEBUG, "ediv=0x%02x rand=0x%llx ",
+                       key_sec->ediv, key_sec->rand_num);
+    }
+}
+
+static int
+ble_store_ram_find_sec(const struct ble_store_key_sec *key_sec,
+                       const struct ble_store_value_sec *value_secs,
+                       int num_value_secs)
+{
+    const struct ble_store_value_sec *cur;
+    int skipped;
+    int i;
+
+    skipped = 0;
+
+    for (i = 0; i < num_value_secs; i++) {
+        cur = value_secs + i;
+
+        if (ble_addr_cmp(&key_sec->peer_addr, BLE_ADDR_ANY)) {
+            if (ble_addr_cmp(&cur->peer_addr, &key_sec->peer_addr)) {
+                continue;
+            }
+        }
+
+        if (key_sec->ediv_rand_present) {
+            if (cur->ediv != key_sec->ediv) {
+                continue;
+            }
+
+            if (cur->rand_num != key_sec->rand_num) {
+                continue;
+            }
+        }
+
+        if (key_sec->idx > skipped) {
+            skipped++;
+            continue;
+        }
+
+        return i;
+    }
+
+    return -1;
+}
+
+static int
+ble_store_ram_read_our_sec(const struct ble_store_key_sec *key_sec,
+                           struct ble_store_value_sec *value_sec)
+{
+    int idx;
+
+    idx = ble_store_ram_find_sec(key_sec, ble_store_ram_our_secs,
+                                 ble_store_ram_num_our_secs);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    *value_sec = ble_store_ram_our_secs[idx];
+    return 0;
+}
+
+static int
+ble_store_ram_write_our_sec(const struct ble_store_value_sec *value_sec)
+{
+    struct ble_store_key_sec key_sec;
+    int idx;
+
+    BLE_HS_LOG(DEBUG, "persisting our sec; ");
+    ble_store_ram_print_value_sec(value_sec);
+
+    ble_store_key_from_value_sec(&key_sec, value_sec);
+    idx = ble_store_ram_find_sec(&key_sec, ble_store_ram_our_secs,
+                                 ble_store_ram_num_our_secs);
+    if (idx == -1) {
+        if (ble_store_ram_num_our_secs >= MYNEWT_VAL(BLE_STORE_MAX_BONDS)) {
+            BLE_HS_LOG(DEBUG, "error persisting our sec; too many entries "
+                              "(%d)\n", ble_store_ram_num_our_secs);
+            return BLE_HS_ESTORE_CAP;
+        }
+
+        idx = ble_store_ram_num_our_secs;
+        ble_store_ram_num_our_secs++;
+    }
+
+    ble_store_ram_our_secs[idx] = *value_sec;
+    return 0;
+}
+
+static int
+ble_store_ram_delete_obj(void *values, int value_size, int idx,
+                         int *num_values)
+{
+    uint8_t *dst;
+    uint8_t *src;
+    int move_count;
+
+    (*num_values)--;
+    if (idx < *num_values) {
+        dst = values;
+        dst += idx * value_size;
+        src = dst + value_size;
+
+        move_count = *num_values - idx;
+        memmove(dst, src, move_count);
+    }
+
+    return 0;
+}
+
+static int
+ble_store_ram_delete_sec(const struct ble_store_key_sec *key_sec,
+                         struct ble_store_value_sec *value_secs,
+                         int *num_value_secs)
+{
+    int idx;
+    int rc;
+
+    idx = ble_store_ram_find_sec(key_sec, value_secs, *num_value_secs);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_store_ram_delete_obj(value_secs, sizeof *value_secs, idx,
+                                  num_value_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_ram_delete_our_sec(const struct ble_store_key_sec *key_sec)
+{
+    int rc;
+
+    rc = ble_store_ram_delete_sec(key_sec, ble_store_ram_our_secs,
+                                  &ble_store_ram_num_our_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_ram_delete_peer_sec(const struct ble_store_key_sec *key_sec)
+{
+    int rc;
+
+    rc = ble_store_ram_delete_sec(key_sec, ble_store_ram_peer_secs,
+                                  &ble_store_ram_num_peer_secs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_ram_read_peer_sec(const struct ble_store_key_sec *key_sec,
+                            struct ble_store_value_sec *value_sec)
+{
+    int idx;
+
+    idx = ble_store_ram_find_sec(key_sec, ble_store_ram_peer_secs,
+                             ble_store_ram_num_peer_secs);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    *value_sec = ble_store_ram_peer_secs[idx];
+    return 0;
+}
+
+static int
+ble_store_ram_write_peer_sec(const struct ble_store_value_sec *value_sec)
+{
+    struct ble_store_key_sec key_sec;
+    int idx;
+
+    BLE_HS_LOG(DEBUG, "persisting peer sec; ");
+    ble_store_ram_print_value_sec(value_sec);
+
+    ble_store_key_from_value_sec(&key_sec, value_sec);
+    idx = ble_store_ram_find_sec(&key_sec, ble_store_ram_peer_secs,
+                                 ble_store_ram_num_peer_secs);
+    if (idx == -1) {
+        if (ble_store_ram_num_peer_secs >= MYNEWT_VAL(BLE_STORE_MAX_BONDS)) {
+            BLE_HS_LOG(DEBUG, "error persisting peer sec; too many entries "
+                             "(%d)\n", ble_store_ram_num_peer_secs);
+            return BLE_HS_ESTORE_CAP;
+        }
+
+        idx = ble_store_ram_num_peer_secs;
+        ble_store_ram_num_peer_secs++;
+    }
+
+    ble_store_ram_peer_secs[idx] = *value_sec;
+    return 0;
+}
+
+/*****************************************************************************
+ * $cccd                                                                     *
+ *****************************************************************************/
+
+static int
+ble_store_ram_find_cccd(const struct ble_store_key_cccd *key)
+{
+    struct ble_store_value_cccd *cccd;
+    int skipped;
+    int i;
+
+    skipped = 0;
+    for (i = 0; i < ble_store_ram_num_cccds; i++) {
+        cccd = ble_store_ram_cccds + i;
+
+        if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) {
+            if (ble_addr_cmp(&cccd->peer_addr, &key->peer_addr)) {
+                continue;
+            }
+        }
+
+        if (key->chr_val_handle != 0) {
+            if (cccd->chr_val_handle != key->chr_val_handle) {
+                continue;
+            }
+        }
+
+        if (key->idx > skipped) {
+            skipped++;
+            continue;
+        }
+
+        return i;
+    }
+
+    return -1;
+}
+
+static int
+ble_store_ram_delete_cccd(const struct ble_store_key_cccd *key_cccd)
+{
+    int idx;
+    int rc;
+
+    idx = ble_store_ram_find_cccd(key_cccd);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    rc = ble_store_ram_delete_obj(ble_store_ram_cccds,
+                                  sizeof *ble_store_ram_cccds,
+                                  idx,
+                                  &ble_store_ram_num_cccds);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_store_ram_read_cccd(const struct ble_store_key_cccd *key_cccd,
+                        struct ble_store_value_cccd *value_cccd)
+{
+    int idx;
+
+    idx = ble_store_ram_find_cccd(key_cccd);
+    if (idx == -1) {
+        return BLE_HS_ENOENT;
+    }
+
+    *value_cccd = ble_store_ram_cccds[idx];
+    return 0;
+}
+
+static int
+ble_store_ram_write_cccd(const struct ble_store_value_cccd *value_cccd)
+{
+    struct ble_store_key_cccd key_cccd;
+    int idx;
+
+    ble_store_key_from_value_cccd(&key_cccd, value_cccd);
+    idx = ble_store_ram_find_cccd(&key_cccd);
+    if (idx == -1) {
+        if (ble_store_ram_num_cccds >= MYNEWT_VAL(BLE_STORE_MAX_CCCDS)) {
+            BLE_HS_LOG(DEBUG, "error persisting cccd; too many entries (%d)\n",
+                       ble_store_ram_num_cccds);
+            return BLE_HS_ESTORE_CAP;
+        }
+
+        idx = ble_store_ram_num_cccds;
+        ble_store_ram_num_cccds++;
+    }
+
+    ble_store_ram_cccds[idx] = *value_cccd;
+    return 0;
+}
+
+/*****************************************************************************
+ * $api                                                                      *
+ *****************************************************************************/
+
+/**
+ * Searches the database for an object matching the specified criteria.
+ *
+ * @return                      0 if a key was found; else BLE_HS_ENOENT.
+ */
+int
+ble_store_ram_read(int obj_type, const union ble_store_key *key,
+                   union ble_store_value *value)
+{
+    int rc;
+
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        /* An encryption procedure (bonding) is being attempted.  The nimble
+         * stack is asking us to look in our key database for a long-term key
+         * corresponding to the specified ediv and random number.
+         *
+         * Perform a key lookup and populate the context object with the
+         * result.  The nimble stack will use this key if this function returns
+         * success.
+         */
+        BLE_HS_LOG(DEBUG, "looking up peer sec; ");
+        ble_store_ram_print_key_sec(&key->sec);
+        BLE_HS_LOG(DEBUG, "\n");
+        rc = ble_store_ram_read_peer_sec(&key->sec, &value->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        BLE_HS_LOG(DEBUG, "looking up our sec; ");
+        ble_store_ram_print_key_sec(&key->sec);
+        BLE_HS_LOG(DEBUG, "\n");
+        rc = ble_store_ram_read_our_sec(&key->sec, &value->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        rc = ble_store_ram_read_cccd(&key->cccd, &value->cccd);
+        return rc;
+
+    default:
+        return BLE_HS_ENOTSUP;
+    }
+}
+
+/**
+ * Adds the specified object to the database.
+ *
+ * @return                      0 on success; BLE_HS_ESTORE_CAP if the database
+ *                                  is full.
+ */
+int
+ble_store_ram_write(int obj_type, const union ble_store_value *val)
+{
+    int rc;
+
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        rc = ble_store_ram_write_peer_sec(&val->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        rc = ble_store_ram_write_our_sec(&val->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        rc = ble_store_ram_write_cccd(&val->cccd);
+        return rc;
+
+    default:
+        return BLE_HS_ENOTSUP;
+    }
+}
+
+int
+ble_store_ram_delete(int obj_type, const union ble_store_key *key)
+{
+    int rc;
+
+    switch (obj_type) {
+    case BLE_STORE_OBJ_TYPE_PEER_SEC:
+        rc = ble_store_ram_delete_peer_sec(&key->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_OUR_SEC:
+        rc = ble_store_ram_delete_our_sec(&key->sec);
+        return rc;
+
+    case BLE_STORE_OBJ_TYPE_CCCD:
+        rc = ble_store_ram_delete_cccd(&key->cccd);
+        return rc;
+
+    default:
+        return BLE_HS_ENOTSUP;
+    }
+}
+
+void
+ble_store_ram_init(void)
+{
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    ble_hs_cfg.store_read_cb = ble_store_ram_read;
+    ble_hs_cfg.store_write_cb = ble_store_ram_write;
+    ble_hs_cfg.store_delete_cb = ble_store_ram_delete;
+
+    /* Re-initialize BSS values in case of unit tests. */
+    ble_store_ram_num_our_secs = 0;
+    ble_store_ram_num_peer_secs = 0;
+    ble_store_ram_num_cccds = 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/store/ram/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/store/ram/syscfg.yml
new file mode 100644
index 00000000..442211dd
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/store/ram/syscfg.yml
@@ -0,0 +1,23 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+
+syscfg.defs:
+    BLE_STORE_RAM_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the RAM BLE store.
+        value: 500
+
diff --git a/src/libs/mynewt-nimble/nimble/host/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/syscfg.yml
new file mode 100644
index 00000000..e72e8d52
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/syscfg.yml
@@ -0,0 +1,471 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs:
+    BLE_HOST:
+        description: 'Indicates that a BLE host is present.'
+        value: 1
+
+    BLE_HS_AUTO_START:
+        description: >
+                Causes the BLE host to automatically start during system
+                initialization.
+        value: 1
+
+    # Debug settings.
+    BLE_HS_DEBUG:
+        description: 'Enables extra runtime assertions.'
+        value: 0
+    BLE_HS_PHONY_HCI_ACKS:
+        description: >
+            Rather than wait for HCI acknowledgements from a controller, the
+            host simulates incoming acks.  Only recommended for test code
+            running in the simulator.
+        value: 0
+    BLE_HS_REQUIRE_OS:
+        description: >
+            Specifies whether the host can depend on the kernel being present.
+            This should only be disabled for unit tests running in the
+            simulator.
+        value: 1
+
+    # Monitor interface settings
+    BLE_MONITOR_UART:
+        description: Enables monitor interface over UART
+        value: 0
+    BLE_MONITOR_UART_DEV:
+        description: Monitor interface UART device
+        value: '"uart0"'
+    BLE_MONITOR_UART_BAUDRATE:
+        description: Baudrate for monitor interface UART
+        value: 1000000
+    BLE_MONITOR_UART_BUFFER_SIZE:
+        description: >
+            Monitor interface ringbuffer size for UART.
+            This value should be a power of 2.
+        value: 64
+    BLE_MONITOR_RTT:
+        description: Enables monitor interface over RTT
+        value: 0
+    BLE_MONITOR_RTT_BUFFER_NAME:
+        description: Monitor interface upstream buffer name
+        value: '"btmonitor"'
+    BLE_MONITOR_RTT_BUFFER_SIZE:
+        description: Monitor interface upstream buffer size
+        value: 256
+    BLE_MONITOR_RTT_BUFFERED:
+        description: >
+            Enables buffering when using monitor interface over RTT. The data
+            are written to RTT once complete packet is created in intermediate
+            buffer. This allows to skip complete packet if there is not enough
+            space in RTT buffer (e.g. there is no reader connected). If disabled,
+            monitor will simply block waiting for RTT to free space in buffer.
+        value: 1
+    BLE_MONITOR_CONSOLE_BUFFER_SIZE:
+        description: >
+            Size of internal buffer for console output. Any line exceeding this
+            length value will be split.
+        value: 128
+
+    # L2CAP settings.
+    BLE_L2CAP_MAX_CHANS:
+        description: >
+            The number of L2CAP channels to allocate.  The default value allows
+            for the signal, ATT, and SM channels for each connection.
+        value: '3*MYNEWT_VAL_BLE_MAX_CONNECTIONS'
+    BLE_L2CAP_SIG_MAX_PROCS:
+        description: >
+            The maximum number of concurrent L2CAP signal procedures.
+        value: 1
+    BLE_L2CAP_JOIN_RX_FRAGS:
+        description: >
+            Whether to collapse incoming L2CAP fragments into a minimal set of
+            mbufs.
+                1: Slower, more memory efficient.
+                0: Faster, less memory efficient.
+        value: 1
+    BLE_L2CAP_RX_FRAG_TIMEOUT:
+        description: >
+            Expiry time for incoming data packets (ms).  If this much time
+            passes since the previous fragment was received, the connection is
+            terminated.  A value of 0 means no timeout.
+        value: 30000
+    BLE_L2CAP_COC_MAX_NUM:
+        description: >
+            Defines maximum number of LE Connection Oriented Channels channels.
+            When set to (0), LE COC is not compiled in.
+        value: 0
+    BLE_L2CAP_COC_MPS:
+        description: >
+            Defines the MPS of L2CAP COC module. This is actually NimBLE's internal
+            L2CAP MTU. The default MPS size is chosen in a way, that the MPS plus
+            the required HCI and L2CAP headers fit into the smallest available
+            MSYS blocks.
+        value: 'MYNEWT_VAL_MSYS_1_BLOCK_SIZE-8'
+
+    BLE_L2CAP_ENHANCED_COC:
+        description: >
+            Enables LE Enhanced CoC mode.
+        value: 0
+        restrictions:
+            - '(BLE_L2CAP_COC_MAX_NUM > 0) && (BLE_VERSION >= 52) if 1'
+
+    # Security manager settings.
+    BLE_SM_LEGACY:
+        description: 'Security manager legacy pairing.'
+        value: 1
+    BLE_SM_SC:
+        description: 'Security manager secure connections (4.2).'
+        value: 0
+
+    BLE_SM_MAX_PROCS:
+        description: >
+            The maximum number of concurrent security manager procedures.
+        value: 1
+    BLE_SM_IO_CAP:
+        description: >
+            The IO capabilities to report during pairing.  Valid values are:
+                BLE_HS_IO_DISPLAY_ONLY
+                BLE_HS_IO_DISPLAY_YESNO
+                BLE_HS_IO_KEYBOARD_ONLY
+                BLE_HS_IO_NO_INPUT_OUTPUT
+                BLE_HS_IO_KEYBOARD_DISPLAY
+        value: 'BLE_HS_IO_NO_INPUT_OUTPUT'
+    BLE_SM_OOB_DATA_FLAG:
+        description: >
+            Whether the out-of-band pairing algorithm is advertised. (0/1)
+        value: 0
+    BLE_SM_BONDING:
+        description: >
+            Enables bonding (persistence and restoration of secure links). (0/1)
+        value: 0
+    BLE_SM_MITM:
+        description: >
+            Whether man-in-the-middle protection is advertised during
+            pairing. (0/1)
+        value: 0
+    BLE_SM_KEYPRESS:
+        description: >
+            Whether keypress support is advertised during pairing. (0/1)
+        value: 0
+    BLE_SM_OUR_KEY_DIST:
+        description: >
+            A bitmap indicating which keys to distribute during pairing.  The
+            bits are defined as follows:
+                0x01: BLE_SM_PAIR_KEY_DIST_ENC
+                0x02: BLE_SM_PAIR_KEY_DIST_ID
+                0x04: BLE_SM_PAIR_KEY_DIST_SIGN
+                0x08: BLE_SM_PAIR_KEY_DIST_LINK
+        value: 0
+    BLE_SM_THEIR_KEY_DIST:
+        description: >
+            A bitmap indicating which keys to accept during pairing.  The
+            bits are defined as follows:
+                0x01: BLE_SM_PAIR_KEY_DIST_ENC
+                0x02: BLE_SM_PAIR_KEY_DIST_ID
+                0x04: BLE_SM_PAIR_KEY_DIST_SIGN
+                0x08: BLE_SM_PAIR_KEY_DIST_LINK
+        value: 0
+    BLE_SM_SC_DEBUG_KEYS:
+        description: >
+            Enable SM debug mode. In this mode SM uses predefined DH key pair as
+            described in Core Specification 5.0, Vol. 3, Part H, 2.3.5.6.1. This
+            allows to decrypt air traffic easily and thus should be only used
+            for debugging.
+        value: 0
+
+    # GAP options.
+    BLE_GAP_MAX_PENDING_CONN_PARAM_UPDATE:
+        description: >
+            Controls the number of connection parameter updates that can be pending
+            simultaneously. Devices with many concurrent connections may need
+            to increase this value.
+        value: 1
+
+    # Supported GATT procedures.  By default:
+    #     o Notify and indicate are enabled;
+    #     o All other procedures are enabled for centrals.
+    BLE_GATT_DISC_ALL_SVCS:
+        description: >
+            Enables the Discover All Primary Services GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_DISC_SVC_UUID:
+        description: >
+            Enables the Discover Primary Services by Service UUID GATT
+            procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_FIND_INC_SVCS:
+        description: >
+            Enables the Find Included Services GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_DISC_ALL_CHRS:
+        description: >
+            Enables the Discover All Characteristics of a Service GATT
+            procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_DISC_CHR_UUID:
+        description: >
+            Enables the Discover Characteristics by UUID GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_DISC_ALL_DSCS:
+        description: >
+            Enables the Discover All Primary Services GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_READ:
+        description: >
+            Enables the Read Characteristic Value GATT procedure. (0/1)
+            (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_READ_UUID:
+        description: >
+            Enables the Read Using Characteristic UUID GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_READ_LONG:
+        description: >
+            Enables the Read Long Characteristic Values GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_READ_MULT:
+        description: >
+            Enables the Read Multiple Characteristic Values GATT procedure.
+            (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_WRITE_NO_RSP:
+        description: >
+            Enables the Write Without Response GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_SIGNED_WRITE:
+        description: >
+            Enables the Signed Write Without Response GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_WRITE:
+        description: >
+            Enables the Write Characteristic Value GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_WRITE_LONG:
+        description: >
+            Enables the Write Long Characteristic Values GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_WRITE_RELIABLE:
+        description: >
+            Enables the Reliable Writes GATT procedure. (0/1)
+        value: MYNEWT_VAL_BLE_ROLE_CENTRAL
+    BLE_GATT_NOTIFY:
+        description: >
+            Enables sending and receiving of GATT notifications. (0/1)
+        value: 1
+    BLE_GATT_INDICATE:
+        description: >
+            Enables sending and receiving of GATT indications. (0/1)
+        value: 1
+
+    # GATT options.
+    BLE_GATT_READ_MAX_ATTRS:
+        description: >
+            The maximum number of attributes that can be read with a single
+            GATT Read Multiple Characteristic Values procedure. (0/1)
+        value: 8
+    BLE_GATT_WRITE_MAX_ATTRS:
+        description: >
+            The maximum number of attributes that can be written with a single
+            GATT Reliable Write procedure. (0/1)
+        value: 4
+    BLE_GATT_MAX_PROCS:
+        description: >
+            The maximum number of concurrent client GATT procedures. (0/1)
+        value: 4
+    BLE_GATT_RESUME_RATE:
+        description: >
+            The rate to periodically resume GATT procedures that have stalled
+            due to memory exhaustion. (0/1)  Units are milliseconds. (0/1)
+        value: 1000
+
+    # Supported server ATT commands. (0/1)
+    BLE_ATT_SVR_FIND_INFO:
+        description: >
+            Enables processing of incoming Find Information Request ATT
+            commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_FIND_TYPE:
+        description: >
+            Enables processing of incoming Find By Type Value Request ATT
+            commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_READ_TYPE:
+        description: >
+            Enables processing of incoming Read By Type Request ATT commands.
+            (0/1)
+        value: 1
+    BLE_ATT_SVR_READ:
+        description: >
+            Enables processing of incoming Read Request ATT commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_READ_BLOB:
+        description: >
+            Enables processing of incoming Read Blob Request ATT commands.
+            (0/1)
+        value: 1
+    BLE_ATT_SVR_READ_MULT:
+        description: >
+            Enables processing of incoming Read Multiple Request ATT commands.
+            (0/1)
+        value: 1
+    BLE_ATT_SVR_READ_GROUP_TYPE:
+        description: >
+            Enables processing of incoming Read by Group Type Request ATT
+            commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_WRITE:
+        description: >
+            Enables processing of incoming Write Request ATT commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_WRITE_NO_RSP:
+        description: >
+            Enables processing of incoming Write Command ATT commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_SIGNED_WRITE:
+        description: >
+            Enables processing of incoming Signed Write Command ATT commands.
+            (0/1)
+        value: 1
+    BLE_ATT_SVR_QUEUED_WRITE:
+        description: >
+            Enables processing of incoming Prepare Write Request and Execute
+            Write Request ATT commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_NOTIFY:
+        description: >
+            Enables processing of incoming Handle Value Notification ATT
+            commands. (0/1)
+        value: 1
+    BLE_ATT_SVR_INDICATE:
+        description: >
+            Enables processing of incoming Handle Value Indication ATT
+            commands.  (0/1)
+        value: 1
+
+    # ATT options.
+    BLE_ATT_PREFERRED_MTU:
+        description: The preferred MTU to indicate in MTU exchange commands.
+        value: 256
+
+    BLE_ATT_SVR_MAX_PREP_ENTRIES:
+        description: >
+            A GATT server uses these when a peer performs a "write long
+            characteristic values" or "write long characteristic descriptors"
+            procedure.  One of these resources is consumed each time a peer
+            sends a partial write.
+        value: 64
+
+    BLE_ATT_SVR_QUEUED_WRITE_TMO:
+        description: >
+            Expiry time for incoming ATT queued writes (ms).  If this much
+            time passes since the previous prepared write was received, the
+            connection is terminated.  A value of 0 means no timeout.
+        value: 30000
+
+    # Privacy options.
+    BLE_RPA_TIMEOUT:
+        description: >
+            The rate that new random addresses should be generated (seconds).
+        value: 300
+
+    # Store settings.
+    BLE_STORE_MAX_BONDS:
+        description: >
+            Maximum number of bonds that can be persisted.  Note: increasing
+            this value may also require increasing the capacity of the
+            underlying storage mechanism.
+        value: 3
+    BLE_STORE_MAX_CCCDS:
+        description: >
+            Maximum number of client characteristic configuration descriptors
+            that can be persisted.  Note: increasing this value may also
+            require increasing the capacity of the underlying storage
+            mechanism.
+
+        value: 8
+
+    BLE_MESH:
+        description: >
+            This option enables Bluetooth Mesh support. The specific
+            features that are available may depend on other features
+            that have been enabled in the stack, such as GATT support.
+        value: 0
+
+    # Flow control settings.
+    BLE_HS_FLOW_CTRL:
+        description: >
+            Whether to enable host-side flow control.  This should only be
+            enabled in host-only setups (i.e., not combined-host-controller).
+        value: 0
+
+    BLE_HS_FLOW_CTRL_ITVL:
+        description: >
+            The interval, in milliseconds, that the host should provide
+            number-of-completed-packets updates to the controller.
+        value: 1000
+
+    BLE_HS_FLOW_CTRL_THRESH:
+        description: >
+            If the number of data buffers available to the controller falls to
+            this number, immediately send a number-of-completed-packets event.
+            The free buffer count is calculated as follows:
+            (total-acl-bufs - bufs-freed-since-last-num-completed-event).
+        value: 2
+
+    BLE_HS_FLOW_CTRL_TX_ON_DISCONNECT:
+        description: >
+            If enabled, the host will immediately transmit a
+            host-number-of-completed-packets command whenever a connection
+            terminates.  This behavior is not required by the standard, but is
+            a necessary workaround when interfacing with some controllers.
+        value: 0
+
+    BLE_HS_STOP_ON_SHUTDOWN:
+        description: >
+            Stops the Bluetooth host when the system shuts down.  Stopping
+            entails aborting all GAP procedures and terminating open
+            connections.
+        value: 1
+
+    BLE_HS_STOP_ON_SHUTDOWN_TIMEOUT:
+        description: >
+            Timeout used in NimBLE's host stop procedure in ms.
+        value: 2000
+
+    BLE_HS_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the NimBLE host.
+        value: 200
+
+    ### Log settings.
+
+    BLE_HS_LOG_MOD:
+        description: 'Numeric module ID to use for BLE host log messages.'
+        value: 4
+    BLE_HS_LOG_LVL:
+        description: 'Minimum level for the BLE host log.'
+        value: 1
+
+syscfg.logs:
+    BLE_HS_LOG:
+        module: MYNEWT_VAL(BLE_HS_LOG_MOD)
+        level: MYNEWT_VAL(BLE_HS_LOG_LVL)
+
+syscfg.vals.BLE_MESH:
+    BLE_SM_SC: 1
diff --git a/src/libs/mynewt-nimble/nimble/host/test/pkg.yml b/src/libs/mynewt-nimble/nimble/host/test/pkg.yml
new file mode 100644
index 00000000..dd1ad18b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/pkg.yml
@@ -0,0 +1,34 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+pkg.name: nimble/host/test
+pkg.type: unittest
+pkg.description: "NimBLE host unit tests."
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+
+pkg.deps:
+    - "@apache-mynewt-core/test/testutil"
+    - nimble/host
+    - nimble/host/store/config
+
+pkg.deps.SELFTEST:
+    - "@apache-mynewt-core/sys/console/stub"
+    - "@apache-mynewt-core/sys/log/full"
+    - "@apache-mynewt-core/sys/stats/stub"
+    - nimble/transport/ram
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_att_clt_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_att_clt_test.c
new file mode 100644
index 00000000..787d4bca
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_att_clt_test.c
@@ -0,0 +1,548 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+/**
+ * @return                      The handle of the new test connection.
+ */
+static uint16_t
+ble_att_clt_test_misc_init(void)
+{
+    ble_hs_test_util_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}), NULL,
+                                 NULL);
+    return 2;
+}
+
+static void
+ble_att_clt_test_misc_verify_tx_write(uint16_t handle_id, void *value,
+                                      int value_len, int is_req)
+{
+    struct ble_att_write_req req;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    if (is_req) {
+        ble_att_write_req_parse(om->om_data, om->om_len, &req);
+    } else {
+        ble_att_write_cmd_parse(om->om_data, om->om_len, &req);
+    }
+
+    TEST_ASSERT(req.bawq_handle == handle_id);
+    TEST_ASSERT(om->om_len == BLE_ATT_WRITE_REQ_BASE_SZ + value_len);
+    TEST_ASSERT(memcmp(om->om_data + BLE_ATT_WRITE_REQ_BASE_SZ, value,
+                       value_len) == 0);
+}
+
+static void
+ble_att_clt_test_tx_write_req_or_cmd(uint16_t conn_handle, uint16_t handle,
+                                     void *value, int value_len, int is_req)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(value, value_len);
+    if (is_req) {
+        rc = ble_att_clt_tx_write_req(conn_handle, handle, om);
+    } else {
+        rc = ble_att_clt_tx_write_cmd(conn_handle, handle, om);
+    }
+    TEST_ASSERT(rc == 0);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_find_info)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    rc = ble_att_clt_tx_find_info(conn_handle, 1, 0xffff);
+    TEST_ASSERT(rc == 0);
+
+    /*** Error: start handle of 0. */
+    rc = ble_att_clt_tx_find_info(conn_handle, 0, 0xffff);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Error: start handle greater than end handle. */
+    rc = ble_att_clt_tx_find_info(conn_handle, 500, 499);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Success; start and end handles equal. */
+    rc = ble_att_clt_tx_find_info(conn_handle, 500, 500);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_rx_find_info)
+{
+    struct ble_att_find_info_rsp rsp;
+    uint16_t conn_handle;
+    uint8_t buf[1024];
+    uint8_t uuid128_1[16] = { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 };
+    int off;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** One 128-bit UUID. */
+    /* Receive response with attribute mapping. */
+    off = 0;
+    rsp.bafp_format = BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT;
+    ble_att_find_info_rsp_write(buf + off, sizeof buf - off, &rsp);
+    off += BLE_ATT_FIND_INFO_RSP_BASE_SZ;
+
+    put_le16(buf + off, 1);
+    off += 2;
+    memcpy(buf + off, uuid128_1, 16);
+    off += 16;
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    /*** One 16-bit UUID. */
+    /* Receive response with attribute mapping. */
+    off = 0;
+    rsp.bafp_format = BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT;
+    ble_att_find_info_rsp_write(buf + off, sizeof buf - off, &rsp);
+    off += BLE_ATT_FIND_INFO_RSP_BASE_SZ;
+
+    put_le16(buf + off, 2);
+    off += 2;
+    put_le16(buf + off, 0x000f);
+    off += 2;
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    /*** Two 16-bit UUIDs. */
+    /* Receive response with attribute mappings. */
+    off = 0;
+    rsp.bafp_format = BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT;
+    ble_att_find_info_rsp_write(buf + off, sizeof buf - off, &rsp);
+    off += BLE_ATT_FIND_INFO_RSP_BASE_SZ;
+
+    put_le16(buf + off, 3);
+    off += 2;
+    put_le16(buf + off, 0x0010);
+    off += 2;
+
+    put_le16(buf + off, 4);
+    off += 2;
+    put_le16(buf + off, 0x0011);
+    off += 2;
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+ble_att_clt_test_case_tx_write_req_or_cmd(int is_req)
+{
+    uint16_t conn_handle;
+    uint8_t value300[500] = { 0 };
+    uint8_t value5[5] = { 6, 7, 54, 34, 8 };
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** 5-byte write. */
+    ble_att_clt_test_tx_write_req_or_cmd(conn_handle, 0x1234, value5,
+                                         sizeof value5, is_req);
+    ble_att_clt_test_misc_verify_tx_write(0x1234, value5, sizeof value5,
+                                          is_req);
+
+    /*** Overlong write; verify command truncated to ATT MTU. */
+    ble_att_clt_test_tx_write_req_or_cmd(conn_handle, 0xab83, value300,
+                                         sizeof value300, is_req);
+    ble_att_clt_test_misc_verify_tx_write(0xab83, value300,
+                                          BLE_ATT_MTU_DFLT - 3, is_req);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+ble_att_clt_test_misc_prep_good(uint16_t handle, uint16_t offset,
+                                uint8_t *attr_data, uint16_t attr_data_len)
+{
+    struct ble_att_prep_write_cmd req;
+    struct os_mbuf *om;
+    uint16_t conn_handle;
+    int rc;
+    int i;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    om = ble_hs_test_util_om_from_flat(attr_data, attr_data_len);
+    rc = ble_att_clt_tx_prep_write(conn_handle, handle, offset, om);
+    TEST_ASSERT(rc == 0);
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(om->om_len == BLE_ATT_PREP_WRITE_CMD_BASE_SZ + attr_data_len);
+
+    ble_att_prep_write_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.bapc_handle == handle);
+    TEST_ASSERT(req.bapc_offset == offset);
+    for (i = 0; i < attr_data_len; i++) {
+        TEST_ASSERT(om->om_data[BLE_ATT_PREP_WRITE_CMD_BASE_SZ + i] ==
+                    attr_data[i]);
+    }
+}
+
+static void
+ble_att_clt_test_misc_exec_good(uint8_t flags)
+{
+    struct ble_att_exec_write_req req;
+    struct os_mbuf *om;
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    rc = ble_att_clt_tx_exec_write(conn_handle, flags);
+    TEST_ASSERT(rc == 0);
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(om->om_len == BLE_ATT_EXEC_WRITE_REQ_SZ);
+
+    ble_att_exec_write_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.baeq_flags == flags);
+}
+
+static void
+ble_att_clt_test_misc_prep_bad(uint16_t handle, uint16_t offset,
+                               uint8_t *attr_data, uint16_t attr_data_len,
+                               int status)
+{
+    struct os_mbuf *om;
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    om = ble_hs_test_util_om_from_flat(attr_data, attr_data_len);
+
+    rc = ble_att_clt_tx_prep_write(conn_handle, handle, offset, om);
+    TEST_ASSERT(rc == status);
+}
+
+static void
+ble_att_clt_test_misc_tx_mtu(uint16_t conn_handle, uint16_t mtu, int status)
+{
+    int rc;
+
+    rc = ble_att_clt_tx_mtu(conn_handle, mtu);
+    TEST_ASSERT(rc == status);
+
+    if (rc == 0) {
+        ble_hs_test_util_verify_tx_mtu_cmd(1, mtu);
+    }
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_write)
+{
+    ble_att_clt_test_case_tx_write_req_or_cmd(0);
+    ble_att_clt_test_case_tx_write_req_or_cmd(1);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_read)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    rc = ble_att_clt_tx_read(conn_handle, 1);
+    TEST_ASSERT(rc == 0);
+
+    /*** Error: handle of 0. */
+    rc = ble_att_clt_tx_read(conn_handle, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_rx_read)
+{
+    uint16_t conn_handle;
+    uint8_t buf[1024];
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Basic success. */
+    buf[0] = BLE_ATT_OP_READ_RSP;
+    buf[1] = 0;
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 2);
+    TEST_ASSERT(rc == 0);
+
+    /*** Larger response. */
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 20);
+    TEST_ASSERT(rc == 0);
+
+    /*** Zero-length response. */
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 1);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_read_blob)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    rc = ble_att_clt_tx_read_blob(conn_handle, 1, 0);
+    TEST_ASSERT(rc == 0);
+
+    /*** Error: handle of 0. */
+    rc = ble_att_clt_tx_read_blob(conn_handle, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_rx_read_blob)
+{
+    uint16_t conn_handle;
+    uint8_t buf[1024];
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Basic success. */
+    buf[0] = BLE_ATT_OP_READ_BLOB_RSP;
+    buf[1] = 0;
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 2);
+    TEST_ASSERT(rc == 0);
+
+    /*** Larger response. */
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 20);
+    TEST_ASSERT(rc == 0);
+
+    /*** Zero-length response. */
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 1);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_read_mult)
+{
+    struct os_mbuf *om;
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    rc = ble_att_clt_tx_read_mult(conn_handle, ((uint16_t[]){ 1, 2 }), 2);
+    TEST_ASSERT(rc == 0);
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(om->om_len == BLE_ATT_READ_MULT_REQ_BASE_SZ + 4);
+
+    ble_att_read_mult_req_parse(om->om_data, om->om_len);
+    TEST_ASSERT(get_le16(om->om_data + BLE_ATT_READ_MULT_REQ_BASE_SZ) == 1);
+    TEST_ASSERT(get_le16(om->om_data + BLE_ATT_READ_MULT_REQ_BASE_SZ + 2) == 2);
+
+    /*** Error: no handles. */
+    rc = ble_att_clt_tx_read_mult(conn_handle, NULL, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_rx_read_mult)
+{
+    uint16_t conn_handle;
+    uint8_t buf[1024];
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Basic success. */
+    ble_att_read_mult_rsp_write(buf, sizeof buf);
+    put_le16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 0, 12);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_READ_MULT_RSP_BASE_SZ + 2);
+    TEST_ASSERT(rc == 0);
+
+    /*** Larger response. */
+    put_le16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 0, 12);
+    put_le16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 2, 43);
+    put_le16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 4, 91);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_READ_MULT_RSP_BASE_SZ + 6);
+    TEST_ASSERT(rc == 0);
+
+    /*** Zero-length response. */
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_READ_MULT_RSP_BASE_SZ + 0);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_prep_write)
+{
+    uint8_t attr_data[512];
+    int i;
+
+    for (i = 0; i < sizeof attr_data; i++) {
+        attr_data[i] = i;
+    }
+
+    /*** Success. */
+    ble_att_clt_test_misc_prep_good(123, 0, attr_data, 16);
+    ble_att_clt_test_misc_prep_good(5432, 100, attr_data, 2);
+    ble_att_clt_test_misc_prep_good(0x1234, 400, attr_data, 0);
+    ble_att_clt_test_misc_prep_good(5432, 0, attr_data,
+                                    BLE_ATT_MTU_DFLT -
+                                        BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    ble_att_clt_test_misc_prep_good(0x1234, 507, attr_data, 5);
+
+    /*** Error: handle of 0. */
+    ble_att_clt_test_misc_prep_bad(0, 0, attr_data, 16, BLE_HS_EINVAL);
+
+    /*** Error: offset + length greater than maximum attribute size. */
+    ble_att_clt_test_misc_prep_bad(1, 507, attr_data, 6, BLE_HS_EINVAL);
+
+    /*** Error: packet larger than MTU. */
+    ble_att_clt_test_misc_prep_bad(1, 0, attr_data,
+                                   BLE_ATT_MTU_DFLT -
+                                       BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1,
+                                   BLE_HS_EINVAL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_rx_prep_write)
+{
+    struct ble_att_prep_write_cmd rsp;
+    uint16_t conn_handle;
+    uint8_t buf[1024];
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Basic success. */
+    rsp.bapc_handle = 0x1234;
+    rsp.bapc_offset = 0;
+    ble_att_prep_write_rsp_write(buf, sizeof buf, &rsp);
+    memset(buf + BLE_ATT_PREP_WRITE_CMD_BASE_SZ, 1, 5);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 5);
+    TEST_ASSERT(rc == 0);
+
+    /*** 0-length write. */
+    rsp.bapc_handle = 0x1234;
+    rsp.bapc_offset = 0;
+    ble_att_prep_write_rsp_write(buf, sizeof buf, &rsp);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_exec_write)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    ble_att_clt_test_misc_exec_good(BLE_ATT_EXEC_WRITE_F_CANCEL);
+    ble_att_clt_test_misc_exec_good(BLE_ATT_EXEC_WRITE_F_EXECUTE);
+
+    /*** Success: nonzero == execute. */
+    rc = ble_att_clt_tx_exec_write(conn_handle, 0x02);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_att_clt_test_tx_mtu)
+{
+    uint16_t conn_handle;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    ble_att_clt_test_misc_tx_mtu(conn_handle, 50, 0);
+
+    /*** Error: repeated sends. */
+    ble_att_clt_test_misc_tx_mtu(conn_handle, 50, BLE_HS_EALREADY);
+    ble_att_clt_test_misc_tx_mtu(conn_handle, 60, BLE_HS_EALREADY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_att_clt_suite)
+{
+    ble_att_clt_test_tx_find_info();
+    ble_att_clt_test_rx_find_info();
+    ble_att_clt_test_tx_read();
+    ble_att_clt_test_rx_read();
+    ble_att_clt_test_tx_read_blob();
+    ble_att_clt_test_rx_read_blob();
+    ble_att_clt_test_tx_read_mult();
+    ble_att_clt_test_rx_read_mult();
+    ble_att_clt_test_tx_write();
+    ble_att_clt_test_tx_prep_write();
+    ble_att_clt_test_rx_prep_write();
+    ble_att_clt_test_tx_exec_write();
+    ble_att_clt_test_tx_mtu();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_att_svr_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_att_svr_test.c
new file mode 100644
index 00000000..60ab14b4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_att_svr_test.c
@@ -0,0 +1,2138 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "ble_hs_test.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+static uint8_t *ble_att_svr_test_attr_r_1;
+static uint16_t ble_att_svr_test_attr_r_1_len;
+static uint8_t *ble_att_svr_test_attr_r_2;
+static uint16_t ble_att_svr_test_attr_r_2_len;
+
+static uint8_t ble_att_svr_test_attr_w_1[1024];
+static uint16_t ble_att_svr_test_attr_w_1_len;
+static uint8_t ble_att_svr_test_attr_w_2[1024];
+static uint16_t ble_att_svr_test_attr_w_2_len;
+
+static uint16_t ble_att_svr_test_n_conn_handle;
+static uint16_t ble_att_svr_test_n_attr_handle;
+static uint8_t ble_att_svr_test_attr_n[1024];
+static uint16_t ble_att_svr_test_attr_n_len;
+
+static void
+ble_att_svr_test_assert_mbufs_freed(void)
+{
+    /* When checking for mbuf leaks, ensure no stale prep entries. */
+    static const struct ble_hs_test_util_mbuf_params mbuf_params = {
+        .prev_tx = 1,
+        .rx_queue = 1,
+        .prep_list = 0,
+    };
+
+    ble_hs_test_util_assert_mbufs_freed(&mbuf_params);
+}
+
+static int
+ble_att_svr_test_misc_gap_cb(struct ble_gap_event *event, void *arg)
+{
+    int rc;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_NOTIFY_RX:
+        ble_att_svr_test_n_conn_handle = event->notify_rx.conn_handle;
+        ble_att_svr_test_n_attr_handle = event->notify_rx.attr_handle;
+        TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(event->notify_rx.om) <=
+                          sizeof ble_att_svr_test_attr_n);
+        ble_att_svr_test_attr_n_len = OS_MBUF_PKTLEN(event->notify_rx.om);
+        rc = os_mbuf_copydata(event->notify_rx.om, 0,
+                              ble_att_svr_test_attr_n_len,
+                              ble_att_svr_test_attr_n);
+        TEST_ASSERT_FATAL(rc == 0);
+        break;
+
+    default:
+        break;
+    }
+
+    return 0;
+}
+
+/**
+ * @return                      The handle of the new test connection.
+ */
+static uint16_t
+ble_att_svr_test_misc_init(uint16_t mtu)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_att_svr_test_misc_gap_cb, NULL);
+
+    ble_hs_lock();
+
+    rc = ble_hs_misc_conn_chan_find(2, BLE_L2CAP_CID_ATT, &conn, &chan);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    if (mtu != 0) {
+        chan->my_mtu = mtu;
+        chan->peer_mtu = mtu;
+        chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
+    }
+
+    ble_hs_unlock();
+
+    ble_att_svr_test_attr_r_1_len = 0;
+    ble_att_svr_test_attr_r_2_len = 0;
+    ble_att_svr_test_attr_w_1_len = 0;
+
+    return 2;
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_r_1(uint16_t conn_handle, uint16_t attr_handle,
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
+{
+    int rc;
+
+    switch (op) {
+    case BLE_ATT_ACCESS_OP_READ:
+        if (offset > ble_att_svr_test_attr_r_1_len) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        rc = os_mbuf_append(*om, ble_att_svr_test_attr_r_1 + offset,
+                            ble_att_svr_test_attr_r_1_len - offset);
+        TEST_ASSERT_FATAL(rc == 0);
+        return 0;
+
+    default:
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_r_2(uint16_t conn_handle, uint16_t attr_handle,
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
+{
+    int rc;
+
+    switch (op) {
+    case BLE_ATT_ACCESS_OP_READ:
+        if (offset > ble_att_svr_test_attr_r_2_len) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        rc = os_mbuf_append(*om, ble_att_svr_test_attr_r_2 + offset,
+                            ble_att_svr_test_attr_r_2_len - offset);
+        TEST_ASSERT_FATAL(rc == 0);
+        return 0;
+
+    default:
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_r_err(uint16_t conn_handle, uint16_t attr_handle,
+                                    uint8_t op, uint16_t offset,
+                                    struct os_mbuf **om, void *arg)
+{
+    int rc;
+
+    rc = os_mbuf_append(*om, (uint8_t[4]){1,2,3,4}, 4);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    return BLE_ATT_ERR_UNLIKELY;
+}
+
+#define BLE_ATT_SVR_TEST_LAST_SVC  11
+#define BLE_ATT_SVR_TEST_LAST_ATTR 24
+
+static int
+ble_att_svr_test_misc_attr_fn_r_group(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      uint8_t op,
+                                      uint16_t offset,
+                                      struct os_mbuf **om,
+                                      void *arg)
+{
+    uint8_t *src;
+    int rc;
+
+    /* Service 0x1122 from 1 to 5 */
+    /* Service 0x2233 from 6 to 10 */
+    /* Service 010203...0f from 11 to 24 */
+
+    static uint8_t vals[25][16] = {
+        [1] =   { 0x22, 0x11 },
+        [2] =   { 0x01, 0x11 },
+        [3] =   { 0x02, 0x11 },
+        [4] =   { 0x03, 0x11 },
+        [5] =   { 0x04, 0x11 },
+        [6] =   { 0x33, 0x22 },
+        [7] =   { 0x01, 0x22 },
+        [8] =   { 0x02, 0x22 },
+        [9] =   { 0x03, 0x22 },
+        [10] =  { 0x04, 0x22 },
+        [11] =  { 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16 },
+        [12] =  { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1 },
+        [13] =  { 0xdd, 0xdd },
+        [14] =  { 0x55, 0x55 },
+        [15] =  { 0xdd, 0xdd },
+        [16] =  { 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2 },
+        [17] =  { 0xdd, 0xdd },
+        [18] =  { 0x66, 0x66 },
+        [19] =  { 0xdd, 0xdd },
+        [20] =  { 0x77, 0x77 },
+        [21] =  { 0xdd, 0xdd },
+        [22] =  { 0x88, 0x88 },
+        [23] =  { 0xdd, 0xdd },
+        [24] =  { 0x99, 0x99 },
+    };
+
+    static uint8_t zeros[14];
+
+    if (op != BLE_ATT_ACCESS_OP_READ) {
+        return -1;
+    }
+
+    TEST_ASSERT_FATAL(attr_handle >= 1 &&
+                      attr_handle <= BLE_ATT_SVR_TEST_LAST_ATTR);
+
+    src = &vals[attr_handle][0];
+    if (memcmp(src + 2, zeros, 14) == 0) {
+        rc = os_mbuf_append(*om, src, 2);
+    } else {
+        rc = os_mbuf_append(*om, src, 16);
+    }
+    if (rc != 0) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
+    return 0;
+}
+
+static void
+ble_att_svr_test_misc_register_uuid(const ble_uuid_t *uuid, uint8_t flags,
+                                       uint16_t expected_handle,
+                                       ble_att_svr_access_fn *fn)
+{
+    uint16_t handle;
+    int rc;
+
+    rc = ble_att_svr_register(uuid, flags, 0, &handle, fn, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(handle == expected_handle);
+}
+
+static void
+ble_att_svr_test_misc_register_group_attrs(void)
+{
+    /* Service 0x1122 from 1 to 5 */
+    /* Service 0x2233 from 6 to 10 */
+    /* Service 010203...0f from 11 to 24 */
+
+    static const ble_uuid16_t uuid_svc =
+        BLE_UUID16_INIT(BLE_ATT_UUID_PRIMARY_SERVICE);
+    static const ble_uuid16_t uuid_inc =
+        BLE_UUID16_INIT(BLE_ATT_UUID_INCLUDE);
+    static const ble_uuid16_t uuid_chr =
+        BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
+    static ble_uuid16_t uuids[24];
+
+    int i;
+
+    /* Service 0x1122 from 1 to 5 */
+    ble_att_svr_test_misc_register_uuid(&uuid_svc.u, HA_FLAG_PERM_RW, 1,
+                                        ble_att_svr_test_misc_attr_fn_r_group);
+    for (i = 2; i <= 5; i++) {
+        if ((i - 2) % 2 == 0) {
+            ble_att_svr_test_misc_register_uuid(&uuid_chr.u, HA_FLAG_PERM_RW, i,
+                                                ble_att_svr_test_misc_attr_fn_r_group);
+        } else {
+            uuids[i] = *BLE_UUID16(BLE_UUID16_DECLARE(i));
+            ble_att_svr_test_misc_register_uuid(&uuids[i].u, HA_FLAG_PERM_RW, i,
+                ble_att_svr_test_misc_attr_fn_r_group);
+        }
+    }
+
+    /* Service 0x2233 from 6 to 10 */
+    ble_att_svr_test_misc_register_uuid(&uuid_svc.u, HA_FLAG_PERM_RW, 6,
+                                        ble_att_svr_test_misc_attr_fn_r_group);
+    for (i = 7; i <= 10; i++) {
+        ble_att_svr_test_misc_register_uuid(&uuid_inc.u, HA_FLAG_PERM_RW, i,
+                                            ble_att_svr_test_misc_attr_fn_r_group);
+    }
+
+    /* Service 010203...0f from 11 to 24 */
+    ble_att_svr_test_misc_register_uuid(&uuid_svc.u, HA_FLAG_PERM_RW, 11,
+                                        ble_att_svr_test_misc_attr_fn_r_group);
+    for (i = 12; i <= 24; i++) {
+        if ((i - 12) % 2 == 0) {
+            ble_att_svr_test_misc_register_uuid(&uuid_chr.u, HA_FLAG_PERM_RW, i,
+                                                ble_att_svr_test_misc_attr_fn_r_group);
+        } else {
+            uuids[i] = *BLE_UUID16(BLE_UUID16_DECLARE(i));
+            ble_att_svr_test_misc_register_uuid(&uuids[i].u, HA_FLAG_PERM_RW, i,
+                ble_att_svr_test_misc_attr_fn_r_group);
+        }
+    }
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_rw_1(uint16_t conn_handle, uint16_t attr_handle,
+                                   uint8_t op, uint16_t offset,
+                                   struct os_mbuf **om, void *arg)
+{
+    int rc;
+
+    switch (op) {
+    case BLE_ATT_ACCESS_OP_READ:
+        if (offset > ble_att_svr_test_attr_w_1_len) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        rc = os_mbuf_append(*om, ble_att_svr_test_attr_w_1 + offset,
+                            ble_att_svr_test_attr_w_1_len - offset);
+        TEST_ASSERT_FATAL(rc == 0);
+        return 0;
+
+    case BLE_ATT_ACCESS_OP_WRITE:
+        rc = os_mbuf_copydata(*om, 0, OS_MBUF_PKTLEN(*om),
+                              ble_att_svr_test_attr_w_1);
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_att_svr_test_attr_w_1_len = OS_MBUF_PKTLEN(*om);
+        return 0;
+
+    default:
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_w_1(uint16_t conn_handle, uint16_t attr_handle,
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
+{
+    int rc;
+
+    switch (op) {
+    case BLE_ATT_ACCESS_OP_WRITE:
+        rc = os_mbuf_copydata(*om, 0, OS_MBUF_PKTLEN(*om),
+                              ble_att_svr_test_attr_w_1);
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_att_svr_test_attr_w_1_len = OS_MBUF_PKTLEN(*om);
+        return 0;
+
+    default:
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_w_2(uint16_t conn_handle, uint16_t attr_handle,
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
+{
+    int rc;
+
+    switch (op) {
+    case BLE_ATT_ACCESS_OP_WRITE:
+        rc = os_mbuf_copydata(*om, 0, OS_MBUF_PKTLEN(*om),
+                              ble_att_svr_test_attr_w_2);
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_att_svr_test_attr_w_2_len = OS_MBUF_PKTLEN(*om);
+        return 0;
+
+    default:
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+static int
+ble_att_svr_test_misc_attr_fn_w_fail(uint16_t conn_handle,
+                                     uint16_t attr_handle,
+                                     uint8_t op, uint16_t offset,
+                                     struct os_mbuf **om, void *arg)
+{
+    return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+}
+
+static void
+ble_att_svr_test_misc_verify_w_1(void *data, int data_len)
+{
+    TEST_ASSERT(ble_att_svr_test_attr_w_1_len == data_len);
+    TEST_ASSERT(memcmp(ble_att_svr_test_attr_w_1, data, data_len) == 0);
+}
+
+static void
+ble_att_svr_test_misc_verify_w_2(void *data, int data_len)
+{
+    TEST_ASSERT(ble_att_svr_test_attr_w_2_len == data_len);
+    TEST_ASSERT(memcmp(ble_att_svr_test_attr_w_2, data, data_len) == 0);
+}
+
+static void
+ble_att_svr_test_misc_rx_read_mult_req(uint16_t conn_handle,
+                                       uint16_t *handles, int num_handles,
+                                       int success)
+{
+    int rc;
+
+    rc = ble_hs_test_util_rx_att_read_mult_req(conn_handle, handles,
+                                               num_handles);
+    if (success) {
+        TEST_ASSERT(rc == 0);
+    } else {
+        TEST_ASSERT(rc != 0);
+    }
+}
+
+static void
+ble_att_svr_test_misc_verify_tx_read_mult_rsp(
+    uint16_t conn_handle, struct ble_hs_test_util_flat_attr *attrs,
+    int num_attrs)
+{
+    struct ble_l2cap_chan *chan;
+    struct os_mbuf *om;
+    uint16_t attr_len;
+    uint16_t mtu;
+    uint8_t u8;
+    int rc;
+    int off;
+    int i;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, 1, &u8);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(u8 == BLE_ATT_OP_READ_MULT_RSP);
+
+    ble_hs_lock();
+
+    rc = ble_hs_misc_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT,
+                                    NULL, &chan);
+    TEST_ASSERT_FATAL(rc == 0);
+    mtu = ble_att_chan_mtu(chan);
+
+    ble_hs_unlock();
+
+    off = 1;
+    for (i = 0; i < num_attrs; i++) {
+        attr_len = min(attrs[i].value_len, mtu - off);
+
+        rc = os_mbuf_cmpf(om, off, attrs[i].value, attr_len);
+        TEST_ASSERT(rc == 0);
+
+        off += attr_len;
+    }
+
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == off);
+}
+
+static void
+ble_att_svr_test_misc_verify_all_read_mult(
+    uint16_t conn_handle, struct ble_hs_test_util_flat_attr *attrs,
+    int num_attrs)
+{
+    uint16_t handles[256];
+    int i;
+
+    TEST_ASSERT_FATAL(num_attrs <= sizeof handles / sizeof handles[0]);
+
+    for (i = 0; i < num_attrs; i++) {
+        handles[i] = attrs[i].handle;
+    }
+
+    ble_att_svr_test_misc_rx_read_mult_req(conn_handle, handles, num_attrs, 1);
+    ble_att_svr_test_misc_verify_tx_read_mult_rsp(conn_handle,
+                                                  attrs, num_attrs);
+}
+
+static void
+ble_att_svr_test_misc_verify_tx_mtu_rsp(uint16_t conn_handle)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    uint16_t my_mtu;
+    int rc;
+
+    ble_hs_lock();
+
+    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    assert(rc == 0);
+    my_mtu = chan->my_mtu;
+
+    ble_hs_unlock();
+
+    ble_hs_test_util_verify_tx_mtu_cmd(0, my_mtu);
+}
+
+struct ble_att_svr_test_type_value_entry {
+    uint16_t first;        /* 0 on last entry */
+    uint16_t last;
+};
+
+static void
+ble_att_svr_test_misc_verify_tx_find_type_value_rsp(
+    struct ble_att_svr_test_type_value_entry *entries)
+{
+    struct ble_att_svr_test_type_value_entry *entry;
+    struct os_mbuf *om;
+    uint16_t u16;
+    uint8_t op;
+    int off;
+    int rc;
+
+    off = 0;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+
+    rc = os_mbuf_copydata(om, off, 1, &op);
+    TEST_ASSERT(rc == 0);
+    off += 1;
+
+    TEST_ASSERT(op == BLE_ATT_OP_FIND_TYPE_VALUE_RSP);
+
+    for (entry = entries; entry->first != 0; entry++) {
+        rc = os_mbuf_copydata(om, off, 2, &u16);
+        TEST_ASSERT(rc == 0);
+        put_le16(&u16, u16);
+        TEST_ASSERT(u16 == entry->first);
+        off += 2;
+
+        rc = os_mbuf_copydata(om, off, 2, &u16);
+        TEST_ASSERT(rc == 0);
+        put_le16(&u16, u16);
+        TEST_ASSERT(u16 == entry->last);
+        off += 2;
+    }
+
+    /* Ensure there is no extra data in the response. */
+    TEST_ASSERT(off == OS_MBUF_PKTHDR(om)->omp_len);
+}
+
+/** Returns the number of entries successfully verified. */
+
+struct ble_att_svr_test_type_entry {
+    uint16_t handle;  /* 0 on last entry */
+    void *value;
+    int value_len;
+};
+
+/** Returns the number of entries successfully verified. */
+static void
+ble_att_svr_test_misc_verify_tx_read_type_rsp(
+    struct ble_att_svr_test_type_entry *entries)
+{
+    struct ble_att_svr_test_type_entry *entry;
+    struct ble_att_read_type_rsp rsp;
+    struct os_mbuf *om;
+    uint16_t handle;
+    uint8_t buf[512];
+    int off;
+    int rc;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT(om);
+
+    ble_att_read_type_rsp_parse(om->om_data, om->om_len, &rsp);
+
+    off = BLE_ATT_READ_TYPE_RSP_BASE_SZ;
+    for (entry = entries; entry->handle != 0; entry++) {
+        TEST_ASSERT_FATAL(rsp.batp_length ==
+                          BLE_ATT_READ_TYPE_ADATA_BASE_SZ + entry->value_len);
+
+        rc = os_mbuf_copydata(om, off, 2, &handle);
+        TEST_ASSERT(rc == 0);
+        handle = get_le16(&handle);
+        TEST_ASSERT(handle == entry->handle);
+        off += 2;
+
+        rc = os_mbuf_copydata(om, off, entry->value_len, buf);
+        TEST_ASSERT(rc == 0);
+        TEST_ASSERT(memcmp(entry->value, buf, entry->value_len) == 0);
+        off += entry->value_len;
+    }
+
+    /* Ensure there is no extra data in the response. */
+    TEST_ASSERT(off == OS_MBUF_PKTLEN(om));
+}
+
+static void
+ble_att_svr_test_misc_verify_tx_prep_write_rsp(uint16_t attr_handle,
+                                               uint16_t offset,
+                                               void *data, int data_len)
+{
+    struct ble_att_prep_write_cmd rsp;
+    struct os_mbuf *om;
+    uint8_t buf[1024];
+    int rc;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), buf);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_att_prep_write_rsp_parse(buf, sizeof buf, &rsp);
+
+    TEST_ASSERT(rsp.bapc_handle == attr_handle);
+    TEST_ASSERT(rsp.bapc_offset == offset);
+    TEST_ASSERT(memcmp(buf + BLE_ATT_PREP_WRITE_CMD_BASE_SZ, data,
+                       data_len) == 0);
+
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) ==
+                BLE_ATT_PREP_WRITE_CMD_BASE_SZ + data_len);
+}
+
+static void
+ble_att_svr_test_misc_verify_tx_exec_write_rsp(void)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT(om);
+
+    ble_att_exec_write_rsp_parse(om->om_data, om->om_len);
+}
+
+static void
+ble_att_svr_test_misc_mtu_exchange(uint16_t my_mtu, uint16_t peer_sent,
+                                   uint16_t peer_actual, uint16_t chan_mtu)
+{
+    struct ble_att_mtu_cmd req;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+    uint8_t buf[BLE_ATT_MTU_CMD_SZ];
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(my_mtu);
+
+    req.bamc_mtu = peer_sent;
+    ble_att_mtu_req_write(buf, sizeof buf, &req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_att_svr_test_misc_verify_tx_mtu_rsp(conn_handle);
+
+    ble_hs_lock();
+    rc = ble_hs_misc_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT,
+                                    &conn, &chan);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(chan->peer_mtu == peer_actual);
+    TEST_ASSERT(ble_att_chan_mtu(chan) == chan_mtu);
+    ble_hs_unlock();
+
+}
+
+static void
+ble_att_svr_test_misc_prep_write(uint16_t conn_handle, uint16_t attr_handle,
+                                 uint16_t offset, void *data,
+                                 int data_len, uint8_t error_code)
+{
+    int rc;
+
+    rc = ble_hs_test_util_rx_att_prep_write_req(conn_handle, attr_handle,
+                                                offset, data, data_len);
+    if (error_code == 0) {
+        TEST_ASSERT(rc == 0);
+        ble_att_svr_test_misc_verify_tx_prep_write_rsp(attr_handle, offset,
+                                                       data, data_len);
+    } else {
+        TEST_ASSERT(rc != 0);
+        ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_PREP_WRITE_REQ,
+                                                attr_handle, error_code);
+    }
+}
+
+static void
+ble_att_svr_test_misc_exec_write(uint16_t conn_handle, uint8_t flags,
+                                 uint8_t error_code, uint16_t error_handle)
+{
+    int rc;
+
+    rc = ble_hs_test_util_rx_att_exec_write_req(conn_handle, flags);
+    if (error_code == 0) {
+        TEST_ASSERT(rc == 0);
+        ble_att_svr_test_misc_verify_tx_exec_write_rsp();
+    } else {
+        TEST_ASSERT(rc != 0);
+        ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_EXEC_WRITE_REQ,
+                                                error_handle, error_code);
+    }
+}
+
+static void
+ble_att_svr_test_misc_rx_notify(uint16_t conn_handle, uint16_t attr_handle,
+                                void *attr_val, int attr_len, int good)
+{
+    struct ble_att_notify_req req;
+    uint8_t buf[1024];
+    int off;
+    int rc;
+
+    req.banq_handle = attr_handle;
+    ble_att_notify_req_write(buf, sizeof buf, &req);
+    off = BLE_ATT_NOTIFY_REQ_BASE_SZ;
+
+    memcpy(buf + off, attr_val, attr_len);
+    off += attr_len;
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    if (good) {
+        TEST_ASSERT(rc == 0);
+    } else {
+        TEST_ASSERT(rc == BLE_HS_EBADDATA);
+    }
+}
+
+static void
+ble_att_svr_test_misc_verify_notify(uint16_t conn_handle, uint16_t attr_handle,
+                                    void *attr_val, int attr_len, int good)
+{
+    ble_att_svr_test_n_conn_handle = 0xffff;
+    ble_att_svr_test_n_attr_handle = 0;
+    ble_att_svr_test_attr_n_len = 0;
+
+    ble_att_svr_test_misc_rx_notify(conn_handle, attr_handle, attr_val,
+                                    attr_len, good);
+
+    if (good) {
+        TEST_ASSERT(ble_att_svr_test_n_conn_handle == conn_handle);
+        TEST_ASSERT(ble_att_svr_test_n_attr_handle == attr_handle);
+        TEST_ASSERT(ble_att_svr_test_attr_n_len == attr_len);
+        TEST_ASSERT(memcmp(ble_att_svr_test_attr_n, attr_val, attr_len) == 0);
+    } else {
+        TEST_ASSERT(ble_att_svr_test_n_conn_handle == 0xffff);
+        TEST_ASSERT(ble_att_svr_test_n_attr_handle == 0);
+        TEST_ASSERT(ble_att_svr_test_attr_n_len == 0);
+    }
+}
+
+static void
+ble_att_svr_test_misc_verify_tx_indicate_rsp(void)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT(om);
+
+    ble_att_indicate_rsp_parse(om->om_data, om->om_len);
+}
+
+static void
+ble_att_svr_test_misc_rx_indicate(uint16_t conn_handle, uint16_t attr_handle,
+                                  void *attr_val, int attr_len, int good)
+{
+    int rc;
+
+    rc = ble_hs_test_util_rx_att_indicate_req(conn_handle, attr_handle,
+                                              attr_val, attr_len);
+    if (good) {
+        TEST_ASSERT(rc == 0);
+    } else {
+        TEST_ASSERT(rc == BLE_HS_EBADDATA);
+    }
+}
+
+static void
+ble_att_svr_test_misc_verify_indicate(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      void *attr_val, int attr_len, int good)
+{
+    ble_att_svr_test_n_conn_handle = 0xffff;
+    ble_att_svr_test_n_attr_handle = 0;
+    ble_att_svr_test_attr_n_len = 0;
+
+    ble_att_svr_test_misc_rx_indicate(conn_handle, attr_handle, attr_val,
+                                      attr_len, good);
+
+    if (good) {
+        TEST_ASSERT(ble_att_svr_test_n_conn_handle == conn_handle);
+        TEST_ASSERT(ble_att_svr_test_n_attr_handle == attr_handle);
+        TEST_ASSERT(ble_att_svr_test_attr_n_len == attr_len);
+        TEST_ASSERT(memcmp(ble_att_svr_test_attr_n, attr_val, attr_len) == 0);
+        ble_att_svr_test_misc_verify_tx_indicate_rsp();
+    } else {
+        TEST_ASSERT(ble_att_svr_test_n_conn_handle == 0xffff);
+        TEST_ASSERT(ble_att_svr_test_n_attr_handle == 0);
+        TEST_ASSERT(ble_att_svr_test_attr_n_len == 0);
+        TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
+    }
+}
+
+TEST_CASE_SELF(ble_att_svr_test_mtu)
+{
+    /*** MTU too low; should pretend peer sent default value instead. */
+    ble_att_svr_test_misc_mtu_exchange(BLE_ATT_MTU_DFLT, 5,
+                                       BLE_ATT_MTU_DFLT, BLE_ATT_MTU_DFLT);
+
+    /*** MTUs equal. */
+    ble_att_svr_test_misc_mtu_exchange(50, 50, 50, 50);
+
+    /*** Peer's higher than mine. */
+    ble_att_svr_test_misc_mtu_exchange(50, 100, 100, 50);
+
+    /*** Mine higher than peer's. */
+    ble_att_svr_test_misc_mtu_exchange(100, 50, 50, 50);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_read)
+{
+    struct ble_hs_conn *conn;
+    struct os_mbuf *om;
+    uint16_t attr_handle;
+    uint16_t conn_handle;
+    const ble_uuid_t *uuid_sec = BLE_UUID128_DECLARE( \
+        1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const ble_uuid_t *uuid_bad = BLE_UUID128_DECLARE( \
+        2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const ble_uuid_t *uuid = BLE_UUID128_DECLARE( \
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, );
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /*** Nonexistent attribute. */
+    attr_handle = 0;
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, attr_handle);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ, 0,
+                                       BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Application error. */
+    rc = ble_att_svr_register(uuid_bad, HA_FLAG_PERM_RW, 0, &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_r_err, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, attr_handle);
+    TEST_ASSERT(rc == BLE_HS_EAPP);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ, attr_handle,
+                                       BLE_ATT_ERR_UNLIKELY);
+
+    /*** Successful read. */
+    ble_att_svr_test_attr_r_1 = (uint8_t[]){0,1,2,3,4,5,6,7};
+    ble_att_svr_test_attr_r_1_len = 8;
+    rc = ble_att_svr_register(uuid, HA_FLAG_PERM_RW, 0, &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, attr_handle);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_rsp(
+        ble_att_svr_test_attr_r_1, ble_att_svr_test_attr_r_1_len);
+
+    /*** Partial read. */
+    ble_att_svr_test_attr_r_1 =
+        (uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,
+                    22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39};
+    ble_att_svr_test_attr_r_1_len = 40;
+
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, attr_handle);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_rsp(ble_att_svr_test_attr_r_1,
+                                        BLE_ATT_MTU_DFLT - 1);
+
+    /*** Read requires encryption. */
+    /* Insufficient authentication. */
+    rc = ble_att_svr_register(uuid_sec, BLE_ATT_F_READ | BLE_ATT_F_READ_ENC, 0,
+                              &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, attr_handle);
+    TEST_ASSERT(rc == BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHEN));
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ, attr_handle,
+                                       BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
+
+    /* Security check bypassed for local reads. */
+    rc = ble_att_svr_read_local(attr_handle, &om);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(os_mbuf_cmpf(om, 0, ble_att_svr_test_attr_r_1,
+                               ble_att_svr_test_attr_r_1_len) == 0);
+    rc = os_mbuf_free_chain(om);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure no response got sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /* Encrypt link; success. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    conn->bhc_sec_state.encrypted = 1;
+    ble_hs_unlock();
+
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, attr_handle);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_rsp(ble_att_svr_test_attr_r_1,
+                                        BLE_ATT_MTU_DFLT - 1);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_read_blob)
+{
+    uint16_t attr_handle;
+    uint16_t conn_handle;
+    const ble_uuid_t *uuid = BLE_UUID128_DECLARE( \
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /*** Nonexistent attribute. */
+    rc = ble_hs_test_util_rx_att_read_blob_req(conn_handle, 0, 0);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_BLOB_REQ, 0,
+                                       BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Successful partial read. */
+    ble_att_svr_test_attr_r_1 =
+        (uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,
+                    22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39};
+    ble_att_svr_test_attr_r_1_len = 40;
+    rc = ble_att_svr_register(uuid, HA_FLAG_PERM_RW, 0, &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_read_blob_req(conn_handle, attr_handle, 0);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_blob_rsp(ble_att_svr_test_attr_r_1,
+                                                  BLE_ATT_MTU_DFLT - 1);
+
+    /*** Read remainder of attribute. */
+    rc = ble_hs_test_util_rx_att_read_blob_req(conn_handle, attr_handle,
+                                           BLE_ATT_MTU_DFLT - 1);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_blob_rsp(
+        ble_att_svr_test_attr_r_1 + BLE_ATT_MTU_DFLT - 1,
+        40 - (BLE_ATT_MTU_DFLT - 1));
+
+    /*** Zero-length read. */
+    rc = ble_hs_test_util_rx_att_read_blob_req(conn_handle, attr_handle,
+                                           ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_blob_rsp(ble_att_svr_test_attr_r_1,
+                                                  0);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_read_mult)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    struct ble_hs_test_util_flat_attr attrs[2] = {
+        {
+            .handle = 0,
+            .offset = 0,
+            .value = { 1, 2, 3, 4 },
+            .value_len = 4,
+        },
+        {
+            .handle = 0,
+            .offset = 0,
+            .value = { 2, 3, 4, 5, 6 },
+            .value_len = 5,
+        },
+    };
+
+    ble_att_svr_test_attr_r_1 = attrs[0].value;
+    ble_att_svr_test_attr_r_1_len = attrs[0].value_len;
+    ble_att_svr_test_attr_r_2 = attrs[1].value;
+    ble_att_svr_test_attr_r_2_len = attrs[1].value_len;
+
+    rc = ble_att_svr_register(BLE_UUID16_DECLARE(0x1111), HA_FLAG_PERM_RW, 0,
+                              &attrs[0].handle,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_att_svr_register(BLE_UUID16_DECLARE(0x2222), HA_FLAG_PERM_RW, 0,
+                              &attrs[1].handle,
+                              ble_att_svr_test_misc_attr_fn_r_2, NULL);
+    TEST_ASSERT(rc == 0);
+
+    /*** Single nonexistent attribute. */
+    ble_att_svr_test_misc_rx_read_mult_req(
+        conn_handle, ((uint16_t[]){ 100 }), 1, 0);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ,
+                                            100, BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Single attribute. */
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, &attrs[0], 1);
+
+    /*** Two attributes. */
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, attrs, 2);
+
+    /*** Reverse order. */
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, attrs, 2);
+
+    /*** Second attribute nonexistent; verify only error txed. */
+    ble_att_svr_test_misc_rx_read_mult_req(
+        conn_handle, ((uint16_t[]){ attrs[0].handle, 100 }), 2, 0);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ,
+                                            100, BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Response too long; verify only MTU bytes sent. */
+    attrs[0].value_len = 20;
+    memcpy(attrs[0].value,
+           ((uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19}),
+           attrs[0].value_len);
+    ble_att_svr_test_attr_r_1_len = attrs[0].value_len;
+
+    attrs[1].value_len = 20;
+    memcpy(attrs[1].value,
+           ((uint8_t[]){
+                22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39
+           }),
+           attrs[1].value_len);
+    ble_att_svr_test_attr_r_2_len = attrs[1].value_len;
+
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, attrs, 2);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_write)
+{
+    struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+    uint16_t attr_handle;
+    const ble_uuid_t *uuid_sec = BLE_UUID128_DECLARE( \
+        2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const ble_uuid_t *uuid_rw = BLE_UUID128_DECLARE( \
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const ble_uuid_t *uuid_r = BLE_UUID128_DECLARE( \
+        1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+    int rc;
+
+    static const uint8_t attr_val[8] = { 0, 1, 2, 3, 4, 5, 6, 7 };
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /*** Nonexistent attribute. */
+    rc = ble_hs_test_util_rx_att_write_req(conn_handle, 0,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_WRITE_REQ, 0, BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Write not permitted if non-local. */
+    /* Non-local write (fail). */
+    rc = ble_att_svr_register(uuid_r, BLE_ATT_F_READ, 0, &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_w_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_write_req(conn_handle, attr_handle,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc == BLE_HS_EREJECT);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
+                                       attr_handle,
+                                       BLE_ATT_ERR_WRITE_NOT_PERMITTED);
+
+    /* Local write (success). */
+    rc = ble_hs_test_util_write_local_flat(attr_handle,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure no response got sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /*** Successful write. */
+    rc = ble_att_svr_register(uuid_rw, HA_FLAG_PERM_RW, 0, &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_w_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_write_req(conn_handle, attr_handle,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_write_rsp();
+
+    /*** Write requires encryption. */
+    /* Insufficient authentication. */
+    rc = ble_att_svr_register(uuid_sec, BLE_ATT_F_WRITE | BLE_ATT_F_WRITE_ENC,
+                              0, &attr_handle,
+                              ble_att_svr_test_misc_attr_fn_w_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_write_req(conn_handle, attr_handle,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc == BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHEN));
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
+                                       attr_handle,
+                                       BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
+
+    /* Security check bypassed for local writes. */
+    rc = ble_hs_test_util_write_local_flat(attr_handle,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure no response got sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /* Encrypt link; success. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    conn->bhc_sec_state.encrypted = 1;
+    ble_hs_unlock();
+
+    rc = ble_hs_test_util_rx_att_write_req(conn_handle, attr_handle,
+                                           attr_val, sizeof attr_val);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_write_rsp();
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_find_info)
+{
+    uint16_t conn_handle;
+    uint16_t handle1;
+    uint16_t handle2;
+    uint16_t handle3;
+    const ble_uuid_t *uuid1 =
+        BLE_UUID128_DECLARE(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 ,11, 12, 13, 14, 15);
+    const ble_uuid_t *uuid2 =
+        BLE_UUID128_DECLARE(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16);
+    const ble_uuid_t *uuid3 = BLE_UUID16_DECLARE(0x000f);
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(128);
+
+    /*** Start handle of 0. */
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, 0, 0);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_INFO_REQ, 0, BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Start handle > end handle. */
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, 101, 100);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_INFO_REQ, 101, BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** No attributes. */
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, 200, 300);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_INFO_REQ, 200, BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** Range too late. */
+    rc = ble_att_svr_register(uuid1, HA_FLAG_PERM_RW, 0, &handle1,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, 200, 300);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_INFO_REQ, 200, BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** One 128-bit entry. */
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, handle1, handle1);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_find_info_rsp(
+        ((struct ble_hs_test_util_att_info_entry[]) { {
+            .handle = handle1,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .handle = 0,
+        } }));
+
+    /*** Two 128-bit entries. */
+    rc = ble_att_svr_register(uuid2, HA_FLAG_PERM_RW, 0, &handle2,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, handle1, handle2);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_find_info_rsp(
+        ((struct ble_hs_test_util_att_info_entry[]) { {
+            .handle = handle1,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .handle = handle2,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        }, {
+            .handle = 0,
+        } }));
+
+    /*** Two 128-bit entries; 16-bit entry doesn't get sent. */
+    rc = ble_att_svr_register(uuid3, HA_FLAG_PERM_RW, 0, &handle3,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, handle1, handle3);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_find_info_rsp(
+        ((struct ble_hs_test_util_att_info_entry[]) { {
+            .handle = handle1,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .handle = handle2,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        }, {
+            .handle = 0,
+        } }));
+
+    /*** Remaining 16-bit entry requested. */
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, handle3, handle3);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_find_info_rsp(
+        ((struct ble_hs_test_util_att_info_entry[]) { {
+            .handle = handle3,
+            .uuid = BLE_UUID16_DECLARE(0x000f),
+        }, {
+            .handle = 0,
+        } }));
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_find_type_value)
+{
+    uint16_t conn_handle;
+    uint16_t handle1;
+    uint16_t handle2;
+    uint16_t handle3;
+    uint16_t handle4;
+    uint16_t handle5;
+    uint16_t handle_desc;
+    const ble_uuid_t *uuid1 = BLE_UUID16_DECLARE(0x2800);
+    const ble_uuid_t *uuid2 = BLE_UUID16_DECLARE(0x2803);
+    const ble_uuid_t *uuid3 =
+        BLE_UUID128_DECLARE(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(128);
+
+    ble_att_svr_test_attr_r_1 = (uint8_t[]){0x99, 0x99};
+    ble_att_svr_test_attr_r_1_len = 2;
+
+    /*** Start handle of 0. */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 0, 0, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 0,
+        BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Start handle > end handle. */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 101, 100, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 101,
+        BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** No attributes. */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 200, 300, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 200,
+        BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** Range too late. */
+    rc = ble_att_svr_register(uuid1, HA_FLAG_PERM_RW, 0, &handle1,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 200, 300, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 200,
+        BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** One entry, one attribute. */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, handle1, handle1, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_find_type_value_rsp(
+        ((struct ble_att_svr_test_type_value_entry[]) { {
+            .first = handle1,
+            .last = handle1,
+        }, {
+            .first = 0,
+        } }));
+
+    /*** One entry, two attributes. */
+    rc = ble_att_svr_register(uuid2, HA_FLAG_PERM_RW, 0, &handle2,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, handle1, handle2, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_find_type_value_rsp(
+        ((struct ble_att_svr_test_type_value_entry[]) { {
+            .first = handle1,
+            .last = handle2,
+        }, {
+            .first = 0,
+        } }));
+
+    /*** Entry 1: four attributes; entry 2 (invalid value): one attribute;
+     *   entry 3: one attribute; Check that invalid value is not returned. */
+    ble_att_svr_test_attr_r_2 = (uint8_t[]){0x00, 0x00};
+    ble_att_svr_test_attr_r_2_len = 2;
+
+    rc = ble_att_svr_register(uuid3, HA_FLAG_PERM_RW, 0, &handle_desc,
+                              ble_att_svr_test_misc_attr_fn_r_2, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_att_svr_register(uuid2, HA_FLAG_PERM_RW, 0, &handle3,
+                              ble_att_svr_test_misc_attr_fn_r_2, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_att_svr_register(uuid1, HA_FLAG_PERM_RW, 0, &handle4,
+                              ble_att_svr_test_misc_attr_fn_r_2, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_att_svr_register(uuid1, HA_FLAG_PERM_RW, 0, &handle5,
+                              ble_att_svr_test_misc_attr_fn_r_1, NULL);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 0x0001, 0xffff, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_find_type_value_rsp(
+        ((struct ble_att_svr_test_type_value_entry[]) { {
+            .first = handle1,
+            .last = handle3,
+        }, {
+            .first = handle5,
+            .last = handle5,
+        }, {
+            .first = 0,
+        } }));
+
+    /*** As above, check proper range is returned with smaller search range */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 0x0001, 0x0001, 0x2800, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_find_type_value_rsp(
+        ((struct ble_att_svr_test_type_value_entry[]) { {
+            .first = handle1,
+            .last = handle3,
+        }, {
+            .first = 0,
+        } }));
+
+    /*** As above, check grouping by Characteristic UUID */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, handle1, handle3, 0x2803, ble_att_svr_test_attr_r_1,
+        ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_find_type_value_rsp(
+        ((struct ble_att_svr_test_type_value_entry[]) { {
+            .first = handle2,
+            .last = handle_desc,
+        }, {
+            .first = 0,
+        } }));
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+static void
+ble_att_svr_test_misc_read_type(uint16_t mtu)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(mtu);
+
+    /*** Start handle of 0. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 0, 0,
+                                                 BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_TYPE_REQ, 0,
+        BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Start handle > end handle. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 101, 100,
+                                                 BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_TYPE_REQ, 101,
+        BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** No attributes. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 1, 0xffff,
+                                                 BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_TYPE_REQ, 1,
+        BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** Range too late. */
+    ble_att_svr_test_misc_register_group_attrs();
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 200, 300,
+                                                 BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_TYPE_REQ, 200,
+        BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** One characteristic from one service. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 1, 2,
+                                                 BLE_ATT_UUID_CHARACTERISTIC);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_read_type_rsp(
+        ((struct ble_att_svr_test_type_entry[]) { {
+            .handle = 2,
+            .value = (uint8_t[]){ 0x01, 0x11 },
+            .value_len = 2,
+        }, {
+            .handle = 0,
+        } }));
+
+    /*** Both characteristics from one service. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 1, 10,
+                                                 BLE_ATT_UUID_CHARACTERISTIC);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_read_type_rsp(
+        ((struct ble_att_svr_test_type_entry[]) { {
+            .handle = 2,
+            .value = (uint8_t[]){ 0x01, 0x11 },
+            .value_len = 2,
+        }, {
+            .handle = 4,
+            .value = (uint8_t[]){ 0x03, 0x11 },
+            .value_len = 2,
+        }, {
+            .handle = 0,
+        } }));
+
+    /*** Ensure 16-bit and 128-bit values are retrieved separately. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 11, 0xffff,
+                                                 BLE_ATT_UUID_CHARACTERISTIC);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_read_type_rsp(
+        ((struct ble_att_svr_test_type_entry[]) { {
+            .handle = 12,
+            .value = (uint8_t[]){ 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1 },
+            .value_len = 16,
+        }, {
+            .handle = 0,
+        } }));
+
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 13, 0xffff,
+                                                 BLE_ATT_UUID_CHARACTERISTIC);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_read_type_rsp(
+        ((struct ble_att_svr_test_type_entry[]) { {
+            .handle = 14,
+            .value = (uint8_t[]){ 0x55, 0x55 },
+            .value_len = 2,
+        }, {
+            .handle = 0,
+        } }));
+
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 15, 0xffff,
+                                                 BLE_ATT_UUID_CHARACTERISTIC);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_read_type_rsp(
+        ((struct ble_att_svr_test_type_entry[]) { {
+            .handle = 16,
+            .value = (uint8_t[]){ 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2 },
+            .value_len = 16,
+        }, {
+            .handle = 0,
+        } }));
+
+    /*** Read until the end of the attribute list. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 17, 0xffff,
+                                                 BLE_ATT_UUID_CHARACTERISTIC);
+    TEST_ASSERT(rc == 0);
+    ble_att_svr_test_misc_verify_tx_read_type_rsp(
+        ((struct ble_att_svr_test_type_entry[]) { {
+            .handle = 18,
+            .value = (uint8_t[]){ 0x66, 0x66 },
+            .value_len = 2,
+        }, {
+            .handle = 20,
+            .value = (uint8_t[]){ 0x77, 0x77 },
+            .value_len = 2,
+        }, {
+            .handle = 22,
+            .value = (uint8_t[]){ 0x88, 0x88 },
+            .value_len = 2,
+        }, {
+            .handle = 24,
+            .value = (uint8_t[]){ 0x99, 0x99 },
+            .value_len = 2,
+        }, {
+            .handle = 0,
+        } }));
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_read_type)
+{
+    ble_att_svr_test_misc_read_type(0);
+    ble_att_svr_test_misc_read_type(128);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_read_group_type)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(128);
+
+    /*** Start handle of 0. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 0, 0, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_GROUP_TYPE_REQ, 0,
+        BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Start handle > end handle. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 101, 100, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_GROUP_TYPE_REQ, 101,
+        BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Invalid group UUID (0x1234). */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 110, 150, 0x1234);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_GROUP_TYPE_REQ, 110,
+        BLE_ATT_ERR_UNSUPPORTED_GROUP);
+
+    /*** No attributes. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 1, 0xffff, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_GROUP_TYPE_REQ, 1,
+        BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** Range too late. */
+    ble_att_svr_test_misc_register_group_attrs();
+
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 200, 300, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(
+        BLE_ATT_OP_READ_GROUP_TYPE_REQ, 200,
+        BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** One 16-bit UUID service. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 1, 5, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_group_type_rsp(
+        ((struct ble_hs_test_util_att_group_type_entry[]) { {
+            .start_handle = 1,
+            .end_handle = 5,
+            .uuid = BLE_UUID16_DECLARE(0x1122),
+        }, {
+            .start_handle = 0,
+        } }));
+
+    /*** Two 16-bit UUID services. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 1, 10, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_group_type_rsp(
+        ((struct ble_hs_test_util_att_group_type_entry[]) { {
+            .start_handle = 1,
+            .end_handle = 5,
+            .uuid = BLE_UUID16_DECLARE(0x1122),
+        }, {
+            .start_handle = 6,
+            .end_handle = 10,
+            .uuid = BLE_UUID16_DECLARE(0x2233),
+        }, {
+            .start_handle = 0,
+        } }));
+
+    /*** Two 16-bit UUID services; ensure 128-bit service not returned. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 1, 100, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_group_type_rsp(
+        ((struct ble_hs_test_util_att_group_type_entry[]) { {
+            .start_handle = 1,
+            .end_handle = 5,
+            .uuid = BLE_UUID16_DECLARE(0x1122),
+        }, {
+            .start_handle = 6,
+            .end_handle = 10,
+            .uuid = BLE_UUID16_DECLARE(0x2233),
+        }, {
+            .start_handle = 0,
+        } }));
+
+    /*** One 128-bit service. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 11, 100, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_verify_tx_read_group_type_rsp(
+        ((struct ble_hs_test_util_att_group_type_entry[]) { {
+            .start_handle = 11,
+            .end_handle = 0xffff,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        }, {
+            .start_handle = 0,
+        } }));
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_prep_write)
+{
+    struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+    int i;
+
+    static uint8_t data[1024];
+
+    conn_handle = ble_att_svr_test_misc_init(205);
+
+    /* Initialize some attribute data. */
+    for (i = 0; i < sizeof data; i++) {
+        data[i] = i;
+    }
+
+    /* Register two writable attributes. */
+    ble_att_svr_test_misc_register_uuid(BLE_UUID16_DECLARE(0x1234),
+                                          HA_FLAG_PERM_RW, 1,
+                                          ble_att_svr_test_misc_attr_fn_w_1);
+    ble_att_svr_test_misc_register_uuid(BLE_UUID16_DECLARE(0x8989),
+                                          HA_FLAG_PERM_RW, 2,
+                                          ble_att_svr_test_misc_attr_fn_w_2);
+
+    /* 3: not writable. */
+    ble_att_svr_test_misc_register_uuid(BLE_UUID16_DECLARE(0xabab),
+                                          BLE_ATT_F_READ, 3,
+                                          ble_att_svr_test_misc_attr_fn_r_1);
+    /* 4: Encryption required. */
+    ble_att_svr_test_misc_register_uuid(
+        BLE_UUID16_DECLARE(0xabac), BLE_ATT_F_WRITE | BLE_ATT_F_WRITE_ENC, 4,
+        ble_att_svr_test_misc_attr_fn_w_1);
+
+    /* 5: Encryption+authentication required. */
+    ble_att_svr_test_misc_register_uuid(
+        BLE_UUID16_DECLARE(0xabad),
+        BLE_ATT_F_WRITE | BLE_ATT_F_WRITE_ENC | BLE_ATT_F_WRITE_AUTHEN,
+        5, ble_att_svr_test_misc_attr_fn_w_1);
+
+    /* 6: Write callback always fails. */
+    ble_att_svr_test_misc_register_uuid(
+        BLE_UUID16_DECLARE(0xabae), BLE_ATT_F_WRITE, 6,
+        ble_att_svr_test_misc_attr_fn_w_fail);
+
+    /*** Empty write succeeds. */
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     0, 0);
+
+    /*** Empty cancel succeeds. */
+    ble_att_svr_test_misc_exec_write(conn_handle, 0, 0, 0);
+
+    /*** Failure for prep write to nonexistent attribute. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 53525, 0, data, 10,
+                                     BLE_ATT_ERR_INVALID_HANDLE);
+
+    /*** Failure due to write-not-permitted. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 3, 0, data, 35,
+                                     BLE_ATT_ERR_WRITE_NOT_PERMITTED);
+
+    /*** Failure due to insufficient authentication (encryption required). */
+    ble_att_svr_test_misc_prep_write(conn_handle, 4, 0, data, 1,
+                                     BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
+
+    /*** Encrypt connection; ensure previous prep write now succeeds. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_sec_state.encrypted = 1;
+    ble_hs_unlock();
+
+    ble_att_svr_test_misc_prep_write(conn_handle, 4, 0, data, 1, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, 0, 0, 0);
+
+    /*** Failure due to insufficient authentication (not authenticated). */
+    ble_att_svr_test_misc_prep_write(conn_handle, 5, 0, data, 35,
+                                     BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
+
+    /*** Failure for write starting at nonzero offset. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 1, data, 10, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     BLE_ATT_ERR_INVALID_OFFSET, 1);
+    ble_att_svr_test_misc_verify_w_1(NULL, 0);
+
+    /*** Success for clear starting at nonzero offset. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 1, data, 10, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, 0, 0, 0);
+    ble_att_svr_test_misc_verify_w_1(NULL, 0);
+
+    /*** Failure for write with gap. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 10, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 11, data, 10, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     BLE_ATT_ERR_INVALID_OFFSET, 1);
+    ble_att_svr_test_misc_verify_w_1(NULL, 0);
+
+    /*** Success for clear with gap. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 10, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 11, data, 10, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, 0, 0, 0);
+    ble_att_svr_test_misc_verify_w_1(NULL, 0);
+
+    /*** Failure for overlong write. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 200, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 200, data + 200, 200, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 400, data + 400, 200, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN, 1);
+    ble_att_svr_test_misc_verify_w_1(NULL, 0);
+
+    /*** Successful two part write. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 20, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 20, data + 20, 20, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     0, 0);
+    ble_att_svr_test_misc_verify_w_1(data, 40);
+
+    /*** Successful three part write. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 35, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 35, data + 35, 43, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 78, data + 78, 1, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     0, 0);
+    ble_att_svr_test_misc_verify_w_1(data, 79);
+
+    /*** Successful two part write to two attributes. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 7, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 7, data + 7, 10, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 2, 0, data, 20, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 2, 20, data + 20, 10, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     0, 0);
+    ble_att_svr_test_misc_verify_w_1(data, 17);
+    ble_att_svr_test_misc_verify_w_2(data, 30);
+
+    /*** Fail write to second attribute; ensure first write doesn't occur. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 5, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 5, data + 5, 2, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 2, 0, data, 11, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 2, 12, data + 11, 19, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     BLE_ATT_ERR_INVALID_OFFSET, 2);
+    ble_att_svr_test_misc_verify_w_1(data, 17);
+    ble_att_svr_test_misc_verify_w_2(data, 30);
+
+    /*** Successful out of order write to two attributes. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 9, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 2, 0, data, 18, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 9, data + 9, 3, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 2, 18, data + 18, 43, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     0, 0);
+    ble_att_svr_test_misc_verify_w_1(data, 12);
+    ble_att_svr_test_misc_verify_w_2(data, 61);
+
+    /*** Fail due to attribute callback error. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 6, 0, data, 35, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 6, 35, data + 35, 43, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 6, 78, data + 78, 1, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_EXECUTE,
+                                     BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN, 6);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_notify)
+{
+    uint16_t conn_handle;
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /*** Successful notifies; verify callback is executed. */
+    /* 3-length attribute. */
+    ble_att_svr_test_misc_verify_notify(conn_handle, 10,
+                                        (uint8_t[]) { 1, 2, 3 }, 3, 1);
+    /* 1-length attribute. */
+    ble_att_svr_test_misc_verify_notify(conn_handle, 1,
+                                        (uint8_t[]) { 0xff }, 1, 1);
+    /* 0-length attribute. */
+    ble_att_svr_test_misc_verify_notify(conn_handle, 43, NULL, 0, 1);
+
+    /*** Bad notifies; verify callback is not executed. */
+    /* Attribute handle of 0. */
+    ble_att_svr_test_misc_verify_notify(conn_handle, 0,
+                                        (uint8_t[]) { 1, 2, 3 }, 3, 0);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_prep_write_tmo)
+{
+    int32_t ticks_from_now;
+    uint16_t conn_handle;
+    int rc;
+    int i;
+
+    static uint8_t data[1024];
+
+    conn_handle = ble_att_svr_test_misc_init(205);
+
+    /* Initialize some attribute data. */
+    for (i = 0; i < sizeof data; i++) {
+        data[i] = i;
+    }
+
+    /* Register a writable attribute. */
+    ble_att_svr_test_misc_register_uuid(BLE_UUID16_DECLARE(0x1234),
+                                          HA_FLAG_PERM_RW, 1,
+                                          ble_att_svr_test_misc_attr_fn_w_1);
+
+    /* Ensure timer is not set. */
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT_FATAL(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Receive a prepare write request. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 0, data, 7, 0);
+
+    /* Ensure timer will expire in 30 seconds. */
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_ATT_SVR_QUEUED_WRITE_TMO);
+
+    /* Almost let the timer expire. */
+    os_time_advance(BLE_HS_ATT_SVR_QUEUED_WRITE_TMO - 1);
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == 1);
+
+    /* Receive a second prepare write request. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 1, 7, data + 7, 10, 0);
+
+    /* Ensure timer got reset. */
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_ATT_SVR_QUEUED_WRITE_TMO);
+
+    /* Allow the timer to expire. */
+    ble_hs_test_util_hci_ack_set_disconnect(0);
+    os_time_advance(BLE_HS_ATT_SVR_QUEUED_WRITE_TMO);
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Ensure connection was terminated. */
+    ble_hs_test_util_hci_verify_tx_disconnect(2, BLE_ERR_REM_USER_CONN_TERM);
+
+    /* Free connection.  This is needed so that the prep write mbufs get
+     * freed and no mbuf leak gets reported.
+     */
+    rc = ble_hs_atomic_conn_delete(conn_handle);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_indicate)
+{
+    uint16_t conn_handle;
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /*** Successful indicates; verify callback is executed. */
+    /* 3-length attribute. */
+    ble_att_svr_test_misc_verify_indicate(conn_handle, 10,
+                                          (uint8_t[]) { 1, 2, 3 }, 3, 1);
+    /* 1-length attribute. */
+    ble_att_svr_test_misc_verify_indicate(conn_handle, 1,
+                                          (uint8_t[]) { 0xff }, 1, 1);
+    /* 0-length attribute. */
+    ble_att_svr_test_misc_verify_indicate(conn_handle, 43, NULL, 0, 1);
+
+    /*** Bad indicates; verify callback is not executed. */
+    /* Attribute handle of 0. */
+    ble_att_svr_test_misc_verify_indicate(conn_handle, 0,
+                                          (uint8_t[]) { 1, 2, 3 }, 3, 0);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_oom)
+{
+    struct os_mbuf *oms;
+    uint16_t conn_handle;
+    int rc;
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /* Register an attribute (primary service) for incoming read commands. */
+    ble_att_svr_test_misc_register_uuid(
+        BLE_UUID16_DECLARE(BLE_ATT_UUID_PRIMARY_SERVICE),
+        HA_FLAG_PERM_RW, 1, ble_att_svr_test_misc_attr_fn_rw_1);
+    ble_att_svr_test_attr_w_1_len = 2;
+    ble_att_svr_test_attr_w_1[0] = 0x12;
+    ble_att_svr_test_attr_w_1[1] = 0x34;
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming request. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+
+    /*** MTU; always respond affirmatively, even when no mbufs. */
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_mtu_cmd(conn_handle, 1, 100);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure we were able to send a real response. */
+    ble_att_svr_test_misc_verify_tx_mtu_rsp(conn_handle);
+
+    /*** Find information; always respond affirmatively, even when no mbufs. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_find_info_req(conn_handle, 1, 100);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure we were able to send a real response. */
+    ble_hs_test_util_verify_tx_find_info_rsp(
+        (struct ble_hs_test_util_att_info_entry[]) {
+            { .handle = 1, .uuid = BLE_UUID16_DECLARE(BLE_ATT_UUID_PRIMARY_SERVICE) },
+            { 0 },
+        });
+
+    /*** Find by type value. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_find_type_value_req(
+        conn_handle, 1, 100, 0x0001, ((uint8_t[2]){0x99, 0x99}), 2);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOMEM);
+
+    /* Ensure we were able to send an error response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 1,
+                                       BLE_ATT_ERR_INSUFFICIENT_RES);
+
+    /*** Read by type; always respond affirmatively, even when no mbufs. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_read_type_req16(conn_handle, 100, 0xffff,
+                                                 BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOENT);
+
+    /* Ensure we were able to send a non-OOM error response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_TYPE_REQ, 100,
+                                       BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** Read; always respond affirmatively, even when no mbufs. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_read_req(conn_handle, 1);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure we were able to send a real response. */
+    ble_hs_test_util_verify_tx_read_rsp(ble_att_svr_test_attr_w_1,
+                                        ble_att_svr_test_attr_w_1_len);
+
+    /*** Read blob; always respond affirmatively, even when no mbufs. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_read_blob_req(conn_handle, 1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure we were able to send a real response. */
+    ble_hs_test_util_verify_tx_read_blob_rsp(ble_att_svr_test_attr_w_1,
+                                             ble_att_svr_test_attr_w_1_len);
+
+    /*** Read multiple. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_read_mult_req(conn_handle,
+                                               ((uint16_t[2]){0x0001, 0x0002}),
+                                               2);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOMEM);
+
+    /* Ensure we were able to send an error response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ, 0,
+                                       BLE_ATT_ERR_INSUFFICIENT_RES);
+
+    /***
+     * Read by group type; always respond affirmatively, even when no
+     * mbufs.
+     */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_read_group_type_req16(
+        conn_handle, 11, 100, BLE_ATT_UUID_PRIMARY_SERVICE);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOENT);
+
+    /* Ensure we were able to send a non-OOM error response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_GROUP_TYPE_REQ, 11,
+                                       BLE_ATT_ERR_ATTR_NOT_FOUND);
+
+    /*** Write. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_write_req(conn_handle, 1,
+                                           ((uint8_t[1]){1}), 1);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOMEM);
+
+    /* Ensure we were able to send an error response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ, 1,
+                                       BLE_ATT_ERR_INSUFFICIENT_RES);
+
+    /*** Write command; no response. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_write_cmd(conn_handle, 1,
+                                           ((uint8_t[1]){1}), 1);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure no response sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /*** Prepare write. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_prep_write_req(conn_handle, 1, 0,
+                                                ((uint8_t[1]){1}), 1);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOMEM);
+
+    /* Ensure we were able to send an error response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_PREP_WRITE_REQ, 1,
+                                       BLE_ATT_ERR_INSUFFICIENT_RES);
+
+    /*** Notify; no response. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_notify_req(conn_handle, 1,
+                                            ((uint8_t[1]){1}), 1);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure no response sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /*** Indicate. */
+    ble_hs_test_util_prev_tx_dequeue();
+
+    /* Receive a request. */
+    rc = ble_hs_test_util_rx_att_indicate_req(conn_handle, 1,
+                                              ((uint8_t[1]){1}), 1);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOMEM);
+
+    /* Ensure we were able to send a real response. */
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_INDICATE_REQ, 1,
+                                       BLE_ATT_ERR_INSUFFICIENT_RES);
+
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_CASE_SELF(ble_att_svr_test_unsupported_req)
+{
+    uint16_t conn_handle;
+    int rc;
+    uint8_t buf[] = {0x3f, 0x00, 0x00, 0x01, 0x02, 0x03};
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /* Put handle into buf */
+    (*(uint16_t *)&buf[1]) = htole16(conn_handle);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(0x3f, 0,
+                                       BLE_ATT_ERR_REQ_NOT_SUPPORTED);
+
+    /* Check for no response when unknown command is sent */
+    buf[0] = 0x4f;
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                    buf, sizeof buf);
+    TEST_ASSERT(rc != 0);
+
+    /* Ensure no response sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    ble_att_svr_test_assert_mbufs_freed();
+}
+
+TEST_SUITE(ble_att_svr_suite)
+{
+    ble_att_svr_test_mtu();
+    ble_att_svr_test_read();
+    ble_att_svr_test_read_blob();
+    ble_att_svr_test_read_mult();
+    ble_att_svr_test_write();
+    ble_att_svr_test_find_info();
+    ble_att_svr_test_find_type_value();
+    ble_att_svr_test_read_type();
+    ble_att_svr_test_read_group_type();
+    ble_att_svr_test_prep_write();
+    ble_att_svr_test_prep_write_tmo();
+    ble_att_svr_test_notify();
+    ble_att_svr_test_indicate();
+    ble_att_svr_test_oom();
+    ble_att_svr_test_unsupported_req();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gap_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gap_test.c
new file mode 100644
index 00000000..7496e316
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gap_test.c
@@ -0,0 +1,3168 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_HCI_SET_SCAN_PARAM_LEN          (7)
+#define BLE_HCI_SET_SCAN_ENABLE_LEN         (2)
+#define BLE_HCI_DISCONNECT_CMD_LEN          (3)
+#define BLE_HCI_SET_ADV_PARAM_LEN           (15)
+#define BLE_HCI_SET_ADV_ENABLE_LEN          (1)
+#define BLE_HCI_CONN_UPDATE_LEN             (14)
+#define BLE_HCI_CONN_PARAM_REPLY_LEN        (14)
+#define BLE_HCI_CONN_PARAM_NEG_REPLY_LEN    (3)
+
+static struct ble_gap_event ble_gap_test_event;
+static int ble_gap_test_conn_status;
+static struct ble_gap_conn_desc ble_gap_test_conn_desc;
+static void *ble_gap_test_conn_arg;
+static struct ble_gap_upd_params ble_gap_test_conn_peer_params;
+static struct ble_gap_upd_params ble_gap_test_conn_self_params;
+
+static int ble_gap_test_disc_event_type;
+static struct ble_gap_disc_desc ble_gap_test_disc_desc;
+static void *ble_gap_test_disc_arg;
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_reset_cb_info(void)
+{
+    memset(&ble_gap_test_event, 0xff, sizeof ble_gap_test_event);
+    ble_gap_test_conn_status = -1;
+    memset(&ble_gap_test_conn_desc, 0xff, sizeof ble_gap_test_conn_desc);
+    ble_gap_test_conn_arg = (void *)-1;
+
+    ble_gap_test_disc_event_type = -1;
+    memset(&ble_gap_test_disc_desc, 0xff, sizeof ble_gap_test_disc_desc);
+    ble_gap_test_disc_arg = (void *)-1;
+}
+
+static void
+ble_gap_test_util_init(void)
+{
+    ble_hs_test_util_init();
+    ble_hs_test_util_set_static_rnd_addr((uint8_t[6]){ 1, 2, 3, 4, 5, 0xc0 });
+    ble_gap_test_util_reset_cb_info();
+}
+
+static int
+ble_gap_test_util_disc_cb(struct ble_gap_event *event, void *arg)
+{
+    ble_gap_test_disc_event_type = event->type;
+    ble_gap_test_disc_arg = arg;
+
+    if (event->type == BLE_GAP_EVENT_DISC) {
+        ble_gap_test_disc_desc = event->disc;
+    }
+
+    return 0;
+}
+
+static int
+ble_gap_test_util_connect_cb(struct ble_gap_event *event, void *arg)
+{
+    int *fail_reason;
+    int ret;
+
+    ble_gap_test_event = *event;
+    ble_gap_test_conn_arg = arg;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_CONNECT:
+        ble_gap_test_conn_status = event->connect.status;
+        ret = ble_gap_conn_find(event->connect.conn_handle,
+                                &ble_gap_test_conn_desc);
+        TEST_ASSERT_FATAL(ble_gap_test_conn_status || ret == 0);
+        break;
+
+    case BLE_GAP_EVENT_DISCONNECT:
+        ble_gap_test_conn_status = event->disconnect.reason;
+        ble_gap_test_conn_desc = event->disconnect.conn;
+        break;
+
+    case BLE_GAP_EVENT_CONN_UPDATE:
+        ble_gap_test_conn_status = event->conn_update.status;
+        ret = ble_gap_conn_find(event->conn_update.conn_handle,
+                               &ble_gap_test_conn_desc);
+        TEST_ASSERT_FATAL(ret == 0);
+        break;
+
+    case BLE_GAP_EVENT_TERM_FAILURE:
+        ble_gap_test_conn_status = event->term_failure.status;
+        ret = ble_gap_conn_find(event->term_failure.conn_handle,
+                                &ble_gap_test_conn_desc);
+        TEST_ASSERT_FATAL(ret == 0);
+        break;
+
+    case BLE_GAP_EVENT_ADV_COMPLETE:
+        ble_gap_test_conn_arg = arg;
+        break;
+
+    case BLE_GAP_EVENT_CONN_UPDATE_REQ:
+        ble_gap_test_conn_peer_params = *event->conn_update_req.peer_params;
+        *event->conn_update_req.self_params = ble_gap_test_conn_self_params;
+        ret = ble_gap_conn_find(event->conn_update_req.conn_handle,
+                                &ble_gap_test_conn_desc);
+        TEST_ASSERT_FATAL(ret == 0);
+
+        fail_reason = arg;
+        if (fail_reason == NULL) {
+            return 0;
+        } else {
+            return *fail_reason;
+        }
+        break;
+
+    case BLE_GAP_EVENT_MTU:
+        break;
+
+    default:
+        TEST_ASSERT_FATAL(0);
+        break;
+    }
+
+    return 0;
+}
+
+static int
+ble_gap_test_util_copy_cb(struct ble_gap_event *event, void *arg)
+{
+    ble_gap_test_event = *event;
+    ble_gap_test_conn_arg = arg;
+
+    return 0;
+}
+
+static void
+ble_gap_test_util_verify_tx_clear_wl(void)
+{
+    uint8_t param_len;
+
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_CLEAR_WHITE_LIST,
+                                   &param_len);
+    TEST_ASSERT(param_len == 0);
+}
+
+static void
+ble_gap_test_util_verify_tx_add_wl(ble_addr_t *addr)
+{
+    uint8_t param_len;
+    uint8_t *param;
+    int i;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_ADD_WHITE_LIST,
+                                           &param_len);
+    TEST_ASSERT(param_len == 7);
+    TEST_ASSERT(param[0] == addr->type);
+    for (i = 0; i < 6; i++) {
+        TEST_ASSERT(param[1 + i] == addr->val[i]);
+    }
+}
+
+static void
+ble_gap_test_util_verify_tx_set_scan_params(uint8_t own_addr_type,
+                                            uint8_t scan_type,
+                                            uint16_t itvl,
+                                            uint16_t scan_window,
+                                            uint8_t filter_policy)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_SET_SCAN_PARAMS,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_SET_SCAN_PARAM_LEN);
+    TEST_ASSERT(param[0] == scan_type);
+    TEST_ASSERT(get_le16(param + 1) == itvl);
+    TEST_ASSERT(get_le16(param + 3) == scan_window);
+    TEST_ASSERT(param[5] == own_addr_type);
+    TEST_ASSERT(param[6] == filter_policy);
+}
+
+static void
+ble_gap_test_util_verify_tx_scan_enable(uint8_t enable,
+                                        uint8_t filter_duplicates)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_SET_SCAN_ENABLE,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_SET_SCAN_ENABLE_LEN);
+    TEST_ASSERT(param[0] == enable);
+    TEST_ASSERT(param[1] == filter_duplicates);
+}
+
+static void
+ble_hs_test_util_hci_verify_tx_create_conn_cancel(void)
+{
+    uint8_t param_len;
+
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_CREATE_CONN_CANCEL,
+                                   &param_len);
+    TEST_ASSERT(param_len == 0);
+}
+
+static void
+ble_gap_test_util_verify_tx_disconnect(void)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LINK_CTRL,
+                                           BLE_HCI_OCF_DISCONNECT_CMD,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_DISCONNECT_CMD_LEN);
+    TEST_ASSERT(get_le16(param + 0) == 2);
+    TEST_ASSERT(param[2] == BLE_ERR_REM_USER_CONN_TERM);
+}
+
+static void
+ble_gap_test_util_verify_tx_adv_params(void)
+{
+    uint8_t param_len;
+
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_SET_ADV_PARAMS,
+                                   &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_SET_ADV_PARAM_LEN);
+
+    /* Note: Content of message verified in ble_hs_adv_test.c. */
+}
+
+static void
+ble_gap_test_util_verify_tx_adv_data(void)
+{
+    uint8_t param_len;
+
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_SET_ADV_DATA,
+                                   &param_len);
+    /* Note: Content of message verified in ble_hs_adv_test.c. */
+}
+
+#if 0
+static void
+ble_gap_test_util_verify_tx_rsp_data(void)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA,
+                                           &param_len);
+    (void)param; /* XXX: Verify other fields. */
+}
+#endif
+
+static void
+ble_gap_test_util_verify_tx_adv_enable(int enabled)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_SET_ADV_ENABLE,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_SET_ADV_ENABLE_LEN);
+    TEST_ASSERT(param[0] == !!enabled);
+}
+
+static void
+ble_gap_test_util_verify_tx_update_conn(struct ble_gap_upd_params *params)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_CONN_UPDATE,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_CONN_UPDATE_LEN);
+    TEST_ASSERT(get_le16(param + 0) == 2);
+    TEST_ASSERT(get_le16(param + 2) == params->itvl_min);
+    TEST_ASSERT(get_le16(param + 4) == params->itvl_max);
+    TEST_ASSERT(get_le16(param + 6) == params->latency);
+    TEST_ASSERT(get_le16(param + 8) == params->supervision_timeout);
+    TEST_ASSERT(get_le16(param + 10) == params->min_ce_len);
+    TEST_ASSERT(get_le16(param + 12) == params->max_ce_len);
+}
+
+static void
+ble_gap_test_util_verify_tx_params_reply_pos(void)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_REM_CONN_PARAM_RR,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_CONN_PARAM_REPLY_LEN);
+    TEST_ASSERT(get_le16(param + 0) == 2);
+    TEST_ASSERT(get_le16(param + 2) == ble_gap_test_conn_self_params.itvl_min);
+    TEST_ASSERT(get_le16(param + 4) == ble_gap_test_conn_self_params.itvl_max);
+    TEST_ASSERT(get_le16(param + 6) == ble_gap_test_conn_self_params.latency);
+    TEST_ASSERT(get_le16(param + 8) ==
+                ble_gap_test_conn_self_params.supervision_timeout);
+    TEST_ASSERT(get_le16(param + 10) ==
+                ble_gap_test_conn_self_params.min_ce_len);
+    TEST_ASSERT(get_le16(param + 12) ==
+                ble_gap_test_conn_self_params.max_ce_len);
+}
+
+static void
+ble_gap_test_util_verify_tx_params_reply_neg(uint8_t reason)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_CONN_PARAM_NEG_REPLY_LEN);
+    TEST_ASSERT(get_le16(param + 0) == 2);
+    TEST_ASSERT(param[2] == reason);
+}
+
+static void
+ble_gap_test_util_rx_update_complete(
+    uint8_t status,
+    const struct ble_gap_upd_params *params)
+{
+    struct ble_hci_ev_le_subev_conn_upd_complete evt;
+
+    evt.subev_code = BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE;
+    evt.status = status;
+    evt.conn_handle = htole16(2);
+    evt.conn_itvl = htole16(params->itvl_max);
+    evt.conn_latency = htole16(params->latency);
+    evt.supervision_timeout = htole16(params->supervision_timeout);
+
+    ble_gap_rx_update_complete(&evt);
+}
+
+static int
+ble_gap_test_util_rx_param_req(struct ble_gap_upd_params *params, int pos,
+                               int *cmd_idx, int cmd_fail_idx,
+                               uint8_t fail_status)
+{
+    struct ble_hci_ev_le_subev_rem_conn_param_req evt;
+    uint16_t opcode;
+    uint8_t hci_status;
+
+    evt.subev_code = BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ;
+    evt.conn_handle = htole16(2);
+    evt.min_interval = htole16(params->itvl_min);
+    evt.max_interval = htole16(params->itvl_max);
+    evt.latency = htole16(params->latency);
+    evt.timeout = params->supervision_timeout;
+
+    if (pos) {
+        opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_RR);
+    } else {
+        opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR);
+    }
+    if (*cmd_idx == cmd_fail_idx) {
+        hci_status = fail_status;
+    } else {
+        hci_status = 0;
+    }
+    (*cmd_idx)++;
+
+    ble_hs_test_util_hci_ack_set(opcode, hci_status);
+    ble_gap_rx_param_req(&evt);
+
+    return hci_status;
+}
+
+/*****************************************************************************
+ * $white list                                                               *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_wl_set(ble_addr_t *addrs, int addrs_count, int cmd_fail_idx,
+                         uint8_t fail_status)
+{
+    int cmd_idx;
+    int rc;
+    int i;
+
+    ble_gap_test_util_init();
+    cmd_idx = 0;
+
+    rc = ble_hs_test_util_wl_set(addrs, addrs_count, cmd_fail_idx,
+                                 fail_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(fail_status));
+
+    /* Verify tx of clear white list command. */
+    ble_gap_test_util_verify_tx_clear_wl();
+    if (cmd_idx >= cmd_fail_idx) {
+        return;
+    }
+    cmd_idx++;
+
+    /* Verify tx of add white list commands. */
+    for (i = 0; i < addrs_count; i++) {
+        ble_gap_test_util_verify_tx_add_wl(addrs + i);
+        if (cmd_idx >= cmd_fail_idx) {
+            return;
+        }
+        cmd_idx++;
+    }
+}
+
+TEST_CASE_SELF(ble_gap_test_case_wl_bad_args)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /*** 0 white list entries. */
+    rc = ble_hs_test_util_wl_set(NULL, 0, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Invalid address type. */
+    rc = ble_hs_test_util_wl_set(
+        ((ble_addr_t[]) { {
+            5, { 1, 2, 3, 4, 5, 6 }
+        }, }),
+        1, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** White-list-using connection in progress. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, NULL, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_wl_set(
+        ((ble_addr_t[]) { {
+            BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }
+        }, }),
+        1, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EBUSY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_wl_ctlr_fail)
+{
+    int i;
+
+    ble_addr_t addrs[] = {
+        { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        { BLE_ADDR_PUBLIC, { 2, 3, 4, 5, 6, 7 } },
+        { BLE_ADDR_PUBLIC, { 3, 4, 5, 6, 7, 8 } },
+        { BLE_ADDR_PUBLIC, { 4, 5, 6, 7, 8, 9 } },
+    };
+    int addrs_count = sizeof addrs / sizeof addrs[0];
+
+    for (i = 0; i < 5; i++) {
+        ble_gap_test_util_wl_set(addrs, addrs_count, i,
+                                 BLE_ERR_UNSPECIFIED);
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_wl_good)
+{
+    ble_addr_t addrs[] = {
+        { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        { BLE_ADDR_PUBLIC, { 2, 3, 4, 5, 6, 7 } },
+        { BLE_ADDR_PUBLIC, { 3, 4, 5, 6, 7, 8 } },
+        { BLE_ADDR_PUBLIC, { 4, 5, 6, 7, 8, 9 } },
+    };
+    int addrs_count = sizeof addrs / sizeof addrs[0];
+
+    ble_gap_test_util_wl_set(addrs, addrs_count, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_wl)
+{
+    ble_gap_test_case_wl_good();
+    ble_gap_test_case_wl_bad_args();
+    ble_gap_test_case_wl_ctlr_fail();
+}
+
+/*****************************************************************************
+ * $discovery                                                                *
+ *****************************************************************************/
+
+static int
+ble_gap_test_util_disc(uint8_t own_addr_type,
+                       const struct ble_gap_disc_params *disc_params,
+                       struct ble_gap_disc_desc *desc, int cmd_fail_idx,
+                       uint8_t fail_status)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    TEST_ASSERT(!ble_gap_disc_active());
+
+    /* Begin the discovery procedure. */
+    rc = ble_hs_test_util_disc(own_addr_type, BLE_HS_FOREVER, disc_params,
+                               ble_gap_test_util_disc_cb, NULL, cmd_fail_idx,
+                               fail_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(fail_status));
+    if (rc == 0) {
+        TEST_ASSERT(ble_gap_master_in_progress());
+        ble_gap_rx_adv_report(desc);
+    } else {
+        TEST_ASSERT(ble_gap_test_disc_event_type == -1);
+    }
+
+    if (cmd_fail_idx > 0) {
+        /* Verify tx of set scan parameters command. */
+        ble_gap_test_util_verify_tx_set_scan_params(
+            own_addr_type,
+            disc_params->passive ?
+                BLE_HCI_SCAN_TYPE_PASSIVE :
+                BLE_HCI_SCAN_TYPE_ACTIVE,
+            disc_params->itvl,
+            disc_params->window,
+            disc_params->filter_policy);
+    }
+
+    if (cmd_fail_idx > 1) {
+        /* Verify tx of scan enable command. */
+        ble_gap_test_util_verify_tx_scan_enable(
+            1, disc_params->filter_duplicates);
+    }
+
+    if (rc == 0) {
+        TEST_ASSERT(ble_gap_disc_active());
+    }
+
+    return rc;
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_bad_args)
+{
+    struct ble_gap_disc_params params;
+    int rc;
+
+    params.itvl = 0;
+    params.window = 0;
+    params.filter_policy = BLE_HCI_SCAN_FILT_NO_WL;
+    params.limited = 0;
+    params.passive = 0;
+    params.filter_duplicates = 0;
+
+    ble_gap_test_util_init();
+
+    /*** Invalid filter policy. */
+    params.filter_policy = 6;
+    rc = ble_gap_disc(BLE_OWN_ADDR_PUBLIC, 0, &params,
+                      ble_gap_test_util_disc_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_good)
+{
+    uint8_t adv_data[32];
+    uint8_t flags;
+    uint8_t own_addr_type;
+    int passive;
+    int limited;
+    int rc;
+
+    struct ble_gap_disc_desc desc = {
+        .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
+        .addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        .length_data = 0,
+        .rssi = 0,
+        .data = adv_data,
+    };
+    struct ble_gap_disc_params disc_params = {
+        .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1,
+        .window = BLE_GAP_SCAN_SLOW_WINDOW1,
+        .filter_policy = BLE_HCI_CONN_FILT_NO_WL,
+        .limited = 0,
+        .passive = 0,
+        .filter_duplicates = 0,
+    };
+
+    flags = BLE_HS_ADV_F_DISC_LTD;
+    rc = ble_hs_adv_set_flat(BLE_HS_ADV_TYPE_FLAGS, 1, &flags,
+                             adv_data, &desc.length_data,
+                             sizeof adv_data);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    for (own_addr_type = 0;
+         own_addr_type <= BLE_OWN_ADDR_RPA_RANDOM_DEFAULT;
+         own_addr_type++)
+    for (passive = 0; passive <= 1; passive++)
+    for (limited = 0; limited <= 1; limited++) {
+        disc_params.passive = passive;
+        disc_params.limited = limited;
+        ble_gap_test_util_disc(own_addr_type, &disc_params, &desc, -1, 0);
+
+        TEST_ASSERT(ble_gap_master_in_progress());
+        TEST_ASSERT(ble_gap_test_disc_event_type == BLE_GAP_EVENT_DISC);
+        TEST_ASSERT(ble_gap_test_disc_desc.event_type ==
+                    BLE_HCI_ADV_TYPE_ADV_IND);
+        TEST_ASSERT(ble_gap_test_disc_desc.addr.type ==
+                    BLE_ADDR_PUBLIC);
+        TEST_ASSERT(ble_gap_test_disc_desc.length_data == 3);
+        TEST_ASSERT(ble_gap_test_disc_desc.rssi == 0);
+        TEST_ASSERT(memcmp(ble_gap_test_disc_desc.addr.val, desc.addr.val,
+                    6) == 0);
+        TEST_ASSERT(ble_gap_test_disc_arg == NULL);
+
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_ltd_mismatch)
+{
+    int rc;
+    struct ble_gap_disc_desc desc_gen = {
+        .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
+        .length_data = 3,
+        .rssi = 0,
+        .addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        .data = (uint8_t[BLE_HS_ADV_MAX_SZ]){
+            2,
+            BLE_HS_ADV_TYPE_FLAGS,
+            BLE_HS_ADV_F_DISC_GEN,
+        },
+    };
+
+    struct ble_gap_disc_desc desc_lim = {
+        .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
+        .length_data = 3,
+        .rssi = 0,
+        .addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        .data = (uint8_t[BLE_HS_ADV_MAX_SZ]){
+            2,
+            BLE_HS_ADV_TYPE_FLAGS,
+            BLE_HS_ADV_F_DISC_LTD,
+        },
+    };
+
+    struct ble_gap_disc_params disc_params = {
+        .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1,
+        .window = BLE_GAP_SCAN_SLOW_WINDOW1,
+        .filter_policy = BLE_HCI_CONN_FILT_NO_WL,
+        .limited = 1,
+        .passive = 0,
+        .filter_duplicates = 0,
+    };
+
+    rc = ble_gap_test_util_disc(BLE_OWN_ADDR_PUBLIC, &disc_params, &desc_gen,
+                                -1, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* Verify that the report was ignored because of a mismatched LTD flag. */
+    TEST_ASSERT(ble_gap_test_disc_event_type == -1);
+
+    /* Stop the scan and swap the flags. */
+    rc = ble_hs_test_util_disc_cancel(0);
+    TEST_ASSERT(rc == 0);
+
+    disc_params.limited = 0;
+    rc = ble_gap_test_util_disc(BLE_OWN_ADDR_PUBLIC, &disc_params, &desc_lim,
+                                -1, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* This time we should have reported the advertisement; general discovery
+     * hears everything.
+     */
+    TEST_ASSERT(ble_gap_test_disc_event_type == BLE_GAP_EVENT_DISC);
+
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_hci_fail)
+{
+    int fail_idx;
+    int limited;
+    int rc;
+
+    struct ble_gap_disc_desc desc = {
+        .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
+        .length_data = 0,
+        .rssi = 0,
+        .addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        .data = NULL,
+    };
+    struct ble_gap_disc_params disc_params = {
+        .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1,
+        .window = BLE_GAP_SCAN_SLOW_WINDOW1,
+        .filter_policy = BLE_HCI_CONN_FILT_NO_WL,
+        .limited = 0,
+        .passive = 0,
+        .filter_duplicates = 0,
+    };
+
+    for (limited = 0; limited <= 1; limited++) {
+        disc_params.limited = limited;
+
+        for (fail_idx = 0; fail_idx < 2; fail_idx++) {
+            rc = ble_gap_test_util_disc(BLE_OWN_ADDR_PUBLIC, &disc_params,
+                                        &desc, fail_idx, BLE_ERR_UNSUPPORTED);
+            TEST_ASSERT(rc == BLE_HS_HCI_ERR(BLE_ERR_UNSUPPORTED));
+            TEST_ASSERT(!ble_gap_master_in_progress());
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+ble_gap_test_util_disc_dflts_once(int limited)
+{
+    struct ble_gap_disc_params params;
+    uint16_t exp_window;
+    uint16_t exp_itvl;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    memset(&params, 0, sizeof params);
+    params.limited = limited;
+
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, 0, &params,
+                               ble_gap_test_util_disc_cb, NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    if (limited) {
+        exp_itvl = BLE_GAP_LIM_DISC_SCAN_INT;
+        exp_window = BLE_GAP_LIM_DISC_SCAN_WINDOW;
+    } else {
+        exp_itvl = BLE_GAP_SCAN_FAST_INTERVAL_MIN;
+        exp_window = BLE_GAP_SCAN_FAST_WINDOW;
+    }
+    ble_gap_test_util_verify_tx_set_scan_params(
+        BLE_OWN_ADDR_PUBLIC,
+        BLE_HCI_SCAN_TYPE_ACTIVE,
+        exp_itvl,
+        exp_window,
+        BLE_HCI_SCAN_FILT_NO_WL);
+
+    ble_gap_test_util_verify_tx_scan_enable(1, 0);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_dflts)
+{
+    ble_gap_test_util_disc_dflts_once(0);
+    ble_gap_test_util_disc_dflts_once(1);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_already)
+{
+    static const struct ble_gap_disc_params disc_params = { 0 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a discovery procedure. */
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_gap_test_util_disc_cb,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EALREADY if we try to discover. */
+    rc = ble_gap_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER, &disc_params,
+                               ble_gap_test_util_disc_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_busy)
+{
+    static const struct ble_gap_disc_params disc_params = { 0 };
+    static const ble_addr_t peer_addr = {
+        BLE_ADDR_PUBLIC,
+        { 1, 2, 3, 4, 5, 6 }
+    };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a connect procedure. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EBUSY if we try to discover. */
+    rc = ble_gap_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER, &disc_params,
+                               ble_gap_test_util_disc_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EBUSY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_disc)
+{
+    ble_gap_test_case_disc_bad_args();
+    ble_gap_test_case_disc_good();
+    ble_gap_test_case_disc_ltd_mismatch();
+    ble_gap_test_case_disc_hci_fail();
+    ble_gap_test_case_disc_dflts();
+    ble_gap_test_case_disc_already();
+    ble_gap_test_case_disc_busy();
+}
+
+/*****************************************************************************
+ * $direct connect                                                           *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_good)
+{
+    struct ble_gap_conn_complete evt;
+    struct ble_gap_conn_params params;
+    int rc;
+
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+
+    ble_gap_test_util_init();
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_conn_active());
+
+    params.scan_itvl = 0x12;
+    params.scan_window = 0x11;
+    params.itvl_min = 25;
+    params.itvl_max = 26;
+    params.latency = 1;
+    params.supervision_timeout = 20;
+    params.min_ce_len = 3;
+    params.max_ce_len = 4;
+
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC,
+                                  &peer_addr, 0, &params,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_conn_active());
+
+    TEST_ASSERT(ble_gap_master_in_progress());
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == BLE_HS_ENOTCONN);
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony
+     * ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    /* Receive connection complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 2;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
+    memcpy(evt.peer_addr, peer_addr.val, 6);
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr.val, 6) == 0);
+
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_bad_args)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /*** Invalid address type. */
+    rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC,
+                         &((ble_addr_t) { 5, { 1, 2, 3, 4, 5, 6 }}), 0, NULL,
+                         ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /*** Connection already in progress. */
+    rc = ble_hs_test_util_connect(
+        BLE_OWN_ADDR_PUBLIC,
+        &((ble_addr_t) { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }}),
+        0, NULL, ble_gap_test_util_connect_cb,
+        NULL, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    rc = ble_gap_connect(
+        BLE_OWN_ADDR_PUBLIC,
+        &((ble_addr_t) { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }}),
+        0, NULL, ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_dflt_params)
+{
+    static const ble_addr_t peer_addr = {
+        BLE_ADDR_PUBLIC,
+        { 2, 3, 8, 6, 6, 1 }
+    };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC,
+                                  &peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_already)
+{
+    static const struct ble_gap_conn_params conn_params = { 0 };
+    static const ble_addr_t peer_addr = {
+        BLE_ADDR_PUBLIC,
+        { 1, 2, 3, 4, 5, 6 }
+    };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a connect procedure. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EALREADY if we try to connect. */
+    rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr, BLE_HS_FOREVER,
+                         &conn_params, ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_done)
+{
+    static const struct ble_gap_conn_params conn_params = { 0 };
+    static const ble_addr_t peer_addr = {
+        BLE_ADDR_PUBLIC,
+        { 1, 2, 3, 4, 5, 6 }
+    };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Successfully connect to the peer. */
+    ble_hs_test_util_create_conn(2, peer_addr.val,
+                                 ble_gap_test_util_connect_cb, NULL);
+
+    /* Ensure host indicates BLE_HS_EDONE if we try to connect to the same
+     * peer.
+     */
+    rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC,  &peer_addr, BLE_HS_FOREVER,
+                         &conn_params, ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EDONE);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_busy)
+{
+    static const struct ble_gap_disc_params disc_params = { 0 };
+    static const struct ble_gap_conn_params conn_params = { 0 };
+    static const ble_addr_t peer_addr = {
+        BLE_ADDR_PUBLIC,
+        { 1, 2, 3, 4, 5, 6 }
+    };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a discovery procedure. */
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_gap_test_util_disc_cb,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EBUSY if we try to connect. */
+    rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr, BLE_HS_FOREVER,
+                         &conn_params, ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EBUSY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_gen_fail_evt)
+{
+    static const ble_addr_t peer_addr = {BLE_ADDR_PUBLIC, {1, 2, 3, 4, 5, 6}};
+    struct ble_gap_conn_complete evt;
+    struct ble_hci_ev_disconn_cmp disc_evt;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a connect procedure. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr, 0, NULL,
+                                  ble_gap_test_util_copy_cb, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Controller indicates failure via connect complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 6;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
+    evt.peer_addr_type = BLE_ADDR_PUBLIC;
+    memcpy(evt.peer_addr, peer_addr.val, 6);
+
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure failed connect was reported to application. */
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_event.connect.status ==
+                BLE_HS_HCI_ERR(BLE_ERR_SUCCESS));
+
+    memset(&disc_evt, 0, sizeof disc_evt);
+    disc_evt.conn_handle = htole16(6);
+    disc_evt.status = BLE_ERR_SUCCESS;
+    disc_evt.reason = BLE_ERR_CONN_ESTABLISHMENT;
+
+    ble_gap_rx_disconn_complete(&disc_evt);
+
+    /* Ensure failed connect was reported to application. */
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(ble_gap_test_event.disconnect.reason ==
+                BLE_HS_HCI_ERR(BLE_ERR_CONN_ESTABLISHMENT));
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_conn_gen)
+{
+    ble_gap_test_case_conn_gen_good();
+    ble_gap_test_case_conn_gen_bad_args();
+    ble_gap_test_case_conn_gen_dflt_params();
+    ble_gap_test_case_conn_gen_already();
+    ble_gap_test_case_conn_gen_done();
+    ble_gap_test_case_conn_gen_busy();
+    ble_gap_test_case_conn_gen_fail_evt();
+}
+
+/*****************************************************************************
+ * $cancel                                                                   *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_conn_cancel(uint8_t hci_status)
+{
+    struct ble_gap_conn_complete evt;
+    int rc;
+
+    /* Initiate cancel procedure. */
+    rc = ble_hs_test_util_conn_cancel(hci_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(hci_status));
+
+    /* Verify tx of cancel create connection command. */
+    ble_hs_test_util_hci_verify_tx_create_conn_cancel();
+    if (rc != 0) {
+        return;
+    }
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* Receive connection complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_UNK_CONN_ID;
+    /* test if host correctly ignores other fields if status is error */
+    evt.connection_handle = 0x0fff;
+
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_event.connect.status == BLE_HS_EAPP);
+}
+
+static void
+ble_gap_test_util_conn_and_cancel(uint8_t *peer_addr, uint8_t hci_status)
+{
+    ble_addr_t addr = { BLE_ADDR_PUBLIC };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    memcpy(addr.val, peer_addr, 6);
+
+    /* Begin creating a connection. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* Initiate cancel procedure. */
+    ble_gap_test_util_conn_cancel(hci_status);
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == BLE_HS_ENOTCONN);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_cancel_bad_args)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Initiate cancel procedure with no connection in progress. */
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    rc = ble_hs_test_util_conn_cancel(0);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_cancel_good)
+{
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_conn_and_cancel(peer_addr, 0);
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_event.connect.status == BLE_HS_EAPP);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == BLE_HS_CONN_HANDLE_NONE);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_cancel_ctlr_fail)
+{
+    struct ble_gap_conn_complete evt;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_conn_and_cancel(peer_addr, BLE_ERR_REPEATED_ATTEMPTS);
+
+    /* Make sure the host didn't invoke the application callback.  The cancel
+     * failure was indicated via the return code from the gap call.
+     */
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony
+     * ack
+     */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    /* Allow connection complete to succeed. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 2;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
+    memcpy(evt.peer_addr, peer_addr, 6);
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_conn_cancel)
+{
+    ble_gap_test_case_conn_cancel_good();
+    ble_gap_test_case_conn_cancel_bad_args();
+    ble_gap_test_case_conn_cancel_ctlr_fail();
+}
+
+/*****************************************************************************
+ * $terminate                                                                *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_terminate(uint8_t *peer_addr, uint8_t hci_status)
+{
+    struct ble_hci_ev_disconn_cmp evt;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Create a connection. */
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    /* Reset the callback event code; we don't care about the successful
+     * connection in this test.
+     */
+    ble_gap_test_event.type = -1;
+
+    /* Terminate the connection. */
+    rc = ble_hs_test_util_conn_terminate(2, hci_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(hci_status));
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Verify tx of disconnect command. */
+    ble_gap_test_util_verify_tx_disconnect();
+
+    if (hci_status == 0) {
+        /* Receive disconnection complete event. */
+        evt.conn_handle = htole16(2);
+        evt.status = 0;
+        evt.reason = BLE_ERR_CONN_TERM_LOCAL;
+        ble_gap_rx_disconn_complete(&evt);
+    }
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_terminate_bad_args)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /*** Nonexistent connection. */
+    rc = ble_hs_test_util_conn_terminate(2, 0);
+    TEST_ASSERT(rc == BLE_HS_ENOTCONN);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_terminate_good)
+{
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_terminate(peer_addr, 0);
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(ble_gap_test_conn_status ==
+                BLE_HS_HCI_ERR(BLE_ERR_CONN_TERM_LOCAL));
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(ble_gap_test_conn_desc.peer_id_addr.type ==
+                BLE_ADDR_PUBLIC);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_arg == NULL);
+
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == BLE_HS_ENOTCONN);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_terminate_ctlr_fail)
+{
+    struct ble_hci_ev_disconn_cmp evt;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    /* Create a connection. */
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    /* Terminate the connection. */
+    rc = ble_hs_test_util_conn_terminate(2, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Verify tx of disconnect command. */
+    ble_gap_test_util_verify_tx_disconnect();
+
+    /* Receive failed disconnection complete event. */
+    evt.conn_handle = htole16(2);
+    evt.status = BLE_ERR_UNSUPPORTED;
+    evt.reason = 0;
+    ble_gap_rx_disconn_complete(&evt);
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_TERM_FAILURE);
+    TEST_ASSERT(ble_gap_test_conn_status ==
+                BLE_HS_HCI_ERR(BLE_ERR_UNSUPPORTED));
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(ble_gap_test_conn_desc.peer_id_addr.type ==
+                BLE_ADDR_PUBLIC);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_arg == NULL);
+
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_terminate_hci_fail)
+{
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_terminate(peer_addr, BLE_ERR_REPEATED_ATTEMPTS);
+
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+    TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_conn_terminate)
+{
+    ble_gap_test_case_conn_terminate_bad_args();
+    ble_gap_test_case_conn_terminate_good();
+    ble_gap_test_case_conn_terminate_ctlr_fail();
+    ble_gap_test_case_conn_terminate_hci_fail();
+}
+
+/*****************************************************************************
+ * $conn find                                                                *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_gap_test_case_conn_find)
+{
+
+    struct ble_gap_conn_desc desc;
+    struct ble_hs_conn *conn;
+    uint8_t pub_addr[6];
+    int rc;
+
+    /*** We are master; public addresses. */
+    ble_gap_test_util_init();
+
+    rc = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, pub_addr, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_create_rpa_conn(8,
+                                     BLE_OWN_ADDR_PUBLIC,
+                                     ((uint8_t[6]){0,0,0,0,0,0}),
+                                     BLE_ADDR_PUBLIC,
+                                     ((uint8_t[6]){2,3,4,5,6,7}),
+                                     ((uint8_t[6]){0,0,0,0,0,0}),
+                                     BLE_HS_TEST_CONN_FEAT_ALL,
+                                     ble_gap_test_util_connect_cb,
+                                     NULL);
+
+    rc = ble_gap_conn_find(8, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.conn_handle == 8);
+    TEST_ASSERT(desc.our_id_addr.type == BLE_ADDR_PUBLIC);
+    TEST_ASSERT(desc.our_ota_addr.type == BLE_ADDR_PUBLIC);
+    TEST_ASSERT(desc.peer_ota_addr.type == BLE_ADDR_PUBLIC);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_MASTER);
+    TEST_ASSERT(memcmp(desc.our_ota_addr.val, pub_addr, 6) == 0);
+    TEST_ASSERT(memcmp(desc.our_id_addr.val, pub_addr, 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_ota_addr.val,
+                       ((uint8_t[6]){2,3,4,5,6,7}), 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_id_addr.val,
+                       ((uint8_t[6]){2,3,4,5,6,7}), 6) == 0);
+    TEST_ASSERT(desc.conn_itvl == BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(desc.conn_latency == BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+    TEST_ASSERT(desc.master_clock_accuracy == 0);
+    TEST_ASSERT(!desc.sec_state.encrypted);
+    TEST_ASSERT(!desc.sec_state.authenticated);
+    TEST_ASSERT(!desc.sec_state.bonded);
+
+    /*** Swap roles. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(8);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    rc = ble_gap_conn_find(8, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_SLAVE);
+
+    /*** We are master; RPAs. */
+    ble_gap_test_util_init();
+
+    rc = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, pub_addr, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_create_rpa_conn(54,
+                                     BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+                                     ((uint8_t[6]){0x40,1,2,3,4,5}),
+                                     BLE_ADDR_RANDOM_ID,
+                                     ((uint8_t[6]){3,4,5,6,7,8}),
+                                     ((uint8_t[6]){0x50,1,2,3,4,5}),
+                                     BLE_HS_TEST_CONN_FEAT_ALL,
+                                     ble_gap_test_util_connect_cb,
+                                     NULL);
+
+    rc = ble_gap_conn_find(54, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.conn_handle == 54);
+    TEST_ASSERT(desc.our_id_addr.type == BLE_ADDR_PUBLIC);
+    TEST_ASSERT(desc.our_ota_addr.type == BLE_ADDR_RANDOM);
+    TEST_ASSERT(desc.peer_ota_addr.type == BLE_ADDR_RANDOM);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_MASTER);
+    TEST_ASSERT(memcmp(desc.our_ota_addr.val,
+                       ((uint8_t[6]){0x40,1,2,3,4,5}), 6) == 0);
+    TEST_ASSERT(memcmp(desc.our_id_addr.val, pub_addr, 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_ota_addr.val,
+                       ((uint8_t[6]){0x50,1,2,3,4,5}), 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_id_addr.val,
+                       ((uint8_t[6]){3,4,5,6,7,8}), 6) == 0);
+    TEST_ASSERT(desc.conn_itvl == BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(desc.conn_latency == BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+    TEST_ASSERT(desc.master_clock_accuracy == 0);
+    TEST_ASSERT(!desc.sec_state.encrypted);
+    TEST_ASSERT(!desc.sec_state.authenticated);
+    TEST_ASSERT(!desc.sec_state.bonded);
+
+    /*** Swap roles. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(54);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    rc = ble_gap_conn_find(54, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_SLAVE);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_conn_find)
+{
+    ble_gap_test_case_conn_find();
+}
+
+/*****************************************************************************
+ * $advertise                                                                *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_adv(uint8_t own_addr_type,
+                      const ble_addr_t *peer_addr, uint8_t conn_mode,
+                      uint8_t disc_mode, int connect_status,
+                      int cmd_fail_idx, uint8_t fail_status)
+{
+    struct ble_gap_conn_complete evt;
+    struct ble_gap_adv_params adv_params;
+    struct ble_hs_adv_fields adv_fields;
+    uint8_t hci_status;
+    int cmd_idx;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = conn_mode;
+    adv_params.disc_mode = disc_mode;
+
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    cmd_idx = 0;
+
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+    rc = ble_hs_test_util_adv_set_fields(&adv_fields, cmd_fail_idx,
+                                         fail_status);
+    if (cmd_fail_idx < 2) {
+        hci_status = fail_status;
+    } else {
+        hci_status = 0;
+    }
+    TEST_ASSERT_FATAL(rc == BLE_HS_HCI_ERR(hci_status));
+    cmd_idx += 2;
+
+    if (rc == 0) {
+        ble_gap_test_util_verify_tx_adv_data();
+    }
+
+    if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
+        rc = ble_hs_test_util_adv_start(own_addr_type,
+                                        peer_addr, &adv_params, BLE_HS_FOREVER,
+                                        ble_gap_test_util_connect_cb, NULL,
+                                        cmd_fail_idx - cmd_idx, fail_status);
+
+        TEST_ASSERT(rc == BLE_HS_HCI_ERR(fail_status));
+        cmd_idx++;
+    }
+
+    if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
+        /* Verify tx of set advertising params command. */
+        ble_gap_test_util_verify_tx_adv_params();
+    }
+    cmd_idx++;
+
+    if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
+        /* Verify tx of set advertise enable command. */
+        ble_gap_test_util_verify_tx_adv_enable(1);
+    }
+    cmd_idx++;
+
+    if (connect_status != -1 &&
+        (fail_status == 0 || cmd_fail_idx >= cmd_idx)) {
+
+        TEST_ASSERT(ble_gap_adv_active());
+
+        /* Receive a connection complete event. */
+        if (conn_mode != BLE_GAP_CONN_MODE_NON) {
+            if (connect_status == BLE_ERR_SUCCESS) {
+                /*
+                 * ble_gap_rx_conn_complete() will send extra HCI command, need
+                 * phony ack
+                 */
+                ble_hs_test_util_hci_ack_set(
+                    ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                                BLE_HCI_OCF_LE_RD_REM_FEAT),
+                    0);
+            }
+
+            memset(&evt, 0, sizeof evt);
+            evt.status = connect_status;
+
+            if (connect_status == BLE_ERR_SUCCESS) {
+                evt.connection_handle = 2;
+                evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE;
+                memcpy(evt.peer_addr, peer_addr->val, 6);
+            } else {
+                /* test if host correctly ignores other fields if status is
+                 * error
+                 */
+                evt.connection_handle = 0x0fff;
+            }
+
+            rc = ble_gap_rx_conn_complete(&evt, 0);
+            TEST_ASSERT(rc == 0);
+
+            if (connect_status == 0 ||
+                connect_status == BLE_ERR_DIR_ADV_TMO) {
+
+                TEST_ASSERT(!ble_gap_adv_active());
+            } else {
+                TEST_ASSERT(ble_gap_adv_active());
+            }
+        }
+    }
+}
+
+TEST_CASE_SELF(ble_gap_test_case_adv_bad_args)
+{
+    struct ble_gap_adv_params adv_params;
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    ble_addr_t peer_addr_inv = { 12, { 1, 2, 3, 4, 5, 6 }};
+    int rc;
+
+    ble_gap_test_util_init();
+
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    /*** Invalid discoverable mode. */
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.disc_mode = 43;
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    &peer_addr, &adv_params, BLE_HS_FOREVER,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    /*** Invalid connectable mode. */
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = 27;
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    &peer_addr, &adv_params, BLE_HS_FOREVER,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    /*** Invalid peer address type with directed advertisable mode. */
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_DIR;
+    rc = ble_hs_test_util_adv_start(
+        BLE_OWN_ADDR_PUBLIC,
+        &peer_addr_inv, &adv_params, BLE_HS_FOREVER,
+        ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    /*** Advertising already in progress. */
+    adv_params = ble_hs_test_util_adv_params;
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    &peer_addr, &adv_params, BLE_HS_FOREVER,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_adv_active());
+
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    &peer_addr, &adv_params, BLE_HS_FOREVER,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+    TEST_ASSERT(ble_gap_adv_active());
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+ble_gap_test_util_adv_verify_dflt_params(uint8_t own_addr_type,
+                                         const ble_addr_t *peer_addr,
+                                         uint8_t conn_mode,
+                                         uint8_t disc_mode)
+{
+    struct ble_hci_le_set_adv_params_cp hci_cmd;
+    struct ble_gap_adv_params adv_params;
+    uint8_t *hci_buf;
+    uint8_t hci_param_len;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = conn_mode;
+    adv_params.disc_mode = disc_mode;
+
+    /* Let stack calculate all default parameters. */
+    adv_params.itvl_min = 0;
+    adv_params.itvl_max = 0;
+    adv_params.channel_map = 0;
+    adv_params.filter_policy = 0;
+    adv_params.high_duty_cycle = 0;
+
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC, peer_addr,
+                                    &adv_params, BLE_HS_FOREVER,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure default parameters properly filled in. */
+    hci_buf = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                             BLE_HCI_OCF_LE_SET_ADV_PARAMS,
+                                             &hci_param_len);
+    TEST_ASSERT_FATAL(hci_buf != NULL);
+    TEST_ASSERT_FATAL(hci_param_len == BLE_HCI_SET_ADV_PARAM_LEN);
+
+    hci_cmd.min_interval = get_le16(hci_buf + 0);
+    hci_cmd.max_interval = get_le16(hci_buf + 2);
+    hci_cmd.type = hci_buf[4];
+    hci_cmd.own_addr_type = hci_buf[5];
+    hci_cmd.peer_addr_type = hci_buf[6];
+    memcpy(hci_cmd.peer_addr, hci_buf + 7, 6);
+    hci_cmd.chan_map = hci_buf[13];
+    hci_cmd.filter_policy = hci_buf[14];
+
+    if (conn_mode == BLE_GAP_CONN_MODE_NON) {
+        TEST_ASSERT(hci_cmd.min_interval == BLE_GAP_ADV_FAST_INTERVAL2_MIN);
+        TEST_ASSERT(hci_cmd.max_interval == BLE_GAP_ADV_FAST_INTERVAL2_MAX);
+    } else {
+        TEST_ASSERT(hci_cmd.min_interval == BLE_GAP_ADV_FAST_INTERVAL1_MIN);
+        TEST_ASSERT(hci_cmd.max_interval == BLE_GAP_ADV_FAST_INTERVAL1_MAX);
+    }
+
+    if (conn_mode == BLE_GAP_CONN_MODE_NON) {
+        if (disc_mode == BLE_GAP_DISC_MODE_NON) {
+            TEST_ASSERT(hci_cmd.type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND);
+        } else {
+            TEST_ASSERT(hci_cmd.type == BLE_HCI_ADV_TYPE_ADV_SCAN_IND);
+        }
+    } else if (conn_mode == BLE_GAP_CONN_MODE_UND) {
+        TEST_ASSERT(hci_cmd.type == BLE_HCI_ADV_TYPE_ADV_IND);
+    } else {
+        TEST_ASSERT(hci_cmd.type == BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD);
+    }
+}
+
+TEST_CASE_SELF(ble_gap_test_case_adv_dflt_params)
+{
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            ble_gap_test_util_adv_verify_dflt_params(
+                BLE_OWN_ADDR_PUBLIC, &peer_addr, c, d);
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_adv_good)
+{
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            ble_gap_test_util_adv(BLE_OWN_ADDR_PUBLIC,
+                                  &peer_addr, c, d, BLE_ERR_SUCCESS, -1, 0);
+
+            if (c != BLE_GAP_CONN_MODE_NON) {
+                TEST_ASSERT(!ble_gap_adv_active());
+                TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+                TEST_ASSERT(ble_gap_test_conn_status == 0);
+                TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+                TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                                   peer_addr.val, 6) == 0);
+                TEST_ASSERT(ble_gap_test_conn_arg == NULL);
+            }
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_adv_ctlr_fail)
+{
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON + 1; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            ble_gap_test_util_adv(BLE_OWN_ADDR_PUBLIC,
+                                  &peer_addr, c, d, BLE_ERR_DIR_ADV_TMO,
+                                  -1, 0);
+
+            TEST_ASSERT(!ble_gap_adv_active());
+            TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_ADV_COMPLETE);
+            TEST_ASSERT(ble_gap_test_conn_desc.conn_handle ==
+                        BLE_HS_CONN_HANDLE_NONE);
+            TEST_ASSERT(ble_gap_test_conn_arg == NULL);
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_adv_hci_fail)
+{
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int fail_idx;
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            for (fail_idx = 0; fail_idx < 4; fail_idx++) {
+                ble_gap_test_util_adv(BLE_OWN_ADDR_PUBLIC,
+                                      &peer_addr,
+                                      c, d, 0, fail_idx, BLE_ERR_UNSUPPORTED);
+
+                TEST_ASSERT(!ble_gap_adv_active());
+                TEST_ASSERT(ble_gap_test_event.type == 0xff);
+            }
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_adv)
+{
+    ble_gap_test_case_adv_bad_args();
+    ble_gap_test_case_adv_dflt_params();
+    ble_gap_test_case_adv_good();
+    ble_gap_test_case_adv_ctlr_fail();
+    ble_gap_test_case_adv_hci_fail();
+}
+
+/*****************************************************************************
+ * $stop advertise                                                           *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_stop_adv(const ble_addr_t *peer_addr,
+                           uint8_t conn_mode, uint8_t disc_mode,
+                           int cmd_fail_idx, uint8_t fail_status)
+{
+    uint8_t hci_status;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start advertising; don't rx a successful connection event. */
+    ble_gap_test_util_adv(BLE_OWN_ADDR_PUBLIC, peer_addr,
+                          conn_mode, disc_mode, -1, -1, 0);
+
+    TEST_ASSERT(ble_gap_adv_active());
+
+    /* Stop advertising. */
+    hci_status = cmd_fail_idx == 0 ? fail_status : 0;
+
+    rc = ble_hs_test_util_adv_stop(hci_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(hci_status));
+
+    /* Verify tx of advertising enable command. */
+    ble_gap_test_util_verify_tx_adv_enable(0);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_stop_adv_good)
+{
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            ble_gap_test_util_stop_adv(&peer_addr, c, d, -1, 0);
+            TEST_ASSERT(!ble_gap_adv_active());
+            TEST_ASSERT(ble_gap_test_event.type == 0xff);
+            TEST_ASSERT(ble_gap_test_conn_status == -1);
+            TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == (uint16_t)-1);
+            TEST_ASSERT(ble_gap_test_conn_arg == (void *)-1);
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_stop_adv_hci_fail)
+{
+    ble_addr_t peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            ble_gap_test_util_stop_adv(&peer_addr, c, d,
+                                       0, BLE_ERR_UNSUPPORTED);
+            TEST_ASSERT(ble_gap_adv_active());
+            TEST_ASSERT(ble_gap_test_event.type == 0xff);
+            TEST_ASSERT(ble_gap_test_conn_status == -1);
+            TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == (uint16_t)-1);
+            TEST_ASSERT(ble_gap_test_conn_arg == (void *)-1);
+        }
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_stop_adv)
+{
+    ble_gap_test_case_stop_adv_good();
+    ble_gap_test_case_stop_adv_hci_fail();
+}
+
+/*****************************************************************************
+ * $update connection                                                        *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_update_verify_params(struct ble_gap_upd_params *params,
+                                      uint8_t ble_hs_err)
+{
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    rc = ble_hs_test_util_conn_update(2, params, 0);
+    TEST_ASSERT(rc == ble_hs_err);
+}
+
+static void
+ble_gap_test_util_update_no_l2cap(struct ble_gap_upd_params *params,
+                                  int master,
+                                  uint8_t hci_status, int event_status)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    if (!master) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find(2);
+        TEST_ASSERT_FATAL(conn != NULL);
+        conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+        ble_hs_unlock();
+    }
+
+    /* Erase callback info reported during connection establishment; we only
+     * care about updates.
+     */
+    ble_gap_test_util_reset_cb_info();
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    rc = ble_hs_test_util_conn_update(2, params, hci_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(hci_status));
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Verify tx of connection update command. */
+    ble_gap_test_util_verify_tx_update_conn(params);
+
+    if (rc == 0) {
+        TEST_ASSERT(ble_gap_dbg_update_active(2));
+
+        /* Attempt two duplicate updates; ensure BLE_HS_EALREADY gets returned
+         * both times.  Make sure initial update still completes successfully
+         * (MYNEWT-702).
+         */
+        rc = ble_hs_test_util_conn_update(2, params, 0);
+        TEST_ASSERT(rc == BLE_HS_EALREADY);
+        rc = ble_hs_test_util_conn_update(2, params, 0);
+        TEST_ASSERT(rc == BLE_HS_EALREADY);
+
+        /* Receive connection update complete event. */
+        ble_gap_test_util_rx_update_complete(event_status, params);
+
+        TEST_ASSERT(!ble_gap_master_in_progress());
+        TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+        TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+        TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(event_status));
+        if (event_status == 0) {
+            TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+            TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                               peer_addr, 6) == 0);
+            TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == params->itvl_max);
+            TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
+                        params->latency);
+            TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                        params->supervision_timeout);
+        }
+    } else {
+        TEST_ASSERT(!ble_gap_master_in_progress());
+        TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+        TEST_ASSERT(ble_gap_test_event.type == 0xff);
+    }
+}
+
+static void
+ble_gap_test_util_update_l2cap(struct ble_gap_upd_params *params,
+                               uint16_t l2cap_result)
+{
+    struct ble_l2cap_sig_update_params l2cap_params;
+    struct ble_hs_conn *conn;
+    uint8_t id;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn_feat(2, peer_addr,
+                                      BLE_HS_TEST_CONN_FEAT_NO_CONN_PARAM,
+                                      ble_gap_test_util_connect_cb, NULL);
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    /* Erase callback info reported during connection establishment; we only
+     * care about updates.
+     */
+    ble_gap_test_util_reset_cb_info();
+
+    rc = ble_hs_test_util_conn_update(2, params, 0xFF);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_gap_dbg_update_active(2));
+
+    l2cap_params.itvl_min = params->itvl_min;
+    l2cap_params.itvl_max = params->itvl_max;
+    l2cap_params.slave_latency = params->latency;
+    l2cap_params.timeout_multiplier = params->supervision_timeout;
+    id = ble_hs_test_util_verify_tx_l2cap_update_req(&l2cap_params);
+
+    /* Receive l2cap connection parameter update response. */
+    ble_hs_test_util_rx_l2cap_update_rsp(2, id, l2cap_result);
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    if (l2cap_result == BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT) {
+        /* Receive connection update complete event. */
+        ble_gap_test_util_rx_update_complete(0, params);
+    }
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    if (l2cap_result != BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT) {
+        TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_EREJECT);
+    } else {
+        TEST_ASSERT(ble_gap_test_conn_status == 0);
+        TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == params->itvl_max);
+        TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == params->latency);
+        TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                    params->supervision_timeout);
+    }
+
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+}
+
+static void
+ble_gap_test_util_update_l2cap_tmo(struct ble_gap_upd_params *params,
+                                   uint8_t hci_status, uint8_t event_status,
+                                   int rx_l2cap)
+{
+    struct ble_l2cap_sig_update_params l2cap_params;
+    struct ble_hs_conn *conn;
+    uint8_t id;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn_feat(2, peer_addr,
+                                      BLE_HS_TEST_CONN_FEAT_NO_CONN_PARAM,
+                                      ble_gap_test_util_connect_cb, NULL);
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    /* Erase callback info reported during connection establishment; we only
+     * care about updates.
+     */
+    ble_gap_test_util_reset_cb_info();
+
+    rc = ble_hs_test_util_conn_update(2, params, 0xFF);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_gap_dbg_update_active(2));
+
+    if (rx_l2cap) {
+        l2cap_params.itvl_min = params->itvl_min;
+        l2cap_params.itvl_max = params->itvl_max;
+        l2cap_params.slave_latency = params->latency;
+        l2cap_params.timeout_multiplier = params->supervision_timeout;
+        id = ble_hs_test_util_verify_tx_l2cap_update_req(&l2cap_params);
+
+        /* Receive l2cap connection parameter update response. */
+        ble_hs_test_util_rx_l2cap_update_rsp(
+            2, id, BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT);
+
+        TEST_ASSERT(!ble_gap_dbg_update_active(2));
+    } else {
+        TEST_ASSERT(ble_gap_dbg_update_active(2));
+    }
+
+    /* Ensure no update event reported. */
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+
+    /* Advance 29 seconds; ensure no timeout reported.
+     * Note: L2CAP signaling timeout is 30 sec, GAP update timeout is 40 sec
+     */
+    os_time_advance(29 * OS_TICKS_PER_SEC);
+    ble_gap_timer();
+    ble_l2cap_sig_timer();
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+
+    /* Advance 30th second; ensure timeout reported. */
+    os_time_advance(1 * OS_TICKS_PER_SEC);
+
+    /* If L2CAP response has been received, GAP Timer is removed */
+     if (!rx_l2cap) {
+
+         /* Timeout will result in a terminate HCI command being sent; schedule ack
+          * from controller.
+          */
+         ble_hs_test_util_hci_ack_set_disconnect(0);
+
+         ble_gap_timer();
+         ble_l2cap_sig_timer();
+
+         /* Verify terminate was sent. */
+         ble_gap_test_util_verify_tx_disconnect();
+
+        TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+        TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_ETIMEOUT);
+        TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+        TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                           peer_addr, 6) == 0);
+    } else {
+        ble_gap_timer();
+        ble_l2cap_sig_timer();
+
+        TEST_ASSERT(ble_gap_test_event.type == 0xff);
+    }
+}
+
+static void
+ble_gap_test_util_update_peer(uint8_t status,
+                              struct ble_gap_upd_params *params)
+{
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Receive connection update complete event. */
+    ble_gap_test_util_rx_update_complete(status, params);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(status));
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+
+    if (status == 0) {
+        TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == params->itvl_max);
+        TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == params->latency);
+        TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                    params->supervision_timeout);
+    }
+
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+}
+
+static void
+ble_gap_test_util_update_req_pos(struct ble_gap_upd_params *peer_params,
+                                 struct ble_gap_upd_params *self_params,
+                                 int cmd_fail_idx, uint8_t hci_status)
+{
+    int cmd_idx;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+    cmd_idx = 0;
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    ble_gap_test_conn_self_params = *self_params;
+    rc = ble_gap_test_util_rx_param_req(peer_params, 1, &cmd_idx, cmd_fail_idx,
+                                        hci_status);
+    if (rc != 0) {
+        goto hci_fail;
+    }
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* We don't maintain an update entry when the peer initiates. */
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    /* Verify tx of connection parameters reply command. */
+    ble_gap_test_util_verify_tx_params_reply_pos();
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    /* Receive connection update complete event. */
+    ble_gap_test_util_rx_update_complete(0, self_params);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_conn_status == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == self_params->itvl_max);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == self_params->latency);
+    TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                self_params->supervision_timeout);
+
+    return;
+
+hci_fail:
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(hci_status));
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
+                BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
+                BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+}
+
+static void
+ble_gap_test_util_update_req_neg(struct ble_gap_upd_params *peer_params,
+                                 int cmd_fail_idx, uint8_t hci_status)
+{
+    int cmd_idx;
+    int reason;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+    cmd_idx = 0;
+
+    reason = BLE_ERR_UNSPECIFIED;
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 &reason);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    rc = ble_gap_test_util_rx_param_req(peer_params, 0, &cmd_idx, cmd_fail_idx,
+                                        hci_status);
+    if (rc != 0) {
+        goto hci_fail;
+    }
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    /* Verify tx of connection parameters negative reply command. */
+    ble_gap_test_util_verify_tx_params_reply_neg(reason);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    return;
+
+hci_fail:
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(hci_status));
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
+                BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
+                BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+}
+
+static void
+ble_gap_test_util_update_req_concurrent(
+    struct ble_gap_upd_params *init_params,
+    struct ble_gap_upd_params *peer_params,
+    struct ble_gap_upd_params *self_params,
+    int cmd_fail_idx,
+    uint8_t fail_status)
+{
+    uint8_t hci_status;
+    int cmd_idx;
+    int rc;
+
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    hci_status = cmd_fail_idx == 0 ? fail_status : 0;
+    rc = ble_hs_test_util_conn_update(2, init_params, hci_status);
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(hci_status));
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Verify tx of connection update command. */
+    ble_gap_test_util_verify_tx_update_conn(init_params);
+
+    if (rc == 0) {
+        TEST_ASSERT(ble_gap_dbg_update_active(2));
+    } else {
+        TEST_ASSERT(!ble_gap_dbg_update_active(2));
+        return;
+    }
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_dbg_update_active(2));
+
+    /* Receive connection parameter update request from peer. */
+    ble_gap_test_conn_self_params = *self_params;
+    rc = ble_gap_test_util_rx_param_req(peer_params, 1, &cmd_idx, cmd_fail_idx,
+                                        hci_status);
+    if (rc != 0) {
+        goto hci_fail;
+    }
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_dbg_update_active(2));
+
+    /* Verify tx of connection parameters reply command. */
+    ble_gap_test_util_verify_tx_params_reply_pos();
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_dbg_update_active(2));
+
+    /* Receive connection update complete event. */
+    ble_gap_test_util_rx_update_complete(0, self_params);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_dbg_update_active(2));
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_conn_status == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == self_params->itvl_max);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == self_params->latency);
+    TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                self_params->supervision_timeout);
+
+    return;
+
+hci_fail:
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(fail_status));
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr.val,
+                       peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
+                BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
+                BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_conn_good)
+{
+    ble_gap_test_util_update_no_l2cap(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        1, 0, 0);
+
+    ble_gap_test_util_update_no_l2cap(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        1, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_conn_verify_params)
+{
+    /* GOOD */
+    ble_gap_test_util_update_verify_params(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        0);
+
+    /* BAD */
+    ble_gap_test_util_update_verify_params(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 1,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        BLE_HS_EINVAL);
+
+    ble_gap_test_util_update_verify_params(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 0x0C80 + 1,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        BLE_HS_EINVAL);
+
+    ble_gap_test_util_update_verify_params(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 50,
+            .supervision_timeout = 200,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        BLE_HS_EINVAL);
+
+    ble_gap_test_util_update_verify_params(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .latency = 0x01F4,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        BLE_HS_EINVAL);
+
+    ble_gap_test_util_update_verify_params(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 600,
+            .supervision_timeout = 300,
+            .latency = 1,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        BLE_HS_EINVAL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_conn_bad)
+{
+    ble_gap_test_util_update_no_l2cap(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        1, 0, BLE_ERR_LMP_COLLISION);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_conn_hci_fail)
+{
+    ble_gap_test_util_update_no_l2cap(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        1, BLE_ERR_UNSUPPORTED, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_conn_l2cap)
+{
+    struct ble_gap_upd_params params = {
+        .itvl_min = 10,
+        .itvl_max = 100,
+        .supervision_timeout = 200,
+        .min_ce_len = 123,
+        .max_ce_len = 456,
+    };
+
+    /* Accepted L2CAP. */
+    ble_gap_test_util_update_l2cap(&params,
+                                   BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT);
+
+    /* Rejected L2CAP. */
+    ble_gap_test_util_update_l2cap(&params,
+                                   BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_peer_good)
+{
+    ble_gap_test_util_update_peer(0,
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 0,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}));
+
+    ble_gap_test_util_update_peer(0,
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 100,
+            .supervision_timeout = 100,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}));
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_req_good)
+{
+    ble_gap_test_util_update_req_pos(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        -1, 0);
+
+    ble_gap_test_util_update_req_pos(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 100,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 554,
+            .max_ce_len = 554,
+        }}),
+        -1, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_req_hci_fail)
+{
+    ble_gap_test_util_update_req_pos(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        0, BLE_ERR_UNSUPPORTED);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_req_reject)
+{
+    ble_gap_test_util_update_req_neg(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        -1, 0);
+
+    ble_gap_test_util_update_req_neg(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        -1, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_concurrent_good)
+{
+    ble_gap_test_util_update_req_concurrent(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        -1, 0);
+
+    ble_gap_test_util_update_req_concurrent(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 20,
+            .itvl_max = 200,
+            .supervision_timeout = 350,
+            .min_ce_len = 111,
+            .max_ce_len = 222,
+        }}),
+        -1, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_concurrent_hci_fail)
+{
+    ble_gap_test_util_update_req_concurrent(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 20,
+            .itvl_max = 200,
+            .supervision_timeout = 350,
+            .min_ce_len = 111,
+            .max_ce_len = 222,
+        }}),
+        0, BLE_ERR_UNSUPPORTED);
+
+    ble_gap_test_util_update_req_concurrent(
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 10,
+            .itvl_max = 100,
+            .supervision_timeout = 200,
+            .min_ce_len = 123,
+            .max_ce_len = 456,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 50,
+            .itvl_max = 500,
+            .supervision_timeout = 800,
+            .min_ce_len = 555,
+            .max_ce_len = 888,
+        }}),
+        ((struct ble_gap_upd_params[]) { {
+            .itvl_min = 20,
+            .itvl_max = 200,
+            .supervision_timeout = 350,
+            .min_ce_len = 111,
+            .max_ce_len = 222,
+        }}),
+        1, BLE_ERR_UNSUPPORTED);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_update_conn)
+{
+    ble_gap_test_case_update_conn_good();
+    ble_gap_test_case_update_conn_bad();
+    ble_gap_test_case_update_conn_hci_fail();
+    ble_gap_test_case_update_conn_l2cap();
+    ble_gap_test_case_update_peer_good();
+    ble_gap_test_case_update_req_good();
+    ble_gap_test_case_update_req_hci_fail();
+    ble_gap_test_case_update_req_reject();
+    ble_gap_test_case_update_concurrent_good();
+    ble_gap_test_case_update_concurrent_hci_fail();
+    ble_gap_test_case_update_conn_verify_params();
+}
+
+/*****************************************************************************
+ * $timeout                                                                  *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_conn_forever(void)
+{
+    int32_t ticks_from_now;
+
+    /* Initiate a connect procedure with no timeout. */
+    ble_hs_test_util_connect(
+        BLE_OWN_ADDR_PUBLIC,
+        &((ble_addr_t) { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }}),
+        BLE_HS_FOREVER,
+        NULL, ble_gap_test_util_connect_cb,
+        NULL, 0);
+
+    /* Ensure no pending GAP event. */
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Advance 100 seconds; ensure no timeout reported. */
+    os_time_advance(100 * OS_TICKS_PER_SEC);
+    ble_gap_timer();
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+    TEST_ASSERT(ble_gap_conn_active());
+}
+
+static void
+ble_gap_test_util_conn_timeout(int32_t duration_ms)
+{
+    struct ble_gap_conn_complete evt;
+    uint32_t duration_ticks;
+    int32_t ticks_from_now;
+    int rc;
+
+    TEST_ASSERT_FATAL(duration_ms != BLE_HS_FOREVER);
+
+    /* Initiate a connect procedure with the specified timeout. */
+    ble_hs_test_util_connect(
+        BLE_OWN_ADDR_PUBLIC,
+        &((ble_addr_t) { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }}),
+        duration_ms,
+        NULL, ble_gap_test_util_connect_cb,
+        NULL, 0);
+
+    /* Ensure next GAP event is at the expected time. */
+    rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+    TEST_ASSERT_FATAL(rc == 0);
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == duration_ticks);
+
+    /* Advance duration ms; ensure timeout event does not get reported before
+     * connection complete event rxed.
+     */
+    os_time_advance(duration_ms);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
+        0);
+
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Ensure cancel create connection command was sent. */
+    ble_hs_test_util_hci_verify_tx_create_conn_cancel();
+
+    /* Ensure timer has been stopped. */
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Receive the connection complete event indicating a successful cancel. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_UNK_CONN_ID;
+    /* test if host correctly ignores other fields if status is error */
+    evt.connection_handle = 0x0fff;
+
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure the GAP event was triggered. */
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_ETIMEOUT);
+
+    /* Clear GAP event for remainder of test. */
+    ble_gap_test_util_reset_cb_info();
+}
+
+static void
+ble_gap_test_util_disc_forever(void)
+{
+    struct ble_gap_disc_params params;
+    int32_t ticks_from_now;
+
+    memset(&params, 0, sizeof params);
+
+    /* Initiate a discovery procedure with no timeout. */
+    ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC,
+                          BLE_HS_FOREVER, &params, ble_gap_test_util_disc_cb,
+                          NULL, -1, 0);
+
+    /* Ensure no pending GAP event. */
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Advance 100 seconds; ensure no timeout reported. */
+    os_time_advance(100 * OS_TICKS_PER_SEC);
+    TEST_ASSERT(ble_gap_test_disc_event_type == -1);
+    TEST_ASSERT(ble_gap_disc_active());
+}
+
+static void
+ble_gap_test_util_disc_timeout(int32_t duration_ms)
+{
+    struct ble_gap_disc_params params;
+    uint32_t duration_ticks;
+    int32_t ticks_from_now;
+    int rc;
+
+    TEST_ASSERT_FATAL(duration_ms != BLE_HS_FOREVER);
+
+    memset(&params, 0, sizeof params);
+
+    /* Initiate a discovery procedure with the specified timeout. */
+    ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC,
+                          duration_ms, &params, ble_gap_test_util_disc_cb,
+                          NULL, -1, 0);
+
+    /* Ensure next GAP event is at the expected time. */
+    rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+    TEST_ASSERT_FATAL(rc == 0);
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == duration_ticks);
+
+    /* Advance duration ms; ensure timeout event was reported. */
+    os_time_advance(duration_ms);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        0);
+    ticks_from_now = ble_gap_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    TEST_ASSERT(ble_gap_test_disc_event_type == BLE_GAP_EVENT_DISC_COMPLETE);
+
+    /* Clear GAP event for remainder of test. */
+    ble_gap_test_util_reset_cb_info();
+}
+
+TEST_CASE_SELF(ble_gap_test_case_update_timeout)
+{
+    struct ble_gap_upd_params params = {
+        .itvl_min = 10,
+        .itvl_max = 100,
+        .supervision_timeout = 200,
+        .min_ce_len = 123,
+        .max_ce_len = 456,
+    };
+
+    /* L2CAP - Local unsupported; L2CAP timeout. */
+    ble_gap_test_util_update_l2cap_tmo(&params, BLE_ERR_UNKNOWN_HCI_CMD, 0, 0);
+
+    /* L2CAP - Remote unsupported; L2CAP timeout. */
+    ble_gap_test_util_update_l2cap_tmo(&params, 0, BLE_ERR_UNSUPP_REM_FEATURE,
+                                       0);
+
+    /* L2CAP - Remote unsupported; LL timeout. */
+    ble_gap_test_util_update_l2cap_tmo(&params, 0, BLE_ERR_UNSUPP_REM_FEATURE,
+                                       1);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_timeout_conn_forever)
+{
+    ble_gap_test_util_init();
+
+    /* 30 ms. */
+    ble_gap_test_util_conn_timeout(30);
+
+    /* No timeout. */
+    ble_gap_test_util_conn_forever();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_timeout_conn_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* 30 ms. */
+    ble_gap_test_util_conn_timeout(30);
+
+    /* 20 ms. */
+    ble_gap_test_util_conn_timeout(20);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_forever_conn_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* No timeout. */
+    ble_gap_test_util_conn_forever();
+
+    /* Cancel connect procedure manually. */
+    ble_gap_test_util_conn_cancel(0);
+
+    /* Clear GAP event for remainder of test. */
+    ble_gap_test_util_reset_cb_info();
+
+    /* 30 ms. */
+    ble_gap_test_util_conn_timeout(30);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_timeout_disc_forever)
+{
+    ble_gap_test_util_init();
+
+    /* 30 ms. */
+    ble_gap_test_util_disc_timeout(30);
+
+    /* No timeout. */
+    ble_gap_test_util_disc_forever();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_timeout_disc_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* 30 ms. */
+    ble_gap_test_util_disc_timeout(30);
+
+    /* 20 ms. */
+    ble_gap_test_util_disc_timeout(20);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_disc_forever_disc_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* No timeout. */
+    ble_gap_test_util_disc_forever();
+
+    /* Cancel discovery procedure manually. */
+    ble_hs_test_util_disc_cancel(0);
+
+    /* 30 ms. */
+    ble_gap_test_util_disc_timeout(30);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_conn_timeout_disc_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* 15 seconds. */
+    ble_gap_test_util_conn_timeout(15000);
+
+    /* 1280 ms. */
+    ble_gap_test_util_disc_timeout(1280);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_timeout)
+{
+    ble_gap_test_case_conn_timeout_conn_forever();
+    ble_gap_test_case_conn_timeout_conn_timeout();
+    ble_gap_test_case_conn_forever_conn_timeout();
+
+    ble_gap_test_case_disc_timeout_disc_forever();
+    ble_gap_test_case_disc_timeout_disc_timeout();
+    ble_gap_test_case_disc_forever_disc_timeout();
+
+    ble_gap_test_case_conn_timeout_disc_timeout();
+
+    ble_gap_test_case_update_timeout();
+}
+
+TEST_CASE_SELF(ble_gap_test_case_mtu_us)
+{
+    const uint8_t peer_addr[6] = { 1,2,3,4,5,6 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    ble_att_set_preferred_mtu(200);
+
+    rc = ble_gattc_exchange_mtu(2, NULL, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_test_util_verify_tx_mtu_cmd(1, 200);
+
+    rc = ble_hs_test_util_rx_att_mtu_cmd(2, 0, 123);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_MTU);
+    TEST_ASSERT(ble_gap_test_event.mtu.conn_handle == 2);
+    TEST_ASSERT(ble_gap_test_event.mtu.channel_id == BLE_L2CAP_CID_ATT);
+    TEST_ASSERT(ble_gap_test_event.mtu.value == 123);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_mtu_peer)
+{
+    const uint8_t peer_addr[6] = { 1,2,3,4,5,6 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    ble_att_set_preferred_mtu(200);
+
+    rc = ble_hs_test_util_rx_att_mtu_cmd(2, 1, 123);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_test_util_verify_tx_mtu_cmd(0, 200);
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_MTU);
+    TEST_ASSERT(ble_gap_test_event.mtu.conn_handle == 2);
+    TEST_ASSERT(ble_gap_test_event.mtu.channel_id == BLE_L2CAP_CID_ATT);
+    TEST_ASSERT(ble_gap_test_event.mtu.value == 123);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_mtu)
+{
+    ble_gap_test_case_mtu_us();
+    ble_gap_test_case_mtu_peer();
+}
+
+/*****************************************************************************
+ * $set cb                                                                   *
+ *****************************************************************************/
+
+static int
+ble_gap_test_util_set_cb_event(struct ble_gap_event *event, void *arg)
+{
+    struct ble_gap_event *event_arg;
+
+    event_arg = arg;
+
+    *event_arg = *event;
+
+    return 0;
+}
+
+TEST_CASE_SELF(ble_gap_test_case_set_cb_good)
+{
+    const uint8_t peer_addr[6] = { 1,2,3,4,5,6 };
+    struct ble_gap_event event;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    /* Reconfigure the callback. */
+    rc = ble_gap_set_event_cb(2, ble_gap_test_util_set_cb_event, &event);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Terminate the connection and ensure the new callback gets called. */
+    rc = ble_hs_test_util_conn_terminate(2, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_hci_rx_disconn_complete_event(2, 0, BLE_ERR_REM_USER_CONN_TERM);
+
+    TEST_ASSERT(event.type == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(event.disconnect.reason ==
+                BLE_HS_HCI_ERR(BLE_ERR_REM_USER_CONN_TERM));
+    TEST_ASSERT(event.disconnect.conn.conn_handle == 2);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gap_test_case_set_cb_bad)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Ensure error is reported when specified connection doesn't exist. */
+    rc = ble_gap_set_event_cb(123, ble_gap_test_util_set_cb_event, NULL);
+    TEST_ASSERT(rc == BLE_HS_ENOTCONN);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gap_test_suite_set_cb)
+{
+    ble_gap_test_case_set_cb_good();
+    ble_gap_test_case_set_cb_bad();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_conn_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_conn_test.c
new file mode 100644
index 00000000..8d95f743
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_conn_test.c
@@ -0,0 +1,746 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_GATT_BREAK_TEST_READ_ATTR_HANDLE        0x9383
+#define BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE       0x1234
+
+static uint8_t ble_gatt_conn_test_write_value[] = { 1, 3, 64, 21, 6 };
+
+struct ble_gatt_conn_test_arg {
+    uint16_t exp_conn_handle;
+    int exp_status;
+    int called;
+};
+
+static struct ble_gap_event ble_gatt_conn_test_gap_event;
+
+static void
+ble_gatt_conn_test_util_init(void)
+{
+    ble_hs_test_util_init();
+    memset(&ble_gatt_conn_test_gap_event, -1,
+           sizeof ble_gatt_conn_test_gap_event);
+}
+
+static int
+ble_gatt_conn_test_indicate_cb(struct ble_gap_event *event, void *arg)
+{
+    /* Only record indication failures. */
+    if (event->type == BLE_GAP_EVENT_NOTIFY_TX &&
+        event->notify_tx.status != 0) {
+
+        ble_gatt_conn_test_gap_event = *event;
+    }
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_attr_cb(uint16_t conn_handle, uint16_t attr_handle,
+                           uint8_t op, uint16_t offset, struct os_mbuf **om,
+                           void *arg)
+{
+    uint8_t *buf;
+
+    switch (op) {
+    case BLE_ATT_ACCESS_OP_READ:
+        buf = os_mbuf_extend(*om, 1);
+        TEST_ASSERT_FATAL(buf != NULL);
+        *buf = 1;
+        return 0;
+
+    default:
+        return -1;
+    }
+}
+
+static int
+ble_gatt_conn_test_mtu_cb(uint16_t conn_handle,
+                          const struct ble_gatt_error *error,
+                          uint16_t mtu, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(mtu == 0);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_disc_all_svcs_cb(uint16_t conn_handle,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_svc *service,
+                                    void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(service == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_disc_svc_uuid_cb(uint16_t conn_handle,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_svc *service,
+                                    void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(service == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_find_inc_svcs_cb(uint16_t conn_handle,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_svc *service,
+                                    void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(service == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_disc_all_chrs_cb(uint16_t conn_handle,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_chr *chr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(chr == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_disc_chr_uuid_cb(uint16_t conn_handle,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_chr *chr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(chr == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_disc_all_dscs_cb(uint16_t conn_handle,
+                                    const struct ble_gatt_error *error,
+                                    uint16_t chr_val_handle,
+                                    const struct ble_gatt_dsc *dsc,
+                                    void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(dsc == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_read_cb(uint16_t conn_handle,
+                           const struct ble_gatt_error *error,
+                           struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attr == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_read_uuid_cb(uint16_t conn_handle,
+                                const struct ble_gatt_error *error,
+                                struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attr == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_read_long_cb(uint16_t conn_handle,
+                                const struct ble_gatt_error *error,
+                                struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attr == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+static int
+ble_gatt_conn_test_read_mult_cb(uint16_t conn_handle,
+                                const struct ble_gatt_error *error,
+                                struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attr->om == NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_write_cb(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            struct ble_gatt_attr *attr,
+                            void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attr != NULL);
+    TEST_ASSERT(attr->handle == BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_write_long_cb(uint16_t conn_handle,
+                                 const struct ble_gatt_error *error,
+                                 struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attr != NULL);
+    TEST_ASSERT(attr->handle == BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+static int
+ble_gatt_conn_test_write_rel_cb(uint16_t conn_handle,
+                                const struct ble_gatt_error *error,
+                                struct ble_gatt_attr *attrs,
+                                uint8_t num_attrs,
+                                void *arg)
+{
+    struct ble_gatt_conn_test_arg *cb_arg;
+
+    cb_arg = arg;
+
+    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
+    TEST_ASSERT(!cb_arg->called);
+    TEST_ASSERT_FATAL(error != NULL);
+    TEST_ASSERT(error->status == cb_arg->exp_status);
+    TEST_ASSERT(attrs != NULL);
+
+    cb_arg->called++;
+
+    return 0;
+}
+
+TEST_CASE_SELF(ble_gatt_conn_test_disconnect)
+{
+    struct ble_gatt_conn_test_arg mtu_arg            = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg disc_all_svcs_arg  = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg disc_svc_uuid_arg  = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg find_inc_svcs_arg  = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg disc_all_chrs_arg  = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg disc_chr_uuid_arg  = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg disc_all_dscs_arg  = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg read_arg           = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg read_uuid_arg      = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg read_long_arg      = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg read_mult_arg      = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg write_arg          = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg write_long_arg     = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_conn_test_arg write_rel_arg      = { 0, BLE_HS_ENOTCONN };
+    struct ble_gatt_attr attr;
+    uint16_t attr_handle;
+    uint16_t offset = 0;
+    int rc;
+
+    ble_gatt_conn_test_util_init();
+
+    /*** Register an attribute to allow indicatations to be sent. */
+    rc = ble_att_svr_register(BLE_UUID16_DECLARE(0x1212), BLE_ATT_F_READ, 0,
+                              &attr_handle,
+                              ble_gatt_conn_test_attr_cb, NULL);
+    TEST_ASSERT(rc == 0);
+
+    /* Create three connections. */
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){1,2,3,4,5,6,7,8}),
+                                 ble_gatt_conn_test_indicate_cb, NULL);
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_gatt_conn_test_indicate_cb, NULL);
+    ble_hs_test_util_create_conn(3, ((uint8_t[]){3,4,5,6,7,8,9,10}),
+                                 ble_gatt_conn_test_indicate_cb, NULL);
+
+    /*** Schedule some GATT procedures. */
+    /* Connection 1. */
+    mtu_arg.exp_conn_handle = 1;
+    ble_gattc_exchange_mtu(1, ble_gatt_conn_test_mtu_cb, &mtu_arg);
+
+    disc_all_svcs_arg.exp_conn_handle = 1;
+    rc = ble_gattc_disc_all_svcs(1, ble_gatt_conn_test_disc_all_svcs_cb,
+                                 &disc_all_svcs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    disc_svc_uuid_arg.exp_conn_handle = 1;
+    rc = ble_gattc_disc_svc_by_uuid(1, BLE_UUID16_DECLARE(0x1111),
+                                    ble_gatt_conn_test_disc_svc_uuid_cb,
+                                    &disc_svc_uuid_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    find_inc_svcs_arg.exp_conn_handle = 1;
+    rc = ble_gattc_find_inc_svcs(1, 1, 0xffff,
+                                 ble_gatt_conn_test_find_inc_svcs_cb,
+                                 &find_inc_svcs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    disc_all_chrs_arg.exp_conn_handle = 1;
+    rc = ble_gattc_disc_all_chrs(1, 1, 0xffff,
+                                 ble_gatt_conn_test_disc_all_chrs_cb,
+                                 &disc_all_chrs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Connection 2. */
+    disc_all_dscs_arg.exp_conn_handle = 2;
+    rc = ble_gattc_disc_all_dscs(2, 3, 0xffff,
+                                 ble_gatt_conn_test_disc_all_dscs_cb,
+                                 &disc_all_dscs_arg);
+
+    disc_chr_uuid_arg.exp_conn_handle = 2;
+    rc = ble_gattc_disc_chrs_by_uuid(2, 2, 0xffff, BLE_UUID16_DECLARE(0x2222),
+                                     ble_gatt_conn_test_disc_chr_uuid_cb,
+                                     &disc_chr_uuid_arg);
+
+    read_arg.exp_conn_handle = 2;
+    rc = ble_gattc_read(2, BLE_GATT_BREAK_TEST_READ_ATTR_HANDLE,
+                        ble_gatt_conn_test_read_cb, &read_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    read_uuid_arg.exp_conn_handle = 2;
+    rc = ble_gattc_read_by_uuid(2, 1, 0xffff, BLE_UUID16_DECLARE(0x3333),
+                                ble_gatt_conn_test_read_uuid_cb,
+                                &read_uuid_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    read_long_arg.exp_conn_handle = 2;
+    rc = ble_gattc_read_long(2, BLE_GATT_BREAK_TEST_READ_ATTR_HANDLE, offset,
+                             ble_gatt_conn_test_read_long_cb, &read_long_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Connection 3. */
+    read_mult_arg.exp_conn_handle = 3;
+    rc = ble_gattc_read_mult(3, ((uint16_t[3]){5,6,7}), 3,
+                             ble_gatt_conn_test_read_mult_cb, &read_mult_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    write_arg.exp_conn_handle = 3;
+    rc = ble_hs_test_util_gatt_write_flat(
+        3, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
+        ble_gatt_conn_test_write_value, sizeof ble_gatt_conn_test_write_value,
+        ble_gatt_conn_test_write_cb, &write_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    write_long_arg.exp_conn_handle = 3;
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        3, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
+        ble_gatt_conn_test_write_value, sizeof ble_gatt_conn_test_write_value,
+        ble_gatt_conn_test_write_long_cb, &write_long_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    attr.handle = 8;
+    attr.offset = 0;
+    attr.om = os_msys_get_pkthdr(0, 0);
+    write_rel_arg.exp_conn_handle = 3;
+    rc = ble_gattc_write_reliable(
+        3, &attr, 1, ble_gatt_conn_test_write_rel_cb, &write_rel_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    rc = ble_gattc_indicate(3, attr_handle);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /*** Start the procedures. */
+
+    /*** Break the connections; verify proper callbacks got called. */
+    /* Connection 1. */
+    ble_gattc_connection_broken(1);
+    TEST_ASSERT(mtu_arg.called == 1);
+    TEST_ASSERT(disc_all_svcs_arg.called == 1);
+    TEST_ASSERT(disc_svc_uuid_arg.called == 1);
+    TEST_ASSERT(find_inc_svcs_arg.called == 1);
+    TEST_ASSERT(disc_all_chrs_arg.called == 1);
+    TEST_ASSERT(disc_chr_uuid_arg.called == 0);
+    TEST_ASSERT(disc_all_dscs_arg.called == 0);
+    TEST_ASSERT(read_arg.called == 0);
+    TEST_ASSERT(read_uuid_arg.called == 0);
+    TEST_ASSERT(read_long_arg.called == 0);
+    TEST_ASSERT(read_mult_arg.called == 0);
+    TEST_ASSERT(write_arg.called == 0);
+    TEST_ASSERT(write_long_arg.called == 0);
+    TEST_ASSERT(write_rel_arg.called == 0);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.type == 255);
+
+    /* Connection 2. */
+    ble_gattc_connection_broken(2);
+    TEST_ASSERT(mtu_arg.called == 1);
+    TEST_ASSERT(disc_all_svcs_arg.called == 1);
+    TEST_ASSERT(disc_svc_uuid_arg.called == 1);
+    TEST_ASSERT(find_inc_svcs_arg.called == 1);
+    TEST_ASSERT(disc_all_chrs_arg.called == 1);
+    TEST_ASSERT(disc_chr_uuid_arg.called == 1);
+    TEST_ASSERT(disc_all_dscs_arg.called == 1);
+    TEST_ASSERT(read_arg.called == 1);
+    TEST_ASSERT(read_uuid_arg.called == 1);
+    TEST_ASSERT(read_long_arg.called == 1);
+    TEST_ASSERT(read_mult_arg.called == 0);
+    TEST_ASSERT(write_arg.called == 0);
+    TEST_ASSERT(write_long_arg.called == 0);
+    TEST_ASSERT(write_rel_arg.called == 0);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.type == 255);
+
+    /* Connection 3. */
+    ble_gattc_connection_broken(3);
+    TEST_ASSERT(mtu_arg.called == 1);
+    TEST_ASSERT(disc_all_svcs_arg.called == 1);
+    TEST_ASSERT(disc_svc_uuid_arg.called == 1);
+    TEST_ASSERT(find_inc_svcs_arg.called == 1);
+    TEST_ASSERT(disc_all_chrs_arg.called == 1);
+    TEST_ASSERT(disc_chr_uuid_arg.called == 1);
+    TEST_ASSERT(disc_all_dscs_arg.called == 1);
+    TEST_ASSERT(read_arg.called == 1);
+    TEST_ASSERT(read_uuid_arg.called == 1);
+    TEST_ASSERT(read_long_arg.called == 1);
+    TEST_ASSERT(read_mult_arg.called == 1);
+    TEST_ASSERT(write_arg.called == 1);
+    TEST_ASSERT(write_long_arg.called == 1);
+    TEST_ASSERT(write_rel_arg.called == 1);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.type == BLE_GAP_EVENT_NOTIFY_TX);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.notify_tx.status ==
+                BLE_HS_ENOTCONN);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.notify_tx.conn_handle == 3);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.notify_tx.attr_handle ==
+                attr_handle);
+    TEST_ASSERT(ble_gatt_conn_test_gap_event.notify_tx.indication);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+ble_gatt_conn_test_util_timeout(uint16_t conn_handle,
+                                struct ble_gatt_conn_test_arg *arg)
+{
+    int32_t ticks_from_now;
+
+    ticks_from_now = ble_gattc_timer();
+    TEST_ASSERT(ticks_from_now == 30 * OS_TICKS_PER_SEC);
+
+    os_time_advance(29 * OS_TICKS_PER_SEC);
+    ticks_from_now = ble_gattc_timer();
+    TEST_ASSERT(ticks_from_now == 1 * OS_TICKS_PER_SEC);
+
+    ble_hs_test_util_hci_ack_set_disconnect(0);
+    os_time_advance(1 * OS_TICKS_PER_SEC);
+    ticks_from_now = ble_gattc_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Ensure connection was terminated due to proecedure timeout. */
+    ble_hs_test_util_hci_rx_disconn_complete_event(conn_handle, 0,
+                                                   BLE_ERR_REM_USER_CONN_TERM);
+
+    /* Ensure GATT callback was called with timeout status. */
+    if (arg != NULL) {
+        TEST_ASSERT(arg->called == 1);
+    }
+}
+
+TEST_CASE_SELF(ble_gatt_conn_test_timeout)
+{
+    static const uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+    struct ble_gatt_conn_test_arg mtu_arg            = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg disc_all_svcs_arg  = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg disc_svc_uuid_arg  = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg find_inc_svcs_arg  = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg disc_all_chrs_arg  = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg disc_chr_uuid_arg  = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg disc_all_dscs_arg  = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg read_arg           = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg read_uuid_arg      = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg read_long_arg      = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg read_mult_arg      = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg write_arg          = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg write_long_arg     = { 1, BLE_HS_ETIMEOUT };
+    struct ble_gatt_conn_test_arg write_rel_arg      = { 1, BLE_HS_ETIMEOUT };
+
+    struct ble_gatt_attr attr;
+    int32_t ticks_from_now;
+    uint16_t attr_handle;
+    uint16_t offset = 0;
+    int rc;
+
+    ble_gatt_conn_test_util_init();
+
+    ticks_from_now = ble_gattc_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /*** Register an attribute to allow indicatations to be sent. */
+    rc = ble_att_svr_register(BLE_UUID16_DECLARE(0x1212), BLE_ATT_F_READ, 0,
+                              &attr_handle,
+                              ble_gatt_conn_test_attr_cb, NULL);
+    TEST_ASSERT(rc == 0);
+
+    /*** MTU. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_exchange_mtu(1, ble_gatt_conn_test_mtu_cb, &mtu_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &mtu_arg);
+
+    /*** Discover all services. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_disc_all_svcs(1, ble_gatt_conn_test_disc_all_svcs_cb,
+                                 &disc_all_svcs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &disc_all_svcs_arg);
+
+    /*** Discover services by UUID. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_disc_svc_by_uuid(1, BLE_UUID16_DECLARE(0x1111),
+                                    ble_gatt_conn_test_disc_svc_uuid_cb,
+                                    &disc_svc_uuid_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &disc_svc_uuid_arg);
+
+    /*** Find included services. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_find_inc_svcs(1, 1, 0xffff,
+                                 ble_gatt_conn_test_find_inc_svcs_cb,
+                                 &find_inc_svcs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &find_inc_svcs_arg);
+
+    /*** Discover all characteristics. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_disc_all_chrs(1, 1, 0xffff,
+                                 ble_gatt_conn_test_disc_all_chrs_cb,
+                                 &disc_all_chrs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &disc_all_chrs_arg);
+
+    /*** Discover all descriptors. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_disc_all_dscs(1, 3, 0xffff,
+                                 ble_gatt_conn_test_disc_all_dscs_cb,
+                                 &disc_chr_uuid_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &disc_chr_uuid_arg);
+
+    /*** Discover characteristics by UUID. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_disc_chrs_by_uuid(1, 2, 0xffff, BLE_UUID16_DECLARE(0x2222),
+                                     ble_gatt_conn_test_disc_chr_uuid_cb,
+                                     &disc_all_dscs_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &disc_all_dscs_arg);
+
+    /*** Read. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_read(1, BLE_GATT_BREAK_TEST_READ_ATTR_HANDLE,
+                        ble_gatt_conn_test_read_cb, &read_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &read_arg);
+
+    /*** Read by UUID. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_read_by_uuid(1, 1, 0xffff, BLE_UUID16_DECLARE(0x3333),
+                                ble_gatt_conn_test_read_uuid_cb,
+                                &read_uuid_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &read_uuid_arg);
+
+    /*** Read long. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_read_long(1, BLE_GATT_BREAK_TEST_READ_ATTR_HANDLE, offset,
+                             ble_gatt_conn_test_read_long_cb,
+                             &read_long_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &read_long_arg);
+
+    /*** Read multiple. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_read_mult(1, ((uint16_t[3]){5,6,7}), 3,
+                             ble_gatt_conn_test_read_mult_cb,
+                             &read_mult_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &read_mult_arg);
+
+    /*** Write. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_hs_test_util_gatt_write_flat(
+        1, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
+        ble_gatt_conn_test_write_value, sizeof ble_gatt_conn_test_write_value,
+        ble_gatt_conn_test_write_cb, &write_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &write_arg);
+
+    /*** Write long. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        1, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
+        ble_gatt_conn_test_write_value, sizeof ble_gatt_conn_test_write_value,
+        ble_gatt_conn_test_write_long_cb, &write_long_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &write_long_arg);
+
+    /*** Write reliable. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    attr.handle = 8;
+    attr.offset = 0;
+    attr.om = os_msys_get_pkthdr(0, 0);
+    rc = ble_gattc_write_reliable(
+        1, &attr, 1, ble_gatt_conn_test_write_rel_cb, &write_rel_arg);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, &write_rel_arg);
+
+    /*** Indication. */
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+    rc = ble_gattc_indicate(1, attr_handle);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_gatt_conn_test_util_timeout(1, NULL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_conn_suite)
+{
+    ble_gatt_conn_test_disconnect();
+    ble_gatt_conn_test_timeout();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_c_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_c_test.c
new file mode 100644
index 00000000..e19db341
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_c_test.c
@@ -0,0 +1,722 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include <limits.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "host/ble_gatt.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+struct ble_gatt_disc_c_test_char {
+    uint16_t def_handle;
+    uint16_t val_handle;
+    uint8_t properties;
+    const ble_uuid_t *uuid;
+};
+
+#define BLE_GATT_DISC_C_TEST_MAX_CHARS  256
+static struct ble_gatt_chr
+    ble_gatt_disc_c_test_chars[BLE_GATT_DISC_C_TEST_MAX_CHARS];
+static int ble_gatt_disc_c_test_num_chars;
+static int ble_gatt_disc_c_test_rx_complete;
+
+static void
+ble_gatt_disc_c_test_init(void)
+{
+    ble_hs_test_util_init();
+
+    ble_gatt_disc_c_test_num_chars = 0;
+    ble_gatt_disc_c_test_rx_complete = 0;
+}
+
+static int
+ble_gatt_disc_c_test_misc_rx_rsp_once(
+    uint16_t conn_handle, struct ble_gatt_disc_c_test_char *chars)
+{
+    struct ble_att_read_type_rsp rsp;
+    uint8_t buf[1024];
+    int off;
+    int rc;
+    int i;
+
+    /* Send the pending ATT Read By Type Request. */
+
+    if (chars[0].uuid->type == BLE_UUID_TYPE_16) {
+       rsp.batp_length = BLE_ATT_READ_TYPE_ADATA_BASE_SZ +
+                         BLE_GATT_CHR_DECL_SZ_16;
+    } else {
+       rsp.batp_length = BLE_ATT_READ_TYPE_ADATA_BASE_SZ +
+                         BLE_GATT_CHR_DECL_SZ_128;
+    }
+
+    ble_att_read_type_rsp_write(buf, BLE_ATT_READ_TYPE_RSP_BASE_SZ, &rsp);
+
+    off = BLE_ATT_READ_TYPE_RSP_BASE_SZ;
+    for (i = 0; ; i++) {
+        if (chars[i].def_handle == 0) {
+            /* No more services. */
+            break;
+        }
+
+        /* If the value length is changing, we need a separate response. */
+        if (((chars[i].uuid->type == BLE_UUID_TYPE_16) ^
+            (chars[0].uuid->type == BLE_UUID_TYPE_16)) != 0) {
+            break;
+        }
+
+        if (chars[i].uuid->type == BLE_UUID_TYPE_16) {
+            if (off + BLE_ATT_READ_TYPE_ADATA_SZ_16 >
+                ble_att_mtu(conn_handle)) {
+
+                /* Can't fit any more entries. */
+                break;
+            }
+        } else {
+            if (off + BLE_ATT_READ_TYPE_ADATA_SZ_128 >
+                ble_att_mtu(conn_handle)) {
+
+                /* Can't fit any more entries. */
+                break;
+            }
+        }
+
+        put_le16(buf + off, chars[i].def_handle);
+        off += 2;
+
+        buf[off] = chars[i].properties;
+        off++;
+
+        put_le16(buf + off, chars[i].val_handle);
+        off += 2;
+
+        ble_uuid_flat(chars[i].uuid, buf + off);
+        off += ble_uuid_length(chars[i].uuid);
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    return i;
+}
+
+static void
+ble_gatt_disc_c_test_misc_rx_rsp(uint16_t conn_handle,
+                                 uint16_t end_handle,
+                                 struct ble_gatt_disc_c_test_char *chars)
+{
+    int count;
+    int idx;
+
+    idx = 0;
+    while (chars[idx].def_handle != 0) {
+        count = ble_gatt_disc_c_test_misc_rx_rsp_once(conn_handle,
+                                                      chars + idx);
+        if (count == 0) {
+            break;
+        }
+        idx += count;
+    }
+
+    if (chars[idx - 1].def_handle != end_handle) {
+        /* Send the pending ATT Request. */
+        ble_hs_test_util_rx_att_err_rsp(conn_handle, BLE_ATT_OP_READ_TYPE_REQ,
+                                        BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                        chars[idx - 1].def_handle);
+    }
+}
+
+static void
+ble_gatt_disc_c_test_misc_verify_chars(struct ble_gatt_disc_c_test_char *chars,
+                                       int stop_after)
+{
+    int i;
+
+    if (stop_after == 0) {
+        stop_after = BLE_GATT_DISC_C_TEST_MAX_CHARS;
+    }
+
+    for (i = 0; i < stop_after && chars[i].def_handle != 0; i++) {
+        TEST_ASSERT(chars[i].def_handle ==
+                    ble_gatt_disc_c_test_chars[i].def_handle);
+        TEST_ASSERT(chars[i].val_handle ==
+                    ble_gatt_disc_c_test_chars[i].val_handle);
+        TEST_ASSERT(ble_uuid_cmp(chars[i].uuid,
+                                 &ble_gatt_disc_c_test_chars[i].uuid.u) == 0);
+    }
+
+    TEST_ASSERT(i == ble_gatt_disc_c_test_num_chars);
+    TEST_ASSERT(ble_gatt_disc_c_test_rx_complete);
+}
+
+static int
+ble_gatt_disc_c_test_misc_cb(uint16_t conn_handle,
+                             const struct ble_gatt_error *error,
+                             const struct ble_gatt_chr *chr, void *arg)
+{
+    struct ble_gatt_chr *dst;
+    int *stop_after;
+
+    TEST_ASSERT(error != NULL);
+    TEST_ASSERT(!ble_gatt_disc_c_test_rx_complete);
+
+    stop_after = arg;
+
+    switch (error->status) {
+    case 0:
+        TEST_ASSERT_FATAL(ble_gatt_disc_c_test_num_chars <
+                          BLE_GATT_DISC_C_TEST_MAX_CHARS);
+
+        dst = ble_gatt_disc_c_test_chars + ble_gatt_disc_c_test_num_chars++;
+        *dst = *chr;
+        break;
+
+    case BLE_HS_EDONE:
+        ble_gatt_disc_c_test_rx_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+
+    if (*stop_after > 0) {
+        (*stop_after)--;
+        if (*stop_after == 0) {
+            ble_gatt_disc_c_test_rx_complete = 1;
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+static void
+ble_gatt_disc_c_test_misc_all(uint16_t start_handle, uint16_t end_handle,
+                              int stop_after,
+                              struct ble_gatt_disc_c_test_char *chars)
+{
+    int num_left;
+    int rc;
+
+    ble_gatt_disc_c_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    num_left = stop_after;
+    rc = ble_gattc_disc_all_chrs(2, start_handle, end_handle,
+                                 ble_gatt_disc_c_test_misc_cb, &num_left);
+    TEST_ASSERT(rc == 0);
+
+    ble_gatt_disc_c_test_misc_rx_rsp(2, end_handle, chars);
+    ble_gatt_disc_c_test_misc_verify_chars(chars, stop_after);
+}
+
+static void
+ble_gatt_disc_c_test_misc_uuid(uint16_t start_handle, uint16_t end_handle,
+                               int stop_after, const ble_uuid_t *uuid,
+                               struct ble_gatt_disc_c_test_char *rsp_chars,
+                               struct ble_gatt_disc_c_test_char *ret_chars)
+{
+    int rc;
+
+    ble_gatt_disc_c_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_gattc_disc_chrs_by_uuid(2, start_handle, end_handle,
+                                     uuid,
+                                     ble_gatt_disc_c_test_misc_cb,
+                                     &stop_after);
+    TEST_ASSERT(rc == 0);
+
+    ble_gatt_disc_c_test_misc_rx_rsp(2, end_handle, rsp_chars);
+    ble_gatt_disc_c_test_misc_verify_chars(ret_chars, 0);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_c_test_disc_all)
+{
+    /*** One 16-bit characteristic. */
+    ble_gatt_disc_c_test_misc_all(50, 100, 0,
+                                  (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, { 0 }
+    });
+
+    /*** Two 16-bit characteristics. */
+    ble_gatt_disc_c_test_misc_all(50, 100, 0,
+                                  (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 57,
+            .val_handle = 58,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, { 0 }
+    });
+
+    /*** Five 16-bit characteristics. */
+    ble_gatt_disc_c_test_misc_all(50, 100, 0,
+                                  (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 57,
+            .val_handle = 58,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, {
+            .def_handle = 59,
+            .val_handle = 60,
+            .uuid = BLE_UUID16_DECLARE(0x5372),
+        }, {
+            .def_handle = 61,
+            .val_handle = 62,
+            .uuid = BLE_UUID16_DECLARE(0xab93),
+        }, {
+            .def_handle = 63,
+            .val_handle = 64,
+            .uuid = BLE_UUID16_DECLARE(0x0023),
+        }, { 0 }
+    });
+
+    /*** Interleaved 16-bit and 128-bit characteristics. */
+    ble_gatt_disc_c_test_misc_all(50, 100, 0,
+                                  (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 83,
+            .val_handle = 84,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 87,
+            .val_handle = 88,
+            .uuid = BLE_UUID128_DECLARE(0, 1, 2, 3, 4, 5, 6, 7,
+                                        8, 9, 10, 11, 12, 13, 14, 15),
+        }, {
+            .def_handle = 91,
+            .val_handle = 92,
+            .uuid = BLE_UUID16_DECLARE(0x0003),
+        }, {
+            .def_handle = 93,
+            .val_handle = 94,
+            .uuid = BLE_UUID128_DECLARE(1, 0, 4, 0, 6, 9, 17, 7,
+                                        8, 43, 7, 4, 12, 43, 19, 35),
+        }, {
+            .def_handle = 98,
+            .val_handle = 99,
+            .uuid = BLE_UUID16_DECLARE(0xabfa),
+        }, { 0 }
+    });
+
+    /*** Ends with final handle ID. */
+    ble_gatt_disc_c_test_misc_all(50, 100, 0,
+                                  (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 99,
+            .val_handle = 100,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, { 0 }
+    });
+
+    /*** Stop after two characteristics. */
+    ble_gatt_disc_c_test_misc_all(50, 100, 2,
+                                  (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 57,
+            .val_handle = 58,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, {
+            .def_handle = 59,
+            .val_handle = 60,
+            .uuid = BLE_UUID16_DECLARE(0x5372),
+        }, {
+            .def_handle = 61,
+            .val_handle = 62,
+            .uuid = BLE_UUID16_DECLARE(0xab93),
+        }, {
+            .def_handle = 63,
+            .val_handle = 64,
+            .uuid = BLE_UUID16_DECLARE(0x0023),
+        }, { 0 }
+    });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_c_test_disc_uuid)
+{
+    /*** One 16-bit characteristic. */
+    ble_gatt_disc_c_test_misc_uuid(50, 100, 0, BLE_UUID16_DECLARE(0x2010),
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, { 0 } },
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, { 0 } }
+    );
+
+    /*** No matching characteristics. */
+    ble_gatt_disc_c_test_misc_uuid(50, 100, 0, BLE_UUID16_DECLARE(0x2010),
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x1234),
+        }, { 0 } },
+        (struct ble_gatt_disc_c_test_char[]) {
+        { 0 } }
+    );
+
+    /*** 2/5 16-bit characteristics. */
+    ble_gatt_disc_c_test_misc_uuid(50, 100, 0, BLE_UUID16_DECLARE(0x2010),
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 57,
+            .val_handle = 58,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, {
+            .def_handle = 59,
+            .val_handle = 60,
+            .uuid = BLE_UUID16_DECLARE(0x5372),
+        }, {
+            .def_handle = 61,
+            .val_handle = 62,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 63,
+            .val_handle = 64,
+            .uuid = BLE_UUID16_DECLARE(0x0023),
+        }, { 0 } },
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 61,
+            .val_handle = 62,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, { 0 } }
+    );
+
+    /*** Interleaved 16-bit and 128-bit characteristics. */
+    ble_gatt_disc_c_test_misc_uuid(
+        50, 100, 0,
+        BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 83,
+            .val_handle = 84,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 87,
+            .val_handle = 88,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .def_handle = 91,
+            .val_handle = 92,
+            .uuid = BLE_UUID16_DECLARE(0x0003),
+        }, {
+            .def_handle = 93,
+            .val_handle = 94,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .def_handle = 98,
+            .val_handle = 99,
+            .uuid = BLE_UUID16_DECLARE(0xabfa),
+        }, { 0 } },
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 87,
+            .val_handle = 88,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .def_handle = 93,
+            .val_handle = 94,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, { 0 } }
+    );
+
+    /*** Ends with final handle ID. */
+    ble_gatt_disc_c_test_misc_uuid(50, 100, 0, BLE_UUID16_DECLARE(0x64ba),
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 99,
+            .val_handle = 100,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, { 0 } },
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 99,
+            .val_handle = 100,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, { 0 } }
+    );
+
+    /*** Stop after first characteristic. */
+    ble_gatt_disc_c_test_misc_uuid(50, 100, 1, BLE_UUID16_DECLARE(0x2010),
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 57,
+            .val_handle = 58,
+            .uuid = BLE_UUID16_DECLARE(0x64ba),
+        }, {
+            .def_handle = 59,
+            .val_handle = 60,
+            .uuid = BLE_UUID16_DECLARE(0x5372),
+        }, {
+            .def_handle = 61,
+            .val_handle = 62,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, {
+            .def_handle = 63,
+            .val_handle = 64,
+            .uuid = BLE_UUID16_DECLARE(0x0023),
+        }, { 0 } },
+        (struct ble_gatt_disc_c_test_char[]) {
+        {
+            .def_handle = 55,
+            .val_handle = 56,
+            .uuid = BLE_UUID16_DECLARE(0x2010),
+        }, { 0 } }
+    );
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_c_test_oom_all)
+{
+    /* Retrieve enough characteristics to require two transactions. */
+    struct ble_gatt_disc_c_test_char chrs[] = {
+        {
+            .def_handle = 93,
+            .val_handle = 94,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        },
+        {
+            .def_handle = 95,
+            .val_handle = 96,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        },
+        { 0 }
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int stop_after;
+    int num_chrs;
+    int rc;
+
+    ble_gatt_disc_c_test_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover all characteristics procedure. */
+    stop_after = 0;
+    rc = ble_gattc_disc_all_chrs(1, 1, 0xffff,
+                                 ble_gatt_disc_c_test_misc_cb, &stop_after);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    num_chrs = ble_gatt_disc_c_test_misc_rx_rsp_once(1, chrs);
+
+    /* Make sure there are still undiscovered characteristics. */
+    TEST_ASSERT_FATAL(num_chrs < sizeof chrs / sizeof chrs[0] - 1);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_disc_c_test_misc_rx_rsp_once(1, chrs + num_chrs);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify that procedure completes when mbufs are available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    ble_hs_test_util_rx_att_err_rsp(1,
+                                    BLE_ATT_OP_READ_TYPE_REQ,
+                                    BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                    1);
+    ble_gatt_disc_c_test_misc_verify_chars(chrs, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_c_test_oom_uuid)
+{
+    /* Retrieve enough characteristics to require two transactions. */
+    struct ble_gatt_disc_c_test_char chrs[] = {
+        {
+            .def_handle = 93,
+            .val_handle = 94,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        },
+        {
+            .def_handle = 95,
+            .val_handle = 96,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        },
+        { 0 }
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int stop_after;
+    int num_chrs;
+    int rc;
+
+    ble_gatt_disc_c_test_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover characteristics by UUID procedure. */
+    stop_after = 0;
+    rc = ble_gattc_disc_chrs_by_uuid(1, 1, 0xffff, chrs[0].uuid,
+                                     ble_gatt_disc_c_test_misc_cb,
+                                     &stop_after);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    num_chrs = ble_gatt_disc_c_test_misc_rx_rsp_once(1, chrs);
+
+    /* Make sure there are still undiscovered characteristics. */
+    TEST_ASSERT_FATAL(num_chrs < sizeof chrs / sizeof chrs[0] - 1);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_disc_c_test_misc_rx_rsp_once(1, chrs + num_chrs);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify that procedure completes when mbufs are available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    ble_hs_test_util_rx_att_err_rsp(1,
+                                    BLE_ATT_OP_READ_TYPE_REQ,
+                                    BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                    1);
+    ble_gatt_disc_c_test_misc_verify_chars(chrs, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_disc_c_test_suite)
+{
+    ble_gatt_disc_c_test_disc_all();
+    ble_gatt_disc_c_test_disc_uuid();
+    ble_gatt_disc_c_test_oom_all();
+    ble_gatt_disc_c_test_oom_uuid();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_d_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_d_test.c
new file mode 100644
index 00000000..e405c86f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_d_test.c
@@ -0,0 +1,446 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include <limits.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "host/ble_gatt.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+struct ble_gatt_disc_d_test_dsc {
+    uint16_t chr_val_handle; /* 0 if last entry. */
+    uint16_t dsc_handle;
+    ble_uuid_any_t dsc_uuid;
+};
+
+#define BLE_GATT_DISC_D_TEST_MAX_DSCS  256
+static struct ble_gatt_disc_d_test_dsc
+    ble_gatt_disc_d_test_dscs[BLE_GATT_DISC_D_TEST_MAX_DSCS];
+static int ble_gatt_disc_d_test_num_dscs;
+static int ble_gatt_disc_d_test_rx_complete;
+
+static void
+ble_gatt_disc_d_test_init(void)
+{
+    ble_hs_test_util_init();
+
+    ble_gatt_disc_d_test_num_dscs = 0;
+    ble_gatt_disc_d_test_rx_complete = 0;
+}
+
+static int
+ble_gatt_disc_d_test_misc_rx_rsp_once(
+    uint16_t conn_handle, struct ble_gatt_disc_d_test_dsc *dscs)
+{
+    struct ble_att_find_info_rsp rsp;
+    uint8_t buf[1024];
+    int off;
+    int rc;
+    int i;
+
+    /* Send the pending ATT Read By Type Request. */
+
+    if (dscs[0].dsc_uuid.u.type == BLE_UUID_TYPE_16) {
+        rsp.bafp_format = BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT;
+    } else {
+        rsp.bafp_format = BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT;
+    }
+
+    ble_att_find_info_rsp_write(buf, BLE_ATT_FIND_INFO_RSP_BASE_SZ, &rsp);
+
+    off = BLE_ATT_FIND_INFO_RSP_BASE_SZ;
+    for (i = 0; ; i++) {
+        if (dscs[i].chr_val_handle == 0) {
+            /* No more descriptors. */
+            break;
+        }
+
+        if (dscs[i].dsc_uuid.u.type == BLE_UUID_TYPE_16) {
+            if (off + BLE_ATT_FIND_INFO_IDATA_16_SZ >
+                ble_att_mtu(conn_handle)) {
+
+                /* Can't fit any more entries. */
+                break;
+            }
+        } else {
+            if (off + BLE_ATT_FIND_INFO_IDATA_128_SZ >
+                ble_att_mtu(conn_handle)) {
+
+                /* Can't fit any more entries. */
+                break;
+            }
+        }
+
+        /* If the value length is changing, we need a separate response. */
+        if (((dscs[0].dsc_uuid.u.type == BLE_UUID_TYPE_16) ^
+            (dscs[i].dsc_uuid.u.type == BLE_UUID_TYPE_16)) != 0) {
+            break;
+        }
+
+        put_le16(buf + off, dscs[i].dsc_handle);
+        off += 2;
+
+        ble_uuid_flat(&dscs[i].dsc_uuid.u, buf + off);
+        off += ble_uuid_length(&dscs[i].dsc_uuid.u);
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    return i;
+}
+
+static void
+ble_gatt_disc_d_test_misc_rx_rsp(uint16_t conn_handle,
+                                 uint16_t end_handle,
+                                 struct ble_gatt_disc_d_test_dsc *dscs)
+{
+    int count;
+    int idx;
+
+    idx = 0;
+    while (dscs[idx].chr_val_handle != 0) {
+        count = ble_gatt_disc_d_test_misc_rx_rsp_once(conn_handle, dscs + idx);
+        if (count == 0) {
+            break;
+        }
+        idx += count;
+    }
+
+    if (dscs[idx - 1].dsc_handle != end_handle) {
+        /* Send the pending ATT Request. */
+        ble_hs_test_util_rx_att_err_rsp(conn_handle, BLE_ATT_OP_FIND_INFO_REQ,
+                                        BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                        end_handle);
+    }
+}
+
+static void
+ble_gatt_disc_d_test_misc_verify_dscs(struct ble_gatt_disc_d_test_dsc *dscs,
+                                      int stop_after)
+{
+    int i;
+
+    if (stop_after == 0) {
+        stop_after = BLE_GATT_DISC_D_TEST_MAX_DSCS;
+    }
+
+    for (i = 0; i < stop_after && dscs[i].chr_val_handle != 0; i++) {
+        TEST_ASSERT(dscs[i].chr_val_handle ==
+                    ble_gatt_disc_d_test_dscs[i].chr_val_handle);
+        TEST_ASSERT(dscs[i].dsc_handle ==
+                    ble_gatt_disc_d_test_dscs[i].dsc_handle);
+        TEST_ASSERT(ble_uuid_cmp(&dscs[i].dsc_uuid.u,
+                                 &ble_gatt_disc_d_test_dscs[i].dsc_uuid.u) == 0);
+    }
+
+    TEST_ASSERT(i == ble_gatt_disc_d_test_num_dscs);
+    TEST_ASSERT(ble_gatt_disc_d_test_rx_complete);
+}
+
+static int
+ble_gatt_disc_d_test_misc_cb(uint16_t conn_handle,
+                             const struct ble_gatt_error *error,
+                             uint16_t chr_val_handle,
+                             const struct ble_gatt_dsc *dsc,
+                             void *arg)
+{
+    struct ble_gatt_disc_d_test_dsc *dst;
+    int *stop_after;
+
+    TEST_ASSERT(error != NULL);
+    TEST_ASSERT(!ble_gatt_disc_d_test_rx_complete);
+
+    stop_after = arg;
+
+    switch (error->status) {
+    case 0:
+        TEST_ASSERT_FATAL(ble_gatt_disc_d_test_num_dscs <
+                          BLE_GATT_DISC_D_TEST_MAX_DSCS);
+
+        dst = ble_gatt_disc_d_test_dscs + ble_gatt_disc_d_test_num_dscs++;
+        dst->chr_val_handle = chr_val_handle;
+        dst->dsc_handle = dsc->handle;
+        dst->dsc_uuid = dsc->uuid;
+        break;
+
+    case BLE_HS_EDONE:
+        ble_gatt_disc_d_test_rx_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+
+    if (*stop_after > 0) {
+        (*stop_after)--;
+        if (*stop_after == 0) {
+            ble_gatt_disc_d_test_rx_complete = 1;
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+static void
+ble_gatt_disc_d_test_misc_all(uint16_t chr_val_handle, uint16_t end_handle,
+                              int stop_after,
+                              struct ble_gatt_disc_d_test_dsc *dscs)
+{
+    int num_left;
+    int rc;
+
+    ble_gatt_disc_d_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    num_left = stop_after;
+    rc = ble_gattc_disc_all_dscs(2, chr_val_handle, end_handle,
+                                 ble_gatt_disc_d_test_misc_cb, &num_left);
+    TEST_ASSERT(rc == 0);
+
+    ble_gatt_disc_d_test_misc_rx_rsp(2, end_handle, dscs);
+    ble_gatt_disc_d_test_misc_verify_dscs(dscs, stop_after);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_d_test_1)
+{
+    /*** One 16-bit descriptor. */
+    ble_gatt_disc_d_test_misc_all(5, 10, 0,
+        ((struct ble_gatt_disc_d_test_dsc[]) { {
+            .chr_val_handle = 5,
+            .dsc_handle = 6,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x1234),
+        }, {
+            0
+        } })
+    );
+
+    /*** Two 16-bit descriptors. */
+    ble_gatt_disc_d_test_misc_all(50, 100, 0,
+        ((struct ble_gatt_disc_d_test_dsc[]) { {
+            .chr_val_handle = 50,
+            .dsc_handle = 51,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x1111),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 52,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x2222),
+        }, {
+            0
+        } })
+    );
+
+    /*** Five 16-bit descriptors. */
+    ble_gatt_disc_d_test_misc_all(50, 100, 0,
+        ((struct ble_gatt_disc_d_test_dsc[]) { {
+            .chr_val_handle = 50,
+            .dsc_handle = 51,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x1111),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 52,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x2222),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 53,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x3333),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 54,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x4444),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 55,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x5555),
+        }, {
+            0
+        } })
+    );
+
+    /*** Interleaved 16-bit and 128-bit descriptors. */
+    ble_gatt_disc_d_test_misc_all(50, 100, 0,
+        ((struct ble_gatt_disc_d_test_dsc[]) { {
+            .chr_val_handle = 50,
+            .dsc_handle = 51,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x1111),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 52,
+            .dsc_uuid.u128 = BLE_UUID128_INIT( 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 53,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x3333),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 54,
+            .dsc_uuid.u128 = BLE_UUID128_INIT(1,0,4,0,6,9,17,7,8,43,7,4,12,43,19,35),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 55,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x5555),
+        }, {
+            0
+        } })
+    );
+
+    /*** Ends with final handle ID. */
+    ble_gatt_disc_d_test_misc_all(50, 52, 0,
+        ((struct ble_gatt_disc_d_test_dsc[]) { {
+            .chr_val_handle = 50,
+            .dsc_handle = 51,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x1111),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 52,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x2222),
+        }, {
+            0
+        } })
+    );
+
+    /*** Stop after two descriptors. */
+    ble_gatt_disc_d_test_misc_all(50, 100, 2,
+        ((struct ble_gatt_disc_d_test_dsc[]) { {
+            .chr_val_handle = 50,
+            .dsc_handle = 51,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x1111),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 52,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x2222),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 53,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x3333),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 54,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x4444),
+        }, {
+            .chr_val_handle = 50,
+            .dsc_handle = 55,
+            .dsc_uuid.u16 = BLE_UUID16_INIT(0x5555),
+        }, {
+            0
+        } })
+    );
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_d_test_oom_all)
+{
+    struct ble_gatt_disc_d_test_dsc dscs[] = {
+        {
+            .chr_val_handle = 543,
+            .dsc_handle = 548,
+            .dsc_uuid.u128 = BLE_UUID128_INIT(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        },
+        {
+            .chr_val_handle = 543,
+            .dsc_handle = 549,
+            .dsc_uuid.u128 = BLE_UUID128_INIT(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        },
+        { 0 }
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int stop_after;
+    int num_dscs;
+    int rc;
+
+    ble_gatt_disc_d_test_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover all characteristics procedure. */
+    stop_after = 0;
+    rc = ble_gattc_disc_all_dscs(1, 543, 560,
+                                 ble_gatt_disc_d_test_misc_cb, &stop_after);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    num_dscs = ble_gatt_disc_d_test_misc_rx_rsp_once(1, dscs);
+
+    /* Make sure there are still undiscovered services. */
+    TEST_ASSERT_FATAL(num_dscs < sizeof dscs / sizeof dscs[0] - 1);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_disc_d_test_misc_rx_rsp_once(1, dscs + num_dscs);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure succeeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    ble_hs_test_util_rx_att_err_rsp(1,
+                                    BLE_ATT_OP_READ_TYPE_REQ,
+                                    BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                    1);
+    ble_gatt_disc_d_test_misc_verify_dscs(dscs, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_disc_d_test_suite)
+{
+    ble_gatt_disc_d_test_1();
+    ble_gatt_disc_d_test_oom_all();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_s_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_s_test.c
new file mode 100644
index 00000000..3e7a3026
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_disc_s_test.c
@@ -0,0 +1,631 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+struct ble_gatt_disc_s_test_svc {
+    uint16_t start_handle;
+    uint16_t end_handle;
+    const ble_uuid_t *uuid;
+};
+
+#define BLE_GATT_DISC_S_TEST_MAX_SERVICES  256
+static struct ble_gatt_svc
+    ble_gatt_disc_s_test_svcs[BLE_GATT_DISC_S_TEST_MAX_SERVICES];
+static int ble_gatt_disc_s_test_num_svcs;
+static int ble_gatt_disc_s_test_rx_complete;
+
+static void
+ble_gatt_disc_s_test_init(void)
+{
+    ble_hs_test_util_init();
+    ble_gatt_disc_s_test_num_svcs = 0;
+    ble_gatt_disc_s_test_rx_complete = 0;
+}
+
+static int
+ble_gatt_disc_s_test_misc_svc_length(struct ble_gatt_disc_s_test_svc *service)
+{
+    if (service->uuid->type == BLE_UUID_TYPE_16) {
+        return 6;
+    } else {
+        return 20;
+    }
+}
+
+static int
+ble_gatt_disc_s_test_misc_rx_all_rsp_once(
+    uint16_t conn_handle, struct ble_gatt_disc_s_test_svc *services)
+{
+    struct ble_att_read_group_type_rsp rsp;
+    uint8_t buf[1024];
+    int off;
+    int rc;
+    int i;
+
+    /* Send the pending ATT Read By Group Type Request. */
+
+    rsp.bagp_length = ble_gatt_disc_s_test_misc_svc_length(services);
+    ble_att_read_group_type_rsp_write(buf, BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ,
+                                      &rsp);
+
+    off = BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ;
+    for (i = 0; ; i++) {
+        if (services[i].start_handle == 0) {
+            /* No more services. */
+            break;
+        }
+
+        rc = ble_gatt_disc_s_test_misc_svc_length(services + i);
+        if (rc != rsp.bagp_length) {
+            /* UUID length is changing; Need a separate response. */
+            break;
+        }
+
+        if (services[i].uuid->type == BLE_UUID_TYPE_16) {
+            if (off + BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16 >
+                ble_att_mtu(conn_handle)) {
+
+                /* Can't fit any more entries. */
+                break;
+            }
+        } else {
+            if (off + BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128 >
+                ble_att_mtu(conn_handle)) {
+
+                /* Can't fit any more entries. */
+                break;
+            }
+        }
+
+        put_le16(buf + off, services[i].start_handle);
+        off += 2;
+
+        put_le16(buf + off, services[i].end_handle);
+        off += 2;
+
+        ble_uuid_flat(services[i].uuid, buf + off);
+        off += ble_uuid_length(services[i].uuid);
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    return i;
+}
+
+static void
+ble_gatt_disc_s_test_misc_rx_all_rsp(
+    uint16_t conn_handle, struct ble_gatt_disc_s_test_svc *services)
+{
+    int count;
+    int idx;
+
+    idx = 0;
+    while (services[idx].start_handle != 0) {
+        count = ble_gatt_disc_s_test_misc_rx_all_rsp_once(conn_handle,
+                                                          services + idx);
+        idx += count;
+    }
+
+    if (services[idx - 1].end_handle != 0xffff) {
+        /* Send the pending ATT Request. */
+        ble_hs_test_util_rx_att_err_rsp(conn_handle,
+                                        BLE_ATT_OP_READ_GROUP_TYPE_REQ,
+                                        BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                        services[idx - 1].start_handle);
+    }
+}
+
+static int
+ble_gatt_disc_s_test_misc_rx_uuid_rsp_once(
+    uint16_t conn_handle, struct ble_gatt_disc_s_test_svc *services)
+{
+    uint8_t buf[1024];
+    int off;
+    int rc;
+    int i;
+
+    /* Send the pending ATT Find By Type Value Request. */
+
+    buf[0] = BLE_ATT_OP_FIND_TYPE_VALUE_RSP;
+    off = BLE_ATT_FIND_TYPE_VALUE_RSP_BASE_SZ;
+    for (i = 0; ; i++) {
+        if (services[i].start_handle == 0) {
+            /* No more services. */
+            break;
+        }
+
+        if (off + BLE_ATT_FIND_TYPE_VALUE_HINFO_BASE_SZ >
+            ble_att_mtu(conn_handle)) {
+
+            /* Can't fit any more entries. */
+            break;
+        }
+
+        put_le16(buf + off, services[i].start_handle);
+        off += 2;
+
+        put_le16(buf + off, services[i].end_handle);
+        off += 2;
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    return i;
+}
+
+static void
+ble_gatt_disc_s_test_misc_rx_uuid_rsp(
+    uint16_t conn_handle, struct ble_gatt_disc_s_test_svc *services)
+{
+    int count;
+    int idx;
+
+    idx = 0;
+    while (services[idx].start_handle != 0) {
+        count = ble_gatt_disc_s_test_misc_rx_uuid_rsp_once(conn_handle,
+                                                           services + idx);
+        idx += count;
+    }
+
+    if (services[idx - 1].end_handle != 0xffff) {
+        /* Send the pending ATT Request. */
+        ble_hs_test_util_rx_att_err_rsp(conn_handle,
+                                        BLE_ATT_OP_FIND_TYPE_VALUE_REQ,
+                                        BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                        services[idx - 1].start_handle);
+    }
+}
+
+static void
+ble_gatt_disc_s_test_misc_verify_services(
+    struct ble_gatt_disc_s_test_svc *services)
+{
+    int i;
+
+    for (i = 0; services[i].start_handle != 0; i++) {
+        TEST_ASSERT(services[i].start_handle ==
+                    ble_gatt_disc_s_test_svcs[i].start_handle);
+        TEST_ASSERT(services[i].end_handle ==
+                    ble_gatt_disc_s_test_svcs[i].end_handle);
+
+        TEST_ASSERT(ble_uuid_cmp(services[i].uuid,
+                    &ble_gatt_disc_s_test_svcs[i].uuid.u) == 0);
+    }
+
+    TEST_ASSERT(i == ble_gatt_disc_s_test_num_svcs);
+    TEST_ASSERT(ble_gatt_disc_s_test_rx_complete);
+}
+
+static int
+ble_gatt_disc_s_test_misc_disc_cb(uint16_t conn_handle,
+                                  const struct ble_gatt_error *error,
+                                  const struct ble_gatt_svc *service,
+                                  void *arg)
+{
+    TEST_ASSERT(error != NULL);
+    TEST_ASSERT(!ble_gatt_disc_s_test_rx_complete);
+
+    switch (error->status) {
+    case 0:
+        TEST_ASSERT(service != NULL);
+        TEST_ASSERT_FATAL(ble_gatt_disc_s_test_num_svcs <
+                          BLE_GATT_DISC_S_TEST_MAX_SERVICES);
+        ble_gatt_disc_s_test_svcs[ble_gatt_disc_s_test_num_svcs++] = *service;
+        break;
+
+    case BLE_HS_EDONE:
+        TEST_ASSERT(service == NULL);
+        ble_gatt_disc_s_test_rx_complete = 1;
+        break;
+
+    case BLE_HS_ETIMEOUT:
+        ble_gatt_disc_s_test_rx_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+    }
+
+    return 0;
+}
+
+static void
+ble_gatt_disc_s_test_misc_good_all(struct ble_gatt_disc_s_test_svc *services)
+{
+    int rc;
+
+    ble_gatt_disc_s_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_gattc_disc_all_svcs(2, ble_gatt_disc_s_test_misc_disc_cb, NULL);
+    TEST_ASSERT(rc == 0);
+
+    ble_gatt_disc_s_test_misc_rx_all_rsp(2, services);
+    ble_gatt_disc_s_test_misc_verify_services(services);
+}
+
+static void
+ble_gatt_disc_s_test_misc_good_uuid(
+    struct ble_gatt_disc_s_test_svc *services)
+{
+    int rc;
+
+    ble_gatt_disc_s_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_gattc_disc_svc_by_uuid(2, services[0].uuid,
+                                    ble_gatt_disc_s_test_misc_disc_cb, NULL);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_verify_tx_disc_svc_uuid(services[0].uuid);
+
+    ble_gatt_disc_s_test_misc_rx_uuid_rsp(2, services);
+    ble_gatt_disc_s_test_misc_verify_services(services);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_s_test_disc_all)
+{
+    /*** One 128-bit service. */
+    ble_gatt_disc_s_test_misc_good_all((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 0 }
+    });
+
+    /*** Two 128-bit services. */
+    ble_gatt_disc_s_test_misc_good_all((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 10, 50,   BLE_UUID128_DECLARE(2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 ), },
+        { 0 }
+    });
+
+    /*** Five 128-bit services. */
+    ble_gatt_disc_s_test_misc_good_all((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 10, 50,   BLE_UUID128_DECLARE(2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 ), },
+        { 80, 120,  BLE_UUID128_DECLARE(3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 ), },
+        { 123, 678, BLE_UUID128_DECLARE(4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 ), },
+        { 751, 999, BLE_UUID128_DECLARE(5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5 ), },
+        { 0 }
+    });
+
+    /*** One 128-bit service, one 16-bit-service. */
+    ble_gatt_disc_s_test_misc_good_all((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 6, 7,     BLE_UUID16_DECLARE(0x1234) },
+        { 0 }
+    });
+
+    /*** End with handle 0xffff. */
+    ble_gatt_disc_s_test_misc_good_all((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 7, 0xffff,BLE_UUID128_DECLARE(2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 ), },
+        { 0 }
+    });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_s_test_disc_uuid)
+{
+    /*** 128-bit service; one entry. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 0 }
+    });
+
+    /*** 128-bit service; two entries. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 8, 43,    BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 0 }
+    });
+
+    /*** 128-bit service; five entries. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 8, 43,    BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 67, 100,  BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 102, 103, BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 262, 900, BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 0 }
+    });
+
+    /*** 128-bit service; end with handle 0xffff. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 7, 0xffff,BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 0 }
+    });
+
+    /*** 16-bit service; one entry. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID16_DECLARE(0x1234) },
+        { 0 }
+    });
+
+    /*** 16-bit service; two entries. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID16_DECLARE(0x1234) },
+        { 85, 243,  BLE_UUID16_DECLARE(0x1234) },
+        { 0 }
+    });
+
+    /*** 16-bit service; five entries. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID16_DECLARE(0x1234) },
+        { 85, 243,  BLE_UUID16_DECLARE(0x1234) },
+        { 382, 383, BLE_UUID16_DECLARE(0x1234) },
+        { 562, 898, BLE_UUID16_DECLARE(0x1234) },
+        { 902, 984, BLE_UUID16_DECLARE(0x1234) },
+        { 0 }
+    });
+
+    /*** 16-bit service; end with handle 0xffff. */
+    ble_gatt_disc_s_test_misc_good_uuid((struct ble_gatt_disc_s_test_svc[]) {
+        { 1, 5,     BLE_UUID16_DECLARE(0x1234) },
+        { 9, 0xffff,BLE_UUID16_DECLARE(0x1234) },
+        { 0 }
+    });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_s_test_oom_all)
+{
+    struct ble_gatt_disc_s_test_svc svcs[] = {
+        { 1, 5,     BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 6, 10,    BLE_UUID128_DECLARE(2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 ), },
+        { 0 },
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int num_svcs;
+    int rc;
+
+    ble_gatt_disc_s_test_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover all services procedure. */
+    rc = ble_gattc_disc_all_svcs(1, ble_gatt_disc_s_test_misc_disc_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    num_svcs = ble_gatt_disc_s_test_misc_rx_all_rsp_once(1, svcs);
+
+    /* Make sure there are still undiscovered services. */
+    TEST_ASSERT_FATAL(num_svcs < sizeof svcs / sizeof svcs[0] - 1);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_disc_s_test_misc_rx_all_rsp_once(1, svcs + num_svcs);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    ble_hs_test_util_rx_att_err_rsp(1,
+                                    BLE_ATT_OP_READ_GROUP_TYPE_REQ,
+                                    BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                    1);
+    ble_gatt_disc_s_test_misc_verify_services(svcs);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_s_test_oom_uuid)
+{
+    /* Retrieve enough services to require two transactions. */
+    struct ble_gatt_disc_s_test_svc svcs[] = {
+        { 1, 5,   BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 6, 10,  BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 11, 15, BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 16, 20, BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 21, 25, BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 26, 30, BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 0 },
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int num_svcs;
+    int rc;
+
+    ble_gatt_disc_s_test_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover all services procedure. */
+    rc = ble_gattc_disc_svc_by_uuid(1, svcs[0].uuid,
+                                    ble_gatt_disc_s_test_misc_disc_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    num_svcs = ble_gatt_disc_s_test_misc_rx_uuid_rsp_once(1, svcs);
+
+    /* Make sure there are still undiscovered services. */
+    TEST_ASSERT_FATAL(num_svcs < sizeof svcs / sizeof svcs[0] - 1);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_disc_s_test_misc_rx_uuid_rsp_once(1, svcs + num_svcs);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify that procedure completes when mbufs are available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    ble_hs_test_util_rx_att_err_rsp(1,
+                                    BLE_ATT_OP_READ_GROUP_TYPE_REQ,
+                                    BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                    1);
+    ble_gatt_disc_s_test_misc_verify_services(svcs);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_disc_s_test_oom_timeout)
+{
+    struct ble_gatt_disc_s_test_svc svcs[] = {
+        { 1, 5,  BLE_UUID128_DECLARE(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ), },
+        { 6, 10, BLE_UUID128_DECLARE(2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 ), },
+        { 0 },
+    };
+
+    struct os_mbuf *oms_temp;
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int rc;
+    int i;
+
+    ble_gatt_disc_s_test_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover all services procedure. */
+    rc = ble_gattc_disc_all_svcs(1, ble_gatt_disc_s_test_misc_disc_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_disc_s_test_misc_rx_all_rsp_once(1, svcs);
+
+    /* Keep trying to resume for 30 seconds, but never free any mbufs.  Verify
+     * procedure eventually times out.
+     */
+    for (i = 0; i < 30; i++) {
+        /* Ensure no follow-up request got sent.  It should not have gotten
+         * sent due to mbuf exhaustion.
+         */
+        ble_hs_test_util_prev_tx_queue_clear();
+        TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+        oms_temp = ble_hs_test_util_mbuf_alloc_all_but(0);
+        if (oms_temp != NULL) {
+            os_mbuf_concat(oms, oms_temp);
+        }
+
+        /* Verify that we will resume the stalled GATT procedure in one
+         * second.
+         */
+        ticks_until = ble_gattc_timer();
+        TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+        os_time_advance(ticks_until);
+    }
+
+    /* Verify the procedure has timed out.  The connection should now be
+     * in the process of being terminated.  XXX: Check this.
+     */
+    ble_hs_test_util_hci_ack_set_disconnect(0);
+    ble_gattc_timer();
+
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == BLE_HS_FOREVER);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_disc_s_test_suite)
+{
+    ble_gatt_disc_s_test_disc_all();
+    ble_gatt_disc_s_test_disc_uuid();
+    ble_gatt_disc_s_test_oom_all();
+    ble_gatt_disc_s_test_oom_uuid();
+    ble_gatt_disc_s_test_oom_timeout();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_find_s_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_find_s_test.c
new file mode 100644
index 00000000..172bdd33
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_find_s_test.c
@@ -0,0 +1,433 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+static struct ble_gatt_svc ble_gatt_find_s_test_svcs[256];
+static int ble_gatt_find_s_test_num_svcs;
+static int ble_gatt_find_s_test_proc_complete;
+
+struct ble_gatt_find_s_test_entry {
+    uint16_t inc_handle; /* 0 indicates no more entries. */
+    uint16_t start_handle;
+    uint16_t end_handle;
+    const ble_uuid_t *uuid;
+};
+
+static void
+ble_gatt_find_s_test_misc_init(void)
+{
+    ble_hs_test_util_init();
+    ble_gatt_find_s_test_num_svcs = 0;
+    ble_gatt_find_s_test_proc_complete = 0;
+}
+
+static int
+ble_gatt_find_s_test_misc_cb(uint16_t conn_handle,
+                             const struct ble_gatt_error *error,
+                             const struct ble_gatt_svc *service,
+                             void *arg)
+{
+    TEST_ASSERT(!ble_gatt_find_s_test_proc_complete);
+    TEST_ASSERT(error != NULL);
+
+    switch (error->status) {
+    case 0:
+        ble_gatt_find_s_test_svcs[ble_gatt_find_s_test_num_svcs++] = *service;
+        break;
+
+    case BLE_HS_EDONE:
+        ble_gatt_find_s_test_proc_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+
+    return 0;
+}
+
+static void
+ble_gatt_find_s_test_misc_verify_incs(
+    struct ble_gatt_find_s_test_entry *entries)
+{
+    int i;
+
+    for (i = 0; entries[i].inc_handle != 0; i++) {
+        TEST_ASSERT(ble_gatt_find_s_test_svcs[i].start_handle ==
+                    entries[i].start_handle);
+        TEST_ASSERT(ble_gatt_find_s_test_svcs[i].end_handle ==
+                    entries[i].end_handle);
+        TEST_ASSERT(ble_uuid_cmp(&ble_gatt_find_s_test_svcs[i].uuid.u,
+                                 entries[i].uuid) == 0);
+    }
+
+    TEST_ASSERT(i == ble_gatt_find_s_test_num_svcs);
+    TEST_ASSERT(ble_gatt_find_s_test_proc_complete);
+}
+
+static int
+ble_gatt_find_s_test_misc_rx_read_type(
+    uint16_t conn_handle, struct ble_gatt_find_s_test_entry *entries)
+{
+    struct ble_att_read_type_rsp rsp;
+    uint8_t buf[1024];
+    int off;
+    int rc;
+    int i;
+
+    memset(&rsp, 0, sizeof rsp);
+
+    off = BLE_ATT_READ_TYPE_RSP_BASE_SZ;
+    for (i = 0; entries[i].inc_handle != 0; i++) {
+        if (rsp.batp_length == BLE_GATTS_INC_SVC_LEN_NO_UUID + 2) {
+            break;
+        }
+
+        if (entries[i].uuid->type != BLE_UUID_TYPE_16) {
+            if (rsp.batp_length != 0) {
+                break;
+            }
+            rsp.batp_length = BLE_GATTS_INC_SVC_LEN_NO_UUID + 2;
+        } else {
+            rsp.batp_length = BLE_GATTS_INC_SVC_LEN_UUID + 2;
+        }
+
+        TEST_ASSERT_FATAL(off + rsp.batp_length <= sizeof buf);
+
+        put_le16(buf + off, entries[i].inc_handle);
+        off += 2;
+
+        put_le16(buf + off, entries[i].start_handle);
+        off += 2;
+
+        put_le16(buf + off, entries[i].end_handle);
+        off += 2;
+
+        if (entries[i].uuid->type == BLE_UUID_TYPE_16) {
+            put_le16(buf + off, ble_uuid_u16(entries[i].uuid));
+            off += 2;
+        }
+    }
+
+    if (i == 0) {
+        ble_hs_test_util_rx_att_err_rsp(conn_handle, BLE_ATT_OP_READ_TYPE_REQ,
+                                        BLE_ATT_ERR_ATTR_NOT_FOUND, 0);
+        return 0;
+    }
+
+    ble_att_read_type_rsp_write(buf + 0, BLE_ATT_READ_TYPE_RSP_BASE_SZ, &rsp);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    return i;
+}
+
+static void
+ble_gatt_find_s_test_misc_rx_read(uint16_t conn_handle, const ble_uuid_t *uuid)
+{
+    uint8_t buf[17];
+    int rc;
+
+    TEST_ASSERT(uuid->type == BLE_UUID_TYPE_128);
+
+    buf[0] = BLE_ATT_OP_READ_RSP;
+    ble_uuid_flat(uuid, buf + 1);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 17);
+    TEST_ASSERT(rc == 0);
+}
+
+static void
+ble_gatt_find_s_test_misc_verify_tx_read_type(uint16_t start_handle,
+                                              uint16_t end_handle)
+{
+    struct ble_att_read_type_req req;
+    struct os_mbuf *om;
+    uint16_t uuid16;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    ble_att_read_type_req_parse(om->om_data, om->om_len, &req);
+
+    TEST_ASSERT(req.batq_start_handle == start_handle);
+    TEST_ASSERT(req.batq_end_handle == end_handle);
+    TEST_ASSERT(om->om_len == BLE_ATT_READ_TYPE_REQ_BASE_SZ + 2);
+    uuid16 = get_le16(om->om_data + BLE_ATT_READ_TYPE_REQ_BASE_SZ);
+    TEST_ASSERT(uuid16 == BLE_ATT_UUID_INCLUDE);
+}
+
+static void
+ble_gatt_find_s_test_misc_verify_tx_read(uint16_t handle)
+{
+    struct ble_att_read_req req;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    ble_att_read_req_parse(om->om_data, om->om_len, &req);
+
+    TEST_ASSERT(req.barq_handle == handle);
+    TEST_ASSERT(om->om_len == BLE_ATT_READ_REQ_SZ);
+}
+
+static void
+ble_gatt_find_s_test_misc_find_inc(uint16_t conn_handle,
+                                   uint16_t start_handle, uint16_t end_handle,
+                                   struct ble_gatt_find_s_test_entry *entries)
+{
+    struct ble_gatt_svc service;
+    int cur_start;
+    int num_found;
+    int idx;
+    int rc;
+    int i;
+
+    rc = ble_gattc_find_inc_svcs(conn_handle, start_handle, end_handle,
+                                 ble_gatt_find_s_test_misc_cb, &service);
+    TEST_ASSERT(rc == 0);
+
+    cur_start = start_handle;
+    idx = 0;
+    while (1) {
+        ble_gatt_find_s_test_misc_verify_tx_read_type(cur_start, end_handle);
+        num_found = ble_gatt_find_s_test_misc_rx_read_type(conn_handle,
+                                                           entries + idx);
+        if (num_found == 0) {
+            break;
+        }
+
+        if (entries[idx].uuid->type == BLE_UUID_TYPE_128) {
+            TEST_ASSERT(num_found == 1);
+            ble_gatt_find_s_test_misc_verify_tx_read(
+                entries[idx].start_handle);
+            ble_gatt_find_s_test_misc_rx_read(conn_handle,
+                                              entries[idx].uuid);
+        }
+
+        idx += num_found;
+        cur_start = entries[idx - 1].inc_handle + 1;
+    }
+    TEST_ASSERT(idx == ble_gatt_find_s_test_num_svcs);
+    TEST_ASSERT(ble_gatt_find_s_test_proc_complete);
+
+    for (i = 0; i < ble_gatt_find_s_test_num_svcs; i++) {
+        TEST_ASSERT(ble_gatt_find_s_test_svcs[i].start_handle ==
+                    entries[i].start_handle);
+        TEST_ASSERT(ble_gatt_find_s_test_svcs[i].end_handle ==
+                    entries[i].end_handle);
+        TEST_ASSERT(ble_uuid_cmp(&ble_gatt_find_s_test_svcs[i].uuid.u,
+                                 entries[i].uuid) == 0);
+    }
+}
+
+TEST_CASE_SELF(ble_gatt_find_s_test_1)
+{
+    /* Two 16-bit UUID services; one response. */
+    ble_gatt_find_s_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+    ble_gatt_find_s_test_misc_find_inc(2, 5, 10,
+        ((struct ble_gatt_find_s_test_entry[]) { {
+            .inc_handle = 6,
+            .start_handle = 35,
+            .end_handle = 49,
+            .uuid = BLE_UUID16_DECLARE(0x5155),
+        }, {
+            .inc_handle = 9,
+            .start_handle = 543,
+            .end_handle = 870,
+            .uuid = BLE_UUID16_DECLARE(0x1122),
+        }, {
+            0,
+        } })
+    );
+
+    /* One 128-bit UUID service; two responses. */
+    ble_gatt_find_s_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+    ble_gatt_find_s_test_misc_find_inc(2, 34, 100,
+        ((struct ble_gatt_find_s_test_entry[]) { {
+            .inc_handle = 36,
+            .start_handle = 403,
+            .end_handle = 859,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        }, {
+            0,
+        } })
+    );
+
+    /* Two 128-bit UUID service; four responses. */
+    ble_gatt_find_s_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+    ble_gatt_find_s_test_misc_find_inc(2, 34, 100,
+        ((struct ble_gatt_find_s_test_entry[]) { {
+            .inc_handle = 36,
+            .start_handle = 403,
+            .end_handle = 859,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        }, {
+            .inc_handle = 39,
+            .start_handle = 900,
+            .end_handle = 932,
+            .uuid = BLE_UUID128_DECLARE(2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17),
+        }, {
+            0,
+        } })
+    );
+
+    /* Two 16-bit UUID; three 128-bit UUID; seven responses. */
+    ble_gatt_find_s_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+    ble_gatt_find_s_test_misc_find_inc(2, 1, 100,
+        ((struct ble_gatt_find_s_test_entry[]) { {
+            .inc_handle = 36,
+            .start_handle = 403,
+            .end_handle = 859,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        }, {
+            .inc_handle = 37,
+            .start_handle = 35,
+            .end_handle = 49,
+            .uuid = BLE_UUID16_DECLARE(0x5155),
+        }, {
+            .inc_handle = 38,
+            .start_handle = 543,
+            .end_handle = 870,
+            .uuid = BLE_UUID16_DECLARE(0x1122),
+        }, {
+            .inc_handle = 39,
+            .start_handle = 900,
+            .end_handle = 932,
+            .uuid = BLE_UUID128_DECLARE(2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17),
+        }, {
+            .inc_handle = 40,
+            .start_handle = 940,
+            .end_handle = 950,
+            .uuid = BLE_UUID128_DECLARE(3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18),
+        }, {
+            0,
+        } })
+    );
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_find_s_test_oom)
+{
+
+    struct ble_gatt_find_s_test_entry incs[] = {
+        {
+            .inc_handle = 21,
+            .start_handle = 800,
+            .end_handle = 899,
+            .uuid = BLE_UUID128_DECLARE(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15),
+        },
+        {
+            .inc_handle = 22,
+            .start_handle = 900,
+            .end_handle = 999,
+            .uuid = BLE_UUID128_DECLARE(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16),
+        },
+        { 0 }
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int rc;
+
+    ble_gatt_find_s_test_misc_init();
+
+    ble_hs_test_util_create_conn(1, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a discover all characteristics procedure. */
+    rc = ble_gattc_find_inc_svcs(1, 20, 30,
+                                 ble_gatt_find_s_test_misc_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_find_s_test_misc_rx_read_type(1, incs);
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure succeeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* We can't cause a memory exhaustion error on the follow up request.  The
+     * GATT client frees the read response immediately before sending the
+     * follow-up request, so there is always an mbuf available.
+     */
+    /* XXX: Find a way to test this. */
+    ble_gatt_find_s_test_misc_rx_read(1, incs[0].uuid);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_find_s_test_misc_rx_read_type(1, incs + 1);
+
+    /* Verify the procedure succeeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    ble_gatt_find_s_test_misc_rx_read(1, incs[1].uuid);
+
+    ble_hs_test_util_rx_att_err_rsp(1,
+                                    BLE_ATT_OP_READ_TYPE_REQ,
+                                    BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                    1);
+
+    ble_gatt_find_s_test_misc_verify_incs(incs);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_find_s_test_suite)
+{
+    ble_gatt_find_s_test_1();
+    ble_gatt_find_s_test_oom();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_read_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_read_test.c
new file mode 100644
index 00000000..572b0bf1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_read_test.c
@@ -0,0 +1,923 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include <limits.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+struct ble_gatt_read_test_attr {
+    uint16_t conn_handle;
+    uint16_t handle;
+    uint8_t value_len;
+    uint8_t value[BLE_ATT_ATTR_MAX_LEN];
+};
+
+#define BLE_GATT_READ_TEST_MAX_ATTRS    256
+
+struct ble_gatt_read_test_attr
+    ble_gatt_read_test_attrs[BLE_GATT_READ_TEST_MAX_ATTRS];
+int ble_gatt_read_test_num_attrs;
+int ble_gatt_read_test_complete;
+
+uint16_t ble_gatt_read_test_bad_conn_handle;
+int ble_gatt_read_test_bad_status;
+
+static void
+ble_gatt_read_test_misc_init(void)
+{
+    ble_hs_test_util_init();
+    ble_gatt_read_test_num_attrs = 0;
+    ble_gatt_read_test_complete = 0;
+    ble_gatt_read_test_bad_conn_handle = 0;
+    ble_gatt_read_test_bad_status = 0;
+
+    memset(&ble_gatt_read_test_attrs[0], 0,
+           sizeof ble_gatt_read_test_attrs[0]);
+}
+
+static int
+ble_gatt_read_test_cb(uint16_t conn_handle, const struct ble_gatt_error *error,
+                      struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_read_test_attr *dst;
+    int *stop_after;
+    int rc;
+
+    stop_after = arg;
+
+    TEST_ASSERT_FATAL(error != NULL);
+
+    if (error->status != 0) {
+        ble_gatt_read_test_bad_conn_handle = conn_handle;
+        ble_gatt_read_test_bad_status = error->status;
+        ble_gatt_read_test_complete = 1;
+        return 0;
+    }
+
+    if (attr == NULL) {
+        ble_gatt_read_test_complete = 1;
+        return 0;
+    }
+
+    TEST_ASSERT_FATAL(ble_gatt_read_test_num_attrs <
+                      BLE_GATT_READ_TEST_MAX_ATTRS);
+    dst = ble_gatt_read_test_attrs + ble_gatt_read_test_num_attrs++;
+
+    TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(attr->om) <= sizeof dst->value);
+
+    dst->conn_handle = conn_handle;
+    dst->handle = attr->handle;
+    dst->value_len = OS_MBUF_PKTLEN(attr->om);
+    rc = os_mbuf_copydata(attr->om, 0, OS_MBUF_PKTLEN(attr->om), dst->value);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    if (stop_after != NULL && *stop_after > 0) {
+        (*stop_after)--;
+        if (*stop_after == 0) {
+            ble_gatt_read_test_complete = 1;
+            return 1;
+        }
+    } else {
+        ble_gatt_read_test_complete = 1;
+    }
+
+    return 0;
+}
+
+static int
+ble_gatt_read_test_long_cb(uint16_t conn_handle,
+                           const struct ble_gatt_error *error,
+                           struct ble_gatt_attr *attr, void *arg)
+{
+    struct ble_gatt_read_test_attr *dst;
+    int *reads_left;
+    int rc;
+
+    reads_left = arg;
+
+    TEST_ASSERT_FATAL(error != NULL);
+
+    if (error->status != 0) {
+        ble_gatt_read_test_bad_conn_handle = conn_handle;
+        ble_gatt_read_test_bad_status = error->status;
+        ble_gatt_read_test_complete = 1;
+        return 0;
+    }
+
+    if (attr == NULL) {
+        ble_gatt_read_test_complete = 1;
+        return 0;
+    }
+
+    dst = ble_gatt_read_test_attrs + 0;
+
+    TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(attr->om) <=
+        dst->value_len + sizeof dst->value);
+    TEST_ASSERT(attr->offset == dst->value_len);
+
+    if (attr->offset == 0) {
+        dst->conn_handle = conn_handle;
+        dst->handle = attr->handle;
+    } else {
+        TEST_ASSERT(conn_handle == dst->conn_handle);
+        TEST_ASSERT(attr->handle == dst->handle);
+    }
+    rc = os_mbuf_copydata(attr->om, 0, OS_MBUF_PKTLEN(attr->om),
+                          dst->value + dst->value_len);
+    TEST_ASSERT_FATAL(rc == 0);
+    dst->value_len += OS_MBUF_PKTLEN(attr->om);
+
+    if (reads_left != NULL && *reads_left > 0) {
+        (*reads_left)--;
+        if (*reads_left == 0) {
+            ble_gatt_read_test_complete = 1;
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+static void
+ble_gatt_read_test_misc_rx_rsp_good_raw(uint16_t conn_handle,
+                                        uint8_t att_op,
+                                        const void *data, int data_len)
+{
+    uint8_t buf[1024];
+    int rc;
+
+    TEST_ASSERT_FATAL(data_len <= sizeof buf);
+
+    /* Send the pending ATT Read Request. */
+
+    buf[0] = att_op;
+    memcpy(buf + 1, data, data_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, 1 + data_len);
+    TEST_ASSERT(rc == 0);
+}
+
+static void
+ble_gatt_read_test_misc_rx_rsp_good(uint16_t conn_handle,
+                                    struct ble_hs_test_util_flat_attr *attr)
+{
+    ble_gatt_read_test_misc_rx_rsp_good_raw(conn_handle, BLE_ATT_OP_READ_RSP,
+                                            attr->value,
+                                            attr->value_len);
+}
+
+static void
+ble_gatt_read_test_misc_rx_rsp_bad(uint16_t conn_handle,
+                                   uint8_t att_error, uint16_t err_handle)
+{
+    /* Send the pending ATT Read Request. */
+
+    ble_hs_test_util_rx_att_err_rsp(conn_handle, BLE_ATT_OP_READ_REQ,
+                                    att_error, err_handle);
+}
+
+static int
+ble_gatt_read_test_misc_uuid_rx_rsp_good(
+    uint16_t conn_handle, struct ble_hs_test_util_flat_attr *attrs)
+{
+    struct ble_att_read_type_rsp rsp;
+    uint8_t buf[1024];
+    int prev_len;
+    int off;
+    int rc;
+    int i;
+
+    if (ble_gatt_read_test_complete || attrs[0].handle == 0) {
+        return 0;
+    }
+
+    /* Send the pending ATT Read By Type Request. */
+
+    rsp.batp_length = 2 + attrs[0].value_len;
+    ble_att_read_type_rsp_write(buf, sizeof buf, &rsp);
+
+    prev_len = 0;
+    off = BLE_ATT_READ_TYPE_RSP_BASE_SZ;
+    for (i = 0; attrs[i].handle != 0; i++) {
+        if (prev_len != 0 && prev_len != attrs[i].value_len) {
+            break;
+        }
+        prev_len = attrs[i].value_len;
+
+        put_le16(buf + off, attrs[i].handle);
+        off += 2;
+
+        memcpy(buf + off, attrs[i].value, attrs[i].value_len);
+        off += attrs[i].value_len;
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    TEST_ASSERT(rc == 0);
+
+    return i;
+}
+
+static void
+ble_gatt_read_test_misc_verify_good(struct ble_hs_test_util_flat_attr *attr)
+{
+    int rc;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Exchange MTU: We need plus 1 for the read response opcode */
+    ble_hs_test_util_set_att_mtu(2, attr->value_len + 1);
+
+    rc = ble_gattc_read(2, attr->handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_good(2, attr);
+
+    TEST_ASSERT(ble_gatt_read_test_num_attrs == 1);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].handle == attr->handle);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].value_len == attr->value_len);
+    TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[0].value, attr->value,
+                       attr->value_len) == 0);
+}
+
+static void
+ble_gatt_read_test_misc_verify_bad(uint8_t att_status,
+                                   struct ble_hs_test_util_flat_attr *attr)
+{
+    int rc;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_gattc_read(2, attr->handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_bad(2, att_status, attr->handle);
+
+    TEST_ASSERT(ble_gatt_read_test_num_attrs == 0);
+    TEST_ASSERT(ble_gatt_read_test_bad_conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_bad_status ==
+                BLE_HS_ERR_ATT_BASE + att_status);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+}
+
+static void
+ble_gatt_read_test_misc_uuid_verify_good(
+    uint16_t start_handle, uint16_t end_handle, const ble_uuid_t *uuid,
+    int stop_after, struct ble_hs_test_util_flat_attr *attrs)
+{
+    int num_read;
+    int idx;
+    int rc;
+    int i;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_gattc_read_by_uuid(2, start_handle, end_handle, uuid,
+                                ble_gatt_read_test_cb, &stop_after);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    idx = 0;
+    while (1) {
+        num_read = ble_gatt_read_test_misc_uuid_rx_rsp_good(2, attrs + idx);
+        if (num_read == 0) {
+            ble_hs_test_util_rx_att_err_rsp(2, BLE_ATT_OP_READ_TYPE_REQ,
+                                            BLE_ATT_ERR_ATTR_NOT_FOUND,
+                                            start_handle);
+            break;
+        }
+
+        idx += num_read;
+    }
+
+    TEST_ASSERT(ble_gatt_read_test_complete);
+    TEST_ASSERT(idx == ble_gatt_read_test_num_attrs);
+
+    for (i = 0; i < idx; i++) {
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].conn_handle == 2);
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].handle == attrs[i].handle);
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].value_len ==
+                    attrs[i].value_len);
+        TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[i].value, attrs[i].value,
+                           attrs[i].value_len) == 0);
+    }
+    TEST_ASSERT(!ble_gattc_any_jobs());
+}
+
+static void
+ble_gatt_read_test_misc_long_verify_good(
+    int max_reads, struct ble_hs_test_util_flat_attr *attr)
+{
+    int reads_left;
+    int chunk_sz;
+    int rem_len;
+    int att_op;
+    uint16_t offset = 0;
+    int off;
+    int rc;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    if (max_reads == 0) {
+        max_reads = INT_MAX;
+    }
+    reads_left = max_reads;
+    rc = ble_gattc_read_long(2, attr->handle, offset,
+                             ble_gatt_read_test_long_cb, &reads_left);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    off = 0;
+    rem_len = attr->value_len;
+    do {
+        if (rem_len > BLE_ATT_MTU_DFLT - 1) {
+            chunk_sz = BLE_ATT_MTU_DFLT - 1;
+        } else {
+            chunk_sz = rem_len;
+        }
+        if (off == 0) {
+            att_op = BLE_ATT_OP_READ_RSP;
+        } else {
+            att_op = BLE_ATT_OP_READ_BLOB_RSP;
+        }
+        ble_gatt_read_test_misc_rx_rsp_good_raw(2, att_op,
+                                                attr->value + off, chunk_sz);
+        rem_len -= chunk_sz;
+        off += chunk_sz;
+    } while (rem_len > 0 && reads_left > 0);
+
+    TEST_ASSERT(ble_gatt_read_test_complete);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].handle == attr->handle);
+    if (reads_left > 0) {
+        TEST_ASSERT(ble_gatt_read_test_attrs[0].value_len == attr->value_len);
+    }
+    TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[0].value, attr->value,
+                       ble_gatt_read_test_attrs[0].value_len) == 0);
+}
+
+static void
+ble_gatt_read_test_misc_long_verify_bad(
+    uint8_t att_status, struct ble_hs_test_util_flat_attr *attr)
+{
+    uint16_t offset = 0;
+    int rc;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_gattc_read_long(2, attr->handle, offset,
+                             ble_gatt_read_test_long_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_bad(2, att_status, attr->handle);
+
+    TEST_ASSERT(ble_gatt_read_test_num_attrs == 0);
+    TEST_ASSERT(ble_gatt_read_test_bad_conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_bad_status ==
+                BLE_HS_ERR_ATT_BASE + att_status);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+}
+
+static int
+ble_gatt_read_test_misc_extract_handles(
+    struct ble_hs_test_util_flat_attr *attrs, uint16_t *handles)
+{
+    int i;
+
+    for (i = 0; attrs[i].handle != 0; i++) {
+        handles[i] = attrs[i].handle;
+    }
+    return i;
+}
+
+static void
+ble_gatt_read_test_misc_mult_verify_good(
+    struct ble_hs_test_util_flat_attr *attrs)
+{
+    uint8_t expected_value[512];
+    uint16_t handles[256];
+    int num_attrs;
+    int chunk_sz;
+    int off;
+    int rc;
+    int i;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    num_attrs = ble_gatt_read_test_misc_extract_handles(attrs, handles);
+
+    off = 0;
+    for (i = 0; i < num_attrs; i++) {
+        if (attrs[i].value_len > BLE_ATT_MTU_DFLT - 1 - off) {
+            chunk_sz = BLE_ATT_MTU_DFLT - 1 - off;
+        } else {
+            chunk_sz = attrs[i].value_len;
+        }
+
+        if (chunk_sz > 0) {
+            memcpy(expected_value + off, attrs[i].value, chunk_sz);
+            off += chunk_sz;
+        }
+    }
+
+    rc = ble_gattc_read_mult(2, handles, num_attrs,
+                             ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_good_raw(2, BLE_ATT_OP_READ_MULT_RSP,
+                                            expected_value, off);
+
+    TEST_ASSERT(ble_gatt_read_test_complete);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].value_len == off);
+    TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[0].value, expected_value,
+                       off) == 0);
+}
+
+static void
+ble_gatt_read_test_misc_mult_verify_bad(
+    uint8_t att_status, uint16_t err_handle,
+    struct ble_hs_test_util_flat_attr *attrs)
+{
+    uint16_t handles[256];
+    int num_attrs;
+    int rc;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    num_attrs = ble_gatt_read_test_misc_extract_handles(attrs, handles);
+
+    rc = ble_gattc_read_mult(2, handles, num_attrs,
+                             ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_bad(2, att_status, err_handle);
+
+    TEST_ASSERT(ble_gatt_read_test_num_attrs == 0);
+    TEST_ASSERT(ble_gatt_read_test_bad_conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_bad_status ==
+                BLE_HS_ERR_ATT_BASE + att_status);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+}
+
+TEST_CASE_SELF(ble_gatt_read_test_by_handle)
+{
+    /* Read a seven-byte attribute. */
+    ble_gatt_read_test_misc_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 43,
+        .value = { 1,2,3,4,5,6,7 },
+        .value_len = 7
+    } });
+
+    /* Read a one-byte attribute. */
+    ble_gatt_read_test_misc_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 0x5432,
+        .value = { 0xff },
+        .value_len = 1
+    } });
+
+    /* Read a 200-byte attribute. */
+    ble_gatt_read_test_misc_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 815,
+        .value = { 0 },
+        .value_len = 200,
+    } });
+
+    /* Fail due to attribute not found. */
+    ble_gatt_read_test_misc_verify_bad(BLE_ATT_ERR_ATTR_NOT_FOUND,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 719,
+            .value = { 1,2,3,4,5,6,7 },
+            .value_len = 7
+        } });
+
+    /* Fail due to invalid PDU. */
+    ble_gatt_read_test_misc_verify_bad(BLE_ATT_ERR_INVALID_PDU,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 65,
+            .value = { 0xfa, 0x4c },
+            .value_len = 2
+        } });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_read_test_by_uuid)
+{
+    /* Read a single seven-byte attribute. */
+    ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16_DECLARE(0x1234), 0,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 43,
+            .value = { 1,2,3,4,5,6,7 },
+            .value_len = 7
+        }, {
+            0,
+        } });
+
+    /* Read two seven-byte attributes; one response. */
+    ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16_DECLARE(0x1234), 0,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 43,
+            .value = { 1,2,3,4,5,6,7 },
+            .value_len = 7
+        }, {
+            .handle = 44,
+            .value = { 2,3,4,5,6,7,8 },
+            .value_len = 7
+        }, {
+            0,
+        } });
+
+    /* Read two attributes; two responses. */
+    ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16_DECLARE(0x1234), 0,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 43,
+            .value = { 1,2,3,4,5,6,7 },
+            .value_len = 7
+        }, {
+            .handle = 44,
+            .value = { 2,3,4 },
+            .value_len = 3
+        }, {
+            0,
+        } });
+
+    /* Stop after three reads. */
+    ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16_DECLARE(0x1234), 3,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 43,
+            .value = { 1,2,3,4,5,6,7 },
+            .value_len = 7
+        }, {
+            .handle = 44,
+            .value = { 2,3,4 },
+            .value_len = 3
+        }, {
+            .handle = 45,
+            .value = { 2,3,4 },
+            .value_len = 3
+        }, {
+            .handle = 46,
+            .value = { 3,4,5,6 },
+            .value_len = 4
+        }, {
+            .handle = 47,
+            .value = { 2,3,4 },
+            .value_len = 3
+        }, {
+            0,
+        } });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_read_test_long)
+{
+    uint8_t data512[512];
+    int i;
+
+    for (i = 0; i < sizeof data512; i++) {
+        data512[i] = i;
+    }
+
+    /* Read a seven-byte attribute. */
+    ble_gatt_read_test_misc_long_verify_good(0,
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 43,
+        .value = { 1,2,3,4,5,6,7 },
+        .value_len = 7
+    } });
+
+    /* Read a zero-byte attribute. */
+    ble_gatt_read_test_misc_long_verify_good(0,
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 43,
+        .value = { 0 },
+        .value_len = 0
+    } });
+
+    /* Read a 60-byte attribute; three requests. */
+    ble_gatt_read_test_misc_long_verify_good(0,
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 34,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+            33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+            49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60
+        },
+        .value_len = 60
+    } });
+
+    /* Stop after two reads. */
+    ble_gatt_read_test_misc_long_verify_good(2,
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 34,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+            33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+            49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60
+        },
+        .value_len = 60
+    } });
+
+    /* Fail due to attribute not found. */
+    ble_gatt_read_test_misc_long_verify_bad(BLE_ATT_ERR_ATTR_NOT_FOUND,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 719,
+            .value = { 1, 2, 3, 4, 5, 6, 7 },
+            .value_len = 7
+        } });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_read_test_mult)
+{
+    uint8_t data512[512];
+    int i;
+
+    for (i = 0; i < sizeof data512; i++) {
+        data512[i] = i;
+    }
+
+    /* Read one attribute. */
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 43,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
+        .value_len = 7
+    }, {
+        0
+    } });
+
+    /* Read two attributes. */
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 43,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
+        .value_len = 7,
+    }, {
+        .handle = 44,
+        .value = { 8, 9, 10, 11 },
+        .value_len = 4,
+    }, {
+        0
+    } });
+
+    /* Read two attributes (swap order). */
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 44,
+        .value = { 8, 9, 10, 11 },
+        .value_len = 4,
+    }, {
+        .handle = 43,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
+        .value_len = 7,
+    }, {
+        0
+    } });
+
+    /* Read five attributes. */
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
+        .handle = 43,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
+        .value_len = 7,
+    }, {
+        .handle = 44,
+        .value = { 8, 9, 10, 11 },
+        .value_len = 4,
+    }, {
+        .handle = 145,
+        .value = { 12, 13 },
+        .value_len = 2,
+    }, {
+        .handle = 191,
+        .value = { 14, 15, 16 },
+        .value_len = 3,
+    }, {
+        .handle = 352,
+        .value = { 17, 18, 19, 20 },
+        .value_len = 4,
+    }, {
+        0
+    } });
+
+    /* Fail due to attribute not found. */
+    ble_gatt_read_test_misc_mult_verify_bad(BLE_ATT_ERR_ATTR_NOT_FOUND, 719,
+        (struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 719,
+            .value = { 1,2,3,4,5,6,7 },
+            .value_len = 7
+        }, {
+            0
+        } });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_read_test_concurrent)
+{
+    int rc;
+    int i;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /***
+     * Perform three concurrent reads.  Assert that each response is correctly
+     * matched up with its corresponding GATT procedure.
+     */
+
+    struct ble_hs_test_util_flat_attr attrs[3] = {
+        {
+            .handle = 1,
+            .offset = 0,
+            .value_len = 3,
+            .value = { 1, 2, 3 },
+        },
+        {
+            .handle = 2,
+            .offset = 0,
+            .value_len = 4,
+            .value = { 2, 3, 4, 5 },
+        },
+        {
+            .handle = 3,
+            .offset = 0,
+            .value_len = 5,
+            .value = { 3, 4, 5, 6, 7 },
+        },
+    };
+
+    rc = ble_gattc_read(2, attrs[0].handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = ble_gattc_read(2, attrs[1].handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = ble_gattc_read(2, attrs[2].handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_good(2, attrs + 0);
+    ble_gatt_read_test_misc_rx_rsp_good(2, attrs + 1);
+    ble_gatt_read_test_misc_rx_rsp_good(2, attrs + 2);
+
+    TEST_ASSERT(ble_gatt_read_test_num_attrs == 3);
+
+    for (i = 0; i < 3; i++) {
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].handle == attrs[i].handle);
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].value_len ==
+                    attrs[i].value_len);
+        TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[i].value, attrs[i].value,
+                           attrs[i].value_len) == 0);
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_read_test_long_oom)
+{
+    static const struct ble_hs_test_util_flat_attr attr = {
+        .handle = 34,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+            33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+            49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60
+        },
+        .value_len = 60,
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int reads_left;
+    int chunk_sz;
+    uint16_t offset = 0;
+    int off;
+    int rc;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a read long procedure. */
+    off = 0;
+    reads_left = 0;
+    rc = ble_gattc_read_long(2, attr.handle, offset, ble_gatt_read_test_long_cb,
+                             &reads_left);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    chunk_sz = ble_att_mtu(2) - BLE_ATT_READ_RSP_BASE_SZ;
+    ble_gatt_read_test_misc_rx_rsp_good_raw(2, BLE_ATT_OP_READ_RSP,
+                                            attr.value + off, chunk_sz);
+    off += chunk_sz;
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    chunk_sz = ble_att_mtu(2) - BLE_ATT_READ_RSP_BASE_SZ;
+    ble_gatt_read_test_misc_rx_rsp_good_raw(2, BLE_ATT_OP_READ_RSP,
+                                            attr.value + off, chunk_sz);
+    off += chunk_sz;
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify that procedure completes when mbufs are available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    chunk_sz = attr.value_len - off;
+    ble_gatt_read_test_misc_rx_rsp_good_raw(2, BLE_ATT_OP_READ_RSP,
+                                            attr.value + off, chunk_sz);
+    off += chunk_sz;
+
+    TEST_ASSERT(ble_gatt_read_test_complete);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].conn_handle == 2);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].handle == attr.handle);
+    TEST_ASSERT(ble_gatt_read_test_attrs[0].value_len == attr.value_len);
+    TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[0].value, attr.value,
+                       ble_gatt_read_test_attrs[0].value_len) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_read_test_suite)
+{
+    ble_gatt_read_test_by_handle();
+    ble_gatt_read_test_by_uuid();
+    ble_gatt_read_test_long();
+    ble_gatt_read_test_mult();
+    ble_gatt_read_test_concurrent();
+    ble_gatt_read_test_long_oom();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_write_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_write_test.c
new file mode 100644
index 00000000..caa8e565
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatt_write_test.c
@@ -0,0 +1,832 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "ble_hs_test.h"
+#include "host/ble_gatt.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_GATT_WRITE_TEST_MAX_ATTRS   128
+
+static int ble_gatt_write_test_cb_called;
+
+static uint8_t ble_gatt_write_test_attr_value[BLE_ATT_ATTR_MAX_LEN];
+static struct ble_gatt_error ble_gatt_write_test_error;
+
+static struct ble_hs_test_util_flat_attr
+ble_gatt_write_test_attrs[BLE_GATT_WRITE_TEST_MAX_ATTRS];
+static int ble_gatt_write_test_num_attrs;
+
+static void
+ble_gatt_write_test_init(void)
+{
+    int i;
+
+    ble_hs_test_util_init();
+    ble_gatt_write_test_cb_called = 0;
+    ble_gatt_write_test_num_attrs = 0;
+
+    for (i = 0; i < sizeof ble_gatt_write_test_attr_value; i++) {
+        ble_gatt_write_test_attr_value[i] = i;
+    }
+}
+
+static int
+ble_gatt_write_test_cb_good(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            struct ble_gatt_attr *attr, void *arg)
+{
+    int *attr_len;
+
+    attr_len = arg;
+
+    TEST_ASSERT(error != NULL);
+    TEST_ASSERT(conn_handle == 2);
+
+    ble_gatt_write_test_error = *error;
+
+    if (attr_len != NULL) {
+        TEST_ASSERT(error->status == 0);
+        TEST_ASSERT(attr->handle == 100);
+    }
+
+    ble_gatt_write_test_cb_called = 1;
+
+    return 0;
+}
+
+static void
+ble_gatt_write_test_rx_rsp(uint16_t conn_handle)
+{
+    uint8_t op;
+    int rc;
+
+    op = BLE_ATT_OP_WRITE_RSP;
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                &op, 1);
+    TEST_ASSERT(rc == 0);
+}
+
+static void
+ble_gatt_write_test_rx_prep_rsp(uint16_t conn_handle, uint16_t attr_handle,
+                                uint16_t offset,
+                                const void *attr_data, uint16_t attr_data_len)
+{
+    struct ble_att_prep_write_cmd rsp;
+    uint8_t buf[512];
+    int rc;
+
+    rsp.bapc_handle = attr_handle;
+    rsp.bapc_offset = offset;
+    ble_att_prep_write_rsp_write(buf, sizeof buf, &rsp);
+
+    memcpy(buf + BLE_ATT_PREP_WRITE_CMD_BASE_SZ, attr_data, attr_data_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_PREP_WRITE_CMD_BASE_SZ + attr_data_len);
+    TEST_ASSERT(rc == 0);
+}
+
+static void
+ble_gatt_write_test_rx_exec_rsp(uint16_t conn_handle)
+{
+    uint8_t op;
+    int rc;
+
+    op = BLE_ATT_OP_EXEC_WRITE_RSP;
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                &op, 1);
+    TEST_ASSERT(rc == 0);
+}
+
+static void
+ble_gatt_write_test_misc_long_good(int attr_len)
+{
+    uint16_t mtu;
+    int off;
+    int len;
+    int rc;
+
+    ble_gatt_write_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    mtu = ble_att_mtu(2);
+
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, 100, ble_gatt_write_test_attr_value, attr_len,
+        ble_gatt_write_test_cb_good, &attr_len);
+    TEST_ASSERT(rc == 0);
+
+    off = 0;
+    while (off < attr_len) {
+        len = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        if (off + len > attr_len) {
+            len = attr_len - off;
+        }
+
+        /* Send the pending ATT Prep Write Command. */
+        ble_hs_test_util_verify_tx_prep_write(
+            100, off, ble_gatt_write_test_attr_value + off, len);
+
+        /* Receive Prep Write response. */
+        ble_gatt_write_test_rx_prep_rsp(
+            2, 100, off, ble_gatt_write_test_attr_value + off, len);
+
+        /* Verify callback hasn't gotten called. */
+        TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+        off += len;
+    }
+
+    /* Verify execute write request sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_EXECUTE);
+
+    /* Receive Exec Write response. */
+    ble_gatt_write_test_rx_exec_rsp(2);
+
+    /* Verify callback got called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+}
+
+typedef void ble_gatt_write_test_long_fail_fn(uint16_t conn_handle,
+                                              int off, int len);
+
+static void
+ble_gatt_write_test_misc_long_bad(int attr_len,
+                                  ble_gatt_write_test_long_fail_fn *cb)
+{
+    uint16_t mtu;
+    int fail_now;
+    int off;
+    int len;
+    int rc;
+
+    ble_gatt_write_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+    mtu = ble_att_mtu(2);
+
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, 100, ble_gatt_write_test_attr_value, attr_len,
+        ble_gatt_write_test_cb_good, NULL);
+    TEST_ASSERT(rc == 0);
+
+    fail_now = 0;
+    off = 0;
+    while (off < attr_len) {
+        len = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        if (off + len > attr_len) {
+            len = attr_len - off;
+        }
+
+        /* Send the pending ATT Prep Write Command. */
+        ble_hs_test_util_verify_tx_prep_write(
+            100, off, ble_gatt_write_test_attr_value + off, len);
+
+        /* Receive Prep Write response. */
+        len = BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        if (off + len >= attr_len) {
+            len = attr_len - off;
+            fail_now = 1;
+        }
+        if (!fail_now) {
+            ble_gatt_write_test_rx_prep_rsp(
+                2, 100, off, ble_gatt_write_test_attr_value + off, len);
+        } else {
+            cb(2, off, len);
+            break;
+        }
+
+        /* Verify callback hasn't gotten called. */
+        TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+        off += len;
+    }
+
+    /* Verify callback was called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+    TEST_ASSERT(ble_gatt_write_test_error.status == BLE_HS_EBADDATA);
+    TEST_ASSERT(ble_gatt_write_test_error.att_handle == 0);
+}
+
+static void
+ble_gatt_write_test_misc_long_fail_handle(uint16_t conn_handle,
+                                          int off, int len)
+{
+    ble_gatt_write_test_rx_prep_rsp(
+        conn_handle, 99, off, ble_gatt_write_test_attr_value + off,
+        len);
+}
+
+static void
+ble_gatt_write_test_misc_long_fail_offset(uint16_t conn_handle,
+                                          int off, int len)
+{
+    ble_gatt_write_test_rx_prep_rsp(
+        conn_handle, 100, off + 1, ble_gatt_write_test_attr_value + off,
+        len);
+}
+
+static void
+ble_gatt_write_test_misc_long_fail_value(uint16_t conn_handle,
+                                         int off, int len)
+{
+    ble_gatt_write_test_rx_prep_rsp(
+        conn_handle, 100, off, ble_gatt_write_test_attr_value + off + 1,
+        len);
+}
+
+static void
+ble_gatt_write_test_misc_long_fail_length(uint16_t conn_handle,
+                                          int off, int len)
+{
+    ble_gatt_write_test_rx_prep_rsp(
+        conn_handle, 100, off, ble_gatt_write_test_attr_value + off,
+        len - 1);
+}
+
+static int
+ble_gatt_write_test_reliable_cb_good(uint16_t conn_handle,
+                                     const struct ble_gatt_error *error,
+                                     struct ble_gatt_attr *attrs,
+                                     uint8_t num_attrs, void *arg)
+{
+    int i;
+
+    TEST_ASSERT_FATAL(num_attrs <= BLE_GATT_WRITE_TEST_MAX_ATTRS);
+
+    TEST_ASSERT(conn_handle == 2);
+
+    ble_gatt_write_test_num_attrs = num_attrs;
+    for (i = 0; i < num_attrs; i++) {
+        ble_hs_test_util_attr_to_flat(ble_gatt_write_test_attrs + i,
+                                      attrs + i);
+    }
+
+    ble_gatt_write_test_cb_called = 1;
+
+    return 0;
+}
+
+static void
+ble_gatt_write_test_misc_reliable_good(
+    struct ble_hs_test_util_flat_attr *flat_attrs)
+{
+    const struct ble_hs_test_util_flat_attr *attr;
+    struct ble_gatt_attr attrs[16];
+    uint16_t mtu;
+    int num_attrs;
+    int attr_idx;
+    int len;
+    int off;
+    int rc;
+    int i;
+
+    ble_gatt_write_test_init();
+
+    for (num_attrs = 0; flat_attrs[num_attrs].handle != 0; num_attrs++) {
+        TEST_ASSERT_FATAL(num_attrs < sizeof attrs / sizeof attrs[0]);
+        ble_hs_test_util_attr_from_flat(attrs + num_attrs,
+                                        flat_attrs + num_attrs);
+    }
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+    mtu = ble_att_mtu(2);
+
+    rc = ble_gattc_write_reliable(2, attrs, num_attrs,
+                                  ble_gatt_write_test_reliable_cb_good, NULL);
+    TEST_ASSERT(rc == 0);
+
+    attr_idx = 0;
+    off = 0;
+    while (attr_idx < num_attrs) {
+        attr = flat_attrs + attr_idx;
+
+        len = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        if (off + len > attr->value_len) {
+            len = attr->value_len - off;
+        }
+
+        /* Send the pending ATT Prep Write Command. */
+        ble_hs_test_util_verify_tx_prep_write(attr->handle, off,
+                                              attr->value + off, len);
+
+        /* Receive Prep Write response. */
+        ble_gatt_write_test_rx_prep_rsp(2, attr->handle, off,
+                                        attr->value + off, len);
+
+        /* Verify callback hasn't gotten called. */
+        TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+        off += len;
+        if (off >= attr->value_len) {
+            attr_idx++;
+            off = 0;
+        }
+    }
+
+    /* Verify execute write request sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_EXECUTE);
+
+    /* Receive Exec Write response. */
+    ble_gatt_write_test_rx_exec_rsp(2);
+
+    /* Verify callback got called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+    TEST_ASSERT(ble_gatt_write_test_num_attrs == num_attrs);
+    for (i = 0; i < num_attrs; i++) {
+        rc = ble_hs_test_util_flat_attr_cmp(
+            ble_gatt_write_test_attrs + i, flat_attrs + i);
+        TEST_ASSERT(rc == 0);
+    }
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_no_rsp)
+{
+    int attr_len;
+    int rc;
+
+    ble_gatt_write_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    attr_len = 4;
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(
+        2, 100, ble_gatt_write_test_attr_value, attr_len);
+    TEST_ASSERT(rc == 0);
+
+    /* Send the pending ATT Write Command. */
+
+    /* No response expected; verify callback not called. */
+    TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_rsp)
+{
+    int attr_len;
+
+    ble_gatt_write_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    attr_len = 4;
+    ble_hs_test_util_gatt_write_flat(2, 100, ble_gatt_write_test_attr_value,
+                                     attr_len, ble_gatt_write_test_cb_good,
+                                     &attr_len);
+
+    /* Send the pending ATT Write Command. */
+
+    /* Response not received yet; verify callback not called. */
+    TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+    /* Receive write response. */
+    ble_gatt_write_test_rx_rsp(2);
+
+    /* Verify callback got called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_good)
+{
+    /*** 1 prep write req/rsp. */
+    ble_gatt_write_test_misc_long_good(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+
+    /*** 2 prep write reqs/rsps. */
+    ble_gatt_write_test_misc_long_good(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1);
+
+    /*** Maximum reqs/rsps. */
+    ble_gatt_write_test_misc_long_good(BLE_ATT_ATTR_MAX_LEN);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_bad_handle)
+{
+    /*** 1 prep write req/rsp. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+        ble_gatt_write_test_misc_long_fail_handle);
+
+    /*** 2 prep write reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1,
+        ble_gatt_write_test_misc_long_fail_handle);
+
+    /*** Maximum reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_ATTR_MAX_LEN,
+        ble_gatt_write_test_misc_long_fail_handle);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_bad_offset)
+{
+    /*** 1 prep write req/rsp. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+        ble_gatt_write_test_misc_long_fail_offset);
+
+    /*** 2 prep write reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1,
+        ble_gatt_write_test_misc_long_fail_offset);
+
+    /*** Maximum reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_ATTR_MAX_LEN,
+        ble_gatt_write_test_misc_long_fail_offset);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_bad_value)
+{
+    /*** 1 prep write req/rsp. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+        ble_gatt_write_test_misc_long_fail_value);
+
+    /*** 2 prep write reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1,
+        ble_gatt_write_test_misc_long_fail_value);
+
+    /*** Maximum reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_ATTR_MAX_LEN,
+        ble_gatt_write_test_misc_long_fail_value);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_bad_length)
+{
+    /*** 1 prep write req/rsp. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+        ble_gatt_write_test_misc_long_fail_length);
+
+    /*** 2 prep write reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1,
+        ble_gatt_write_test_misc_long_fail_length);
+
+    /*** Maximum reqs/rsps. */
+    ble_gatt_write_test_misc_long_bad(
+        BLE_ATT_ATTR_MAX_LEN,
+        ble_gatt_write_test_misc_long_fail_length);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_reliable_good)
+{
+    /*** 1 attribute. */
+    ble_gatt_write_test_misc_reliable_good(
+        ((struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 100,
+            .value_len = 2,
+            .value = { 1, 2 },
+        }, {
+            0
+        } }));
+
+    /*** 2 attributes. */
+    ble_gatt_write_test_misc_reliable_good(
+        ((struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 100,
+            .value_len = 2,
+            .value = { 1,2 },
+        }, {
+            .handle = 113,
+            .value_len = 6,
+            .value = { 5,6,7,8,9,10 },
+        }, {
+            0
+        } }));
+
+    /*** 3 attributes. */
+    ble_gatt_write_test_misc_reliable_good(
+        ((struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 100,
+            .value_len = 2,
+            .value = { 1,2 },
+        }, {
+            .handle = 113,
+            .value_len = 6,
+            .value = { 5,6,7,8,9,10 },
+        }, {
+            .handle = 144,
+            .value_len = 1,
+            .value = { 0xff },
+        }, {
+            0
+        } }));
+
+    /*** Long attributes. */
+    ble_gatt_write_test_misc_reliable_good(
+        ((struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 100,
+            .value_len = 20,
+            .value = { 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20 },
+        }, {
+            .handle = 144,
+            .value_len = 20,
+            .value = { 11,12,13,14,15,16,17,18,19,110,
+                       111,112,113,114,115,116,117,118,119,120 },
+        }, {
+            0
+        } }));
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_queue_full)
+{
+    int off;
+    int len;
+    int rc;
+    int i;
+
+    ble_gatt_write_test_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, 100, ble_gatt_write_test_attr_value, 128,
+        ble_gatt_write_test_cb_good, NULL);
+    TEST_ASSERT(rc == 0);
+
+    off = 0;
+    for (i = 0; i < 2; i++) {
+        /* Verify prep write request was sent. */
+        TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() != NULL);
+
+        /* Receive Prep Write response. */
+        len = BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        ble_gatt_write_test_rx_prep_rsp(
+            2, 100, off, ble_gatt_write_test_attr_value + off, len);
+
+        /* Verify callback hasn't gotten called. */
+        TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+        off += len;
+    }
+
+    /* Verify prep write request was sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() != NULL);
+
+    /* Receive queue full error. */
+    ble_hs_test_util_rx_att_err_rsp(2, BLE_ATT_OP_PREP_WRITE_REQ,
+                                    BLE_ATT_ERR_PREPARE_QUEUE_FULL, 100);
+
+    /* Verify callback was called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+    TEST_ASSERT(ble_gatt_write_test_error.status ==
+                BLE_HS_ATT_ERR(BLE_ATT_ERR_PREPARE_QUEUE_FULL));
+    TEST_ASSERT(ble_gatt_write_test_error.att_handle == 100);
+
+    /* Verify clear queue command got sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_CANCEL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_long_oom)
+{
+    static const struct ble_hs_test_util_flat_attr attr = {
+        .handle = 34,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20,
+        },
+        .value_len = 20,
+    };
+
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int chunk_sz;
+    int off;
+    int rc;
+
+    ble_gatt_write_test_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a write long procedure. */
+    off = 0;
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, attr.handle, attr.value, attr.value_len,
+        ble_gatt_write_test_cb_good, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    chunk_sz = ble_att_mtu(2) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+
+    ble_hs_test_util_verify_tx_prep_write(attr.handle, off,
+                                          attr.value + off, chunk_sz);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_write_test_rx_prep_rsp(2, attr.handle, off, attr.value + off,
+                                    chunk_sz);
+    off += chunk_sz;
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    chunk_sz = attr.value_len - off;
+    ble_hs_test_util_verify_tx_prep_write(attr.handle, off,
+                                          attr.value + off, chunk_sz);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_write_test_rx_prep_rsp(
+        2, attr.handle, off, attr.value + off, chunk_sz);
+    off += chunk_sz;
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify that procedure completes when mbufs are available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Verify execute write request sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_EXECUTE);
+
+    /* Receive Exec Write response. */
+    ble_gatt_write_test_rx_exec_rsp(2);
+
+    /* Verify callback got called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatt_write_test_reliable_oom)
+{
+    static const struct ble_hs_test_util_flat_attr attr = {
+        .handle = 34,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20,
+        },
+        .value_len = 20,
+    };
+
+    struct ble_gatt_attr mattr;
+    struct os_mbuf *oms;
+    int32_t ticks_until;
+    int chunk_sz;
+    int off;
+    int rc;
+
+    ble_gatt_write_test_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /* Initiate a write reliable procedure. */
+    ble_hs_test_util_attr_from_flat(&mattr, &attr);
+
+    off = 0;
+    rc = ble_gattc_write_reliable(2, &mattr, 1,
+                                  ble_gatt_write_test_reliable_cb_good, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    chunk_sz = ble_att_mtu(2) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+
+    ble_hs_test_util_verify_tx_prep_write(attr.handle, off,
+                                          attr.value + off, chunk_sz);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_write_test_rx_prep_rsp(2, attr.handle, off, attr.value + off,
+                                    chunk_sz);
+    off += chunk_sz;
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify the procedure proceeds after mbufs become available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    chunk_sz = attr.value_len - off;
+    ble_hs_test_util_verify_tx_prep_write(attr.handle, off,
+                                          attr.value + off, chunk_sz);
+
+    /* Exhaust the msys pool.  Leave one mbuf for the forthcoming response. */
+    oms = ble_hs_test_util_mbuf_alloc_all_but(1);
+    ble_gatt_write_test_rx_prep_rsp(
+        2, attr.handle, off, attr.value + off, chunk_sz);
+    off += chunk_sz;
+
+    /* Ensure no follow-up request got sent.  It should not have gotten sent
+     * due to mbuf exhaustion.
+     */
+    ble_hs_test_util_prev_tx_queue_clear();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue_pullup() == NULL);
+
+    /* Verify that we will resume the stalled GATT procedure in one second. */
+    ticks_until = ble_gattc_timer();
+    TEST_ASSERT(ticks_until == os_time_ms_to_ticks32(MYNEWT_VAL(BLE_GATT_RESUME_RATE)));
+
+    /* Verify that procedure completes when mbufs are available. */
+    rc = os_mbuf_free_chain(oms);
+    TEST_ASSERT_FATAL(rc == 0);
+    os_time_advance(ticks_until);
+    ble_gattc_timer();
+
+    /* Verify execute write request sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_EXECUTE);
+
+    /* Receive Exec Write response. */
+    ble_gatt_write_test_rx_exec_rsp(2);
+
+    /* Verify callback got called. */
+    TEST_ASSERT(ble_gatt_write_test_cb_called);
+    TEST_ASSERT(!ble_gattc_any_jobs());
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatt_write_test_suite)
+{
+    ble_gatt_write_test_no_rsp();
+    ble_gatt_write_test_rsp();
+    ble_gatt_write_test_long_good();
+    ble_gatt_write_test_long_bad_handle();
+    ble_gatt_write_test_long_bad_offset();
+    ble_gatt_write_test_long_bad_value();
+    ble_gatt_write_test_long_bad_length();
+    ble_gatt_write_test_long_queue_full();
+    ble_gatt_write_test_reliable_good();
+    ble_gatt_write_test_long_oom();
+    ble_gatt_write_test_reliable_oom();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_notify_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_notify_test.c
new file mode 100644
index 00000000..6e04ac36
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_notify_test.c
@@ -0,0 +1,1090 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_GATTS_NOTIFY_TEST_CHR_1_UUID    0x1111
+#define BLE_GATTS_NOTIFY_TEST_CHR_2_UUID    0x2222
+
+#define BLE_GATTS_NOTIFY_TEST_MAX_EVENTS    16
+
+static uint8_t ble_gatts_notify_test_peer_addr[6] = {2,3,4,5,6,7};
+
+static int
+ble_gatts_notify_test_misc_access(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg);
+static void
+ble_gatts_notify_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
+                                  void *arg);
+
+static const struct ble_gatt_svc_def ble_gatts_notify_test_svcs[] = { {
+    .type = BLE_GATT_SVC_TYPE_PRIMARY,
+    .uuid = BLE_UUID16_DECLARE(0x1234),
+    .characteristics = (struct ble_gatt_chr_def[]) { {
+        .uuid = BLE_UUID16_DECLARE(BLE_GATTS_NOTIFY_TEST_CHR_1_UUID),
+        .access_cb = ble_gatts_notify_test_misc_access,
+        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY |
+                 BLE_GATT_CHR_F_INDICATE,
+    }, {
+        .uuid = BLE_UUID16_DECLARE(BLE_GATTS_NOTIFY_TEST_CHR_2_UUID),
+        .access_cb = ble_gatts_notify_test_misc_access,
+        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY |
+                 BLE_GATT_CHR_F_INDICATE,
+    }, {
+        0
+    } },
+}, {
+    0
+} };
+
+static uint16_t ble_gatts_notify_test_chr_1_def_handle;
+static uint8_t ble_gatts_notify_test_chr_1_val[1024];
+static int ble_gatts_notify_test_chr_1_len;
+static uint16_t ble_gatts_notify_test_chr_2_def_handle;
+static uint8_t ble_gatts_notify_test_chr_2_val[1024];
+static int ble_gatts_notify_test_chr_2_len;
+
+static struct ble_gap_event
+ble_gatts_notify_test_events[BLE_GATTS_NOTIFY_TEST_MAX_EVENTS];
+
+static int ble_gatts_notify_test_num_events;
+
+typedef int ble_store_write_fn(int obj_type, const union ble_store_value *val);
+
+typedef int ble_store_delete_fn(int obj_type, const union ble_store_key *key);
+
+static int
+ble_gatts_notify_test_util_gap_event(struct ble_gap_event *event, void *arg)
+{
+    switch (event->type) {
+    case BLE_GAP_EVENT_NOTIFY_TX:
+    case BLE_GAP_EVENT_SUBSCRIBE:
+        TEST_ASSERT_FATAL(ble_gatts_notify_test_num_events <
+                          BLE_GATTS_NOTIFY_TEST_MAX_EVENTS);
+
+        ble_gatts_notify_test_events[ble_gatts_notify_test_num_events++] =
+            *event;
+
+    default:
+        break;
+    }
+
+    return 0;
+}
+
+static uint16_t
+ble_gatts_notify_test_misc_read_notify(uint16_t conn_handle,
+                                       uint16_t chr_def_handle)
+{
+    struct ble_att_read_req req;
+    struct os_mbuf *om;
+    uint8_t buf[BLE_ATT_READ_REQ_SZ];
+    uint16_t flags;
+    int rc;
+
+    req.barq_handle = chr_def_handle + 2;
+    ble_att_read_req_write(buf, sizeof buf, &req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT_FATAL(om->om_len == 3);
+    TEST_ASSERT_FATAL(om->om_data[0] == BLE_ATT_OP_READ_RSP);
+
+    flags = get_le16(om->om_data + 1);
+    return flags;
+}
+
+static void
+ble_gatts_notify_test_misc_try_enable_notify(uint16_t conn_handle,
+                                             uint16_t chr_def_handle,
+                                             uint16_t flags, int fail)
+{
+    struct ble_att_write_req req;
+    uint8_t buf[BLE_ATT_WRITE_REQ_BASE_SZ + 2];
+    int rc;
+
+    req.bawq_handle = chr_def_handle + 2;
+    ble_att_write_req_write(buf, sizeof buf, &req);
+
+    put_le16(buf + BLE_ATT_WRITE_REQ_BASE_SZ, flags);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    if (fail) {
+        TEST_ASSERT_FATAL(rc != 0);
+        ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
+                                           req.bawq_handle,
+                                           BLE_ATT_ERR_REQ_NOT_SUPPORTED);
+    } else {
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_hs_test_util_verify_tx_write_rsp();
+    }
+}
+
+static void
+ble_gatts_notify_test_misc_enable_notify(uint16_t conn_handle,
+                                         uint16_t chr_def_handle,
+                                         uint16_t flags)
+{
+    ble_gatts_notify_test_misc_try_enable_notify(conn_handle,
+                                                 chr_def_handle,
+                                                 flags, 0);
+}
+
+static void
+ble_gatts_notify_test_util_next_event(struct ble_gap_event *event)
+{
+    TEST_ASSERT_FATAL(ble_gatts_notify_test_num_events > 0);
+
+    *event = *ble_gatts_notify_test_events;
+
+    ble_gatts_notify_test_num_events--;
+    if (ble_gatts_notify_test_num_events > 0) {
+        memmove(ble_gatts_notify_test_events + 0,
+                ble_gatts_notify_test_events + 1,
+                ble_gatts_notify_test_num_events * sizeof *event);
+    }
+}
+
+static void
+ble_gatts_notify_test_util_verify_sub_event(uint16_t conn_handle,
+                                            uint8_t attr_handle,
+                                            uint8_t reason,
+                                            uint8_t prevn, uint8_t curn,
+                                            uint8_t previ, uint8_t curi)
+{
+    struct ble_gap_event event;
+
+    ble_gatts_notify_test_util_next_event(&event);
+
+    TEST_ASSERT(event.type == BLE_GAP_EVENT_SUBSCRIBE);
+    TEST_ASSERT(event.subscribe.conn_handle == conn_handle);
+    TEST_ASSERT(event.subscribe.attr_handle == attr_handle);
+    TEST_ASSERT(event.subscribe.reason == reason);
+    TEST_ASSERT(event.subscribe.prev_notify == prevn);
+    TEST_ASSERT(event.subscribe.cur_notify == curn);
+    TEST_ASSERT(event.subscribe.prev_indicate == previ);
+    TEST_ASSERT(event.subscribe.cur_indicate == curi);
+}
+
+static void
+ble_gatts_notify_test_util_verify_tx_event(uint16_t conn_handle,
+                                           uint8_t attr_handle,
+                                           int status,
+                                           int indication)
+{
+    struct ble_gap_event event;
+
+    ble_gatts_notify_test_util_next_event(&event);
+
+    TEST_ASSERT(event.type == BLE_GAP_EVENT_NOTIFY_TX);
+    TEST_ASSERT(event.notify_tx.status == status);
+    TEST_ASSERT(event.notify_tx.conn_handle == conn_handle);
+    TEST_ASSERT(event.notify_tx.attr_handle == attr_handle);
+    TEST_ASSERT(event.notify_tx.indication == indication);
+}
+
+static void
+ble_gatts_notify_test_util_verify_ack_event(uint16_t conn_handle,
+                                            uint8_t attr_handle)
+{
+    ble_gatts_notify_test_util_verify_tx_event(conn_handle, attr_handle,
+                                               BLE_HS_EDONE, 1);
+}
+
+static void
+ble_gatts_notify_test_misc_init(uint16_t *out_conn_handle, int bonding,
+                                uint16_t chr1_flags, uint16_t chr2_flags)
+{
+    struct ble_hs_conn *conn;
+    uint16_t flags;
+    int exp_num_cccds;
+
+    ble_hs_test_util_init();
+    ble_gatts_notify_test_num_events = 0;
+
+    ble_hs_test_util_reg_svcs(ble_gatts_notify_test_svcs,
+                              ble_gatts_notify_test_misc_reg_cb,
+                              NULL);
+    TEST_ASSERT_FATAL(ble_gatts_notify_test_chr_1_def_handle != 0);
+    TEST_ASSERT_FATAL(ble_gatts_notify_test_chr_2_def_handle != 0);
+
+    ble_hs_test_util_create_conn(2, ble_gatts_notify_test_peer_addr,
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+    *out_conn_handle = 2;
+
+    if (bonding) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find(2);
+        TEST_ASSERT_FATAL(conn != NULL);
+        conn->bhc_sec_state.encrypted = 1;
+        conn->bhc_sec_state.authenticated = 1;
+        conn->bhc_sec_state.bonded = 1;
+        ble_hs_unlock();
+    }
+
+    /* Ensure notifications disabled on new connection. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        2, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == 0);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        2, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == 0);
+
+    /* Set initial notification / indication state and verify that subscription
+     * callback gets executed.
+     */
+    if (chr1_flags != 0) {
+        ble_gatts_notify_test_misc_enable_notify(
+            2, ble_gatts_notify_test_chr_1_def_handle, chr1_flags);
+
+        ble_gatts_notify_test_util_verify_sub_event(
+            *out_conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_WRITE,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    }
+    if (chr2_flags != 0) {
+        ble_gatts_notify_test_misc_enable_notify(
+            2, ble_gatts_notify_test_chr_2_def_handle, chr2_flags);
+
+        ble_gatts_notify_test_util_verify_sub_event(
+            *out_conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_WRITE,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    }
+
+    /* Ensure no extraneous subscription callbacks were executed. */
+    TEST_ASSERT(ble_gatts_notify_test_num_events == 0);
+
+    /* Toss both write responses. */
+    ble_hs_test_util_prev_tx_queue_clear();
+
+    /* Ensure notification / indication state reads back correctly. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        2, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == chr1_flags);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        2, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == chr2_flags);
+
+    /* Ensure both CCCDs still persisted. */
+    if (bonding) {
+        exp_num_cccds = (chr1_flags != 0) + (chr2_flags != 0);
+    } else {
+        exp_num_cccds = 0;
+    }
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == exp_num_cccds);
+}
+
+static void
+ble_gatts_notify_test_disconnect(uint16_t conn_handle,
+                                 uint8_t chr1_flags,
+                                 uint8_t chr1_indicate_in_progress,
+                                 uint8_t chr2_flags,
+                                 uint8_t chr2_indicate_in_progress)
+{
+    ble_hs_test_util_conn_disconnect(conn_handle);
+
+    if (chr1_indicate_in_progress) {
+        ble_gatts_notify_test_util_verify_tx_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_HS_ENOTCONN,
+            1);
+    }
+
+    /* Verify subscription callback executed for each subscribed
+     * characteristic.
+     */
+    if (chr1_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_TERM,
+            chr1_flags == BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+            chr1_flags == BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+    }
+
+    if (chr2_indicate_in_progress) {
+        ble_gatts_notify_test_util_verify_tx_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_HS_ENOTCONN,
+            1);
+    }
+
+    if (chr2_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_TERM,
+            chr2_flags == BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+            chr2_flags == BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+    }
+}
+
+static void
+ble_gatts_notify_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
+                                  void *arg)
+{
+    uint16_t uuid16;
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_CHR) {
+        uuid16 = ble_uuid_u16(ctxt->chr.chr_def->uuid);
+        switch (uuid16) {
+        case BLE_GATTS_NOTIFY_TEST_CHR_1_UUID:
+            ble_gatts_notify_test_chr_1_def_handle = ctxt->chr.def_handle;
+            break;
+
+        case BLE_GATTS_NOTIFY_TEST_CHR_2_UUID:
+            ble_gatts_notify_test_chr_2_def_handle = ctxt->chr.def_handle;
+            break;
+
+        default:
+            TEST_ASSERT_FATAL(0);
+            break;
+        }
+    }
+}
+
+static int
+ble_gatts_notify_test_misc_access(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg)
+{
+    int rc;
+
+    TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    TEST_ASSERT(conn_handle == 0xffff);
+
+    if (attr_handle == ble_gatts_notify_test_chr_1_def_handle + 1) {
+        TEST_ASSERT(ctxt->chr ==
+                    &ble_gatts_notify_test_svcs[0].characteristics[0]);
+        rc = os_mbuf_copyinto(ctxt->om, 0, ble_gatts_notify_test_chr_1_val,
+                              ble_gatts_notify_test_chr_1_len);
+        TEST_ASSERT_FATAL(rc == 0);
+    } else if (attr_handle == ble_gatts_notify_test_chr_2_def_handle + 1) {
+        TEST_ASSERT(ctxt->chr ==
+                    &ble_gatts_notify_test_svcs[0].characteristics[1]);
+        rc = os_mbuf_copyinto(ctxt->om, 0, ble_gatts_notify_test_chr_2_val,
+                              ble_gatts_notify_test_chr_2_len);
+        TEST_ASSERT_FATAL(rc == 0);
+    } else {
+        TEST_ASSERT_FATAL(0);
+    }
+
+    return 0;
+}
+
+static void
+ble_gatts_notify_test_misc_rx_indicate_rsp(uint16_t conn_handle,
+                                           uint16_t attr_handle)
+{
+    uint8_t buf[BLE_ATT_INDICATE_RSP_SZ];
+    int rc;
+
+    ble_att_indicate_rsp_write(buf, sizeof buf);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_gatts_notify_test_util_verify_ack_event(conn_handle, attr_handle);
+}
+
+static void
+ble_gatts_notify_test_misc_verify_tx_n(uint16_t conn_handle,
+                                       uint16_t attr_handle,
+                                       const uint8_t *attr_data, int attr_len)
+{
+    struct ble_att_notify_req req;
+    struct os_mbuf *om;
+    int i;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    ble_att_notify_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.banq_handle == attr_handle);
+
+    for (i = 0; i < attr_len; i++) {
+        TEST_ASSERT(om->om_data[BLE_ATT_NOTIFY_REQ_BASE_SZ + i] ==
+                    attr_data[i]);
+    }
+
+    ble_gatts_notify_test_util_verify_tx_event(conn_handle, attr_handle, 0, 0);
+}
+
+static void
+ble_gatts_notify_test_misc_verify_tx_i(uint16_t conn_handle,
+                                       uint16_t attr_handle,
+                                       const uint8_t *attr_data, int attr_len)
+{
+    struct ble_att_indicate_req req;
+    struct os_mbuf *om;
+    int i;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    ble_att_indicate_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.baiq_handle == attr_handle);
+
+    for (i = 0; i < attr_len; i++) {
+        TEST_ASSERT(om->om_data[BLE_ATT_INDICATE_REQ_BASE_SZ + i] ==
+                    attr_data[i]);
+    }
+
+    ble_gatts_notify_test_util_verify_tx_event(conn_handle, attr_handle, 0, 1);
+}
+
+static void
+ble_gatts_notify_test_misc_verify_tx_gen(uint16_t conn_handle, int attr_idx,
+                                         uint8_t chr_flags)
+{
+    uint16_t attr_handle;
+    uint16_t attr_len;
+    void *attr_val;
+
+    switch (attr_idx) {
+    case 1:
+        attr_handle = ble_gatts_notify_test_chr_1_def_handle + 1;
+        attr_len = ble_gatts_notify_test_chr_1_len;
+        attr_val = ble_gatts_notify_test_chr_1_val;
+        break;
+
+    case 2:
+        attr_handle = ble_gatts_notify_test_chr_2_def_handle + 1;
+        attr_len = ble_gatts_notify_test_chr_2_len;
+        attr_val = ble_gatts_notify_test_chr_2_val;
+        break;
+
+    default:
+        TEST_ASSERT_FATAL(0);
+        break;
+    }
+
+    switch (chr_flags) {
+    case 0:
+        break;
+
+    case BLE_GATTS_CLT_CFG_F_NOTIFY:
+        ble_gatts_notify_test_misc_verify_tx_n(conn_handle, attr_handle,
+                                               attr_val, attr_len);
+        break;
+
+    case BLE_GATTS_CLT_CFG_F_INDICATE:
+        ble_gatts_notify_test_misc_verify_tx_i(conn_handle, attr_handle,
+                                               attr_val, attr_len);
+        break;
+
+    default:
+        TEST_ASSERT_FATAL(0);
+        break;
+    }
+}
+
+static void
+ble_gatts_notify_test_restore_bonding(uint16_t conn_handle,
+                                      uint8_t chr1_flags, uint8_t chr1_tx,
+                                      uint8_t chr2_flags, uint8_t chr2_tx)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_sec_state.encrypted = 1;
+    conn->bhc_sec_state.authenticated = 1;
+    conn->bhc_sec_state.bonded = 1;
+    ble_hs_unlock();
+
+    ble_gatts_bonding_restored(conn_handle);
+
+    /* Verify subscription callback executed for each subscribed
+     * characteristic.
+     */
+    if (chr1_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_RESTORE,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+
+    }
+    if (chr1_tx) {
+        ble_gatts_notify_test_misc_verify_tx_gen(conn_handle, 1, chr1_flags);
+    }
+
+    if (chr2_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_RESTORE,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    }
+    if (chr2_tx) {
+        ble_gatts_notify_test_misc_verify_tx_gen(conn_handle, 2, chr2_flags);
+    }
+}
+
+TEST_CASE_SELF(ble_gatts_notify_test_n)
+{
+    static const uint8_t fourbytes[] = { 1, 2, 3, 4 };
+    struct os_mbuf *om;
+    uint16_t conn_handle;
+    uint16_t flags;
+    int rc;
+
+    ble_gatts_notify_test_misc_init(&conn_handle, 0,
+                                    BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                    BLE_GATTS_CLT_CFG_F_NOTIFY);
+
+    /* Ensure notifications read back as enabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_NOTIFY);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_NOTIFY);
+
+    /* Verify custom notification data. */
+    om = ble_hs_mbuf_from_flat(fourbytes, sizeof fourbytes);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    rc = ble_gattc_notify_custom(conn_handle,
+                                 ble_gatts_notify_test_chr_1_def_handle + 1,
+                                 om);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatts_notify_test_misc_verify_tx_n(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        fourbytes,
+        sizeof fourbytes);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xab;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Verify notification sent properly. */
+    ble_gatts_notify_test_misc_verify_tx_n(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        ble_gatts_notify_test_chr_1_val,
+        ble_gatts_notify_test_chr_1_len);
+
+    /* Update characteristic 2's value. */
+    ble_gatts_notify_test_chr_2_len = 16;
+    memcpy(ble_gatts_notify_test_chr_2_val,
+           ((uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}), 16);
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    /* Verify notification sent properly. */
+    ble_gatts_notify_test_misc_verify_tx_n(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1,
+        ble_gatts_notify_test_chr_2_val,
+        ble_gatts_notify_test_chr_2_len);
+
+    /***
+     * Disconnect, modify characteristic values, and reconnect.  Ensure
+     * notifications are not sent and are no longer enabled.
+     */
+
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xdd;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Update characteristic 2's value. */
+    ble_gatts_notify_test_chr_2_len = 16;
+    memcpy(ble_gatts_notify_test_chr_2_val,
+           ((uint8_t[]){1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}), 16);
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    /* Ensure no notifications sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /* Ensure notifications disabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == 0);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_notify_test_i)
+{
+    static const uint8_t fourbytes[] = { 1, 2, 3, 4 };
+    struct os_mbuf *om;
+    uint16_t conn_handle;
+    uint16_t flags;
+    int rc;
+
+    ble_gatts_notify_test_misc_init(&conn_handle, 0,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE);
+
+    /* Verify custom indication data. */
+    om = ble_hs_mbuf_from_flat(fourbytes, sizeof fourbytes);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    rc = ble_gattc_indicate_custom(conn_handle,
+                                   ble_gatts_notify_test_chr_1_def_handle + 1,
+                                   om);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        fourbytes,
+        sizeof fourbytes);
+
+    /* Receive the confirmation for the indication. */
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xab;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Verify indication sent properly. */
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        ble_gatts_notify_test_chr_1_val,
+        ble_gatts_notify_test_chr_1_len);
+
+    /* Update characteristic 2's value. */
+    ble_gatts_notify_test_chr_2_len = 16;
+    memcpy(ble_gatts_notify_test_chr_2_val,
+           ((uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}), 16);
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    /* Verify the second indication doesn't get sent until the first is
+     * confirmed.
+     */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
+
+    /* Receive the confirmation for the first indication. */
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Verify indication sent properly. */
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1,
+        ble_gatts_notify_test_chr_2_val,
+        ble_gatts_notify_test_chr_2_len);
+
+    /* Receive the confirmation for the second indication. */
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    /* Verify no pending GATT jobs. */
+    TEST_ASSERT(!ble_gattc_any_jobs());
+
+    /***
+     * Disconnect, modify characteristic values, and reconnect.  Ensure
+     * indications are not sent and are no longer enabled.
+     */
+
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xdd;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Update characteristic 2's value. */
+    ble_gatts_notify_test_chr_2_len = 16;
+    memcpy(ble_gatts_notify_test_chr_2_val,
+           ((uint8_t[]){1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}), 16);
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    /* Ensure no indications sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /* Ensure indications disabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == 0);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_notify_test_bonded_n)
+{
+    uint16_t conn_handle;
+    uint16_t flags;
+
+    ble_gatts_notify_test_misc_init(&conn_handle, 1,
+                                    BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                    BLE_GATTS_CLT_CFG_F_NOTIFY);
+
+    /* Disconnect. */
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
+
+    /* Ensure both CCCDs still persisted. */
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == 2);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xdd;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Update characteristic 2's value. */
+    ble_gatts_notify_test_chr_2_len = 16;
+    memcpy(ble_gatts_notify_test_chr_2_val,
+           ((uint8_t[]){1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}), 16);
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    /* Reconnect; ensure notifications don't get sent while unbonded and that
+     * notifications appear disabled.
+     */
+
+    ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    ble_gatts_notify_test_num_events = 0;
+    /* Ensure no notifications sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /* Ensure notifications disabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == 0);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == 0);
+
+    /* Simulate a successful encryption procedure (bonding restoration). */
+    ble_gatts_notify_test_restore_bonding(conn_handle,
+                                          BLE_GATTS_CLT_CFG_F_NOTIFY, 1,
+                                          BLE_GATTS_CLT_CFG_F_NOTIFY, 1);
+
+    /* Ensure notifications enabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_NOTIFY);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_NOTIFY);
+
+    /* Ensure both CCCDs still persisted. */
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == 2);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_notify_test_bonded_i)
+{
+    uint16_t conn_handle;
+    uint16_t flags;
+
+    ble_gatts_notify_test_misc_init(&conn_handle, 1,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE);
+
+    /* Disconnect. */
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    /* Ensure both CCCDs still persisted. */
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == 2);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xab;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Update characteristic 2's value. */
+    ble_gatts_notify_test_chr_2_len = 16;
+    memcpy(ble_gatts_notify_test_chr_2_val,
+           ((uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}), 16);
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    /* Reconnect; ensure notifications don't get sent while unbonded and that
+     * notifications appear disabled.
+     */
+
+    ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    /* Ensure no indications sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /* Ensure notifications disabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == 0);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == 0);
+
+    /* Simulate a successful encryption procedure (bonding restoration). */
+    ble_gatts_notify_test_restore_bonding(conn_handle,
+                                          BLE_GATTS_CLT_CFG_F_INDICATE, 1,
+                                          BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    /* Verify the second indication doesn't get sent until the first is
+     * confirmed.
+     */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
+
+    /* Receive the confirmation for the first indication. */
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Verify indication sent properly. */
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1,
+        ble_gatts_notify_test_chr_2_val,
+        ble_gatts_notify_test_chr_2_len);
+
+    /* Receive the confirmation for the second indication. */
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1);
+
+    /* Verify no pending GATT jobs. */
+    TEST_ASSERT(!ble_gattc_any_jobs());
+
+    /* Ensure notifications enabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+
+    /* Ensure both CCCDs still persisted. */
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == 2);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_notify_test_bonded_i_no_ack)
+{
+    struct ble_store_value_cccd value_cccd;
+    struct ble_store_key_cccd key_cccd;
+    uint16_t conn_handle;
+    uint16_t flags;
+    int rc;
+
+    ble_gatts_notify_test_misc_init(&conn_handle, 1,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    /* Update characteristic 1's value. */
+    ble_gatts_notify_test_chr_1_len = 1;
+    ble_gatts_notify_test_chr_1_val[0] = 0xab;
+    ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Verify indication sent properly. */
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        ble_gatts_notify_test_chr_1_val,
+        ble_gatts_notify_test_chr_1_len);
+
+    /* Verify 'updated' state is still persisted. */
+    key_cccd.peer_addr = *BLE_ADDR_ANY;
+    key_cccd.chr_val_handle = ble_gatts_notify_test_chr_1_def_handle + 1;
+    key_cccd.idx = 0;
+
+    rc = ble_store_read_cccd(&key_cccd, &value_cccd);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(value_cccd.value_changed);
+
+    /* Disconnect. */
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 1, 0, 0);
+
+    /* Ensure CCCD still persisted. */
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == 1);
+
+    /* Reconnect. */
+    ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    /* Simulate a successful encryption procedure (bonding restoration). */
+    ble_gatts_notify_test_restore_bonding(conn_handle,
+                                          BLE_GATTS_CLT_CFG_F_INDICATE, 1,
+                                          0, 0);
+
+    /* Receive the confirmation for the indication. */
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
+
+    /* Verify no pending GATT jobs. */
+    TEST_ASSERT(!ble_gattc_any_jobs());
+
+    /* Ensure indication enabled. */
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
+    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    flags = ble_gatts_notify_test_misc_read_notify(
+        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
+    TEST_ASSERT(flags == 0);
+
+    /* Ensure CCCD still persisted. */
+    TEST_ASSERT(ble_hs_test_util_num_cccds() == 1);
+
+    /* Verify 'updated' state is no longer persisted. */
+    rc = ble_store_read_cccd(&key_cccd, &value_cccd);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(!value_cccd.value_changed);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_notify_test_disallowed)
+{
+    uint16_t chr1_val_handle;
+    uint16_t chr2_val_handle;
+    uint16_t chr3_val_handle;
+
+    const struct ble_gatt_svc_def svcs[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(1),
+            .access_cb = ble_gatts_notify_test_misc_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
+            .val_handle = &chr1_val_handle,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(2),
+            .access_cb = ble_gatts_notify_test_misc_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_INDICATE,
+            .val_handle = &chr2_val_handle,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(3),
+            .access_cb = ble_gatts_notify_test_misc_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY |
+                     BLE_GATT_CHR_F_INDICATE,
+            .val_handle = &chr3_val_handle,
+        }, {
+            0
+        } },
+    }, {
+        0
+    } };
+
+    ble_hs_test_util_init();
+
+    ble_hs_test_util_reg_svcs(svcs, NULL, NULL);
+    TEST_ASSERT_FATAL(chr1_val_handle != 0);
+    TEST_ASSERT_FATAL(chr2_val_handle != 0);
+    TEST_ASSERT_FATAL(chr3_val_handle != 0);
+
+    ble_hs_test_util_create_conn(2, ble_gatts_notify_test_peer_addr,
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    /* Attempt to enable notifications on chr1 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr1_val_handle - 1, BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
+
+    /* Attempt to enable indications on chr1 should fail. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr1_val_handle - 1, BLE_GATTS_CLT_CFG_F_INDICATE, 1);
+
+    /* Attempt to enable notifications on chr2 should fail. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr2_val_handle - 1, BLE_GATTS_CLT_CFG_F_NOTIFY, 1);
+
+    /* Attempt to enable indications on chr2 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr2_val_handle - 1, BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    /* Attempt to enable notifications on chr3 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr3_val_handle - 1, BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
+
+    /* Attempt to enable indications on chr3 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr3_val_handle - 1, BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatts_notify_suite)
+{
+    ble_gatts_notify_test_n();
+    ble_gatts_notify_test_i();
+
+    ble_gatts_notify_test_bonded_n();
+    ble_gatts_notify_test_bonded_i();
+
+    ble_gatts_notify_test_bonded_i_no_ack();
+
+    ble_gatts_notify_test_disallowed();
+
+    /* XXX: Test corner cases:
+     *     o Bonding after CCCD configuration.
+     *     o Disconnect prior to rx of indicate ack.
+     */
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_read_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_read_test.c
new file mode 100644
index 00000000..381b39ad
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_read_test.c
@@ -0,0 +1,257 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_GATTS_READ_TEST_CHR_1_UUID    0x1111
+#define BLE_GATTS_READ_TEST_CHR_2_UUID    0x2222
+
+static uint8_t ble_gatts_read_test_peer_addr[6] = {2,3,4,5,6,7};
+
+static int
+ble_gatts_read_test_util_access_1(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg);
+
+static int
+ble_gatts_read_test_util_access_2(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg);
+static void
+ble_gatts_read_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
+                                void *arg);
+
+static const struct ble_gatt_svc_def ble_gatts_read_test_svcs[] = { {
+    .type = BLE_GATT_SVC_TYPE_PRIMARY,
+    .uuid = BLE_UUID16_DECLARE(0x1234),
+    .characteristics = (struct ble_gatt_chr_def[]) { {
+        .uuid = BLE_UUID16_DECLARE(BLE_GATTS_READ_TEST_CHR_1_UUID),
+        .access_cb = ble_gatts_read_test_util_access_1,
+        .flags = BLE_GATT_CHR_F_READ
+    }, {
+        .uuid = BLE_UUID16_DECLARE(BLE_GATTS_READ_TEST_CHR_2_UUID),
+        .access_cb = ble_gatts_read_test_util_access_2,
+        .flags = BLE_GATT_CHR_F_READ
+    }, {
+        0
+    } },
+}, {
+    0
+} };
+
+static uint16_t ble_gatts_read_test_chr_1_def_handle;
+static uint16_t ble_gatts_read_test_chr_1_val_handle;
+static uint8_t ble_gatts_read_test_chr_1_val[1024];
+static int ble_gatts_read_test_chr_1_len;
+static uint16_t ble_gatts_read_test_chr_2_def_handle;
+static uint16_t ble_gatts_read_test_chr_2_val_handle;
+
+static void
+ble_gatts_read_test_misc_init(uint16_t *out_conn_handle)
+{
+    ble_hs_test_util_init();
+
+    ble_hs_test_util_reg_svcs(ble_gatts_read_test_svcs,
+                              ble_gatts_read_test_misc_reg_cb,
+                              NULL);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_1_def_handle != 0);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_1_val_handle ==
+                      ble_gatts_read_test_chr_1_def_handle + 1);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_2_def_handle != 0);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_2_val_handle ==
+                      ble_gatts_read_test_chr_2_def_handle + 1);
+
+    ble_hs_test_util_create_conn(2, ble_gatts_read_test_peer_addr, NULL, NULL);
+
+    if (out_conn_handle != NULL) {
+        *out_conn_handle = 2;
+    }
+}
+
+static void
+ble_gatts_read_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
+                                void *arg)
+{
+    uint16_t uuid16;
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_CHR) {
+        uuid16 = ble_uuid_u16(ctxt->chr.chr_def->uuid);
+        switch (uuid16) {
+        case BLE_GATTS_READ_TEST_CHR_1_UUID:
+            ble_gatts_read_test_chr_1_def_handle = ctxt->chr.def_handle;
+            ble_gatts_read_test_chr_1_val_handle = ctxt->chr.val_handle;
+            break;
+
+        case BLE_GATTS_READ_TEST_CHR_2_UUID:
+            ble_gatts_read_test_chr_2_def_handle = ctxt->chr.def_handle;
+            ble_gatts_read_test_chr_2_val_handle = ctxt->chr.val_handle;
+            break;
+
+        default:
+            TEST_ASSERT_FATAL(0);
+            break;
+        }
+    }
+}
+
+static int
+ble_gatts_read_test_util_access_1(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg)
+{
+    int rc;
+
+    TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    TEST_ASSERT_FATAL(attr_handle == ble_gatts_read_test_chr_1_val_handle);
+
+    TEST_ASSERT(ctxt->chr ==
+                &ble_gatts_read_test_svcs[0].characteristics[0]);
+
+    rc = os_mbuf_append(ctxt->om, ble_gatts_read_test_chr_1_val,
+                        ble_gatts_read_test_chr_1_len);
+    TEST_ASSERT(rc == 0);
+
+    return 0;
+}
+
+static int
+ble_gatts_read_test_util_access_2(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg)
+{
+    uint8_t *buf;
+
+    TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    TEST_ASSERT_FATAL(attr_handle == ble_gatts_read_test_chr_2_def_handle + 1);
+
+    TEST_ASSERT(ctxt->chr ==
+                &ble_gatts_read_test_svcs[0].characteristics[1]);
+
+    buf = os_mbuf_extend(ctxt->om, 6);
+    TEST_ASSERT_FATAL(buf != NULL);
+
+    buf[0] = 0;
+    buf[1] = 10;
+    buf[2] = 20;
+    buf[3] = 30;
+    buf[4] = 40;
+    buf[5] = 50;
+
+    return 0;
+}
+
+static void
+ble_gatts_read_test_once(uint16_t conn_handle, uint16_t attr_id,
+                         void *expected_value, uint16_t expected_len)
+{
+    struct ble_att_read_req read_req;
+    uint8_t buf[BLE_ATT_READ_REQ_SZ];
+    int rc;
+
+    read_req.barq_handle = attr_id;
+    ble_att_read_req_write(buf, sizeof buf, &read_req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_verify_tx_read_rsp(expected_value, expected_len);
+}
+
+TEST_CASE_SELF(ble_gatts_read_test_case_basic)
+{
+    uint16_t conn_handle;
+
+    ble_gatts_read_test_misc_init(&conn_handle);
+
+    /*** Application points attribute at static data. */
+    ble_gatts_read_test_chr_1_val[0] = 1;
+    ble_gatts_read_test_chr_1_val[1] = 2;
+    ble_gatts_read_test_chr_1_val[2] = 3;
+    ble_gatts_read_test_chr_1_len = 3;
+    ble_gatts_read_test_once(conn_handle,
+                             ble_gatts_read_test_chr_1_val_handle,
+                             ble_gatts_read_test_chr_1_val,
+                             ble_gatts_read_test_chr_1_len);
+
+    /*** Application uses stack-provided buffer for dynamic attribute. */
+    ble_gatts_read_test_once(conn_handle,
+                             ble_gatts_read_test_chr_2_def_handle + 1,
+                             ((uint8_t[6]){0,10,20,30,40,50}), 6);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_read_test_case_long)
+{
+    struct ble_att_read_blob_req read_blob_req;
+    struct ble_att_read_req read_req;
+    uint8_t buf[max(BLE_ATT_READ_REQ_SZ, BLE_ATT_READ_BLOB_REQ_SZ)];
+    uint16_t conn_handle;
+    int rc;
+    int i;
+
+    ble_gatts_read_test_misc_init(&conn_handle);
+
+    /*** Prepare characteristic value. */
+    ble_gatts_read_test_chr_1_len = 40;
+    for (i = 0; i < ble_gatts_read_test_chr_1_len; i++) {
+        ble_gatts_read_test_chr_1_val[i] = i;
+    }
+
+    /* Receive first read request. */
+    read_req.barq_handle = ble_gatts_read_test_chr_1_val_handle;
+    ble_att_read_req_write(buf, sizeof buf, &read_req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_verify_tx_read_rsp(ble_gatts_read_test_chr_1_val, 22);
+
+    /* Receive follow-up read blob request. */
+    read_blob_req.babq_handle = ble_gatts_read_test_chr_1_val_handle;
+    read_blob_req.babq_offset = 22;
+    ble_att_read_blob_req_write(buf, sizeof buf, &read_blob_req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure response starts at appropriate offset (22). */
+    ble_hs_test_util_verify_tx_read_blob_rsp(
+        ble_gatts_read_test_chr_1_val + 22, 18);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatts_read_test_suite)
+{
+    ble_gatts_read_test_case_basic();
+    ble_gatts_read_test_case_long();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_reg_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_reg_test.c
new file mode 100644
index 00000000..ea69028e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_gatts_reg_test.c
@@ -0,0 +1,719 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_GATTS_REG_TEST_MAX_ENTRIES  256
+
+struct ble_gatts_reg_test_entry {
+    uint8_t op;
+    ble_uuid_any_t uuid;
+    uint16_t handle;
+    uint16_t val_handle; /* If a characteristic. */
+
+    const struct ble_gatt_svc_def *svc;
+    const struct ble_gatt_chr_def *chr;
+    const struct ble_gatt_dsc_def *dsc;
+};
+
+static struct ble_gatts_reg_test_entry
+ble_gatts_reg_test_entries[BLE_GATTS_REG_TEST_MAX_ENTRIES];
+
+static int ble_gatts_reg_test_num_entries;
+
+static void
+ble_gatts_reg_test_init(void)
+{
+    ble_hs_test_util_init();
+    ble_gatts_reg_test_num_entries = 0;
+}
+
+static void
+ble_gatts_reg_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
+{
+    struct ble_gatts_reg_test_entry *entry;
+
+    TEST_ASSERT_FATAL(ble_gatts_reg_test_num_entries <
+                      BLE_GATTS_REG_TEST_MAX_ENTRIES);
+
+    entry = ble_gatts_reg_test_entries + ble_gatts_reg_test_num_entries++;
+    memset(entry, 0, sizeof *entry);
+
+    entry->op = ctxt->op;
+    switch (ctxt->op) {
+    case BLE_GATT_REGISTER_OP_SVC:
+        ble_uuid_to_any(ctxt->svc.svc_def->uuid, &entry->uuid);
+        entry->handle = ctxt->svc.handle;
+        entry->svc = ctxt->svc.svc_def;
+        break;
+
+    case BLE_GATT_REGISTER_OP_CHR:
+        ble_uuid_to_any(ctxt->chr.chr_def->uuid, &entry->uuid);
+        entry->handle = ctxt->chr.def_handle;
+        entry->val_handle = ctxt->chr.val_handle;
+        entry->svc = ctxt->chr.svc_def;
+        entry->chr = ctxt->chr.chr_def;
+        break;
+
+    case BLE_GATT_REGISTER_OP_DSC:
+        ble_uuid_to_any(ctxt->dsc.dsc_def->uuid, &entry->uuid);
+        entry->handle = ctxt->dsc.handle;
+        entry->svc = ctxt->dsc.svc_def;
+        entry->chr = ctxt->dsc.chr_def;
+        entry->dsc = ctxt->dsc.dsc_def;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+}
+
+static void
+ble_gatts_reg_test_misc_lookup_good(struct ble_gatts_reg_test_entry *entry)
+{
+    uint16_t chr_def_handle;
+    uint16_t chr_val_handle;
+    uint16_t svc_handle;
+    uint16_t dsc_handle;
+    int rc;
+
+    switch (entry->op) {
+    case BLE_GATT_REGISTER_OP_SVC:
+        rc = ble_gatts_find_svc(&entry->uuid.u, &svc_handle);
+        TEST_ASSERT_FATAL(rc == 0);
+        TEST_ASSERT(svc_handle == entry->handle);
+        break;
+
+    case BLE_GATT_REGISTER_OP_CHR:
+        rc = ble_gatts_find_chr(entry->svc->uuid, entry->chr->uuid,
+                                &chr_def_handle, &chr_val_handle);
+        TEST_ASSERT_FATAL(rc == 0);
+        TEST_ASSERT(chr_def_handle == entry->handle);
+        TEST_ASSERT(chr_val_handle == entry->val_handle);
+        break;
+
+    case BLE_GATT_REGISTER_OP_DSC:
+        rc = ble_gatts_find_dsc(entry->svc->uuid, entry->chr->uuid,
+                                entry->dsc->uuid, &dsc_handle);
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+}
+
+static void
+ble_gatts_reg_test_misc_lookup_bad(struct ble_gatts_reg_test_entry *entry)
+{
+    struct ble_gatts_reg_test_entry *cur;
+    ble_uuid_any_t wrong_uuid;
+    int rc;
+    int i;
+
+    switch (entry->op) {
+    case BLE_GATT_REGISTER_OP_SVC:
+        /* Wrong service UUID. */
+        ble_uuid_to_any(entry->svc->uuid, &wrong_uuid);
+        wrong_uuid.u16.value++;
+        rc = ble_gatts_find_svc(&wrong_uuid.u, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+        break;
+
+    case BLE_GATT_REGISTER_OP_CHR:
+        /* Correct service UUID, wrong characteristic UUID. */
+        ble_uuid_to_any(entry->chr->uuid, &wrong_uuid);
+        wrong_uuid.u16.value++;
+        rc = ble_gatts_find_chr(entry->svc->uuid, &wrong_uuid.u, NULL, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        /* Incorrect service UUID, correct characteristic UUID. */
+        ble_uuid_to_any(entry->svc->uuid, &wrong_uuid);
+        wrong_uuid.u16.value++;
+        rc = ble_gatts_find_chr(&wrong_uuid.u, entry->chr->uuid, NULL, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        /* Existing (but wrong) service, correct characteristic UUID. */
+        for (i = 0; i < ble_gatts_reg_test_num_entries; i++) {
+            cur = ble_gatts_reg_test_entries + i;
+            switch (cur->op) {
+            case BLE_GATT_REGISTER_OP_SVC:
+                if (cur->svc != entry->svc) {
+                    rc = ble_gatts_find_chr(cur->svc->uuid,
+                                            entry->chr->uuid,
+                                            NULL, NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_CHR:
+                /* Characteristic that isn't in this service. */
+                if (cur->svc != entry->svc) {
+                    rc = ble_gatts_find_chr(entry->svc->uuid,
+                                            cur->chr->uuid,
+                                            NULL, NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_DSC:
+                /* Use descriptor UUID instead of characteristic UUID. */
+                rc = ble_gatts_find_chr(entry->svc->uuid,
+                                        cur->dsc->uuid,
+                                        NULL, NULL);
+                TEST_ASSERT(rc == BLE_HS_ENOENT);
+                break;
+
+            default:
+                TEST_ASSERT(0);
+                break;
+            }
+        }
+        break;
+
+    case BLE_GATT_REGISTER_OP_DSC:
+        /* Correct svc/chr UUID, wrong dsc UUID. */
+        ble_uuid_to_any(entry->dsc->uuid, &wrong_uuid);
+        wrong_uuid.u128.value[15]++;
+        rc = ble_gatts_find_dsc(entry->svc->uuid, entry->chr->uuid,
+                                &wrong_uuid.u, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        /* Incorrect svc UUID, correct chr/dsc UUID. */
+        ble_uuid_to_any(entry->svc->uuid, &wrong_uuid);
+        wrong_uuid.u128.value[15]++;
+        rc = ble_gatts_find_dsc(&wrong_uuid.u, entry->chr->uuid,
+                                entry->dsc->uuid, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        for (i = 0; i < ble_gatts_reg_test_num_entries; i++) {
+            cur = ble_gatts_reg_test_entries + i;
+            switch (cur->op) {
+            case BLE_GATT_REGISTER_OP_SVC:
+                /* Existing (but wrong) svc, correct chr/dsc UUID. */
+                if (cur->svc != entry->svc) {
+                    rc = ble_gatts_find_dsc(cur->svc->uuid,
+                                            entry->chr->uuid,
+                                            entry->dsc->uuid,
+                                            NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_CHR:
+                /* Existing (but wrong) svc/chr, correct dsc UUID. */
+                if (cur->chr != entry->chr) {
+                    rc = ble_gatts_find_dsc(cur->svc->uuid,
+                                            cur->chr->uuid,
+                                            entry->dsc->uuid,
+                                            NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_DSC:
+                /* Descriptor that isn't in this characteristic. */
+                if (cur->chr != entry->chr) {
+                    rc = ble_gatts_find_dsc(cur->svc->uuid,
+                                            cur->chr->uuid,
+                                            entry->dsc->uuid,
+                                            NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            default:
+                TEST_ASSERT(0);
+                break;
+            }
+        }
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+}
+
+static void
+ble_gatts_reg_test_misc_verify_entry(uint8_t op, const ble_uuid_t *uuid)
+{
+    struct ble_gatts_reg_test_entry *entry;
+    int i;
+
+    for (i = 0; i < ble_gatts_reg_test_num_entries; i++) {
+        entry = ble_gatts_reg_test_entries + i;
+        if (entry->op == op && ble_uuid_cmp(&entry->uuid.u, uuid) == 0) {
+            break;
+        }
+    }
+    TEST_ASSERT_FATAL(entry != NULL);
+
+    /* Verify that characteristic value handle was properly assigned at
+     * registration.
+     */
+    if (op == BLE_GATT_REGISTER_OP_CHR) {
+        TEST_ASSERT(*entry->chr->val_handle == entry->val_handle);
+    }
+
+    /* Verify that the entry can be looked up. */
+    ble_gatts_reg_test_misc_lookup_good(entry);
+
+    /* Verify that "barely incorrect" UUID information doesn't retrieve any
+     * handles.
+     */
+    ble_gatts_reg_test_misc_lookup_bad(entry);
+}
+
+static int
+ble_gatts_reg_test_misc_dummy_access(uint16_t conn_handle,
+                                     uint16_t attr_handle,
+                                     struct ble_gatt_access_ctxt *ctxt,
+                                     void *arg)
+{
+    return 0;
+}
+
+TEST_CASE_SELF(ble_gatts_reg_test_svc_return)
+{
+    int rc;
+
+    /*** Missing UUID. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_no_uuid[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_no_uuid, NULL, NULL);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Circular dependency. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_circ[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .includes = (const struct ble_gatt_svc_def*[]) { svcs_circ + 1, NULL },
+    }, {
+        .type = BLE_GATT_SVC_TYPE_SECONDARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .includes = (const struct ble_gatt_svc_def*[]) { svcs_circ + 0, NULL },
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_circ, NULL, NULL);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Success. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_good[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .includes = (const struct ble_gatt_svc_def*[]) { svcs_good + 1, NULL },
+    }, {
+        .type = BLE_GATT_SVC_TYPE_SECONDARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_good, NULL, NULL);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_reg_test_chr_return)
+{
+    int rc;
+
+    /*** Missing callback. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_no_chr_cb[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            0
+        } },
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_no_chr_cb, NULL, NULL);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Success. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_good[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            0
+        } },
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_good, NULL, NULL);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_reg_test_dsc_return)
+{
+    int rc;
+
+    /*** Missing callback. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_no_dsc_cb[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid = BLE_UUID16_DECLARE(0x8888),
+                .att_flags = 5,
+            }, {
+                0
+            } },
+        }, {
+            0
+        } },
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_no_dsc_cb, NULL, NULL);
+    TEST_ASSERT(rc == BLE_HS_EINVAL);
+
+    /*** Success. */
+    ble_gatts_reg_test_init();
+    struct ble_gatt_svc_def svcs_good[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid = BLE_UUID16_DECLARE(0x8888),
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+                .att_flags = 5,
+            }, {
+                0
+            } },
+        }, {
+            0
+        } },
+    }, {
+        0
+    } };
+
+    rc = ble_gatts_register_svcs(svcs_good, NULL, NULL);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+ble_gatts_reg_test_misc_svcs(struct ble_gatt_svc_def *svcs)
+{
+    const struct ble_gatt_svc_def *svc;
+    const struct ble_gatt_chr_def *chr;
+    const struct ble_gatt_dsc_def *dsc;
+    int rc;
+
+    ble_gatts_reg_test_init();
+
+    /* Register all the attributes. */
+    rc = ble_gatts_register_svcs(svcs, ble_gatts_reg_test_misc_reg_cb,
+                                 NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Verify that the appropriate callbacks were executed. */
+    for (svc = svcs; svc->type != BLE_GATT_SVC_TYPE_END; svc++) {
+        ble_gatts_reg_test_misc_verify_entry(BLE_GATT_REGISTER_OP_SVC,
+                                             svc->uuid);
+
+        if (svc->characteristics != NULL) {
+            for (chr = svc->characteristics; chr->uuid != NULL; chr++) {
+                ble_gatts_reg_test_misc_verify_entry(BLE_GATT_REGISTER_OP_CHR,
+                                                     chr->uuid);
+
+                if (chr->descriptors != NULL) {
+                    for (dsc = chr->descriptors; dsc->uuid != NULL; dsc++) {
+                        ble_gatts_reg_test_misc_verify_entry(
+                            BLE_GATT_REGISTER_OP_DSC, dsc->uuid);
+                    }
+                }
+            }
+        }
+    }
+}
+
+TEST_CASE_SELF(ble_gatts_reg_test_svc_cb)
+{
+    /*** 1 primary. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+    }, {
+        0
+    } });
+
+    /*** 3 primary. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+    }, {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x2234),
+    }, {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x3234),
+    }, {
+        0
+    } });
+
+    /*** 1 primary, 1 secondary. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+    }, {
+        .type = BLE_GATT_SVC_TYPE_SECONDARY,
+        .uuid = BLE_UUID16_DECLARE(0x2222),
+    }, {
+        0
+    } });
+
+    /*** 1 primary, 1 secondary, 1 include. */
+    struct ble_gatt_svc_def svcs[] = {
+        [0] = {
+            .type = BLE_GATT_SVC_TYPE_PRIMARY,
+            .uuid = BLE_UUID16_DECLARE(0x1234),
+            .includes = (const struct ble_gatt_svc_def*[]) { svcs + 1, NULL, },
+        },
+        [1] = {
+            .type = BLE_GATT_SVC_TYPE_SECONDARY,
+            .uuid = BLE_UUID16_DECLARE(0x2222),
+        }, {
+            0
+        }
+    };
+    ble_gatts_reg_test_misc_svcs(svcs);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_reg_test_chr_cb)
+{
+    uint16_t val_handles[16];
+
+    /*** 1 characteristic. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
+        }, {
+            0
+        } },
+    }, {
+        0
+    } });
+
+    /*** 3 characteristics. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
+        }, {
+            .uuid = BLE_UUID16_DECLARE(0x2222),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_WRITE,
+            .val_handle = val_handles + 1,
+        }, {
+            0
+        } },
+    }, {
+        .type = BLE_GATT_SVC_TYPE_SECONDARY,
+        .uuid = BLE_UUID16_DECLARE(0x5678),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x3333),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 2,
+        }, {
+            0
+        } },
+    }, {
+        0
+    } });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_gatts_reg_test_dsc_cb)
+{
+    uint16_t val_handles[16];
+
+    /*** 1 descriptor. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid = BLE_UUID16_DECLARE(0x111a),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                0
+            } },
+        }, {
+            0
+        } },
+    }, {
+        0
+    } });
+
+    /*** 5+ descriptors. */
+    ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x1111),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid = BLE_UUID16_DECLARE(0x111a),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                0
+            } },
+        }, {
+            .uuid = BLE_UUID16_DECLARE(0x2222),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_WRITE,
+            .val_handle = val_handles + 1,
+        }, {
+            0
+        } },
+    }, {
+        .type = BLE_GATT_SVC_TYPE_SECONDARY,
+        .uuid = BLE_UUID16_DECLARE(0x5678),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid = BLE_UUID16_DECLARE(0x3333),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 2,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid = BLE_UUID16_DECLARE(0x333a),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid = BLE_UUID16_DECLARE(0x333b),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid = BLE_UUID16_DECLARE(0x333c),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid = BLE_UUID16_DECLARE(0x333e),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                0
+            } },
+        }, {
+            .uuid = BLE_UUID16_DECLARE(0x4444),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 3,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid = BLE_UUID16_DECLARE(0x444a),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid = BLE_UUID16_DECLARE(0x444b),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid = BLE_UUID16_DECLARE(0x444c),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid = BLE_UUID16_DECLARE(0x444e),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                0
+            } },
+        }, {
+            0
+        } },
+    }, {
+        0
+    } });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_gatts_reg_suite)
+{
+    ble_gatts_reg_test_svc_return();
+    ble_gatts_reg_test_chr_return();
+    ble_gatts_reg_test_dsc_return();
+
+    ble_gatts_reg_test_svc_cb();
+    ble_gatts_reg_test_chr_cb();
+    ble_gatts_reg_test_dsc_cb();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_adv_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_adv_test.c
new file mode 100644
index 00000000..b267dc2a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_adv_test.c
@@ -0,0 +1,1280 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_ADV_TEST_DATA_OFF   4
+
+#define BLE_HCI_SET_ADV_DATA_LEN            (32)
+#define BLE_HCI_SET_SCAN_RSP_DATA_LEN       (32)
+
+static void
+ble_hs_adv_test_misc_verify_tx_adv_data_hdr(uint8_t *cmd, int data_len)
+{
+    uint16_t opcode;
+
+    opcode = get_le16(cmd + 0);
+    TEST_ASSERT(BLE_HCI_OGF(opcode) == BLE_HCI_OGF_LE);
+    TEST_ASSERT(BLE_HCI_OCF(opcode) == BLE_HCI_OCF_LE_SET_ADV_DATA);
+
+    TEST_ASSERT(cmd[2] == BLE_HCI_SET_ADV_DATA_LEN);
+    TEST_ASSERT(cmd[3] == data_len);
+}
+
+static void
+ble_hs_adv_test_misc_verify_tx_rsp_data_hdr(uint8_t *cmd, int data_len)
+{
+    uint16_t opcode;
+
+    opcode = get_le16(cmd + 0);
+    TEST_ASSERT(BLE_HCI_OGF(opcode) == BLE_HCI_OGF_LE);
+    TEST_ASSERT(BLE_HCI_OCF(opcode) == BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA);
+
+    TEST_ASSERT(cmd[2] == BLE_HCI_SET_SCAN_RSP_DATA_LEN);
+    TEST_ASSERT(cmd[3] == data_len);
+}
+
+static void
+ble_hs_adv_test_misc_verify_tx_field(uint8_t *cmd, uint8_t type,
+                                     uint8_t val_len, void *val)
+{
+    TEST_ASSERT(cmd[0] == val_len + 1);
+    TEST_ASSERT(cmd[1] == type);
+    TEST_ASSERT(memcmp(cmd + 2, val, val_len) == 0);
+}
+
+struct ble_hs_adv_test_field {
+    uint8_t type;       /* 0 indicates end of array. */
+    uint8_t *val;
+    uint8_t val_len;
+};
+
+static int
+ble_hs_adv_test_misc_calc_data_len(struct ble_hs_adv_test_field *fields)
+{
+    struct ble_hs_adv_test_field *field;
+    int len;
+
+    len = 0;
+    if (fields != NULL) {
+        for (field = fields; field->type != 0; field++) {
+            len += 2 + field->val_len;
+        }
+    }
+
+    return len;
+}
+
+static void
+ble_hs_adv_test_misc_verify_tx_fields(uint8_t *cmd,
+                                      struct ble_hs_adv_test_field *fields)
+{
+    struct ble_hs_adv_test_field *field;
+
+    for (field = fields; field->type != 0; field++) {
+        ble_hs_adv_test_misc_verify_tx_field(cmd, field->type, field->val_len,
+                                             field->val);
+        cmd += 2 + field->val_len;
+    }
+}
+
+static void
+ble_hs_adv_test_misc_verify_tx_adv_data(struct ble_hs_adv_test_field *fields)
+{
+    int data_len;
+    uint8_t *cmd;
+
+    cmd = ble_hs_test_util_hci_out_last();
+    TEST_ASSERT_FATAL(cmd != NULL);
+
+    data_len = ble_hs_adv_test_misc_calc_data_len(fields);
+    ble_hs_adv_test_misc_verify_tx_adv_data_hdr(cmd, data_len);
+    if (fields != NULL) {
+        ble_hs_adv_test_misc_verify_tx_fields(cmd + BLE_ADV_TEST_DATA_OFF,
+                                              fields);
+    }
+}
+
+static void
+ble_hs_adv_test_misc_verify_tx_rsp_data(struct ble_hs_adv_test_field *fields)
+{
+    int data_len;
+    uint8_t *cmd;
+
+    cmd = ble_hs_test_util_hci_out_last();
+    TEST_ASSERT_FATAL(cmd != NULL);
+
+    data_len = ble_hs_adv_test_misc_calc_data_len(fields);
+    ble_hs_adv_test_misc_verify_tx_rsp_data_hdr(cmd, data_len);
+    if (fields != NULL) {
+        ble_hs_adv_test_misc_verify_tx_fields(cmd + BLE_ADV_TEST_DATA_OFF,
+                                              fields);
+    }
+}
+
+static void
+ble_hs_adv_test_misc_tx_and_verify_data(
+    uint8_t disc_mode,
+    struct ble_hs_adv_fields *adv_fields,
+    struct ble_hs_adv_test_field *test_adv_fields,
+    struct ble_hs_adv_fields *rsp_fields,
+    struct ble_hs_adv_test_field *test_rsp_fields)
+{
+    struct ble_gap_adv_params adv_params;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.disc_mode = disc_mode;
+
+    rc = ble_hs_test_util_adv_set_fields(adv_fields, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_adv_test_misc_verify_tx_adv_data(test_adv_fields);
+
+    if (test_rsp_fields != NULL) {
+        rc = ble_hs_test_util_adv_rsp_set_fields(rsp_fields, 0, 0);
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_hs_adv_test_misc_verify_tx_rsp_data(test_rsp_fields);
+    }
+
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, &adv_params,
+                                    BLE_HS_FOREVER, NULL, NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Discard the adv-enable command. */
+    ble_hs_test_util_hci_out_last();
+
+    /* Ensure the same data gets sent on repeated advertise procedures. */
+    rc = ble_hs_test_util_adv_stop(0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, &adv_params,
+                                    BLE_HS_FOREVER, NULL, NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Discard the adv-enable command. */
+    ble_hs_test_util_hci_out_last();
+}
+
+TEST_CASE_SELF(ble_hs_adv_test_case_user)
+{
+    struct ble_hs_adv_fields adv_fields;
+    struct ble_hs_adv_fields rsp_fields;
+
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+
+    /*** Complete 16-bit service class UUIDs. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
+    adv_fields.uuids16 = (ble_uuid16_t[]) {
+        BLE_UUID16_INIT(0x0001),
+        BLE_UUID16_INIT(0x1234),
+        BLE_UUID16_INIT(0x54ab)
+    };
+    adv_fields.num_uuids16 = 3;
+    adv_fields.uuids16_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_UUIDS16,
+                .val = (uint8_t[]) { 0x01, 0x00, 0x34, 0x12, 0xab, 0x54 },
+                .val_len = 6,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Incomplete 16-bit service class UUIDs. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.uuids16 = (ble_uuid16_t[]) {
+        BLE_UUID16_INIT(0x0001),
+        BLE_UUID16_INIT(0x1234),
+        BLE_UUID16_INIT(0x54ab)
+    };
+    adv_fields.num_uuids16 = 3;
+    adv_fields.uuids16_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_UUIDS16,
+                .val = (uint8_t[]) { 0x01, 0x00, 0x34, 0x12, 0xab, 0x54 },
+                .val_len = 6,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Complete 32-bit service class UUIDs. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.uuids32 = (ble_uuid32_t[]) {
+        BLE_UUID32_INIT(0x12345678),
+        BLE_UUID32_INIT(0xabacadae)
+    };
+    adv_fields.num_uuids32 = 2;
+    adv_fields.uuids32_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_UUIDS32,
+                .val = (uint8_t[]) { 0x78,0x56,0x34,0x12,0xae,0xad,0xac,0xab },
+                .val_len = 8,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Incomplete 32-bit service class UUIDs. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.uuids32 = (ble_uuid32_t[]) {
+        BLE_UUID32_INIT(0x12345678),
+        BLE_UUID32_INIT(0xabacadae)
+    };
+    adv_fields.num_uuids32 = 2;
+    adv_fields.uuids32_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_UUIDS32,
+                .val = (uint8_t[]) { 0x78,0x56,0x34,0x12,0xae,0xad,0xac,0xab },
+                .val_len = 8,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Complete 128-bit service class UUIDs. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.uuids128 = (ble_uuid128_t[]) {
+        BLE_UUID128_INIT(0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                         0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff)
+    };
+    adv_fields.num_uuids128 = 1;
+    adv_fields.uuids128_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_UUIDS128,
+                .val = (uint8_t[]) {
+                    0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                    0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+                },
+                .val_len = 16,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Incomplete 128-bit service class UUIDs. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.uuids128 = BLE_UUID128(BLE_UUID128_DECLARE(
+        0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+        0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+    ));
+    adv_fields.num_uuids128 = 1;
+    adv_fields.uuids128_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_UUIDS128,
+                .val = (uint8_t[]) {
+                    0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                    0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+                },
+                .val_len = 16,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Complete name. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.name = (uint8_t *)"myname";
+    adv_fields.name_len = 6;
+    adv_fields.name_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_NAME,
+                .val = (uint8_t*)"myname",
+                .val_len = 6,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Incomplete name. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.name = (uint8_t *)"myname";
+    adv_fields.name_len = 6;
+    adv_fields.name_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_NAME,
+                .val = (uint8_t*)"myname",
+                .val_len = 6,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Slave interval range. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.slave_itvl_range = (uint8_t[]){ 1,2,3,4 };
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x16 - Service data - 16-bit UUID. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.svc_data_uuid16 = (uint8_t[]){ 1,2,3,4 };
+    adv_fields.svc_data_uuid16_len = 4;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID16,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = 4,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x17 - Public target address. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.public_tgt_addr = (uint8_t[]){ 1,2,3,4,5,6, 6,5,4,3,2,1 };
+    adv_fields.num_public_tgt_addrs = 2;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR,
+                .val = (uint8_t[]){ 1,2,3,4,5,6, 6,5,4,3,2,1 },
+                .val_len = 2 * BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x19 - Appearance. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.appearance = 0x1234;
+    adv_fields.appearance_is_present = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_APPEARANCE,
+                .val = (uint8_t[]){ 0x34, 0x12 },
+                .val_len = BLE_HS_ADV_APPEARANCE_LEN,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x1a - Advertising interval. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.adv_itvl = 0x1234;
+    adv_fields.adv_itvl_is_present = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_ADV_ITVL,
+                .val = (uint8_t[]){ 0x34, 0x12 },
+                .val_len = BLE_HS_ADV_ADV_ITVL_LEN,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x20 - Service data - 32-bit UUID. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.svc_data_uuid32 = (uint8_t[]){ 1,2,3,4,5 };
+    adv_fields.svc_data_uuid32_len = 5;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID32,
+                .val = (uint8_t[]) { 1,2,3,4,5 },
+                .val_len = 5,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x21 - Service data - 128-bit UUID. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.svc_data_uuid128 =
+        (uint8_t[]){ 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18 };
+    adv_fields.svc_data_uuid128_len = 18;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID128,
+                .val = (uint8_t[]){ 1,2,3,4,5,6,7,8,9,10,
+                                    11,12,13,14,15,16,17,18 },
+                .val_len = 18,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0x24 - URI. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.uri = (uint8_t[]){ 1,2,3,4 };
+    adv_fields.uri_len = 4;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_URI,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = 4,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** 0xff - Manufacturer specific data. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.mfg_data = (uint8_t[]){ 1,2,3,4 };
+    adv_fields.mfg_data_len = 4;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_MFG_DATA,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = 4,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_adv_test_case_user_rsp)
+{
+    struct ble_hs_adv_fields rsp_fields;
+    struct ble_hs_adv_fields adv_fields;
+
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+    adv_fields.tx_pwr_lvl_is_present = 1;
+
+    /*** Complete 16-bit service class UUIDs. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uuids16 = (ble_uuid16_t[]) {
+        BLE_UUID16_INIT(0x0001),
+        BLE_UUID16_INIT(0x1234),
+        BLE_UUID16_INIT(0x54ab)
+    };
+    rsp_fields.num_uuids16 = 3;
+    rsp_fields.uuids16_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_UUIDS16,
+                .val = (uint8_t[]) { 0x01, 0x00, 0x34, 0x12, 0xab, 0x54 },
+                .val_len = 6,
+            },
+            { 0 },
+        });
+
+    /*** Incomplete 16-bit service class UUIDs. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uuids16 = (ble_uuid16_t[]) {
+        BLE_UUID16_INIT(0x0001),
+        BLE_UUID16_INIT(0x1234),
+        BLE_UUID16_INIT(0x54ab)
+    };
+    rsp_fields.num_uuids16 = 3;
+    rsp_fields.uuids16_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_UUIDS16,
+                .val = (uint8_t[]) { 0x01, 0x00, 0x34, 0x12, 0xab, 0x54 },
+                .val_len = 6,
+            },
+            { 0 },
+        });
+
+    /*** Complete 32-bit service class UUIDs. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uuids32 = (ble_uuid32_t[]) {
+        BLE_UUID32_INIT(0x12345678),
+        BLE_UUID32_INIT(0xabacadae)
+    };
+    rsp_fields.num_uuids32 = 2;
+    rsp_fields.uuids32_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_UUIDS32,
+                .val = (uint8_t[]) { 0x78,0x56,0x34,0x12,0xae,0xad,0xac,0xab },
+                .val_len = 8,
+            },
+            { 0 },
+        });
+
+    /*** Incomplete 32-bit service class UUIDs. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uuids32 = (ble_uuid32_t[]) {
+        BLE_UUID32_INIT(0x12345678),
+        BLE_UUID32_INIT(0xabacadae)
+    };
+    rsp_fields.num_uuids32 = 2;
+    rsp_fields.uuids32_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_UUIDS32,
+                .val = (uint8_t[]) { 0x78,0x56,0x34,0x12,0xae,0xad,0xac,0xab },
+                .val_len = 8,
+            },
+            { 0 },
+        });
+
+    /*** Complete 128-bit service class UUIDs. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uuids128 = (ble_uuid128_t[]) {
+        BLE_UUID128_INIT(0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                         0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff)
+    };
+    rsp_fields.num_uuids128 = 1;
+    rsp_fields.uuids128_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_UUIDS128,
+                .val = (uint8_t[]) {
+                    0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                    0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+                },
+                .val_len = 16,
+            },
+            { 0 },
+        });
+
+    /*** Incomplete 128-bit service class UUIDs. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uuids128 = (ble_uuid128_t[]) {
+        BLE_UUID128_INIT(0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                         0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff)
+    };
+    rsp_fields.num_uuids128 = 1;
+    rsp_fields.uuids128_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_UUIDS128,
+                .val = (uint8_t[]) {
+                    0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                    0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+                },
+                .val_len = 16,
+            },
+            { 0 },
+        });
+
+    /*** Complete name. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.name = (uint8_t *)"myname";
+    rsp_fields.name_len = 6;
+    rsp_fields.name_is_complete = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_COMP_NAME,
+                .val = (uint8_t*)"myname",
+                .val_len = 6,
+            },
+            { 0 },
+        });
+
+    /*** Incomplete name. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.name = (uint8_t *)"myname";
+    rsp_fields.name_len = 6;
+    rsp_fields.name_is_complete = 0;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_INCOMP_NAME,
+                .val = (uint8_t*)"myname",
+                .val_len = 6,
+            },
+            { 0 },
+        });
+
+    /*** Slave interval range. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.slave_itvl_range = (uint8_t[]){ 1,2,3,4 };
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN,
+            },
+            { 0 },
+        });
+
+    /*** 0x16 - Service data - 16-bit UUID. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.svc_data_uuid16 = (uint8_t[]){ 1,2,3,4 };
+    rsp_fields.svc_data_uuid16_len = 4;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID16,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = 4,
+            },
+            { 0 },
+        });
+
+    /*** 0x17 - Public target address. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.public_tgt_addr = (uint8_t[]){ 1,2,3,4,5,6, 6,5,4,3,2,1 };
+    rsp_fields.num_public_tgt_addrs = 2;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR,
+                .val = (uint8_t[]){ 1,2,3,4,5,6, 6,5,4,3,2,1 },
+                .val_len = 2 * BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN,
+            },
+            { 0 },
+        });
+
+    /*** 0x19 - Appearance. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.appearance = 0x1234;
+    rsp_fields.appearance_is_present = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_APPEARANCE,
+                .val = (uint8_t[]){ 0x34, 0x12 },
+                .val_len = BLE_HS_ADV_APPEARANCE_LEN,
+            },
+            { 0 },
+        });
+
+    /*** 0x1a - Advertising interval. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.adv_itvl = 0x1234;
+    rsp_fields.adv_itvl_is_present = 1;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_ADV_ITVL,
+                .val = (uint8_t[]){ 0x34, 0x12 },
+                .val_len = BLE_HS_ADV_ADV_ITVL_LEN,
+            },
+            { 0 },
+        });
+
+    /*** 0x20 - Service data - 32-bit UUID. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.svc_data_uuid32 = (uint8_t[]){ 1,2,3,4,5 };
+    rsp_fields.svc_data_uuid32_len = 5;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID32,
+                .val = (uint8_t[]) { 1,2,3,4,5 },
+                .val_len = 5,
+            },
+            { 0 },
+        });
+
+    /*** 0x21 - Service data - 128-bit UUID. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.svc_data_uuid128 =
+        (uint8_t[]){ 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18 };
+    rsp_fields.svc_data_uuid128_len = 18;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID128,
+                .val = (uint8_t[]){ 1,2,3,4,5,6,7,8,9,10,
+                                    11,12,13,14,15,16,17,18 },
+                .val_len = 18,
+            },
+            { 0 },
+        });
+
+    /*** 0x24 - URI. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.uri = (uint8_t[]){ 1,2,3,4 };
+    rsp_fields.uri_len = 4;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_URI,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = 4,
+            },
+            { 0 },
+        });
+
+    /*** 0xff - Manufacturer specific data. */
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+    rsp_fields.mfg_data = (uint8_t[]){ 1,2,3,4 };
+    rsp_fields.mfg_data_len = 4;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .val_len = 1,
+            },
+            {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            { 0 },
+        },
+        &rsp_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_MFG_DATA,
+                .val = (uint8_t[]) { 1,2,3,4 },
+                .val_len = 4,
+            },
+            { 0 },
+        });
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_adv_test_case_user_full_payload)
+{
+    /* Intentionally allocate an extra byte. */
+    static const uint8_t mfg_data[30] = {
+        0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+        0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10,
+        0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18,
+        0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e,
+    };
+
+    struct ble_hs_adv_fields adv_fields;
+    struct ble_hs_adv_fields rsp_fields;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+
+    /***
+     * An advertisement should allow 31 bytes of user data.  Each field has a
+     * two-byte header, leaving 29 bytes of payload.
+     */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.mfg_data = (void *)mfg_data;
+    adv_fields.mfg_data_len = 29;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_MFG_DATA,
+                .val = (void *)mfg_data,
+                .val_len = 29,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Fail with 30 bytes. */
+    rc = ble_hs_test_util_adv_stop(0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    adv_fields.mfg_data_len = 30;
+    rc = ble_gap_adv_set_fields(&adv_fields);
+    TEST_ASSERT(rc == BLE_HS_EMSGSIZE);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_hs_adv_test_suite)
+{
+    ble_hs_adv_test_case_user();
+    ble_hs_adv_test_case_user_rsp();
+    ble_hs_adv_test_case_user_full_payload();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_conn_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_conn_test.c
new file mode 100644
index 00000000..04137d4f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_conn_test.c
@@ -0,0 +1,224 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+#include "../src/ble_gap_priv.h"
+
+static int
+ble_hs_conn_test_util_any(void)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_first();
+    ble_hs_unlock();
+
+    return conn != NULL;
+}
+
+TEST_CASE_SELF(ble_hs_conn_test_direct_connect_success)
+{
+    struct ble_gap_conn_complete evt;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    ble_addr_t addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int rc;
+
+    ble_hs_test_util_init();
+
+    /* Ensure no current or pending connections. */
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_hs_conn_test_util_any());
+
+    /* Initiate connection. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC,
+                                        &addr, 0, NULL, NULL, NULL, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    /* Receive successful connection complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 2;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
+    memcpy(evt.peer_addr, addr.val, 6);
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_first();
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_handle == 2);
+    TEST_ASSERT(memcmp(conn->bhc_peer_addr.val, addr.val, 6) == 0);
+
+    chan = ble_hs_conn_chan_find_by_scid(conn, BLE_L2CAP_CID_ATT);
+    TEST_ASSERT_FATAL(chan != NULL);
+    TEST_ASSERT(chan->my_mtu == MYNEWT_VAL(BLE_ATT_PREFERRED_MTU));
+    TEST_ASSERT(chan->peer_mtu == 0);
+
+    ble_hs_unlock();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_conn_test_direct_connectable_success)
+{
+    struct ble_gap_conn_complete evt;
+    struct ble_gap_adv_params adv_params;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    ble_addr_t addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int rc;
+
+    ble_hs_test_util_init();
+
+    /* Ensure no current or pending connections. */
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
+    TEST_ASSERT(!ble_hs_conn_test_util_any());
+
+    /* Initiate advertising. */
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_DIR;
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    &addr, &adv_params, BLE_HS_FOREVER,
+                                    NULL, NULL, 0, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_adv_active());
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    /* Receive successful connection complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 2;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE;
+    memcpy(evt.peer_addr, addr.val, 6);
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_first();
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_handle == 2);
+    TEST_ASSERT(memcmp(conn->bhc_peer_addr.val, addr.val, 6) == 0);
+
+    chan = ble_hs_conn_chan_find_by_scid(conn, BLE_L2CAP_CID_ATT);
+    TEST_ASSERT_FATAL(chan != NULL);
+    TEST_ASSERT(chan->my_mtu == MYNEWT_VAL(BLE_ATT_PREFERRED_MTU));
+    TEST_ASSERT(chan->peer_mtu == 0);
+
+    ble_hs_unlock();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_conn_test_undirect_connectable_success)
+{
+    struct ble_hs_adv_fields adv_fields;
+    struct ble_gap_conn_complete evt;
+    struct ble_gap_adv_params adv_params;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    ble_addr_t addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int rc;
+
+    ble_hs_test_util_init();
+
+    /* Ensure no current or pending connections. */
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
+    TEST_ASSERT(!ble_hs_conn_test_util_any());
+
+    /* Initiate advertising. */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.tx_pwr_lvl_is_present = 1;
+    rc = ble_hs_test_util_adv_set_fields(&adv_fields, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    &addr, &adv_params,
+                                    BLE_HS_FOREVER,
+                                    NULL, NULL, 0, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_adv_active());
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    /* Receive successful connection complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 2;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE;
+    memcpy(evt.peer_addr, addr.val, 6);
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_first();
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_handle == 2);
+    TEST_ASSERT(memcmp(conn->bhc_peer_addr.val, addr.val, 6) == 0);
+
+    chan = ble_hs_conn_chan_find_by_scid(conn, BLE_L2CAP_CID_ATT);
+    TEST_ASSERT_FATAL(chan != NULL);
+    TEST_ASSERT(chan->my_mtu == MYNEWT_VAL(BLE_ATT_PREFERRED_MTU));
+    TEST_ASSERT(chan->peer_mtu == 0);
+
+    ble_hs_unlock();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_hs_conn_suite)
+{
+    ble_hs_conn_test_direct_connect_success();
+    ble_hs_conn_test_direct_connectable_success();
+    ble_hs_conn_test_undirect_connectable_success();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_hci_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_hci_test.c
new file mode 100644
index 00000000..5f72742c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_hci_test.c
@@ -0,0 +1,342 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble_hs_test.h"
+#include "testutil/testutil.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_HCI_READ_RSSI_ACK_PARAM_LEN     (3)  /* No status byte. */
+
+TEST_CASE_SELF(ble_hs_hci_test_event_bad)
+{
+    uint8_t *buf;
+    int rc;
+
+    /*** Invalid event code. */
+    buf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    TEST_ASSERT_FATAL(buf != NULL);
+
+    buf[0] = 0xff;
+    buf[1] = 0;
+    rc = ble_hs_hci_evt_process((void*)buf);
+    TEST_ASSERT(rc == BLE_HS_ENOTSUP);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_hci_test_rssi)
+{
+    uint8_t params[BLE_HCI_READ_RSSI_ACK_PARAM_LEN];
+    uint16_t opcode;
+    int8_t rssi;
+    int rc;
+
+    opcode = ble_hs_hci_util_opcode_join(BLE_HCI_OGF_STATUS_PARAMS,
+                                  BLE_HCI_OCF_RD_RSSI);
+
+    /*** Success. */
+    /* Connection handle. */
+    put_le16(params + 0, 1);
+
+    /* RSSI. */
+    params[2] = -8;
+
+    ble_hs_test_util_hci_ack_set_params(opcode, 0, params, sizeof params);
+
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(rssi == -8);
+
+    /*** Failure: incorrect connection handle. */
+    put_le16(params + 0, 99);
+
+    ble_hs_test_util_hci_ack_set_params(opcode, 0, params, sizeof params);
+
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
+    TEST_ASSERT(rc == BLE_HS_ECONTROLLER);
+
+    /*** Failure: params too short. */
+    ble_hs_test_util_hci_ack_set_params(opcode, 0, params, sizeof params - 1);
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
+    TEST_ASSERT(rc == BLE_HS_ECONTROLLER);
+
+    /*** Failure: params too long. */
+    ble_hs_test_util_hci_ack_set_params(opcode, 0, params, sizeof params + 1);
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
+    TEST_ASSERT(rc == BLE_HS_ECONTROLLER);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_hci_acl_one_conn)
+{
+    struct ble_hs_test_util_hci_num_completed_pkts_entry ncpe[2];
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+    uint8_t data[256];
+    int rc;
+    int i;
+
+    memset(ncpe, 0, sizeof(ncpe));
+    for (i = 0; i < sizeof data; i++) {
+        data[i] = i;
+    }
+
+    ble_hs_test_util_init();
+
+    /* The controller has room for five 20-byte payloads. */
+    rc = ble_hs_hci_set_buf_sz(20, 5);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5);
+
+    ble_hs_test_util_create_conn(1, peer_addr, NULL, NULL);
+
+    /* Ensure the ATT doesn't truncate our data packets. */
+    ble_hs_test_util_set_att_mtu(1, 256);
+
+    /* Send two 3-byte data packets. */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data, 3);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data, 3);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 3);
+
+    /* Send fragmented packet (two fragments). */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data, 25);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 1);
+
+    ble_hs_test_util_prev_tx_queue_clear();
+
+    /* Receive a number-of-completed-packets event.  Ensure available buffer
+     * count increases.
+     */
+    ncpe[0].handle_id = 1;
+    ncpe[0].num_pkts = 3;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 4);
+
+    /* Use all remaining buffers (four fragments). */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data, 70);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+
+    /* Attempt to transmit eight more fragments. */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data, 160);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+
+    /* Receive number-of-completed-packets: 5. */
+    ncpe[0].handle_id = 1;
+    ncpe[0].num_pkts = 5;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+
+    /* Receive number-of-completed-packets: 4. */
+    ncpe[0].handle_id = 1;
+    ncpe[0].num_pkts = 5;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 1);
+
+    /* Ensure the stalled fragments were sent in the expected order. */
+    ble_hs_test_util_verify_tx_write_cmd(100, data, 70);
+    ble_hs_test_util_verify_tx_write_cmd(100, data, 160);
+
+    /* Receive a disconnection-complete event. Ensure available buffer count
+     * increases.
+     */
+    ble_hs_test_util_hci_rx_disconn_complete_event(1, 0, BLE_ERR_CONN_TERM_LOCAL);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_hci_acl_two_conn)
+{
+    struct ble_hs_test_util_hci_num_completed_pkts_entry ncpe[2];
+    const struct ble_hs_conn *conn1;
+    const struct ble_hs_conn *conn2;
+    uint8_t peer_addr1[6] = { 1, 2, 3, 4, 5, 6 };
+    uint8_t peer_addr2[6] = { 2, 3, 4, 5, 6, 7 };
+    uint8_t data[256];
+    int rc;
+    int i;
+
+    memset(ncpe, 0, sizeof(ncpe));
+    for (i = 0; i < sizeof data; i++) {
+        data[i] = i;
+    }
+
+    ble_hs_test_util_init();
+
+    /* The controller has room for five 20-byte payloads*/
+    rc = ble_hs_hci_set_buf_sz(20, 5);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5);
+
+    ble_hs_test_util_create_conn(1, peer_addr1, NULL, NULL);
+    ble_hs_test_util_create_conn(2, peer_addr2, NULL, NULL);
+
+    /* This test inspects the connection objects after unlocking the host
+     * mutex.  It is not OK for real code to do this, but this test can assume
+     * the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn1 = ble_hs_conn_find_assert(1);
+    conn2 = ble_hs_conn_find_assert(2);
+    ble_hs_unlock();
+
+    /* Ensure the ATT doesn't truncate our data packets. */
+    ble_hs_test_util_set_att_mtu(1, 256);
+    ble_hs_test_util_set_att_mtu(2, 256);
+
+    /* Tx two fragments over connection 1. */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data, 25);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 3);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /* Tx two fragments over connection 2. */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(2, 100, data + 10, 25);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 1);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /* Tx four fragments over connection 2. */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(2, 100, data + 20, 70);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+    TEST_ASSERT_FATAL(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG);
+
+    /* Tx four fragments over connection 1. */
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(1, 100, data + 30, 70);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /**
+     * controller: (11 222)
+     *     conn 1: 1111
+     *     conn 2: 222
+     */
+
+    /* Receive number-of-completed-packets: conn=2, num-pkts=1. */
+    ncpe[0].handle_id = 2;
+    ncpe[0].num_pkts = 1;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+
+    /**
+     * controller: (11 222)
+     *     conn 1: 1111
+     *     conn 2: 22
+     */
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+    TEST_ASSERT_FATAL(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG);
+
+    /* Receive number-of-completed-packets: conn=1, num-pkts=1. */
+    ncpe[0].handle_id = 1;
+    ncpe[0].num_pkts = 1;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+
+    /**
+     * controller: (1 2222)
+     *     conn 1: 1111
+     *     conn 2: 2
+     */
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+    TEST_ASSERT_FATAL(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG);
+
+    /* Receive number-of-completed-packets: conn=1, num-pkts=1. */
+    ncpe[0].handle_id = 1;
+    ncpe[0].num_pkts = 1;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+
+    /**
+     * controller: (22222)
+     *     conn 1: 1111
+     *     conn 2: -
+     */
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+    TEST_ASSERT_FATAL(!(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /* Receive number-of-completed-packets: conn=2, num-pkts=3. */
+    ncpe[0].handle_id = 2;
+    ncpe[0].num_pkts = 3;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+
+    /**
+     * controller: (11122)
+     *     conn 1: 1
+     *     conn 2: -
+     */
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 0);
+    TEST_ASSERT_FATAL(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG);
+    TEST_ASSERT_FATAL(!(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /* Receive number-of-completed-packets: conn=2, num-pkts=2. */
+    ncpe[0].handle_id = 2;
+    ncpe[0].num_pkts = 2;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+
+    /**
+     * controller: (1111)
+     *     conn 1: -
+     *     conn 2: -
+     */
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 1);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+    TEST_ASSERT_FATAL(!(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /* Receive number-of-completed-packets: conn=1, num-pkts=4. */
+    ncpe[0].handle_id = 1;
+    ncpe[0].num_pkts = 4;
+    ble_hs_test_util_hci_rx_num_completed_pkts_event(ncpe);
+
+    /**
+     * controller: ()
+     *     conn 1: -
+     *     conn 2: -
+     */
+    TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5);
+    TEST_ASSERT_FATAL(!(conn1->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+    TEST_ASSERT_FATAL(!(conn2->bhc_flags & BLE_HS_CONN_F_TX_FRAG));
+
+    /*** Verify payloads. */
+    ble_hs_test_util_verify_tx_write_cmd(100, data, 25);
+    ble_hs_test_util_verify_tx_write_cmd(100, data + 10, 25);
+    ble_hs_test_util_verify_tx_write_cmd(100, data + 20, 70);
+    ble_hs_test_util_verify_tx_write_cmd(100, data + 30, 70);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_hs_hci_suite)
+{
+    ble_hs_hci_test_event_bad();
+    ble_hs_hci_test_rssi();
+    ble_hs_hci_acl_one_conn();
+    ble_hs_hci_acl_two_conn();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_id_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_id_test.c
new file mode 100644
index 00000000..c5fe6ce2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_id_test.c
@@ -0,0 +1,124 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+static int
+ble_hs_id_test_util_infer_auto(int privacy, uint8_t *own_addr_type)
+{
+    int rc;
+
+    rc = ble_hs_id_infer_auto(privacy, own_addr_type);
+
+    return rc;
+}
+
+TEST_CASE_SELF(ble_hs_id_test_case_auto_none)
+{
+    uint8_t own_addr_type;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    /* Clear public address. */
+    ble_hs_id_set_pub((uint8_t[6]){ 0, 0, 0, 0, 0, 0 });
+
+    rc = ble_hs_id_test_util_infer_auto(0, &own_addr_type);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOADDR);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_id_test_case_auto_public)
+{
+    uint8_t own_addr_type;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    rc = ble_hs_id_test_util_infer_auto(0, &own_addr_type);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(own_addr_type == BLE_OWN_ADDR_PUBLIC);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_id_test_case_auto_random)
+{
+    uint8_t own_addr_type;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    /* Configure a random address. */
+    ble_hs_test_util_set_static_rnd_addr((uint8_t[6]){ 1, 2, 3, 4, 5, 0xc0 });
+
+    rc = ble_hs_id_test_util_infer_auto(0, &own_addr_type);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(own_addr_type == BLE_OWN_ADDR_RANDOM);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_id_test_case_auto_rpa_pub)
+{
+    uint8_t own_addr_type;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    rc = ble_hs_id_test_util_infer_auto(1, &own_addr_type);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(own_addr_type == BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_hs_id_test_case_auto_rpa_rnd)
+{
+    uint8_t own_addr_type;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    /* Configure a random address. */
+    ble_hs_test_util_set_static_rnd_addr((uint8_t[6]){ 1, 2, 3, 4, 5, 0xc0 });
+
+    rc = ble_hs_id_test_util_infer_auto(1, &own_addr_type);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(own_addr_type == BLE_OWN_ADDR_RPA_RANDOM_DEFAULT);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_hs_id_test_suite_auto)
+{
+    ble_hs_id_test_case_auto_none();
+    ble_hs_id_test_case_auto_public();
+    ble_hs_id_test_case_auto_random();
+    ble_hs_id_test_case_auto_rpa_pub();
+    ble_hs_id_test_case_auto_rpa_rnd();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_pvcy_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_pvcy_test.c
new file mode 100644
index 00000000..79b93b83
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_pvcy_test.c
@@ -0,0 +1,509 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_HS_PVCY_TEST_MAX_GAP_EVENTS 256
+static struct ble_gap_event
+ble_hs_pvcy_test_gap_events[BLE_HS_PVCY_TEST_MAX_GAP_EVENTS];
+static int ble_hs_pvcy_test_num_gap_events;
+
+static void
+ble_hs_pvcy_test_util_init(void)
+{
+    ble_hs_test_util_init();
+    ble_hs_pvcy_test_num_gap_events = 0;
+}
+
+static int
+ble_hs_pvcy_test_util_gap_event(struct ble_gap_event *event, void *arg)
+{
+    TEST_ASSERT_FATAL(ble_hs_pvcy_test_num_gap_events <
+                      BLE_HS_PVCY_TEST_MAX_GAP_EVENTS);
+    ble_hs_pvcy_test_gap_events[ble_hs_pvcy_test_num_gap_events++] = *event;
+
+    return 0;
+}
+
+static void
+ble_hs_pvcy_test_util_all_gap_procs(int adv_status,
+                                    int conn_status,
+                                    int disc_status)
+{
+    struct ble_gap_disc_params disc_params;
+    ble_addr_t peer_addr;
+    int rc;
+
+    /* Advertise. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    NULL, &ble_hs_test_util_adv_params,
+                                    BLE_HS_FOREVER,
+                                    ble_hs_pvcy_test_util_gap_event,
+                                    NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == adv_status);
+
+    if (rc == 0) {
+        rc = ble_hs_test_util_adv_stop(0);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    /* Connect. */
+    peer_addr = (ble_addr_t){ BLE_ADDR_PUBLIC, {1,2,3,4,5,6} };
+    rc = ble_hs_test_util_connect(BLE_ADDR_PUBLIC, &peer_addr,
+                                  BLE_HS_FOREVER, NULL,
+                                  ble_hs_pvcy_test_util_gap_event, NULL, 0);
+    TEST_ASSERT_FATAL(rc == conn_status);
+
+    if (rc == 0) {
+        ble_hs_test_util_conn_cancel_full();
+    }
+
+    /* Discover. */
+    disc_params = (struct ble_gap_disc_params){ 0 };
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_hs_pvcy_test_util_gap_event,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == disc_status);
+
+    if (rc == 0) {
+        rc = ble_hs_test_util_disc_cancel(0);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+}
+
+static void
+ble_hs_pvcy_test_util_add_irk_set_acks(bool scanning, bool connecting)
+{
+    ble_hs_test_util_hci_ack_append(
+        BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE), 0);
+
+    if (connecting) {
+        ble_hs_test_util_hci_ack_append(
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
+            0);
+    }
+
+    if (scanning) {
+        ble_hs_test_util_hci_ack_append(
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+            0);
+    }
+
+    ble_hs_test_util_hci_ack_append(
+        BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST), 0);
+    ble_hs_test_util_hci_ack_append(
+        BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PRIVACY_MODE), 0);
+}
+
+static void
+ble_hs_pvcy_test_util_start_host(int num_expected_irks)
+{
+    int rc;
+    int i;
+
+    /* Clear our IRK.  This ensures the full startup sequence, including
+     * setting the default IRK, takes place.  We need this so that we can plan
+     * which HCI acks to fake.
+     */
+    rc = ble_hs_test_util_set_our_irk((uint8_t[16]){0}, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_test_util_hci_out_clear();
+
+    ble_hs_test_util_hci_ack_set_startup();
+
+    for (i = 0; i < num_expected_irks; i++) {
+        ble_hs_pvcy_test_util_add_irk_set_acks(false, false);
+    }
+
+    ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
+    rc = ble_hs_start();
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Discard startup HCI commands. */
+    ble_hs_test_util_hci_out_adj(ble_hs_test_util_hci_startup_seq_cnt());
+}
+
+static void
+ble_hs_pvcy_test_util_add_irk_verify_tx(const ble_addr_t *peer_addr,
+                                        const uint8_t *peer_irk,
+                                        const uint8_t *local_irk,
+                                        bool scanning,
+                                        bool connecting)
+{
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_SET_ADV_ENABLE,
+                                   NULL);
+
+    if (connecting) {
+        ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                       BLE_HCI_OCF_LE_CREATE_CONN_CANCEL,
+                                       NULL);
+    }
+
+    if (scanning) {
+        ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                       BLE_HCI_OCF_LE_SET_SCAN_ENABLE,
+                                       NULL);
+    }
+
+    ble_hs_test_util_hci_verify_tx_add_irk(peer_addr->type,
+                                           peer_addr->val,
+                                           peer_irk,
+                                           local_irk);
+
+    ble_hs_test_util_hci_verify_tx_set_priv_mode(peer_addr->type,
+                                                 peer_addr->val,
+                                                 BLE_GAP_PRIVATE_MODE_DEVICE);
+}
+
+static void
+ble_hs_pvcy_test_util_add_irk(const ble_addr_t *peer_addr,
+                              const uint8_t *peer_irk,
+                              const uint8_t *local_irk,
+                              bool scanning,
+                              bool connecting)
+{
+    int num_acks;
+    int rc;
+
+    ble_hs_pvcy_test_util_add_irk_set_acks(scanning, connecting);
+
+    rc = ble_hs_pvcy_add_entry(peer_addr->val, peer_addr->type, peer_irk);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    num_acks = 3;
+    if (scanning) {
+        num_acks++;
+    }
+    if (connecting) {
+        num_acks++;
+    }
+    ble_hs_test_util_hci_out_adj(-num_acks);
+    ble_hs_pvcy_test_util_add_irk_verify_tx(peer_addr, peer_irk, local_irk,
+                                            scanning, connecting);
+}
+
+static void
+ble_hs_pvcy_test_util_add_arbitrary_irk(bool scanning, bool connecting)
+{
+    ble_addr_t peer_addr;
+
+    peer_addr = (ble_addr_t) {
+        .type = BLE_ADDR_PUBLIC,
+        .val = {1,2,3,4,5,6},
+    };
+    ble_hs_pvcy_test_util_add_irk(
+        &peer_addr,
+        (uint8_t[16]){1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16},
+        ble_hs_pvcy_default_irk,
+        scanning,
+        connecting);
+}
+
+static void
+ble_hs_pvcy_test_util_restore_irk(const struct ble_store_value_sec *value_sec,
+                                  bool scanning,
+                                  bool connecting)
+{
+    int rc;
+
+    ble_hs_pvcy_test_util_add_irk_set_acks(scanning, connecting);
+
+    rc = ble_store_write_peer_sec(value_sec);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec->peer_addr,
+                                            value_sec->irk,
+                                            ble_hs_pvcy_default_irk,
+                                            scanning,
+                                            connecting);
+}
+
+TEST_CASE_SELF(ble_hs_pvcy_test_case_restore_irks)
+{
+    struct ble_store_value_sec value_sec1;
+    struct ble_store_value_sec value_sec2;
+
+    ble_hs_pvcy_test_util_init();
+
+    /*** No persisted IRKs. */
+    ble_hs_pvcy_test_util_start_host(0);
+
+    /*** One persisted IRK. */
+
+    /* Persist IRK; ensure it automatically gets added to the list. */
+    value_sec1 = (struct ble_store_value_sec) {
+        .peer_addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
+        .key_size = 16,
+        .ediv = 1,
+        .rand_num = 2,
+        .irk = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 },
+        .irk_present = 1,
+    };
+    ble_hs_pvcy_test_util_restore_irk(&value_sec1, false, false);
+
+    /* Ensure it gets added to list on startup. */
+    ble_hs_pvcy_test_util_start_host(1);
+    ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec1.peer_addr,
+                                            value_sec1.irk,
+                                            ble_hs_pvcy_default_irk,
+                                            false, false);
+
+    /* Two persisted IRKs. */
+    value_sec2 = (struct ble_store_value_sec) {
+        .peer_addr = { BLE_ADDR_PUBLIC, { 2, 3, 4, 5, 6, 7 } },
+        .key_size = 16,
+        .ediv = 12,
+        .rand_num = 20,
+        .irk = { 4, 4, 4, 4, 5, 5, 5, 6, 6, 6, 9, 9, 9, 9, 9, 10 },
+        .irk_present = 1,
+    };
+    ble_hs_pvcy_test_util_restore_irk(&value_sec2, false, false);
+
+    /* Ensure both get added to list on startup. */
+    ble_hs_pvcy_test_util_start_host(2);
+    ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec1.peer_addr,
+                                            value_sec1.irk,
+                                            ble_hs_pvcy_default_irk,
+                                            false, false);
+    ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec2.peer_addr,
+                                            value_sec2.irk,
+                                            ble_hs_pvcy_default_irk,
+                                            false, false);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/** No active GAP procedures. */
+TEST_CASE_SELF(ble_hs_pvcy_test_case_add_irk_idle)
+{
+    ble_hs_pvcy_test_util_init();
+
+    ble_hs_pvcy_test_util_add_arbitrary_irk(false, false);
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*** Advertising active. */
+TEST_CASE_SELF(ble_hs_pvcy_test_case_add_irk_adv)
+{
+    int rc;
+
+    ble_hs_pvcy_test_util_init();
+
+    /* Start an advertising procedure. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    NULL, &ble_hs_test_util_adv_params,
+                                    BLE_HS_FOREVER,
+                                    ble_hs_pvcy_test_util_gap_event,
+                                    NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_pvcy_test_util_add_arbitrary_irk(false, false);
+
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 1);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].type ==
+                BLE_GAP_EVENT_ADV_COMPLETE);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].adv_complete.reason ==
+                BLE_HS_EPREEMPTED);
+
+    /* Ensure GAP procedures are no longer preempted. */
+    ble_hs_pvcy_test_util_all_gap_procs(0, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*** Discovery active. */
+TEST_CASE_SELF(ble_hs_pvcy_test_case_add_irk_disc)
+{
+    struct ble_gap_disc_params disc_params;
+    int rc;
+
+    ble_hs_pvcy_test_util_init();
+
+    /* Start an advertising procedure. */
+    disc_params = (struct ble_gap_disc_params){ 0 };
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_hs_pvcy_test_util_gap_event,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_pvcy_test_util_add_arbitrary_irk(true, false);
+
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 1);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].type ==
+                BLE_GAP_EVENT_DISC_COMPLETE);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].disc_complete.reason ==
+                BLE_HS_EPREEMPTED);
+
+    /* Ensure GAP procedures are no longer preempted. */
+    ble_hs_pvcy_test_util_all_gap_procs(0, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*** Connect active. */
+TEST_CASE_SELF(ble_hs_pvcy_test_case_add_irk_conn)
+{
+    ble_addr_t peer_addr;
+    int rc;
+
+    ble_hs_pvcy_test_util_init();
+
+    /* Start a connect procedure. */
+    peer_addr = (ble_addr_t){ BLE_ADDR_PUBLIC, {1,2,3,4,5,6} };
+    rc = ble_hs_test_util_connect(BLE_ADDR_PUBLIC, &peer_addr,
+                                  BLE_HS_FOREVER, NULL,
+                                  ble_hs_pvcy_test_util_gap_event, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_pvcy_test_util_add_arbitrary_irk(false, true);
+
+    /* Cancel is now in progress. */
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 0);
+
+    /* Ensure no GAP procedures are allowed. */
+    ble_hs_pvcy_test_util_all_gap_procs(BLE_HS_EPREEMPTED,
+                                        BLE_HS_EALREADY,
+                                        BLE_HS_EBUSY);
+
+    /* Receive cancel event. */
+    ble_hs_test_util_rx_conn_cancel_evt();
+
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 1);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].type ==
+                BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].connect.status ==
+                BLE_HS_EPREEMPTED);
+
+    /* Ensure GAP procedures are no longer preempted. */
+    ble_hs_pvcy_test_util_all_gap_procs(0, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*** Advertising and discovery active. */
+TEST_CASE_SELF(ble_hs_pvcy_test_case_add_irk_adv_disc)
+{
+    struct ble_gap_disc_params disc_params;
+    int rc;
+
+    ble_hs_pvcy_test_util_init();
+
+    /* Start an advertising procedure. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    NULL, &ble_hs_test_util_adv_params,
+                                    BLE_HS_FOREVER,
+                                    ble_hs_pvcy_test_util_gap_event,
+                                    NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Start a discovery procedure. */
+    disc_params = (struct ble_gap_disc_params){ 0 };
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_hs_pvcy_test_util_gap_event,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_pvcy_test_util_add_arbitrary_irk(true, false);
+
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 2);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].type ==
+                BLE_GAP_EVENT_ADV_COMPLETE);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].adv_complete.reason ==
+                BLE_HS_EPREEMPTED);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[1].type ==
+                BLE_GAP_EVENT_DISC_COMPLETE);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[1].disc_complete.reason ==
+                BLE_HS_EPREEMPTED);
+
+    /* Ensure GAP procedures are no longer preempted. */
+    ble_hs_pvcy_test_util_all_gap_procs(0, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*** Advertising and connecting active. */
+TEST_CASE_SELF(ble_hs_pvcy_test_case_add_irk_adv_conn)
+{
+    ble_addr_t peer_addr;
+    int rc;
+
+    ble_hs_pvcy_test_util_init();
+
+    /* Start an advertising procedure. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC,
+                                    NULL, &ble_hs_test_util_adv_params,
+                                    BLE_HS_FOREVER,
+                                    ble_hs_pvcy_test_util_gap_event,
+                                    NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Start a connect procedure. */
+    peer_addr = (ble_addr_t){ BLE_ADDR_PUBLIC, {1,2,3,4,5,6} };
+    rc = ble_hs_test_util_connect(BLE_ADDR_PUBLIC, &peer_addr,
+                                  BLE_HS_FOREVER, NULL,
+                                  ble_hs_pvcy_test_util_gap_event, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_pvcy_test_util_add_arbitrary_irk(false, true);
+
+    /* Cancel is now in progress. */
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 1);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].type ==
+                BLE_GAP_EVENT_ADV_COMPLETE);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[0].adv_complete.reason ==
+                BLE_HS_EPREEMPTED);
+
+    /* Ensure no GAP procedures are allowed. */
+    ble_hs_pvcy_test_util_all_gap_procs(BLE_HS_EPREEMPTED,
+                                        BLE_HS_EALREADY,
+                                        BLE_HS_EBUSY);
+
+    /* Receive cancel event. */
+    ble_hs_test_util_rx_conn_cancel_evt();
+
+    TEST_ASSERT(ble_hs_pvcy_test_num_gap_events == 2);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[1].type ==
+                BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_hs_pvcy_test_gap_events[1].connect.status ==
+                BLE_HS_EPREEMPTED);
+
+    /* Ensure GAP procedures are no longer preempted. */
+    ble_hs_pvcy_test_util_all_gap_procs(0, 0, 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_hs_pvcy_test_suite_irk)
+{
+    ble_hs_pvcy_test_case_restore_irks();
+    ble_hs_pvcy_test_case_add_irk_idle();
+    ble_hs_pvcy_test_case_add_irk_adv();
+    ble_hs_pvcy_test_case_add_irk_disc();
+    ble_hs_pvcy_test_case_add_irk_conn();
+    ble_hs_pvcy_test_case_add_irk_adv_disc();
+    ble_hs_pvcy_test_case_add_irk_adv_conn();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_stop_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_stop_test.c
new file mode 100644
index 00000000..526d5f5a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_stop_test.c
@@ -0,0 +1,203 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "host/ble_hs.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BHST_MAX_EVENTS     32
+
+static struct ble_gap_event bhst_events[BHST_MAX_EVENTS];
+static int bhst_num_events;
+
+static struct ble_hs_stop_listener bhst_listener;
+static struct os_sem bhst_sem;
+
+static int
+bhst_gap_event(struct ble_gap_event *event, void *arg)
+{
+    TEST_ASSERT_FATAL(bhst_num_events < BHST_MAX_EVENTS);
+
+    bhst_events[bhst_num_events++] = *event;
+    return 0;
+}
+
+static void
+bhst_stop_cb(int status, void *arg)
+{
+    int rc;
+
+    rc = os_sem_release(&bhst_sem);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+TEST_CASE_TASK(ble_hs_stop_test_new_procs)
+{
+    static const struct ble_gap_disc_params disc_params;
+    static const struct ble_gap_adv_params adv_params;
+
+    static const ble_addr_t peer_addr = {
+        BLE_ADDR_PUBLIC,
+        { 1, 2, 3, 4, 5, 6 }
+    };
+
+    int rc;
+
+    rc = os_sem_init(&bhst_sem, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Stop the host and wait for the stop procedure to complete. */
+    ble_hs_test_util_hci_ack_set(
+        BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE), 0);
+
+    rc = ble_hs_stop(&bhst_listener, bhst_stop_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = os_sem_pend(&bhst_sem, OS_TIMEOUT_NEVER);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /*** Ensure all GAP procedures fail. */
+
+    /* Advertise. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, &adv_params,
+                                    BLE_HS_FOREVER, bhst_gap_event, NULL,
+                                    0, 0);
+    TEST_ASSERT(rc == BLE_HS_EDISABLED);
+
+    /* Discover. */
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, bhst_gap_event, NULL, 0, 0);
+    TEST_ASSERT(rc == BLE_HS_EDISABLED);
+
+    /* Connect. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr,
+                                  BLE_HS_FOREVER, NULL,
+                                  bhst_gap_event, NULL, 0);
+    TEST_ASSERT(rc == BLE_HS_EDISABLED);
+
+    /*** Restart stack; ensure GAP procedures succeed. */
+
+    ble_hs_test_util_hci_ack_set_startup();
+    ble_hs_sched_start();
+
+    /* Advertise. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, &adv_params,
+                                    BLE_HS_FOREVER, bhst_gap_event, NULL,
+                                    0, 0);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_adv_stop(0);
+    TEST_ASSERT(rc == 0);
+
+    /* Discover. */
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, bhst_gap_event, NULL, 0, 0);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_disc_cancel(0);
+    TEST_ASSERT(rc == 0);
+
+    /* Connect. */
+    rc = ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &peer_addr,
+                                  BLE_HS_FOREVER, NULL,
+                                  bhst_gap_event, NULL, 0);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_hs_test_util_conn_cancel(0);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_TASK(ble_hs_stop_test_cur_procs)
+{
+    static const struct ble_gap_disc_params disc_params;
+    static const struct ble_gap_adv_params adv_params;
+
+    int rc;
+
+    rc = os_sem_init(&bhst_sem, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Advertise. */
+    rc = ble_hs_test_util_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, &adv_params,
+                                    BLE_HS_FOREVER, bhst_gap_event, NULL,
+                                    0, 0);
+    TEST_ASSERT(rc == 0);
+
+    /* Discover. */
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, bhst_gap_event, NULL, 0, 0);
+    TEST_ASSERT(rc == 0);
+
+    /* Preload the host with HCI acks for the cancel commands that will be sent
+     * automatically when the host stops.
+     */
+    ble_hs_test_util_hci_ack_set(
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+        0);
+    ble_hs_test_util_hci_ack_append(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        0);
+
+    /* Stop the host and wait for the stop procedure to complete. */
+    bhst_num_events = 0;
+    rc = ble_hs_stop(&bhst_listener, bhst_stop_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = os_sem_pend(&bhst_sem, OS_TIMEOUT_NEVER);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure the GAP procedure cancellations were reported. */
+    TEST_ASSERT_FATAL(bhst_num_events == 2);
+    TEST_ASSERT(bhst_events[0].type == BLE_GAP_EVENT_ADV_COMPLETE);
+    TEST_ASSERT(bhst_events[0].adv_complete.reason == BLE_HS_EPREEMPTED);
+    TEST_ASSERT(bhst_events[1].type == BLE_GAP_EVENT_DISC_COMPLETE);
+    TEST_ASSERT(bhst_events[1].disc_complete.reason == BLE_HS_EPREEMPTED);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static void
+bhst_pre_test(void *arg)
+{
+    ble_hs_test_util_init_no_sysinit_no_start();
+
+    /* Preload the host with HCI acks for the startup sequence. */
+    ble_hs_test_util_hci_ack_set_startup();
+}
+
+TEST_SUITE(ble_hs_stop_test_suite)
+{
+    tu_suite_set_pre_test_cb(bhst_pre_test, NULL);
+
+    ble_hs_stop_test_new_procs();
+    ble_hs_stop_test_cur_procs();
+}
+
+int
+ble_stop_test_all(void)
+{
+    ble_hs_stop_test_suite();
+
+    return tu_any_failed;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test.c
new file mode 100644
index 00000000..adf99423
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test.c
@@ -0,0 +1,83 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/hci_common.h"
+#include "testutil/testutil.h"
+#include "ble_hs_test_util.h"
+#include "ble_hs_test.h"
+
+#if MYNEWT_VAL(SELFTEST)
+
+int
+main(int argc, char **argv)
+{
+    /* XXX: This test must come before the others; it causes privacy to be
+     * enabled.  Subsequent tests depend on this.  This is wrong - each test
+     * should enable privacy as needed, but the test util functions are so low
+     * level that they make this very difficult to arrange (individual HCI
+     * commands and responses).
+     *
+     * To fix this, we should implement a set of higher level BLE test
+     * functions that don't require individual HCI commands to be specified.
+     */
+    ble_gap_test_suite_disc();
+
+    ble_att_clt_suite();
+    ble_att_svr_suite();
+    ble_gap_test_suite_adv();
+    ble_gap_test_suite_conn_cancel();
+    ble_gap_test_suite_conn_find();
+    ble_gap_test_suite_conn_gen();
+    ble_gap_test_suite_conn_terminate();
+    ble_gap_test_suite_mtu();
+    ble_gap_test_suite_set_cb();
+    ble_gap_test_suite_stop_adv();
+    ble_gap_test_suite_timeout();
+    ble_gap_test_suite_update_conn();
+    ble_gap_test_suite_wl();
+    ble_gatt_conn_suite();
+    ble_gatt_disc_c_test_suite();
+    ble_gatt_disc_d_test_suite();
+    ble_gatt_disc_s_test_suite();
+    ble_gatt_find_s_test_suite();
+    ble_gatt_read_test_suite();
+    ble_gatt_write_test_suite();
+    ble_gatts_notify_suite();
+    ble_gatts_read_test_suite();
+    ble_gatts_reg_suite();
+    ble_hs_adv_test_suite();
+    ble_hs_conn_suite();
+    ble_hs_hci_suite();
+    ble_hs_id_test_suite_auto();
+    ble_hs_pvcy_test_suite_irk();
+    ble_l2cap_test_suite();
+    ble_os_test_suite();
+    ble_sm_gen_test_suite();
+    ble_sm_lgcy_test_suite();
+    ble_sm_sc_test_suite();
+    ble_store_suite();
+    ble_uuid_test_suite();
+
+    return tu_any_failed;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test.h b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test.h
new file mode 100644
index 00000000..3fb1454e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test.h
@@ -0,0 +1,62 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_TEST_
+#define H_BLE_HS_TEST_
+
+#include "testutil/testutil.h"
+
+TEST_SUITE_DECL(ble_att_clt_suite);
+TEST_SUITE_DECL(ble_att_svr_suite);
+TEST_SUITE_DECL(ble_gap_test_suite_adv);
+TEST_SUITE_DECL(ble_gap_test_suite_conn_cancel);
+TEST_SUITE_DECL(ble_gap_test_suite_conn_find);
+TEST_SUITE_DECL(ble_gap_test_suite_conn_gen);
+TEST_SUITE_DECL(ble_gap_test_suite_conn_terminate);
+TEST_SUITE_DECL(ble_gap_test_suite_disc);
+TEST_SUITE_DECL(ble_gap_test_suite_mtu);
+TEST_SUITE_DECL(ble_gap_test_suite_set_cb);
+TEST_SUITE_DECL(ble_gap_test_suite_stop_adv);
+TEST_SUITE_DECL(ble_gap_test_suite_timeout);
+TEST_SUITE_DECL(ble_gap_test_suite_update_conn);
+TEST_SUITE_DECL(ble_gap_test_suite_wl);
+TEST_SUITE_DECL(ble_gatt_conn_suite);
+TEST_SUITE_DECL(ble_gatt_disc_c_test_suite);
+TEST_SUITE_DECL(ble_gatt_disc_d_test_suite);
+TEST_SUITE_DECL(ble_gatt_disc_s_test_suite);
+TEST_SUITE_DECL(ble_gatt_find_s_test_suite);
+TEST_SUITE_DECL(ble_gatt_read_test_suite);
+TEST_SUITE_DECL(ble_gatt_write_test_suite);
+TEST_SUITE_DECL(ble_gatts_notify_suite);
+TEST_SUITE_DECL(ble_gatts_read_test_suite);
+TEST_SUITE_DECL(ble_gatts_reg_suite);
+TEST_SUITE_DECL(ble_hs_adv_test_suite);
+TEST_SUITE_DECL(ble_hs_conn_suite);
+TEST_SUITE_DECL(ble_hs_hci_suite);
+TEST_SUITE_DECL(ble_hs_id_test_suite_auto);
+TEST_SUITE_DECL(ble_hs_pvcy_test_suite_irk);
+TEST_SUITE_DECL(ble_l2cap_test_suite);
+TEST_SUITE_DECL(ble_os_test_suite);
+TEST_SUITE_DECL(ble_sm_gen_test_suite);
+TEST_SUITE_DECL(ble_sm_lgcy_test_suite);
+TEST_SUITE_DECL(ble_sm_sc_test_suite);
+TEST_SUITE_DECL(ble_store_suite);
+TEST_SUITE_DECL(ble_uuid_test_suite);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util.c
new file mode 100644
index 00000000..5ee17e76
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util.c
@@ -0,0 +1,2048 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "sysinit/sysinit.h"
+#include "stats/stats.h"
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_hs_id.h"
+#include "store/config/ble_store_config.h"
+#include "transport/ram/ble_hci_ram.h"
+#include "ble_hs_test_util.h"
+
+/* Our global device address. */
+uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
+
+static STAILQ_HEAD(, os_mbuf_pkthdr) ble_hs_test_util_prev_tx_queue;
+struct os_mbuf *ble_hs_test_util_prev_tx_cur;
+
+int ble_sm_test_store_obj_type;
+union ble_store_key ble_sm_test_store_key;
+union ble_store_value ble_sm_test_store_value;
+
+const struct ble_gap_adv_params ble_hs_test_util_adv_params = {
+    .conn_mode = BLE_GAP_CONN_MODE_UND,
+    .disc_mode = BLE_GAP_DISC_MODE_GEN,
+
+    .itvl_min = 0,
+    .itvl_max = 0,
+    .channel_map = 0,
+    .filter_policy = 0,
+    .high_duty_cycle = 0,
+};
+
+void
+ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om)
+{
+    struct os_mbuf_pkthdr *omp;
+
+    assert(OS_MBUF_IS_PKTHDR(om));
+
+    omp = OS_MBUF_PKTHDR(om);
+    if (STAILQ_EMPTY(&ble_hs_test_util_prev_tx_queue)) {
+        STAILQ_INSERT_HEAD(&ble_hs_test_util_prev_tx_queue, omp, omp_next);
+    } else {
+        STAILQ_INSERT_TAIL(&ble_hs_test_util_prev_tx_queue, omp, omp_next);
+    }
+}
+
+static struct os_mbuf *
+ble_hs_test_util_prev_tx_dequeue_once(struct hci_data_hdr *out_hci_hdr)
+{
+    struct os_mbuf_pkthdr *omp;
+    struct os_mbuf *om;
+    int rc;
+
+    omp = STAILQ_FIRST(&ble_hs_test_util_prev_tx_queue);
+    if (omp == NULL) {
+        return NULL;
+    }
+    STAILQ_REMOVE_HEAD(&ble_hs_test_util_prev_tx_queue, omp_next);
+
+    om = OS_MBUF_PKTHDR_TO_MBUF(omp);
+
+    rc = ble_hs_hci_util_data_hdr_strip(om, out_hci_hdr);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(out_hci_hdr->hdh_len == OS_MBUF_PKTLEN(om));
+
+    return om;
+}
+
+struct os_mbuf *
+ble_hs_test_util_prev_tx_dequeue(void)
+{
+    struct ble_l2cap_hdr l2cap_hdr;
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    uint8_t pb;
+    int rc;
+
+    rc = os_mbuf_free_chain(ble_hs_test_util_prev_tx_cur);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    om = ble_hs_test_util_prev_tx_dequeue_once(&hci_hdr);
+    if (om != NULL) {
+        pb = BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc);
+        TEST_ASSERT_FATAL(pb == BLE_HCI_PB_FIRST_NON_FLUSH);
+
+        rc = ble_l2cap_parse_hdr(om, 0, &l2cap_hdr);
+        TEST_ASSERT_FATAL(rc == 0);
+
+        os_mbuf_adj(om, BLE_L2CAP_HDR_SZ);
+
+        ble_hs_test_util_prev_tx_cur = om;
+        while (OS_MBUF_PKTLEN(ble_hs_test_util_prev_tx_cur) <
+               l2cap_hdr.len) {
+
+            om = ble_hs_test_util_prev_tx_dequeue_once(&hci_hdr);
+            TEST_ASSERT_FATAL(om != NULL);
+
+            pb = BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc);
+            TEST_ASSERT_FATAL(pb == BLE_HCI_PB_MIDDLE);
+
+            os_mbuf_concat(ble_hs_test_util_prev_tx_cur, om);
+        }
+    } else {
+        ble_hs_test_util_prev_tx_cur = NULL;
+    }
+
+    return ble_hs_test_util_prev_tx_cur;
+}
+
+struct os_mbuf *
+ble_hs_test_util_prev_tx_dequeue_pullup(void)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+    if (om != NULL) {
+        om = os_mbuf_pullup(om, OS_MBUF_PKTLEN(om));
+        TEST_ASSERT_FATAL(om != NULL);
+        ble_hs_test_util_prev_tx_cur = om;
+    }
+
+    return om;
+}
+
+int
+ble_hs_test_util_prev_tx_queue_sz(void)
+{
+    struct os_mbuf_pkthdr *omp;
+    int cnt;
+
+    cnt = 0;
+    STAILQ_FOREACH(omp, &ble_hs_test_util_prev_tx_queue, omp_next) {
+        cnt++;
+    }
+
+    return cnt;
+}
+
+void
+ble_hs_test_util_prev_tx_queue_clear(void)
+{
+    while (!STAILQ_EMPTY(&ble_hs_test_util_prev_tx_queue)) {
+        ble_hs_test_util_prev_tx_dequeue();
+    }
+}
+
+static void
+ble_hs_test_util_conn_params_dflt(struct ble_gap_conn_params *conn_params)
+{
+    conn_params->scan_itvl = 0x0010;
+    conn_params->scan_window = 0x0010;
+    conn_params->itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN;
+    conn_params->itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX;
+    conn_params->latency = BLE_GAP_INITIAL_CONN_LATENCY;
+    conn_params->supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT;
+    conn_params->min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
+    conn_params->max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
+}
+
+static void
+ble_hs_test_util_hcc_from_conn_params(
+    struct hci_create_conn *hcc, uint8_t own_addr_type,
+    const ble_addr_t *peer_addr, const struct ble_gap_conn_params *conn_params)
+{
+    hcc->scan_itvl = conn_params->scan_itvl;
+    hcc->scan_window = conn_params->scan_window;
+
+    if (peer_addr == NULL) {
+        hcc->filter_policy = BLE_HCI_CONN_FILT_USE_WL;
+        hcc->peer_addr_type = 0;
+        memset(hcc->peer_addr, 0, 6);
+    } else {
+        hcc->filter_policy = BLE_HCI_CONN_FILT_NO_WL;
+        hcc->peer_addr_type = peer_addr->type;
+        memcpy(hcc->peer_addr, peer_addr->val, 6);
+    }
+    hcc->own_addr_type = own_addr_type;
+    hcc->conn_itvl_min = conn_params->itvl_min;
+    hcc->conn_itvl_max = conn_params->itvl_max;
+    hcc->conn_latency = conn_params->latency;
+    hcc->supervision_timeout = conn_params->supervision_timeout;
+    hcc->min_ce_len = conn_params->min_ce_len;
+    hcc->max_ce_len = conn_params->max_ce_len;
+}
+
+void
+ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t own_addr_type,
+                                 const uint8_t *our_rpa,
+                                 uint8_t peer_addr_type,
+                                 const uint8_t *peer_id_addr,
+                                 const uint8_t *peer_rpa,
+                                 uint8_t conn_features,
+                                 ble_gap_event_fn *cb, void *cb_arg)
+{
+    ble_addr_t addr;
+    struct ble_gap_conn_complete evt;
+    struct ble_hci_ev_le_subev_rd_rem_used_feat evt2;
+    int rc;
+
+    addr.type = peer_addr_type;
+    memcpy(addr.val, peer_id_addr, 6);
+
+    rc = ble_hs_test_util_connect(own_addr_type, &addr, 0, NULL, cb, cb_arg,
+                                  0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = handle;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
+    evt.peer_addr_type = peer_addr_type;
+    memcpy(evt.peer_addr, peer_id_addr, 6);
+    evt.conn_itvl = BLE_GAP_INITIAL_CONN_ITVL_MAX;
+    evt.conn_latency = BLE_GAP_INITIAL_CONN_LATENCY;
+    evt.supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT;
+    memcpy(evt.local_rpa, our_rpa, 6);
+    memcpy(evt.peer_rpa, peer_rpa, 6);
+
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+
+    evt2.subev_code = BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT;
+    evt2.status = BLE_ERR_SUCCESS;
+    evt2.conn_handle = htole16(handle);
+    memcpy(evt2.features, ((uint8_t[]){ conn_features, 0, 0, 0, 0, 0, 0, 0 }),
+           8);
+
+    ble_gap_rx_rd_rem_sup_feat_complete(&evt2);
+
+    ble_hs_test_util_hci_out_clear();
+}
+
+void
+ble_hs_test_util_create_conn(uint16_t handle, const uint8_t *peer_id_addr,
+                             ble_gap_event_fn *cb, void *cb_arg)
+{
+    static uint8_t null_addr[6];
+
+    ble_hs_test_util_create_rpa_conn(handle, BLE_OWN_ADDR_PUBLIC, null_addr,
+                                     BLE_ADDR_PUBLIC, peer_id_addr,
+                                     null_addr, BLE_HS_TEST_CONN_FEAT_ALL,
+                                     cb, cb_arg);
+}
+
+void
+ble_hs_test_util_create_conn_feat(uint16_t handle, const uint8_t *peer_id_addr,
+                                  uint8_t conn_features, ble_gap_event_fn *cb,
+                                  void *cb_arg)
+{
+    static uint8_t null_addr[6];
+
+    ble_hs_test_util_create_rpa_conn(handle, BLE_OWN_ADDR_PUBLIC, null_addr,
+                                     BLE_ADDR_PUBLIC, peer_id_addr,
+                                     null_addr, conn_features, cb, cb_arg);
+}
+
+int
+ble_hs_test_util_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                         int32_t duration_ms,
+                         const struct ble_gap_conn_params *params,
+                         ble_gap_event_fn *cb, void *cb_arg,
+                         uint8_t ack_status)
+{
+    struct ble_gap_conn_params dflt_params;
+    struct hci_create_conn hcc;
+    int rc;
+
+    /* This function ensures the most recently sent HCI command is the expected
+     * create connection command.  If the current test case has unverified HCI
+     * commands, assume we are not interested in them and clear the queue.
+     */
+    ble_hs_test_util_hci_out_clear();
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CREATE_CONN),
+        ack_status);
+
+    rc = ble_gap_connect(own_addr_type, peer_addr, duration_ms, params, cb,
+                         cb_arg);
+    if (ack_status != 0) {
+        TEST_ASSERT(rc == BLE_HS_HCI_ERR(ack_status));
+    } else if (rc != 0) {
+        return rc;
+    }
+
+    if (params == NULL) {
+        ble_hs_test_util_conn_params_dflt(&dflt_params);
+        params = &dflt_params;
+    }
+
+    ble_hs_test_util_hcc_from_conn_params(&hcc, own_addr_type, peer_addr,
+                                          params);
+    ble_hs_test_util_hci_verify_tx_create_conn(&hcc);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_conn_cancel(uint8_t ack_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
+        ack_status);
+
+    rc = ble_gap_conn_cancel();
+    return rc;
+}
+
+void
+ble_hs_test_util_rx_conn_cancel_evt(void)
+{
+    ble_hs_test_util_conn_cancel(0);
+    ble_hs_test_util_hci_rx_conn_cancel_evt();
+}
+
+void
+ble_hs_test_util_conn_cancel_full(void)
+{
+    ble_hs_test_util_conn_cancel(0);
+    ble_hs_test_util_rx_conn_cancel_evt();
+}
+
+int
+ble_hs_test_util_conn_terminate(uint16_t conn_handle, uint8_t hci_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set_disconnect(hci_status);
+    rc = ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+    return rc;
+}
+
+void
+ble_hs_test_util_conn_disconnect(uint16_t conn_handle)
+{
+    int rc;
+
+    rc = ble_hs_test_util_conn_terminate(conn_handle, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Receive disconnection complete event. */
+    ble_hs_test_util_hci_rx_disconn_complete_event(conn_handle, 0,
+                                                   BLE_ERR_CONN_TERM_LOCAL);
+}
+
+int
+ble_hs_test_util_disc(uint8_t own_addr_type, int32_t duration_ms,
+                      const struct ble_gap_disc_params *disc_params,
+                      ble_gap_event_fn *cb, void *cb_arg, int fail_idx,
+                      uint8_t fail_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set_disc(own_addr_type, fail_idx, fail_status);
+    rc = ble_gap_disc(own_addr_type, duration_ms, disc_params,
+                      cb, cb_arg);
+    return rc;
+}
+
+int
+ble_hs_test_util_disc_cancel(uint8_t ack_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        ack_status);
+
+    rc = ble_gap_disc_cancel();
+    return rc;
+}
+
+static void
+ble_hs_test_util_verify_tx_rd_pwr(void)
+{
+    uint8_t param_len;
+
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
+                                   &param_len);
+    TEST_ASSERT(param_len == 0);
+}
+
+int
+ble_hs_test_util_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                                int cmd_fail_idx, uint8_t hci_status)
+{
+    struct ble_hs_test_util_hci_ack acks[3];
+    int auto_pwr;
+    int rc;
+    int i;
+
+    auto_pwr = adv_fields->tx_pwr_lvl_is_present &&
+               adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+    i = 0;
+    if (auto_pwr) {
+        acks[i] = (struct ble_hs_test_util_hci_ack) {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
+            ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
+            {0},
+            1,
+        };
+        i++;
+    }
+
+    acks[i] = (struct ble_hs_test_util_hci_ack) {
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_DATA),
+        ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
+    };
+    i++;
+
+    memset(acks + i, 0, sizeof acks[i]);
+    ble_hs_test_util_hci_ack_set_seq(acks);
+
+    rc = ble_gap_adv_set_fields(adv_fields);
+    if (rc == 0 && auto_pwr) {
+        /* Verify tx of set advertising params command. */
+        ble_hs_test_util_verify_tx_rd_pwr();
+    }
+
+    return rc;
+}
+
+int
+ble_hs_test_util_adv_rsp_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                                    int cmd_fail_idx, uint8_t hci_status)
+{
+    struct ble_hs_test_util_hci_ack acks[3];
+    int auto_pwr;
+    int rc;
+    int i;
+
+    auto_pwr = adv_fields->tx_pwr_lvl_is_present &&
+               adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+    i = 0;
+    if (auto_pwr) {
+        acks[i] = (struct ble_hs_test_util_hci_ack) {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
+            ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
+            {0},
+            1,
+        };
+        i++;
+    }
+
+    acks[i] = (struct ble_hs_test_util_hci_ack) {
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA),
+        ble_hs_test_util_hci_misc_exp_status(i, cmd_fail_idx, hci_status),
+    };
+    i++;
+
+    memset(acks + i, 0, sizeof acks[i]);
+    ble_hs_test_util_hci_ack_set_seq(acks);
+
+    rc = ble_gap_adv_rsp_set_fields(adv_fields);
+    if (rc == 0 && auto_pwr) {
+        /* Verify tx of set advertising params command. */
+        ble_hs_test_util_verify_tx_rd_pwr();
+    }
+
+    return rc;
+}
+
+int
+ble_hs_test_util_adv_start(uint8_t own_addr_type, const ble_addr_t *peer_addr,
+                           const struct ble_gap_adv_params *adv_params,
+                           int32_t duration_ms,
+                           ble_gap_event_fn *cb, void *cb_arg,
+                           int fail_idx, uint8_t fail_status)
+{
+    struct ble_hs_test_util_hci_ack acks[6];
+    int rc;
+    int i;
+
+    i = 0;
+
+    acks[i] = (struct ble_hs_test_util_hci_ack) {
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_PARAMS),
+        fail_idx == i ? fail_status : 0,
+    };
+    i++;
+
+    acks[i] = (struct ble_hs_test_util_hci_ack) {
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+        ble_hs_test_util_hci_misc_exp_status(i, fail_idx, fail_status),
+    };
+    i++;
+
+    memset(acks + i, 0, sizeof acks[i]);
+
+    ble_hs_test_util_hci_ack_set_seq(acks);
+
+    rc = ble_gap_adv_start(own_addr_type, peer_addr,
+                           duration_ms, adv_params, cb, cb_arg);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_adv_stop(uint8_t hci_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set(
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+        hci_status);
+
+    rc = ble_gap_adv_stop();
+    return rc;
+}
+
+int
+ble_hs_test_util_wl_set(ble_addr_t *addrs, uint8_t addrs_count,
+                        int fail_idx, uint8_t fail_status)
+{
+    struct ble_hs_test_util_hci_ack acks[64];
+    int cmd_idx;
+    int rc;
+    int i;
+
+    TEST_ASSERT_FATAL(addrs_count < 63);
+
+    cmd_idx = 0;
+    acks[cmd_idx] = (struct ble_hs_test_util_hci_ack) {
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CLEAR_WHITE_LIST),
+        ble_hs_test_util_hci_misc_exp_status(cmd_idx, fail_idx, fail_status),
+    };
+    cmd_idx++;
+
+    for (i = 0; i < addrs_count; i++) {
+        acks[cmd_idx] = (struct ble_hs_test_util_hci_ack) {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_ADD_WHITE_LIST),
+            ble_hs_test_util_hci_misc_exp_status(cmd_idx, fail_idx, fail_status),
+        };
+
+        cmd_idx++;
+    }
+    memset(acks + cmd_idx, 0, sizeof acks[cmd_idx]);
+
+    ble_hs_test_util_hci_ack_set_seq(acks);
+    rc = ble_gap_wl_set(addrs, addrs_count);
+    return rc;
+}
+
+int
+ble_hs_test_util_conn_update(uint16_t conn_handle,
+                             struct ble_gap_upd_params *params,
+                             uint8_t hci_status)
+{
+    int rc;
+
+    /*
+     * 0xFF is magic value used for cases where we expect update over L2CAP to
+     * be triggered - in this case we don't need phony ack.
+     */
+    if (hci_status != 0xFF) {
+        ble_hs_test_util_hci_ack_set(
+                BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CONN_UPDATE),
+                hci_status);
+    }
+
+    rc = ble_gap_update_params(conn_handle, params);
+    return rc;
+}
+
+int
+ble_hs_test_util_set_our_irk(const uint8_t *irk, int fail_idx,
+                             uint8_t hci_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set_seq(((struct ble_hs_test_util_hci_ack[]) {
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+            ble_hs_test_util_hci_misc_exp_status(0, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
+            ble_hs_test_util_hci_misc_exp_status(1, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+            ble_hs_test_util_hci_misc_exp_status(2, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+            ble_hs_test_util_hci_misc_exp_status(3, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
+            ble_hs_test_util_hci_misc_exp_status(4, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
+            ble_hs_test_util_hci_misc_exp_status(5, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
+            ble_hs_test_util_hci_misc_exp_status(6, fail_idx, hci_status),
+        },
+        {
+            0
+        }
+    }));
+
+    rc = ble_hs_pvcy_set_our_irk(irk);
+    return rc;
+}
+
+int
+ble_hs_test_util_security_initiate(uint16_t conn_handle, uint8_t hci_status)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set(
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_START_ENCRYPT), hci_status);
+
+    rc = ble_gap_security_initiate(conn_handle);
+    return rc;
+}
+
+int
+ble_hs_test_util_l2cap_rx_first_frag(uint16_t conn_handle, uint16_t cid,
+                                     struct hci_data_hdr *hci_hdr,
+                                     struct os_mbuf *om)
+{
+    int rc;
+
+    om = ble_l2cap_prepend_hdr(om, cid, OS_MBUF_PKTLEN(om));
+    TEST_ASSERT_FATAL(om != NULL);
+
+    rc = ble_hs_test_util_l2cap_rx(conn_handle, hci_hdr, om);
+    return rc;
+}
+
+int
+ble_hs_test_util_l2cap_rx(uint16_t conn_handle,
+                          struct hci_data_hdr *hci_hdr,
+                          struct os_mbuf *om)
+{
+    struct ble_hs_conn *conn;
+    ble_l2cap_rx_fn *rx_cb;
+    int reject_cid;
+    int rc;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        rc = ble_l2cap_rx(conn, hci_hdr, om, &rx_cb, &reject_cid);
+    } else {
+        rc = os_mbuf_free_chain(om);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+    } else if (rc == 0) {
+        TEST_ASSERT_FATAL(rx_cb != NULL);
+        rc = rx_cb(conn->bhc_rx_chan);
+        ble_l2cap_remove_rx(conn, conn->bhc_rx_chan);
+    } else if (rc == BLE_HS_EAGAIN) {
+        /* More fragments on the way. */
+        rc = 0;
+    } else {
+        if (reject_cid != -1) {
+            ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, 0, 0, reject_cid);
+        }
+    }
+
+    return rc;
+}
+
+int
+ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle, uint16_t cid,
+                                       const void *data, int len)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    rc = os_mbuf_append(om, data, len);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    hci_hdr.hdh_handle_pb_bc =
+        ble_hs_hci_util_handle_pb_bc_join(conn_handle,
+                                          BLE_HCI_PB_FIRST_FLUSH, 0);
+    hci_hdr.hdh_len = OS_MBUF_PKTHDR(om)->omp_len;
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, cid, &hci_hdr, om);
+    return rc;
+}
+
+void
+ble_hs_test_util_set_att_mtu(uint16_t conn_handle, uint16_t mtu)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    if (mtu <= BLE_ATT_MTU_DFLT) {
+        return;
+    }
+
+    ble_hs_lock();
+
+    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    assert(rc == 0);
+    chan->my_mtu = mtu;
+    chan->peer_mtu = mtu;
+    chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
+
+    ble_hs_unlock();
+}
+
+int
+ble_hs_test_util_rx_att_mtu_cmd(uint16_t conn_handle, int is_req, uint16_t mtu)
+{
+    struct ble_att_mtu_cmd cmd;
+    uint8_t buf[BLE_ATT_MTU_CMD_SZ];
+    int rc;
+
+    cmd.bamc_mtu = mtu;
+
+    if (is_req) {
+        ble_att_mtu_req_write(buf, sizeof buf, &cmd);
+    } else {
+        ble_att_mtu_rsp_write(buf, sizeof buf, &cmd);
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_find_info_req(uint16_t conn_handle,
+                                      uint16_t start_handle,
+                                      uint16_t end_handle)
+{
+    struct ble_att_find_info_req req;
+    uint8_t buf[BLE_ATT_FIND_INFO_REQ_SZ];
+    int rc;
+
+    req.bafq_start_handle = start_handle;
+    req.bafq_end_handle = end_handle;
+
+    ble_att_find_info_req_write(buf, sizeof buf, &req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_find_type_value_req(uint16_t conn_handle,
+                                            uint16_t start_handle,
+                                            uint16_t end_handle,
+                                            uint16_t attr_type,
+                                            const void *attr_val,
+                                            uint16_t attr_len)
+{
+    struct ble_att_find_type_value_req req;
+    uint8_t buf[BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ + 16];
+    int rc;
+
+    TEST_ASSERT(attr_len <= 16);
+
+    req.bavq_start_handle = start_handle;
+    req.bavq_end_handle = end_handle;
+    req.bavq_attr_type = attr_type;
+
+    ble_att_find_type_value_req_write(buf, sizeof buf, &req);
+    memcpy(buf + BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, attr_val, attr_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ + attr_len);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_type_req(uint16_t conn_handle,
+                                      uint16_t start_handle,
+                                      uint16_t end_handle,
+                                      const ble_uuid_t *uuid)
+{
+    struct ble_att_read_type_req req;
+    uint8_t buf[BLE_ATT_READ_TYPE_REQ_SZ_128];
+    int req_len;
+    int rc;
+
+    req.batq_start_handle = start_handle;
+    req.batq_end_handle = end_handle;
+
+    ble_att_read_type_req_write(buf, sizeof buf, &req);
+
+    ble_uuid_flat(uuid, buf + BLE_ATT_READ_TYPE_REQ_BASE_SZ);
+    req_len = BLE_ATT_READ_TYPE_REQ_BASE_SZ + ble_uuid_length(uuid);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, req_len);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_type_req16(uint16_t conn_handle,
+                                        uint16_t start_handle,
+                                        uint16_t end_handle,
+                                        uint16_t uuid16)
+{
+    int rc;
+
+    rc = ble_hs_test_util_rx_att_read_type_req(conn_handle, start_handle,
+                                               end_handle,
+                                               BLE_UUID16_DECLARE(uuid16));
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_req(uint16_t conn_handle, uint16_t attr_handle)
+{
+    struct ble_att_read_req req;
+    uint8_t buf[BLE_ATT_READ_REQ_SZ];
+    int rc;
+
+    req.barq_handle = attr_handle;
+    ble_att_read_req_write(buf, sizeof buf, &req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_blob_req(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      uint16_t offset)
+{
+    struct ble_att_read_blob_req req;
+    uint8_t buf[BLE_ATT_READ_BLOB_REQ_SZ];
+    int rc;
+
+    req.babq_handle = attr_handle;
+    req.babq_offset = offset;
+    ble_att_read_blob_req_write(buf, sizeof buf, &req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_mult_req(uint16_t conn_handle,
+                                      const uint16_t *handles,
+                                      int num_handles)
+{
+    uint8_t buf[256];
+    int off;
+    int rc;
+    int i;
+
+    ble_att_read_mult_req_write(buf, sizeof buf);
+
+    off = BLE_ATT_READ_MULT_REQ_BASE_SZ;
+    for (i = 0; i < num_handles; i++) {
+        put_le16(buf + off, handles[i]);
+        off += 2;
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, off);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_group_type_req(uint16_t conn_handle,
+                                            uint16_t start_handle,
+                                            uint16_t end_handle,
+                                            const ble_uuid_t *uuid)
+{
+    struct ble_att_read_group_type_req req;
+    uint8_t buf[BLE_ATT_READ_GROUP_TYPE_REQ_SZ_128];
+    int req_len;
+    int rc;
+
+    req.bagq_start_handle = start_handle;
+    req.bagq_end_handle = end_handle;
+
+    ble_uuid_flat(uuid, buf + BLE_ATT_READ_TYPE_REQ_BASE_SZ);
+    req_len = BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ + ble_uuid_length(uuid);
+
+    ble_att_read_group_type_req_write(buf, sizeof buf, &req);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, req_len);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_read_group_type_req16(uint16_t conn_handle,
+                                              uint16_t start_handle,
+                                              uint16_t end_handle,
+                                              uint16_t uuid16)
+{
+    int rc;
+
+    rc = ble_hs_test_util_rx_att_read_group_type_req(conn_handle, start_handle,
+                                                     end_handle,
+                                                     BLE_UUID16_DECLARE(uuid16));
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_write_req(uint16_t conn_handle, uint16_t attr_handle,
+                                  const void *attr_val, uint16_t attr_len)
+{
+    struct ble_att_write_req req;
+    uint8_t buf[BLE_ATT_WRITE_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
+    int rc;
+
+    req.bawq_handle = attr_handle;
+    ble_att_write_req_write(buf, sizeof buf, &req);
+
+    memcpy(buf + BLE_ATT_WRITE_REQ_BASE_SZ, attr_val, attr_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_WRITE_REQ_BASE_SZ + attr_len);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_write_cmd(uint16_t conn_handle, uint16_t attr_handle,
+                                  const void *attr_val, uint16_t attr_len)
+{
+    struct ble_att_write_req req;
+    uint8_t buf[BLE_ATT_WRITE_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
+    int rc;
+
+    req.bawq_handle = attr_handle;
+    ble_att_write_cmd_write(buf, sizeof buf, &req);
+
+    memcpy(buf + BLE_ATT_WRITE_REQ_BASE_SZ, attr_val, attr_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_WRITE_REQ_BASE_SZ + attr_len);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_prep_write_req(uint16_t conn_handle,
+                                       uint16_t attr_handle,
+                                       uint16_t offset,
+                                       const void *attr_val,
+                                       uint16_t attr_len)
+{
+    struct ble_att_prep_write_cmd prep_req;
+    uint8_t buf[BLE_ATT_PREP_WRITE_CMD_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
+    int rc;
+
+    prep_req.bapc_handle = attr_handle;
+    prep_req.bapc_offset = offset;
+    ble_att_prep_write_req_write(buf, sizeof buf, &prep_req);
+    memcpy(buf + BLE_ATT_PREP_WRITE_CMD_BASE_SZ, attr_val, attr_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_PREP_WRITE_CMD_BASE_SZ + attr_len);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_exec_write_req(uint16_t conn_handle, uint8_t flags)
+{
+    struct ble_att_exec_write_req exec_req;
+    uint8_t buf[BLE_ATT_EXEC_WRITE_REQ_SZ];
+    int rc;
+
+    exec_req.baeq_flags = flags;
+    ble_att_exec_write_req_write(buf, sizeof buf, &exec_req);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf,
+                                                BLE_ATT_EXEC_WRITE_REQ_SZ);
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_notify_req(uint16_t conn_handle,
+                                   uint16_t attr_handle,
+                                   void *attr_val,
+                                   uint16_t attr_len)
+{
+    struct ble_att_notify_req req;
+    uint8_t buf[BLE_ATT_NOTIFY_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
+    int rc;
+
+    req.banq_handle = attr_handle;
+    ble_att_notify_req_write(buf, sizeof buf, &req);
+    memcpy(buf + BLE_ATT_NOTIFY_REQ_BASE_SZ, attr_val, attr_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_NOTIFY_REQ_BASE_SZ + attr_len);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_rx_att_indicate_req(uint16_t conn_handle,
+                                     uint16_t attr_handle,
+                                     void *attr_val,
+                                     uint16_t attr_len)
+{
+    struct ble_att_indicate_req req;
+    uint8_t buf[BLE_ATT_INDICATE_REQ_BASE_SZ + BLE_ATT_ATTR_MAX_LEN];
+    int rc;
+
+    req.baiq_handle = attr_handle;
+    ble_att_indicate_req_write(buf, sizeof buf, &req);
+    memcpy(buf + BLE_ATT_INDICATE_REQ_BASE_SZ, attr_val, attr_len);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_INDICATE_REQ_BASE_SZ + attr_len);
+
+    return rc;
+}
+
+void
+ble_hs_test_util_rx_att_err_rsp(uint16_t conn_handle, uint8_t req_op,
+                                uint8_t error_code, uint16_t err_handle)
+{
+    struct ble_att_error_rsp rsp;
+    uint8_t buf[BLE_ATT_ERROR_RSP_SZ];
+    int rc;
+
+    rsp.baep_req_op = req_op;
+    rsp.baep_handle = err_handle;
+    rsp.baep_error_code = error_code;
+
+    ble_att_error_rsp_write(buf, sizeof buf, &rsp);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+}
+
+void
+ble_hs_test_util_verify_tx_prep_write(uint16_t attr_handle, uint16_t offset,
+                                      const void *data, int data_len)
+{
+    struct ble_att_prep_write_cmd req;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) ==
+                BLE_ATT_PREP_WRITE_CMD_BASE_SZ + data_len);
+
+    om = os_mbuf_pullup(om, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    ble_att_prep_write_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.bapc_handle == attr_handle);
+    TEST_ASSERT(req.bapc_offset == offset);
+    TEST_ASSERT(os_mbuf_cmpf(om, BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+                             data, data_len) == 0);
+}
+
+void
+ble_hs_test_util_verify_tx_exec_write(uint8_t expected_flags)
+{
+    struct ble_att_exec_write_req req;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(om->om_len == BLE_ATT_EXEC_WRITE_REQ_SZ);
+
+    ble_att_exec_write_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.baeq_flags == expected_flags);
+}
+
+void
+ble_hs_test_util_verify_tx_find_type_value(uint16_t start_handle,
+                                           uint16_t end_handle,
+                                           uint16_t attr_type,
+                                           const void *value,
+                                           uint16_t value_len)
+{
+    struct ble_att_find_type_value_req req;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(om->om_len == BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ + value_len);
+
+    ble_att_find_type_value_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.bavq_start_handle == start_handle);
+    TEST_ASSERT(req.bavq_end_handle == end_handle);
+    TEST_ASSERT(req.bavq_attr_type == attr_type);
+    TEST_ASSERT(memcmp(om->om_data + BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ,
+                       value,
+                       value_len) == 0);
+}
+
+void
+ble_hs_test_util_verify_tx_disc_svc_uuid(const ble_uuid_t *uuid)
+{
+    uint8_t uuid_buf[16];
+
+    ble_uuid_flat(uuid, uuid_buf);
+    ble_hs_test_util_verify_tx_find_type_value(
+        1, 0xffff, BLE_ATT_UUID_PRIMARY_SERVICE,
+        uuid_buf, ble_uuid_length(uuid));
+}
+
+void
+ble_hs_test_util_verify_tx_read_rsp_gen(uint8_t att_op,
+                                        uint8_t *attr_data, int attr_len)
+{
+    struct os_mbuf *om;
+    uint8_t u8;
+    int rc;
+    int i;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, 1, &u8);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(u8 == att_op);
+
+    for (i = 0; i < attr_len; i++) {
+        rc = os_mbuf_copydata(om, i + 1, 1, &u8);
+        TEST_ASSERT(rc == 0);
+        TEST_ASSERT(u8 == attr_data[i]);
+    }
+
+    rc = os_mbuf_copydata(om, i + 1, 1, &u8);
+    TEST_ASSERT(rc != 0);
+}
+
+void
+ble_hs_test_util_verify_tx_read_rsp(uint8_t *attr_data, int attr_len)
+{
+    ble_hs_test_util_verify_tx_read_rsp_gen(BLE_ATT_OP_READ_RSP,
+                                            attr_data, attr_len);
+}
+
+void
+ble_hs_test_util_verify_tx_read_blob_rsp(uint8_t *attr_data, int attr_len)
+{
+    ble_hs_test_util_verify_tx_read_rsp_gen(BLE_ATT_OP_READ_BLOB_RSP,
+                                            attr_data, attr_len);
+}
+
+void
+ble_hs_test_util_verify_tx_write_rsp(void)
+{
+    struct os_mbuf *om;
+    uint8_t u8;
+    int rc;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, 1, &u8);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(u8 == BLE_ATT_OP_WRITE_RSP);
+}
+
+void
+ble_hs_test_util_verify_tx_mtu_cmd(int is_req, uint16_t mtu)
+{
+    struct ble_att_mtu_cmd cmd;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    if (is_req) {
+        ble_att_mtu_req_parse(om->om_data, om->om_len, &cmd);
+    } else {
+        ble_att_mtu_rsp_parse(om->om_data, om->om_len, &cmd);
+    }
+
+    TEST_ASSERT(cmd.bamc_mtu == mtu);
+}
+
+void
+ble_hs_test_util_verify_tx_find_info_rsp(
+    struct ble_hs_test_util_att_info_entry *entries)
+{
+    struct ble_hs_test_util_att_info_entry *entry;
+    struct ble_att_find_info_rsp rsp;
+    struct os_mbuf *om;
+    uint16_t handle;
+    uint8_t buf[BLE_ATT_FIND_INFO_RSP_BASE_SZ];
+    ble_uuid_any_t uuid;
+    int off;
+    int rc;
+
+    off = 0;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om);
+
+    rc = os_mbuf_copydata(om, off, sizeof buf, buf);
+    TEST_ASSERT(rc == 0);
+    off += sizeof buf;
+
+    ble_att_find_info_rsp_parse(buf, sizeof buf, &rsp);
+
+    for (entry = entries; entry->handle != 0; entry++) {
+        rc = os_mbuf_copydata(om, off, 2, &handle);
+        TEST_ASSERT(rc == 0);
+        off += 2;
+
+        handle = get_le16((void *)&handle);
+        TEST_ASSERT(handle == entry->handle);
+
+        if (entry->uuid->type == BLE_UUID_TYPE_16) {
+            TEST_ASSERT(rsp.bafp_format ==
+                        BLE_ATT_FIND_INFO_RSP_FORMAT_16BIT);
+
+            ble_uuid_init_from_att_mbuf(&uuid, om, off, 2);
+            TEST_ASSERT(rc == 0);
+            off += 2;
+        } else {
+            TEST_ASSERT(rsp.bafp_format ==
+                        BLE_ATT_FIND_INFO_RSP_FORMAT_128BIT);
+
+            rc = ble_uuid_init_from_att_mbuf(&uuid, om, off, 16);
+            TEST_ASSERT(rc == 0);
+            off += 16;
+        }
+
+        TEST_ASSERT(ble_uuid_cmp(entry->uuid, &uuid.u) == 0);
+    }
+
+    /* Ensure there is no extra data in the response. */
+    TEST_ASSERT(off == OS_MBUF_PKTHDR(om)->omp_len);
+}
+
+void
+ble_hs_test_util_verify_tx_read_group_type_rsp(
+    struct ble_hs_test_util_att_group_type_entry *entries)
+{
+    struct ble_hs_test_util_att_group_type_entry *entry;
+    struct ble_att_read_group_type_rsp rsp;
+    struct os_mbuf *om;
+    uint16_t u16;
+    ble_uuid_any_t uuid;
+    int off;
+    int rc;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om);
+
+    ble_att_read_group_type_rsp_parse(om->om_data, om->om_len, &rsp);
+
+    off = BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ;
+    for (entry = entries; entry->start_handle != 0; entry++) {
+        if (entry->uuid->type == BLE_UUID_TYPE_16) {
+            TEST_ASSERT(rsp.bagp_length ==
+                        BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16);
+        } else {
+            TEST_ASSERT(rsp.bagp_length ==
+                        BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_128);
+        }
+
+        rc = os_mbuf_copydata(om, off, 2, &u16);
+        TEST_ASSERT(rc == 0);
+        put_le16(&u16, u16);
+        TEST_ASSERT(u16 == entry->start_handle);
+        off += 2;
+
+        rc = os_mbuf_copydata(om, off, 2, &u16);
+        TEST_ASSERT(rc == 0);
+        put_le16(&u16, u16);
+        TEST_ASSERT(u16 == entry->end_handle);
+        off += 2;
+
+        if (entry->uuid->type == BLE_UUID_TYPE_16) {
+            rc = ble_uuid_init_from_att_mbuf(&uuid, om, off, 2);
+            TEST_ASSERT(rc == 0);
+        } else {
+            rc = ble_uuid_init_from_att_mbuf(&uuid, om, off, 16);
+            TEST_ASSERT(rc == 0);
+        }
+
+        TEST_ASSERT(ble_uuid_cmp(&uuid.u, entry->uuid) == 0);
+        off += ble_uuid_length(&uuid.u);
+    }
+
+    /* Ensure there is no extra data in the response. */
+    TEST_ASSERT(off == OS_MBUF_PKTLEN(om));
+}
+
+void
+ble_hs_test_util_verify_tx_err_rsp(uint8_t req_op, uint16_t handle,
+                                   uint8_t error_code)
+{
+    struct ble_att_error_rsp rsp;
+    struct os_mbuf *om;
+    uint8_t buf[BLE_ATT_ERROR_RSP_SZ];
+    int rc;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_att_error_rsp_parse(buf, sizeof buf, &rsp);
+
+    TEST_ASSERT(rsp.baep_req_op == req_op);
+    TEST_ASSERT(rsp.baep_handle == handle);
+    TEST_ASSERT(rsp.baep_error_code == error_code);
+}
+
+void
+ble_hs_test_util_verify_tx_write_cmd(uint16_t handle, const void *data,
+                                     uint16_t data_len)
+{
+    struct ble_att_write_req req;
+    struct os_mbuf *om;
+    uint8_t buf[BLE_ATT_WRITE_CMD_BASE_SZ];
+    int rc;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_att_write_cmd_parse(buf, sizeof buf, &req);
+
+    TEST_ASSERT(req.bawq_handle == handle);
+
+    os_mbuf_adj(om, sizeof buf);
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == data_len);
+    TEST_ASSERT(os_mbuf_cmpf(om, 0, data, data_len) == 0);
+}
+
+static struct os_mbuf *
+ble_hs_test_util_verify_tx_l2cap_sig_hdr(uint8_t op, uint8_t id,
+                                   uint16_t payload_len,
+                                   struct ble_l2cap_sig_hdr *out_hdr)
+{
+    struct ble_l2cap_sig_hdr hdr;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == BLE_L2CAP_SIG_HDR_SZ + payload_len);
+    ble_l2cap_sig_hdr_parse(om->om_data, om->om_len, &hdr);
+    TEST_ASSERT(hdr.op == op);
+    if (id != 0) {
+        TEST_ASSERT(hdr.identifier == id);
+    }
+    TEST_ASSERT(hdr.length == payload_len);
+
+    om->om_data += BLE_L2CAP_SIG_HDR_SZ;
+    om->om_len -= BLE_L2CAP_SIG_HDR_SZ;
+
+    if (out_hdr != NULL) {
+        *out_hdr = hdr;
+    }
+
+    return om;
+}
+
+int
+ble_hs_test_util_inject_rx_l2cap_sig(uint16_t conn_handle, uint8_t opcode,
+                              uint8_t id, void *cmd, uint16_t cmd_size)
+{
+    void *r;
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    int rc;
+
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(2, BLE_HCI_PB_FIRST_FLUSH,
+                         BLE_L2CAP_HDR_SZ + BLE_L2CAP_SIG_HDR_SZ + cmd_size);
+
+    r = ble_l2cap_sig_cmd_get(opcode, id, cmd_size, &om);
+    TEST_ASSERT_FATAL(r != NULL);
+
+    memcpy(r, cmd, cmd_size);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SIG,
+                                              &hci_hdr, om);
+    return rc;
+}
+
+/**
+ * @return  The L2CAP sig identifier in the request/response.
+ */
+uint8_t
+ble_hs_test_util_verify_tx_l2cap_sig(uint16_t opcode, void *cmd,
+                                     uint16_t cmd_size)
+{
+    struct ble_l2cap_sig_hdr hdr;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_verify_tx_l2cap_sig_hdr(opcode, 0, cmd_size, &hdr);
+    om = os_mbuf_pullup(om, cmd_size);
+
+    /* Verify payload. */
+    TEST_ASSERT(memcmp(om->om_data, cmd, cmd_size) == 0);
+
+    return hdr.identifier;
+}
+
+void
+ble_hs_test_util_verify_tx_l2cap(struct os_mbuf *txom)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    /* TODO Handle fragmentation */
+    TEST_ASSERT_FATAL(os_mbuf_cmpm(om, 0, txom, 0, OS_MBUF_PKTLEN(om)) == 0);
+}
+
+void
+ble_hs_test_util_inject_rx_l2cap(uint16_t conn_handle, uint16_t cid,
+                                 struct os_mbuf *rxom)
+{
+    struct hci_data_hdr hci_hdr;
+    int rc;
+
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(2, BLE_HCI_PB_FIRST_FLUSH,
+                                             BLE_L2CAP_HDR_SZ +
+                                             BLE_L2CAP_SIG_HDR_SZ +
+                                             OS_MBUF_PKTLEN(rxom));
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, cid, &hci_hdr, rxom);
+    TEST_ASSERT(rc == 0);
+}
+
+static void
+ble_l2cap_test_update_req_swap(struct ble_l2cap_sig_update_req *dst,
+                               struct ble_l2cap_sig_update_req *src)
+{
+    dst->itvl_min = le16toh(src->itvl_min);
+    dst->itvl_max = le16toh(src->itvl_max);
+    dst->slave_latency = le16toh(src->slave_latency);
+    dst->timeout_multiplier = le16toh(src->timeout_multiplier);
+}
+
+static void
+ble_l2cap_test_update_req_parse(void *payload, int len,
+                               struct ble_l2cap_sig_update_req *dst)
+{
+    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_UPDATE_REQ_SZ);
+    ble_l2cap_test_update_req_swap(dst, payload);
+}
+
+/**
+ * @return                      The L2CAP sig identifier in the request.
+ */
+uint8_t
+ble_hs_test_util_verify_tx_l2cap_update_req(
+    struct ble_l2cap_sig_update_params *params)
+{
+    struct ble_l2cap_sig_update_req req;
+    struct ble_l2cap_sig_hdr hdr;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_verify_tx_l2cap_sig_hdr(BLE_L2CAP_SIG_OP_UPDATE_REQ,
+                                                  0,
+                                                  BLE_L2CAP_SIG_UPDATE_REQ_SZ,
+                                                  &hdr);
+
+    /* Verify payload. */
+    ble_l2cap_test_update_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.itvl_min == params->itvl_min);
+    TEST_ASSERT(req.itvl_max == params->itvl_max);
+    TEST_ASSERT(req.slave_latency == params->slave_latency);
+    TEST_ASSERT(req.timeout_multiplier == params->timeout_multiplier);
+
+    return hdr.identifier;
+}
+
+static void
+ble_l2cap_sig_update_rsp_parse(void *payload, int len,
+                               struct ble_l2cap_sig_update_rsp *dst)
+{
+    struct ble_l2cap_sig_update_rsp *src = payload;
+
+    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_UPDATE_RSP_SZ);
+    dst->result = le16toh(src->result);
+}
+
+int
+ble_hs_test_util_rx_l2cap_update_rsp(uint16_t conn_handle,
+                                     uint8_t id, uint16_t result)
+{
+    struct ble_l2cap_sig_update_rsp *rsp;
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    int rc;
+
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + BLE_L2CAP_SIG_HDR_SZ + BLE_L2CAP_SIG_UPDATE_RSP_SZ);
+
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_UPDATE_RSP, id,
+                                BLE_L2CAP_SIG_UPDATE_RSP_SZ, &om);
+    TEST_ASSERT_FATAL(rsp != NULL);
+
+    rsp->result = htole16(result);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SIG,
+                                              &hci_hdr, om);
+    return rc;
+}
+
+void
+ble_hs_test_util_verify_tx_l2cap_update_rsp(uint8_t exp_id,
+                                            uint16_t exp_result)
+{
+    struct ble_l2cap_sig_update_rsp rsp;
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_verify_tx_l2cap_sig_hdr(BLE_L2CAP_SIG_OP_UPDATE_RSP,
+                                            exp_id,
+                                            BLE_L2CAP_SIG_UPDATE_RSP_SZ,
+                                            NULL);
+
+    ble_l2cap_sig_update_rsp_parse(om->om_data, om->om_len, &rsp);
+    TEST_ASSERT(rsp.result == exp_result);
+}
+
+void
+ble_hs_test_util_set_static_rnd_addr(const uint8_t *addr)
+{
+    int rc;
+
+    ble_hs_test_util_hci_ack_set(
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_RAND_ADDR), 0);
+
+    rc = ble_hs_id_set_rnd(addr);
+
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_hci_out_first();
+}
+
+struct os_mbuf *
+ble_hs_test_util_om_from_flat(const void *buf, uint16_t len)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_mbuf_from_flat(buf, len);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    return om;
+}
+
+int
+ble_hs_test_util_flat_attr_cmp(const struct ble_hs_test_util_flat_attr *a,
+                               const struct ble_hs_test_util_flat_attr *b)
+{
+    if (a->handle != b->handle) {
+        return -1;
+    }
+    if (a->offset != b->offset) {
+        return -1;
+    }
+    if (a->value_len != b->value_len) {
+        return -1;
+    }
+    return memcmp(a->value, b->value, a->value_len);
+}
+
+void
+ble_hs_test_util_attr_to_flat(struct ble_hs_test_util_flat_attr *flat,
+                              const struct ble_gatt_attr *attr)
+{
+    int rc;
+
+    flat->handle = attr->handle;
+    flat->offset = attr->offset;
+    rc = ble_hs_mbuf_to_flat(attr->om, flat->value, sizeof flat->value,
+                           &flat->value_len);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+void
+ble_hs_test_util_attr_from_flat(struct ble_gatt_attr *attr,
+                                const struct ble_hs_test_util_flat_attr *flat)
+{
+    attr->handle = flat->handle;
+    attr->offset = flat->offset;
+    attr->om = ble_hs_test_util_om_from_flat(flat->value, flat->value_len);
+}
+
+int
+ble_hs_test_util_read_local_flat(uint16_t attr_handle, uint16_t max_len,
+                                 void *buf, uint16_t *out_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    rc = ble_att_svr_read_local(attr_handle, &om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(om) <= max_len);
+
+    rc = os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), buf);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    *out_len = OS_MBUF_PKTLEN(om);
+
+    rc = os_mbuf_free_chain(om);
+    TEST_ASSERT_FATAL(rc == 0);
+    return 0;
+}
+
+int
+ble_hs_test_util_write_local_flat(uint16_t attr_handle,
+                                  const void *buf, uint16_t buf_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(buf, buf_len);
+    rc = ble_att_svr_write_local(attr_handle, om);
+    return rc;
+}
+
+int
+ble_hs_test_util_gatt_write_flat(uint16_t conn_handle, uint16_t attr_handle,
+                                 const void *data, uint16_t data_len,
+                                 ble_gatt_attr_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(data, data_len);
+    rc = ble_gattc_write(conn_handle, attr_handle, om, cb, cb_arg);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_gatt_write_no_rsp_flat(uint16_t conn_handle,
+                                        uint16_t attr_handle,
+                                        const void *data, uint16_t data_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(data, data_len);
+    rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, om);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_gatt_write_long_flat(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      const void *data, uint16_t data_len,
+                                      ble_gatt_attr_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *om;
+    uint16_t offset = 0;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(data, data_len);
+    rc = ble_gattc_write_long(conn_handle, attr_handle, offset, om, cb, cb_arg);
+
+    return rc;
+}
+
+static int
+ble_hs_test_util_mbuf_chain_len(const struct os_mbuf *om)
+{
+    int count;
+
+    count = 0;
+    while (om != NULL) {
+        count++;
+        om = SLIST_NEXT(om, om_next);
+    }
+
+    return count;
+}
+
+static int
+ble_hs_test_util_num_mbufs(void)
+{
+    return os_msys_count() + ble_hs_hci_frag_num_mbufs();
+}
+
+static int
+ble_hs_test_util_num_mbufs_free(void)
+{
+    return os_msys_num_free() + ble_hs_hci_frag_num_mbufs_free();
+}
+
+struct os_mbuf *
+ble_hs_test_util_mbuf_alloc_all_but(int count)
+{
+    struct os_mbuf *prev;
+    struct os_mbuf *om;
+    int rc;
+    int i;
+
+    /* Allocate all available mbufs and put them in a single chain. */
+    prev = NULL;
+    while (1) {
+        om = os_msys_get(0, 0);
+        if (om == NULL) {
+            break;
+        }
+
+        if (prev != NULL) {
+            SLIST_NEXT(om, om_next) = prev;
+        }
+
+        prev = om;
+    }
+
+    /* Now free 'count' mbufs. */
+    for (i = 0; i < count; i++) {
+        TEST_ASSERT_FATAL(prev != NULL);
+        om = SLIST_NEXT(prev, om_next);
+        rc = os_mbuf_free(prev);
+        TEST_ASSERT_FATAL(rc == 0);
+
+        prev = om;
+    }
+
+    return prev;
+}
+
+int
+ble_hs_test_util_mbuf_count(const struct ble_hs_test_util_mbuf_params *params)
+{
+    const struct ble_att_prep_entry *prep;
+    const struct os_mbuf_pkthdr *omp;
+    const struct ble_l2cap_chan *chan;
+    const struct ble_hs_conn *conn;
+    const struct os_mbuf *om;
+    int count;
+    int i;
+
+    ble_hs_process_rx_data_queue();
+
+    count = ble_hs_test_util_num_mbufs_free();
+
+    if (params->prev_tx) {
+        count += ble_hs_test_util_mbuf_chain_len(ble_hs_test_util_prev_tx_cur);
+        STAILQ_FOREACH(omp, &ble_hs_test_util_prev_tx_queue, omp_next) {
+            om = OS_MBUF_PKTHDR_TO_MBUF(omp);
+            count += ble_hs_test_util_mbuf_chain_len(om);
+        }
+    }
+
+    ble_hs_lock();
+    for (i = 0; ; i++) {
+        conn = ble_hs_conn_find_by_idx(i);
+        if (conn == NULL) {
+            break;
+        }
+
+        if (params->rx_queue) {
+            SLIST_FOREACH(chan, &conn->bhc_channels, next) {
+                count += ble_hs_test_util_mbuf_chain_len(chan->rx_buf);
+            }
+        }
+
+        if (params->prep_list) {
+            SLIST_FOREACH(prep, &conn->bhc_att_svr.basc_prep_list, bape_next) {
+                count += ble_hs_test_util_mbuf_chain_len(prep->bape_value);
+            }
+        }
+    }
+    ble_hs_unlock();
+
+    return count;
+}
+
+void
+ble_hs_test_util_assert_mbufs_freed(
+    const struct ble_hs_test_util_mbuf_params *params)
+{
+    static const struct ble_hs_test_util_mbuf_params dflt = {
+        .prev_tx = 1,
+        .rx_queue = 1,
+        .prep_list = 1,
+    };
+
+    int count;
+
+    if (params == NULL) {
+        params = &dflt;
+    }
+
+    count = ble_hs_test_util_mbuf_count(params);
+    TEST_ASSERT(count == ble_hs_test_util_num_mbufs());
+}
+
+static int
+ble_hs_test_util_pkt_txed(struct os_mbuf *om, void *arg)
+{
+    ble_hs_test_util_prev_tx_enqueue(om);
+    return 0;
+}
+
+static int
+ble_hs_test_util_hci_txed(uint8_t *cmdbuf, void *arg)
+{
+    ble_hs_test_util_hci_out_enqueue(cmdbuf);
+    ble_hci_trans_buf_free(cmdbuf);
+    return 0;
+}
+
+int
+ble_hs_test_util_num_cccds(void)
+{
+    struct ble_store_value_cccd val;
+    struct ble_store_key_cccd key = { };
+    int rc;
+
+    key.peer_addr = *BLE_ADDR_ANY;
+    for (key.idx = 0; ; key.idx++) {
+        rc = ble_store_read_cccd(&key, &val);
+        switch (rc) {
+        case 0:
+            break;
+
+        case BLE_HS_ENOENT:
+            return key.idx;
+
+        default:
+            TEST_ASSERT_FATAL(0);
+        }
+    }
+}
+
+int
+ble_hs_test_util_num_our_secs(void)
+{
+    struct ble_store_value_sec val;
+    struct ble_store_key_sec key = { };
+    int rc;
+
+    key.peer_addr = *BLE_ADDR_ANY;
+    for (key.idx = 0; ; key.idx++) {
+        rc = ble_store_read_our_sec(&key, &val);
+        switch (rc) {
+        case 0:
+            break;
+
+        case BLE_HS_ENOENT:
+            return key.idx;
+
+        default:
+            TEST_ASSERT_FATAL(0);
+        }
+    }
+}
+
+int
+ble_hs_test_util_num_peer_secs(void)
+{
+    struct ble_store_value_sec val;
+    struct ble_store_key_sec key = { };
+    int rc;
+
+    key.peer_addr = *BLE_ADDR_ANY;
+    for (key.idx = 0; ; key.idx++) {
+        rc = ble_store_read_peer_sec(&key, &val);
+        switch (rc) {
+        case 0:
+            break;
+
+        case BLE_HS_ENOENT:
+            return key.idx;
+
+        default:
+            TEST_ASSERT_FATAL(0);
+        }
+    }
+}
+
+static int
+ble_hs_test_util_store_read(int obj_type, const union ble_store_key *key,
+                            union ble_store_value *value)
+{
+    int rc;
+
+    ble_sm_test_store_obj_type = obj_type;
+    ble_sm_test_store_key = *key;
+
+    rc = ble_store_config_read(obj_type, key, value);
+    ble_sm_test_store_value = *value;
+
+    return rc;
+}
+
+static int
+ble_hs_test_util_store_write(int obj_type, const union ble_store_value *value)
+{
+    int rc;
+
+    ble_sm_test_store_obj_type = obj_type;
+
+    rc = ble_store_config_write(obj_type, value);
+    ble_sm_test_store_value = *value;
+
+    return rc;
+}
+
+static int
+ble_hs_test_util_store_delete(int obj_type, const union ble_store_key *key)
+{
+    int rc;
+
+    ble_sm_test_store_obj_type = obj_type;
+    ble_sm_test_store_key = *key;
+
+    rc = ble_store_config_delete(obj_type, key);
+    return rc;
+}
+
+void
+ble_hs_test_util_reg_svcs(const struct ble_gatt_svc_def *svcs,
+                          ble_gatt_register_fn *reg_cb,
+                          void *cb_arg)
+{
+    int rc;
+
+    ble_hs_cfg.gatts_register_cb = reg_cb;
+    ble_hs_cfg.gatts_register_arg = cb_arg;
+
+    rc = ble_gatts_reset();
+    TEST_ASSERT_FATAL(rc == 0);
+
+    rc = ble_gatts_add_svcs(svcs);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    rc = ble_gatts_start();
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+
+void
+ble_hs_test_util_init_no_sysinit_no_start(void)
+{
+    STAILQ_INIT(&ble_hs_test_util_prev_tx_queue);
+    ble_hs_test_util_prev_tx_cur = NULL;
+
+    ble_hs_hci_set_phony_ack_cb(NULL);
+
+    ble_hci_trans_cfg_ll(ble_hs_test_util_hci_txed, NULL,
+                         ble_hs_test_util_pkt_txed, NULL);
+
+    ble_hs_test_util_hci_ack_set_startup();
+
+    ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
+
+    ble_hs_max_services = 16;
+    ble_hs_max_client_configs = 32;
+    ble_hs_max_attrs = 64;
+
+    ble_hs_test_util_hci_out_clear();
+
+    ble_hs_cfg.store_read_cb = ble_hs_test_util_store_read;
+    ble_hs_cfg.store_write_cb = ble_hs_test_util_store_write;
+    ble_hs_cfg.store_delete_cb = ble_hs_test_util_store_delete;
+
+    ble_store_clear();
+}
+
+void
+ble_hs_test_util_init_no_start(void)
+{
+    sysinit();
+    ble_hs_test_util_init_no_sysinit_no_start();
+}
+
+void
+ble_hs_test_util_init(void)
+{
+    int rc;
+
+    ble_hs_test_util_init_no_start();
+
+    rc = ble_hs_start();
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_hci_out_clear();
+    ble_hs_test_util_hci_acks_clear();
+
+    /* Clear random address. */
+    ble_hs_id_rnd_reset();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util.h b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util.h
new file mode 100644
index 00000000..411443cf
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util.h
@@ -0,0 +1,305 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_TEST_UTIL_
+#define H_BLE_HS_TEST_UTIL_
+
+#include <inttypes.h>
+#include "host/ble_gap.h"
+#include "ble_hs_priv.h"
+#include "ble_hs_test_util_hci.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* TODO  this is currently copied from ble_ll_conn_priv.h and eventually
+ * should not be needed...
+ */
+struct hci_create_conn
+{
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    uint8_t filter_policy;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+    uint8_t own_addr_type;
+    uint16_t conn_itvl_min;
+    uint16_t conn_itvl_max;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint16_t min_ce_len;
+    uint16_t max_ce_len;
+};
+
+struct ble_hs_conn;
+struct ble_l2cap_chan;
+struct hci_disconn_complete;
+struct hci_create_conn;
+
+#define BLE_HS_TEST_UTIL_LE_OPCODE(ocf) \
+    ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE, (ocf))
+
+#define BLE_HS_TEST_UTIL_PUB_ADDR_VAL { 0x0a, 0x54, 0xab, 0x49, 0x7f, 0x06 }
+
+extern const struct ble_gap_adv_params ble_hs_test_util_adv_params;
+
+struct ble_hs_test_util_flat_attr {
+    uint16_t handle;
+    uint16_t offset;
+    uint8_t value[BLE_ATT_ATTR_MAX_LEN];
+    uint16_t value_len;
+};
+
+struct ble_hs_test_util_mbuf_params {
+    unsigned prev_tx:1;
+    unsigned rx_queue:1;
+    unsigned prep_list:1;
+};
+
+struct ble_hs_test_util_att_info_entry {
+    uint16_t handle;        /* 0 on last entry */
+    const ble_uuid_t *uuid;
+};
+
+struct ble_hs_test_util_att_group_type_entry {
+    uint16_t start_handle;  /* 0 on last entry */
+    uint16_t end_handle;    /* 0 on last entry */
+    const ble_uuid_t *uuid;
+};
+
+#define BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(handle, pb, len) \
+    ((struct hci_data_hdr) {                            \
+        .hdh_handle_pb_bc = ((handle)  << 0) |          \
+                            ((pb)      << 12),          \
+        .hdh_len = (len)                                \
+    })
+
+#define BLE_HS_TEST_CONN_FEAT_ALL               (0xFF)
+#define BLE_HS_TEST_CONN_FEAT_NO_CONN_PARAM     (0xFD)
+
+void ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om);
+struct os_mbuf *ble_hs_test_util_prev_tx_dequeue(void);
+struct os_mbuf *ble_hs_test_util_prev_tx_dequeue_pullup(void);
+int ble_hs_test_util_prev_tx_queue_sz(void);
+void ble_hs_test_util_prev_tx_queue_clear(void);
+
+void ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t own_addr_type,
+                                      const uint8_t *our_rpa,
+                                      uint8_t peer_addr_type,
+                                      const uint8_t *peer_id_addr,
+                                      const uint8_t *peer_rpa,
+                                      uint8_t conn_features,
+                                      ble_gap_event_fn *cb, void *cb_arg);
+void ble_hs_test_util_create_conn(uint16_t handle, const uint8_t *addr,
+                                  ble_gap_event_fn *cb, void *cb_arg);
+void ble_hs_test_util_create_conn_feat(uint16_t handle, const uint8_t *addr,
+                                  uint8_t conn_features,
+                                  ble_gap_event_fn *cb, void *cb_arg);
+int ble_hs_test_util_connect(uint8_t own_addr_type,
+                                   const ble_addr_t *peer_addr,
+                                   int32_t duration_ms,
+                                   const struct ble_gap_conn_params *params,
+                                   ble_gap_event_fn *cb,
+                                   void *cb_arg,
+                                   uint8_t ack_status);
+int ble_hs_test_util_conn_cancel(uint8_t ack_status);
+void ble_hs_test_util_rx_conn_cancel_evt(void);
+void ble_hs_test_util_conn_cancel_full(void);
+int ble_hs_test_util_conn_terminate(uint16_t conn_handle, uint8_t hci_status);
+void ble_hs_test_util_rx_disconn_complete(uint16_t conn_handle,
+                                          uint8_t reason);
+void ble_hs_test_util_conn_disconnect(uint16_t conn_handle);
+int ble_hs_test_util_disc(uint8_t own_addr_type, int32_t duration_ms,
+                          const struct ble_gap_disc_params *disc_params,
+                          ble_gap_event_fn *cb, void *cb_arg, int fail_idx,
+                          uint8_t fail_status);
+int ble_hs_test_util_disc_cancel(uint8_t ack_status);
+int ble_hs_test_util_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                                    int cmd_fail_idx, uint8_t hci_status);
+int ble_hs_test_util_adv_rsp_set_fields(
+    const struct ble_hs_adv_fields *adv_fields,
+    int cmd_fail_idx, uint8_t hci_status);
+int ble_hs_test_util_adv_start(uint8_t own_addr_type,
+                               const ble_addr_t *peer_addr,
+                               const struct ble_gap_adv_params *adv_params,
+                               int32_t duration_ms,
+                               ble_gap_event_fn *cb, void *cb_arg,
+                               int fail_idx, uint8_t fail_status);
+int ble_hs_test_util_adv_stop(uint8_t hci_status);
+int ble_hs_test_util_wl_set(ble_addr_t *addrs, uint8_t addrs_count,
+                            int fail_idx, uint8_t fail_status);
+int ble_hs_test_util_conn_update(uint16_t conn_handle,
+                                 struct ble_gap_upd_params *params,
+                                 uint8_t hci_status);
+int ble_hs_test_util_set_our_irk(const uint8_t *irk, int fail_idx,
+                                 uint8_t hci_status);
+int ble_hs_test_util_security_initiate(uint16_t conn_handle,
+                                       uint8_t hci_status);
+int ble_hs_test_util_l2cap_rx_first_frag(uint16_t conn_handle, uint16_t cid,
+                                         struct hci_data_hdr *hci_hdr,
+                                         struct os_mbuf *om);
+int ble_hs_test_util_l2cap_rx(uint16_t conn_handle,
+                              struct hci_data_hdr *hci_hdr,
+                              struct os_mbuf *om);
+int ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle, uint16_t cid,
+                                           const void *data, int len);
+uint8_t ble_hs_test_util_verify_tx_l2cap_sig(uint16_t opcode, void *cmd,
+                                                 uint16_t cmd_size);
+int ble_hs_test_util_inject_rx_l2cap_sig(uint16_t conn_handle, uint8_t opcode,
+                                     uint8_t id, void *cmd, uint16_t cmd_size);
+void ble_hs_test_util_verify_tx_l2cap(struct os_mbuf *txom);
+void ble_hs_test_util_inject_rx_l2cap(uint16_t conn_handle, uint16_t cid,
+                                      struct os_mbuf *rxom);
+void ble_hs_test_util_set_att_mtu(uint16_t conn_handle, uint16_t mtu);
+int ble_hs_test_util_rx_att_mtu_cmd(uint16_t conn_handle, int is_req,
+                                    uint16_t mtu);
+int ble_hs_test_util_rx_att_find_info_req(uint16_t conn_handle,
+                                          uint16_t start_handle,
+                                          uint16_t end_handle);
+int ble_hs_test_util_rx_att_find_type_value_req(uint16_t conn_handle,
+                                                uint16_t start_handle,
+                                                uint16_t end_handle,
+                                                uint16_t attr_type,
+                                                const void *attr_val,
+                                                uint16_t attr_len);
+int ble_hs_test_util_rx_att_read_type_req(uint16_t conn_handle,
+                                          uint16_t start_handle,
+                                          uint16_t end_handle,
+                                          const ble_uuid_t *uuid);
+int ble_hs_test_util_rx_att_read_type_req16(uint16_t conn_handle,
+                                            uint16_t start_handle,
+                                            uint16_t end_handle,
+                                            uint16_t uuid16);
+int ble_hs_test_util_rx_att_read_req(uint16_t conn_handle,
+                                     uint16_t attr_handle);
+int ble_hs_test_util_rx_att_read_blob_req(uint16_t conn_handle,
+                                          uint16_t attr_handle,
+                                          uint16_t offset);
+int ble_hs_test_util_rx_att_read_mult_req(uint16_t conn_handle,
+                                          const uint16_t *handles,
+                                          int num_handles);
+int ble_hs_test_util_rx_att_read_group_type_req(uint16_t conn_handle,
+                                                uint16_t start_handle,
+                                                uint16_t end_handle,
+                                                const ble_uuid_t *uuid);
+int ble_hs_test_util_rx_att_read_group_type_req16(uint16_t conn_handle,
+                                                  uint16_t start_handle,
+                                                  uint16_t end_handle,
+                                                  uint16_t uuid16);
+int ble_hs_test_util_rx_att_write_req(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      const void *attr_val,
+                                      uint16_t attr_len);
+int ble_hs_test_util_rx_att_write_cmd(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      const void *attr_val,
+                                      uint16_t attr_len);
+int ble_hs_test_util_rx_att_prep_write_req(uint16_t conn_handle,
+                                           uint16_t attr_handle,
+                                           uint16_t offset,
+                                           const void *attr_val,
+                                           uint16_t attr_len);
+int ble_hs_test_util_rx_att_exec_write_req(uint16_t conn_handle,
+                                           uint8_t flags);
+int ble_hs_test_util_rx_att_notify_req(uint16_t conn_handle,
+                                       uint16_t attr_handle,
+                                       void *attr_val,
+                                       uint16_t attr_len);
+int ble_hs_test_util_rx_att_indicate_req(uint16_t conn_handle,
+                                         uint16_t attr_handle,
+                                         void *attr_val,
+                                         uint16_t attr_len);
+void ble_hs_test_util_rx_att_err_rsp(uint16_t conn_handle, uint8_t req_op,
+                                     uint8_t error_code, uint16_t err_handle);
+void ble_hs_test_util_verify_tx_prep_write(uint16_t attr_handle,
+                                           uint16_t offset,
+                                           const void *data, int data_len);
+void ble_hs_test_util_verify_tx_exec_write(uint8_t expected_flags);
+void ble_hs_test_util_verify_tx_find_type_value(uint16_t start_handle,
+                                                uint16_t end_handle,
+                                                uint16_t attr_type,
+                                                const void *value,
+                                                uint16_t value_len);
+void ble_hs_test_util_verify_tx_disc_svc_uuid(const ble_uuid_t *uuid);
+void ble_hs_test_util_verify_tx_read_rsp(uint8_t *attr_data, int attr_len);
+void ble_hs_test_util_verify_tx_read_blob_rsp(uint8_t *attr_data,
+                                              int attr_len);
+void ble_hs_test_util_verify_tx_write_rsp(void);
+void ble_hs_test_util_verify_tx_find_info_rsp(
+    struct ble_hs_test_util_att_info_entry *entries);
+void ble_hs_test_util_verify_tx_mtu_cmd(int is_req, uint16_t mtu);
+void ble_hs_test_util_verify_tx_read_group_type_rsp(
+    struct ble_hs_test_util_att_group_type_entry *entries);
+void ble_hs_test_util_verify_tx_err_rsp(uint8_t req_op, uint16_t handle,
+                                        uint8_t error_code);
+void ble_hs_test_util_verify_tx_write_cmd(uint16_t handle, const void *data,
+                                          uint16_t data_len);
+
+uint8_t ble_hs_test_util_verify_tx_l2cap_update_req(
+    struct ble_l2cap_sig_update_params *params);
+int ble_hs_test_util_rx_l2cap_update_rsp(uint16_t conn_handle,
+                                         uint8_t id, uint16_t result);
+void ble_hs_test_util_verify_tx_l2cap_update_rsp(uint8_t exp_id,
+                                                 uint16_t exp_result);
+void ble_hs_test_util_set_static_rnd_addr(const uint8_t *addr);
+struct os_mbuf *ble_hs_test_util_om_from_flat(const void *buf, uint16_t len);
+int ble_hs_test_util_flat_attr_cmp(const struct ble_hs_test_util_flat_attr *a,
+                                   const struct ble_hs_test_util_flat_attr *b);
+void ble_hs_test_util_attr_to_flat(struct ble_hs_test_util_flat_attr *flat,
+                                   const struct ble_gatt_attr *attr);
+void ble_hs_test_util_attr_from_flat(
+    struct ble_gatt_attr *attr, const struct ble_hs_test_util_flat_attr *flat);
+int ble_hs_test_util_read_local_flat(uint16_t attr_handle, uint16_t max_len,
+                                     void *buf, uint16_t *out_len);
+int ble_hs_test_util_write_local_flat(uint16_t attr_handle,
+                                      const void *buf, uint16_t buf_len);
+int ble_hs_test_util_gatt_write_flat(uint16_t conn_handle,
+                                     uint16_t attr_handle,
+                                     const void *data, uint16_t data_len,
+                                     ble_gatt_attr_fn *cb, void *cb_arg);
+int ble_hs_test_util_gatt_write_no_rsp_flat(uint16_t conn_handle,
+                                            uint16_t attr_handle,
+                                            const void *data,
+                                            uint16_t data_len);
+int ble_hs_test_util_gatt_write_long_flat(uint16_t conn_handle,
+                                          uint16_t attr_handle,
+                                          const void *data, uint16_t data_len,
+                                          ble_gatt_attr_fn *cb, void *cb_arg);
+struct os_mbuf *ble_hs_test_util_mbuf_alloc_all_but(int count);
+int ble_hs_test_util_mbuf_count(
+    const struct ble_hs_test_util_mbuf_params *params);
+void ble_hs_test_util_assert_mbufs_freed(
+    const struct ble_hs_test_util_mbuf_params *params);
+void ble_hs_test_util_post_test(void *arg);
+int ble_hs_test_util_num_cccds(void);
+int ble_hs_test_util_num_our_secs(void);
+int ble_hs_test_util_num_peer_secs(void);
+void ble_hs_test_util_reg_svcs(const struct ble_gatt_svc_def *svcs,
+                               ble_gatt_register_fn *reg_cb,
+                               void *cb_arg);
+void ble_hs_test_util_init_no_start(void);
+void ble_hs_test_util_init_no_sysinit_no_start(void);
+void ble_hs_test_util_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util_hci.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util_hci.c
new file mode 100644
index 00000000..a53c5d9f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util_hci.c
@@ -0,0 +1,616 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "testutil/testutil.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "transport/ram/ble_hci_ram.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN  (5)
+#define BLE_HCI_EVENT_CMD_STATUS_LEN        (6)
+#define BLE_HCI_ADD_TO_RESOLV_LIST_LEN      (39)
+#define BLE_HCI_LE_SET_PRIVACY_MODE_LEN     (8)
+#define BLE_HCI_DISCONNECT_CMD_LEN          (3)
+#define BLE_HCI_EVENT_HDR_LEN               (2)
+#define BLE_HCI_EVENT_DISCONN_COMPLETE_LEN  (4)
+
+#define BLE_HS_TEST_UTIL_PREV_HCI_TX_CNT      64
+
+static uint8_t
+ble_hs_test_util_hci_out_queue[BLE_HS_TEST_UTIL_PREV_HCI_TX_CNT][260];
+static int ble_hs_test_util_hci_out_queue_sz;
+static uint8_t ble_hs_test_util_hci_out_cur[260];
+
+static struct ble_hs_test_util_hci_ack
+ble_hs_test_util_hci_acks[BLE_HS_TEST_UTIL_PHONY_ACK_MAX];
+static int ble_hs_test_util_hci_num_acks;
+
+/*****************************************************************************
+ * $tx queue                                                                 *
+ *****************************************************************************/
+
+void
+ble_hs_test_util_hci_out_adj(int count)
+{
+    if (count >= 0) {
+        TEST_ASSERT(ble_hs_test_util_hci_out_queue_sz >= count);
+
+        ble_hs_test_util_hci_out_queue_sz -= count;
+        if (ble_hs_test_util_hci_out_queue_sz > 0) {
+            memmove(
+                ble_hs_test_util_hci_out_queue,
+                ble_hs_test_util_hci_out_queue + count,
+                sizeof ble_hs_test_util_hci_out_queue[0] *
+                ble_hs_test_util_hci_out_queue_sz);
+        }
+    } else {
+        TEST_ASSERT(ble_hs_test_util_hci_out_queue_sz >= -count);
+
+        ble_hs_test_util_hci_out_adj(
+            ble_hs_test_util_hci_out_queue_sz + count);
+    }
+}
+
+void *
+ble_hs_test_util_hci_out_first(void)
+{
+    if (ble_hs_test_util_hci_out_queue_sz == 0) {
+        return NULL;
+    }
+
+    memcpy(ble_hs_test_util_hci_out_cur, ble_hs_test_util_hci_out_queue[0],
+           sizeof ble_hs_test_util_hci_out_cur);
+
+    ble_hs_test_util_hci_out_adj(1);
+
+    return ble_hs_test_util_hci_out_cur;
+}
+
+void *
+ble_hs_test_util_hci_out_last(void)
+{
+    if (ble_hs_test_util_hci_out_queue_sz == 0) {
+        return NULL;
+    }
+
+    ble_hs_test_util_hci_out_queue_sz--;
+    memcpy(ble_hs_test_util_hci_out_cur,
+           ble_hs_test_util_hci_out_queue + ble_hs_test_util_hci_out_queue_sz,
+           sizeof ble_hs_test_util_hci_out_cur);
+
+    return ble_hs_test_util_hci_out_cur;
+}
+
+void
+ble_hs_test_util_hci_out_enqueue(void *cmd)
+{
+    TEST_ASSERT_FATAL(ble_hs_test_util_hci_out_queue_sz <
+                      BLE_HS_TEST_UTIL_PREV_HCI_TX_CNT);
+    memcpy(ble_hs_test_util_hci_out_queue + ble_hs_test_util_hci_out_queue_sz,
+           cmd, 260);
+
+    ble_hs_test_util_hci_out_queue_sz++;
+}
+
+void
+ble_hs_test_util_hci_out_clear(void)
+{
+    ble_hs_test_util_hci_out_queue_sz = 0;
+}
+
+void
+ble_hs_test_util_hci_acks_clear(void)
+{
+    ble_hs_test_util_hci_num_acks = 0;
+}
+
+/*****************************************************************************
+ * $build                                                                    *
+ *****************************************************************************/
+
+void
+ble_hs_test_util_hci_build_cmd_complete(uint8_t *dst, int len,
+                                    uint8_t param_len, uint8_t num_pkts,
+                                    uint16_t opcode)
+{
+    TEST_ASSERT(len >= BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN);
+
+    dst[0] = BLE_HCI_EVCODE_COMMAND_COMPLETE;
+    dst[1] = 3 + param_len;
+    dst[2] = num_pkts;
+    put_le16(dst + 3, opcode);
+}
+
+void
+ble_hs_test_util_hci_build_cmd_status(uint8_t *dst, int len,
+                                  uint8_t status, uint8_t num_pkts,
+                                  uint16_t opcode)
+{
+    TEST_ASSERT(len >= BLE_HCI_EVENT_CMD_STATUS_LEN);
+
+    dst[0] = BLE_HCI_EVCODE_COMMAND_STATUS;
+    dst[1] = BLE_HCI_EVENT_CMD_STATUS_LEN;
+    dst[2] = status;
+    dst[3] = num_pkts;
+    put_le16(dst + 4, opcode);
+}
+
+static void
+ble_hs_test_util_hci_build_ack_params(
+    struct ble_hs_test_util_hci_ack *ack,
+    uint16_t opcode, uint8_t status, void *params, uint8_t params_len)
+{
+    ack->opcode = opcode;
+    ack->status = status;
+
+    if (params == NULL || params_len == 0) {
+        ack->evt_params_len = 0;
+    } else {
+        memcpy(ack->evt_params, params, params_len);
+        ack->evt_params_len = params_len;
+    }
+}
+
+/*****************************************************************************
+ * $ack                                                                      *
+ *****************************************************************************/
+
+static int
+ble_hs_test_util_hci_ack_cb(uint8_t *ack, int ack_buf_len)
+{
+    struct ble_hs_test_util_hci_ack *entry;
+
+    if (ble_hs_test_util_hci_num_acks == 0) {
+        return BLE_HS_ETIMEOUT_HCI;
+    }
+
+    entry = ble_hs_test_util_hci_acks;
+
+    ble_hs_test_util_hci_build_cmd_complete(ack, 256,
+                                        entry->evt_params_len + 1, 1,
+                                        entry->opcode);
+    ack[BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN] = entry->status;
+    memcpy(ack + BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN + 1, entry->evt_params,
+           entry->evt_params_len);
+
+    ble_hs_test_util_hci_num_acks--;
+    if (ble_hs_test_util_hci_num_acks > 0) {
+        memmove(ble_hs_test_util_hci_acks,
+                ble_hs_test_util_hci_acks + 1,
+                sizeof *entry * ble_hs_test_util_hci_num_acks);
+    }
+
+    return 0;
+}
+
+void
+ble_hs_test_util_hci_ack_set_params(uint16_t opcode, uint8_t status,
+                                    void *params, uint8_t params_len)
+{
+    struct ble_hs_test_util_hci_ack *ack;
+
+    ack = ble_hs_test_util_hci_acks + 0;
+    ble_hs_test_util_hci_build_ack_params(ack, opcode, status, params,
+                                          params_len);
+    ble_hs_test_util_hci_num_acks = 1;
+
+    ble_hs_hci_set_phony_ack_cb(ble_hs_test_util_hci_ack_cb);
+}
+
+void
+ble_hs_test_util_hci_ack_set(uint16_t opcode, uint8_t status)
+{
+    ble_hs_test_util_hci_ack_set_params(opcode, status, NULL, 0);
+}
+
+void
+ble_hs_test_util_hci_ack_append_params(uint16_t opcode, uint8_t status,
+                                       void *params, uint8_t params_len)
+{
+    struct ble_hs_test_util_hci_ack *ack;
+
+    ack = ble_hs_test_util_hci_acks + ble_hs_test_util_hci_num_acks;
+    ble_hs_test_util_hci_build_ack_params(ack, opcode, status, params,
+                                          params_len);
+    ble_hs_test_util_hci_num_acks++;
+
+    ble_hs_hci_set_phony_ack_cb(ble_hs_test_util_hci_ack_cb);
+}
+
+void
+ble_hs_test_util_hci_ack_append(uint16_t opcode, uint8_t status)
+{
+    ble_hs_test_util_hci_ack_append_params(opcode, status, NULL, 0);
+}
+
+static const struct ble_hs_test_util_hci_ack hci_startup_seq[] = {
+    {
+        .opcode = ble_hs_hci_util_opcode_join(BLE_HCI_OGF_CTLR_BASEBAND,
+                                              BLE_HCI_OCF_CB_RESET),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+         BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOCAL_VER),
+        .evt_params = { 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+        .evt_params_len = 8,
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+         BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT),
+        .evt_params = { 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00},
+        .evt_params_len = 8,
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_SET_EVENT_MASK),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_SET_EVENT_MASK2),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_EVENT_MASK),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_BUF_SIZE),
+        /* Use a very low buffer size (20) to test fragmentation.
+         * Use a large num-pkts (200) to prevent controller buf exhaustion.
+         */
+        .evt_params = { 0x14, 0x00, 200 },
+        .evt_params_len = 3,
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT),
+        .evt_params = { 0 },
+        .evt_params_len = 8,
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_BD_ADDR),
+        .evt_params = BLE_HS_TEST_UTIL_PUB_ADDR_VAL,
+        .evt_params_len = 6,
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
+    },
+    {
+        .opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
+    },
+    { 0 }
+};
+
+void
+ble_hs_test_util_hci_ack_set_seq(const struct ble_hs_test_util_hci_ack *acks)
+{
+    int i;
+
+    for (i = 0; acks[i].opcode != 0; i++) {
+        ble_hs_test_util_hci_acks[i] = acks[i];
+    }
+    ble_hs_test_util_hci_num_acks = i;
+
+    ble_hs_hci_set_phony_ack_cb(ble_hs_test_util_hci_ack_cb);
+}
+
+int
+ble_hs_test_util_hci_startup_seq_cnt(void)
+{
+    /* last element is terminator, don't count it*/
+    return sizeof(hci_startup_seq)/sizeof(hci_startup_seq[0]) - 1;
+}
+
+void
+ble_hs_test_util_hci_ack_set_startup(void)
+{
+    /* Receive acknowledgements for the startup sequence.  We sent the
+     * corresponding requests when the host task was started.
+     */
+    ble_hs_test_util_hci_ack_set_seq(hci_startup_seq);
+}
+
+void
+ble_hs_test_util_hci_ack_set_disc(uint8_t own_addr_type,
+                                  int fail_idx, uint8_t fail_status)
+{
+    static bool privacy_enabled;
+
+    /*
+     * SET_RPA_TMO should be expected only when test uses RPA and privacy has
+     * not yet been enabled. The Bluetooth stack remembers that privacy is
+     * enabled and does not send SET_RPA_TMO every time. For test purpose
+     * let's track privacy state in here.
+     */
+    if ((own_addr_type == BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT ||
+         own_addr_type == BLE_OWN_ADDR_RPA_RANDOM_DEFAULT) &&
+        !privacy_enabled) {
+
+        privacy_enabled = true;
+        ble_hs_test_util_hci_ack_set_seq(((struct ble_hs_test_util_hci_ack[]) {
+            {
+                BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_RPA_TMO),
+                ble_hs_test_util_hci_misc_exp_status(0, fail_idx, fail_status),
+            },
+            {
+                BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_PARAMS),
+                ble_hs_test_util_hci_misc_exp_status(1, fail_idx, fail_status),
+            },
+            {
+                BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+                ble_hs_test_util_hci_misc_exp_status(2, fail_idx, fail_status),
+            },
+
+            { 0 }
+        }));
+    } else {
+        ble_hs_test_util_hci_ack_set_seq(((struct ble_hs_test_util_hci_ack[]) {
+            {
+                BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_PARAMS),
+                ble_hs_test_util_hci_misc_exp_status(0, fail_idx, fail_status),
+            },
+            {
+                BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+                ble_hs_test_util_hci_misc_exp_status(1, fail_idx, fail_status),
+            },
+
+            { 0 }
+        }));
+    }
+}
+
+void
+ble_hs_test_util_hci_ack_set_disconnect(uint8_t hci_status)
+{
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LINK_CTRL,
+                                    BLE_HCI_OCF_DISCONNECT_CMD),
+        hci_status);
+}
+
+/*****************************************************************************
+ * $verify tx                                                                *
+ *****************************************************************************/
+
+void
+ble_hs_test_util_hci_verify_tx_add_irk(uint8_t addr_type,
+                                       const uint8_t *addr,
+                                       const uint8_t *peer_irk,
+                                       const uint8_t *local_irk)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_ADD_RESOLV_LIST,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_ADD_TO_RESOLV_LIST_LEN);
+
+    TEST_ASSERT(param[0] == addr_type);
+    TEST_ASSERT(memcmp(param + 1, addr, 6) == 0);
+    TEST_ASSERT(memcmp(param + 7, peer_irk, 16) == 0);
+    TEST_ASSERT(memcmp(param + 23, local_irk, 16) == 0);
+}
+
+void
+ble_hs_test_util_hci_verify_tx_set_priv_mode(uint8_t addr_type,
+                                             const uint8_t *addr,
+                                             uint8_t priv_mode)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_SET_PRIVACY_MODE,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_LE_SET_PRIVACY_MODE_LEN);
+
+    TEST_ASSERT(param[0] == addr_type);
+    TEST_ASSERT(memcmp(param + 1, addr, 6) == 0);
+    TEST_ASSERT(param[7] == priv_mode);
+}
+
+void
+ble_hs_test_util_hci_verify_tx_disconnect(uint16_t handle, uint8_t reason)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LINK_CTRL,
+                                           BLE_HCI_OCF_DISCONNECT_CMD,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_DISCONNECT_CMD_LEN);
+
+    TEST_ASSERT(get_le16(param + 0) == handle);
+    TEST_ASSERT(param[2] == reason);
+}
+
+void
+ble_hs_test_util_hci_verify_tx_create_conn(const struct hci_create_conn *exp)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_CREATE_CONN,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_CREATE_CONN_LEN);
+
+    TEST_ASSERT(get_le16(param + 0) == exp->scan_itvl);
+    TEST_ASSERT(get_le16(param + 2) == exp->scan_window);
+    TEST_ASSERT(param[4] == exp->filter_policy);
+    TEST_ASSERT(param[5] == exp->peer_addr_type);
+    TEST_ASSERT(memcmp(param + 6, exp->peer_addr, 6) == 0);
+    TEST_ASSERT(param[12] == exp->own_addr_type);
+    TEST_ASSERT(get_le16(param + 13) == exp->conn_itvl_min);
+    TEST_ASSERT(get_le16(param + 15) == exp->conn_itvl_max);
+    TEST_ASSERT(get_le16(param + 17) == exp->conn_latency);
+    TEST_ASSERT(get_le16(param + 19) == exp->supervision_timeout);
+    TEST_ASSERT(get_le16(param + 21) == exp->min_ce_len);
+    TEST_ASSERT(get_le16(param + 23) == exp->max_ce_len);
+}
+
+uint8_t *
+ble_hs_test_util_hci_verify_tx(uint8_t ogf, uint16_t ocf,
+                               uint8_t *out_param_len)
+{
+    uint16_t opcode;
+    uint8_t *cmd;
+
+    cmd = ble_hs_test_util_hci_out_first();
+    TEST_ASSERT_FATAL(cmd != NULL);
+
+    opcode = get_le16(cmd);
+    TEST_ASSERT(BLE_HCI_OGF(opcode) == ogf);
+    TEST_ASSERT(BLE_HCI_OCF(opcode) == ocf);
+
+    if (out_param_len != NULL) {
+        *out_param_len = cmd[2];
+    }
+
+    return cmd + 3;
+}
+
+/*****************************************************************************
+ * $rx                                                                       *
+ *****************************************************************************/
+
+static void
+ble_hs_test_util_hci_rx_evt(uint8_t *evt)
+{
+    uint8_t *evbuf;
+    int totlen;
+    int rc;
+
+    totlen = BLE_HCI_EVENT_HDR_LEN + evt[1];
+    TEST_ASSERT_FATAL(totlen <= UINT8_MAX + BLE_HCI_EVENT_HDR_LEN);
+
+    evbuf = ble_hci_trans_buf_alloc(
+        BLE_HCI_TRANS_BUF_EVT_LO);
+    TEST_ASSERT_FATAL(evbuf != NULL);
+
+    memcpy(evbuf, evt, totlen);
+
+    if (os_started()) {
+        rc = ble_hci_trans_ll_evt_tx(evbuf);
+    } else {
+        rc = ble_hs_hci_evt_process((void *)evbuf);
+    }
+
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+void
+ble_hs_test_util_hci_rx_num_completed_pkts_event(
+    struct ble_hs_test_util_hci_num_completed_pkts_entry *entries)
+{
+    struct ble_hs_test_util_hci_num_completed_pkts_entry *entry;
+    uint8_t buf[1024];
+    int num_entries;
+    int off;
+    int i;
+
+    /* Count number of entries. */
+    num_entries = 0;
+    for (entry = entries; entry->handle_id != 0; entry++) {
+        num_entries++;
+    }
+    TEST_ASSERT_FATAL(num_entries <= UINT8_MAX);
+
+    buf[0] = BLE_HCI_EVCODE_NUM_COMP_PKTS;
+    buf[2] = num_entries;
+
+    off = 3;
+    for (i = 0; i < num_entries; i++) {
+        put_le16(buf + off, entries[i].handle_id);
+        off += 2;
+        put_le16(buf + off, entries[i].num_pkts);
+        off += 2;
+    }
+
+    buf[1] = off - 2;
+
+    ble_hs_test_util_hci_rx_evt(buf);
+}
+
+void
+ble_hs_test_util_hci_rx_disconn_complete_event(uint16_t conn_handle,
+                                               uint8_t status, uint8_t reason)
+{
+    uint8_t buf[BLE_HCI_EVENT_HDR_LEN + BLE_HCI_EVENT_DISCONN_COMPLETE_LEN];
+
+    buf[0] = BLE_HCI_EVCODE_DISCONN_CMP;
+    buf[1] = BLE_HCI_EVENT_DISCONN_COMPLETE_LEN;
+    buf[2] = status;
+    put_le16(buf + 3, conn_handle);
+    buf[5] = reason;
+
+    ble_hs_test_util_hci_rx_evt(buf);
+}
+
+void
+ble_hs_test_util_hci_rx_conn_cancel_evt(void)
+{
+    struct ble_gap_conn_complete evt;
+    int rc;
+
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_UNK_CONN_ID;
+    /* test if host correctly ignores other fields if status is error */
+    evt.connection_handle = 0x0fff;
+
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+int
+ble_hs_test_util_hci_misc_exp_status(int cmd_idx, int fail_idx,
+                                     uint8_t fail_status)
+{
+    if (cmd_idx == fail_idx) {
+        return BLE_HS_HCI_ERR(fail_status);
+    } else {
+        return 0;
+    }
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util_hci.h b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util_hci.h
new file mode 100644
index 00000000..dde04557
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_hs_test_util_hci.h
@@ -0,0 +1,105 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_HS_TEST_UTIL_HCI_
+#define H_BLE_HS_TEST_UTIL_HCI_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* leave this as macro so it may be used for static const initialization */
+#define ble_hs_hci_util_opcode_join(ogf, ocf) (((ogf) << 10) | (ocf))
+
+#define BLE_HS_TEST_UTIL_PHONY_ACK_MAX  64
+struct ble_hs_test_util_hci_ack {
+    uint16_t opcode;
+    uint8_t status;
+    uint8_t evt_params[256];
+    uint8_t evt_params_len;
+};
+
+struct ble_hs_test_util_hci_num_completed_pkts_entry {
+    uint16_t handle_id; /* 0 for terminating entry in array. */
+    uint16_t num_pkts;
+};
+
+/* $out queue */
+void ble_hs_test_util_hci_out_adj(int count);
+void *ble_hs_test_util_hci_out_first(void);
+void *ble_hs_test_util_hci_out_last(void);
+void ble_hs_test_util_hci_out_enqueue(void *cmd);
+void ble_hs_test_util_hci_out_clear(void);
+void ble_hs_test_util_hci_acks_clear(void);
+
+/* $build */
+void ble_hs_test_util_hci_build_cmd_complete(uint8_t *dst, int len,
+                                             uint8_t param_len,
+                                             uint8_t num_pkts,
+                                             uint16_t opcode);
+void ble_hs_test_util_hci_build_cmd_status(uint8_t *dst, int len,
+                                           uint8_t status, uint8_t num_pkts,
+                                           uint16_t opcode);
+
+/* $ack */
+void ble_hs_test_util_hci_ack_set_params(uint16_t opcode, uint8_t status,
+                                         void *params, uint8_t params_len);
+void ble_hs_test_util_hci_ack_set(uint16_t opcode, uint8_t status);
+void ble_hs_test_util_hci_ack_append_params(uint16_t opcode, uint8_t status,
+                                            void *params, uint8_t params_len);
+void ble_hs_test_util_hci_ack_append(uint16_t opcode, uint8_t status);
+void ble_hs_test_util_hci_ack_set_seq(const struct ble_hs_test_util_hci_ack *acks);
+void ble_hs_test_util_hci_ack_set_startup(void);
+void ble_hs_test_util_hci_ack_set_disc(uint8_t own_addr_type,
+                                       int fail_idx, uint8_t fail_status);
+void ble_hs_test_util_hci_ack_set_disconnect(uint8_t hci_status);
+
+int ble_hs_test_util_hci_startup_seq_cnt(void);
+
+/* $verify tx */
+void ble_hs_test_util_hci_verify_tx_add_irk(uint8_t addr_type,
+                                            const uint8_t *addr,
+                                            const uint8_t *peer_irk,
+                                            const uint8_t *local_irk);
+void ble_hs_test_util_hci_verify_tx_set_priv_mode(uint8_t addr_type,
+                                                  const uint8_t *addr,
+                                                  uint8_t priv_mode);
+void ble_hs_test_util_hci_verify_tx_disconnect(uint16_t handle,
+                                               uint8_t reason);
+void ble_hs_test_util_hci_verify_tx_create_conn(
+    const struct hci_create_conn *exp);
+uint8_t *ble_hs_test_util_hci_verify_tx(uint8_t ogf, uint16_t ocf,
+                                        uint8_t *out_param_len);
+
+/* $rx */
+void ble_hs_test_util_hci_rx_num_completed_pkts_event(
+    struct ble_hs_test_util_hci_num_completed_pkts_entry *entries);
+void ble_hs_test_util_hci_rx_disconn_complete_event(uint16_t conn_handle,
+                                                    uint8_t status, uint8_t reason);
+void ble_hs_test_util_hci_rx_conn_cancel_evt(void);
+
+/* $misc */
+int ble_hs_test_util_hci_misc_exp_status(int cmd_idx, int fail_idx,
+                                         uint8_t fail_status);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_l2cap_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_l2cap_test.c
new file mode 100644
index 00000000..2b17da06
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_l2cap_test.c
@@ -0,0 +1,1500 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_HCI_CONN_UPDATE_LEN             (14)
+
+#define BLE_L2CAP_TEST_PSM                   (90)
+#define BLE_L2CAP_TEST_CID                   (99)
+#define BLE_L2CAP_TEST_COC_MTU               (256)
+/* We use same pool for incoming and outgoing sdu */
+#define BLE_L2CAP_TEST_COC_BUF_COUNT         (6 * MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM))
+
+static uint16_t ble_l2cap_test_update_conn_handle;
+static int ble_l2cap_test_update_status;
+static void *ble_l2cap_test_update_arg;
+
+static void *test_sdu_coc_mem;
+struct os_mbuf_pool sdu_os_mbuf_pool;
+static struct os_mempool sdu_coc_mbuf_mempool;
+static uint16_t current_cid  = 0x0040;
+/*****************************************************************************
+ * $util                                                                     *
+ *****************************************************************************/
+
+static void
+ble_l2cap_test_util_init(void)
+{
+    ble_hs_test_util_init();
+    ble_l2cap_test_update_conn_handle = BLE_HS_CONN_HANDLE_NONE;
+    ble_l2cap_test_update_status = -1;
+    ble_l2cap_test_update_arg = (void *)(uintptr_t)-1;
+    int rc;
+
+    if (test_sdu_coc_mem) {
+        free(test_sdu_coc_mem);
+    }
+
+    /* For testing we want to support all the available channels */
+    test_sdu_coc_mem = malloc(
+        OS_MEMPOOL_BYTES(BLE_L2CAP_TEST_COC_BUF_COUNT,BLE_L2CAP_TEST_COC_MTU));
+    assert(test_sdu_coc_mem != NULL);
+
+    rc = os_mempool_init(&sdu_coc_mbuf_mempool, BLE_L2CAP_TEST_COC_BUF_COUNT,
+                         BLE_L2CAP_TEST_COC_MTU, test_sdu_coc_mem,
+                         "test_coc_sdu_pool");
+    assert(rc == 0);
+
+    rc = os_mbuf_pool_init(&sdu_os_mbuf_pool, &sdu_coc_mbuf_mempool,
+                           BLE_L2CAP_TEST_COC_MTU, BLE_L2CAP_TEST_COC_BUF_COUNT);
+    assert(rc == 0);
+
+}
+
+static void
+ble_l2cap_test_util_rx_update_req(uint16_t conn_handle, uint8_t id,
+                                  struct ble_l2cap_sig_update_params *params)
+{
+    struct ble_l2cap_sig_update_req *req;
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    int rc;
+
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + BLE_L2CAP_SIG_HDR_SZ + BLE_L2CAP_SIG_UPDATE_REQ_SZ);
+
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_UPDATE_REQ, id,
+                                BLE_L2CAP_SIG_UPDATE_REQ_SZ, &om);
+    TEST_ASSERT_FATAL(req != NULL);
+
+    req->itvl_min = htole16(params->itvl_min);
+    req->itvl_max = htole16(params->itvl_max);
+    req->slave_latency = htole16(params->slave_latency);
+    req->timeout_multiplier = htole16(params->timeout_multiplier);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CONN_UPDATE), 0);
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SIG,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_l2cap_test_util_verify_tx_update_conn(
+    struct ble_gap_upd_params *params)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_CONN_UPDATE,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_CONN_UPDATE_LEN);
+    TEST_ASSERT(get_le16(param + 0) == 2);
+    TEST_ASSERT(get_le16(param + 2) == params->itvl_min);
+    TEST_ASSERT(get_le16(param + 4) == params->itvl_max);
+    TEST_ASSERT(get_le16(param + 6) == params->latency);
+    TEST_ASSERT(get_le16(param + 8) == params->supervision_timeout);
+    TEST_ASSERT(get_le16(param + 10) == params->min_ce_len);
+    TEST_ASSERT(get_le16(param + 12) == params->max_ce_len);
+}
+
+static int
+ble_l2cap_test_util_dummy_rx(struct ble_l2cap_chan *chan)
+{
+    return 0;
+}
+
+static void
+ble_l2cap_test_util_create_conn(uint16_t conn_handle, uint8_t *addr,
+                                ble_gap_event_fn *cb, void *cb_arg)
+{
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+
+    ble_hs_test_util_create_conn(conn_handle, addr, cb, cb_arg);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    TEST_ASSERT_FATAL(conn != NULL);
+
+    chan = ble_l2cap_chan_alloc(conn_handle);
+    TEST_ASSERT_FATAL(chan != NULL);
+
+    chan->scid = BLE_L2CAP_TEST_CID;
+    chan->my_mtu = 240;
+    chan->rx_fn = ble_l2cap_test_util_dummy_rx;
+
+    ble_hs_conn_chan_insert(conn, chan);
+
+    ble_hs_test_util_hci_out_clear();
+
+    ble_hs_unlock();
+}
+
+static int
+ble_l2cap_test_util_rx_first_frag(uint16_t conn_handle,
+                                  uint16_t l2cap_frag_len,
+                                  uint16_t cid, uint16_t l2cap_len)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    uint16_t hci_len;
+    void *v;
+    int rc;
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    v = os_mbuf_extend(om, l2cap_frag_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    om = ble_l2cap_prepend_hdr(om, cid, l2cap_len);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    hci_len = sizeof hci_hdr + l2cap_frag_len;
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(conn_handle,
+                                          BLE_HCI_PB_FIRST_FLUSH, hci_len);
+    rc = ble_hs_test_util_l2cap_rx(conn_handle, &hci_hdr, om);
+    return rc;
+}
+
+static int
+ble_l2cap_test_util_rx_next_frag(uint16_t conn_handle, uint16_t hci_len)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int rc;
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    v = os_mbuf_extend(om, hci_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(conn_handle,
+                                          BLE_HCI_PB_MIDDLE, hci_len);
+    rc = ble_hs_test_util_l2cap_rx(conn_handle, &hci_hdr, om);
+    return rc;
+}
+
+static void
+ble_l2cap_test_util_verify_first_frag(uint16_t conn_handle,
+                                      uint16_t l2cap_frag_len,
+                                      uint16_t l2cap_len)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    rc = ble_l2cap_test_util_rx_first_frag(conn_handle, l2cap_frag_len,
+                                           BLE_L2CAP_TEST_CID, l2cap_len);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_rx_chan != NULL &&
+                conn->bhc_rx_chan->scid == BLE_L2CAP_TEST_CID);
+
+    ble_hs_unlock();
+}
+
+static void
+ble_l2cap_test_util_verify_middle_frag(uint16_t conn_handle,
+                                       uint16_t hci_len)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    rc = ble_l2cap_test_util_rx_next_frag(conn_handle, hci_len);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_rx_chan != NULL &&
+                conn->bhc_rx_chan->scid == BLE_L2CAP_TEST_CID);
+
+    ble_hs_unlock();
+}
+
+static void
+ble_l2cap_test_util_verify_last_frag(uint16_t conn_handle,
+                                     uint16_t hci_len)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    rc = ble_l2cap_test_util_rx_next_frag(conn_handle, hci_len);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_rx_chan == NULL);
+
+    ble_hs_unlock();
+}
+
+/*****************************************************************************
+ * $rx                                                                       *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_l2cap_test_case_bad_header)
+{
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+
+    rc = ble_l2cap_test_util_rx_first_frag(2, 14, 1234, 10);
+    TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_bad_handle)
+{
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+
+    rc = ble_l2cap_test_util_rx_first_frag(1234, 14, 1234, 10);
+    TEST_ASSERT(rc == BLE_HS_ENOTCONN);
+
+    /* Ensure we did not send anything in return. */
+    TEST_ASSERT_FATAL(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*****************************************************************************
+ * $fragmentation                                                            *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_l2cap_test_case_frag_single)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+
+    /*** HCI header specifies middle fragment without start. */
+    hci_hdr = BLE_HS_TEST_UTIL_L2CAP_HCI_HDR(2, BLE_HCI_PB_MIDDLE, 10);
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    om = ble_l2cap_prepend_hdr(om, 0, 5);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    rc = ble_hs_test_util_l2cap_rx(2, &hci_hdr, om);
+    TEST_ASSERT(rc == BLE_HS_EBADDATA);
+
+    /*** Packet consisting of three fragments. */
+    ble_l2cap_test_util_verify_first_frag(2, 10, 30);
+    ble_l2cap_test_util_verify_middle_frag(2, 10);
+    ble_l2cap_test_util_verify_last_frag(2, 10);
+
+    /*** Packet consisting of five fragments. */
+    ble_l2cap_test_util_verify_first_frag(2, 8, 49);
+    ble_l2cap_test_util_verify_middle_frag(2, 13);
+    ble_l2cap_test_util_verify_middle_frag(2, 2);
+    ble_l2cap_test_util_verify_middle_frag(2, 21);
+    ble_l2cap_test_util_verify_last_frag(2, 5);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_frag_multiple)
+{
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+    ble_l2cap_test_util_create_conn(3, ((uint8_t[]){2,3,4,5,6,7}),
+                                    NULL, NULL);
+    ble_l2cap_test_util_create_conn(4, ((uint8_t[]){3,4,5,6,7,8}),
+                                    NULL, NULL);
+
+    ble_l2cap_test_util_verify_first_frag(2, 3, 10);
+    ble_l2cap_test_util_verify_first_frag(3, 2, 5);
+    ble_l2cap_test_util_verify_middle_frag(2, 6);
+    ble_l2cap_test_util_verify_first_frag(4, 1, 4);
+    ble_l2cap_test_util_verify_middle_frag(3, 2);
+    ble_l2cap_test_util_verify_last_frag(3, 1);
+    ble_l2cap_test_util_verify_middle_frag(4, 2);
+    ble_l2cap_test_util_verify_last_frag(4, 1);
+    ble_l2cap_test_util_verify_last_frag(2, 1);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_frag_channels)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+    uint16_t data_len = 30;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+
+    /* Receive a starting fragment on the first channel. */
+    rc = ble_l2cap_test_util_rx_first_frag(2, 14, BLE_L2CAP_TEST_CID, data_len);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_rx_chan != NULL &&
+                conn->bhc_rx_chan->scid == BLE_L2CAP_TEST_CID);
+    ble_hs_unlock();
+
+    /* Receive a starting fragment on a different channel.  The first fragment
+     * should get discarded.
+     */
+    ble_hs_test_util_set_att_mtu(conn->bhc_handle, data_len);
+    rc = ble_l2cap_test_util_rx_first_frag(2, 14, BLE_L2CAP_CID_ATT, data_len);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    TEST_ASSERT(conn->bhc_rx_chan != NULL &&
+                conn->bhc_rx_chan->scid == BLE_L2CAP_CID_ATT);
+    ble_hs_unlock();
+
+    /* Terminate the connection.  The received fragments should get freed.
+     * Mbuf leaks are tested in the post-test-case callback.
+     */
+    ble_hs_test_util_conn_disconnect(2);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_frag_timeout)
+{
+    int32_t ticks_from_now;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+
+    /* Ensure timer is not set. */
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT_FATAL(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Receive the first fragment of a multipart ACL data packet. */
+    rc = ble_l2cap_test_util_rx_first_frag(2, 14, BLE_L2CAP_TEST_CID, 30);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure timer will expire in 30 seconds. */
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT));
+
+    /* Almost let the timer expire. */
+    os_time_advance(MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) - 1);
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == 1);
+
+    /* Receive a second fragment. */
+    rc = ble_l2cap_test_util_rx_next_frag(2, 14);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure timer got reset. */
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT));
+
+    /* Allow the timer to expire. */
+    ble_hs_test_util_hci_ack_set_disconnect(0);
+    os_time_advance(MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT));
+    ticks_from_now = ble_hs_conn_timer();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Ensure connection was terminated. */
+    ble_hs_test_util_hci_verify_tx_disconnect(2, BLE_ERR_REM_USER_CONN_TERM);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*****************************************************************************
+ * $unsolicited response                                                     *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_unsol_rsp)
+{
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    NULL, NULL);
+
+    /* Receive an unsolicited response. */
+    rc = ble_hs_test_util_rx_l2cap_update_rsp(2, 100, 0);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure we did not send anything in return. */
+    TEST_ASSERT_FATAL(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*****************************************************************************
+ * $update                                                                   *
+ *****************************************************************************/
+
+static int
+ble_l2cap_test_util_conn_cb(struct ble_gap_event *event, void *arg)
+{
+    int *accept;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_L2CAP_UPDATE_REQ:
+        accept = arg;
+        return !*accept;
+
+    default:
+        return 0;
+    }
+}
+
+static void
+ble_l2cap_test_util_peer_updates(int accept)
+{
+    struct ble_l2cap_sig_update_params l2cap_params;
+    struct ble_gap_upd_params params;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    ble_l2cap_test_util_conn_cb,
+                                    &accept);
+
+    l2cap_params.itvl_min = 0x200;
+    l2cap_params.itvl_max = 0x300;
+    l2cap_params.slave_latency = 0;
+    l2cap_params.timeout_multiplier = 0x500;
+    ble_l2cap_test_util_rx_update_req(2, 1, &l2cap_params);
+
+    /* Ensure an update response command got sent. */
+    ble_hs_test_util_verify_tx_l2cap_update_rsp(1, !accept);
+
+    if (accept) {
+        params.itvl_min = 0x200;
+        params.itvl_max = 0x300;
+        params.latency = 0;
+        params.supervision_timeout = 0x500;
+        params.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
+        params.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
+        ble_l2cap_test_util_verify_tx_update_conn(&params);
+    } else {
+        /* Ensure no update got scheduled. */
+        TEST_ASSERT(!ble_gap_dbg_update_active(2));
+    }
+}
+
+static void
+ble_l2cap_test_util_update_cb(uint16_t conn_handle, int status, void *arg)
+{
+    ble_l2cap_test_update_conn_handle = conn_handle;
+    ble_l2cap_test_update_status = status;
+    ble_l2cap_test_update_arg = arg;
+}
+
+static void
+ble_l2cap_test_util_we_update(int peer_accepts)
+{
+    struct ble_l2cap_sig_update_params params;
+    uint8_t id;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    ble_l2cap_test_util_conn_cb, NULL);
+
+    /* Only the slave can initiate the L2CAP connection update procedure. */
+    ble_hs_atomic_conn_set_flags(2, BLE_HS_CONN_F_MASTER, 0);
+
+    params.itvl_min = 0x200;
+    params.itvl_max = 0x300;
+    params.slave_latency = 0;
+    params.timeout_multiplier = 0x100;
+    rc = ble_l2cap_sig_update(2, &params, ble_l2cap_test_util_update_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure an update request got sent. */
+    id = ble_hs_test_util_verify_tx_l2cap_update_req(&params);
+
+    /* Receive response from peer. */
+    rc = ble_hs_test_util_rx_l2cap_update_rsp(2, id, !peer_accepts);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback got called. */
+    if (peer_accepts) {
+        TEST_ASSERT(ble_l2cap_test_update_status == 0);
+    } else {
+        TEST_ASSERT(ble_l2cap_test_update_status == BLE_HS_EREJECT);
+    }
+    TEST_ASSERT(ble_l2cap_test_update_arg == NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_update_accept)
+{
+    ble_l2cap_test_util_peer_updates(1);
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_update_reject)
+{
+    ble_l2cap_test_util_peer_updates(0);
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_update_init_accept)
+{
+    ble_l2cap_test_util_we_update(1);
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_update_init_reject)
+{
+    ble_l2cap_test_util_we_update(0);
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_update_init_fail_master)
+{
+    struct ble_l2cap_sig_update_params params;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    ble_l2cap_test_util_conn_cb, NULL);
+
+    params.itvl_min = 0x200;
+    params.itvl_max = 0x300;
+    params.slave_latency = 0;
+    params.timeout_multiplier = 0x100;
+    rc = ble_l2cap_sig_update(2, &params, ble_l2cap_test_util_update_cb, NULL);
+    TEST_ASSERT_FATAL(rc == BLE_HS_EINVAL);
+
+    /* Ensure callback never called. */
+    TEST_ASSERT(ble_l2cap_test_update_status == -1);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_update_init_fail_bad_id)
+{
+    struct ble_l2cap_sig_update_params params;
+    uint8_t id;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    ble_l2cap_test_util_conn_cb, NULL);
+
+    /* Only the slave can initiate the L2CAP connection update procedure. */
+    ble_hs_atomic_conn_set_flags(2, BLE_HS_CONN_F_MASTER, 0);
+
+    params.itvl_min = 0x200;
+    params.itvl_max = 0x300;
+    params.slave_latency = 0;
+    params.timeout_multiplier = 0x100;
+    rc = ble_l2cap_sig_update(2, &params, ble_l2cap_test_util_update_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure an update request got sent. */
+    id = ble_hs_test_util_verify_tx_l2cap_update_req(&params);
+
+    /* Receive response from peer with incorrect ID. */
+    rc = ble_hs_test_util_rx_l2cap_update_rsp(2, id + 1, 0);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback did not get called. */
+    TEST_ASSERT(ble_l2cap_test_update_status == -1);
+
+    /* Receive response from peer with correct ID. */
+    rc = ble_hs_test_util_rx_l2cap_update_rsp(2, id, 0);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback got called. */
+    TEST_ASSERT(ble_l2cap_test_update_status == 0);
+    TEST_ASSERT(ble_l2cap_test_update_arg == NULL);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/* Test enum but first four events matches to events which L2CAP sends to
+ * application. We need this in order to add additional SEND_DATA event for
+ * testing
+ */
+
+enum {
+    BLE_L2CAP_TEST_EVENT_COC_CONNECT = 0,
+    BLE_L2CAP_TEST_EVENT_COC_DISCONNECT,
+    BLE_L2CAP_TEST_EVENT_COC_ACCEPT,
+    BLE_L2CAP_TEST_EVENT_COC_RECV_DATA,
+    BLE_L2CAP_TEST_EVENT_COC_SEND_DATA,
+};
+
+struct event {
+    uint8_t type;
+    uint16_t early_error;
+    uint16_t l2cap_status;
+    uint16_t app_status;
+    uint8_t handled;
+    uint8_t *data;
+    uint16_t data_len;
+};
+
+struct test_data {
+    struct event event[3];
+    uint16_t expected_num_of_ev;
+    uint16_t expected_num_iters;
+    /* This we use to track number of events sent to application*/
+    uint16_t event_cnt;
+    /* This we use to track verified events (received or not) */
+    uint16_t event_iter;
+    uint16_t psm;
+    uint16_t mtu;
+    uint8_t num;
+    struct ble_l2cap_chan *chan[5];
+};
+
+static int
+ble_l2cap_test_event(struct ble_l2cap_event *event, void *arg)
+{
+    struct test_data *t = arg;
+    struct event *ev = &t->event[t->event_cnt++];
+    struct os_mbuf *sdu_rx;
+
+    assert(ev->type == event->type);
+    ev->handled = 1;
+    switch(event->type) {
+    case BLE_L2CAP_EVENT_COC_CONNECTED:
+        assert(ev->app_status == event->connect.status);
+        t->chan[0] = event->connect.chan;
+        return 0;
+    case BLE_L2CAP_EVENT_COC_DISCONNECTED:
+        return 0;
+    case BLE_L2CAP_EVENT_COC_ACCEPT:
+        if (ev->app_status != 0) {
+            return ev->app_status;
+        }
+
+        sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+        assert(sdu_rx != NULL);
+        ble_l2cap_recv_ready(event->accept.chan, sdu_rx);
+
+        return 0;
+
+    case BLE_L2CAP_EVENT_COC_DATA_RECEIVED:
+        sdu_rx = os_mbuf_pullup(event->receive.sdu_rx,
+                                    OS_MBUF_PKTLEN(event->receive.sdu_rx));
+        TEST_ASSERT(memcmp(sdu_rx->om_data, ev->data, ev->data_len) == 0);
+        return 0;
+    case BLE_L2CAP_EVENT_COC_TX_UNSTALLED:
+        /* TODO Add tests for this */
+        return 0;
+    default:
+        return 0;
+    }
+}
+
+static uint16_t ble_l2cap_calculate_credits(uint16_t mtu, uint16_t mps)
+{
+    int credits;
+
+    credits = mtu / mps;
+    if (mtu % mps) {
+        credits++;
+    }
+
+    return credits;
+}
+
+static void
+ble_l2cap_test_coc_connect_multi(struct test_data *t)
+{
+    struct ble_l2cap_sig_credit_base_connect_req *req;
+    struct ble_l2cap_sig_credit_base_connect_rsp *rsp;
+    struct os_mbuf *sdu_rx[t->num];
+    struct event *ev = &t->event[t->event_iter++];
+    uint8_t id;
+    int rc;
+    int i;
+
+    req = malloc(sizeof(*req) + (sizeof(uint16_t) * t->num));
+    rsp = malloc(sizeof(*rsp) + (sizeof(uint16_t) * t->num));
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]) {1, 2, 3, 4, 5, 6}),
+                                    ble_l2cap_test_util_conn_cb, NULL);
+
+    for (i = 0; i < t->num; i++) {
+        sdu_rx[i] = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+        assert(sdu_rx[i] != NULL);
+    }
+
+    rc = ble_l2cap_sig_ecoc_connect(2, t->psm, t->mtu, t->num, sdu_rx,
+                                   ble_l2cap_test_event, t);
+    TEST_ASSERT_FATAL(rc == ev->early_error);
+
+    if (rc != 0) {
+        for (i = 0; i< t->num; i++) {
+            rc = os_mbuf_free_chain(sdu_rx[i]);
+            TEST_ASSERT_FATAL(rc == 0);
+        }
+
+        return;
+    }
+
+    req->credits = htole16(
+                        ble_l2cap_calculate_credits(t->mtu,
+                                                    MYNEWT_VAL(BLE_L2CAP_COC_MPS)));
+    req->mps = htole16(MYNEWT_VAL(BLE_L2CAP_COC_MPS));
+    req->mtu = htole16(t->mtu);
+    req->psm = htole16(t->psm);
+    for (i = 0; i < t->num; i++) {
+        req->scids[i] = htole16(current_cid + i);
+    }
+
+    /* Ensure an update request got sent. */
+    id = ble_hs_test_util_verify_tx_l2cap_sig(
+                                            BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ,
+                                            req, sizeof(*req) + t->num * sizeof(uint16_t));
+
+    /* Use some different parameters for peer. Just keep mtu same for testing
+     * only*/
+    rsp->credits = htole16(10);
+    for (i = 0; i < t->num; i++) {
+        rsp->dcids[i] = htole16(current_cid + i);
+    }
+    rsp->mps = htole16(MYNEWT_VAL(BLE_L2CAP_COC_MPS) + 16);
+    rsp->mtu = htole16(t->mtu);
+    rsp->result = htole16(ev->l2cap_status);
+
+    rc = ble_hs_test_util_inject_rx_l2cap_sig(2,
+                                              BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP,
+                                              id, rsp, sizeof(*rsp) + t->num * sizeof(uint16_t));
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback got called. */
+    TEST_ASSERT(ev->handled);
+}
+
+static void
+ble_l2cap_test_coc_connect(struct test_data *t)
+{
+    struct ble_l2cap_sig_le_con_req req = {};
+    struct ble_l2cap_sig_le_con_rsp rsp = {};
+    struct os_mbuf *sdu_rx;
+    struct event *ev = &t->event[t->event_iter++];
+    uint8_t id;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    ble_l2cap_test_util_conn_cb, NULL);
+
+    sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+    assert(sdu_rx != NULL);
+
+    rc = ble_l2cap_sig_coc_connect(2, t->psm, t->mtu, sdu_rx,
+                                   ble_l2cap_test_event, t);
+    TEST_ASSERT_FATAL(rc == ev->early_error);
+
+    if (rc != 0) {
+        rc = os_mbuf_free_chain(sdu_rx);
+        TEST_ASSERT_FATAL(rc == 0);
+        return;
+    }
+
+    req.credits = htole16(
+                        ble_l2cap_calculate_credits(t->mtu,
+                                                    MYNEWT_VAL(BLE_L2CAP_COC_MPS)));
+    req.mps = htole16(MYNEWT_VAL(BLE_L2CAP_COC_MPS));
+    req.mtu = htole16(t->mtu);
+    req.psm = htole16(t->psm);
+    req.scid = htole16(current_cid);
+
+    /* Ensure an update request got sent. */
+    id = ble_hs_test_util_verify_tx_l2cap_sig(
+                                            BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ,
+                                            &req, sizeof(req));
+
+    /* Use some different parameters for peer. Just keep mtu same for testing
+     * only*/
+    rsp.credits = htole16(10);
+    rsp.dcid = htole16(current_cid);
+    rsp.mps = htole16(MYNEWT_VAL(BLE_L2CAP_COC_MPS) + 16);
+    rsp.mtu = htole16(t->mtu);
+    rsp.result = htole16(ev->l2cap_status);
+
+    rc = ble_hs_test_util_inject_rx_l2cap_sig(2,
+                                              BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_RSP,
+                                              id, &rsp, sizeof(rsp));
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback got called. */
+    TEST_ASSERT(ev->handled);
+}
+
+static void
+ble_l2cap_test_coc_connect_by_peer(struct test_data *t)
+{
+    struct ble_l2cap_sig_le_con_req req = {};
+    struct ble_l2cap_sig_le_con_rsp rsp = {};
+    uint8_t id = 10;
+    int rc;
+    struct event *ev = &t->event[t->event_iter++];
+
+    ble_l2cap_test_util_create_conn(2, ((uint8_t[]){1,2,3,4,5,6}),
+                                    ble_l2cap_test_util_conn_cb, NULL);
+
+    /* Use some different parameters for peer */
+    req.credits = htole16(30);
+    req.mps = htole16(MYNEWT_VAL(BLE_L2CAP_COC_MPS) + 16);
+    req.mtu = htole16(t->mtu);
+    req.psm = htole16(t->psm);
+    req.scid = htole16(0x0040);
+
+    /* Receive remote request*/
+    rc = ble_hs_test_util_inject_rx_l2cap_sig(2,
+                                              BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ,
+                                              id, &req, sizeof(req));
+    TEST_ASSERT_FATAL(rc == 0);
+
+    if (ev->type == BLE_L2CAP_EVENT_COC_ACCEPT) {
+        /* Lets check if there is accept event */
+        TEST_ASSERT(ev->handled);
+        /* Ensure callback got called. */
+        ev = &t->event[t->event_iter++];
+    }
+
+    if (ev->l2cap_status != 0) {
+        rsp.result = htole16(ev->l2cap_status);
+    } else {
+        /* Receive response from peer.*/
+        rsp.credits = htole16(
+                            ble_l2cap_calculate_credits(t->mtu,
+                                                        MYNEWT_VAL(BLE_L2CAP_COC_MPS)));
+        rsp.dcid = htole16(current_cid);
+        rsp.mps = htole16(MYNEWT_VAL(BLE_L2CAP_COC_MPS));
+        rsp.mtu = htole16(t->mtu);
+    }
+
+    /* Ensure we sent response. */
+    TEST_ASSERT(id == ble_hs_test_util_verify_tx_l2cap_sig(
+                                            BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_RSP,
+                                            &rsp, sizeof(rsp)));
+
+    if (ev->l2cap_status == 0) {
+        TEST_ASSERT(ev->handled);
+    } else {
+        TEST_ASSERT(!ev->handled);
+    }
+}
+
+static void
+ble_l2cap_test_coc_disc(struct test_data *t)
+{
+    struct ble_l2cap_sig_disc_req req;
+    struct event *ev = &t->event[t->event_iter++];
+    uint8_t id;
+    int rc;
+
+    rc = ble_l2cap_sig_disconnect(t->chan[0]);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    req.dcid = htole16(t->chan[0]->dcid);
+    req.scid = htole16(t->chan[0]->scid);
+
+    /* Ensure an update request got sent. */
+    id = ble_hs_test_util_verify_tx_l2cap_sig(BLE_L2CAP_SIG_OP_DISCONN_REQ,
+                                                   &req, sizeof(req));
+
+    /* Receive response from peer. Note it shall be same as request */
+    rc = ble_hs_test_util_inject_rx_l2cap_sig(2, BLE_L2CAP_SIG_OP_DISCONN_RSP,
+                                           id, &req, sizeof(req));
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback got called. */
+    TEST_ASSERT(ev->handled);
+}
+
+static void
+ble_l2cap_test_coc_disc_by_peer(struct test_data *t)
+{
+    struct ble_l2cap_sig_disc_req req;
+    struct event *ev = &t->event[t->event_iter++];
+    uint8_t id = 10;
+    int rc;
+
+    /* Receive disconnect request from peer. Note that source cid
+     * and destination cid are from peer perspective */
+    req.dcid = htole16(t->chan[0]->scid);
+    req.scid = htole16(t->chan[0]->dcid);
+
+    rc = ble_hs_test_util_inject_rx_l2cap_sig(2, BLE_L2CAP_SIG_OP_DISCONN_REQ,
+                                       id, &req, sizeof(req));
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback got called. */
+    TEST_ASSERT(ev->handled);
+
+    /* Ensure an we sent back response. Note that payload is same as request,
+     * lets reuse it */
+    TEST_ASSERT(ble_hs_test_util_verify_tx_l2cap_sig(
+                                        BLE_L2CAP_SIG_OP_DISCONN_RSP,
+                                        &req, sizeof(req)) == id);
+}
+
+static void
+ble_l2cap_test_coc_invalid_disc_by_peer(struct test_data *t)
+{
+    struct ble_l2cap_sig_disc_req req;
+    uint8_t id = 10;
+    int rc;
+    struct event *ev = &t->event[t->event_iter++];
+
+    /* Receive disconnect request from peer. Note that source cid
+     * and destination cid are from peer perspective */
+    req.dcid = htole16(t->chan[0]->scid);
+    req.scid = htole16(0);
+
+    rc = ble_hs_test_util_inject_rx_l2cap_sig(2, BLE_L2CAP_SIG_OP_DISCONN_REQ,
+                                       id, &req, sizeof(req));
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure callback HAS NOT BEEN*/
+    TEST_ASSERT(!ev->handled);
+}
+
+static void
+ble_l2cap_test_coc_send_data(struct test_data *t)
+{
+    struct os_mbuf *sdu;
+    struct os_mbuf *sdu_copy;
+    struct event *ev = &t->event[t->event_iter++];
+    int rc;
+
+    /* Send data event is created only for testing.
+     * Since application callback do caching of real stack event
+     * and checks the type of the event, lets increase event counter here and
+     * fake that this event is handled*/
+    t->event_cnt++;
+
+    sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+    assert(sdu != NULL);
+
+    sdu_copy = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+    assert(sdu_copy != NULL);
+
+    rc = os_mbuf_append(sdu, ev->data, ev->data_len);
+    TEST_ASSERT(rc == 0);
+
+    rc = os_mbuf_append(sdu_copy, ev->data, ev->data_len);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_l2cap_send(t->chan[0], sdu);
+    TEST_ASSERT(rc == ev->early_error);
+
+    if (rc) {
+        rc = os_mbuf_free(sdu);
+        TEST_ASSERT_FATAL(rc == 0);
+
+        rc = os_mbuf_free(sdu_copy);
+        TEST_ASSERT_FATAL(rc == 0);
+        return;
+    }
+
+    /* Add place for SDU len */
+    sdu_copy = os_mbuf_prepend_pullup(sdu_copy, 2);
+    assert(sdu_copy != NULL);
+    put_le16(sdu_copy->om_data, ev->data_len);
+
+    ble_hs_test_util_verify_tx_l2cap(sdu);
+
+    rc = os_mbuf_free_chain(sdu_copy);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_l2cap_test_coc_recv_data(struct test_data *t)
+{
+    struct os_mbuf *sdu;
+    int rc;
+    struct event *ev = &t->event[t->event_iter++];
+
+    sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+    assert(sdu != NULL);
+
+    rc = os_mbuf_append(sdu, ev->data, ev->data_len);
+    TEST_ASSERT(rc == 0);
+
+    /* TODO  handle fragmentation */
+
+    /* Add place for SDU len */
+    sdu = os_mbuf_prepend_pullup(sdu, 2);
+    assert(sdu != NULL);
+    put_le16(sdu->om_data, ev->data_len);
+
+    ble_hs_test_util_inject_rx_l2cap(2, t->chan[0]->scid, sdu);
+}
+
+static void
+ble_l2cap_test_set_chan_test_conf(uint16_t psm, uint16_t mtu,
+                                  struct test_data *t)
+{
+    memset(t, 0, sizeof(*t));
+
+    t->psm = psm;
+    t->mtu = mtu;
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_conn_invalid_psm)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+    t.expected_num_iters = 1;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = BLE_HS_ENOTSUP;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM;
+
+    ble_l2cap_test_coc_connect(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+    TEST_ASSERT(t.expected_num_iters == t.event_iter);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_conn_out_of_resource)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+    t.expected_num_iters = 1;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = BLE_HS_ENOMEM;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_NO_RESOURCES;
+
+    ble_l2cap_test_coc_connect(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+    TEST_ASSERT(t.expected_num_iters == t.event_iter);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_conn_invalid_cid)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = BLE_HS_EREJECT;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID;
+
+    ble_l2cap_test_coc_connect(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_conn_insuff_authen)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = BLE_HS_EAUTHEN;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN;
+
+    ble_l2cap_test_coc_connect(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_conn_insuff_author)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = BLE_HS_EAUTHOR;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR;
+
+    ble_l2cap_test_coc_connect(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_incoming_conn_invalid_psm)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 0;
+    t.expected_num_iters = 1;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM;
+
+    ble_l2cap_test_coc_connect_by_peer(&t);
+
+    TEST_ASSERT(t.expected_num_iters == t.event_iter);
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_incoming_conn_rejected_by_app)
+{
+    struct test_data t;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+    t.expected_num_iters = 2;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_ACCEPT;
+    t.event[0].app_status = BLE_HS_ENOMEM;
+
+    /* This event will not be called and test is going to verify it*/
+    t.event[1].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[1].l2cap_status = BLE_L2CAP_COC_ERR_NO_RESOURCES;
+
+    /* Register server */
+    rc = ble_l2cap_create_server(t.psm, BLE_L2CAP_TEST_COC_MTU,
+                                 ble_l2cap_test_event, &t);
+    TEST_ASSERT(rc == 0);
+
+    ble_l2cap_test_coc_connect_by_peer(&t);
+
+    TEST_ASSERT(t.expected_num_iters == t.event_iter);
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_incoming_conn_success)
+{
+    struct test_data t;
+    int rc;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 2;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_ACCEPT;
+    t.event[1].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+
+    /* Register server */
+    rc = ble_l2cap_create_server(t.psm, BLE_L2CAP_TEST_COC_MTU,
+                                 ble_l2cap_test_event, &t);
+    TEST_ASSERT(rc == 0);
+
+    ble_l2cap_test_coc_connect_by_peer(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_disconnect_succeed)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t. expected_num_of_ev = 2;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = 0;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS;
+    t.event[1].type = BLE_L2CAP_EVENT_COC_DISCONNECTED;
+
+    ble_l2cap_test_coc_connect(&t);
+    ble_l2cap_test_coc_disc(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_incoming_disconnect_succeed)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 2;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = 0;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS;
+    t.event[1].type = BLE_L2CAP_EVENT_COC_DISCONNECTED;
+
+    ble_l2cap_test_coc_connect(&t);
+    ble_l2cap_test_coc_disc_by_peer(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_incoming_disconnect_failed)
+{
+    struct test_data t;
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 1;
+    t.expected_num_iters = 2;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[0].app_status = 0;
+    t.event[0].l2cap_status = BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS;
+    t.event[1].type = BLE_L2CAP_EVENT_COC_DISCONNECTED;
+
+    ble_l2cap_test_coc_connect(&t);
+    ble_l2cap_test_coc_invalid_disc_by_peer(&t);
+
+    TEST_ASSERT(t.expected_num_iters == t.event_iter);
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_coc_send_data_succeed)
+{
+    struct test_data t;
+    uint8_t buf[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 3;
+
+    t.event[0].type = BLE_L2CAP_TEST_EVENT_COC_CONNECT;
+    t.event[1].type = BLE_L2CAP_TEST_EVENT_COC_SEND_DATA;
+    t.event[1].data = buf;
+    t.event[1].data_len = sizeof(buf);
+    t.event[2].type = BLE_L2CAP_TEST_EVENT_COC_DISCONNECT;
+
+    ble_l2cap_test_coc_connect(&t);
+    ble_l2cap_test_coc_send_data(&t);
+    ble_l2cap_test_coc_disc(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_coc_send_data_failed_too_big_sdu)
+{
+    struct test_data t = {};
+    uint16_t small_mtu = 27;
+    uint8_t buf[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
+                    1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM, small_mtu, &t);
+    t.expected_num_of_ev = 3;
+
+    t.event[0].type = BLE_L2CAP_TEST_EVENT_COC_CONNECT;
+    t.event[1].type = BLE_L2CAP_TEST_EVENT_COC_SEND_DATA;
+    t.event[1].data = buf;
+    t.event[1].data_len = sizeof(buf);
+    t.event[1].early_error = BLE_HS_EBADDATA;
+    t.event[2].type = BLE_L2CAP_TEST_EVENT_COC_DISCONNECT;
+
+    ble_l2cap_test_coc_connect(&t);
+    ble_l2cap_test_coc_send_data(&t);
+    ble_l2cap_test_coc_disc(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_coc_recv_data_succeed)
+{
+    struct test_data t = {};
+    uint8_t buf[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
+
+    ble_l2cap_test_util_init();
+
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 3;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[1].type = BLE_L2CAP_EVENT_COC_DATA_RECEIVED;
+    t.event[1].data = buf;
+    t.event[1].data_len = sizeof(buf);
+    t.event[2].type = BLE_L2CAP_EVENT_COC_DISCONNECTED;
+
+    ble_l2cap_test_coc_connect(&t);
+    ble_l2cap_test_coc_recv_data(&t);
+    ble_l2cap_test_coc_disc(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_l2cap_test_case_sig_coc_conn_multi)
+{
+    struct test_data t;
+    int rc;
+
+    ble_l2cap_test_util_init();
+    ble_l2cap_test_set_chan_test_conf(BLE_L2CAP_TEST_PSM,
+                                      BLE_L2CAP_TEST_COC_MTU, &t);
+    t.expected_num_of_ev = 2;
+    t.num = 2;
+
+    t.event[0].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+    t.event[1].type = BLE_L2CAP_EVENT_COC_CONNECTED;
+
+    /* Register server */
+    rc = ble_l2cap_create_server(t.psm, BLE_L2CAP_TEST_COC_MTU,
+                                 ble_l2cap_test_event, &t);
+    TEST_ASSERT(rc == 0);
+
+    ble_l2cap_test_coc_connect_multi(&t);
+
+    TEST_ASSERT(t.expected_num_of_ev == t.event_cnt);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_l2cap_test_suite)
+{
+    ble_l2cap_test_case_bad_header();
+    ble_l2cap_test_case_bad_handle();
+    ble_l2cap_test_case_frag_single();
+    ble_l2cap_test_case_frag_multiple();
+    ble_l2cap_test_case_frag_channels();
+    ble_l2cap_test_case_frag_timeout();
+    ble_l2cap_test_case_sig_unsol_rsp();
+    ble_l2cap_test_case_sig_update_accept();
+    ble_l2cap_test_case_sig_update_reject();
+    ble_l2cap_test_case_sig_update_init_accept();
+    ble_l2cap_test_case_sig_update_init_reject();
+    ble_l2cap_test_case_sig_update_init_fail_master();
+    ble_l2cap_test_case_sig_update_init_fail_bad_id();
+    ble_l2cap_test_case_sig_coc_conn_invalid_psm();
+    ble_l2cap_test_case_sig_coc_conn_out_of_resource();
+    ble_l2cap_test_case_sig_coc_conn_invalid_cid();
+    ble_l2cap_test_case_sig_coc_conn_insuff_authen();
+    ble_l2cap_test_case_sig_coc_conn_insuff_author();
+    ble_l2cap_test_case_sig_coc_incoming_conn_invalid_psm();
+    ble_l2cap_test_case_sig_coc_incoming_conn_rejected_by_app();
+    ble_l2cap_test_case_sig_coc_incoming_conn_success();
+    ble_l2cap_test_case_sig_coc_disconnect_succeed();
+    ble_l2cap_test_case_sig_coc_incoming_disconnect_succeed();
+    ble_l2cap_test_case_sig_coc_incoming_disconnect_failed();
+    ble_l2cap_test_case_coc_send_data_succeed();
+    ble_l2cap_test_case_coc_send_data_failed_too_big_sdu();
+    ble_l2cap_test_case_coc_recv_data_succeed();
+    ble_l2cap_test_case_sig_coc_conn_multi();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_os_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_os_test.c
new file mode 100644
index 00000000..fa57571b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_os_test.c
@@ -0,0 +1,388 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <string.h>
+#include "os/os.h"
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble_hs_test.h"
+#include "host/ble_gap.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_OS_TEST_STACK_SIZE      256
+#define BLE_OS_TEST_APP_STACK_SIZE  256
+
+#define BLE_OS_TEST_APP_PRIO         9
+#define BLE_OS_TEST_TASK_PRIO        10
+
+static struct os_task ble_os_test_task;
+static struct os_task ble_os_test_app_task;
+static os_stack_t ble_os_test_stack[OS_STACK_ALIGN(BLE_OS_TEST_STACK_SIZE)];
+
+static os_stack_t
+ble_os_test_app_stack[OS_STACK_ALIGN(BLE_OS_TEST_APP_STACK_SIZE)];
+
+static uint8_t ble_os_test_peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
+static void ble_os_test_app_task_handler(void *arg);
+
+static int ble_os_test_gap_event_type;
+
+static void
+ble_os_test_init_app_task(void)
+{
+    int rc;
+
+    rc = os_task_init(&ble_os_test_app_task,
+                      "ble_gap_terminate_test_task",
+                      ble_os_test_app_task_handler, NULL,
+                      BLE_OS_TEST_APP_PRIO, OS_WAIT_FOREVER,
+                      ble_os_test_app_stack,
+                      OS_STACK_ALIGN(BLE_OS_TEST_APP_STACK_SIZE));
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_os_test_misc_init(void)
+{
+    extern os_time_t g_os_time;
+
+    ble_hs_test_util_init_no_start();
+
+    /* Allow the OS to approach tick rollover.  This will help ensure host
+     * timers don't break when the tick counter resets.
+     */
+    g_os_time = UINT32_MAX - 10 * OS_TICKS_PER_SEC;
+
+    /* Receive acknowledgements for the startup sequence.  We sent the
+     * corresponding requests when the host task was started.
+     */
+    ble_hs_test_util_hci_ack_set_startup();
+
+    ble_os_test_init_app_task();
+}
+
+static int
+ble_os_test_misc_conn_exists(uint16_t conn_handle)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+
+    if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+        conn = ble_hs_conn_first();
+    } else {
+        conn = ble_hs_conn_find(conn_handle);
+    }
+
+    ble_hs_unlock();
+
+    return conn != NULL;
+}
+
+static int
+ble_gap_direct_connect_test_connect_cb(struct ble_gap_event *event, void *arg)
+{
+    struct ble_gap_conn_desc desc;
+    int *cb_called;
+    int rc;
+
+    cb_called = arg;
+    *cb_called = 1;
+
+    TEST_ASSERT(event->type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(event->connect.status == 0);
+    TEST_ASSERT(event->connect.conn_handle == 2);
+
+    rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.peer_id_addr.type == BLE_ADDR_PUBLIC);
+    TEST_ASSERT(memcmp(desc.peer_id_addr.val, ble_os_test_peer_addr, 6) == 0);
+
+    return 0;
+}
+
+static void
+ble_gap_direct_connect_test_task_handler(void *arg)
+{
+    struct ble_gap_conn_complete evt;
+    ble_addr_t addr = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    int cb_called;
+    int rc;
+
+    /* Set the connect callback so we can verify that it gets called with the
+     * proper arguments.
+     */
+    cb_called = 0;
+
+    /* Make sure there are no created connections and no connections in
+     * progress.
+     */
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+
+    /* Initiate a direct connection. */
+    ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC, &addr, 0, NULL,
+                             ble_gap_direct_connect_test_connect_cb,
+                             &cb_called, 0);
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(!cb_called);
+
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+
+    /* Receive an HCI connection-complete event. */
+    memset(&evt, 0, sizeof evt);
+    evt.status = BLE_ERR_SUCCESS;
+    evt.connection_handle = 2;
+    memcpy(evt.peer_addr, addr.val, 6);
+    rc = ble_gap_rx_conn_complete(&evt, 0);
+    TEST_ASSERT(rc == 0);
+
+    /* The connection should now be created. */
+    TEST_ASSERT(ble_os_test_misc_conn_exists(2));
+    TEST_ASSERT(cb_called);
+
+    tu_restart();
+}
+
+TEST_CASE_SELF(ble_gap_direct_connect_test_case)
+{
+    ble_os_test_misc_init();
+
+    os_task_init(&ble_os_test_task,
+                 "ble_gap_direct_connect_test_task",
+                 ble_gap_direct_connect_test_task_handler, NULL,
+                 BLE_OS_TEST_TASK_PRIO, OS_WAIT_FOREVER, ble_os_test_stack,
+                 OS_STACK_ALIGN(BLE_OS_TEST_STACK_SIZE));
+
+    os_start();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static int
+ble_os_disc_test_cb(struct ble_gap_event *event, void *arg)
+{
+    int *cb_called;
+
+    cb_called = arg;
+    *cb_called = 1;
+
+    TEST_ASSERT(event->type == BLE_GAP_EVENT_DISC_COMPLETE);
+
+    return 0;
+}
+
+static void
+ble_os_disc_test_task_handler(void *arg)
+{
+    struct ble_gap_disc_params disc_params;
+    int cb_called;
+    int rc;
+
+    /* Receive acknowledgements for the startup sequence.  We sent the
+     * corresponding requests when the host task was started.
+     */
+    ble_hs_test_util_hci_ack_set_startup();
+
+    /* Set the connect callback so we can verify that it gets called with the
+     * proper arguments.
+     */
+    cb_called = 0;
+
+    os_time_delay(10);
+
+    /* Make sure there are no created connections and no connections in
+     * progress.
+     */
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Initiate the general discovery procedure with a 300 ms timeout. */
+    memset(&disc_params, 0, sizeof disc_params);
+    rc = ble_hs_test_util_disc(BLE_OWN_ADDR_PUBLIC, 300, &disc_params,
+                               ble_os_disc_test_cb,
+                               &cb_called, 0, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(ble_gap_master_in_progress());
+    TEST_ASSERT(!cb_called);
+
+    /* Receive acks from the controller. */
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(ble_gap_master_in_progress());
+    TEST_ASSERT(!cb_called);
+
+    /* Wait 100 ms; verify scan still in progress. */
+    os_time_delay(100 * OS_TICKS_PER_SEC / 1000);
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(ble_gap_master_in_progress());
+    TEST_ASSERT(!cb_called);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        0);
+
+    /* Wait 250 more ms; verify scan completed. */
+    os_time_delay(250 * OS_TICKS_PER_SEC / 1000);
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(!ble_gap_master_in_progress());
+    TEST_ASSERT(cb_called);
+
+    tu_restart();
+}
+
+TEST_CASE_SELF(ble_os_disc_test_case)
+{
+    ble_os_test_misc_init();
+
+    os_task_init(&ble_os_test_task,
+                 "ble_os_disc_test_task",
+                 ble_os_disc_test_task_handler, NULL,
+                 BLE_OS_TEST_TASK_PRIO, OS_WAIT_FOREVER, ble_os_test_stack,
+                 OS_STACK_ALIGN(BLE_OS_TEST_STACK_SIZE));
+
+    os_start();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+static int
+ble_gap_terminate_cb(struct ble_gap_event *event, void *arg)
+{
+    int *disconn_handle;
+
+    ble_os_test_gap_event_type = event->type;
+
+    if (event->type == BLE_GAP_EVENT_DISCONNECT) {
+        disconn_handle = arg;
+        *disconn_handle = event->disconnect.conn.conn_handle;
+    }
+
+    return 0;
+}
+
+static void
+ble_gap_terminate_test_task_handler(void *arg)
+{
+    struct ble_gap_conn_complete conn_evt;
+    ble_addr_t addr1 = { BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 }};
+    ble_addr_t addr2 = { BLE_ADDR_PUBLIC, { 2, 3, 4, 5, 6, 7 }};
+    int disconn_handle;
+    int rc;
+
+    /* Receive acknowledgements for the startup sequence.  We sent the
+     * corresponding requests when the host task was started.
+     */
+    ble_hs_test_util_hci_ack_set_startup();
+
+    /* Set the connect callback so we can verify that it gets called with the
+     * proper arguments.
+     */
+    disconn_handle = 0;
+
+    /* Make sure there are no created connections and no connections in
+     * progress.
+     */
+    TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
+    TEST_ASSERT(!ble_gap_master_in_progress());
+
+    /* Create two direct connections. */
+    ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC,
+                             &addr1, 0, NULL, ble_gap_terminate_cb,
+                             &disconn_handle, 0);
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+    memset(&conn_evt, 0, sizeof conn_evt);
+    conn_evt.status = BLE_ERR_SUCCESS;
+    conn_evt.connection_handle = 1;
+    memcpy(conn_evt.peer_addr, addr1.val, 6);
+    rc = ble_gap_rx_conn_complete(&conn_evt, 0);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_connect(BLE_OWN_ADDR_PUBLIC,
+                             &addr2, 0, NULL, ble_gap_terminate_cb,
+                             &disconn_handle, 0);
+    /* ble_gap_rx_conn_complete() will send extra HCI command, need phony ack */
+    ble_hs_test_util_hci_ack_set(ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_REM_FEAT), 0);
+    memset(&conn_evt, 0, sizeof conn_evt);
+    conn_evt.status = BLE_ERR_SUCCESS;
+    conn_evt.connection_handle = 2;
+    memcpy(conn_evt.peer_addr, addr2.val, 6);
+    rc = ble_gap_rx_conn_complete(&conn_evt, 0);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT_FATAL(ble_os_test_misc_conn_exists(1));
+    TEST_ASSERT_FATAL(ble_os_test_misc_conn_exists(2));
+
+    /* Terminate the first one. */
+    rc = ble_hs_test_util_conn_terminate(1, 0);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_hci_rx_disconn_complete_event(1, 0, BLE_ERR_REM_USER_CONN_TERM);
+    TEST_ASSERT(ble_os_test_gap_event_type == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(disconn_handle == 1);
+    TEST_ASSERT_FATAL(!ble_os_test_misc_conn_exists(1));
+    TEST_ASSERT_FATAL(ble_os_test_misc_conn_exists(2));
+
+    /* Terminate the second one. */
+    rc = ble_hs_test_util_conn_terminate(2, 0);
+    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_hci_rx_disconn_complete_event(2, 0, BLE_ERR_REM_USER_CONN_TERM);
+    TEST_ASSERT(ble_os_test_gap_event_type == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(disconn_handle == 2);
+    TEST_ASSERT_FATAL(!ble_os_test_misc_conn_exists(1));
+    TEST_ASSERT_FATAL(!ble_os_test_misc_conn_exists(2));
+
+    tu_restart();
+}
+
+static void
+ble_os_test_app_task_handler(void *arg)
+{
+    while (1) {
+        os_eventq_run(os_eventq_dflt_get());
+    }
+}
+
+TEST_CASE_SELF(ble_gap_terminate_test_case)
+{
+    ble_os_test_misc_init();
+
+    os_task_init(&ble_os_test_task,
+                 "ble_gap_terminate_test_task",
+                 ble_gap_terminate_test_task_handler, NULL,
+                 BLE_OS_TEST_TASK_PRIO, OS_WAIT_FOREVER, ble_os_test_stack,
+                 OS_STACK_ALIGN(BLE_OS_TEST_STACK_SIZE));
+
+    os_start();
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_os_test_suite)
+{
+    ble_os_disc_test_case();
+    ble_gap_direct_connect_test_case();
+    ble_gap_terminate_test_case();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_lgcy_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_lgcy_test.c
new file mode 100644
index 00000000..4529d362
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_lgcy_test.c
@@ -0,0 +1,849 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+#include "ble_sm_test_util.h"
+
+#if NIMBLE_BLE_SM
+
+/**
+ * Legacy pairing
+ * Master: peer
+ * Pair algorithm: just works
+ * Initiator IO capabilities: 4
+ * Responder IO capabilities: 3
+ * Bonding: true
+ * Initiator address type: BLE_ADDR_PUBLIC
+ * Responder address type: BLE_ADDR_PUBLIC
+ * Initiator key distribution: 7
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_lgcy_peer_jw_iio4_rio3_b1_iat0_rat0_ik7_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x05,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .confirm_req[0] = {
+            .value = {
+                0xcd, 0x5b, 0x79, 0x29, 0x53, 0x31, 0x56, 0x23,
+                0x2c, 0x08, 0xed, 0x81, 0x16, 0x55, 0x8e, 0x01,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x49, 0x39, 0x22, 0x0f, 0x7b, 0x1b, 0x80, 0xcd,
+                0xbe, 0x89, 0xd1, 0x4c, 0xbd, 0x6f, 0xda, 0x2c,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x7f, 0x42, 0xc0, 0x2f, 0x1d, 0x07, 0x37, 0xfc,
+                0x04, 0x5b, 0x05, 0x9a, 0xed, 0x67, 0xa5, 0x68,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x42, 0x1a, 0x58, 0xa2, 0x3b, 0x80, 0xde, 0xef,
+                0x95, 0x0d, 0xf7, 0xca, 0x06, 0x05, 0x01, 0x3c,
+            },
+        },
+        .enc_info_req = {
+            .ltk = {
+                0x2f, 0x9b, 0x16, 0xff, 0xf3, 0x73, 0x30, 0x08,
+                0xa8, 0xe5, 0x01, 0xb1, 0x3b, 0xe1, 0x87, 0x00,
+            },
+        },
+        .master_id_req = {
+            .ediv = 0xf8e0,
+            .rand_val = 0xef7c818b00000000,
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0xc6, 0x17, 0xc0, 0x02, 0x40, 0x0d, 0x27, 0x51,
+                0x8a, 0x77, 0xb5, 0xae, 0xd8, 0xa9, 0x7a, 0x7a,
+            },
+        },
+        .enc_info_rsp = {
+            .ltk = {
+                0xd7, 0x07, 0x22, 0x79, 0x24, 0xc6, 0xcb, 0x4d,
+                0xa3, 0xdd, 0x01, 0xfb, 0x48, 0x87, 0xd4, 0xcf,
+            },
+        },
+        .master_id_rsp = {
+            .ediv = 0x9a39,
+            .rand_val = 0x8e76d9b00000000,
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xeb, 0x8a, 0x06, 0xc4, 0x93, 0x51, 0x04, 0xb3,
+                0x8b, 0xbf, 0xe8, 0x1f, 0x0e, 0x96, 0x2a, 0x54,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x14, 0x55, 0x93, 0xe1, 0xd1, 0xe7, 0xc4, 0x5d,
+                0x35, 0x97, 0xd3, 0x05, 0x30, 0xc8, 0x9d, 0x83,
+            },
+        },
+        .stk = {
+            0x1c, 0xd7, 0xb6, 0x35, 0x48, 0xfc, 0x9f, 0xef,
+            0x0e, 0x2f, 0x51, 0x77, 0xed, 0xdd, 0xbc, 0xaf,
+        },
+        .pair_alg = 0,
+        .authenticated = false,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NONE,
+            },
+        },
+    };
+    ble_sm_test_util_peer_lgcy_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Legacy pairing
+ * Master: peer
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 4
+ * Responder IO capabilities: 0
+ * Bonding: true
+ * Initiator address type: BLE_ADDR_PUBLIC
+ * Responder address type: BLE_ADDR_PUBLIC
+ * Initiator key distribution: 7
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_lgcy_peer_pk_iio4_rio0_b1_iat0_rat0_ik7_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x05,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x00,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .confirm_req[0] = {
+            .value = {
+                0xa0, 0x10, 0x4a, 0xaa, 0x8b, 0x53, 0x78, 0xbb,
+                0xd2, 0xae, 0x71, 0x1f, 0x4e, 0x00, 0x70, 0x8b,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x62, 0xf3, 0xba, 0x0e, 0xe5, 0xbe, 0x2e, 0xd8,
+                0x25, 0xb2, 0xec, 0x4c, 0x28, 0x77, 0x28, 0x60,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x84, 0xcf, 0xe4, 0x04, 0x7d, 0xf3, 0xfc, 0xa1,
+                0x3f, 0x75, 0xd6, 0x5a, 0x7c, 0xb7, 0xa4, 0x39,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0xef, 0x6a, 0x61, 0x6e, 0x02, 0x60, 0x7f, 0x5d,
+                0x7f, 0x0d, 0xa6, 0x3c, 0x06, 0x1a, 0x5d, 0xd6,
+            },
+        },
+        .enc_info_req = {
+            .ltk = {
+                0xad, 0x01, 0x6d, 0x76, 0xa9, 0xd0, 0x23, 0xc9,
+                0x40, 0x0c, 0xbf, 0x2a, 0x4c, 0x23, 0x31, 0xc5,
+            },
+        },
+        .master_id_req = {
+            .ediv = 0xa74f,
+            .rand_val = 0x81cab3fd00000000,
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x60, 0x08, 0x49, 0x00, 0x6d, 0x76, 0x98, 0x73,
+                0x9c, 0x95, 0xc4, 0xd9, 0xe8, 0x3a, 0x69, 0xbb,
+            },
+        },
+        .enc_info_rsp = {
+            .ltk = {
+                0x5b, 0x73, 0x39, 0xd9, 0x51, 0x3d, 0x92, 0xa4,
+                0x34, 0x65, 0xa5, 0x70, 0x49, 0xbe, 0x11, 0x28,
+            },
+        },
+        .master_id_rsp = {
+            .ediv = 0x9705,
+            .rand_val = 0x592f1e8d00000000,
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xeb, 0x8a, 0x06, 0xc4, 0x93, 0x51, 0x04, 0xb3,
+                0x8b, 0xbf, 0xe8, 0x1f, 0x0e, 0x96, 0x2a, 0x54,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0xc9, 0x9b, 0xf2, 0x75, 0xb7, 0x0d, 0xe8, 0x60,
+                0x3d, 0xf0, 0xd6, 0xa8, 0x16, 0xc5, 0x6c, 0x2a,
+            },
+        },
+        .stk = {
+            0xf2, 0x3c, 0x36, 0xc4, 0xa1, 0xfb, 0x5a, 0xa7,
+            0x96, 0x20, 0xe4, 0x29, 0xb7, 0x58, 0x22, 0x7a,
+        },
+        .pair_alg = 1,
+        .authenticated = true,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_DISP,
+                .passkey = 46128,
+            },
+        },
+    };
+    ble_sm_test_util_peer_lgcy_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Legacy pairing
+ * Master: us
+ * Pair algorithm: just works
+ * Initiator IO capabilities: 3
+ * Responder IO capabilities: 3
+ * Bonding: true
+ * Initiator address type: BLE_ADDR_PUBLIC
+ * Responder address type: BLE_ADDR_RANDOM
+ * Initiator key distribution: 7
+ * Responder key distribution: 5
+ */
+TEST_CASE_SELF(ble_sm_lgcy_us_jw_iio3_rio3_b1_iat0_rat1_ik7_rk5)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x0f, 0x0e, 0x0d, 0x0c, 0x0b, 0x0a,
+        },
+        .resp_addr_type = BLE_ADDR_RANDOM,
+        .resp_id_addr = {
+            0x11, 0x22, 0x11, 0x22, 0x11, 0xcc,
+        },
+        .pair_req = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x01,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x01,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x05,
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x1c, 0xb6, 0x10, 0xea, 0x02, 0x08, 0x90, 0x64,
+                0xc7, 0xf8, 0xe5, 0x9c, 0xb4, 0x3a, 0x18, 0xca,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0xb8, 0x6f, 0xd1, 0xc6, 0x74, 0x35, 0xa3, 0x94,
+                0x68, 0x2f, 0xf1, 0x4c, 0x78, 0x44, 0xe8, 0x0d,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x40, 0x48, 0x17, 0x4d, 0x42, 0xa0, 0xf8, 0xd5,
+                0xbf, 0x65, 0x67, 0xb8, 0x5e, 0x57, 0x38, 0xac,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x2c, 0xa1, 0xb1, 0xf5, 0x54, 0x9b, 0x43, 0xe9,
+                0xb0, 0x62, 0x6a, 0xb0, 0x02, 0xb8, 0x6c, 0xca,
+            },
+        },
+        .enc_info_req = {
+            .ltk = {
+                0x01, 0x15, 0xb6, 0x93, 0xc9, 0xff, 0xfe, 0x27,
+                0x02, 0x41, 0xfd, 0x7b, 0x0e, 0x31, 0xd4, 0xa6,
+            },
+        },
+        .master_id_req = {
+            .ediv = 0xe4fb,
+            .rand_val = 0x8eee76b100000000,
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x00, 0x2d, 0xf6, 0x3e, 0x5e, 0x0f, 0xd1, 0xe8,
+                0x4e, 0x5f, 0x61, 0x1c, 0x2c, 0x0b, 0xa5, 0x51,
+            },
+        },
+        .enc_info_rsp = {
+            .ltk = {
+                0x88, 0xbc, 0x95, 0x8d, 0xaa, 0x26, 0x8d, 0xd5,
+                0x18, 0xc9, 0x06, 0x70, 0xc2, 0x30, 0x56, 0x4c,
+            },
+        },
+        .master_id_rsp = {
+            .ediv = 0x4413,
+            .rand_val = 0xfad1c27300000000,
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0f, 0x0e, 0x0d, 0x0c, 0x0b, 0x0a,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x03, 0xad, 0xa4, 0xe1, 0x34, 0x76, 0x95, 0x54,
+                0xe5, 0x8f, 0xa4, 0x06, 0x72, 0xe6, 0xfc, 0x65,
+            },
+        },
+        .stk = {
+            0x31, 0x54, 0x42, 0x6c, 0x1c, 0x03, 0x36, 0x44,
+            0x0b, 0x72, 0x90, 0xa5, 0x1f, 0x79, 0x5b, 0xe9,
+        },
+        .pair_alg = 0,
+        .authenticated = false,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NONE,
+            },
+        },
+    };
+    ble_sm_test_util_us_lgcy_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Legacy pairing
+ * Master: us
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 4
+ * Responder IO capabilities: 2
+ * Bonding: true
+ * Initiator address type: BLE_ADDR_PUBLIC
+ * Responder address type: BLE_ADDR_RANDOM
+ * Initiator key distribution: 7
+ * Responder key distribution: 5
+ */
+TEST_CASE_SELF(ble_sm_lgcy_us_pk_iio4_rio2_b1_iat0_rat1_ik7_rk5)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x0f, 0x0e, 0x0d, 0x0c, 0x0b, 0x0a,
+        },
+        .resp_addr_type = BLE_ADDR_RANDOM,
+        .resp_id_addr = {
+            0x11, 0x22, 0x11, 0x22, 0x11, 0xcc,
+        },
+        .pair_req = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x02,
+            .oob_data_flag = 0x00,
+            .authreq = 0x05,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x05,
+        },
+        .confirm_req[0] = {
+            .value = {
+                0xb5, 0xd4, 0xc5, 0xe8, 0xef, 0xef, 0xd8, 0xd7,
+                0x2b, 0x14, 0x34, 0x35, 0x29, 0x18, 0xda, 0x12,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x1a, 0x03, 0x20, 0xda, 0x60, 0x21, 0x9b, 0x4b,
+                0x5d, 0x45, 0x90, 0x64, 0xe1, 0x24, 0x2c, 0xb6,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x45, 0xa3, 0x1a, 0x0b, 0xf6, 0x0f, 0x7c, 0xcf,
+                0x1a, 0xfb, 0xfc, 0x1a, 0xad, 0x62, 0x0e, 0x76,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x82, 0xbb, 0x9f, 0x67, 0xc4, 0x88, 0xcb, 0x58,
+                0xee, 0xf9, 0x34, 0x35, 0x23, 0xa3, 0xd0, 0x22,
+            },
+        },
+        .enc_info_req = {
+            .ltk = {
+                0xfa, 0x43, 0x8f, 0x1f, 0xe6, 0x2a, 0x94, 0x5b,
+                0x54, 0x89, 0x2b, 0x0f, 0xd7, 0x23, 0x77, 0x9e,
+            },
+        },
+        .master_id_req = {
+            .ediv = 0x88b3,
+            .rand_val = 0x7c970e18dec74560,
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x2e, 0x70, 0x3c, 0xbf, 0x20, 0xbe, 0x7d, 0x2d,
+                0xb3, 0x50, 0x46, 0x33, 0x4c, 0x20, 0x0e, 0xc8,
+            },
+        },
+        .enc_info_rsp = {
+            .ltk = {
+                0xc1, 0x64, 0x33, 0x10, 0x0f, 0x70, 0x2f, 0x9c,
+                0xe7, 0x31, 0xc5, 0x32, 0xdd, 0x98, 0x16, 0x75,
+            },
+        },
+        .master_id_rsp = {
+            .ediv = 0x1c19,
+            .rand_val = 0xef308872dc2a4cc2,
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0f, 0x0e, 0x0d, 0x0c, 0x0b, 0x0a,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0xd7, 0x75, 0xfa, 0xed, 0xd7, 0xdd, 0x7b, 0xb3,
+                0xa4, 0x20, 0xea, 0x2f, 0x75, 0x60, 0xb1, 0x84,
+            },
+        },
+        .stk = {
+            0x9e, 0xe8, 0x35, 0x22, 0xb6, 0xbb, 0x54, 0x0d,
+            0x48, 0x1b, 0x25, 0xa0, 0xd8, 0xe2, 0xa5, 0x08,
+        },
+        .pair_alg = 1,
+        .authenticated = true,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_DISP,
+                .passkey = 46128,
+            },
+        },
+    };
+    ble_sm_test_util_us_lgcy_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Legacy pairing
+ * Master: us
+ * Pair algorithm: out of band
+ * Initiator IO capabilities: 3
+ * Responder IO capabilities: 3
+ * Bonding: true
+ * Initiator address type: BLE_ADDR_PUBLIC
+ * Responder address type: BLE_ADDR_PUBLIC
+ * Initiator key distribution: 7
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_lgcy_us_ob_iio3_rio3_b1_iat0_rat0_ik7_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+        },
+        .resp_id_addr = {
+            0x66, 0x33, 0x22, 0x66, 0x55, 0x11,
+        },
+        .pair_req = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x01,
+            .authreq = 0x05,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x01,
+            .authreq = 0x05,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x2c, 0x3f, 0x3e, 0xf5, 0x39, 0x50, 0x78, 0x4a,
+                0x3e, 0x14, 0x1a, 0x51, 0xfb, 0x8d, 0x6c, 0x10,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0xa9, 0x5c, 0x18, 0xb1, 0xdb, 0x51, 0x53, 0xa5,
+                0xd3, 0xe7, 0x72, 0x17, 0xfb, 0xa8, 0xfb, 0xeb,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x40, 0x2f, 0x42, 0xba, 0x10, 0x7b, 0x22, 0x65,
+                0x84, 0xef, 0x63, 0xdf, 0x84, 0x7b, 0x04, 0xef,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x94, 0xdc, 0x3c, 0xef, 0x65, 0xf7, 0x99, 0x2e,
+                0x50, 0x29, 0x97, 0x2a, 0x57, 0xfd, 0xe6, 0x6a,
+            },
+        },
+        .enc_info_req = {
+            .ltk = {
+                0x8c, 0x8e, 0x57, 0xba, 0x17, 0xbb, 0x04, 0xb5,
+                0x16, 0xad, 0x31, 0x37, 0xf8, 0x3e, 0x4f, 0x21,
+            },
+        },
+        .master_id_req = {
+            .ediv = 0xaaa4,
+            .rand_val = 0xc0c830e300000000,
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x66, 0x33, 0x22, 0x66, 0x55, 0x11,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x5a, 0xe4, 0x2b, 0x40, 0x3a, 0x34, 0x1d, 0x94,
+                0x56, 0x7d, 0xf4, 0x41, 0x23, 0x81, 0xc4, 0x11,
+            },
+        },
+        .enc_info_rsp = {
+            .ltk = {
+                0xa6, 0x8e, 0xa0, 0xa4, 0x02, 0x64, 0x4c, 0x09,
+                0x31, 0x25, 0x8a, 0x4f, 0x49, 0x35, 0xb0, 0x1f,
+            },
+        },
+        .master_id_rsp = {
+            .ediv = 0x57a3,
+            .rand_val = 0x8276af9000000000,
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x8e, 0xef, 0x53, 0x5c, 0x1b, 0x21, 0x67, 0x8d,
+                0x07, 0x5e, 0xaa, 0xe8, 0x41, 0xa9, 0x36, 0xcf,
+            },
+        },
+        .stk = {
+            0x4c, 0xd4, 0xa7, 0xee, 0x83, 0xcd, 0xd1, 0x9e,
+            0x84, 0xeb, 0xb8, 0xd2, 0xaf, 0x4a, 0x71, 0x2e,
+        },
+        .pair_alg = 2,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_OOB,
+                .oob = {
+                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+                    0x09, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16,
+                },
+            },
+        },
+    };
+    ble_sm_test_util_us_lgcy_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Legacy pairing
+ * Master: peer
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 4
+ * Responder IO capabilities: 4
+ * Bonding: true
+ * Initiator address type: BLE_ADDR_PUBLIC
+ * Responder address type: BLE_ADDR_PUBLIC
+ * Initiator key distribution: 7
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_lgcy_peer_pk_iio4_rio4_b1_iat0_rat0_ik7_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x05,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x93, 0x64, 0xb1, 0xb0, 0x07, 0x41, 0x22, 0xe7,
+                0x3e, 0x5a, 0x87, 0xf5, 0x1f, 0x25, 0x79, 0x11,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x2d, 0x40, 0x15, 0xc4, 0x21, 0xeb, 0xd5, 0x73,
+                0xc8, 0x5d, 0xb8, 0xb9, 0x45, 0x31, 0xd5, 0x58,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x8c, 0x2c, 0x3b, 0xf3, 0x90, 0xaa, 0x2e, 0xcf,
+                0xc7, 0x5b, 0xf6, 0xae, 0xb6, 0x4c, 0xc3, 0x61,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x7a, 0x94, 0x97, 0x0a, 0xbe, 0xaf, 0xc0, 0x6b,
+                0xd4, 0xf4, 0x04, 0xd1, 0x21, 0x46, 0x34, 0xf3,
+            },
+        },
+        .enc_info_req = {
+            .ltk = {
+                0x3a, 0x10, 0xd1, 0xab, 0x13, 0xee, 0x16, 0xee,
+                0xcf, 0xae, 0xf1, 0x63, 0xf0, 0x6f, 0xb0, 0x89,
+            },
+        },
+        .master_id_req = {
+            .ediv = 0xb634,
+            .rand_val = 0xa99ac2007b4278a8,
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x51, 0x4b, 0x7b, 0x31, 0xf7, 0xa6, 0x8a, 0x60,
+                0x4f, 0x10, 0x04, 0x5f, 0xb8, 0xee, 0xf6, 0xb3,
+            },
+        },
+        .enc_info_rsp = {
+            .ltk = {
+                0xa1, 0x1d, 0xdd, 0xee, 0x85, 0xcb, 0xe0, 0x48,
+                0x1e, 0xdd, 0xa4, 0x9d, 0xed, 0x3f, 0x15, 0x17,
+            },
+        },
+        .master_id_rsp = {
+            .ediv = 0x7e06,
+            .rand_val = 0xe6077f688c5ca67,
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xeb, 0x8a, 0x06, 0xc4, 0x93, 0x51, 0x04, 0xb3,
+                0x8b, 0xbf, 0xe8, 0x1f, 0x0e, 0x96, 0x2a, 0x54,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x16, 0x7a, 0x2e, 0x9d, 0x43, 0x4d, 0x7b, 0x0b,
+                0x88, 0xe2, 0x11, 0xb0, 0x4d, 0xa1, 0xed, 0x08,
+            },
+        },
+        .stk = {
+            0x6c, 0x3e, 0x78, 0x47, 0xe8, 0x57, 0x9f, 0xe9,
+            0x3a, 0x8f, 0x0a, 0xbb, 0xd4, 0x60, 0xf6, 0x0d,
+        },
+        .pair_alg = 1,
+        .authenticated = true,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_INPUT,
+                .passkey = 449182,
+            },
+        },
+    };
+    ble_sm_test_util_peer_lgcy_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_sm_lgcy_test_suite)
+{
+    /*** No privacy. */
+
+    /* Peer as initiator. */
+    ble_sm_lgcy_peer_jw_iio4_rio3_b1_iat0_rat0_ik7_rk7();
+    ble_sm_lgcy_peer_pk_iio4_rio0_b1_iat0_rat0_ik7_rk7();
+    ble_sm_lgcy_peer_pk_iio4_rio4_b1_iat0_rat0_ik7_rk7();
+
+    /* Us as initiator. */
+    ble_sm_lgcy_us_jw_iio3_rio3_b1_iat0_rat1_ik7_rk5();
+    ble_sm_lgcy_us_pk_iio4_rio2_b1_iat0_rat1_ik7_rk5();
+    ble_sm_lgcy_us_ob_iio3_rio3_b1_iat0_rat0_ik7_rk7();
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_sc_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_sc_test.c
new file mode 100644
index 00000000..c3d19550
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_sc_test.c
@@ -0,0 +1,4938 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+#include "ble_sm_test_util.h"
+
+#if NIMBLE_BLE_SM
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: just works
+ * Initiator IO capabilities: 3
+ * Responder IO capabilities: 3
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 5
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_jw_iio3_rio3_b1_iat0_rat0_ik5_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x0d,
+            .resp_key_dist = 0x0f,
+        },
+        .pair_rsp = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x05,
+            .resp_key_dist = 0x07,
+        },
+        .our_priv_key = {
+            0x54, 0x8d, 0x20, 0xb8, 0x97, 0x0b, 0xbc, 0x43,
+            0x9a, 0xad, 0x10, 0x6f, 0x60, 0x74, 0xd4, 0x6a,
+            0x55, 0xc1, 0x7a, 0x17, 0x8b, 0x60, 0xe0, 0xb4,
+            0x5a, 0xe6, 0x58, 0xf1, 0xea, 0x12, 0xd9, 0xfb,
+        },
+        .public_key_req = {
+            .x = {
+                0xbc, 0xf2, 0xd8, 0xa5, 0xdb, 0xa3, 0x95, 0x6c,
+                0x99, 0xf9, 0x11, 0x0d, 0x4d, 0x2e, 0xf0, 0xbd,
+                0xee, 0x9b, 0x69, 0xb6, 0xcd, 0x88, 0x74, 0xbe,
+                0x40, 0xe8, 0xe5, 0xcc, 0xdc, 0x88, 0x44, 0x53,
+            },
+            .y = {
+                0xbf, 0xa9, 0x82, 0x0e, 0x18, 0x7a, 0x14, 0xf8,
+                0x77, 0xfd, 0x8e, 0x92, 0x2a, 0xf8, 0x5d, 0x39,
+                0xd1, 0x6d, 0x92, 0x1f, 0x38, 0x74, 0x99, 0xdc,
+                0x6c, 0x2c, 0x94, 0x23, 0xf9, 0x72, 0x56, 0xab,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x72, 0x8c, 0xd1, 0x88, 0xd7, 0xbe, 0x49, 0xb2,
+                0xc5, 0x5c, 0x95, 0xb3, 0x64, 0xe0, 0x12, 0x32,
+                0xb6, 0xc9, 0x47, 0x63, 0x37, 0x38, 0x5b, 0x9c,
+                0x1e, 0x1b, 0x1a, 0x06, 0x09, 0xe2, 0x31, 0x85,
+            },
+            .y = {
+                0x19, 0x3a, 0x29, 0x69, 0x62, 0xd6, 0x30, 0xe7,
+                0xe8, 0x48, 0x63, 0xdc, 0x00, 0x73, 0x0a, 0x70,
+                0x7d, 0x2e, 0x29, 0xcc, 0x91, 0x77, 0x71, 0xb1,
+                0x75, 0xb8, 0xf7, 0xdc, 0xb0, 0xe2, 0x91, 0x10,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x82, 0xed, 0xd0, 0x62, 0x91, 0x3d, 0x96, 0x7f,
+                0x13, 0xc5, 0x0d, 0x02, 0x2b, 0x5e, 0x43, 0x16,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xa4, 0x34, 0x5f, 0xb3, 0xaf, 0x73, 0x43, 0x64,
+                0xcd, 0x19, 0x1b, 0x5b, 0x87, 0x58, 0x31, 0x66,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0xc0, 0x91, 0xfb, 0xb3, 0x77, 0xa2, 0x02, 0x0b,
+                0xc6, 0xcd, 0x6c, 0x04, 0x51, 0x45, 0x45, 0x39,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x82, 0x65, 0x1d, 0x02, 0xed, 0x89, 0x13, 0x44,
+                0x04, 0x1a, 0x14, 0x7c, 0x32, 0x9a, 0x1e, 0x7d,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x06, 0x3c, 0x28, 0x4a, 0xe5, 0x48, 0x4b, 0x51,
+                0x65, 0x4e, 0x14, 0x5e, 0x2f, 0xdd, 0xfa, 0x22,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x40, 0x53, 0xeb, 0x7a, 0x4d, 0x8e, 0xa2, 0xb5,
+                0xca, 0xa1, 0xb6, 0xae, 0x7e, 0x6a, 0x4d, 0xd9,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0xbc, 0x13, 0x4b, 0x45, 0xda, 0x76, 0x5b, 0xcd,
+                0xc2, 0x43, 0x81, 0xb8, 0xc3, 0x68, 0x12, 0xbb,
+            },
+        },
+        .ltk = {
+            0x63, 0x59, 0x8a, 0x14, 0x09, 0x4b, 0x94, 0x6e,
+            0xff, 0xae, 0x5e, 0x53, 0x86, 0x02, 0xa3, 0x6c,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_JW,
+        .authenticated = 0,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NONE,
+            },
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 0
+ * Responder IO capabilities: 2
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 5
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_pk_iio0_rio2_b1_iat0_rat0_ik5_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x00,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x0d,
+            .resp_key_dist = 0x0f,
+        },
+        .pair_rsp = {
+            .io_cap = 0x02,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x05,
+            .resp_key_dist = 0x07,
+        },
+        .our_priv_key = {
+            0x54, 0x8d, 0x20, 0xb8, 0x97, 0x0b, 0xbc, 0x43,
+            0x9a, 0xad, 0x10, 0x6f, 0x60, 0x74, 0xd4, 0x6a,
+            0x55, 0xc1, 0x7a, 0x17, 0x8b, 0x60, 0xe0, 0xb4,
+            0x5a, 0xe6, 0x58, 0xf1, 0xea, 0x12, 0xd9, 0xfb,
+        },
+        .public_key_req = {
+            .x = {
+                0x22, 0x26, 0xcc, 0x64, 0x4d, 0xc1, 0x01, 0xd1,
+                0xb9, 0x8d, 0xe2, 0xd4, 0xbc, 0x55, 0x37, 0x4c,
+                0x12, 0x81, 0x14, 0x83, 0x81, 0xe8, 0x36, 0x1b,
+                0x78, 0xff, 0x49, 0xfc, 0xe9, 0x2e, 0x56, 0xc0,
+            },
+            .y = {
+                0xd9, 0x31, 0xa5, 0x8d, 0x02, 0xf1, 0x94, 0xb6,
+                0x83, 0x97, 0xd1, 0xfb, 0x01, 0x97, 0x4d, 0x06,
+                0xec, 0x18, 0x8d, 0x4a, 0xd2, 0x14, 0x12, 0x95,
+                0x2d, 0x4d, 0x18, 0xde, 0x4d, 0xaa, 0x91, 0x25,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x72, 0x8c, 0xd1, 0x88, 0xd7, 0xbe, 0x49, 0xb2,
+                0xc5, 0x5c, 0x95, 0xb3, 0x64, 0xe0, 0x12, 0x32,
+                0xb6, 0xc9, 0x47, 0x63, 0x37, 0x38, 0x5b, 0x9c,
+                0x1e, 0x1b, 0x1a, 0x06, 0x09, 0xe2, 0x31, 0x85,
+            },
+            .y = {
+                0x19, 0x3a, 0x29, 0x69, 0x62, 0xd6, 0x30, 0xe7,
+                0xe8, 0x48, 0x63, 0xdc, 0x00, 0x73, 0x0a, 0x70,
+                0x7d, 0x2e, 0x29, 0xcc, 0x91, 0x77, 0x71, 0xb1,
+                0x75, 0xb8, 0xf7, 0xdc, 0xb0, 0xe2, 0x91, 0x10,
+            },
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x2c, 0x16, 0x15, 0x0d, 0xe8, 0x18, 0x50, 0xd8,
+                0xae, 0x04, 0x6c, 0xa8, 0x50, 0xb8, 0xe5, 0x85,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x62, 0x53, 0xfb, 0x69, 0x94, 0x33, 0x11, 0xd3,
+                0x8e, 0x03, 0xd5, 0x05, 0xd7, 0x68, 0x33, 0x16,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xd5, 0x0e, 0x27, 0xcf, 0xa4, 0xc1, 0x52, 0x1b,
+                0xf1, 0x9d, 0x5f, 0xbe, 0xe2, 0xc0, 0x48, 0x38,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x94, 0x31, 0x95, 0x44, 0x6c, 0xc5, 0x73, 0xc8,
+                0x8d, 0x72, 0x06, 0xe7, 0xfd, 0x16, 0x70, 0x5d,
+            },
+        },
+        .confirm_req[1] = {
+            .value = {
+                0x80, 0xae, 0x74, 0xaa, 0x9a, 0xfc, 0x09, 0x97,
+                0x10, 0x01, 0x4e, 0xbb, 0x16, 0x36, 0x6b, 0xc7,
+            },
+        },
+        .confirm_rsp[1] = {
+            .value = {
+                0x5a, 0xb1, 0xe5, 0x81, 0x5a, 0x1b, 0xef, 0xf4,
+                0xa8, 0x3d, 0xaa, 0x3f, 0x02, 0x1f, 0x78, 0x55,
+            },
+        },
+        .random_req[1] = {
+            .value = {
+                0x04, 0x4a, 0xf4, 0xd5, 0x4b, 0x4f, 0x77, 0x37,
+                0x2a, 0x3c, 0xfe, 0x83, 0x34, 0x6b, 0x38, 0x1a,
+            },
+        },
+        .random_rsp[1] = {
+            .value = {
+                0x24, 0xb3, 0x47, 0xc8, 0xb0, 0xa2, 0xa3, 0xd8,
+                0x78, 0x3d, 0x09, 0x8d, 0xea, 0x49, 0xf6, 0x22,
+            },
+        },
+        .confirm_req[2] = {
+            .value = {
+                0x56, 0x5f, 0x07, 0x30, 0x3a, 0xc1, 0x44, 0xf9,
+                0x00, 0x03, 0xb3, 0x93, 0x58, 0xb4, 0x2c, 0x85,
+            },
+        },
+        .confirm_rsp[2] = {
+            .value = {
+                0x50, 0x8a, 0xb3, 0x0b, 0xe4, 0x2e, 0xd3, 0x49,
+                0x59, 0x40, 0xb2, 0x71, 0xc9, 0x49, 0x29, 0x19,
+            },
+        },
+        .random_req[2] = {
+            .value = {
+                0x32, 0x37, 0x8e, 0x63, 0x6d, 0xbd, 0xd6, 0x18,
+                0xee, 0xa7, 0x0e, 0xe5, 0x7e, 0x5f, 0xe1, 0x80,
+            },
+        },
+        .random_rsp[2] = {
+            .value = {
+                0xa2, 0x1a, 0x92, 0xcd, 0xc0, 0x8f, 0x92, 0xb0,
+                0xe6, 0xbe, 0x43, 0x55, 0xc8, 0x47, 0x56, 0x4b,
+            },
+        },
+        .confirm_req[3] = {
+            .value = {
+                0x1b, 0xa0, 0x82, 0xda, 0xfc, 0xaf, 0x3f, 0x9c,
+                0xdf, 0xff, 0xa2, 0x18, 0xba, 0xbd, 0x9b, 0x48,
+            },
+        },
+        .confirm_rsp[3] = {
+            .value = {
+                0x6a, 0x90, 0xb7, 0x1c, 0x93, 0x4e, 0x4a, 0x8b,
+                0xda, 0xe8, 0x13, 0x6e, 0x01, 0x91, 0x74, 0xb1,
+            },
+        },
+        .random_req[3] = {
+            .value = {
+                0x41, 0xbf, 0x60, 0x64, 0x1d, 0xfc, 0xe2, 0xee,
+                0x00, 0xa3, 0x2a, 0xb1, 0xf8, 0x34, 0x6b, 0xeb,
+            },
+        },
+        .random_rsp[3] = {
+            .value = {
+                0xeb, 0x9c, 0xaf, 0x20, 0x14, 0x0f, 0xf2, 0x3e,
+                0xee, 0x45, 0xca, 0xe8, 0xdc, 0x17, 0xab, 0x22,
+            },
+        },
+        .confirm_req[4] = {
+            .value = {
+                0x75, 0x8f, 0x97, 0xbb, 0x87, 0xa8, 0x70, 0xda,
+                0x94, 0x5a, 0xd6, 0x09, 0x78, 0xe3, 0xdd, 0x43,
+            },
+        },
+        .confirm_rsp[4] = {
+            .value = {
+                0x8c, 0x2d, 0xa7, 0x44, 0xd9, 0x15, 0xa8, 0x9e,
+                0xdf, 0x3a, 0x59, 0xa5, 0xee, 0x92, 0x24, 0x3c,
+            },
+        },
+        .random_req[4] = {
+            .value = {
+                0xb9, 0xe0, 0xf3, 0xf6, 0x6f, 0xbd, 0xa0, 0x7a,
+                0x82, 0x20, 0x61, 0xbe, 0xf3, 0xe6, 0x4e, 0xac,
+            },
+        },
+        .random_rsp[4] = {
+            .value = {
+                0xdd, 0x9b, 0xd3, 0x10, 0xed, 0x12, 0xe8, 0xb5,
+                0xa2, 0x59, 0xe1, 0xdc, 0x5c, 0xd8, 0x6e, 0x96,
+            },
+        },
+        .confirm_req[5] = {
+            .value = {
+                0x9d, 0xc7, 0x97, 0x67, 0x8d, 0xd0, 0xd6, 0x1a,
+                0x4d, 0x52, 0xc0, 0x8d, 0x87, 0xa9, 0x75, 0xf5,
+            },
+        },
+        .confirm_rsp[5] = {
+            .value = {
+                0xd4, 0x5d, 0x61, 0x76, 0x38, 0xe3, 0x81, 0x85,
+                0x18, 0x5f, 0xac, 0xde, 0x49, 0x57, 0xf6, 0x9b,
+            },
+        },
+        .random_req[5] = {
+            .value = {
+                0xfe, 0x83, 0xe9, 0xc6, 0xe9, 0xa4, 0x83, 0x0d,
+                0xaf, 0x27, 0x6f, 0x79, 0x7a, 0x2b, 0x2d, 0x1f,
+            },
+        },
+        .random_rsp[5] = {
+            .value = {
+                0xf2, 0x0c, 0x9e, 0x75, 0x5b, 0xb1, 0x8c, 0xf1,
+                0x46, 0x4f, 0x68, 0xe8, 0x0a, 0x65, 0xd5, 0x81,
+            },
+        },
+        .confirm_req[6] = {
+            .value = {
+                0x15, 0x2b, 0x2e, 0x14, 0xf7, 0x31, 0xa2, 0xff,
+                0x93, 0xa7, 0x28, 0x65, 0xb1, 0x68, 0x96, 0xc6,
+            },
+        },
+        .confirm_rsp[6] = {
+            .value = {
+                0x6f, 0x01, 0x22, 0x14, 0x78, 0xfb, 0x93, 0xf4,
+                0xfa, 0xf1, 0x6d, 0x33, 0x49, 0x0e, 0x7d, 0x56,
+            },
+        },
+        .random_req[6] = {
+            .value = {
+                0x05, 0xe5, 0xed, 0x99, 0x63, 0x05, 0x29, 0xb1,
+                0xbd, 0xf7, 0x2b, 0xa6, 0x94, 0xfe, 0x45, 0xb2,
+            },
+        },
+        .random_rsp[6] = {
+            .value = {
+                0x51, 0xf1, 0x2a, 0xa6, 0x7b, 0xe0, 0xb3, 0x20,
+                0x7d, 0x7e, 0xd3, 0x47, 0xfb, 0x83, 0xe1, 0xc6,
+            },
+        },
+        .confirm_req[7] = {
+            .value = {
+                0x9e, 0x7a, 0x3d, 0x12, 0x3b, 0x30, 0x81, 0x23,
+                0x1c, 0x94, 0x42, 0x73, 0x41, 0x68, 0xc6, 0x17,
+            },
+        },
+        .confirm_rsp[7] = {
+            .value = {
+                0x55, 0x31, 0x41, 0xe8, 0x1f, 0x11, 0xa6, 0x06,
+                0x7a, 0x7c, 0x84, 0x10, 0xad, 0xd3, 0x73, 0xcf,
+            },
+        },
+        .random_req[7] = {
+            .value = {
+                0xcb, 0x92, 0x18, 0xf6, 0x59, 0x6a, 0x1b, 0x18,
+                0x63, 0x72, 0x54, 0xc2, 0x1a, 0x3d, 0x09, 0x67,
+            },
+        },
+        .random_rsp[7] = {
+            .value = {
+                0xae, 0xf2, 0x96, 0xfd, 0xff, 0xd7, 0x18, 0xac,
+                0x5d, 0xb2, 0x9d, 0x89, 0x56, 0x2a, 0x19, 0xae,
+            },
+        },
+        .confirm_req[8] = {
+            .value = {
+                0x06, 0x8d, 0x5d, 0x19, 0xb3, 0x27, 0xc9, 0x6a,
+                0xe8, 0x58, 0xe7, 0x17, 0x10, 0x6a, 0xf9, 0xf7,
+            },
+        },
+        .confirm_rsp[8] = {
+            .value = {
+                0xf0, 0xbc, 0x2a, 0x03, 0x1f, 0x9b, 0x7b, 0x58,
+                0x43, 0x0f, 0xf5, 0x17, 0xc4, 0xbd, 0xec, 0x23,
+            },
+        },
+        .random_req[8] = {
+            .value = {
+                0xbe, 0x78, 0xcd, 0x84, 0x91, 0x4a, 0x1b, 0xdd,
+                0x6a, 0x0d, 0x88, 0x72, 0x9e, 0xc2, 0x4f, 0x5a,
+            },
+        },
+        .random_rsp[8] = {
+            .value = {
+                0xff, 0xac, 0xfe, 0x71, 0x2f, 0x6a, 0x13, 0xdc,
+                0xd3, 0x02, 0x81, 0x88, 0xbf, 0xc9, 0x9c, 0xd6,
+            },
+        },
+        .confirm_req[9] = {
+            .value = {
+                0xb0, 0x8d, 0x47, 0x23, 0x7e, 0xdb, 0xf5, 0x64,
+                0x5e, 0x83, 0x52, 0x9f, 0x06, 0x65, 0x84, 0x10,
+            },
+        },
+        .confirm_rsp[9] = {
+            .value = {
+                0x4d, 0x3f, 0xd4, 0x5a, 0x45, 0x57, 0xe9, 0xd7,
+                0x1e, 0x65, 0x7a, 0xa0, 0xd8, 0x5a, 0xa8, 0x29,
+            },
+        },
+        .random_req[9] = {
+            .value = {
+                0xb0, 0xcd, 0xfa, 0x39, 0x0d, 0x2e, 0x07, 0xfe,
+                0x36, 0x47, 0x8d, 0x8e, 0x1a, 0x47, 0x67, 0xf2,
+            },
+        },
+        .random_rsp[9] = {
+            .value = {
+                0xb4, 0xf5, 0x12, 0x64, 0xf4, 0xf6, 0xd7, 0x6e,
+                0xeb, 0x1e, 0x9a, 0x3f, 0x18, 0xba, 0xfb, 0x99,
+            },
+        },
+        .confirm_req[10] = {
+            .value = {
+                0xc9, 0x76, 0xb3, 0x3f, 0x80, 0xd9, 0x0c, 0xfb,
+                0xe3, 0x90, 0x1b, 0x7a, 0xbc, 0xe1, 0x7c, 0xde,
+            },
+        },
+        .confirm_rsp[10] = {
+            .value = {
+                0x21, 0x6a, 0x45, 0x6e, 0x6a, 0xac, 0xba, 0x9e,
+                0x66, 0x39, 0x5b, 0xb6, 0x74, 0xfe, 0x2b, 0x28,
+            },
+        },
+        .random_req[10] = {
+            .value = {
+                0xc0, 0xd4, 0xdf, 0x7b, 0x0f, 0x2f, 0xaa, 0x68,
+                0x4e, 0x3d, 0xa4, 0x59, 0x6f, 0x24, 0xe6, 0x7e,
+            },
+        },
+        .random_rsp[10] = {
+            .value = {
+                0xdf, 0x89, 0x49, 0xe7, 0x9f, 0x60, 0xdd, 0xf6,
+                0x44, 0x97, 0xe3, 0x15, 0x52, 0x65, 0x67, 0x3e,
+            },
+        },
+        .confirm_req[11] = {
+            .value = {
+                0xb0, 0x3f, 0x34, 0xce, 0x7d, 0x2e, 0xf1, 0xab,
+                0x23, 0xd5, 0x89, 0xf5, 0xaa, 0xa8, 0x59, 0x9f,
+            },
+        },
+        .confirm_rsp[11] = {
+            .value = {
+                0xb1, 0x33, 0x6a, 0x64, 0xd8, 0xeb, 0x8b, 0xa0,
+                0xf4, 0x1a, 0x15, 0x28, 0xb9, 0xe4, 0xa1, 0x31,
+            },
+        },
+        .random_req[11] = {
+            .value = {
+                0xd2, 0x88, 0x24, 0xfe, 0x95, 0x11, 0xc5, 0x0a,
+                0x21, 0xfb, 0x96, 0xea, 0x61, 0xb9, 0x8b, 0x26,
+            },
+        },
+        .random_rsp[11] = {
+            .value = {
+                0x8f, 0x22, 0x66, 0x8e, 0x7e, 0x62, 0x34, 0x37,
+                0xfc, 0x4a, 0x48, 0x1f, 0xf7, 0x38, 0x3b, 0x4e,
+            },
+        },
+        .confirm_req[12] = {
+            .value = {
+                0xc4, 0x50, 0xc8, 0x53, 0x58, 0xfb, 0xea, 0x9a,
+                0xdc, 0x35, 0xc7, 0xf3, 0x5b, 0x7c, 0xfb, 0xe4,
+            },
+        },
+        .confirm_rsp[12] = {
+            .value = {
+                0x27, 0xd9, 0x32, 0xd6, 0x43, 0xbf, 0x57, 0x3f,
+                0x35, 0x73, 0x3c, 0x3e, 0xbe, 0x53, 0x19, 0xff,
+            },
+        },
+        .random_req[12] = {
+            .value = {
+                0x99, 0xa1, 0x7a, 0x5f, 0xe0, 0x48, 0x1c, 0x6c,
+                0x84, 0xac, 0xab, 0xed, 0x69, 0x55, 0x1e, 0x66,
+            },
+        },
+        .random_rsp[12] = {
+            .value = {
+                0x37, 0x50, 0x90, 0x35, 0xef, 0x84, 0x06, 0x18,
+                0xfd, 0x3b, 0xc1, 0x8a, 0x46, 0x91, 0xb8, 0x21,
+            },
+        },
+        .confirm_req[13] = {
+            .value = {
+                0x2f, 0xcb, 0x3e, 0xc3, 0xce, 0x82, 0x0b, 0x5c,
+                0xdc, 0x9c, 0xbd, 0x44, 0xf9, 0x04, 0x22, 0x8c,
+            },
+        },
+        .confirm_rsp[13] = {
+            .value = {
+                0xab, 0xf2, 0x2e, 0x40, 0xd0, 0x74, 0x4f, 0xd4,
+                0x26, 0x9c, 0x89, 0x9e, 0x38, 0x77, 0xac, 0x9d,
+            },
+        },
+        .random_req[13] = {
+            .value = {
+                0xbc, 0xda, 0x58, 0xa2, 0x98, 0x88, 0xfe, 0x9f,
+                0x95, 0x0e, 0x3a, 0x91, 0xba, 0xe9, 0xbf, 0x02,
+            },
+        },
+        .random_rsp[13] = {
+            .value = {
+                0x04, 0xb9, 0x4c, 0x26, 0xce, 0x87, 0x8f, 0x17,
+                0xdc, 0xbc, 0x36, 0x94, 0x47, 0x67, 0x9f, 0xde,
+            },
+        },
+        .confirm_req[14] = {
+            .value = {
+                0xbd, 0xb6, 0x54, 0xc8, 0x1f, 0x51, 0x23, 0x98,
+                0x48, 0x3d, 0x47, 0x9d, 0xa3, 0xb8, 0xe7, 0x55,
+            },
+        },
+        .confirm_rsp[14] = {
+            .value = {
+                0x06, 0xc2, 0x7b, 0x80, 0x76, 0x9c, 0x37, 0x78,
+                0x46, 0xc5, 0x45, 0x43, 0x5d, 0x8d, 0x5b, 0x3e,
+            },
+        },
+        .random_req[14] = {
+            .value = {
+                0xef, 0x9e, 0x8a, 0x3a, 0xb7, 0xde, 0xa8, 0x07,
+                0x58, 0x73, 0xe0, 0x07, 0xfc, 0x62, 0xdb, 0x62,
+            },
+        },
+        .random_rsp[14] = {
+            .value = {
+                0xfa, 0xd5, 0xb2, 0x4e, 0x20, 0x01, 0x93, 0xc0,
+                0xb3, 0x76, 0xa5, 0x7a, 0x92, 0x8f, 0xb9, 0x6d,
+            },
+        },
+        .confirm_req[15] = {
+            .value = {
+                0x76, 0x2e, 0xc6, 0x64, 0x6c, 0x13, 0x01, 0x7e,
+                0x34, 0x78, 0x12, 0xb8, 0x1a, 0xb7, 0xf7, 0x39,
+            },
+        },
+        .confirm_rsp[15] = {
+            .value = {
+                0xbd, 0xae, 0x10, 0x32, 0xdb, 0x63, 0x30, 0x6f,
+                0x68, 0x19, 0x49, 0x5e, 0x34, 0x4f, 0x13, 0xc6,
+            },
+        },
+        .random_req[15] = {
+            .value = {
+                0x95, 0x2e, 0xe4, 0xe3, 0xb2, 0xdc, 0x79, 0xad,
+                0x5f, 0x0c, 0x19, 0x9c, 0x47, 0x9c, 0x79, 0x17,
+            },
+        },
+        .random_rsp[15] = {
+            .value = {
+                0x9e, 0x3d, 0x7f, 0xcd, 0x18, 0x40, 0xd7, 0xac,
+                0xa1, 0x45, 0x5f, 0xcb, 0x29, 0x57, 0x2b, 0x63,
+            },
+        },
+        .confirm_req[16] = {
+            .value = {
+                0x10, 0x18, 0x9d, 0xf2, 0xed, 0x76, 0x5c, 0x5f,
+                0x32, 0xa6, 0x29, 0x61, 0x12, 0xb2, 0xb8, 0xa2,
+            },
+        },
+        .confirm_rsp[16] = {
+            .value = {
+                0x3c, 0xd4, 0xbd, 0xe9, 0xd3, 0x29, 0xac, 0xf7,
+                0xfc, 0x04, 0xd3, 0xe4, 0x46, 0x14, 0x28, 0x2c,
+            },
+        },
+        .random_req[16] = {
+            .value = {
+                0x6d, 0xe8, 0x77, 0xc3, 0xab, 0x49, 0x6b, 0x79,
+                0x4f, 0x0f, 0x4c, 0x65, 0xc5, 0x77, 0x68, 0xd9,
+            },
+        },
+        .random_rsp[16] = {
+            .value = {
+                0xd0, 0x59, 0xf3, 0x53, 0xb1, 0x14, 0x81, 0x88,
+                0x26, 0x88, 0xef, 0x4b, 0xa4, 0x7d, 0x0a, 0x84,
+            },
+        },
+        .confirm_req[17] = {
+            .value = {
+                0xa3, 0x96, 0x9f, 0x96, 0x53, 0x0e, 0x38, 0x78,
+                0x9e, 0xbd, 0xf7, 0x65, 0x23, 0x73, 0x99, 0xa7,
+            },
+        },
+        .confirm_rsp[17] = {
+            .value = {
+                0x6b, 0x25, 0x8d, 0x51, 0xd8, 0xc4, 0xd9, 0xbf,
+                0xa6, 0x4f, 0xa3, 0x25, 0x28, 0xb5, 0x7c, 0x05,
+            },
+        },
+        .random_req[17] = {
+            .value = {
+                0xa5, 0xac, 0xd9, 0xb6, 0x9e, 0x98, 0x75, 0xae,
+                0x9b, 0x16, 0xe1, 0x60, 0xc6, 0xa5, 0x07, 0xf2,
+            },
+        },
+        .random_rsp[17] = {
+            .value = {
+                0x65, 0x53, 0x56, 0xe6, 0x2c, 0x22, 0x68, 0xc9,
+                0xb8, 0xbe, 0xb1, 0x40, 0x08, 0xe2, 0xb6, 0xb9,
+            },
+        },
+        .confirm_req[18] = {
+            .value = {
+                0x67, 0xcd, 0x0e, 0x4f, 0xfc, 0x38, 0x7f, 0x8a,
+                0x3b, 0xea, 0xff, 0x86, 0xf3, 0x8a, 0x92, 0xcb,
+            },
+        },
+        .confirm_rsp[18] = {
+            .value = {
+                0x22, 0x95, 0x1f, 0x20, 0xc9, 0x5c, 0x73, 0x39,
+                0xa4, 0xd9, 0xc1, 0x37, 0x9d, 0x94, 0xb2, 0xfd,
+            },
+        },
+        .random_req[18] = {
+            .value = {
+                0xe1, 0x80, 0x82, 0xdd, 0x21, 0x6c, 0xe4, 0x93,
+                0xa3, 0x41, 0x0f, 0xfc, 0x96, 0x42, 0x8b, 0xde,
+            },
+        },
+        .random_rsp[18] = {
+            .value = {
+                0x11, 0x1c, 0xd7, 0x7a, 0xe7, 0x1a, 0x88, 0xdd,
+                0x2a, 0xdf, 0xe5, 0x30, 0xca, 0x0b, 0x9f, 0xb6,
+            },
+        },
+        .confirm_req[19] = {
+            .value = {
+                0x45, 0x9b, 0x36, 0x3d, 0xf8, 0xc0, 0x43, 0x6d,
+                0x94, 0xcf, 0xbd, 0x5f, 0xfe, 0xec, 0xd7, 0x4b,
+            },
+        },
+        .confirm_rsp[19] = {
+            .value = {
+                0xf0, 0xaa, 0xfd, 0xae, 0xb7, 0x73, 0x3c, 0x9d,
+                0x93, 0xd4, 0x00, 0xea, 0x81, 0x31, 0xde, 0x41,
+            },
+        },
+        .random_req[19] = {
+            .value = {
+                0x1a, 0xaa, 0xff, 0x2a, 0xdc, 0xcc, 0x89, 0xbc,
+                0xcf, 0x48, 0x5c, 0x1e, 0x4d, 0x69, 0x85, 0x39,
+            },
+        },
+        .random_rsp[19] = {
+            .value = {
+                0xe7, 0xd0, 0xcb, 0x9a, 0xb5, 0x76, 0xec, 0xfc,
+                0x48, 0xa3, 0x41, 0x48, 0x4c, 0xa7, 0xec, 0xb7,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xe3, 0x4e, 0x42, 0xb5, 0xe3, 0x63, 0x4b, 0x7c,
+                0xf0, 0x9f, 0xef, 0x6e, 0x97, 0xe2, 0x86, 0xc0,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0xea, 0x8a, 0xab, 0x7f, 0x15, 0x21, 0x5a, 0x36,
+                0x9b, 0x56, 0xee, 0x51, 0x61, 0x97, 0xe2, 0x0a,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x85, 0x54, 0x52, 0xe3, 0xb4, 0xe8, 0x26, 0xa4,
+                0x38, 0xb0, 0x4c, 0xa0, 0x41, 0xf5, 0x30, 0x6e,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x6f, 0x93, 0xb8, 0x9c, 0x26, 0x88, 0xb4, 0x20,
+                0x87, 0x95, 0xf2, 0xf4, 0x3a, 0xbe, 0x92, 0xb7,
+            },
+        },
+        .ltk = {
+            0x30, 0xf6, 0xd3, 0x2e, 0x1c, 0x81, 0x2c, 0x96,
+            0x56, 0x30, 0x55, 0xec, 0x9b, 0x72, 0xf4, 0x83,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_PASSKEY,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_INPUT,
+                .passkey = 879894,
+            },
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 2
+ * Responder IO capabilities: 0
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 5
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_pk_iio2_rio0_b1_iat0_rat0_ik5_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x02,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x0d,
+            .resp_key_dist = 0x0f,
+        },
+        .pair_rsp = {
+            .io_cap = 0x00,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x05,
+            .resp_key_dist = 0x07,
+        },
+        .our_priv_key = {
+            0xd6, 0x2f, 0x4f, 0x6b, 0xeb, 0xfc, 0xbd, 0xee,
+            0x9b, 0x94, 0xd7, 0x15, 0x98, 0xc6, 0x0c, 0x83,
+            0x9b, 0xc7, 0xa2, 0x45, 0xfd, 0x00, 0xe8, 0xa4,
+            0x52, 0xe9, 0x70, 0x2f, 0xd7, 0x62, 0xf1, 0xa4,
+        },
+        .public_key_req = {
+            .x = {
+                0xd6, 0xa7, 0xaf, 0xc1, 0x18, 0x8b, 0x92, 0x2f,
+                0xbc, 0xbc, 0x4d, 0xb8, 0x5c, 0xfb, 0x39, 0x7c,
+                0x1e, 0x90, 0x7e, 0xfa, 0xa2, 0x0d, 0xee, 0x9e,
+                0xb4, 0x9e, 0xbe, 0x50, 0xf0, 0xbc, 0x2c, 0x10,
+            },
+            .y = {
+                0xa4, 0x25, 0xad, 0x75, 0xbe, 0xab, 0x1e, 0xcf,
+                0x4e, 0xc8, 0x19, 0xab, 0x6c, 0x68, 0x38, 0xa4,
+                0xe7, 0x43, 0x7b, 0x19, 0xef, 0x28, 0xd5, 0x93,
+                0x52, 0xe9, 0xb9, 0x31, 0x68, 0x60, 0x19, 0x71,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0xbc, 0x6a, 0xcf, 0xc6, 0x8a, 0x3a, 0xdc, 0x89,
+                0xdd, 0xa9, 0xaf, 0x29, 0xc7, 0xaf, 0xe2, 0x8b,
+                0x25, 0xee, 0xce, 0xa6, 0x10, 0x1d, 0x33, 0x2f,
+                0xd5, 0xfc, 0x30, 0xb8, 0xb1, 0x7b, 0xb1, 0x6e,
+            },
+            .y = {
+                0x1a, 0xc6, 0x42, 0x36, 0x98, 0x40, 0x4f, 0x90,
+                0x82, 0xa0, 0x10, 0x3a, 0xa5, 0x0f, 0xcf, 0x57,
+                0xd2, 0x2e, 0x80, 0x9d, 0x61, 0xc7, 0x21, 0xac,
+                0x47, 0x5b, 0x93, 0x75, 0x02, 0x30, 0x40, 0x14,
+            },
+        },
+        .confirm_req[0] = {
+            .value = {
+                0xd1, 0x64, 0x49, 0xa0, 0xc4, 0x28, 0x81, 0x57,
+                0x0c, 0x25, 0x62, 0xfb, 0x2c, 0xa2, 0xb0, 0xc7,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0xea, 0xae, 0x4e, 0x03, 0x00, 0xf9, 0xd1, 0x65,
+                0xc7, 0x6a, 0x0d, 0x74, 0x4f, 0x02, 0x0b, 0x94,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x05, 0xb2, 0x09, 0x9b, 0x36, 0x23, 0x4f, 0x74,
+                0x4e, 0xc9, 0x7a, 0x2c, 0x65, 0x3a, 0xd1, 0xf6,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x50, 0xd8, 0x88, 0xd4, 0x7e, 0xc1, 0x36, 0x92,
+                0x0f, 0xa7, 0x17, 0x3c, 0xb4, 0xeb, 0xee, 0xa6,
+            },
+        },
+        .confirm_req[1] = {
+            .value = {
+                0xab, 0xa2, 0xd0, 0xec, 0xdd, 0xf3, 0xd2, 0xa9,
+                0x2d, 0xde, 0x4b, 0x02, 0x66, 0x45, 0x2f, 0xc0,
+            },
+        },
+        .confirm_rsp[1] = {
+            .value = {
+                0xa9, 0xc1, 0x9d, 0x75, 0xd0, 0xb6, 0xec, 0x06,
+                0x31, 0x87, 0xb6, 0x9d, 0x31, 0xdc, 0x92, 0x7c,
+            },
+        },
+        .random_req[1] = {
+            .value = {
+                0xb9, 0x5b, 0xe0, 0x0f, 0x83, 0xe7, 0x2d, 0x77,
+                0x2f, 0x55, 0x0a, 0x2c, 0xd9, 0xc1, 0x46, 0xcd,
+            },
+        },
+        .random_rsp[1] = {
+            .value = {
+                0xa2, 0x9a, 0x5b, 0x99, 0xb1, 0xc0, 0xc5, 0xd6,
+                0xf1, 0x87, 0x0b, 0x49, 0x9c, 0xfd, 0xfe, 0xd5,
+            },
+        },
+        .confirm_req[2] = {
+            .value = {
+                0x3a, 0x9d, 0x58, 0xe5, 0xb0, 0x31, 0xd9, 0xde,
+                0xac, 0xd2, 0x44, 0xb7, 0xe1, 0xe5, 0x89, 0x50,
+            },
+        },
+        .confirm_rsp[2] = {
+            .value = {
+                0xae, 0x4e, 0x4f, 0x84, 0x5f, 0x4c, 0xd1, 0x9b,
+                0x81, 0x22, 0x9c, 0x68, 0x52, 0xe0, 0x9a, 0xfc,
+            },
+        },
+        .random_req[2] = {
+            .value = {
+                0xa5, 0xbb, 0x5f, 0x9a, 0xa2, 0x97, 0xdb, 0xcd,
+                0x3d, 0xfe, 0xd9, 0x58, 0x21, 0x52, 0x99, 0xb7,
+            },
+        },
+        .random_rsp[2] = {
+            .value = {
+                0xea, 0x44, 0xdd, 0x0c, 0xbf, 0xb5, 0x6b, 0xc7,
+                0xe1, 0x19, 0xe8, 0x0b, 0xc2, 0x15, 0x04, 0x37,
+            },
+        },
+        .confirm_req[3] = {
+            .value = {
+                0xa8, 0xa3, 0xdb, 0x08, 0xca, 0x31, 0xd5, 0xef,
+                0x17, 0x37, 0x77, 0xd0, 0x64, 0x2e, 0x2f, 0x2f,
+            },
+        },
+        .confirm_rsp[3] = {
+            .value = {
+                0xe4, 0xf6, 0xa5, 0x94, 0x1a, 0x09, 0x4b, 0x75,
+                0x79, 0xb8, 0x0c, 0xe6, 0xe2, 0x28, 0x5a, 0x2c,
+            },
+        },
+        .random_req[3] = {
+            .value = {
+                0x1a, 0x3f, 0x80, 0x6f, 0xd3, 0xe8, 0xc5, 0xfb,
+                0x9b, 0xda, 0xa1, 0x07, 0x68, 0x1a, 0x54, 0xbc,
+            },
+        },
+        .random_rsp[3] = {
+            .value = {
+                0x1b, 0x48, 0x22, 0x87, 0x04, 0x24, 0x87, 0xba,
+                0x14, 0xb9, 0x85, 0xb2, 0xa6, 0xf5, 0xea, 0x89,
+            },
+        },
+        .confirm_req[4] = {
+            .value = {
+                0x31, 0xcb, 0xc4, 0x0c, 0x36, 0xb5, 0xe2, 0x32,
+                0xd8, 0x0e, 0xd3, 0x86, 0x96, 0xe3, 0x8c, 0x84,
+            },
+        },
+        .confirm_rsp[4] = {
+            .value = {
+                0x90, 0x11, 0x30, 0x35, 0x5f, 0xe5, 0x45, 0xff,
+                0xab, 0xd3, 0xe0, 0xbe, 0x1c, 0x20, 0x23, 0xb8,
+            },
+        },
+        .random_req[4] = {
+            .value = {
+                0xa0, 0xc7, 0x79, 0x28, 0x87, 0x19, 0xa3, 0x78,
+                0x33, 0xe5, 0x1a, 0x81, 0xba, 0x9b, 0xe3, 0x5c,
+            },
+        },
+        .random_rsp[4] = {
+            .value = {
+                0x43, 0xcf, 0x20, 0x1b, 0x39, 0x3f, 0xdf, 0x73,
+                0x58, 0xd2, 0x0d, 0xc7, 0x41, 0xd7, 0x58, 0xea,
+            },
+        },
+        .confirm_req[5] = {
+            .value = {
+                0x59, 0xda, 0x78, 0xeb, 0xd5, 0xcd, 0x8e, 0x23,
+                0xe5, 0x5e, 0xa7, 0xa5, 0xba, 0x13, 0x00, 0xff,
+            },
+        },
+        .confirm_rsp[5] = {
+            .value = {
+                0x31, 0x7a, 0xf0, 0x56, 0x82, 0x69, 0xdb, 0xcd,
+                0x27, 0x5a, 0x11, 0xd3, 0x65, 0x82, 0x0d, 0xda,
+            },
+        },
+        .random_req[5] = {
+            .value = {
+                0x2e, 0xe8, 0x76, 0x40, 0x9c, 0x49, 0x07, 0x42,
+                0x1e, 0x45, 0x7b, 0x1e, 0x73, 0xa3, 0x71, 0x05,
+            },
+        },
+        .random_rsp[5] = {
+            .value = {
+                0x64, 0x99, 0x42, 0x5d, 0x05, 0xd6, 0x12, 0x41,
+                0x2a, 0x44, 0x55, 0x26, 0xe7, 0x08, 0x5e, 0xfb,
+            },
+        },
+        .confirm_req[6] = {
+            .value = {
+                0x1c, 0x55, 0xe1, 0x75, 0x4f, 0x6e, 0xdd, 0x7e,
+                0xc8, 0xff, 0x76, 0x25, 0xdb, 0x2a, 0x6d, 0xe3,
+            },
+        },
+        .confirm_rsp[6] = {
+            .value = {
+                0xf6, 0x36, 0x78, 0x88, 0x62, 0xa8, 0x78, 0xe6,
+                0xf9, 0xa1, 0x17, 0x63, 0x86, 0xd3, 0xae, 0x60,
+            },
+        },
+        .random_req[6] = {
+            .value = {
+                0x96, 0x9a, 0x1c, 0xbe, 0x82, 0x82, 0xc2, 0xa7,
+                0x18, 0xc3, 0x7b, 0x40, 0x5d, 0x6c, 0x4e, 0xe3,
+            },
+        },
+        .random_rsp[6] = {
+            .value = {
+                0x2b, 0x7d, 0x36, 0xc3, 0xf7, 0x59, 0x63, 0x40,
+                0x6f, 0xc0, 0x2a, 0x2b, 0x1b, 0xd7, 0x41, 0x38,
+            },
+        },
+        .confirm_req[7] = {
+            .value = {
+                0x88, 0x99, 0x53, 0xae, 0x2a, 0xaf, 0x97, 0x5a,
+                0xcc, 0x9f, 0xfd, 0xe2, 0x1d, 0xd3, 0x27, 0x66,
+            },
+        },
+        .confirm_rsp[7] = {
+            .value = {
+                0xdb, 0xae, 0xfb, 0xf7, 0x33, 0xd4, 0xd1, 0xcb,
+                0xfe, 0x75, 0x8e, 0x81, 0x16, 0xd1, 0x49, 0xeb,
+            },
+        },
+        .random_req[7] = {
+            .value = {
+                0x13, 0x5c, 0x00, 0x34, 0xe5, 0x96, 0xd0, 0x97,
+                0xb1, 0x84, 0x3d, 0x00, 0xb4, 0x2a, 0x4a, 0x12,
+            },
+        },
+        .random_rsp[7] = {
+            .value = {
+                0xed, 0x94, 0x1f, 0x41, 0x12, 0xe5, 0x35, 0x5b,
+                0xa6, 0x6a, 0x72, 0x1e, 0xa2, 0x7c, 0xe1, 0x6c,
+            },
+        },
+        .confirm_req[8] = {
+            .value = {
+                0xa3, 0xc7, 0x17, 0xad, 0xb6, 0xe6, 0xaa, 0x16,
+                0x8d, 0x4b, 0x70, 0x5f, 0x49, 0x73, 0xa7, 0x19,
+            },
+        },
+        .confirm_rsp[8] = {
+            .value = {
+                0x10, 0xb0, 0x31, 0xa7, 0x16, 0x61, 0xf7, 0xd6,
+                0xe6, 0x16, 0x9e, 0xb1, 0x9e, 0xb5, 0x5e, 0x94,
+            },
+        },
+        .random_req[8] = {
+            .value = {
+                0x6f, 0xe7, 0x62, 0x73, 0xfb, 0xbf, 0xf1, 0x4a,
+                0x14, 0xa1, 0x09, 0x45, 0xd4, 0xde, 0x26, 0xad,
+            },
+        },
+        .random_rsp[8] = {
+            .value = {
+                0x3f, 0x48, 0xa7, 0xdf, 0x4a, 0xd5, 0x55, 0x26,
+                0xd3, 0x32, 0xbf, 0x98, 0x4a, 0x20, 0xad, 0xb0,
+            },
+        },
+        .confirm_req[9] = {
+            .value = {
+                0x88, 0x1c, 0xef, 0xfe, 0x4e, 0x68, 0x41, 0x7c,
+                0xe8, 0xe8, 0x81, 0x1a, 0xb9, 0x9e, 0xaf, 0xc6,
+            },
+        },
+        .confirm_rsp[9] = {
+            .value = {
+                0xa3, 0x53, 0x2a, 0xe1, 0xbd, 0x9d, 0xbe, 0x89,
+                0xf8, 0xc7, 0x70, 0x6e, 0xa9, 0x12, 0x07, 0x0d,
+            },
+        },
+        .random_req[9] = {
+            .value = {
+                0x52, 0x06, 0x56, 0x09, 0xf4, 0xb2, 0xb9, 0x63,
+                0x3f, 0x2e, 0x59, 0x6c, 0x6b, 0x43, 0xb6, 0xc0,
+            },
+        },
+        .random_rsp[9] = {
+            .value = {
+                0x36, 0xb0, 0x33, 0x84, 0x52, 0xd1, 0x60, 0xac,
+                0x37, 0x81, 0x6b, 0x18, 0x5f, 0xfc, 0x61, 0xb1,
+            },
+        },
+        .confirm_req[10] = {
+            .value = {
+                0xc8, 0x55, 0xb7, 0x9e, 0x3e, 0xf0, 0x26, 0xa4,
+                0x55, 0xb3, 0x1d, 0x4d, 0xa1, 0x5d, 0xa9, 0xaf,
+            },
+        },
+        .confirm_rsp[10] = {
+            .value = {
+                0xb7, 0xb9, 0x6b, 0x8e, 0xef, 0xd3, 0xbc, 0x58,
+                0x10, 0xbe, 0x5a, 0x9a, 0x4d, 0xbc, 0xec, 0xe3,
+            },
+        },
+        .random_req[10] = {
+            .value = {
+                0x55, 0xa1, 0xf4, 0xd7, 0xfa, 0xe1, 0x84, 0x03,
+                0xed, 0xb6, 0x95, 0x63, 0x4b, 0x93, 0x93, 0xc2,
+            },
+        },
+        .random_rsp[10] = {
+            .value = {
+                0x72, 0xa9, 0xe5, 0xf7, 0x48, 0x1f, 0x64, 0x71,
+                0xd9, 0x81, 0xf0, 0xc5, 0x4d, 0x38, 0xac, 0x9a,
+            },
+        },
+        .confirm_req[11] = {
+            .value = {
+                0x12, 0x37, 0x56, 0xa6, 0x66, 0xa1, 0x23, 0xee,
+                0xe3, 0x1e, 0x20, 0x66, 0x66, 0x85, 0x7c, 0xa8,
+            },
+        },
+        .confirm_rsp[11] = {
+            .value = {
+                0x74, 0xaa, 0xbb, 0x5a, 0xdf, 0xd9, 0xc4, 0xaf,
+                0xe4, 0xa7, 0xe6, 0x4b, 0x45, 0x97, 0xf8, 0x7d,
+            },
+        },
+        .random_req[11] = {
+            .value = {
+                0x29, 0xce, 0xcc, 0xb7, 0xb2, 0x1e, 0x0e, 0xa8,
+                0x48, 0x90, 0x43, 0x6d, 0x34, 0xa4, 0xa3, 0x12,
+            },
+        },
+        .random_rsp[11] = {
+            .value = {
+                0x2e, 0xaf, 0x4c, 0x63, 0x84, 0x2c, 0x62, 0x67,
+                0x68, 0x8f, 0x0b, 0xfd, 0xff, 0xef, 0x15, 0x26,
+            },
+        },
+        .confirm_req[12] = {
+            .value = {
+                0xec, 0xcf, 0x6a, 0x60, 0x77, 0x04, 0x2c, 0x62,
+                0x42, 0xf0, 0x21, 0xfd, 0x53, 0xd6, 0x8a, 0xe8,
+            },
+        },
+        .confirm_rsp[12] = {
+            .value = {
+                0x2c, 0x13, 0x65, 0x69, 0xd7, 0x66, 0x04, 0x13,
+                0x3c, 0xa8, 0xfb, 0xe5, 0x76, 0xbb, 0x4f, 0x48,
+            },
+        },
+        .random_req[12] = {
+            .value = {
+                0x0d, 0x93, 0x30, 0xe2, 0x76, 0xf1, 0xbc, 0x24,
+                0x61, 0x0d, 0xcd, 0xef, 0x33, 0x98, 0xe2, 0x3b,
+            },
+        },
+        .random_rsp[12] = {
+            .value = {
+                0xb6, 0x32, 0x69, 0x81, 0xc0, 0x81, 0x46, 0xae,
+                0x8d, 0x5a, 0x17, 0xb5, 0xc0, 0x0f, 0x9f, 0x4e,
+            },
+        },
+        .confirm_req[13] = {
+            .value = {
+                0x89, 0x96, 0x22, 0x0c, 0x76, 0xdf, 0x27, 0x13,
+                0x96, 0x5a, 0x0c, 0x88, 0x65, 0x18, 0x74, 0x52,
+            },
+        },
+        .confirm_rsp[13] = {
+            .value = {
+                0x1c, 0x77, 0x25, 0x22, 0xc0, 0x28, 0x88, 0x45,
+                0x29, 0x62, 0x7a, 0x8e, 0xc0, 0x2a, 0x5c, 0xd8,
+            },
+        },
+        .random_req[13] = {
+            .value = {
+                0xcc, 0x84, 0xb6, 0x98, 0x3e, 0xf9, 0x09, 0xd2,
+                0x71, 0x47, 0x56, 0xb1, 0x09, 0xf5, 0xd2, 0x0b,
+            },
+        },
+        .random_rsp[13] = {
+            .value = {
+                0xf0, 0xcf, 0x1c, 0xa6, 0x24, 0xcd, 0xfa, 0x42,
+                0xa4, 0x93, 0x8b, 0xa0, 0xe3, 0x42, 0x72, 0x51,
+            },
+        },
+        .confirm_req[14] = {
+            .value = {
+                0xab, 0xb0, 0xa3, 0x80, 0x0d, 0xcb, 0x8e, 0xf6,
+                0x6c, 0x07, 0x50, 0xe9, 0x8a, 0x85, 0x02, 0xae,
+            },
+        },
+        .confirm_rsp[14] = {
+            .value = {
+                0xf6, 0x52, 0xd8, 0x34, 0x15, 0x62, 0x9f, 0x6e,
+                0x2b, 0x52, 0xdc, 0x1c, 0x70, 0x17, 0x0a, 0x31,
+            },
+        },
+        .random_req[14] = {
+            .value = {
+                0x8d, 0xc9, 0x0a, 0x45, 0xe9, 0x81, 0x0d, 0x5e,
+                0xbb, 0xd8, 0x94, 0x29, 0x68, 0x42, 0x44, 0xe2,
+            },
+        },
+        .random_rsp[14] = {
+            .value = {
+                0x96, 0x2a, 0x35, 0x39, 0x09, 0xf7, 0x66, 0x5a,
+                0xb6, 0x33, 0x77, 0x6d, 0xba, 0xd3, 0x8a, 0xfb,
+            },
+        },
+        .confirm_req[15] = {
+            .value = {
+                0x53, 0x08, 0x9b, 0x37, 0xc3, 0x79, 0xe6, 0x8c,
+                0x42, 0x30, 0x94, 0x73, 0x6f, 0x39, 0x64, 0x20,
+            },
+        },
+        .confirm_rsp[15] = {
+            .value = {
+                0x4d, 0xb7, 0xe9, 0x50, 0x8e, 0x0f, 0xe0, 0xd5,
+                0x3e, 0xf6, 0x32, 0xdd, 0xb8, 0x18, 0x77, 0xd3,
+            },
+        },
+        .random_req[15] = {
+            .value = {
+                0x8d, 0x49, 0x14, 0xdd, 0x95, 0x57, 0x55, 0x14,
+                0x48, 0x97, 0xd3, 0x73, 0x29, 0xa0, 0xb9, 0x2b,
+            },
+        },
+        .random_rsp[15] = {
+            .value = {
+                0xcf, 0x38, 0x8b, 0xab, 0xe4, 0x2b, 0x3f, 0x13,
+                0xc3, 0xfb, 0x07, 0xee, 0x0e, 0x33, 0x2f, 0x04,
+            },
+        },
+        .confirm_req[16] = {
+            .value = {
+                0xc6, 0x58, 0x13, 0x19, 0x56, 0x06, 0x52, 0x4b,
+                0x3d, 0x5e, 0x9d, 0xa8, 0x48, 0xf2, 0x40, 0xf3,
+            },
+        },
+        .confirm_rsp[16] = {
+            .value = {
+                0xbb, 0x93, 0xd2, 0xed, 0x89, 0x66, 0xa5, 0x1c,
+                0xc9, 0x2a, 0x42, 0x2c, 0xff, 0x4a, 0x80, 0x84,
+            },
+        },
+        .random_req[16] = {
+            .value = {
+                0x3d, 0x9c, 0x11, 0x2a, 0xd3, 0xce, 0x4b, 0x20,
+                0xf2, 0xfb, 0xdd, 0x18, 0x4d, 0x7c, 0x58, 0xb6,
+            },
+        },
+        .random_rsp[16] = {
+            .value = {
+                0xda, 0x80, 0x63, 0x9d, 0xa2, 0x73, 0x61, 0xdd,
+                0x9a, 0x45, 0x91, 0x4d, 0x78, 0x39, 0x54, 0x75,
+            },
+        },
+        .confirm_req[17] = {
+            .value = {
+                0x2e, 0xe4, 0x44, 0xe8, 0xdb, 0xc2, 0xbd, 0x62,
+                0xd1, 0xc4, 0x23, 0x4e, 0x5f, 0x65, 0xb6, 0x3b,
+            },
+        },
+        .confirm_rsp[17] = {
+            .value = {
+                0x19, 0x91, 0xa3, 0xc7, 0x3b, 0x68, 0x12, 0x24,
+                0xcd, 0xd6, 0x02, 0xf5, 0xcd, 0x19, 0x6c, 0x88,
+            },
+        },
+        .random_req[17] = {
+            .value = {
+                0xf0, 0x28, 0x18, 0xe8, 0xa7, 0x3e, 0xd8, 0x21,
+                0x42, 0x58, 0xb3, 0x72, 0xa0, 0x34, 0x89, 0x04,
+            },
+        },
+        .random_rsp[17] = {
+            .value = {
+                0xe9, 0xff, 0x0b, 0x9a, 0xfd, 0x29, 0x95, 0x37,
+                0x89, 0x2c, 0x84, 0xfa, 0x02, 0xa0, 0xb6, 0xeb,
+            },
+        },
+        .confirm_req[18] = {
+            .value = {
+                0x4f, 0x90, 0x70, 0xbe, 0xc4, 0x81, 0x9f, 0xc1,
+                0x74, 0xa3, 0x01, 0x2e, 0x78, 0x7a, 0xe2, 0x61,
+            },
+        },
+        .confirm_rsp[18] = {
+            .value = {
+                0xbb, 0xd5, 0x91, 0xec, 0x81, 0xe0, 0x9b, 0x5e,
+                0xe9, 0xd2, 0x93, 0x57, 0xa8, 0x27, 0xdd, 0x9b,
+            },
+        },
+        .random_req[18] = {
+            .value = {
+                0x78, 0xa4, 0x35, 0x1a, 0xbc, 0xa7, 0x19, 0x8c,
+                0x96, 0x8f, 0x63, 0x9d, 0x11, 0xee, 0x27, 0x44,
+            },
+        },
+        .random_rsp[18] = {
+            .value = {
+                0x39, 0x5b, 0x71, 0xfd, 0x7e, 0x39, 0x6b, 0xbe,
+                0xaf, 0xe1, 0x55, 0x90, 0xa6, 0x58, 0xec, 0xc5,
+            },
+        },
+        .confirm_req[19] = {
+            .value = {
+                0x91, 0x43, 0xe5, 0xc8, 0x26, 0x0c, 0x8c, 0x6c,
+                0xf3, 0xd1, 0x30, 0xb3, 0x22, 0x94, 0x4c, 0x67,
+            },
+        },
+        .confirm_rsp[19] = {
+            .value = {
+                0x51, 0xc4, 0x3e, 0x09, 0xca, 0x03, 0xbe, 0x2c,
+                0xe8, 0x1a, 0x5d, 0x07, 0x12, 0x14, 0x2d, 0x43,
+            },
+        },
+        .random_req[19] = {
+            .value = {
+                0x2f, 0xa1, 0x20, 0xde, 0xf5, 0xb4, 0xa6, 0x92,
+                0x31, 0xe9, 0x86, 0x63, 0xef, 0xc1, 0x85, 0x3b,
+            },
+        },
+        .random_rsp[19] = {
+            .value = {
+                0x41, 0xd0, 0xd0, 0x96, 0x93, 0xd1, 0xcb, 0xed,
+                0xab, 0x27, 0xd5, 0x88, 0x5e, 0xe6, 0x5e, 0x5c,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xec, 0xc5, 0x5f, 0xf3, 0xae, 0xfe, 0x79, 0x65,
+                0x17, 0x5a, 0x60, 0xf7, 0x36, 0x4f, 0x90, 0x45,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0xa7, 0x45, 0x7a, 0x54, 0x1b, 0x64, 0x08, 0x60,
+                0x51, 0x7d, 0x74, 0x27, 0x48, 0xa2, 0xf1, 0x0f,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x36, 0x9a, 0xd9, 0x25, 0x5c, 0xdb, 0x78, 0xdc,
+                0x1d, 0x2c, 0x83, 0xf7, 0xde, 0x99, 0xa0, 0x66,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x61, 0x6e, 0x9a, 0x26, 0xc5, 0xd0, 0x85, 0xdc,
+                0xea, 0x9d, 0xca, 0x3b, 0x17, 0xd7, 0x43, 0x80,
+            },
+        },
+        .ltk = {
+            0xd6, 0x02, 0xba, 0x3d, 0xa2, 0xce, 0x93, 0x1a,
+            0xfd, 0xd6, 0xb5, 0x54, 0x90, 0xc4, 0x2a, 0x8f,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_PASSKEY,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_DISP,
+                .passkey = 222333,
+            },
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: numeric comparison
+ * Initiator IO capabilities: 1
+ * Responder IO capabilities: 1
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 5
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_nc_iio1_rio1_b1_iat0_rat0_ik5_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .resp_id_addr = {
+            0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+        },
+        .pair_req = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x0d,
+            .resp_key_dist = 0x0f,
+        },
+        .pair_rsp = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x05,
+            .resp_key_dist = 0x07,
+        },
+        .our_priv_key = {
+            0xd6, 0x2f, 0x4f, 0x6b, 0xeb, 0xfc, 0xbd, 0xee,
+            0x9b, 0x94, 0xd7, 0x15, 0x98, 0xc6, 0x0c, 0x83,
+            0x9b, 0xc7, 0xa2, 0x45, 0xfd, 0x00, 0xe8, 0xa4,
+            0x52, 0xe9, 0x70, 0x2f, 0xd7, 0x62, 0xf1, 0xa4,
+        },
+        .public_key_req = {
+            .x = {
+                0x41, 0x0d, 0x95, 0x8a, 0x68, 0xb8, 0xcf, 0x07,
+                0x58, 0x25, 0x5f, 0x97, 0xd2, 0x99, 0x71, 0x44,
+                0x06, 0xfc, 0x9c, 0x4d, 0xd1, 0x74, 0x80, 0xed,
+                0x49, 0xd1, 0x36, 0x6b, 0x55, 0x8b, 0x54, 0x3b,
+            },
+            .y = {
+                0x0f, 0x1a, 0x61, 0x45, 0xe5, 0x4b, 0x11, 0x13,
+                0xb3, 0x15, 0x87, 0x09, 0xec, 0x16, 0xf8, 0x41,
+                0x2e, 0xe2, 0x15, 0x93, 0x14, 0x56, 0x9f, 0xcd,
+                0x60, 0x7d, 0x92, 0xec, 0xd3, 0xb5, 0x85, 0xc5,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0xbc, 0x6a, 0xcf, 0xc6, 0x8a, 0x3a, 0xdc, 0x89,
+                0xdd, 0xa9, 0xaf, 0x29, 0xc7, 0xaf, 0xe2, 0x8b,
+                0x25, 0xee, 0xce, 0xa6, 0x10, 0x1d, 0x33, 0x2f,
+                0xd5, 0xfc, 0x30, 0xb8, 0xb1, 0x7b, 0xb1, 0x6e,
+            },
+            .y = {
+                0x1a, 0xc6, 0x42, 0x36, 0x98, 0x40, 0x4f, 0x90,
+                0x82, 0xa0, 0x10, 0x3a, 0xa5, 0x0f, 0xcf, 0x57,
+                0xd2, 0x2e, 0x80, 0x9d, 0x61, 0xc7, 0x21, 0xac,
+                0x47, 0x5b, 0x93, 0x75, 0x02, 0x30, 0x40, 0x14,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x73, 0xc8, 0x56, 0x5e, 0x33, 0x37, 0x26, 0xb6,
+                0x00, 0x65, 0x9c, 0xa1, 0xee, 0xbf, 0x61, 0xf6,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x7c, 0x23, 0x03, 0x70, 0x54, 0xa2, 0x70, 0xe4,
+                0x2d, 0xe9, 0x88, 0x6f, 0x40, 0xd6, 0x2f, 0xb2,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x2d, 0x9f, 0xe8, 0x1d, 0xf2, 0x4e, 0x2e, 0x58,
+                0x16, 0x8c, 0x83, 0x89, 0x92, 0x70, 0xa2, 0xba,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xc0, 0x8a, 0x1c, 0xff, 0x7f, 0xd6, 0xbc, 0xee,
+                0x19, 0xa5, 0xc6, 0x3a, 0xbd, 0x48, 0x4b, 0xc3,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x38, 0x36, 0x83, 0xd5, 0x1a, 0xfb, 0xe6, 0x3c,
+                0x80, 0x0c, 0x81, 0x81, 0x78, 0x12, 0x41, 0x38,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x33, 0x22, 0x11, 0x00, 0x45, 0x0a,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x52, 0xf4, 0xcc, 0x2f, 0xc6, 0xc1, 0xdb, 0x07,
+                0xa5, 0x38, 0xc1, 0x09, 0x82, 0x2e, 0xa3, 0x53,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0xc1, 0xa3, 0x62, 0x6a, 0x9e, 0xaa, 0x37, 0xd9,
+                0x65, 0x9f, 0x7f, 0x5d, 0x62, 0x0c, 0x1c, 0x6c,
+            },
+        },
+        .ltk = {
+            0xd8, 0x7f, 0x0a, 0x94, 0x41, 0xa5, 0xfd, 0x84,
+            0x15, 0x01, 0xb7, 0x2a, 0x7a, 0xe4, 0xfd, 0xfb,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_NUMCMP,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NUMCMP,
+                .numcmp_accept = 1,
+            },
+            .exp_numcmp = 516214,
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: just works
+ * Initiator IO capabilities: 3
+ * Responder IO capabilities: 4
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 7
+ * Responder key distribution: 5
+ */
+TEST_CASE_SELF(ble_sm_sc_us_jw_iio3_rio4_b1_iat0_rat0_ik7_rk5)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+        },
+        .resp_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .pair_req = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x05,
+        },
+        .our_priv_key = {
+            0xaf, 0xce, 0x12, 0x45, 0xa8, 0xe0, 0xa9, 0x45,
+            0x8a, 0x56, 0xc5, 0xbf, 0x3b, 0xf9, 0x04, 0x69,
+            0xf2, 0xf9, 0xe4, 0xd4, 0x7e, 0xb7, 0xc9, 0x65,
+            0xb1, 0x68, 0x3e, 0xab, 0xcd, 0x8e, 0x6f, 0x1f,
+        },
+        .public_key_req = {
+            .x = {
+                0x45, 0xca, 0xda, 0xe3, 0x65, 0x7c, 0xf5, 0x37,
+                0x36, 0x66, 0x8b, 0x3b, 0x54, 0xb9, 0x2b, 0xb2,
+                0x09, 0xd5, 0x6e, 0xe0, 0x04, 0x1d, 0xd6, 0x49,
+                0xff, 0x55, 0x41, 0x35, 0xa0, 0x2f, 0x12, 0xee,
+            },
+            .y = {
+                0x65, 0x41, 0xd3, 0x7b, 0x59, 0xf2, 0xaf, 0x94,
+                0x78, 0xd8, 0x63, 0xc4, 0x9b, 0x9a, 0x9a, 0x92,
+                0x33, 0x0f, 0x14, 0x67, 0x98, 0x51, 0x9d, 0xff,
+                0xef, 0x59, 0xb7, 0x17, 0xc2, 0x16, 0x72, 0x18,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x9e, 0x44, 0x09, 0x57, 0xb7, 0x01, 0x78, 0x5b,
+                0x4e, 0x50, 0x0d, 0x99, 0x0d, 0x52, 0x88, 0x24,
+                0x19, 0xf5, 0x40, 0x53, 0x06, 0x1e, 0x68, 0xd0,
+                0xfd, 0xd2, 0x84, 0x8b, 0xae, 0x9d, 0xf7, 0xd9,
+            },
+            .y = {
+                0xc2, 0xe7, 0xe0, 0x01, 0xb3, 0x2a, 0x1b, 0x01,
+                0x19, 0xd1, 0x14, 0xb5, 0xc8, 0x98, 0x02, 0x2a,
+                0xbe, 0x6b, 0x33, 0x1a, 0x99, 0x18, 0x77, 0x23,
+                0xd4, 0x8b, 0x8c, 0x09, 0xf5, 0x77, 0x20, 0xa0,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0xbd, 0x85, 0xbe, 0x80, 0xd9, 0x77, 0x16, 0xa3,
+                0x65, 0x1a, 0xdf, 0xff, 0x5a, 0x6f, 0x8b, 0x37,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xb5, 0x59, 0x7c, 0x8e, 0x7b, 0x01, 0xac, 0x09,
+                0x8f, 0xe8, 0x97, 0x98, 0x8d, 0x3f, 0xb7, 0x63,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x86, 0x1f, 0x76, 0x11, 0x2e, 0x83, 0xed, 0x99,
+                0x9b, 0xc0, 0x9a, 0xab, 0x7f, 0x94, 0x20, 0xcb,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xe0, 0x9f, 0x87, 0x87, 0x9f, 0x82, 0xc5, 0x06,
+                0x5f, 0x11, 0xfa, 0xa0, 0xe3, 0xbf, 0x72, 0xf2,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x26, 0xc2, 0xf1, 0xb9, 0xf1, 0xc2, 0xbd, 0xcb,
+                0xdb, 0x94, 0x96, 0x8e, 0x08, 0xcc, 0x53, 0xd4,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x74, 0x14, 0xcd, 0x5a, 0x49, 0x2e, 0xb6, 0x0d,
+                0xc6, 0x82, 0xb0, 0x0f, 0x9c, 0xe6, 0xe5, 0x41,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0xfb, 0x93, 0xa2, 0xb7, 0x4d, 0x0e, 0xcc, 0x92,
+                0xe4, 0xbf, 0x5b, 0x3c, 0x6d, 0x87, 0x5b, 0x2d,
+            },
+        },
+        .ltk = {
+            0x2e, 0x6c, 0x8b, 0xdb, 0x9e, 0x19, 0x3e, 0x3d,
+            0x4d, 0x6d, 0x29, 0xbc, 0x89, 0xca, 0x57, 0xed,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_JW,
+        .authenticated = 0,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NONE,
+            },
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 2
+ * Responder IO capabilities: 4
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 7
+ * Responder key distribution: 5
+ */
+TEST_CASE_SELF(ble_sm_sc_us_pk_iio2_rio4_b1_iat0_rat0_ik7_rk5)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+        },
+        .resp_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .pair_req = {
+            .io_cap = 0x02,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x05,
+        },
+        .our_priv_key = {
+            0xb1, 0x6b, 0x4f, 0x81, 0xbc, 0xe3, 0x60, 0x9e,
+            0x00, 0x20, 0xf1, 0x73, 0x3e, 0xfb, 0xcc, 0x6e,
+            0x8c, 0xb6, 0xd2, 0x51, 0xd9, 0x36, 0x8a, 0x6d,
+            0xca, 0x8c, 0xd7, 0xbe, 0x96, 0x03, 0xdf, 0xd6,
+        },
+        .public_key_req = {
+            .x = {
+                0xe5, 0x0f, 0x02, 0x0a, 0x37, 0x90, 0x94, 0x5a,
+                0x06, 0x21, 0xf7, 0xbc, 0xd5, 0xbe, 0xb9, 0x24,
+                0x8a, 0x35, 0xfd, 0xf8, 0x5e, 0xe2, 0x70, 0xd5,
+                0x5a, 0xe8, 0xe7, 0xdd, 0x13, 0x90, 0xeb, 0xd4,
+            },
+            .y = {
+                0x41, 0xc8, 0x51, 0x1a, 0x25, 0x44, 0x01, 0x53,
+                0x42, 0x74, 0x07, 0x9c, 0x18, 0xe6, 0x3b, 0x8a,
+                0xce, 0x7a, 0x37, 0x1f, 0x18, 0x5c, 0x02, 0x7c,
+                0x67, 0x16, 0xf5, 0x30, 0x2b, 0x31, 0xa9, 0xc7,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1b, 0xd2, 0x03, 0x79, 0xb4, 0x9b, 0x0a, 0xd7,
+                0x1b, 0x28, 0x73, 0x2a, 0xd7, 0xe6, 0xa0, 0xd4,
+                0x2d, 0x95, 0x8d, 0x29, 0xaf, 0x6a, 0xab, 0xee,
+                0xa0, 0x0d, 0x13, 0x4d, 0xe7, 0x16, 0x76, 0x91,
+            },
+            .y = {
+                0x2a, 0x26, 0x2c, 0x50, 0x55, 0xd1, 0x2b, 0x83,
+                0xf6, 0x5f, 0xdb, 0x99, 0x5f, 0x85, 0xf6, 0x78,
+                0x1c, 0x14, 0xed, 0xd3, 0x70, 0x5e, 0xe5, 0x2c,
+                0x05, 0x1e, 0x5c, 0xec, 0xf8, 0x65, 0x43, 0x49,
+            },
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x55, 0x2c, 0xaa, 0x41, 0x59, 0x42, 0x4d, 0xfe,
+                0x47, 0x74, 0xcd, 0x2b, 0x11, 0xab, 0x21, 0xe6,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x6a, 0x3c, 0x45, 0xf5, 0xb2, 0xe2, 0x04, 0x30,
+                0xde, 0xd6, 0x3c, 0x6d, 0x85, 0x00, 0x00, 0x2c,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x78, 0x06, 0x04, 0x60, 0x76, 0xe9, 0xc4, 0x5a,
+                0xfb, 0x34, 0x44, 0xae, 0x45, 0xa0, 0x84, 0xde,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x91, 0xc8, 0xfd, 0x1b, 0xb2, 0x85, 0x08, 0x76,
+                0xd3, 0xf1, 0xc4, 0xa0, 0xfa, 0x92, 0x8c, 0x94,
+            },
+        },
+        .confirm_req[1] = {
+            .value = {
+                0xb1, 0x2f, 0x68, 0x35, 0xa1, 0xa5, 0x84, 0xb1,
+                0x4f, 0x1a, 0xb1, 0xb5, 0xf0, 0xb2, 0xbe, 0x61,
+            },
+        },
+        .confirm_rsp[1] = {
+            .value = {
+                0x07, 0xd8, 0x43, 0x74, 0xe8, 0x42, 0xf3, 0xf1,
+                0x87, 0x3d, 0x9e, 0x92, 0xea, 0x33, 0xe8, 0x54,
+            },
+        },
+        .random_req[1] = {
+            .value = {
+                0x4c, 0xb7, 0xcc, 0x6d, 0x90, 0x9f, 0x1e, 0x2d,
+                0x9d, 0x1e, 0x52, 0xa7, 0xe0, 0x0c, 0x7b, 0xf7,
+            },
+        },
+        .random_rsp[1] = {
+            .value = {
+                0x5c, 0x32, 0x82, 0xc8, 0x76, 0x17, 0x3b, 0x18,
+                0x66, 0xda, 0xbf, 0xc3, 0x13, 0x49, 0x05, 0xfb,
+            },
+        },
+        .confirm_req[2] = {
+            .value = {
+                0x27, 0x61, 0x4d, 0x04, 0x64, 0xa9, 0x58, 0xf1,
+                0xe0, 0xf9, 0xe5, 0x78, 0x0b, 0x54, 0x89, 0x0a,
+            },
+        },
+        .confirm_rsp[2] = {
+            .value = {
+                0xe4, 0x8f, 0xdb, 0xc8, 0x35, 0xed, 0x4e, 0x7d,
+                0xbc, 0x92, 0x7f, 0x58, 0x02, 0xaa, 0xbf, 0x6b,
+            },
+        },
+        .random_req[2] = {
+            .value = {
+                0xfe, 0x85, 0x08, 0xe0, 0x35, 0x90, 0x13, 0xa9,
+                0xd3, 0xcf, 0xb6, 0x6d, 0x36, 0xaf, 0xbd, 0x59,
+            },
+        },
+        .random_rsp[2] = {
+            .value = {
+                0x47, 0x40, 0x8e, 0x97, 0xe3, 0xfe, 0x8f, 0x52,
+                0x29, 0x5e, 0x6b, 0x44, 0xdf, 0x0d, 0x60, 0xf4,
+            },
+        },
+        .confirm_req[3] = {
+            .value = {
+                0xac, 0xab, 0x13, 0x7c, 0x1a, 0x6e, 0x7a, 0xdb,
+                0xf6, 0xe8, 0x72, 0x9f, 0xc5, 0xc3, 0x99, 0x1b,
+            },
+        },
+        .confirm_rsp[3] = {
+            .value = {
+                0x79, 0xf2, 0xd1, 0x89, 0x5e, 0xa5, 0xa2, 0x90,
+                0xee, 0x25, 0x36, 0x81, 0x5a, 0x87, 0x20, 0x82,
+            },
+        },
+        .random_req[3] = {
+            .value = {
+                0xd4, 0x46, 0xa0, 0xc4, 0x3d, 0xae, 0x22, 0x06,
+                0xaf, 0x5d, 0x93, 0x96, 0xb7, 0x06, 0xc3, 0x61,
+            },
+        },
+        .random_rsp[3] = {
+            .value = {
+                0x5f, 0x81, 0x97, 0x8b, 0x52, 0x87, 0x1c, 0x67,
+                0xe0, 0x04, 0xcc, 0x50, 0xd9, 0x2b, 0x16, 0xb5,
+            },
+        },
+        .confirm_req[4] = {
+            .value = {
+                0x6c, 0x51, 0xc3, 0x61, 0x77, 0x7f, 0xf1, 0x05,
+                0x9e, 0x0f, 0xba, 0xfd, 0x32, 0x02, 0x09, 0x45,
+            },
+        },
+        .confirm_rsp[4] = {
+            .value = {
+                0x54, 0xe5, 0x24, 0x81, 0x62, 0x68, 0xe2, 0x45,
+                0x86, 0x2c, 0x11, 0x28, 0x15, 0xa8, 0x8e, 0x5b,
+            },
+        },
+        .random_req[4] = {
+            .value = {
+                0xbb, 0x29, 0x3a, 0xba, 0xe6, 0x4f, 0x06, 0xcf,
+                0xa3, 0x13, 0x27, 0xf2, 0xcb, 0xe4, 0xd2, 0xe6,
+            },
+        },
+        .random_rsp[4] = {
+            .value = {
+                0x50, 0xba, 0xd0, 0x0e, 0x26, 0xab, 0x04, 0xf8,
+                0xa2, 0x03, 0x1e, 0x63, 0x9a, 0xf7, 0x15, 0xdc,
+            },
+        },
+        .confirm_req[5] = {
+            .value = {
+                0x12, 0x3e, 0xfe, 0x5a, 0xb1, 0x09, 0x6f, 0x17,
+                0xb7, 0x77, 0x7e, 0x65, 0x88, 0xd4, 0x95, 0x56,
+            },
+        },
+        .confirm_rsp[5] = {
+            .value = {
+                0xc6, 0x9b, 0xac, 0xde, 0x7e, 0x03, 0x7a, 0xd3,
+                0xf1, 0xff, 0x3c, 0x4f, 0x4a, 0x85, 0xba, 0x73,
+            },
+        },
+        .random_req[5] = {
+            .value = {
+                0x17, 0xd5, 0x5e, 0x69, 0x30, 0x2c, 0x1f, 0x01,
+                0x87, 0x9c, 0xd6, 0xd2, 0xe4, 0x48, 0x8c, 0x84,
+            },
+        },
+        .random_rsp[5] = {
+            .value = {
+                0x9d, 0x54, 0x83, 0x4a, 0xcd, 0x93, 0x7c, 0x1e,
+                0x5b, 0xaf, 0xd2, 0x66, 0x8c, 0x2d, 0xaa, 0xc3,
+            },
+        },
+        .confirm_req[6] = {
+            .value = {
+                0xdc, 0x24, 0x69, 0xa8, 0xd3, 0xa9, 0x17, 0x11,
+                0x08, 0x37, 0x1a, 0x1e, 0x92, 0x03, 0xee, 0x36,
+            },
+        },
+        .confirm_rsp[6] = {
+            .value = {
+                0x98, 0xf8, 0x72, 0x71, 0x99, 0xa0, 0xbd, 0xcd,
+                0xb1, 0x97, 0x4c, 0x8a, 0xb8, 0xa8, 0x1a, 0x52,
+            },
+        },
+        .random_req[6] = {
+            .value = {
+                0xbf, 0xb1, 0x8e, 0xa5, 0x14, 0xe3, 0xeb, 0x9e,
+                0x29, 0x27, 0xe0, 0x19, 0xb1, 0xb2, 0x5c, 0xfe,
+            },
+        },
+        .random_rsp[6] = {
+            .value = {
+                0xae, 0x8a, 0x92, 0x78, 0x53, 0x7b, 0xdb, 0x8c,
+                0xec, 0x3a, 0x99, 0x2b, 0x94, 0xf1, 0x17, 0xfe,
+            },
+        },
+        .confirm_req[7] = {
+            .value = {
+                0xcf, 0xaf, 0x70, 0x73, 0x53, 0x65, 0x89, 0x57,
+                0x36, 0x98, 0xd2, 0x28, 0x86, 0x79, 0xfe, 0x85,
+            },
+        },
+        .confirm_rsp[7] = {
+            .value = {
+                0x0d, 0x2d, 0x77, 0x8a, 0x21, 0x11, 0xd9, 0x61,
+                0x9f, 0x80, 0x32, 0x8a, 0x32, 0x09, 0x42, 0x42,
+            },
+        },
+        .random_req[7] = {
+            .value = {
+                0x8b, 0xd2, 0x53, 0xcd, 0x96, 0xd1, 0x14, 0xb5,
+                0xea, 0x17, 0xb1, 0xa3, 0xa8, 0xfc, 0x3c, 0x2b,
+            },
+        },
+        .random_rsp[7] = {
+            .value = {
+                0xc2, 0x4f, 0x84, 0x60, 0x54, 0x79, 0x16, 0xed,
+                0x1a, 0x6e, 0x78, 0xa0, 0x99, 0x58, 0xf2, 0x94,
+            },
+        },
+        .confirm_req[8] = {
+            .value = {
+                0x9a, 0x4c, 0xbc, 0x9c, 0x55, 0x15, 0xa2, 0x4f,
+                0xa2, 0x5d, 0x3b, 0xa7, 0x43, 0xb3, 0x9c, 0x63,
+            },
+        },
+        .confirm_rsp[8] = {
+            .value = {
+                0xa3, 0xb1, 0x88, 0xa5, 0x70, 0xca, 0xa3, 0xa9,
+                0x67, 0x2a, 0xac, 0x99, 0x5e, 0x61, 0x68, 0xa0,
+            },
+        },
+        .random_req[8] = {
+            .value = {
+                0xcf, 0xcf, 0x5b, 0x94, 0xe0, 0xb2, 0x9d, 0x5a,
+                0x86, 0x71, 0x45, 0xce, 0xd9, 0xce, 0x13, 0xba,
+            },
+        },
+        .random_rsp[8] = {
+            .value = {
+                0x10, 0x96, 0x8a, 0x50, 0xa4, 0xd0, 0xaa, 0x5f,
+                0xd6, 0x32, 0xdb, 0x09, 0x7e, 0x22, 0x96, 0x42,
+            },
+        },
+        .confirm_req[9] = {
+            .value = {
+                0xf0, 0x90, 0x61, 0x25, 0x04, 0x29, 0x4f, 0xb6,
+                0x8b, 0xd5, 0x73, 0x49, 0xbd, 0xf7, 0x9b, 0xe7,
+            },
+        },
+        .confirm_rsp[9] = {
+            .value = {
+                0x5b, 0xe6, 0xb4, 0x3f, 0x1b, 0x77, 0x12, 0x75,
+                0x84, 0x94, 0xc6, 0x07, 0xfa, 0xa1, 0x41, 0x94,
+            },
+        },
+        .random_req[9] = {
+            .value = {
+                0x3d, 0x1a, 0xa3, 0x95, 0xec, 0x72, 0x84, 0xf4,
+                0xc5, 0xcd, 0xaa, 0x48, 0xe9, 0x0c, 0x0f, 0xe3,
+            },
+        },
+        .random_rsp[9] = {
+            .value = {
+                0x8a, 0x5a, 0x53, 0xfc, 0x07, 0x52, 0x01, 0xb9,
+                0xe9, 0x2d, 0xe7, 0x9d, 0x8c, 0x7c, 0xc7, 0xb3,
+            },
+        },
+        .confirm_req[10] = {
+            .value = {
+                0xe7, 0x8e, 0xc5, 0x08, 0x7f, 0x7e, 0xb8, 0xdc,
+                0x05, 0x88, 0x3a, 0x92, 0x5a, 0xf5, 0x9b, 0xa9,
+            },
+        },
+        .confirm_rsp[10] = {
+            .value = {
+                0xf7, 0xa2, 0xb6, 0xec, 0xcd, 0xef, 0xcb, 0xb7,
+                0x6f, 0xc3, 0xac, 0x17, 0xe2, 0xfd, 0xfa, 0x42,
+            },
+        },
+        .random_req[10] = {
+            .value = {
+                0x0d, 0xd1, 0xa2, 0x1d, 0xff, 0x74, 0xc5, 0x99,
+                0xe0, 0x67, 0x07, 0x99, 0x95, 0x75, 0x39, 0x76,
+            },
+        },
+        .random_rsp[10] = {
+            .value = {
+                0x2f, 0x13, 0xd1, 0x59, 0xfe, 0x20, 0x60, 0xf0,
+                0x02, 0x0c, 0xea, 0x79, 0xd7, 0x40, 0x86, 0x85,
+            },
+        },
+        .confirm_req[11] = {
+            .value = {
+                0x8b, 0x57, 0x87, 0xdd, 0xb1, 0xcc, 0x2d, 0x65,
+                0xc1, 0xba, 0xac, 0x88, 0x48, 0x23, 0xda, 0xe7,
+            },
+        },
+        .confirm_rsp[11] = {
+            .value = {
+                0xb3, 0xc4, 0x2e, 0xea, 0x33, 0xaf, 0x12, 0x9c,
+                0xb5, 0xab, 0xa1, 0x95, 0x30, 0xca, 0x46, 0x48,
+            },
+        },
+        .random_req[11] = {
+            .value = {
+                0x35, 0x57, 0xcd, 0xd5, 0xd2, 0xf8, 0xd7, 0xf2,
+                0x7b, 0xe3, 0xd7, 0xba, 0x31, 0xa5, 0xca, 0xfd,
+            },
+        },
+        .random_rsp[11] = {
+            .value = {
+                0xe2, 0x3b, 0x20, 0xbe, 0xec, 0xa5, 0x34, 0x3b,
+                0x76, 0x23, 0x53, 0x28, 0x36, 0xc4, 0x60, 0x13,
+            },
+        },
+        .confirm_req[12] = {
+            .value = {
+                0xc9, 0xfe, 0x03, 0x49, 0xe4, 0xff, 0x7e, 0xf7,
+                0x00, 0xd1, 0x2b, 0x13, 0xb1, 0x15, 0x6e, 0x92,
+            },
+        },
+        .confirm_rsp[12] = {
+            .value = {
+                0xbc, 0xa2, 0xf2, 0x03, 0x5c, 0xfd, 0x20, 0x7b,
+                0xd0, 0x1f, 0xd6, 0x50, 0xec, 0xc6, 0x7b, 0x31,
+            },
+        },
+        .random_req[12] = {
+            .value = {
+                0x04, 0x50, 0xea, 0xb8, 0xca, 0x36, 0x1a, 0x61,
+                0x92, 0xed, 0xa0, 0x67, 0x78, 0x15, 0x10, 0xb5,
+            },
+        },
+        .random_rsp[12] = {
+            .value = {
+                0x0c, 0x8e, 0x9d, 0x7b, 0x9d, 0x7e, 0xda, 0x23,
+                0xbb, 0x61, 0xd9, 0xff, 0x46, 0x77, 0x33, 0x1b,
+            },
+        },
+        .confirm_req[13] = {
+            .value = {
+                0x9a, 0xff, 0xd6, 0xe5, 0x1a, 0xc3, 0xd3, 0x37,
+                0x34, 0xeb, 0x3e, 0x3a, 0x8e, 0x0b, 0x86, 0xb4,
+            },
+        },
+        .confirm_rsp[13] = {
+            .value = {
+                0xf6, 0x32, 0x19, 0xb4, 0x08, 0x6b, 0x8a, 0x0f,
+                0xc9, 0x9c, 0x1b, 0x68, 0xb8, 0xa0, 0xd0, 0xc9,
+            },
+        },
+        .random_req[13] = {
+            .value = {
+                0x86, 0xeb, 0x5c, 0xf9, 0x33, 0x54, 0x7d, 0xe4,
+                0xa4, 0xe2, 0xe1, 0xf6, 0x6b, 0xea, 0x34, 0xed,
+            },
+        },
+        .random_rsp[13] = {
+            .value = {
+                0xad, 0x53, 0xa0, 0x6e, 0xde, 0x1d, 0xda, 0x99,
+                0x31, 0x45, 0xe5, 0x3a, 0x73, 0xa1, 0x5e, 0xe1,
+            },
+        },
+        .confirm_req[14] = {
+            .value = {
+                0x93, 0xd4, 0xe0, 0xaa, 0x0c, 0x91, 0xba, 0xde,
+                0xc9, 0x5c, 0x68, 0xb0, 0xce, 0xb6, 0x84, 0xcd,
+            },
+        },
+        .confirm_rsp[14] = {
+            .value = {
+                0x85, 0xc7, 0x05, 0x02, 0x21, 0x9d, 0x4c, 0x4c,
+                0x16, 0xf7, 0x8f, 0x7b, 0xaa, 0xb4, 0x8f, 0x37,
+            },
+        },
+        .random_req[14] = {
+            .value = {
+                0x84, 0xfd, 0xf1, 0x39, 0x1a, 0x9a, 0xa5, 0xb8,
+                0x49, 0xc0, 0x66, 0xdc, 0x33, 0x71, 0x32, 0x87,
+            },
+        },
+        .random_rsp[14] = {
+            .value = {
+                0x5d, 0xaf, 0x38, 0xcd, 0xb5, 0x83, 0xaa, 0xa0,
+                0xab, 0x30, 0x82, 0xed, 0x6f, 0xd2, 0x75, 0xe7,
+            },
+        },
+        .confirm_req[15] = {
+            .value = {
+                0x88, 0x12, 0xe8, 0x89, 0xd4, 0x52, 0x6d, 0xac,
+                0x61, 0x2a, 0x85, 0x85, 0x1e, 0x9c, 0x82, 0x21,
+            },
+        },
+        .confirm_rsp[15] = {
+            .value = {
+                0xc1, 0xe9, 0xcd, 0x21, 0x29, 0x6a, 0x78, 0xe4,
+                0x7b, 0x7d, 0x73, 0x25, 0x9e, 0x9b, 0x95, 0x8b,
+            },
+        },
+        .random_req[15] = {
+            .value = {
+                0x95, 0x87, 0x9d, 0x5a, 0x10, 0x14, 0xa0, 0xdf,
+                0x5e, 0x02, 0x22, 0x39, 0x23, 0xc9, 0xbc, 0xba,
+            },
+        },
+        .random_rsp[15] = {
+            .value = {
+                0x1b, 0x91, 0xe2, 0xdf, 0xca, 0xfe, 0x2b, 0x61,
+                0x33, 0x8c, 0x83, 0xbf, 0xcf, 0xc3, 0x72, 0xcc,
+            },
+        },
+        .confirm_req[16] = {
+            .value = {
+                0xce, 0xc9, 0x68, 0xf7, 0xea, 0x41, 0x18, 0x5c,
+                0x16, 0x6a, 0x98, 0x13, 0x0c, 0x10, 0xc2, 0xa3,
+            },
+        },
+        .confirm_rsp[16] = {
+            .value = {
+                0x97, 0x73, 0xc9, 0x72, 0x68, 0x99, 0x63, 0xed,
+                0x81, 0x3b, 0x5c, 0xee, 0x37, 0xfc, 0xca, 0xae,
+            },
+        },
+        .random_req[16] = {
+            .value = {
+                0x5b, 0x85, 0xb0, 0x1b, 0xc3, 0xde, 0x18, 0xba,
+                0xc1, 0xc7, 0x89, 0x99, 0xfe, 0xcd, 0xdb, 0x6a,
+            },
+        },
+        .random_rsp[16] = {
+            .value = {
+                0x5e, 0x1a, 0xcb, 0xbc, 0xda, 0x41, 0x06, 0x5a,
+                0x14, 0x34, 0x3a, 0xb1, 0xa1, 0x6f, 0xb2, 0xd8,
+            },
+        },
+        .confirm_req[17] = {
+            .value = {
+                0x1d, 0x59, 0x8a, 0xb0, 0x19, 0xe5, 0xff, 0x45,
+                0xb6, 0xc3, 0x33, 0x64, 0xd1, 0x6e, 0xee, 0xdd,
+            },
+        },
+        .confirm_rsp[17] = {
+            .value = {
+                0x4c, 0x9b, 0xe8, 0x68, 0x52, 0x34, 0xef, 0xe1,
+                0x84, 0xbd, 0x37, 0x85, 0x53, 0x0d, 0xd5, 0xc1,
+            },
+        },
+        .random_req[17] = {
+            .value = {
+                0xa6, 0xf7, 0x97, 0x18, 0x9a, 0x3e, 0x9d, 0xcf,
+                0x91, 0xa3, 0xa3, 0x8e, 0xda, 0x8f, 0x8f, 0x90,
+            },
+        },
+        .random_rsp[17] = {
+            .value = {
+                0x94, 0x10, 0x19, 0x17, 0x8d, 0x0a, 0x72, 0xfd,
+                0x24, 0x9d, 0xfd, 0x37, 0x4e, 0xdf, 0x4c, 0x30,
+            },
+        },
+        .confirm_req[18] = {
+            .value = {
+                0xfc, 0x64, 0x8a, 0x8b, 0x37, 0x17, 0x90, 0x6d,
+                0x25, 0x0e, 0xc6, 0x18, 0xc9, 0xc9, 0xc2, 0x2a,
+            },
+        },
+        .confirm_rsp[18] = {
+            .value = {
+                0x50, 0x98, 0x86, 0xf5, 0xc0, 0xda, 0x45, 0x2d,
+                0xea, 0xc8, 0x9d, 0x28, 0x04, 0xd8, 0x73, 0x6f,
+            },
+        },
+        .random_req[18] = {
+            .value = {
+                0x13, 0x10, 0x38, 0xe8, 0x17, 0x6d, 0x72, 0xd5,
+                0x94, 0xaf, 0xed, 0x4f, 0x23, 0xa0, 0x41, 0xfc,
+            },
+        },
+        .random_rsp[18] = {
+            .value = {
+                0xdf, 0xed, 0xf7, 0x08, 0xce, 0x64, 0xbc, 0x11,
+                0x41, 0x7a, 0xd9, 0xf7, 0x4a, 0xd9, 0x4a, 0x15,
+            },
+        },
+        .confirm_req[19] = {
+            .value = {
+                0xae, 0x24, 0x8f, 0xdf, 0xb0, 0x57, 0xc4, 0x9c,
+                0xe6, 0xae, 0x9b, 0xc2, 0x4d, 0x3d, 0x1c, 0xcb,
+            },
+        },
+        .confirm_rsp[19] = {
+            .value = {
+                0xcc, 0x5c, 0xa3, 0xbe, 0xd7, 0x83, 0xee, 0x60,
+                0x80, 0xff, 0x5f, 0x1a, 0x07, 0xbf, 0x4c, 0x33,
+            },
+        },
+        .random_req[19] = {
+            .value = {
+                0x93, 0xc3, 0x62, 0x06, 0xcb, 0xe5, 0xb0, 0x01,
+                0x02, 0x18, 0xa2, 0x50, 0x4c, 0x73, 0xa2, 0x27,
+            },
+        },
+        .random_rsp[19] = {
+            .value = {
+                0x11, 0x2a, 0xd3, 0x06, 0x28, 0x9c, 0xdf, 0x73,
+                0xa5, 0xa4, 0xe5, 0x1e, 0x07, 0xcf, 0xee, 0x71,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x73, 0xa0, 0x40, 0x58, 0x78, 0x20, 0x5f, 0x2c,
+                0xf4, 0x19, 0x23, 0xa8, 0x74, 0xbd, 0xc2, 0x3e,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x5a, 0x30, 0xbc, 0xce, 0xec, 0xdf, 0xf0, 0x32,
+                0x3c, 0x18, 0xa3, 0xd3, 0x3f, 0x20, 0x87, 0x10,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x2e, 0x81, 0x09, 0xde, 0x32, 0xc5, 0x28, 0x34,
+                0xe1, 0x45, 0x4a, 0x35, 0x49, 0xef, 0xa2, 0xed,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x90, 0x3d, 0x26, 0x65, 0xc1, 0xd1, 0x5a, 0x9d,
+                0xda, 0xab, 0x0d, 0x00, 0x05, 0x0e, 0x6c, 0x5d,
+            },
+        },
+        .ltk = {
+            0xf1, 0x41, 0x1a, 0x5b, 0x60, 0xc1, 0x43, 0xc6,
+            0x80, 0x34, 0x5e, 0x7f, 0xd8, 0x0c, 0x75, 0xdc,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_PASSKEY,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_INPUT,
+                .passkey = 516645,
+            },
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 0
+ * Responder IO capabilities: 4
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 7
+ * Responder key distribution: 5
+ */
+TEST_CASE_SELF(ble_sm_sc_us_pk_iio0_rio4_b1_iat0_rat0_ik7_rk5)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+        },
+        .resp_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .pair_req = {
+            .io_cap = 0x00,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x05,
+        },
+        .our_priv_key = {
+            0xb1, 0x6b, 0x4f, 0x81, 0xbc, 0xe3, 0x60, 0x9e,
+            0x00, 0x20, 0xf1, 0x73, 0x3e, 0xfb, 0xcc, 0x6e,
+            0x8c, 0xb6, 0xd2, 0x51, 0xd9, 0x36, 0x8a, 0x6d,
+            0xca, 0x8c, 0xd7, 0xbe, 0x96, 0x03, 0xdf, 0xd6,
+        },
+        .public_key_req = {
+            .x = {
+                0xe5, 0x0f, 0x02, 0x0a, 0x37, 0x90, 0x94, 0x5a,
+                0x06, 0x21, 0xf7, 0xbc, 0xd5, 0xbe, 0xb9, 0x24,
+                0x8a, 0x35, 0xfd, 0xf8, 0x5e, 0xe2, 0x70, 0xd5,
+                0x5a, 0xe8, 0xe7, 0xdd, 0x13, 0x90, 0xeb, 0xd4,
+            },
+            .y = {
+                0x41, 0xc8, 0x51, 0x1a, 0x25, 0x44, 0x01, 0x53,
+                0x42, 0x74, 0x07, 0x9c, 0x18, 0xe6, 0x3b, 0x8a,
+                0xce, 0x7a, 0x37, 0x1f, 0x18, 0x5c, 0x02, 0x7c,
+                0x67, 0x16, 0xf5, 0x30, 0x2b, 0x31, 0xa9, 0xc7,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x03, 0x0d, 0x13, 0x55, 0xd9, 0xee, 0x3f, 0xac,
+                0x8e, 0x8a, 0xa6, 0x2a, 0xcb, 0x60, 0x35, 0xb9,
+                0xb2, 0x4d, 0x63, 0x91, 0x5e, 0xa1, 0xdd, 0xdf,
+                0x60, 0xdc, 0x6e, 0x09, 0xb9, 0x9e, 0xf1, 0x4d,
+            },
+            .y = {
+                0xa8, 0x09, 0x31, 0x1e, 0x39, 0x96, 0x74, 0x41,
+                0xea, 0x19, 0x4f, 0x24, 0x36, 0x57, 0x7c, 0x9f,
+                0x21, 0xa3, 0xad, 0xa1, 0x3d, 0xe2, 0x1c, 0x6a,
+                0xd6, 0xc9, 0xdb, 0xff, 0xce, 0x0a, 0x94, 0x12,
+            },
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x3b, 0x3d, 0xb2, 0x2f, 0x72, 0x0f, 0x93, 0x19,
+                0x95, 0xdb, 0x88, 0xdf, 0x5d, 0x58, 0x95, 0x37,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x98, 0xab, 0x20, 0x8d, 0x51, 0x3b, 0x6c, 0x29,
+                0x2d, 0x73, 0x15, 0xf6, 0x6d, 0x6d, 0xb9, 0xb3,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xc1, 0xdf, 0x20, 0x3d, 0x7b, 0xcb, 0x5f, 0xe2,
+                0x9a, 0x23, 0x9c, 0xba, 0x2f, 0x42, 0x3c, 0xb8,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x7d, 0x82, 0xb4, 0xae, 0x41, 0xdb, 0x67, 0x8f,
+                0x54, 0x01, 0x21, 0x64, 0x31, 0xd4, 0xfc, 0xb5,
+            },
+        },
+        .confirm_req[1] = {
+            .value = {
+                0xc3, 0xa5, 0xd0, 0xdd, 0xd5, 0xec, 0x1d, 0xc3,
+                0x14, 0x95, 0x79, 0xb2, 0x61, 0x4d, 0x4f, 0x36,
+            },
+        },
+        .confirm_rsp[1] = {
+            .value = {
+                0xe2, 0x20, 0xf4, 0x4d, 0xa1, 0x9c, 0x83, 0x51,
+                0x18, 0xf9, 0x35, 0x2a, 0x51, 0x50, 0xdf, 0xe7,
+            },
+        },
+        .random_req[1] = {
+            .value = {
+                0x71, 0xcb, 0x01, 0xb4, 0x83, 0xdc, 0xd8, 0x54,
+                0x0f, 0xe5, 0xd5, 0x6b, 0x6a, 0x0d, 0x98, 0xb6,
+            },
+        },
+        .random_rsp[1] = {
+            .value = {
+                0x30, 0xbf, 0xd3, 0xfd, 0xf4, 0xc2, 0xa1, 0xd0,
+                0xba, 0x4b, 0x27, 0x7c, 0x29, 0x98, 0x54, 0xa2,
+            },
+        },
+        .confirm_req[2] = {
+            .value = {
+                0xf0, 0x92, 0xc4, 0xda, 0x8a, 0x17, 0x7c, 0xc6,
+                0x14, 0x05, 0x7d, 0xbb, 0xfc, 0x7c, 0xcd, 0x0a,
+            },
+        },
+        .confirm_rsp[2] = {
+            .value = {
+                0xf3, 0x89, 0xca, 0xe0, 0xfb, 0xbe, 0x8c, 0xc3,
+                0x4c, 0x6c, 0x6e, 0x11, 0x36, 0x4e, 0xaa, 0x25,
+            },
+        },
+        .random_req[2] = {
+            .value = {
+                0x78, 0x5a, 0xf0, 0x1e, 0x2a, 0x0d, 0x16, 0xb3,
+                0x03, 0x4b, 0x4b, 0x68, 0x17, 0xe0, 0xf0, 0x82,
+            },
+        },
+        .random_rsp[2] = {
+            .value = {
+                0xbf, 0x96, 0xdd, 0xf5, 0x30, 0x2a, 0xe9, 0x8c,
+                0xb9, 0x13, 0xc5, 0xb7, 0x15, 0x1f, 0xa3, 0x9b,
+            },
+        },
+        .confirm_req[3] = {
+            .value = {
+                0x36, 0xe4, 0x4c, 0x00, 0xe7, 0x0d, 0xee, 0xe4,
+                0x95, 0xb8, 0x6a, 0xf9, 0xf7, 0x24, 0xef, 0xea,
+            },
+        },
+        .confirm_rsp[3] = {
+            .value = {
+                0x2e, 0x90, 0x87, 0x85, 0xb8, 0x29, 0x93, 0x9e,
+                0x38, 0xa6, 0xdb, 0x17, 0xb2, 0xa8, 0x32, 0x65,
+            },
+        },
+        .random_req[3] = {
+            .value = {
+                0x0a, 0xee, 0x93, 0xf6, 0x56, 0x35, 0x8e, 0xed,
+                0x3f, 0x45, 0xa5, 0x01, 0x59, 0xeb, 0xea, 0xa8,
+            },
+        },
+        .random_rsp[3] = {
+            .value = {
+                0x38, 0xd0, 0xf8, 0x11, 0x5e, 0x47, 0x72, 0x66,
+                0xce, 0x56, 0x9c, 0x81, 0x5f, 0x52, 0xd4, 0x9a,
+            },
+        },
+        .confirm_req[4] = {
+            .value = {
+                0x2c, 0x98, 0x9b, 0x71, 0xe4, 0xde, 0x6d, 0x20,
+                0x84, 0x30, 0xab, 0x7a, 0xfc, 0x43, 0x82, 0xc6,
+            },
+        },
+        .confirm_rsp[4] = {
+            .value = {
+                0x76, 0xfe, 0x1f, 0x78, 0xaa, 0x42, 0xd5, 0xc6,
+                0x9f, 0xe4, 0xa7, 0xc7, 0xb8, 0xd2, 0x1e, 0x59,
+            },
+        },
+        .random_req[4] = {
+            .value = {
+                0x61, 0x5e, 0x47, 0xb1, 0x77, 0x6f, 0x04, 0xee,
+                0x94, 0xc4, 0x6c, 0xa9, 0xf5, 0xf8, 0x11, 0x6e,
+            },
+        },
+        .random_rsp[4] = {
+            .value = {
+                0xa5, 0xad, 0x98, 0x65, 0x28, 0xfc, 0x6b, 0x02,
+                0x6d, 0x9a, 0x29, 0x61, 0x1c, 0x02, 0x0a, 0x6b,
+            },
+        },
+        .confirm_req[5] = {
+            .value = {
+                0x4d, 0x55, 0x3e, 0x1f, 0x87, 0x12, 0xc7, 0x6c,
+                0xd7, 0x9a, 0xa6, 0xf1, 0x6e, 0x48, 0xd3, 0x7d,
+            },
+        },
+        .confirm_rsp[5] = {
+            .value = {
+                0xff, 0x77, 0x6e, 0xba, 0x1f, 0xb7, 0xbd, 0x0b,
+                0x3f, 0xce, 0xd5, 0x39, 0x81, 0x17, 0x51, 0xfc,
+            },
+        },
+        .random_req[5] = {
+            .value = {
+                0xf9, 0x2c, 0x77, 0x41, 0xcd, 0x9a, 0x10, 0x99,
+                0xc6, 0x70, 0x5a, 0xc8, 0x24, 0x26, 0xb2, 0xc8,
+            },
+        },
+        .random_rsp[5] = {
+            .value = {
+                0xa0, 0x44, 0x0a, 0x8b, 0xda, 0x6a, 0x74, 0x90,
+                0x5f, 0x89, 0x44, 0xa5, 0x9a, 0x58, 0xd5, 0x08,
+            },
+        },
+        .confirm_req[6] = {
+            .value = {
+                0x7e, 0x6e, 0x89, 0xc8, 0xbe, 0xde, 0x1c, 0xc3,
+                0x45, 0xb6, 0x4c, 0x83, 0x71, 0xe2, 0xd6, 0xda,
+            },
+        },
+        .confirm_rsp[6] = {
+            .value = {
+                0x4a, 0x29, 0x7b, 0x88, 0x97, 0xc1, 0x60, 0x85,
+                0x32, 0x7d, 0xf1, 0xaa, 0x04, 0x13, 0x89, 0x11,
+            },
+        },
+        .random_req[6] = {
+            .value = {
+                0x2a, 0xaf, 0x7d, 0x21, 0x4e, 0x14, 0xf5, 0x7e,
+                0xcc, 0x39, 0xf7, 0x56, 0x45, 0x87, 0x23, 0x64,
+            },
+        },
+        .random_rsp[6] = {
+            .value = {
+                0x74, 0xd2, 0xff, 0xf0, 0x19, 0xf7, 0x87, 0xe7,
+                0x0d, 0x65, 0x27, 0x61, 0xea, 0x9e, 0x05, 0x3d,
+            },
+        },
+        .confirm_req[7] = {
+            .value = {
+                0x4f, 0x77, 0x22, 0x08, 0x58, 0xed, 0x8c, 0x60,
+                0xbf, 0xbc, 0x78, 0x0c, 0x80, 0xc9, 0xb7, 0x60,
+            },
+        },
+        .confirm_rsp[7] = {
+            .value = {
+                0xd2, 0x47, 0xfd, 0xea, 0xa3, 0x32, 0x53, 0xc1,
+                0x06, 0xcd, 0x64, 0xeb, 0x88, 0x64, 0x0e, 0xe5,
+            },
+        },
+        .random_req[7] = {
+            .value = {
+                0xc8, 0xd0, 0x45, 0xa8, 0x29, 0xdb, 0x5a, 0x42,
+                0xfe, 0x68, 0xa8, 0x7a, 0x0a, 0x13, 0x22, 0xa4,
+            },
+        },
+        .random_rsp[7] = {
+            .value = {
+                0x78, 0x14, 0x46, 0xe2, 0x47, 0x0e, 0xd4, 0xb4,
+                0xde, 0x35, 0x4a, 0x82, 0x4b, 0x32, 0x9b, 0x46,
+            },
+        },
+        .confirm_req[8] = {
+            .value = {
+                0x24, 0x96, 0xe5, 0x50, 0xfa, 0xff, 0xba, 0xdf,
+                0x6b, 0x76, 0x40, 0x60, 0x56, 0x5e, 0x5a, 0x66,
+            },
+        },
+        .confirm_rsp[8] = {
+            .value = {
+                0x09, 0xfe, 0x15, 0x3e, 0x55, 0xe5, 0xbe, 0xb7,
+                0x8d, 0xaa, 0x04, 0x59, 0xe6, 0x8b, 0x2c, 0x4e,
+            },
+        },
+        .random_req[8] = {
+            .value = {
+                0x02, 0x25, 0xbe, 0x88, 0x37, 0xb4, 0x6e, 0xcb,
+                0xbc, 0xa9, 0xef, 0x5a, 0xfd, 0x1a, 0x5f, 0x5f,
+            },
+        },
+        .random_rsp[8] = {
+            .value = {
+                0x0b, 0x35, 0xdc, 0x9b, 0x3d, 0xf7, 0xa6, 0x99,
+                0xf3, 0xb9, 0x3c, 0x73, 0x67, 0x0e, 0xcc, 0x12,
+            },
+        },
+        .confirm_req[9] = {
+            .value = {
+                0x38, 0x1c, 0xf7, 0xf0, 0x31, 0xb1, 0x20, 0xa0,
+                0x51, 0x1c, 0xf1, 0xbd, 0x67, 0xfa, 0x84, 0xb4,
+            },
+        },
+        .confirm_rsp[9] = {
+            .value = {
+                0x8e, 0xa1, 0xc1, 0xf5, 0x39, 0xf0, 0x00, 0x49,
+                0xfb, 0xfc, 0xdc, 0xdf, 0x87, 0x0e, 0x96, 0x7e,
+            },
+        },
+        .random_req[9] = {
+            .value = {
+                0xd0, 0xed, 0x6c, 0x52, 0x20, 0x4b, 0x7b, 0x24,
+                0xdd, 0x28, 0x53, 0x2d, 0x71, 0x76, 0xfb, 0x8f,
+            },
+        },
+        .random_rsp[9] = {
+            .value = {
+                0xac, 0xd7, 0x34, 0x6b, 0x7b, 0x59, 0x9e, 0x9b,
+                0x5b, 0x37, 0xc6, 0x5c, 0x3e, 0x9d, 0xe2, 0x13,
+            },
+        },
+        .confirm_req[10] = {
+            .value = {
+                0xa6, 0xd7, 0xb6, 0xd6, 0xb5, 0x01, 0x4a, 0x02,
+                0x0d, 0xf0, 0x22, 0xcb, 0x68, 0xad, 0x7d, 0x73,
+            },
+        },
+        .confirm_rsp[10] = {
+            .value = {
+                0x01, 0xcc, 0x5f, 0xbc, 0xd0, 0x22, 0xa1, 0xb2,
+                0x71, 0x9d, 0x5c, 0x97, 0xfa, 0xd3, 0x6a, 0xc7,
+            },
+        },
+        .random_req[10] = {
+            .value = {
+                0x9f, 0x3a, 0x25, 0xc7, 0x9b, 0xb7, 0xb3, 0x51,
+                0xff, 0xde, 0x3b, 0x1c, 0xdd, 0xf5, 0x08, 0x21,
+            },
+        },
+        .random_rsp[10] = {
+            .value = {
+                0x75, 0x1e, 0x8d, 0xa4, 0x5b, 0x35, 0xec, 0xae,
+                0x17, 0xda, 0xa5, 0x43, 0x76, 0x3c, 0x6a, 0x67,
+            },
+        },
+        .confirm_req[11] = {
+            .value = {
+                0xfc, 0x0c, 0x3f, 0x86, 0xbf, 0xbe, 0x96, 0x0f,
+                0x99, 0x11, 0xa5, 0x36, 0x03, 0xcd, 0xbd, 0x7f,
+            },
+        },
+        .confirm_rsp[11] = {
+            .value = {
+                0x48, 0xcd, 0xc8, 0x89, 0xd6, 0x1c, 0x0d, 0xb1,
+                0x90, 0x01, 0x0e, 0xea, 0x80, 0xbc, 0xff, 0xb3,
+            },
+        },
+        .random_req[11] = {
+            .value = {
+                0xec, 0xfc, 0xe3, 0x0a, 0x97, 0xed, 0xe8, 0x51,
+                0x5d, 0x64, 0x3c, 0x73, 0x59, 0x2e, 0x62, 0xac,
+            },
+        },
+        .random_rsp[11] = {
+            .value = {
+                0x81, 0x74, 0x44, 0xca, 0xec, 0x38, 0x20, 0x6d,
+                0x52, 0x27, 0x49, 0x55, 0x61, 0x97, 0x01, 0x34,
+            },
+        },
+        .confirm_req[12] = {
+            .value = {
+                0x27, 0x8c, 0x88, 0x09, 0xcb, 0xd6, 0x45, 0xb7,
+                0x30, 0x4b, 0x1b, 0xcd, 0xc3, 0xac, 0x83, 0xd6,
+            },
+        },
+        .confirm_rsp[12] = {
+            .value = {
+                0xea, 0xbc, 0xe2, 0x43, 0xc8, 0xe0, 0x06, 0xd8,
+                0x7b, 0x3e, 0xa4, 0x55, 0x95, 0xa2, 0x23, 0x9b,
+            },
+        },
+        .random_req[12] = {
+            .value = {
+                0x68, 0x8f, 0xb6, 0x7b, 0x91, 0x0d, 0xc9, 0x30,
+                0xe7, 0xb7, 0xb7, 0x7a, 0x79, 0x29, 0x59, 0x7d,
+            },
+        },
+        .random_rsp[12] = {
+            .value = {
+                0xfd, 0xa1, 0x3d, 0xaf, 0x8d, 0xd2, 0xa0, 0x02,
+                0x82, 0x92, 0xeb, 0x2e, 0x4d, 0x6c, 0x8d, 0x69,
+            },
+        },
+        .confirm_req[13] = {
+            .value = {
+                0x6f, 0xa8, 0x20, 0x81, 0x1c, 0x4b, 0xe8, 0xe3,
+                0xdc, 0xea, 0x39, 0xbd, 0xfb, 0xbf, 0x79, 0xc4,
+            },
+        },
+        .confirm_rsp[13] = {
+            .value = {
+                0x2a, 0x09, 0xec, 0x32, 0x63, 0x3d, 0x38, 0x5d,
+                0x28, 0xb2, 0xb1, 0x62, 0xee, 0x6c, 0x0a, 0x6c,
+            },
+        },
+        .random_req[13] = {
+            .value = {
+                0x35, 0xb5, 0xc5, 0xc0, 0x74, 0x1f, 0x40, 0xac,
+                0x23, 0x52, 0x02, 0x68, 0xdf, 0x62, 0x73, 0xca,
+            },
+        },
+        .random_rsp[13] = {
+            .value = {
+                0xb8, 0xe2, 0x65, 0xdc, 0x22, 0xcb, 0xc2, 0xdb,
+                0x00, 0x60, 0x37, 0xe2, 0xcc, 0xc0, 0x41, 0x72,
+            },
+        },
+        .confirm_req[14] = {
+            .value = {
+                0x05, 0x0b, 0x5c, 0xa7, 0x58, 0x9c, 0x08, 0x81,
+                0x4a, 0x6b, 0x12, 0xae, 0xaa, 0xe5, 0x81, 0xf3,
+            },
+        },
+        .confirm_rsp[14] = {
+            .value = {
+                0xdd, 0x2b, 0xd1, 0xdd, 0x49, 0x92, 0xf3, 0xe1,
+                0xae, 0xf3, 0x6d, 0x89, 0xfd, 0x77, 0xf9, 0xaa,
+            },
+        },
+        .random_req[14] = {
+            .value = {
+                0xbc, 0x27, 0x29, 0x1b, 0xc4, 0xbc, 0x0e, 0x88,
+                0x95, 0x50, 0xf7, 0x92, 0xe6, 0xf7, 0x29, 0xe8,
+            },
+        },
+        .random_rsp[14] = {
+            .value = {
+                0xe7, 0x15, 0xfe, 0x53, 0x77, 0xd9, 0x98, 0x1d,
+                0x5b, 0x4e, 0x37, 0xa3, 0x1f, 0xc9, 0x47, 0x5d,
+            },
+        },
+        .confirm_req[15] = {
+            .value = {
+                0x75, 0x70, 0x9f, 0x84, 0x3e, 0x6b, 0x88, 0xcb,
+                0x66, 0xda, 0x8f, 0x79, 0xbc, 0xf8, 0x44, 0x99,
+            },
+        },
+        .confirm_rsp[15] = {
+            .value = {
+                0x13, 0xe4, 0x43, 0xb2, 0x61, 0x72, 0xfd, 0x33,
+                0xba, 0x87, 0x44, 0x27, 0x6f, 0x9a, 0xea, 0x19,
+            },
+        },
+        .random_req[15] = {
+            .value = {
+                0xda, 0x90, 0x59, 0x72, 0xed, 0x67, 0xde, 0x65,
+                0x21, 0xab, 0x7d, 0x9d, 0x72, 0x8c, 0x88, 0x8e,
+            },
+        },
+        .random_rsp[15] = {
+            .value = {
+                0x94, 0x92, 0x0f, 0x6c, 0x08, 0xde, 0xae, 0xa7,
+                0xfd, 0x36, 0xe0, 0x02, 0xc8, 0xfd, 0xdd, 0x69,
+            },
+        },
+        .confirm_req[16] = {
+            .value = {
+                0x35, 0x68, 0x1e, 0x80, 0x37, 0xc4, 0x91, 0xe8,
+                0xbf, 0x5e, 0x27, 0x0c, 0xaa, 0x8e, 0x85, 0x7b,
+            },
+        },
+        .confirm_rsp[16] = {
+            .value = {
+                0x1e, 0x42, 0x47, 0x29, 0x06, 0xdc, 0x2b, 0x45,
+                0xec, 0x95, 0x23, 0x31, 0x29, 0x24, 0x95, 0xf0,
+            },
+        },
+        .random_req[16] = {
+            .value = {
+                0x4e, 0x9f, 0x5d, 0x5a, 0x8f, 0xf7, 0x28, 0xc9,
+                0x29, 0x62, 0x0a, 0x67, 0x19, 0x17, 0x5e, 0xa7,
+            },
+        },
+        .random_rsp[16] = {
+            .value = {
+                0x7c, 0xd4, 0x13, 0xba, 0x27, 0x16, 0x39, 0xe7,
+                0xf0, 0xbf, 0xec, 0x1e, 0xe5, 0xcc, 0x20, 0x0b,
+            },
+        },
+        .confirm_req[17] = {
+            .value = {
+                0xb9, 0xcd, 0xf5, 0xf9, 0x2b, 0x4f, 0x6d, 0x08,
+                0x51, 0xe0, 0x92, 0x99, 0x15, 0xca, 0x15, 0x2a,
+            },
+        },
+        .confirm_rsp[17] = {
+            .value = {
+                0xb8, 0xf2, 0xf9, 0x61, 0x4f, 0x0e, 0xfd, 0x19,
+                0xcb, 0x5d, 0x7e, 0x93, 0x87, 0x7a, 0x0a, 0x6e,
+            },
+        },
+        .random_req[17] = {
+            .value = {
+                0x8b, 0xf8, 0xc8, 0xeb, 0xe5, 0xdb, 0xcf, 0xfe,
+                0x68, 0x70, 0x1f, 0xbe, 0x1e, 0x3c, 0x94, 0x7d,
+            },
+        },
+        .random_rsp[17] = {
+            .value = {
+                0x0d, 0xfc, 0x68, 0x2e, 0x50, 0x31, 0x9f, 0x60,
+                0xe6, 0x12, 0x72, 0x24, 0x7c, 0xad, 0xf7, 0x48,
+            },
+        },
+        .confirm_req[18] = {
+            .value = {
+                0x27, 0x68, 0x07, 0xaa, 0xa6, 0x33, 0x13, 0x49,
+                0x65, 0x4c, 0x80, 0x54, 0xfb, 0x69, 0xcb, 0x0e,
+            },
+        },
+        .confirm_rsp[18] = {
+            .value = {
+                0xc5, 0x8d, 0x45, 0x81, 0xb0, 0x5a, 0x69, 0x0f,
+                0x6c, 0x89, 0x0b, 0x60, 0x1e, 0x27, 0x9b, 0x9e,
+            },
+        },
+        .random_req[18] = {
+            .value = {
+                0x97, 0x46, 0x95, 0xb5, 0x86, 0xa1, 0xc1, 0x86,
+                0x3a, 0x8a, 0x1f, 0x29, 0x38, 0xe0, 0x69, 0x7f,
+            },
+        },
+        .random_rsp[18] = {
+            .value = {
+                0x8f, 0x0e, 0x56, 0x17, 0x1c, 0x4b, 0x78, 0x1f,
+                0xd1, 0x8a, 0x69, 0xbd, 0x65, 0xe3, 0xde, 0x3c,
+            },
+        },
+        .confirm_req[19] = {
+            .value = {
+                0xd2, 0xa5, 0x4e, 0x31, 0x34, 0xde, 0x68, 0xf0,
+                0x69, 0x88, 0x6f, 0x28, 0xa2, 0xdd, 0xba, 0xe1,
+            },
+        },
+        .confirm_rsp[19] = {
+            .value = {
+                0xf8, 0x5e, 0x4f, 0x4d, 0x56, 0xf6, 0x22, 0xc0,
+                0x57, 0x04, 0x04, 0x45, 0x24, 0x83, 0x09, 0x80,
+            },
+        },
+        .random_req[19] = {
+            .value = {
+                0x64, 0xe1, 0x5a, 0x76, 0x71, 0x94, 0xc0, 0x64,
+                0x2b, 0xea, 0x9d, 0xaf, 0xbd, 0x10, 0x25, 0x9b,
+            },
+        },
+        .random_rsp[19] = {
+            .value = {
+                0x1e, 0x38, 0x6e, 0x66, 0x55, 0xf1, 0x7f, 0x55,
+                0x7c, 0x00, 0xff, 0xad, 0x07, 0x13, 0x25, 0x97,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x98, 0xf1, 0x5a, 0x24, 0x81, 0x5d, 0xb5, 0xac,
+                0x04, 0x4e, 0x3a, 0x31, 0x8b, 0x7d, 0xf6, 0x09,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x1a, 0xb4, 0xf4, 0xf3, 0xc0, 0x5a, 0xf3, 0x13,
+                0x8d, 0x6e, 0x01, 0x16, 0x1e, 0x54, 0xf3, 0xe1,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x4b, 0x01, 0x33, 0x5f, 0x4b, 0xfe, 0x12, 0x8b,
+                0x9f, 0x81, 0x44, 0x78, 0x90, 0x03, 0x9e, 0x53,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0xd2, 0xa1, 0x2c, 0xf0, 0xa6, 0xeb, 0x97, 0x5e,
+                0xac, 0x53, 0xa1, 0x3d, 0x41, 0x40, 0x36, 0x2f,
+            },
+        },
+        .ltk = {
+            0xce, 0x28, 0x91, 0xa9, 0x36, 0xb7, 0xe1, 0xda,
+            0x3c, 0x66, 0xd9, 0x33, 0x3d, 0x03, 0x8e, 0x31,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_PASSKEY,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_DISP,
+                .passkey = 866744,
+            },
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: numeric comparison
+ * Initiator IO capabilities: 1
+ * Responder IO capabilities: 4
+ * Bonding: true
+ * Initiator address type: 0
+ * Responder address type: 0
+ * Initiator key distribution: 7
+ * Responder key distribution: 5
+ */
+TEST_CASE_SELF(ble_sm_sc_us_nc_iio1_rio4_b1_iat0_rat0_ik7_rk5)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {
+            0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+        },
+        .resp_id_addr = {
+            0xca, 0x61, 0xa0, 0x67, 0x94, 0xe0,
+        },
+        .pair_req = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x04,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x05,
+        },
+        .our_priv_key = {
+            0xb1, 0x6b, 0x4f, 0x81, 0xbc, 0xe3, 0x60, 0x9e,
+            0x00, 0x20, 0xf1, 0x73, 0x3e, 0xfb, 0xcc, 0x6e,
+            0x8c, 0xb6, 0xd2, 0x51, 0xd9, 0x36, 0x8a, 0x6d,
+            0xca, 0x8c, 0xd7, 0xbe, 0x96, 0x03, 0xdf, 0xd6,
+        },
+        .public_key_req = {
+            .x = {
+                0xe5, 0x0f, 0x02, 0x0a, 0x37, 0x90, 0x94, 0x5a,
+                0x06, 0x21, 0xf7, 0xbc, 0xd5, 0xbe, 0xb9, 0x24,
+                0x8a, 0x35, 0xfd, 0xf8, 0x5e, 0xe2, 0x70, 0xd5,
+                0x5a, 0xe8, 0xe7, 0xdd, 0x13, 0x90, 0xeb, 0xd4,
+            },
+            .y = {
+                0x41, 0xc8, 0x51, 0x1a, 0x25, 0x44, 0x01, 0x53,
+                0x42, 0x74, 0x07, 0x9c, 0x18, 0xe6, 0x3b, 0x8a,
+                0xce, 0x7a, 0x37, 0x1f, 0x18, 0x5c, 0x02, 0x7c,
+                0x67, 0x16, 0xf5, 0x30, 0x2b, 0x31, 0xa9, 0xc7,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x7c, 0x27, 0x39, 0xdc, 0x10, 0xfa, 0x57, 0x97,
+                0x4a, 0x18, 0xdc, 0x0e, 0xfc, 0x4b, 0xd0, 0xac,
+                0x3a, 0xa4, 0x4c, 0x65, 0xb5, 0xbe, 0x7b, 0xd8,
+                0xd1, 0xfd, 0x9d, 0xf8, 0xe3, 0x00, 0x4e, 0xf3,
+            },
+            .y = {
+                0xae, 0xfd, 0x8e, 0x93, 0xb4, 0xa9, 0x4d, 0xd3,
+                0xb6, 0xbd, 0x4c, 0x1d, 0xc1, 0x7e, 0x67, 0x57,
+                0x07, 0x10, 0x4e, 0xd0, 0x0f, 0x23, 0x23, 0xab,
+                0x09, 0x86, 0xc3, 0xb9, 0x63, 0x14, 0xe4, 0xe5,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x40, 0x35, 0x6d, 0xa1, 0x19, 0xa6, 0x8b, 0xff,
+                0x4f, 0x0c, 0x86, 0x7e, 0x95, 0x7c, 0xb8, 0xc1,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x95, 0x84, 0x0d, 0x2d, 0x7a, 0xfd, 0x5a, 0xa6,
+                0xea, 0xfd, 0x7b, 0xf0, 0x68, 0x95, 0xeb, 0x77,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x25, 0x41, 0xda, 0xdf, 0xdd, 0xca, 0xcd, 0x2e,
+                0x49, 0x79, 0xb0, 0xaa, 0x7a, 0x23, 0x28, 0x7f,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x49, 0xbb, 0x89, 0x9e, 0xa1, 0x10, 0x26, 0x7a,
+                0xe7, 0x42, 0x51, 0xcd, 0x1f, 0x3b, 0x22, 0x1d,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x59, 0x51, 0xc8, 0x7b, 0x4f, 0xae, 0xfe, 0xb8,
+                0x0c, 0x41, 0xe8, 0xe0, 0xf9, 0x4c, 0x2b, 0xc7,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0x37, 0x26, 0xc0, 0x79, 0x59, 0xcd, 0xb7, 0x0f,
+                0xa6, 0xd8, 0xe4, 0x02, 0xc9, 0xe6, 0x02, 0x71,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
+                0xbf, 0x5b, 0xdd, 0x34, 0xc0, 0x53, 0x1e, 0xb8,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x01, 0x01, 0x01, 0x07, 0x08, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x45, 0x69, 0x05, 0xe3, 0x0c, 0x9e, 0x01, 0xb3,
+                0xe8, 0xea, 0xa0, 0x5b, 0x70, 0xd9, 0x62, 0x0e,
+            },
+        },
+        .ltk = {
+            0xf5, 0x60, 0x02, 0x97, 0x2f, 0xbb, 0x3c, 0xe9,
+            0x97, 0xd7, 0xd5, 0x58, 0x04, 0x96, 0xa6, 0xe7,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_NUMCMP,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NUMCMP,
+                .numcmp_accept = 1,
+            },
+            .exp_numcmp = 344302,
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: just works
+ * Initiator IO capabilities: 3
+ * Responder IO capabilities: 3
+ * Bonding: true
+ * Initiator address type: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * Responder address type: BLE_ADDR_PUBLIC_ID
+ * Initiator key distribution: 7
+ * Responder key distribution: 7
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_jw_iio3_rio3_b1_iat2_rat2_ik7_rk7)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_addr_type = BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+        .init_id_addr = {
+            0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+        },
+        .init_rpa = {
+            0xd0, 0x8e, 0xf7, 0x42, 0x8c, 0x69,
+        },
+        .resp_addr_type = BLE_ADDR_PUBLIC_ID,
+        .resp_id_addr = {
+            0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+        },
+        .resp_rpa = {
+            0x1a, 0x6e, 0x83, 0x55, 0x5b, 0x5a,
+        },
+        .pair_req = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_rsp = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .our_priv_key = {
+            0xc5, 0x04, 0xc5, 0xf9, 0x28, 0x95, 0x78, 0x17,
+            0xd5, 0x97, 0x1d, 0x01, 0xbb, 0x2c, 0xcf, 0x77,
+            0x5c, 0x70, 0x52, 0xc6, 0x5e, 0x33, 0x2e, 0xe7,
+            0x79, 0x58, 0xc8, 0xf1, 0xc2, 0x2d, 0xb0, 0x61,
+        },
+        .public_key_req = {
+            .x = {
+                0x54, 0xd9, 0x8f, 0xeb, 0xc1, 0xbb, 0xe6, 0x74,
+                0x8a, 0x55, 0x3a, 0x80, 0x0e, 0xef, 0x90, 0xc9,
+                0xab, 0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62,
+                0x0d, 0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7,
+            },
+            .y = {
+                0x07, 0x33, 0x91, 0x40, 0xde, 0x25, 0xb4, 0x3d,
+                0x81, 0x2f, 0xd2, 0x41, 0x98, 0xe7, 0xaf, 0x0f,
+                0x5f, 0x17, 0x85, 0x1f, 0x75, 0x6e, 0xf4, 0x0e,
+                0x05, 0x19, 0x7f, 0x03, 0x9b, 0xf4, 0x41, 0x23,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1d, 0x44, 0x66, 0x0d, 0x3a, 0x03, 0x71, 0x17,
+                0xb3, 0x10, 0x2e, 0xf0, 0xd3, 0xf8, 0xa2, 0x6c,
+                0x1f, 0xfc, 0xbf, 0x02, 0x62, 0x6b, 0x11, 0x5a,
+                0x76, 0x5b, 0x30, 0x20, 0xb1, 0xef, 0xb3, 0x76,
+            },
+            .y = {
+                0xf1, 0x23, 0x63, 0x75, 0xfc, 0xb6, 0xc9, 0x32,
+                0xa4, 0x36, 0xbe, 0x18, 0xa0, 0x7d, 0x0b, 0x16,
+                0x65, 0x24, 0xd0, 0xe3, 0x74, 0x1b, 0x34, 0x1a,
+                0xf9, 0xe2, 0xcb, 0x30, 0x93, 0x88, 0xd7, 0xfa,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x1e, 0x07, 0x87, 0xb2, 0x54, 0x3a, 0x44, 0x6b,
+                0x97, 0x45, 0xa7, 0xa2, 0x36, 0xf4, 0x10, 0x42,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x99, 0xc4, 0xdf, 0x4a, 0x2f, 0x14, 0xd8, 0x11,
+                0xd3, 0x93, 0x53, 0xac, 0x64, 0xc8, 0x67, 0xe6,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0xc5, 0xb0, 0xf5, 0x2a, 0x65, 0x77, 0x05, 0xb8,
+                0xf7, 0x5b, 0xad, 0x4e, 0xa9, 0x9e, 0x79, 0x98,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xbb, 0x44, 0x9b, 0x1b, 0xcd, 0xfc, 0xdf, 0xff,
+                0xbb, 0x34, 0xb7, 0x3b, 0x3e, 0x30, 0xa1, 0x6e,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x58, 0x8f, 0xbe, 0xa2, 0x5f, 0xe3, 0x0a, 0xbc,
+                0x17, 0x0f, 0x3b, 0x23, 0x27, 0xa5, 0xfb, 0x25,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+            },
+        },
+        .sign_info_req = {
+            .sig_key = {
+                0xd3, 0x46, 0x86, 0xf7, 0xeb, 0x19, 0x0a, 0x18,
+                0x5a, 0xb2, 0xd0, 0x5b, 0x0f, 0x03, 0x64, 0x01,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+                0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x84, 0x91, 0x5d, 0x89, 0xf6, 0xf0, 0x01, 0x65,
+                0xed, 0xa9, 0xcc, 0x9b, 0xa4, 0xd4, 0x97, 0x86,
+            },
+        },
+        .ltk = {
+            0x4b, 0xb6, 0x1d, 0xd2, 0xba, 0xa4, 0x94, 0xe5,
+            0x78, 0xde, 0xee, 0x47, 0x7a, 0x95, 0x91, 0x1c,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_JW,
+        .authenticated = 0,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NONE,
+            },
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: numeric comparison
+ * Initiator IO capabilities: 1
+ * Responder IO capabilities: 1
+ * Bonding: true
+ * Initiator address type: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * Responder address type: BLE_ADDR_PUBLIC_ID
+ * Initiator key distribution: 3
+ * Responder key distribution: 3
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_nc_iio1_rio1_b1_iat2_rat2_ik3_rk3)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_addr_type = BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+        .init_id_addr = {
+            0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+        },
+        .init_rpa = {
+            0xc5, 0xf3, 0x5d, 0x83, 0xcd, 0x4a,
+        },
+        .resp_addr_type = BLE_ADDR_PUBLIC_ID,
+        .resp_id_addr = {
+            0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+        },
+        .resp_rpa = {
+            0x9f, 0x56, 0x57, 0x5e, 0x12, 0x65,
+        },
+        .pair_req = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x03,
+            .resp_key_dist = 0x03,
+        },
+        .pair_rsp = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x03,
+            .resp_key_dist = 0x03,
+        },
+        .our_priv_key = {
+            0xc5, 0x04, 0xc5, 0xf9, 0x28, 0x95, 0x78, 0x17,
+            0xd5, 0x97, 0x1d, 0x01, 0xbb, 0x2c, 0xcf, 0x77,
+            0x5c, 0x70, 0x52, 0xc6, 0x5e, 0x33, 0x2e, 0xe7,
+            0x79, 0x58, 0xc8, 0xf1, 0xc2, 0x2d, 0xb0, 0x61,
+        },
+        .public_key_req = {
+            .x = {
+                0x54, 0xd9, 0x8f, 0xeb, 0xc1, 0xbb, 0xe6, 0x74,
+                0x8a, 0x55, 0x3a, 0x80, 0x0e, 0xef, 0x90, 0xc9,
+                0xab, 0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62,
+                0x0d, 0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7,
+            },
+            .y = {
+                0x07, 0x33, 0x91, 0x40, 0xde, 0x25, 0xb4, 0x3d,
+                0x81, 0x2f, 0xd2, 0x41, 0x98, 0xe7, 0xaf, 0x0f,
+                0x5f, 0x17, 0x85, 0x1f, 0x75, 0x6e, 0xf4, 0x0e,
+                0x05, 0x19, 0x7f, 0x03, 0x9b, 0xf4, 0x41, 0x23,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1d, 0x44, 0x66, 0x0d, 0x3a, 0x03, 0x71, 0x17,
+                0xb3, 0x10, 0x2e, 0xf0, 0xd3, 0xf8, 0xa2, 0x6c,
+                0x1f, 0xfc, 0xbf, 0x02, 0x62, 0x6b, 0x11, 0x5a,
+                0x76, 0x5b, 0x30, 0x20, 0xb1, 0xef, 0xb3, 0x76,
+            },
+            .y = {
+                0xf1, 0x23, 0x63, 0x75, 0xfc, 0xb6, 0xc9, 0x32,
+                0xa4, 0x36, 0xbe, 0x18, 0xa0, 0x7d, 0x0b, 0x16,
+                0x65, 0x24, 0xd0, 0xe3, 0x74, 0x1b, 0x34, 0x1a,
+                0xf9, 0xe2, 0xcb, 0x30, 0x93, 0x88, 0xd7, 0xfa,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x39, 0xba, 0x86, 0x47, 0x06, 0x87, 0x14, 0xe4,
+                0x5c, 0x82, 0xe9, 0x6a, 0x80, 0xca, 0x87, 0xcd,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xce, 0xe2, 0xa3, 0x29, 0x8a, 0xc6, 0x76, 0x1d,
+                0xa2, 0xfd, 0xe0, 0x7f, 0x8c, 0xbe, 0xf8, 0x1d,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x3d, 0xac, 0xf0, 0xfe, 0x7c, 0x78, 0x73, 0x03,
+                0xe2, 0xb6, 0x59, 0x7e, 0x80, 0xb4, 0x69, 0x07,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xaa, 0x95, 0x9f, 0x33, 0x32, 0xa1, 0xbd, 0xf9,
+                0xef, 0xb9, 0x3d, 0xfb, 0x08, 0xd1, 0x28, 0xa0,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x3c, 0x10, 0x17, 0x76, 0x55, 0x65, 0x6f, 0x14,
+                0xfa, 0x80, 0xd3, 0x52, 0x04, 0x82, 0xe2, 0xf7,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0xd4, 0x66, 0x94, 0xc9, 0x96, 0xd0, 0x28, 0x96,
+                0x1c, 0xa1, 0x3b, 0xf7, 0x15, 0x95, 0x95, 0x43,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xb7, 0x98, 0xac, 0x85, 0xc4, 0x0a, 0x69, 0x8d,
+                0xa6, 0xaf, 0xf3, 0x1f, 0x63, 0x3c, 0xf2, 0x33,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+            },
+        },
+        .ltk = {
+            0x95, 0x46, 0xe6, 0x8e, 0x52, 0xcc, 0x05, 0xca,
+            0xf4, 0x59, 0x57, 0x54, 0x8c, 0x0d, 0x51, 0xfc,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_NUMCMP,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NUMCMP,
+                .numcmp_accept = 1,
+            },
+            .exp_numcmp = 70210,
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: peer
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 2
+ * Responder IO capabilities: 0
+ * Bonding: true
+ * Initiator address type: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * Responder address type: BLE_ADDR_PUBLIC_ID
+ * Initiator key distribution: 7
+ * Responder key distribution: 3
+ */
+TEST_CASE_SELF(ble_sm_sc_peer_pk_iio2_rio0_b1_iat2_rat2_ik7_rk3)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_addr_type = BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+        .init_id_addr = {
+            0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+        },
+        .init_rpa = {
+            0x6e, 0x56, 0x09, 0xef, 0x1e, 0x76,
+        },
+        .resp_addr_type = BLE_ADDR_PUBLIC_ID,
+        .resp_id_addr = {
+            0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+        },
+        .resp_rpa = {
+            0xb5, 0x29, 0xdf, 0xb4, 0x9b, 0x62,
+        },
+        .pair_req = {
+            .io_cap = 0x02,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x03,
+        },
+        .pair_rsp = {
+            .io_cap = 0x00,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x03,
+        },
+        .our_priv_key = {
+            0xc5, 0x04, 0xc5, 0xf9, 0x28, 0x95, 0x78, 0x17,
+            0xd5, 0x97, 0x1d, 0x01, 0xbb, 0x2c, 0xcf, 0x77,
+            0x5c, 0x70, 0x52, 0xc6, 0x5e, 0x33, 0x2e, 0xe7,
+            0x79, 0x58, 0xc8, 0xf1, 0xc2, 0x2d, 0xb0, 0x61,
+        },
+        .public_key_req = {
+            .x = {
+                0x54, 0xd9, 0x8f, 0xeb, 0xc1, 0xbb, 0xe6, 0x74,
+                0x8a, 0x55, 0x3a, 0x80, 0x0e, 0xef, 0x90, 0xc9,
+                0xab, 0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62,
+                0x0d, 0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7,
+            },
+            .y = {
+                0x07, 0x33, 0x91, 0x40, 0xde, 0x25, 0xb4, 0x3d,
+                0x81, 0x2f, 0xd2, 0x41, 0x98, 0xe7, 0xaf, 0x0f,
+                0x5f, 0x17, 0x85, 0x1f, 0x75, 0x6e, 0xf4, 0x0e,
+                0x05, 0x19, 0x7f, 0x03, 0x9b, 0xf4, 0x41, 0x23,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1d, 0x44, 0x66, 0x0d, 0x3a, 0x03, 0x71, 0x17,
+                0xb3, 0x10, 0x2e, 0xf0, 0xd3, 0xf8, 0xa2, 0x6c,
+                0x1f, 0xfc, 0xbf, 0x02, 0x62, 0x6b, 0x11, 0x5a,
+                0x76, 0x5b, 0x30, 0x20, 0xb1, 0xef, 0xb3, 0x76,
+            },
+            .y = {
+                0xf1, 0x23, 0x63, 0x75, 0xfc, 0xb6, 0xc9, 0x32,
+                0xa4, 0x36, 0xbe, 0x18, 0xa0, 0x7d, 0x0b, 0x16,
+                0x65, 0x24, 0xd0, 0xe3, 0x74, 0x1b, 0x34, 0x1a,
+                0xf9, 0xe2, 0xcb, 0x30, 0x93, 0x88, 0xd7, 0xfa,
+            },
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x12, 0xe3, 0x01, 0xd0, 0x30, 0x59, 0xca, 0xd9,
+                0x78, 0x0b, 0x45, 0x73, 0xb1, 0x7a, 0x4d, 0xca,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x47, 0x68, 0x16, 0x24, 0xd4, 0x07, 0x60, 0x6c,
+                0xa5, 0x47, 0x6f, 0x05, 0x78, 0x71, 0x3e, 0xa8,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x2a, 0x29, 0xa8, 0xef, 0x0b, 0x70, 0x5f, 0x1b,
+                0x81, 0x4d, 0x97, 0xff, 0xfb, 0x7f, 0x30, 0x90,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x12, 0x9e, 0x1d, 0x12, 0x11, 0x44, 0x36, 0x74,
+                0xa3, 0x0c, 0xea, 0x36, 0x4d, 0xdf, 0x2d, 0x5d,
+            },
+        },
+        .confirm_req[1] = {
+            .value = {
+                0x4d, 0x6a, 0x32, 0xfe, 0xe2, 0xa0, 0xdd, 0x92,
+                0x60, 0x5c, 0x82, 0x7f, 0xa6, 0xa6, 0x24, 0xd6,
+            },
+        },
+        .confirm_rsp[1] = {
+            .value = {
+                0xd5, 0x3e, 0xa7, 0xa0, 0xbf, 0x39, 0x8e, 0xfe,
+                0xfd, 0x73, 0x47, 0x4c, 0x92, 0x8b, 0x74, 0x06,
+            },
+        },
+        .random_req[1] = {
+            .value = {
+                0xc1, 0x88, 0xdf, 0xb0, 0x99, 0xbb, 0xbf, 0xed,
+                0xdc, 0x40, 0x66, 0x55, 0xbe, 0x91, 0x56, 0x9a,
+            },
+        },
+        .random_rsp[1] = {
+            .value = {
+                0xed, 0xed, 0x9a, 0x61, 0xb8, 0x21, 0x03, 0x77,
+                0xa6, 0xcf, 0x34, 0x65, 0x8c, 0x18, 0x82, 0x9f,
+            },
+        },
+        .confirm_req[2] = {
+            .value = {
+                0xdb, 0xea, 0x94, 0x29, 0xe4, 0x44, 0x7d, 0x7b,
+                0xd3, 0x16, 0x81, 0x8e, 0xaf, 0xe6, 0x9c, 0x85,
+            },
+        },
+        .confirm_rsp[2] = {
+            .value = {
+                0x3f, 0xdd, 0x54, 0x76, 0xab, 0x45, 0x7f, 0x53,
+                0x64, 0x6b, 0x37, 0xa6, 0xc7, 0xc6, 0x4a, 0x73,
+            },
+        },
+        .random_req[2] = {
+            .value = {
+                0x5a, 0xf1, 0xfb, 0xde, 0xb3, 0xbe, 0x6e, 0xac,
+                0x68, 0x51, 0x47, 0x8e, 0x0b, 0xcd, 0xc1, 0xa0,
+            },
+        },
+        .random_rsp[2] = {
+            .value = {
+                0x29, 0x0f, 0x5e, 0x83, 0x87, 0xca, 0xd3, 0x21,
+                0xa7, 0x7e, 0x3d, 0x78, 0x47, 0x54, 0xf8, 0xe4,
+            },
+        },
+        .confirm_req[3] = {
+            .value = {
+                0xca, 0x3e, 0xd5, 0xe3, 0x59, 0xb0, 0x5d, 0x1e,
+                0x0f, 0x4c, 0x95, 0x0f, 0x6a, 0x72, 0xcf, 0x25,
+            },
+        },
+        .confirm_rsp[3] = {
+            .value = {
+                0x2f, 0x4d, 0x06, 0x40, 0x09, 0x68, 0x68, 0x45,
+                0x87, 0x79, 0x78, 0x48, 0xda, 0xe4, 0xf5, 0xae,
+            },
+        },
+        .random_req[3] = {
+            .value = {
+                0x63, 0x5a, 0xee, 0x91, 0xe4, 0xf8, 0xe8, 0x69,
+                0xd1, 0x46, 0x18, 0x0d, 0xd2, 0x94, 0xd8, 0x20,
+            },
+        },
+        .random_rsp[3] = {
+            .value = {
+                0x76, 0x36, 0xf5, 0xc2, 0x41, 0xb6, 0x3c, 0x1f,
+                0x36, 0x19, 0x58, 0xce, 0x8f, 0x41, 0xeb, 0x8c,
+            },
+        },
+        .confirm_req[4] = {
+            .value = {
+                0x76, 0xfd, 0x84, 0x0f, 0x0f, 0x58, 0x70, 0x45,
+                0x41, 0x33, 0x5d, 0xce, 0xe5, 0xe2, 0x2f, 0x83,
+            },
+        },
+        .confirm_rsp[4] = {
+            .value = {
+                0x87, 0xcf, 0xdf, 0xa5, 0x60, 0x82, 0x4f, 0x09,
+                0x4c, 0x50, 0x24, 0xba, 0x91, 0x96, 0x0d, 0x65,
+            },
+        },
+        .random_req[4] = {
+            .value = {
+                0x67, 0xdb, 0x73, 0x1e, 0x57, 0x5c, 0xb7, 0x86,
+                0xf8, 0xaf, 0x58, 0xd8, 0x0f, 0x97, 0x47, 0xce,
+            },
+        },
+        .random_rsp[4] = {
+            .value = {
+                0xaa, 0x99, 0x90, 0x05, 0x11, 0xfc, 0xc2, 0xd9,
+                0xb8, 0xd6, 0x9d, 0xef, 0x86, 0x10, 0xcf, 0x26,
+            },
+        },
+        .confirm_req[5] = {
+            .value = {
+                0xfc, 0x22, 0xd9, 0x1f, 0x5f, 0x86, 0x25, 0xe7,
+                0x5e, 0x55, 0x48, 0x35, 0xec, 0x32, 0x37, 0x6d,
+            },
+        },
+        .confirm_rsp[5] = {
+            .value = {
+                0x98, 0xbc, 0x07, 0x72, 0xa2, 0xe7, 0xa7, 0x66,
+                0x64, 0xf7, 0x29, 0x3a, 0xaf, 0x52, 0x18, 0x04,
+            },
+        },
+        .random_req[5] = {
+            .value = {
+                0xd3, 0x36, 0xb9, 0x69, 0x6a, 0x6d, 0x55, 0xbc,
+                0x82, 0xdf, 0x1c, 0x04, 0xa7, 0xd5, 0x00, 0x68,
+            },
+        },
+        .random_rsp[5] = {
+            .value = {
+                0xb9, 0x03, 0xbf, 0xd9, 0x86, 0x5a, 0x1a, 0xb4,
+                0xdc, 0xe6, 0x8f, 0x9b, 0xa4, 0xa8, 0x2a, 0x12,
+            },
+        },
+        .confirm_req[6] = {
+            .value = {
+                0xfe, 0x14, 0xab, 0x1c, 0xfd, 0x36, 0x64, 0x38,
+                0xc1, 0xf8, 0xdd, 0xcd, 0xf4, 0x77, 0xa1, 0xb8,
+            },
+        },
+        .confirm_rsp[6] = {
+            .value = {
+                0x2e, 0x70, 0x54, 0xdc, 0xa6, 0xae, 0xb2, 0xcd,
+                0x4a, 0x26, 0x97, 0xf8, 0xbf, 0xb4, 0xb4, 0x52,
+            },
+        },
+        .random_req[6] = {
+            .value = {
+                0x1e, 0x27, 0x73, 0x94, 0x44, 0xfc, 0xd4, 0x44,
+                0xbf, 0x5b, 0x7d, 0x5d, 0x6d, 0x13, 0x68, 0xb1,
+            },
+        },
+        .random_rsp[6] = {
+            .value = {
+                0xeb, 0xfd, 0x0b, 0xa1, 0x7b, 0xda, 0x61, 0xdc,
+                0x6d, 0xe4, 0x3b, 0x51, 0xa7, 0x09, 0x29, 0x6d,
+            },
+        },
+        .confirm_req[7] = {
+            .value = {
+                0x38, 0x2b, 0x23, 0xb9, 0x18, 0x2d, 0xb9, 0x0b,
+                0xe7, 0x4d, 0x20, 0x83, 0xab, 0x17, 0xfd, 0x88,
+            },
+        },
+        .confirm_rsp[7] = {
+            .value = {
+                0x65, 0x60, 0x85, 0xef, 0x0e, 0x9a, 0x23, 0x96,
+                0xe7, 0xa9, 0xee, 0xba, 0x9e, 0x48, 0xb9, 0x1c,
+            },
+        },
+        .random_req[7] = {
+            .value = {
+                0x8b, 0xa8, 0x7a, 0x33, 0x15, 0x1e, 0xa7, 0x78,
+                0x27, 0x01, 0x3e, 0x90, 0x43, 0x47, 0x5a, 0x9d,
+            },
+        },
+        .random_rsp[7] = {
+            .value = {
+                0x76, 0xf1, 0x21, 0x67, 0x94, 0x20, 0x6f, 0xc7,
+                0x84, 0xc8, 0xdb, 0x07, 0xdb, 0x77, 0xdd, 0x50,
+            },
+        },
+        .confirm_req[8] = {
+            .value = {
+                0x4e, 0x7f, 0x83, 0x8e, 0xa6, 0x28, 0xaa, 0x46,
+                0xa2, 0x69, 0x95, 0x3b, 0xf0, 0x71, 0x14, 0x24,
+            },
+        },
+        .confirm_rsp[8] = {
+            .value = {
+                0x93, 0x0b, 0x4d, 0xbe, 0x49, 0x36, 0xa0, 0x26,
+                0xe9, 0x18, 0x4e, 0xc8, 0x19, 0x59, 0xc1, 0x7d,
+            },
+        },
+        .random_req[8] = {
+            .value = {
+                0x11, 0xa9, 0xce, 0x26, 0x0e, 0x2f, 0x11, 0x0e,
+                0xc1, 0xbd, 0x68, 0x80, 0xc8, 0xf8, 0x41, 0x65,
+            },
+        },
+        .random_rsp[8] = {
+            .value = {
+                0xb6, 0x3d, 0x6b, 0x62, 0xb5, 0x37, 0x31, 0x28,
+                0x79, 0xc4, 0xe2, 0x62, 0xbb, 0x63, 0xf9, 0x91,
+            },
+        },
+        .confirm_req[9] = {
+            .value = {
+                0x5f, 0x55, 0xb5, 0xa4, 0x80, 0xa8, 0x54, 0x47,
+                0xa7, 0x79, 0x87, 0x12, 0x2e, 0x44, 0x92, 0x42,
+            },
+        },
+        .confirm_rsp[9] = {
+            .value = {
+                0x01, 0x69, 0xa2, 0xac, 0xd6, 0x62, 0x8a, 0x64,
+                0xa2, 0x0b, 0xd0, 0xb4, 0x0e, 0x68, 0xe0, 0x88,
+            },
+        },
+        .random_req[9] = {
+            .value = {
+                0x75, 0x1e, 0x56, 0xd0, 0xcb, 0x06, 0xfd, 0x51,
+                0x55, 0xae, 0x77, 0xa4, 0xf2, 0xe7, 0x86, 0x3c,
+            },
+        },
+        .random_rsp[9] = {
+            .value = {
+                0xff, 0xab, 0x8a, 0x7d, 0xb7, 0x40, 0xe5, 0x07,
+                0xfe, 0x8f, 0x74, 0xdb, 0x2c, 0x35, 0x35, 0x12,
+            },
+        },
+        .confirm_req[10] = {
+            .value = {
+                0x1f, 0x2a, 0xed, 0xcd, 0x6b, 0x87, 0xea, 0xa2,
+                0xf8, 0xd8, 0xad, 0x04, 0x23, 0xc7, 0x5d, 0x47,
+            },
+        },
+        .confirm_rsp[10] = {
+            .value = {
+                0x5b, 0x18, 0x2d, 0x96, 0x3b, 0xf6, 0xdc, 0x82,
+                0x3b, 0xfa, 0xc9, 0x81, 0xc7, 0x33, 0xa0, 0x07,
+            },
+        },
+        .random_req[10] = {
+            .value = {
+                0xd1, 0x3a, 0x82, 0xce, 0x31, 0x75, 0xa2, 0xbf,
+                0x6f, 0x12, 0xf2, 0xac, 0xf6, 0xcc, 0xea, 0x34,
+            },
+        },
+        .random_rsp[10] = {
+            .value = {
+                0xcf, 0x11, 0x3d, 0x44, 0x10, 0x0d, 0x26, 0x32,
+                0xa5, 0x61, 0x13, 0xfd, 0xb8, 0xed, 0x31, 0x53,
+            },
+        },
+        .confirm_req[11] = {
+            .value = {
+                0x67, 0x14, 0x8a, 0xf6, 0xc8, 0xb8, 0x73, 0x6b,
+                0xb2, 0xec, 0xa9, 0x61, 0xaa, 0xc0, 0xc9, 0x28,
+            },
+        },
+        .confirm_rsp[11] = {
+            .value = {
+                0xa5, 0xbf, 0x00, 0x07, 0x48, 0xff, 0x30, 0x36,
+                0x20, 0x83, 0xd7, 0xd6, 0xd0, 0x90, 0x46, 0x03,
+            },
+        },
+        .random_req[11] = {
+            .value = {
+                0x75, 0x99, 0x9a, 0xa3, 0xad, 0x9a, 0xe5, 0x9d,
+                0x2f, 0x21, 0xdb, 0x72, 0x2f, 0xaf, 0xb8, 0x79,
+            },
+        },
+        .random_rsp[11] = {
+            .value = {
+                0xa3, 0xb7, 0xb7, 0x46, 0x39, 0x99, 0xc2, 0x82,
+                0xe9, 0x31, 0x8d, 0xc2, 0x28, 0x1b, 0x86, 0x91,
+            },
+        },
+        .confirm_req[12] = {
+            .value = {
+                0x46, 0x2f, 0xc8, 0x0e, 0x2c, 0x70, 0x3a, 0xdb,
+                0x25, 0x2f, 0xce, 0xe6, 0x15, 0x1f, 0x9a, 0x06,
+            },
+        },
+        .confirm_rsp[12] = {
+            .value = {
+                0x9a, 0xa4, 0xe0, 0x03, 0x3a, 0xb5, 0x43, 0x75,
+                0x8e, 0x93, 0x35, 0x25, 0xe6, 0x5e, 0x9d, 0x7f,
+            },
+        },
+        .random_req[12] = {
+            .value = {
+                0x1f, 0x01, 0x32, 0x56, 0x64, 0x45, 0xc5, 0x20,
+                0xd4, 0xad, 0x13, 0x8f, 0xbe, 0x82, 0xc8, 0x01,
+            },
+        },
+        .random_rsp[12] = {
+            .value = {
+                0xd4, 0x3f, 0xa4, 0xc9, 0xe9, 0x2e, 0x62, 0x77,
+                0x4e, 0x21, 0x55, 0xd8, 0xde, 0x31, 0xf5, 0xea,
+            },
+        },
+        .confirm_req[13] = {
+            .value = {
+                0x4e, 0x48, 0x88, 0x4e, 0x4f, 0x74, 0x7e, 0xec,
+                0x99, 0x5d, 0xb1, 0xcb, 0x84, 0x88, 0x80, 0xe9,
+            },
+        },
+        .confirm_rsp[13] = {
+            .value = {
+                0x1a, 0x84, 0xfa, 0x2f, 0xd7, 0x3c, 0x5f, 0xee,
+                0x3e, 0x81, 0xc0, 0x4b, 0x35, 0x4b, 0x7e, 0x98,
+            },
+        },
+        .random_req[13] = {
+            .value = {
+                0xe3, 0x3a, 0xc5, 0x2f, 0x9f, 0x91, 0x93, 0xfb,
+                0xcb, 0xd8, 0x53, 0x63, 0xab, 0xc4, 0xa5, 0x85,
+            },
+        },
+        .random_rsp[13] = {
+            .value = {
+                0xa0, 0xcf, 0xad, 0x30, 0x2d, 0xec, 0xea, 0x81,
+                0xfd, 0x7f, 0xcf, 0x7c, 0x70, 0xc9, 0x89, 0x7b,
+            },
+        },
+        .confirm_req[14] = {
+            .value = {
+                0xe1, 0x64, 0x22, 0x19, 0x41, 0x44, 0x37, 0x2b,
+                0x92, 0x60, 0xa4, 0x1f, 0xd6, 0x53, 0xe0, 0xa0,
+            },
+        },
+        .confirm_rsp[14] = {
+            .value = {
+                0x08, 0xfa, 0xa4, 0xf8, 0x04, 0x08, 0xb8, 0x9f,
+                0x61, 0xb5, 0x68, 0xaf, 0x31, 0x12, 0x8d, 0x3f,
+            },
+        },
+        .random_req[14] = {
+            .value = {
+                0xad, 0x76, 0xc3, 0x1a, 0x4c, 0x64, 0x2c, 0x11,
+                0x5e, 0x48, 0x6d, 0x41, 0xf5, 0x77, 0xc2, 0x40,
+            },
+        },
+        .random_rsp[14] = {
+            .value = {
+                0x1b, 0xec, 0x78, 0x2b, 0xd9, 0xbe, 0x93, 0xbd,
+                0x0b, 0x03, 0xf1, 0xd8, 0x31, 0xe8, 0x60, 0x67,
+            },
+        },
+        .confirm_req[15] = {
+            .value = {
+                0x5e, 0x22, 0x44, 0x09, 0x97, 0xf9, 0xc5, 0xc7,
+                0x23, 0xc7, 0x74, 0x51, 0xe5, 0x9d, 0x5c, 0xed,
+            },
+        },
+        .confirm_rsp[15] = {
+            .value = {
+                0xfe, 0xb2, 0x90, 0xa7, 0x06, 0xaf, 0xdd, 0x6a,
+                0x83, 0x26, 0x3c, 0x78, 0x66, 0xe0, 0x9d, 0xd9,
+            },
+        },
+        .random_req[15] = {
+            .value = {
+                0xb2, 0xa0, 0x75, 0x6f, 0x77, 0xc1, 0x0b, 0x4e,
+                0x99, 0xfa, 0x9a, 0x02, 0xf6, 0xe4, 0x66, 0x27,
+            },
+        },
+        .random_rsp[15] = {
+            .value = {
+                0xf9, 0xdd, 0x69, 0xae, 0xc8, 0x66, 0xa9, 0xab,
+                0xb8, 0x01, 0x38, 0xc3, 0x2a, 0x6b, 0x94, 0x66,
+            },
+        },
+        .confirm_req[16] = {
+            .value = {
+                0x17, 0xc9, 0xf7, 0x2d, 0xe6, 0xb7, 0x99, 0x77,
+                0x65, 0xf7, 0x62, 0xc8, 0x0d, 0x7d, 0xbd, 0x81,
+            },
+        },
+        .confirm_rsp[16] = {
+            .value = {
+                0x39, 0xef, 0xbf, 0x39, 0xfa, 0x79, 0xc3, 0x7b,
+                0x71, 0x40, 0x3c, 0x1f, 0x67, 0xe5, 0x60, 0xe5,
+            },
+        },
+        .random_req[16] = {
+            .value = {
+                0x32, 0xab, 0x8b, 0xed, 0x90, 0x04, 0x5e, 0x17,
+                0xd2, 0x5e, 0xa8, 0x91, 0xf7, 0x77, 0xe3, 0xd7,
+            },
+        },
+        .random_rsp[16] = {
+            .value = {
+                0x6c, 0xc7, 0x14, 0x13, 0xdf, 0xfb, 0xc6, 0xed,
+                0xa3, 0x9c, 0xa7, 0x90, 0xae, 0x4c, 0x61, 0x47,
+            },
+        },
+        .confirm_req[17] = {
+            .value = {
+                0xc5, 0x17, 0x07, 0x35, 0x34, 0xbf, 0xc1, 0x4d,
+                0xc4, 0x57, 0xc0, 0xd9, 0xfd, 0xe9, 0x10, 0x08,
+            },
+        },
+        .confirm_rsp[17] = {
+            .value = {
+                0xbb, 0xcf, 0x41, 0xd2, 0x94, 0xea, 0xbe, 0x2f,
+                0xde, 0xb2, 0xb4, 0x20, 0x72, 0x1c, 0xf8, 0x35,
+            },
+        },
+        .random_req[17] = {
+            .value = {
+                0x59, 0x20, 0xb5, 0xdc, 0xaf, 0xc3, 0x8b, 0x32,
+                0xe6, 0x40, 0x0f, 0x02, 0x67, 0x45, 0x49, 0x1f,
+            },
+        },
+        .random_rsp[17] = {
+            .value = {
+                0xf5, 0x95, 0x60, 0x4c, 0x5f, 0x39, 0x54, 0xbf,
+                0x62, 0x9e, 0x85, 0xca, 0x31, 0x9a, 0x95, 0xee,
+            },
+        },
+        .confirm_req[18] = {
+            .value = {
+                0x36, 0x50, 0x78, 0x6b, 0x0f, 0x11, 0xe3, 0xa9,
+                0x79, 0x3a, 0xa6, 0x9d, 0xd4, 0x8b, 0x13, 0x3f,
+            },
+        },
+        .confirm_rsp[18] = {
+            .value = {
+                0xa5, 0x34, 0x5d, 0x5e, 0x43, 0x01, 0xf2, 0xe1,
+                0x3f, 0xf2, 0x1c, 0x8b, 0x13, 0xf7, 0x17, 0x3e,
+            },
+        },
+        .random_req[18] = {
+            .value = {
+                0x77, 0xa1, 0xbe, 0xbf, 0x49, 0xb8, 0x74, 0x73,
+                0x47, 0x78, 0x2a, 0xf8, 0x66, 0x6b, 0xff, 0xd2,
+            },
+        },
+        .random_rsp[18] = {
+            .value = {
+                0xa2, 0x05, 0x69, 0x65, 0x3f, 0xd4, 0xb4, 0xcd,
+                0xed, 0x8c, 0x36, 0x6d, 0x51, 0x6a, 0xbb, 0xef,
+            },
+        },
+        .confirm_req[19] = {
+            .value = {
+                0xda, 0xd8, 0x96, 0xfd, 0x1c, 0x0d, 0x1e, 0x56,
+                0xe2, 0x62, 0xed, 0x18, 0x4b, 0xd3, 0x46, 0x48,
+            },
+        },
+        .confirm_rsp[19] = {
+            .value = {
+                0xeb, 0x79, 0x5e, 0x52, 0x70, 0x25, 0xa7, 0x41,
+                0x33, 0xfa, 0xac, 0xd3, 0x27, 0x35, 0xfc, 0x5f,
+            },
+        },
+        .random_req[19] = {
+            .value = {
+                0xa8, 0x9c, 0xb9, 0xcd, 0x13, 0xb8, 0xdd, 0xd2,
+                0x09, 0xd6, 0xc8, 0x12, 0xc3, 0x69, 0x9a, 0x64,
+            },
+        },
+        .random_rsp[19] = {
+            .value = {
+                0x06, 0xe3, 0x8a, 0xef, 0xe4, 0x42, 0xae, 0x86,
+                0xef, 0x58, 0x80, 0xe8, 0xe3, 0xa2, 0x09, 0x44,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x6f, 0xa5, 0x37, 0x06, 0x4a, 0x89, 0x98, 0x39,
+                0xf6, 0x69, 0x48, 0x56, 0x17, 0x6d, 0x44, 0x7c,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x82, 0x48, 0xd4, 0x9e, 0xb8, 0x3c, 0xb4, 0xdc,
+                0x44, 0xcb, 0x19, 0xdb, 0xcb, 0xa2, 0x00, 0x5d,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62, 0x0d,
+                0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7, 0x07,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xda, 0x6b, 0x27, 0xa0, 0xac, 0x71, 0xf0, 0xc3,
+                0x75, 0x51, 0xf6, 0x21, 0x94, 0xec, 0x81, 0x92,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x49, 0x5b, 0x11, 0xb3, 0x4c, 0x1a, 0x23, 0x5c,
+                0x61, 0x4f, 0xe3, 0x08, 0xf9, 0x47, 0x8b, 0xdc,
+            },
+        },
+        .ltk = {
+            0x5a, 0x49, 0x28, 0xf0, 0x11, 0x3b, 0x6f, 0x6b,
+            0x3a, 0x69, 0x6d, 0xdd, 0xb2, 0xe5, 0xa8, 0x97,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_PASSKEY,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_DISP,
+                .passkey = 4915,
+            },
+        },
+    };
+    ble_sm_test_util_peer_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: just works
+ * Initiator IO capabilities: 3
+ * Responder IO capabilities: 3
+ * Bonding: true
+ * Initiator address type: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * Responder address type: BLE_ADDR_PUBLIC_ID
+ * Initiator key distribution: 3
+ * Responder key distribution: 3
+ */
+TEST_CASE_SELF(ble_sm_sc_us_jw_iio3_rio3_b1_iat2_rat2_ik3_rk3)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_addr_type = BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+        .init_id_addr = {
+            0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+        },
+        .init_rpa = {
+            0x46, 0x85, 0x37, 0x90, 0x86, 0x58,
+        },
+        .resp_addr_type = BLE_ADDR_PUBLIC_ID,
+        .resp_id_addr = {
+            0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+        },
+        .resp_rpa = {
+            0x6d, 0x59, 0x7d, 0xa9, 0x87, 0x74,
+        },
+        .pair_req = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x03,
+            .resp_key_dist = 0x03,
+        },
+        .pair_rsp = {
+            .io_cap = 0x03,
+            .oob_data_flag = 0x00,
+            .authreq = 0x09,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x03,
+            .resp_key_dist = 0x03,
+        },
+        .our_priv_key = {
+            0xdb, 0x24, 0x2e, 0x91, 0xda, 0xaa, 0x33, 0x33,
+            0x23, 0xa2, 0x1e, 0xbe, 0x06, 0x69, 0xdb, 0xad,
+            0xa9, 0x2a, 0x91, 0xb1, 0x24, 0x0a, 0xc7, 0xaf,
+            0x50, 0x0c, 0x65, 0x5b, 0x97, 0x1e, 0x12, 0x10,
+        },
+        .public_key_req = {
+            .x = {
+                0x54, 0xd9, 0x8f, 0xeb, 0xc1, 0xbb, 0xe6, 0x74,
+                0x8a, 0x55, 0x3a, 0x80, 0x0e, 0xef, 0x90, 0xc9,
+                0xab, 0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62,
+                0x0d, 0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7,
+            },
+            .y = {
+                0x07, 0x33, 0x91, 0x40, 0xde, 0x25, 0xb4, 0x3d,
+                0x81, 0x2f, 0xd2, 0x41, 0x98, 0xe7, 0xaf, 0x0f,
+                0x5f, 0x17, 0x85, 0x1f, 0x75, 0x6e, 0xf4, 0x0e,
+                0x05, 0x19, 0x7f, 0x03, 0x9b, 0xf4, 0x41, 0x23,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1d, 0x44, 0x66, 0x0d, 0x3a, 0x03, 0x71, 0x17,
+                0xb3, 0x10, 0x2e, 0xf0, 0xd3, 0xf8, 0xa2, 0x6c,
+                0x1f, 0xfc, 0xbf, 0x02, 0x62, 0x6b, 0x11, 0x5a,
+                0x76, 0x5b, 0x30, 0x20, 0xb1, 0xef, 0xb3, 0x76,
+            },
+            .y = {
+                0xf1, 0x23, 0x63, 0x75, 0xfc, 0xb6, 0xc9, 0x32,
+                0xa4, 0x36, 0xbe, 0x18, 0xa0, 0x7d, 0x0b, 0x16,
+                0x65, 0x24, 0xd0, 0xe3, 0x74, 0x1b, 0x34, 0x1a,
+                0xf9, 0xe2, 0xcb, 0x30, 0x93, 0x88, 0xd7, 0xfa,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x32, 0x5b, 0xee, 0x46, 0x42, 0x63, 0xca, 0x86,
+                0x2d, 0xe7, 0xd2, 0x75, 0x23, 0x7b, 0x4d, 0x59,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xd4, 0x66, 0x94, 0xc9, 0x96, 0xd0, 0x28, 0x96,
+                0x1c, 0xa1, 0x3b, 0xf7, 0x15, 0x95, 0x95, 0x43,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0xb7, 0x98, 0xac, 0x85, 0xc4, 0x0a, 0x69, 0x8d,
+                0xa6, 0xaf, 0xf3, 0x1f, 0x63, 0x3c, 0xf2, 0x33,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x1a, 0xc7, 0x0b, 0xfe, 0xc0, 0x55, 0xc3, 0xdb,
+                0x94, 0x00, 0x89, 0x4f, 0x0e, 0x64, 0x05, 0xcd,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0xf2, 0x45, 0x41, 0xc0, 0xba, 0x8d, 0x58, 0xec,
+                0x61, 0xfb, 0x48, 0x71, 0xb4, 0x0e, 0x7b, 0x19,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+                0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+            },
+        },
+        .ltk = {
+            0x8b, 0xb6, 0xf6, 0x5a, 0x52, 0x7b, 0xb8, 0xf4,
+            0xb8, 0x4c, 0xe7, 0x60, 0x4f, 0x0b, 0x88, 0xfe,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_JW,
+        .authenticated = 0,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NONE,
+            },
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: numeric comparison
+ * Initiator IO capabilities: 1
+ * Responder IO capabilities: 1
+ * Bonding: true
+ * Initiator address type: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * Responder address type: BLE_ADDR_PUBLIC_ID
+ * Initiator key distribution: 3
+ * Responder key distribution: 3
+ */
+TEST_CASE_SELF(ble_sm_sc_us_nc_iio1_rio1_b1_iat2_rat2_ik3_rk3)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_addr_type = BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+        .init_id_addr = {
+            0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+        },
+        .init_rpa = {
+            0xc5, 0xf3, 0x5d, 0x83, 0xcd, 0x4a,
+        },
+        .resp_addr_type = BLE_ADDR_PUBLIC_ID,
+        .resp_id_addr = {
+            0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+        },
+        .resp_rpa = {
+            0x9f, 0x56, 0x57, 0x5e, 0x12, 0x65,
+        },
+        .pair_req = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x03,
+            .resp_key_dist = 0x03,
+        },
+        .pair_rsp = {
+            .io_cap = 0x01,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x03,
+            .resp_key_dist = 0x03,
+        },
+        .our_priv_key = {
+            0xdb, 0x24, 0x2e, 0x91, 0xda, 0xaa, 0x33, 0x33,
+            0x23, 0xa2, 0x1e, 0xbe, 0x06, 0x69, 0xdb, 0xad,
+            0xa9, 0x2a, 0x91, 0xb1, 0x24, 0x0a, 0xc7, 0xaf,
+            0x50, 0x0c, 0x65, 0x5b, 0x97, 0x1e, 0x12, 0x10,
+        },
+        .public_key_req = {
+            .x = {
+                0x54, 0xd9, 0x8f, 0xeb, 0xc1, 0xbb, 0xe6, 0x74,
+                0x8a, 0x55, 0x3a, 0x80, 0x0e, 0xef, 0x90, 0xc9,
+                0xab, 0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62,
+                0x0d, 0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7,
+            },
+            .y = {
+                0x07, 0x33, 0x91, 0x40, 0xde, 0x25, 0xb4, 0x3d,
+                0x81, 0x2f, 0xd2, 0x41, 0x98, 0xe7, 0xaf, 0x0f,
+                0x5f, 0x17, 0x85, 0x1f, 0x75, 0x6e, 0xf4, 0x0e,
+                0x05, 0x19, 0x7f, 0x03, 0x9b, 0xf4, 0x41, 0x23,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1d, 0x44, 0x66, 0x0d, 0x3a, 0x03, 0x71, 0x17,
+                0xb3, 0x10, 0x2e, 0xf0, 0xd3, 0xf8, 0xa2, 0x6c,
+                0x1f, 0xfc, 0xbf, 0x02, 0x62, 0x6b, 0x11, 0x5a,
+                0x76, 0x5b, 0x30, 0x20, 0xb1, 0xef, 0xb3, 0x76,
+            },
+            .y = {
+                0xf1, 0x23, 0x63, 0x75, 0xfc, 0xb6, 0xc9, 0x32,
+                0xa4, 0x36, 0xbe, 0x18, 0xa0, 0x7d, 0x0b, 0x16,
+                0x65, 0x24, 0xd0, 0xe3, 0x74, 0x1b, 0x34, 0x1a,
+                0xf9, 0xe2, 0xcb, 0x30, 0x93, 0x88, 0xd7, 0xfa,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x39, 0xba, 0x86, 0x47, 0x06, 0x87, 0x14, 0xe4,
+                0x5c, 0x82, 0xe9, 0x6a, 0x80, 0xca, 0x87, 0xcd,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0xce, 0xe2, 0xa3, 0x29, 0x8a, 0xc6, 0x76, 0x1d,
+                0xa2, 0xfd, 0xe0, 0x7f, 0x8c, 0xbe, 0xf8, 0x1d,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x3d, 0xac, 0xf0, 0xfe, 0x7c, 0x78, 0x73, 0x03,
+                0xe2, 0xb6, 0x59, 0x7e, 0x80, 0xb4, 0x69, 0x07,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0xaa, 0x95, 0x9f, 0x33, 0x32, 0xa1, 0xbd, 0xf9,
+                0xef, 0xb9, 0x3d, 0xfb, 0x08, 0xd1, 0x28, 0xa0,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x3c, 0x10, 0x17, 0x76, 0x55, 0x65, 0x6f, 0x14,
+                0xfa, 0x80, 0xd3, 0x52, 0x04, 0x82, 0xe2, 0xf7,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0xd4, 0x66, 0x94, 0xc9, 0x96, 0xd0, 0x28, 0x96,
+                0x1c, 0xa1, 0x3b, 0xf7, 0x15, 0x95, 0x95, 0x43,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xb7, 0x98, 0xac, 0x85, 0xc4, 0x0a, 0x69, 0x8d,
+                0xa6, 0xaf, 0xf3, 0x1f, 0x63, 0x3c, 0xf2, 0x33,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+            },
+        },
+        .ltk = {
+            0x95, 0x46, 0xe6, 0x8e, 0x52, 0xcc, 0x05, 0xca,
+            0xf4, 0x59, 0x57, 0x54, 0x8c, 0x0d, 0x51, 0xfc,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_NUMCMP,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_NUMCMP,
+                .numcmp_accept = 1,
+            },
+            .exp_numcmp = 70210,
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/**
+ * Secure connections pairing
+ * Master: us
+ * Pair algorithm: passkey entry
+ * Initiator IO capabilities: 2
+ * Responder IO capabilities: 0
+ * Bonding: true
+ * Initiator address type: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * Responder address type: BLE_ADDR_PUBLIC_ID
+ * Initiator key distribution: 7
+ * Responder key distribution: 3
+ */
+TEST_CASE_SELF(ble_sm_sc_us_pk_iio2_rio0_b1_iat2_rat2_ik7_rk3)
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+        .init_addr_type = BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT,
+        .init_id_addr = {
+            0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+        },
+        .init_rpa = {
+            0x6e, 0x56, 0x09, 0xef, 0x1e, 0x76,
+        },
+        .resp_addr_type = BLE_ADDR_PUBLIC_ID,
+        .resp_id_addr = {
+            0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+        },
+        .resp_rpa = {
+            0xb5, 0x29, 0xdf, 0xb4, 0x9b, 0x62,
+        },
+        .pair_req = {
+            .io_cap = 0x02,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x03,
+        },
+        .pair_rsp = {
+            .io_cap = 0x00,
+            .oob_data_flag = 0x00,
+            .authreq = 0x0d,
+            .max_enc_key_size = 0x10,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x03,
+        },
+        .our_priv_key = {
+            0xdb, 0x24, 0x2e, 0x91, 0xda, 0xaa, 0x33, 0x33,
+            0x23, 0xa2, 0x1e, 0xbe, 0x06, 0x69, 0xdb, 0xad,
+            0xa9, 0x2a, 0x91, 0xb1, 0x24, 0x0a, 0xc7, 0xaf,
+            0x50, 0x0c, 0x65, 0x5b, 0x97, 0x1e, 0x12, 0x10,
+        },
+        .public_key_req = {
+            .x = {
+                0x54, 0xd9, 0x8f, 0xeb, 0xc1, 0xbb, 0xe6, 0x74,
+                0x8a, 0x55, 0x3a, 0x80, 0x0e, 0xef, 0x90, 0xc9,
+                0xab, 0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62,
+                0x0d, 0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7,
+            },
+            .y = {
+                0x07, 0x33, 0x91, 0x40, 0xde, 0x25, 0xb4, 0x3d,
+                0x81, 0x2f, 0xd2, 0x41, 0x98, 0xe7, 0xaf, 0x0f,
+                0x5f, 0x17, 0x85, 0x1f, 0x75, 0x6e, 0xf4, 0x0e,
+                0x05, 0x19, 0x7f, 0x03, 0x9b, 0xf4, 0x41, 0x23,
+            },
+        },
+        .public_key_rsp = {
+            .x = {
+                0x1d, 0x44, 0x66, 0x0d, 0x3a, 0x03, 0x71, 0x17,
+                0xb3, 0x10, 0x2e, 0xf0, 0xd3, 0xf8, 0xa2, 0x6c,
+                0x1f, 0xfc, 0xbf, 0x02, 0x62, 0x6b, 0x11, 0x5a,
+                0x76, 0x5b, 0x30, 0x20, 0xb1, 0xef, 0xb3, 0x76,
+            },
+            .y = {
+                0xf1, 0x23, 0x63, 0x75, 0xfc, 0xb6, 0xc9, 0x32,
+                0xa4, 0x36, 0xbe, 0x18, 0xa0, 0x7d, 0x0b, 0x16,
+                0x65, 0x24, 0xd0, 0xe3, 0x74, 0x1b, 0x34, 0x1a,
+                0xf9, 0xe2, 0xcb, 0x30, 0x93, 0x88, 0xd7, 0xfa,
+            },
+        },
+        .confirm_req[0] = {
+            .value = {
+                0x12, 0xe3, 0x01, 0xd0, 0x30, 0x59, 0xca, 0xd9,
+                0x78, 0x0b, 0x45, 0x73, 0xb1, 0x7a, 0x4d, 0xca,
+            },
+        },
+        .confirm_rsp[0] = {
+            .value = {
+                0x47, 0x68, 0x16, 0x24, 0xd4, 0x07, 0x60, 0x6c,
+                0xa5, 0x47, 0x6f, 0x05, 0x78, 0x71, 0x3e, 0xa8,
+            },
+        },
+        .random_req[0] = {
+            .value = {
+                0x2a, 0x29, 0xa8, 0xef, 0x0b, 0x70, 0x5f, 0x1b,
+                0x81, 0x4d, 0x97, 0xff, 0xfb, 0x7f, 0x30, 0x90,
+            },
+        },
+        .random_rsp[0] = {
+            .value = {
+                0x12, 0x9e, 0x1d, 0x12, 0x11, 0x44, 0x36, 0x74,
+                0xa3, 0x0c, 0xea, 0x36, 0x4d, 0xdf, 0x2d, 0x5d,
+            },
+        },
+        .confirm_req[1] = {
+            .value = {
+                0x4d, 0x6a, 0x32, 0xfe, 0xe2, 0xa0, 0xdd, 0x92,
+                0x60, 0x5c, 0x82, 0x7f, 0xa6, 0xa6, 0x24, 0xd6,
+            },
+        },
+        .confirm_rsp[1] = {
+            .value = {
+                0xd5, 0x3e, 0xa7, 0xa0, 0xbf, 0x39, 0x8e, 0xfe,
+                0xfd, 0x73, 0x47, 0x4c, 0x92, 0x8b, 0x74, 0x06,
+            },
+        },
+        .random_req[1] = {
+            .value = {
+                0xc1, 0x88, 0xdf, 0xb0, 0x99, 0xbb, 0xbf, 0xed,
+                0xdc, 0x40, 0x66, 0x55, 0xbe, 0x91, 0x56, 0x9a,
+            },
+        },
+        .random_rsp[1] = {
+            .value = {
+                0xed, 0xed, 0x9a, 0x61, 0xb8, 0x21, 0x03, 0x77,
+                0xa6, 0xcf, 0x34, 0x65, 0x8c, 0x18, 0x82, 0x9f,
+            },
+        },
+        .confirm_req[2] = {
+            .value = {
+                0xdb, 0xea, 0x94, 0x29, 0xe4, 0x44, 0x7d, 0x7b,
+                0xd3, 0x16, 0x81, 0x8e, 0xaf, 0xe6, 0x9c, 0x85,
+            },
+        },
+        .confirm_rsp[2] = {
+            .value = {
+                0x3f, 0xdd, 0x54, 0x76, 0xab, 0x45, 0x7f, 0x53,
+                0x64, 0x6b, 0x37, 0xa6, 0xc7, 0xc6, 0x4a, 0x73,
+            },
+        },
+        .random_req[2] = {
+            .value = {
+                0x5a, 0xf1, 0xfb, 0xde, 0xb3, 0xbe, 0x6e, 0xac,
+                0x68, 0x51, 0x47, 0x8e, 0x0b, 0xcd, 0xc1, 0xa0,
+            },
+        },
+        .random_rsp[2] = {
+            .value = {
+                0x29, 0x0f, 0x5e, 0x83, 0x87, 0xca, 0xd3, 0x21,
+                0xa7, 0x7e, 0x3d, 0x78, 0x47, 0x54, 0xf8, 0xe4,
+            },
+        },
+        .confirm_req[3] = {
+            .value = {
+                0xca, 0x3e, 0xd5, 0xe3, 0x59, 0xb0, 0x5d, 0x1e,
+                0x0f, 0x4c, 0x95, 0x0f, 0x6a, 0x72, 0xcf, 0x25,
+            },
+        },
+        .confirm_rsp[3] = {
+            .value = {
+                0x2f, 0x4d, 0x06, 0x40, 0x09, 0x68, 0x68, 0x45,
+                0x87, 0x79, 0x78, 0x48, 0xda, 0xe4, 0xf5, 0xae,
+            },
+        },
+        .random_req[3] = {
+            .value = {
+                0x63, 0x5a, 0xee, 0x91, 0xe4, 0xf8, 0xe8, 0x69,
+                0xd1, 0x46, 0x18, 0x0d, 0xd2, 0x94, 0xd8, 0x20,
+            },
+        },
+        .random_rsp[3] = {
+            .value = {
+                0x76, 0x36, 0xf5, 0xc2, 0x41, 0xb6, 0x3c, 0x1f,
+                0x36, 0x19, 0x58, 0xce, 0x8f, 0x41, 0xeb, 0x8c,
+            },
+        },
+        .confirm_req[4] = {
+            .value = {
+                0x76, 0xfd, 0x84, 0x0f, 0x0f, 0x58, 0x70, 0x45,
+                0x41, 0x33, 0x5d, 0xce, 0xe5, 0xe2, 0x2f, 0x83,
+            },
+        },
+        .confirm_rsp[4] = {
+            .value = {
+                0x87, 0xcf, 0xdf, 0xa5, 0x60, 0x82, 0x4f, 0x09,
+                0x4c, 0x50, 0x24, 0xba, 0x91, 0x96, 0x0d, 0x65,
+            },
+        },
+        .random_req[4] = {
+            .value = {
+                0x67, 0xdb, 0x73, 0x1e, 0x57, 0x5c, 0xb7, 0x86,
+                0xf8, 0xaf, 0x58, 0xd8, 0x0f, 0x97, 0x47, 0xce,
+            },
+        },
+        .random_rsp[4] = {
+            .value = {
+                0xaa, 0x99, 0x90, 0x05, 0x11, 0xfc, 0xc2, 0xd9,
+                0xb8, 0xd6, 0x9d, 0xef, 0x86, 0x10, 0xcf, 0x26,
+            },
+        },
+        .confirm_req[5] = {
+            .value = {
+                0xfc, 0x22, 0xd9, 0x1f, 0x5f, 0x86, 0x25, 0xe7,
+                0x5e, 0x55, 0x48, 0x35, 0xec, 0x32, 0x37, 0x6d,
+            },
+        },
+        .confirm_rsp[5] = {
+            .value = {
+                0x98, 0xbc, 0x07, 0x72, 0xa2, 0xe7, 0xa7, 0x66,
+                0x64, 0xf7, 0x29, 0x3a, 0xaf, 0x52, 0x18, 0x04,
+            },
+        },
+        .random_req[5] = {
+            .value = {
+                0xd3, 0x36, 0xb9, 0x69, 0x6a, 0x6d, 0x55, 0xbc,
+                0x82, 0xdf, 0x1c, 0x04, 0xa7, 0xd5, 0x00, 0x68,
+            },
+        },
+        .random_rsp[5] = {
+            .value = {
+                0xb9, 0x03, 0xbf, 0xd9, 0x86, 0x5a, 0x1a, 0xb4,
+                0xdc, 0xe6, 0x8f, 0x9b, 0xa4, 0xa8, 0x2a, 0x12,
+            },
+        },
+        .confirm_req[6] = {
+            .value = {
+                0xfe, 0x14, 0xab, 0x1c, 0xfd, 0x36, 0x64, 0x38,
+                0xc1, 0xf8, 0xdd, 0xcd, 0xf4, 0x77, 0xa1, 0xb8,
+            },
+        },
+        .confirm_rsp[6] = {
+            .value = {
+                0x2e, 0x70, 0x54, 0xdc, 0xa6, 0xae, 0xb2, 0xcd,
+                0x4a, 0x26, 0x97, 0xf8, 0xbf, 0xb4, 0xb4, 0x52,
+            },
+        },
+        .random_req[6] = {
+            .value = {
+                0x1e, 0x27, 0x73, 0x94, 0x44, 0xfc, 0xd4, 0x44,
+                0xbf, 0x5b, 0x7d, 0x5d, 0x6d, 0x13, 0x68, 0xb1,
+            },
+        },
+        .random_rsp[6] = {
+            .value = {
+                0xeb, 0xfd, 0x0b, 0xa1, 0x7b, 0xda, 0x61, 0xdc,
+                0x6d, 0xe4, 0x3b, 0x51, 0xa7, 0x09, 0x29, 0x6d,
+            },
+        },
+        .confirm_req[7] = {
+            .value = {
+                0x38, 0x2b, 0x23, 0xb9, 0x18, 0x2d, 0xb9, 0x0b,
+                0xe7, 0x4d, 0x20, 0x83, 0xab, 0x17, 0xfd, 0x88,
+            },
+        },
+        .confirm_rsp[7] = {
+            .value = {
+                0x65, 0x60, 0x85, 0xef, 0x0e, 0x9a, 0x23, 0x96,
+                0xe7, 0xa9, 0xee, 0xba, 0x9e, 0x48, 0xb9, 0x1c,
+            },
+        },
+        .random_req[7] = {
+            .value = {
+                0x8b, 0xa8, 0x7a, 0x33, 0x15, 0x1e, 0xa7, 0x78,
+                0x27, 0x01, 0x3e, 0x90, 0x43, 0x47, 0x5a, 0x9d,
+            },
+        },
+        .random_rsp[7] = {
+            .value = {
+                0x76, 0xf1, 0x21, 0x67, 0x94, 0x20, 0x6f, 0xc7,
+                0x84, 0xc8, 0xdb, 0x07, 0xdb, 0x77, 0xdd, 0x50,
+            },
+        },
+        .confirm_req[8] = {
+            .value = {
+                0x4e, 0x7f, 0x83, 0x8e, 0xa6, 0x28, 0xaa, 0x46,
+                0xa2, 0x69, 0x95, 0x3b, 0xf0, 0x71, 0x14, 0x24,
+            },
+        },
+        .confirm_rsp[8] = {
+            .value = {
+                0x93, 0x0b, 0x4d, 0xbe, 0x49, 0x36, 0xa0, 0x26,
+                0xe9, 0x18, 0x4e, 0xc8, 0x19, 0x59, 0xc1, 0x7d,
+            },
+        },
+        .random_req[8] = {
+            .value = {
+                0x11, 0xa9, 0xce, 0x26, 0x0e, 0x2f, 0x11, 0x0e,
+                0xc1, 0xbd, 0x68, 0x80, 0xc8, 0xf8, 0x41, 0x65,
+            },
+        },
+        .random_rsp[8] = {
+            .value = {
+                0xb6, 0x3d, 0x6b, 0x62, 0xb5, 0x37, 0x31, 0x28,
+                0x79, 0xc4, 0xe2, 0x62, 0xbb, 0x63, 0xf9, 0x91,
+            },
+        },
+        .confirm_req[9] = {
+            .value = {
+                0x5f, 0x55, 0xb5, 0xa4, 0x80, 0xa8, 0x54, 0x47,
+                0xa7, 0x79, 0x87, 0x12, 0x2e, 0x44, 0x92, 0x42,
+            },
+        },
+        .confirm_rsp[9] = {
+            .value = {
+                0x01, 0x69, 0xa2, 0xac, 0xd6, 0x62, 0x8a, 0x64,
+                0xa2, 0x0b, 0xd0, 0xb4, 0x0e, 0x68, 0xe0, 0x88,
+            },
+        },
+        .random_req[9] = {
+            .value = {
+                0x75, 0x1e, 0x56, 0xd0, 0xcb, 0x06, 0xfd, 0x51,
+                0x55, 0xae, 0x77, 0xa4, 0xf2, 0xe7, 0x86, 0x3c,
+            },
+        },
+        .random_rsp[9] = {
+            .value = {
+                0xff, 0xab, 0x8a, 0x7d, 0xb7, 0x40, 0xe5, 0x07,
+                0xfe, 0x8f, 0x74, 0xdb, 0x2c, 0x35, 0x35, 0x12,
+            },
+        },
+        .confirm_req[10] = {
+            .value = {
+                0x1f, 0x2a, 0xed, 0xcd, 0x6b, 0x87, 0xea, 0xa2,
+                0xf8, 0xd8, 0xad, 0x04, 0x23, 0xc7, 0x5d, 0x47,
+            },
+        },
+        .confirm_rsp[10] = {
+            .value = {
+                0x5b, 0x18, 0x2d, 0x96, 0x3b, 0xf6, 0xdc, 0x82,
+                0x3b, 0xfa, 0xc9, 0x81, 0xc7, 0x33, 0xa0, 0x07,
+            },
+        },
+        .random_req[10] = {
+            .value = {
+                0xd1, 0x3a, 0x82, 0xce, 0x31, 0x75, 0xa2, 0xbf,
+                0x6f, 0x12, 0xf2, 0xac, 0xf6, 0xcc, 0xea, 0x34,
+            },
+        },
+        .random_rsp[10] = {
+            .value = {
+                0xcf, 0x11, 0x3d, 0x44, 0x10, 0x0d, 0x26, 0x32,
+                0xa5, 0x61, 0x13, 0xfd, 0xb8, 0xed, 0x31, 0x53,
+            },
+        },
+        .confirm_req[11] = {
+            .value = {
+                0x67, 0x14, 0x8a, 0xf6, 0xc8, 0xb8, 0x73, 0x6b,
+                0xb2, 0xec, 0xa9, 0x61, 0xaa, 0xc0, 0xc9, 0x28,
+            },
+        },
+        .confirm_rsp[11] = {
+            .value = {
+                0xa5, 0xbf, 0x00, 0x07, 0x48, 0xff, 0x30, 0x36,
+                0x20, 0x83, 0xd7, 0xd6, 0xd0, 0x90, 0x46, 0x03,
+            },
+        },
+        .random_req[11] = {
+            .value = {
+                0x75, 0x99, 0x9a, 0xa3, 0xad, 0x9a, 0xe5, 0x9d,
+                0x2f, 0x21, 0xdb, 0x72, 0x2f, 0xaf, 0xb8, 0x79,
+            },
+        },
+        .random_rsp[11] = {
+            .value = {
+                0xa3, 0xb7, 0xb7, 0x46, 0x39, 0x99, 0xc2, 0x82,
+                0xe9, 0x31, 0x8d, 0xc2, 0x28, 0x1b, 0x86, 0x91,
+            },
+        },
+        .confirm_req[12] = {
+            .value = {
+                0x46, 0x2f, 0xc8, 0x0e, 0x2c, 0x70, 0x3a, 0xdb,
+                0x25, 0x2f, 0xce, 0xe6, 0x15, 0x1f, 0x9a, 0x06,
+            },
+        },
+        .confirm_rsp[12] = {
+            .value = {
+                0x9a, 0xa4, 0xe0, 0x03, 0x3a, 0xb5, 0x43, 0x75,
+                0x8e, 0x93, 0x35, 0x25, 0xe6, 0x5e, 0x9d, 0x7f,
+            },
+        },
+        .random_req[12] = {
+            .value = {
+                0x1f, 0x01, 0x32, 0x56, 0x64, 0x45, 0xc5, 0x20,
+                0xd4, 0xad, 0x13, 0x8f, 0xbe, 0x82, 0xc8, 0x01,
+            },
+        },
+        .random_rsp[12] = {
+            .value = {
+                0xd4, 0x3f, 0xa4, 0xc9, 0xe9, 0x2e, 0x62, 0x77,
+                0x4e, 0x21, 0x55, 0xd8, 0xde, 0x31, 0xf5, 0xea,
+            },
+        },
+        .confirm_req[13] = {
+            .value = {
+                0x4e, 0x48, 0x88, 0x4e, 0x4f, 0x74, 0x7e, 0xec,
+                0x99, 0x5d, 0xb1, 0xcb, 0x84, 0x88, 0x80, 0xe9,
+            },
+        },
+        .confirm_rsp[13] = {
+            .value = {
+                0x1a, 0x84, 0xfa, 0x2f, 0xd7, 0x3c, 0x5f, 0xee,
+                0x3e, 0x81, 0xc0, 0x4b, 0x35, 0x4b, 0x7e, 0x98,
+            },
+        },
+        .random_req[13] = {
+            .value = {
+                0xe3, 0x3a, 0xc5, 0x2f, 0x9f, 0x91, 0x93, 0xfb,
+                0xcb, 0xd8, 0x53, 0x63, 0xab, 0xc4, 0xa5, 0x85,
+            },
+        },
+        .random_rsp[13] = {
+            .value = {
+                0xa0, 0xcf, 0xad, 0x30, 0x2d, 0xec, 0xea, 0x81,
+                0xfd, 0x7f, 0xcf, 0x7c, 0x70, 0xc9, 0x89, 0x7b,
+            },
+        },
+        .confirm_req[14] = {
+            .value = {
+                0xe1, 0x64, 0x22, 0x19, 0x41, 0x44, 0x37, 0x2b,
+                0x92, 0x60, 0xa4, 0x1f, 0xd6, 0x53, 0xe0, 0xa0,
+            },
+        },
+        .confirm_rsp[14] = {
+            .value = {
+                0x08, 0xfa, 0xa4, 0xf8, 0x04, 0x08, 0xb8, 0x9f,
+                0x61, 0xb5, 0x68, 0xaf, 0x31, 0x12, 0x8d, 0x3f,
+            },
+        },
+        .random_req[14] = {
+            .value = {
+                0xad, 0x76, 0xc3, 0x1a, 0x4c, 0x64, 0x2c, 0x11,
+                0x5e, 0x48, 0x6d, 0x41, 0xf5, 0x77, 0xc2, 0x40,
+            },
+        },
+        .random_rsp[14] = {
+            .value = {
+                0x1b, 0xec, 0x78, 0x2b, 0xd9, 0xbe, 0x93, 0xbd,
+                0x0b, 0x03, 0xf1, 0xd8, 0x31, 0xe8, 0x60, 0x67,
+            },
+        },
+        .confirm_req[15] = {
+            .value = {
+                0x5e, 0x22, 0x44, 0x09, 0x97, 0xf9, 0xc5, 0xc7,
+                0x23, 0xc7, 0x74, 0x51, 0xe5, 0x9d, 0x5c, 0xed,
+            },
+        },
+        .confirm_rsp[15] = {
+            .value = {
+                0xfe, 0xb2, 0x90, 0xa7, 0x06, 0xaf, 0xdd, 0x6a,
+                0x83, 0x26, 0x3c, 0x78, 0x66, 0xe0, 0x9d, 0xd9,
+            },
+        },
+        .random_req[15] = {
+            .value = {
+                0xb2, 0xa0, 0x75, 0x6f, 0x77, 0xc1, 0x0b, 0x4e,
+                0x99, 0xfa, 0x9a, 0x02, 0xf6, 0xe4, 0x66, 0x27,
+            },
+        },
+        .random_rsp[15] = {
+            .value = {
+                0xf9, 0xdd, 0x69, 0xae, 0xc8, 0x66, 0xa9, 0xab,
+                0xb8, 0x01, 0x38, 0xc3, 0x2a, 0x6b, 0x94, 0x66,
+            },
+        },
+        .confirm_req[16] = {
+            .value = {
+                0x17, 0xc9, 0xf7, 0x2d, 0xe6, 0xb7, 0x99, 0x77,
+                0x65, 0xf7, 0x62, 0xc8, 0x0d, 0x7d, 0xbd, 0x81,
+            },
+        },
+        .confirm_rsp[16] = {
+            .value = {
+                0x39, 0xef, 0xbf, 0x39, 0xfa, 0x79, 0xc3, 0x7b,
+                0x71, 0x40, 0x3c, 0x1f, 0x67, 0xe5, 0x60, 0xe5,
+            },
+        },
+        .random_req[16] = {
+            .value = {
+                0x32, 0xab, 0x8b, 0xed, 0x90, 0x04, 0x5e, 0x17,
+                0xd2, 0x5e, 0xa8, 0x91, 0xf7, 0x77, 0xe3, 0xd7,
+            },
+        },
+        .random_rsp[16] = {
+            .value = {
+                0x6c, 0xc7, 0x14, 0x13, 0xdf, 0xfb, 0xc6, 0xed,
+                0xa3, 0x9c, 0xa7, 0x90, 0xae, 0x4c, 0x61, 0x47,
+            },
+        },
+        .confirm_req[17] = {
+            .value = {
+                0xc5, 0x17, 0x07, 0x35, 0x34, 0xbf, 0xc1, 0x4d,
+                0xc4, 0x57, 0xc0, 0xd9, 0xfd, 0xe9, 0x10, 0x08,
+            },
+        },
+        .confirm_rsp[17] = {
+            .value = {
+                0xbb, 0xcf, 0x41, 0xd2, 0x94, 0xea, 0xbe, 0x2f,
+                0xde, 0xb2, 0xb4, 0x20, 0x72, 0x1c, 0xf8, 0x35,
+            },
+        },
+        .random_req[17] = {
+            .value = {
+                0x59, 0x20, 0xb5, 0xdc, 0xaf, 0xc3, 0x8b, 0x32,
+                0xe6, 0x40, 0x0f, 0x02, 0x67, 0x45, 0x49, 0x1f,
+            },
+        },
+        .random_rsp[17] = {
+            .value = {
+                0xf5, 0x95, 0x60, 0x4c, 0x5f, 0x39, 0x54, 0xbf,
+                0x62, 0x9e, 0x85, 0xca, 0x31, 0x9a, 0x95, 0xee,
+            },
+        },
+        .confirm_req[18] = {
+            .value = {
+                0x36, 0x50, 0x78, 0x6b, 0x0f, 0x11, 0xe3, 0xa9,
+                0x79, 0x3a, 0xa6, 0x9d, 0xd4, 0x8b, 0x13, 0x3f,
+            },
+        },
+        .confirm_rsp[18] = {
+            .value = {
+                0xa5, 0x34, 0x5d, 0x5e, 0x43, 0x01, 0xf2, 0xe1,
+                0x3f, 0xf2, 0x1c, 0x8b, 0x13, 0xf7, 0x17, 0x3e,
+            },
+        },
+        .random_req[18] = {
+            .value = {
+                0x77, 0xa1, 0xbe, 0xbf, 0x49, 0xb8, 0x74, 0x73,
+                0x47, 0x78, 0x2a, 0xf8, 0x66, 0x6b, 0xff, 0xd2,
+            },
+        },
+        .random_rsp[18] = {
+            .value = {
+                0xa2, 0x05, 0x69, 0x65, 0x3f, 0xd4, 0xb4, 0xcd,
+                0xed, 0x8c, 0x36, 0x6d, 0x51, 0x6a, 0xbb, 0xef,
+            },
+        },
+        .confirm_req[19] = {
+            .value = {
+                0xda, 0xd8, 0x96, 0xfd, 0x1c, 0x0d, 0x1e, 0x56,
+                0xe2, 0x62, 0xed, 0x18, 0x4b, 0xd3, 0x46, 0x48,
+            },
+        },
+        .confirm_rsp[19] = {
+            .value = {
+                0xeb, 0x79, 0x5e, 0x52, 0x70, 0x25, 0xa7, 0x41,
+                0x33, 0xfa, 0xac, 0xd3, 0x27, 0x35, 0xfc, 0x5f,
+            },
+        },
+        .random_req[19] = {
+            .value = {
+                0xa8, 0x9c, 0xb9, 0xcd, 0x13, 0xb8, 0xdd, 0xd2,
+                0x09, 0xd6, 0xc8, 0x12, 0xc3, 0x69, 0x9a, 0x64,
+            },
+        },
+        .random_rsp[19] = {
+            .value = {
+                0x06, 0xe3, 0x8a, 0xef, 0xe4, 0x42, 0xae, 0x86,
+                0xef, 0x58, 0x80, 0xe8, 0xe3, 0xa2, 0x09, 0x44,
+            },
+        },
+        .dhkey_check_req = {
+            .value = {
+                0x6f, 0xa5, 0x37, 0x06, 0x4a, 0x89, 0x98, 0x39,
+                0xf6, 0x69, 0x48, 0x56, 0x17, 0x6d, 0x44, 0x7c,
+            },
+        },
+        .dhkey_check_rsp = {
+            .value = {
+                0x82, 0x48, 0xd4, 0x9e, 0xb8, 0x3c, 0xb4, 0xdc,
+                0x44, 0xcb, 0x19, 0xdb, 0xcb, 0xa2, 0x00, 0x5d,
+            },
+        },
+        .id_info_req = {
+            .irk = {
+                0x79, 0x12, 0x88, 0x97, 0xd9, 0x1c, 0x62, 0x0d,
+                0x26, 0x43, 0x7d, 0x25, 0x86, 0x79, 0xc7, 0x07,
+            },
+        },
+        .id_addr_info_req = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07,
+            },
+        },
+        .id_info_rsp = {
+            .irk = {
+                0xda, 0x6b, 0x27, 0xa0, 0xac, 0x71, 0xf0, 0xc3,
+                0x75, 0x51, 0xf6, 0x21, 0x94, 0xec, 0x81, 0x92,
+            },
+        },
+        .id_addr_info_rsp = {
+            .addr_type = 0,
+            .bd_addr = {
+                0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+            },
+        },
+        .sign_info_rsp = {
+            .sig_key = {
+                0x49, 0x5b, 0x11, 0xb3, 0x4c, 0x1a, 0x23, 0x5c,
+                0x61, 0x4f, 0xe3, 0x08, 0xf9, 0x47, 0x8b, 0xdc,
+            },
+        },
+        .ltk = {
+            0x5a, 0x49, 0x28, 0xf0, 0x11, 0x3b, 0x6f, 0x6b,
+            0x3a, 0x69, 0x6d, 0xdd, 0xb2, 0xe5, 0xa8, 0x97,
+        },
+        .pair_alg = BLE_SM_PAIR_ALG_PASSKEY,
+        .authenticated = 1,
+        .passkey_info = {
+            .passkey = {
+                .action = BLE_SM_IOACT_INPUT,
+                .passkey = 4915,
+            },
+        },
+    };
+    ble_sm_test_util_us_sc_good(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_sm_sc_test_suite)
+{
+    /*** No privacy. */
+
+    /* Peer as initiator. */
+    ble_sm_sc_peer_jw_iio3_rio3_b1_iat0_rat0_ik5_rk7();
+    ble_sm_sc_peer_pk_iio0_rio2_b1_iat0_rat0_ik5_rk7();
+    ble_sm_sc_peer_pk_iio2_rio0_b1_iat0_rat0_ik5_rk7();
+    ble_sm_sc_peer_nc_iio1_rio1_b1_iat0_rat0_ik5_rk7();
+
+    /* Us as initiator. */
+    ble_sm_sc_us_jw_iio3_rio4_b1_iat0_rat0_ik7_rk5();
+    ble_sm_sc_us_pk_iio2_rio4_b1_iat0_rat0_ik7_rk5();
+    ble_sm_sc_us_pk_iio0_rio4_b1_iat0_rat0_ik7_rk5();
+    ble_sm_sc_us_nc_iio1_rio4_b1_iat0_rat0_ik7_rk5();
+
+    /*** Privacy (id = public). */
+    // FIXME: needs to be fixed due to fix for address type used
+#if 0
+    /* Peer as initiator. */
+    ble_sm_sc_peer_jw_iio3_rio3_b1_iat2_rat2_ik7_rk7();
+    ble_sm_sc_peer_nc_iio1_rio1_b1_iat2_rat2_ik3_rk3();
+    ble_sm_sc_peer_pk_iio2_rio0_b1_iat2_rat2_ik7_rk3();
+
+    /* Us as initiator. */
+    ble_sm_sc_us_jw_iio3_rio3_b1_iat2_rat2_ik3_rk3();
+    ble_sm_sc_us_nc_iio1_rio1_b1_iat2_rat2_ik3_rk3();
+    ble_sm_sc_us_pk_iio2_rio0_b1_iat2_rat2_ik7_rk3();
+#endif
+}
+
+#endif /* NIMBLE_BLE_SM */
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test.c
new file mode 100644
index 00000000..be4285fd
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test.c
@@ -0,0 +1,414 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+#include "ble_sm_test_util.h"
+
+#if NIMBLE_BLE_SM
+
+/*****************************************************************************
+ * $misc                                                                     *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_sm_test_case_f4)
+{
+	uint8_t u[32] = { 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc,
+			  0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef,
+			  0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
+			  0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20 };
+	uint8_t v[32] = { 0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b,
+			  0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59,
+			  0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90,
+			  0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55 };
+	uint8_t x[16] = { 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
+			  0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
+	uint8_t z = 0x00;
+	uint8_t exp[16] = { 0x2d, 0x87, 0x74, 0xa9, 0xbe, 0xa1, 0xed, 0xf1,
+			    0x1c, 0xbd, 0xa9, 0x07, 0xf1, 0x16, 0xc9, 0xf2 };
+	uint8_t res[16];
+	int err;
+
+	err = ble_sm_alg_f4(u, v, x, z, res);
+	TEST_ASSERT_FATAL(err == 0);
+    TEST_ASSERT(memcmp(res, exp, 16) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_f5)
+{
+	uint8_t w[32] = { 0x98, 0xa6, 0xbf, 0x73, 0xf3, 0x34, 0x8d, 0x86,
+			  0xf1, 0x66, 0xf8, 0xb4, 0x13, 0x6b, 0x79, 0x99,
+			  0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34,
+			  0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec };
+	uint8_t n1[16] = { 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
+			   0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
+	uint8_t n2[16] = { 0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21,
+			   0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 };
+    uint8_t a1t = 0x00;
+	uint8_t a1[6] = { 0xce, 0xbf, 0x37, 0x37, 0x12, 0x56 };
+    uint8_t a2t = 0x00;
+    uint8_t a2[6] = { 0xc1, 0xcf, 0x2d, 0x70, 0x13, 0xa7 };
+	uint8_t exp_ltk[16] = { 0x38, 0x0a, 0x75, 0x94, 0xb5, 0x22, 0x05,
+				0x98, 0x23, 0xcd, 0xd7, 0x69, 0x11, 0x79,
+				0x86, 0x69 };
+	uint8_t exp_mackey[16] = { 0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f,
+				   0xfd, 0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1,
+				   0x65, 0x29 };
+	uint8_t mackey[16], ltk[16];
+	int err;
+
+	err = ble_sm_alg_f5(w, n1, n2, a1t, a1, a2t, a2, mackey, ltk);
+	TEST_ASSERT_FATAL(err == 0);
+    TEST_ASSERT(memcmp(mackey, exp_mackey, 16) == 0);
+    TEST_ASSERT(memcmp(ltk, exp_ltk, 16) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_f6)
+{
+	uint8_t w[16] = { 0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f, 0xfd,
+			  0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1, 0x65, 0x29 };
+	uint8_t n1[16] = { 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
+			   0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
+	uint8_t n2[16] = { 0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21,
+			   0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 };
+	uint8_t r[16] = { 0xc8, 0x0f, 0x2d, 0x0c, 0xd2, 0x42, 0xda, 0x08,
+			  0x54, 0xbb, 0x53, 0xb4, 0x3b, 0x34, 0xa3, 0x12 };
+	uint8_t io_cap[3] = { 0x02, 0x01, 0x01 };
+    uint8_t a1t = 0x00;
+	uint8_t a1[6] = { 0xce, 0xbf, 0x37, 0x37, 0x12, 0x56 };
+    uint8_t a2t = 0x00;
+    uint8_t a2[6] = { 0xc1, 0xcf, 0x2d, 0x70, 0x13, 0xa7 };
+	uint8_t exp[16] = { 0x61, 0x8f, 0x95, 0xda, 0x09, 0x0b, 0x6c, 0xd2,
+			    0xc5, 0xe8, 0xd0, 0x9c, 0x98, 0x73, 0xc4, 0xe3 };
+	uint8_t res[16];
+	int err;
+
+	err = ble_sm_alg_f6(w, n1, n2, r, io_cap, a1t, a1, a2t, a2, res);
+	TEST_ASSERT_FATAL(err == 0);
+    TEST_ASSERT(memcmp(res, exp, 16) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_g2)
+{
+	uint8_t u[32] = { 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc,
+			  0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef,
+			  0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
+			  0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20 };
+	uint8_t v[32] = { 0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b,
+			  0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59,
+			  0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90,
+			  0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55 };
+	uint8_t x[16] = { 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff,
+			  0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 };
+	uint8_t y[16] = { 0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21,
+			  0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 };
+	uint32_t exp_val = 0x2f9ed5ba % 1000000;
+	uint32_t val;
+	int err;
+
+	err = ble_sm_alg_g2(u, v, x, y, &val);
+	TEST_ASSERT_FATAL(err == 0);
+	TEST_ASSERT(val == exp_val);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_conn_broken)
+{
+    struct ble_hci_ev_disconn_cmp disconn_evt;
+    int rc;
+
+    ble_sm_test_util_init();
+
+    ble_sm_dbg_set_next_pair_rand(((uint8_t[16]){0}));
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[6]){1,2,3,5,6,7}),
+                                 ble_sm_test_util_conn_cb, NULL);
+
+    /* Initiate the pairing procedure. */
+    rc = ble_hs_test_util_security_initiate(2, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Terminate the connection. */
+    disconn_evt.conn_handle = htole16(2);
+    disconn_evt.status = 0;
+    disconn_evt.reason = BLE_ERR_REM_USER_CONN_TERM;
+    ble_gap_rx_disconn_complete(&disconn_evt);
+
+    /* Verify security callback got called. */
+    TEST_ASSERT(ble_sm_test_gap_status == BLE_HS_ENOTCONN);
+    TEST_ASSERT(!ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(!ble_sm_test_sec_state.authenticated);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*****************************************************************************
+ * $peer                                                                     *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_sm_test_case_peer_fail_inval)
+{
+    /* Invalid role detected before other arguments. */
+    ble_sm_test_util_peer_fail_inval(
+        1,
+        ((uint8_t[]){0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c}),
+        ((uint8_t[]){0x03, 0x02, 0x01, 0x50, 0x13, 0x00}),
+        ((struct ble_sm_pair_cmd[1]) { {
+            .io_cap = 0x14,
+            .oob_data_flag = 0,
+            .authreq = 0x12,
+            .max_enc_key_size = 20,
+            .init_key_dist = 0x0b,
+            .resp_key_dist = 0x11,
+        } }),
+        ((struct ble_sm_pair_fail[1]) { {
+            .reason = BLE_SM_ERR_CMD_NOT_SUPP,
+        } })
+    );
+
+    /* Invalid key size - too small. */
+    ble_sm_test_util_peer_fail_inval(
+        0,
+        ((uint8_t[]){0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c}),
+        ((uint8_t[]){0x03, 0x02, 0x01, 0x50, 0x13, 0x00}),
+        ((struct ble_sm_pair_cmd[1]) { {
+            .io_cap = 0x04,
+            .oob_data_flag = 0,
+            .authreq = 0x5,
+            .max_enc_key_size = 6,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        } }),
+        ((struct ble_sm_pair_fail[1]) { {
+            .reason = BLE_SM_ERR_ENC_KEY_SZ,
+        } })
+    );
+
+    /* Invalid key size - too large. */
+    ble_sm_test_util_peer_fail_inval(
+        0,
+        ((uint8_t[]){0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c}),
+        ((uint8_t[]){0x03, 0x02, 0x01, 0x50, 0x13, 0x00}),
+        ((struct ble_sm_pair_cmd[1]) { {
+            .io_cap = 0x04,
+            .oob_data_flag = 0,
+            .authreq = 0x5,
+            .max_enc_key_size = 17,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        } }),
+        ((struct ble_sm_pair_fail[1]) { {
+            .reason = BLE_SM_ERR_INVAL,
+        } })
+    );
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_peer_lgcy_fail_confirm)
+{
+    ble_sm_test_util_peer_lgcy_fail_confirm(
+        ((uint8_t[]){0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c}),
+        ((uint8_t[]){0x03, 0x02, 0x01, 0x50, 0x13, 0x00}),
+        ((struct ble_sm_pair_cmd[1]) { {
+            .io_cap = 0x04,
+            .oob_data_flag = 0,
+            .authreq = 0x05,
+            .max_enc_key_size = 16,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        } }),
+        ((struct ble_sm_pair_cmd[1]) { {
+            .io_cap = 3,
+            .oob_data_flag = 0,
+            .authreq = 0,
+            .max_enc_key_size = 16,
+            .init_key_dist = 0,
+            .resp_key_dist = 0,
+        } }),
+        ((struct ble_sm_pair_confirm[1]) { {
+            .value = {
+                0x0a, 0xac, 0xa2, 0xae, 0xa6, 0x98, 0xdc, 0x6d,
+                0x65, 0x84, 0x11, 0x69, 0x47, 0x36, 0x8d, 0xa0,
+            },
+        } }),
+        ((struct ble_sm_pair_confirm[1]) { {
+            .value = {
+                0x45, 0xd2, 0x2c, 0x38, 0xd8, 0x91, 0x4f, 0x19,
+                0xa2, 0xd4, 0xfc, 0x7d, 0xad, 0x37, 0x79, 0xe0
+            },
+        } }),
+        ((struct ble_sm_pair_random[1]) { {
+            .value = {
+                0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+                0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+            },
+        } }),
+        ((struct ble_sm_pair_random[1]) { {
+            .value = {
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            },
+        } }),
+        ((struct ble_sm_pair_fail[1]) { {
+            .reason = BLE_SM_ERR_CONFIRM_MISMATCH,
+        } })
+    );
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_peer_bonding_bad)
+{
+    ble_sm_test_util_peer_bonding_bad(0x5684, 32);
+    ble_sm_test_util_peer_bonding_bad(54325, 65437);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_sm_test_case_peer_sec_req_inval)
+{
+    struct ble_sm_pair_fail fail;
+    struct ble_sm_sec_req sec_req;
+    int rc;
+
+    ble_sm_test_util_init();
+
+    ble_sm_dbg_set_next_pair_rand(((uint8_t[16]){0}));
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[6]){1,2,3,5,6,7}),
+                                 ble_sm_test_util_conn_cb,
+                                 NULL);
+
+    /*** We are the slave; reject the security request. */
+    ble_hs_atomic_conn_set_flags(2, BLE_HS_CONN_F_MASTER, 0);
+
+    sec_req.authreq = 0;
+    ble_sm_test_util_rx_sec_req(
+        2, &sec_req, BLE_HS_SM_US_ERR(BLE_SM_ERR_CMD_NOT_SUPP));
+
+    fail.reason = BLE_SM_ERR_CMD_NOT_SUPP;
+    ble_sm_test_util_verify_tx_pair_fail(&fail);
+
+    /*** Pairing already in progress; ignore security request. */
+    ble_hs_atomic_conn_set_flags(2, BLE_HS_CONN_F_MASTER, 1);
+    rc = ble_sm_pair_initiate(2);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_test_util_prev_tx_queue_clear();
+
+    ble_sm_test_util_rx_sec_req(2, &sec_req, BLE_HS_EALREADY);
+    TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+/*****************************************************************************
+ * $us                                                                       *
+ *****************************************************************************/
+
+TEST_CASE_SELF(ble_sm_test_case_us_fail_inval)
+{
+    struct ble_sm_test_params params;
+
+    /* Invalid key size - too small. */
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c},
+        .resp_id_addr = {0x03, 0x02, 0x01, 0x50, 0x13, 0x00},
+        .pair_req = (struct ble_sm_pair_cmd) {
+            .io_cap = 3,
+            .oob_data_flag = 0,
+            .authreq = 0,
+            .max_enc_key_size = 16,
+            .init_key_dist = 0,
+            .resp_key_dist = 0,
+        },
+        .pair_rsp = (struct ble_sm_pair_cmd) {
+            .io_cap = 0x04,
+            .oob_data_flag = 0,
+            .authreq = 0x05,
+            .max_enc_key_size = 6,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_fail = (struct ble_sm_pair_fail) {
+            .reason = BLE_SM_ERR_ENC_KEY_SZ,
+        },
+    };
+    ble_sm_test_util_us_fail_inval(&params);
+
+    /* Invalid key size - too large. */
+    params = (struct ble_sm_test_params) {
+        .init_id_addr = {0xe1, 0xfc, 0xda, 0xf4, 0xb7, 0x6c},
+        .resp_id_addr = {0x03, 0x02, 0x01, 0x50, 0x13, 0x00},
+        .pair_req = (struct ble_sm_pair_cmd) {
+            .io_cap = 3,
+            .oob_data_flag = 0,
+            .authreq = 0,
+            .max_enc_key_size = 16,
+            .init_key_dist = 0,
+            .resp_key_dist = 0,
+        },
+        .pair_rsp = (struct ble_sm_pair_cmd) {
+            .io_cap = 0x04,
+            .oob_data_flag = 0,
+            .authreq = 0x05,
+            .max_enc_key_size = 17,
+            .init_key_dist = 0x07,
+            .resp_key_dist = 0x07,
+        },
+        .pair_fail = (struct ble_sm_pair_fail) {
+            .reason = BLE_SM_ERR_INVAL,
+        },
+    };
+    ble_sm_test_util_us_fail_inval(&params);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_sm_gen_test_suite)
+{
+    ble_sm_test_case_f4();
+    ble_sm_test_case_f5();
+    ble_sm_test_case_f6();
+    ble_sm_test_case_g2();
+
+    ble_sm_test_case_peer_fail_inval();
+    ble_sm_test_case_peer_lgcy_fail_confirm();
+    ble_sm_test_case_us_fail_inval();
+    ble_sm_test_case_peer_bonding_bad();
+    ble_sm_test_case_conn_broken();
+    ble_sm_test_case_peer_sec_req_inval();
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test_util.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test_util.c
new file mode 100644
index 00000000..6170371f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test_util.c
@@ -0,0 +1,2967 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "nimble/hci_common.h"
+#include "nimble/nimble_opt.h"
+#include "host/ble_sm.h"
+#include "ble_hs_test.h"
+#include "host/ble_hs_id.h"
+#include "ble_hs_test_util.h"
+#include "ble_sm_test_util.h"
+
+#define BLE_HCI_LT_KEY_REQ_REPLY_LEN        (18)
+#define BLE_HCI_LT_KEY_REQ_NEG_REPLY_LEN    (2)
+#define BLE_HCI_LT_KEY_REQ_REPLY_ACK_PARAM_LEN (2) /* No status byte. */
+#define BLE_HCI_LT_KEY_REQ_NEG_REPLY_ACK_PARAM_LEN (2)
+#define BLE_HCI_LE_START_ENCRYPT_LEN        (28)
+#define BLE_HCI_ADD_TO_RESOLV_LIST_LEN      (39)
+
+int ble_sm_test_gap_event_type;
+int ble_sm_test_gap_status;
+struct ble_gap_sec_state ble_sm_test_sec_state;
+static struct ble_gap_passkey_params ble_sm_test_ioact;
+
+static struct {
+    /** Handle reported in previous repeat pairing event. */
+    struct ble_gap_repeat_pairing rp;
+
+    struct ble_sm_test_params params;
+
+    /** What the callback should return this time. */
+    int rc;
+
+    /** What the callback should return next time. */
+    int next_rc;
+
+    /**
+     * Whether the callback should erase the conflicting entry before retrying.
+     */
+    int erase_on_retry;
+
+    /** The number of times the event got reported. */
+    int num_calls;
+} ble_sm_test_repeat_pairing;
+
+struct ble_sm_test_util_entity {
+    uint8_t addr_type;
+    uint8_t id_addr_type;
+    uint8_t *id_addr;
+    uint8_t *rpa;
+
+    struct ble_sm_pair_cmd *pair_cmd;
+    struct ble_sm_pair_confirm *confirms;
+    struct ble_sm_pair_random *randoms;
+    struct ble_sm_id_info *id_info;
+    struct ble_sm_id_addr_info *id_addr_info;
+    struct ble_sm_sign_info *sign_info;
+    uint8_t *ltk;
+
+    uint8_t key_dist;
+
+    /*** Secure connections fields. */
+    struct ble_sm_public_key *public_key;
+    struct ble_sm_dhkey_check *dhkey_check;
+
+    /*** Legacy fields. */
+    struct ble_sm_enc_info *enc_info;
+    struct ble_sm_master_id *master_id;
+    uint64_t rand_num;
+    uint16_t ediv;
+};
+
+static void ble_sm_test_util_repeat_pairing(struct ble_sm_test_params *params,
+                                            int sc);
+
+#define BLE_SM_TEST_UTIL_HCI_HDR(handle, pb, len) \
+    ((struct hci_data_hdr) {                            \
+        .hdh_handle_pb_bc = ((handle)  << 0) |          \
+                            ((pb)      << 12),          \
+        .hdh_len = (len)                                \
+    })
+
+static void
+ble_sm_pair_cmd_parse(void *payload, int len, struct ble_sm_pair_cmd *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_pair_cmd));
+
+    u8ptr = payload;
+    cmd->io_cap = u8ptr[0];
+    cmd->oob_data_flag = u8ptr[1];
+    cmd->authreq = u8ptr[2];
+    cmd->max_enc_key_size = u8ptr[3];
+    cmd->init_key_dist = u8ptr[4];
+    cmd->resp_key_dist = u8ptr[5];
+}
+
+static void
+ble_sm_pair_cmd_write(void *payload, int len, int is_req,
+                      struct ble_sm_pair_cmd *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_cmd));
+
+    u8ptr = payload;
+    u8ptr[0] = is_req ? BLE_SM_OP_PAIR_REQ : BLE_SM_OP_PAIR_RSP;
+    u8ptr[1] = cmd->io_cap;
+    u8ptr[2] = cmd->oob_data_flag;
+    u8ptr[3] = cmd->authreq;
+    u8ptr[4] = cmd->max_enc_key_size;
+    u8ptr[5] = cmd->init_key_dist;
+    u8ptr[6] = cmd->resp_key_dist;
+}
+
+static void
+ble_sm_pair_confirm_parse(void *payload, int len,
+                          struct ble_sm_pair_confirm *cmd)
+{
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_pair_confirm));
+    memcpy(cmd->value, payload, sizeof cmd->value);
+}
+
+static void
+ble_sm_pair_confirm_write(void *payload, int len,
+                          struct ble_sm_pair_confirm *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_confirm));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_PAIR_CONFIRM;
+    memcpy(u8ptr + sizeof(struct ble_sm_hdr), cmd->value, sizeof cmd->value);
+}
+
+static void
+ble_sm_pair_random_parse(void *payload, int len,
+                         struct ble_sm_pair_random *cmd)
+{
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_pair_random));
+    memcpy(cmd->value, payload, sizeof cmd->value);
+}
+
+static void
+ble_sm_pair_random_write(void *payload, int len,
+                         struct ble_sm_pair_random *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_random));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_PAIR_RANDOM;
+    memcpy(u8ptr + sizeof(struct ble_sm_hdr), cmd->value, sizeof cmd->value);
+}
+
+static void
+ble_sm_pair_fail_parse(void *payload, int len, struct ble_sm_pair_fail *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_pair_fail));
+
+    u8ptr = payload;
+    cmd->reason = u8ptr[0];
+}
+
+static void
+ble_sm_enc_info_parse(void *payload, int len, struct ble_sm_enc_info *cmd)
+{
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_enc_info));
+
+    memcpy(cmd->ltk, payload, sizeof cmd->ltk);
+}
+
+static void
+ble_sm_enc_info_write(void *payload, int len, struct ble_sm_enc_info *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_enc_info));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_ENC_INFO;
+    memcpy(u8ptr + 1, cmd->ltk, sizeof cmd->ltk);
+}
+
+static void
+ble_sm_master_id_parse(void *payload, int len, struct ble_sm_master_id *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_master_id));
+
+    u8ptr = payload;
+
+    cmd->ediv = get_le16(u8ptr);
+    cmd->rand_val = get_le64(u8ptr + 2);
+}
+
+static void
+ble_sm_master_id_write(void *payload, int len, struct ble_sm_master_id *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_master_id));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_MASTER_ID;
+    put_le16(u8ptr + 1, cmd->ediv);
+    put_le64(u8ptr + 3, cmd->rand_val);
+}
+
+static void
+ble_sm_id_info_parse(void *payload, int len, struct ble_sm_id_info *cmd)
+{
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_id_info));
+
+    memcpy(cmd->irk, payload, 16);
+}
+
+static void
+ble_sm_id_info_write(void *payload, int len, struct ble_sm_id_info *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_id_info));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_IDENTITY_INFO;
+    memcpy(u8ptr + sizeof(struct ble_sm_hdr), cmd->irk, sizeof cmd->irk);
+}
+
+static void
+ble_sm_id_addr_info_parse(void *payload, int len,
+                          struct ble_sm_id_addr_info *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_id_addr_info));
+
+    u8ptr = payload;
+
+    cmd->addr_type = *u8ptr;
+    memcpy(cmd->bd_addr, u8ptr + 1, 6);
+}
+
+static void
+ble_sm_id_addr_info_write(void *payload, int len,
+                          struct ble_sm_id_addr_info *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_id_addr_info));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_IDENTITY_ADDR_INFO;
+    u8ptr[1] = cmd->addr_type;
+    memcpy(u8ptr + 2, cmd->bd_addr, sizeof cmd->bd_addr);
+}
+
+static void
+ble_sm_sign_info_parse(void *payload, int len, struct ble_sm_sign_info *cmd)
+{
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_sign_info));
+
+    memcpy(cmd->sig_key, payload, 16);
+}
+
+static void
+ble_sm_sign_info_write(void *payload, int len, struct ble_sm_sign_info *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_sign_info));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_SIGN_INFO;
+    memcpy(u8ptr + sizeof(struct ble_sm_hdr),
+           cmd->sig_key, sizeof cmd->sig_key);
+}
+
+static void
+ble_sm_sec_req_parse(void *payload, int len, struct ble_sm_sec_req *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_sec_req));
+
+    u8ptr = payload;
+    cmd->authreq = *u8ptr;
+}
+
+static void
+ble_sm_sec_req_write(void *payload, int len, struct ble_sm_sec_req *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_sec_req));
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_SM_OP_SEC_REQ;
+    u8ptr[1] = cmd->authreq;
+}
+
+static void
+ble_sm_public_key_parse(void *payload, int len, struct ble_sm_public_key *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_public_key));
+
+    u8ptr = payload;
+
+    memcpy(cmd->x, u8ptr, sizeof cmd->x);
+    u8ptr += sizeof cmd->x;
+
+    memcpy(cmd->y, u8ptr, sizeof cmd->y);
+    u8ptr += sizeof cmd->y;
+}
+
+static void
+ble_sm_public_key_write(void *payload, int len, struct ble_sm_public_key *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_public_key));
+
+    u8ptr = payload;
+
+    *u8ptr = BLE_SM_OP_PAIR_PUBLIC_KEY;
+    u8ptr++;
+
+    memcpy(u8ptr, cmd->x, sizeof cmd->x);
+    memcpy(u8ptr + 32, cmd->y, sizeof cmd->y);
+}
+
+static void
+ble_sm_dhkey_check_parse(void *payload, int len,
+                         struct ble_sm_dhkey_check *cmd)
+{
+    BLE_HS_DBG_ASSERT(len >= sizeof(struct ble_sm_dhkey_check));
+
+    memcpy(cmd->value, payload, sizeof cmd->value);
+}
+
+static void
+ble_sm_dhkey_check_write(void *payload, int len,
+                         struct ble_sm_dhkey_check *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(
+        len >= sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_dhkey_check));
+
+    u8ptr = payload;
+
+    *u8ptr = BLE_SM_OP_PAIR_DHKEY_CHECK;
+    u8ptr++;
+
+    memcpy(u8ptr, cmd->value, sizeof cmd->value);
+}
+
+void
+ble_sm_test_util_init(void)
+{
+    ble_hs_test_util_init();
+
+    ble_sm_test_gap_event_type = -1;
+    ble_sm_test_gap_status = -1;
+    memset(&ble_sm_test_repeat_pairing, 0, sizeof ble_sm_test_repeat_pairing);
+    ble_sm_test_repeat_pairing.rp.conn_handle = BLE_HS_CONN_HANDLE_NONE;
+
+    memset(&ble_sm_test_ioact, 0, sizeof ble_sm_test_ioact);
+    memset(&ble_sm_test_sec_state, 0xff, sizeof ble_sm_test_sec_state);
+}
+
+static void
+ble_sm_test_util_params_to_entity(struct ble_sm_test_params *params,
+                                  int initiator,
+                                  struct ble_sm_test_util_entity *out_entity)
+{
+    int sc;
+
+    memset(out_entity, 0, sizeof *out_entity);
+
+    sc = params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_SC &&
+         params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_SC;
+
+    if (initiator) {
+        out_entity->key_dist = params->pair_rsp.init_key_dist;
+
+        out_entity->addr_type = params->init_addr_type;
+        out_entity->id_addr = params->init_id_addr;
+        out_entity->rpa = params->init_rpa;
+
+        out_entity->pair_cmd = &params->pair_req;
+        out_entity->confirms = params->confirm_req;
+        out_entity->randoms = params->random_req;
+        out_entity->id_info = &params->id_info_rsp;
+        out_entity->id_addr_info = &params->id_addr_info_rsp;
+        out_entity->sign_info = &params->sign_info_rsp;
+
+        if (sc) {
+            out_entity->ltk = params->ltk;
+            out_entity->public_key = &params->public_key_req;
+            out_entity->dhkey_check = &params->dhkey_check_req;
+        } else {
+            out_entity->enc_info = &params->enc_info_rsp;
+            out_entity->master_id = &params->master_id_rsp;
+            if (out_entity->key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+                out_entity->rand_num = params->master_id_rsp.rand_val;
+                out_entity->ediv = params->master_id_rsp.ediv;
+                out_entity->ltk = params->enc_info_rsp.ltk;
+            }
+        }
+    } else {
+        out_entity->key_dist = params->pair_rsp.resp_key_dist;
+
+        out_entity->addr_type = params->resp_addr_type;
+        out_entity->id_addr = params->resp_id_addr;
+        out_entity->rpa = params->resp_rpa;
+
+        out_entity->pair_cmd = &params->pair_rsp;
+        out_entity->confirms = params->confirm_rsp;
+        out_entity->randoms = params->random_rsp;
+        out_entity->id_info = &params->id_info_req;
+        out_entity->id_addr_info = &params->id_addr_info_req;
+        out_entity->sign_info = &params->sign_info_req;
+
+        if (sc) {
+            out_entity->ltk = params->ltk;
+            out_entity->public_key = &params->public_key_rsp;
+            out_entity->dhkey_check = &params->dhkey_check_rsp;
+        } else {
+            out_entity->enc_info = &params->enc_info_req;
+            out_entity->master_id = &params->master_id_req;
+            if (out_entity->key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+                out_entity->rand_num = params->master_id_req.rand_val;
+                out_entity->ediv = params->master_id_req.ediv;
+                out_entity->ltk = params->enc_info_req.ltk;
+            }
+        }
+    }
+
+    out_entity->id_addr_type =
+        ble_hs_misc_own_addr_type_to_id(out_entity->addr_type);
+}
+
+static void
+ble_sm_test_util_params_to_entities(struct ble_sm_test_params *params,
+                                    int we_are_initiator,
+                                    struct ble_sm_test_util_entity *out_us,
+                                    struct ble_sm_test_util_entity *out_peer)
+{
+    ble_sm_test_util_params_to_entity(params, we_are_initiator, out_us);
+    ble_sm_test_util_params_to_entity(params, !we_are_initiator, out_peer);
+}
+
+static void
+ble_sm_test_util_init_good(struct ble_sm_test_params *params,
+                           int we_are_initiator,
+                           struct ble_hs_conn **out_conn,
+                           struct ble_sm_test_util_entity *out_us,
+                           struct ble_sm_test_util_entity *out_peer)
+{
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init();
+
+    ble_sm_test_util_params_to_entities(params, we_are_initiator,
+                                        out_us, out_peer);
+
+    ble_hs_cfg.sm_io_cap = out_us->pair_cmd->io_cap;
+    ble_hs_cfg.sm_oob_data_flag = out_us->pair_cmd->oob_data_flag;
+    ble_hs_cfg.sm_bonding = !!(out_us->pair_cmd->authreq &
+                               BLE_SM_PAIR_AUTHREQ_BOND);
+    ble_hs_cfg.sm_mitm = !!(out_us->pair_cmd->authreq &
+                            BLE_SM_PAIR_AUTHREQ_MITM);
+    ble_hs_cfg.sm_sc = !!(out_us->pair_cmd->authreq &
+                          BLE_SM_PAIR_AUTHREQ_SC);
+    ble_hs_cfg.sm_keypress = !!(out_us->pair_cmd->authreq &
+                                BLE_SM_PAIR_AUTHREQ_KEYPRESS);
+
+    if (we_are_initiator) {
+        ble_hs_cfg.sm_our_key_dist = out_us->pair_cmd->init_key_dist;
+        ble_hs_cfg.sm_their_key_dist = out_us->pair_cmd->resp_key_dist;
+    } else {
+        ble_hs_cfg.sm_our_key_dist = out_us->pair_cmd->resp_key_dist;
+        ble_hs_cfg.sm_their_key_dist = out_us->pair_cmd->init_key_dist;
+    }
+
+    ble_hs_id_set_pub(out_us->id_addr);
+    ble_sm_dbg_set_next_pair_rand(out_us->randoms[0].value);
+    ble_sm_dbg_set_next_ediv(out_us->ediv);
+    ble_sm_dbg_set_next_master_id_rand(out_us->rand_num);
+    ble_sm_dbg_set_next_ltk(out_us->ltk);
+    ble_hs_test_util_set_our_irk(out_us->id_info->irk, 0, 0);
+    ble_sm_dbg_set_next_csrk(out_us->sign_info->sig_key);
+
+    if (out_us->public_key != NULL) {
+        ble_sm_dbg_set_sc_keys((uint8_t *)out_us->public_key,
+                               params->our_priv_key);
+    }
+
+    ble_hs_test_util_create_rpa_conn(2, out_us->addr_type, out_us->rpa,
+                                     out_peer->addr_type,
+                                     out_peer->id_addr, out_peer->rpa,
+                                     BLE_HS_TEST_CONN_FEAT_ALL,
+                                     ble_sm_test_util_conn_cb,
+                                     NULL);
+
+    /* This test code and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    if (!we_are_initiator) {
+        /* Peer is the initiator so we must be the slave. */
+        conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    }
+
+    if (out_conn != NULL) {
+        *out_conn = conn;
+    }
+}
+
+static int
+ble_sm_test_util_repeat_pairing_cb(const struct ble_gap_repeat_pairing *rp)
+{
+    struct ble_store_value_sec value_sec;
+    struct ble_store_key_sec key_sec;
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    TEST_ASSERT_FATAL(rp->conn_handle != BLE_HS_CONN_HANDLE_NONE);
+
+    ble_sm_test_repeat_pairing.num_calls++;
+
+    rc = ble_gap_conn_find(rp->conn_handle, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    memset(&key_sec, 0, sizeof key_sec);
+    key_sec.peer_addr = desc.peer_id_addr;
+    rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Verify current bond is reported correctly. */
+    TEST_ASSERT(rp->cur_key_size == value_sec.key_size);
+    TEST_ASSERT(rp->cur_authenticated == value_sec.authenticated);
+    TEST_ASSERT(rp->cur_sc == value_sec.sc);
+
+    /* Verify new pairing request is reported correctly. */
+    TEST_ASSERT(
+        rp->new_key_size ==
+        min(ble_sm_test_repeat_pairing.params.pair_req.max_enc_key_size,
+            ble_sm_test_repeat_pairing.params.pair_rsp.max_enc_key_size));
+    TEST_ASSERT(
+        rp->new_authenticated ==
+            !!(ble_sm_test_repeat_pairing.params.passkey_info.passkey.action));
+    TEST_ASSERT(
+        rp->new_sc ==
+            ((ble_sm_test_repeat_pairing.params.pair_req.authreq &
+              BLE_SM_PAIR_AUTHREQ_SC)
+                &&
+             (ble_sm_test_repeat_pairing.params.pair_rsp.authreq &
+              BLE_SM_PAIR_AUTHREQ_SC)));
+    TEST_ASSERT(
+        rp->new_bonding ==
+            ((ble_sm_test_repeat_pairing.params.pair_req.authreq &
+              BLE_SM_PAIR_AUTHREQ_BOND)
+                &&
+             (ble_sm_test_repeat_pairing.params.pair_rsp.authreq &
+              BLE_SM_PAIR_AUTHREQ_BOND)));
+
+    if (ble_sm_test_repeat_pairing.rp.conn_handle ==
+        BLE_HS_CONN_HANDLE_NONE) {
+
+        ble_sm_test_repeat_pairing.rp.conn_handle = rp->conn_handle;
+    } else {
+        /* Ensure the correct connection handle gets reported each time. */
+        TEST_ASSERT(rp->conn_handle ==
+                    ble_sm_test_repeat_pairing.rp.conn_handle);
+    }
+
+    ble_sm_test_repeat_pairing.rp = *rp;
+
+    if (ble_sm_test_repeat_pairing.rc == BLE_GAP_REPEAT_PAIRING_RETRY &&
+        ble_sm_test_repeat_pairing.erase_on_retry) {
+
+        rc = ble_gap_conn_find(rp->conn_handle, &desc);
+        TEST_ASSERT_FATAL(rc == 0);
+
+        rc = ble_store_util_delete_peer(&desc.peer_id_addr);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    rc = ble_sm_test_repeat_pairing.rc;
+    ble_sm_test_repeat_pairing.rc = ble_sm_test_repeat_pairing.next_rc;
+
+    return rc;
+}
+
+int
+ble_sm_test_util_conn_cb(struct ble_gap_event *event, void *arg)
+{
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_ENC_CHANGE:
+        ble_sm_test_gap_status = event->enc_change.status;
+
+        rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_sm_test_sec_state = desc.sec_state;
+        rc = 0;
+        break;
+
+    case BLE_GAP_EVENT_PASSKEY_ACTION:
+        ble_sm_test_ioact = event->passkey.params;
+        rc = 0;
+        break;
+
+    case BLE_GAP_EVENT_REPEAT_PAIRING:
+        rc = ble_sm_test_util_repeat_pairing_cb(&event->repeat_pairing);
+        break;
+
+    default:
+        return 0;
+    }
+
+    ble_sm_test_gap_event_type = event->type;
+    return rc;
+}
+
+static void
+ble_sm_test_util_rx_pair_cmd(uint16_t conn_handle, uint8_t op,
+                             struct ble_sm_pair_cmd *cmd,
+                             int rx_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ +
+        sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_cmd));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_cmd);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_pair_cmd_write(v, payload_len, op == BLE_SM_OP_PAIR_REQ, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT(rc == rx_status);
+}
+
+static void
+ble_sm_test_util_rx_pair_req(uint16_t conn_handle,
+                             struct ble_sm_pair_cmd *req,
+                             int rx_status)
+{
+    ble_sm_test_util_rx_pair_cmd(conn_handle, BLE_SM_OP_PAIR_REQ,
+                                 req, rx_status);
+}
+
+static void
+ble_sm_test_util_rx_pair_rsp(uint16_t conn_handle, struct ble_sm_pair_cmd *rsp,
+                             int rx_status)
+{
+    ble_sm_test_util_rx_pair_cmd(conn_handle, BLE_SM_OP_PAIR_RSP,
+                                 rsp, rx_status);
+}
+
+static void
+ble_sm_test_util_rx_confirm(uint16_t conn_handle,
+                            struct ble_sm_pair_confirm *cmd)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ +
+        sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_confirm));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len =
+        sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_confirm);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_pair_confirm_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_sm_test_util_rx_random(uint16_t conn_handle,
+                           struct ble_sm_pair_random *cmd,
+                           int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_random));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_pair_random);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_pair_random_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+void
+ble_sm_test_util_rx_sec_req(uint16_t conn_handle, struct ble_sm_sec_req *cmd,
+                            int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_sec_req));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_sec_req);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_sec_req_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static void
+ble_sm_test_util_rx_public_key(uint16_t conn_handle,
+                               struct ble_sm_public_key *cmd)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_public_key));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_public_key);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_public_key_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_sm_test_util_rx_dhkey_check(uint16_t conn_handle,
+                                struct ble_sm_dhkey_check *cmd,
+                                int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_dhkey_check));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_dhkey_check);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_dhkey_check_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static void
+ble_sm_test_util_rx_enc_info(uint16_t conn_handle,
+                             struct ble_sm_enc_info *cmd,
+                             int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_enc_info));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_enc_info);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_enc_info_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static void
+ble_sm_test_util_rx_master_id(uint16_t conn_handle,
+                             struct ble_sm_master_id *cmd,
+                             int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_master_id));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_master_id);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_master_id_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static void
+ble_sm_test_util_rx_id_info(uint16_t conn_handle,
+                            struct ble_sm_id_info *cmd,
+                            int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_id_info));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_id_info);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_id_info_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static void
+ble_sm_test_util_rx_id_addr_info(uint16_t conn_handle,
+                                 struct ble_sm_id_addr_info *cmd,
+                                 int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_id_addr_info));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_id_addr_info);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_id_addr_info_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static void
+ble_sm_test_util_rx_sign_info(uint16_t conn_handle,
+                              struct ble_sm_sign_info *cmd,
+                              int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_sign_info));
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = sizeof(struct ble_sm_hdr) + sizeof(struct ble_sm_sign_info);
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_sm_sign_info_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
+static struct os_mbuf *
+ble_sm_test_util_verify_tx_hdr(uint8_t sm_op, uint16_t payload_len)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == sizeof(struct ble_sm_hdr) + payload_len);
+    TEST_ASSERT_FATAL(om->om_data[0] == sm_op);
+
+    om->om_data += sizeof(struct ble_sm_hdr);
+    om->om_len -= sizeof(struct ble_sm_hdr);
+
+    return om;
+}
+
+static void
+ble_sm_test_util_verify_tx_pair_cmd(
+    uint8_t op,
+    struct ble_sm_pair_cmd *exp_cmd)
+{
+    struct ble_sm_pair_cmd cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(op, sizeof(struct ble_sm_pair_cmd));
+    ble_sm_pair_cmd_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(cmd.io_cap == exp_cmd->io_cap);
+    TEST_ASSERT(cmd.oob_data_flag == exp_cmd->oob_data_flag);
+    TEST_ASSERT(cmd.authreq == exp_cmd->authreq);
+    TEST_ASSERT(cmd.max_enc_key_size == exp_cmd->max_enc_key_size);
+    TEST_ASSERT(cmd.init_key_dist == exp_cmd->init_key_dist);
+    TEST_ASSERT(cmd.resp_key_dist == exp_cmd->resp_key_dist);
+}
+
+static void
+ble_sm_test_util_verify_tx_pair_req(
+    struct ble_sm_pair_cmd *exp_req)
+{
+    ble_sm_test_util_verify_tx_pair_cmd(BLE_SM_OP_PAIR_REQ,
+                                              exp_req);
+}
+
+static void
+ble_sm_test_util_verify_tx_pair_rsp(
+    struct ble_sm_pair_cmd *exp_rsp)
+{
+    ble_sm_test_util_verify_tx_pair_cmd(BLE_SM_OP_PAIR_RSP,
+                                              exp_rsp);
+}
+
+static void
+ble_sm_test_util_verify_tx_pair_confirm(
+    struct ble_sm_pair_confirm *exp_cmd)
+{
+    struct ble_sm_pair_confirm cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_PAIR_CONFIRM, sizeof(cmd));
+    ble_sm_pair_confirm_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.value, exp_cmd->value, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_pair_random(
+    struct ble_sm_pair_random *exp_cmd)
+{
+    struct ble_sm_pair_random cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_PAIR_RANDOM,
+                                        sizeof(struct ble_sm_pair_random));
+    ble_sm_pair_random_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.value, exp_cmd->value, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_public_key(
+    struct ble_sm_public_key *exp_cmd)
+{
+    struct ble_sm_public_key cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_PAIR_PUBLIC_KEY,
+                                        sizeof(struct ble_sm_public_key));
+    ble_sm_public_key_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.x, exp_cmd->x, sizeof cmd.x) == 0);
+    TEST_ASSERT(memcmp(cmd.y, exp_cmd->y, sizeof cmd.y) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_dhkey_check(
+    struct ble_sm_dhkey_check *exp_cmd)
+{
+    struct ble_sm_dhkey_check cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_PAIR_DHKEY_CHECK,
+                                        sizeof(struct ble_sm_dhkey_check));
+    ble_sm_dhkey_check_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.value, exp_cmd->value, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_enc_info(struct ble_sm_enc_info *exp_cmd)
+{
+    struct ble_sm_enc_info cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_ENC_INFO,
+                                        sizeof(struct ble_sm_enc_info));
+    ble_sm_enc_info_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.ltk, exp_cmd->ltk, 16) == 0);
+
+    /* Ensure LTK is sent in little endian. */
+    TEST_ASSERT(memcmp(om->om_data, cmd.ltk, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_master_id(struct ble_sm_master_id *exp_cmd)
+{
+    struct ble_sm_master_id cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_MASTER_ID,
+                                        sizeof(struct ble_sm_master_id));
+    ble_sm_master_id_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(cmd.ediv == exp_cmd->ediv);
+    TEST_ASSERT(cmd.rand_val == exp_cmd->rand_val);
+}
+
+static void
+ble_sm_test_util_verify_tx_id_info(struct ble_sm_id_info *exp_cmd)
+{
+    struct ble_sm_id_info cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_IDENTITY_INFO,
+                                        sizeof(struct ble_sm_id_info));
+    ble_sm_id_info_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.irk, exp_cmd->irk, 16) == 0);
+
+    /* Ensure IRK is sent in little endian. */
+    TEST_ASSERT(memcmp(om->om_data, cmd.irk, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_id_addr_info(struct ble_sm_id_addr_info *exp_cmd)
+{
+    struct ble_sm_id_addr_info cmd;
+    struct os_mbuf *om;
+    const uint8_t *our_id_addr;
+    int rc;
+
+    ble_hs_lock();
+    rc = ble_hs_id_addr(exp_cmd->addr_type, &our_id_addr, NULL);
+    ble_hs_unlock();
+
+    TEST_ASSERT_FATAL(rc == 0);
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_IDENTITY_ADDR_INFO,
+                                        sizeof(struct ble_sm_id_addr_info));
+    ble_sm_id_addr_info_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(cmd.addr_type == exp_cmd->addr_type);
+    TEST_ASSERT(memcmp(cmd.bd_addr, exp_cmd->bd_addr, 6) == 0);
+    TEST_ASSERT(memcmp(cmd.bd_addr, our_id_addr, 6) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_sign_info(struct ble_sm_sign_info *exp_cmd)
+{
+    struct ble_sm_sign_info cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_SIGN_INFO,
+                                        sizeof(struct ble_sm_sign_info));
+    ble_sm_sign_info_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(memcmp(cmd.sig_key, exp_cmd->sig_key, 16) == 0);
+
+    /* Ensure CSRK is sent in little endian. */
+    TEST_ASSERT(memcmp(om->om_data, cmd.sig_key, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_sec_req(struct ble_sm_sec_req *exp_cmd)
+{
+    struct ble_sm_sec_req cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_SEC_REQ, sizeof(struct ble_sm_sec_req));
+    ble_sm_sec_req_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(cmd.authreq == exp_cmd->authreq);
+}
+
+void
+ble_sm_test_util_verify_tx_pair_fail(
+    struct ble_sm_pair_fail *exp_cmd)
+{
+    struct ble_sm_pair_fail cmd;
+    struct os_mbuf *om;
+
+    om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_PAIR_FAIL,
+                                        sizeof(struct ble_sm_pair_fail));
+    ble_sm_pair_fail_parse(om->om_data, om->om_len, &cmd);
+
+    TEST_ASSERT(cmd.reason == exp_cmd->reason);
+}
+
+static void
+ble_sm_test_util_rx_lt_key_req(uint16_t conn_handle, uint64_t r, uint16_t ediv)
+{
+    struct ble_hci_ev_le_subev_lt_key_req evt;
+    int rc;
+
+    evt.subev_code = BLE_HCI_LE_SUBEV_LT_KEY_REQ;
+    evt.conn_handle = htole16(conn_handle);
+    evt.rand = htole64(r);
+    evt.div = htole16(ediv);
+
+    rc = ble_sm_ltk_req_rx(&evt);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_lt_key_req_reply(uint16_t conn_handle, uint8_t *stk)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_LT_KEY_REQ_REPLY_LEN);
+    TEST_ASSERT(get_le16(param + 0) == conn_handle);
+    TEST_ASSERT(memcmp(param + 2, stk, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_lt_key_req_neg_reply(uint16_t conn_handle)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_LT_KEY_REQ_NEG_REPLY_LEN);
+    TEST_ASSERT(get_le16(param + 0) == conn_handle);
+}
+
+static void
+ble_sm_test_util_set_lt_key_req_neg_reply_ack(uint8_t status,
+                                              uint16_t conn_handle)
+{
+    static uint8_t params[BLE_HCI_LT_KEY_REQ_NEG_REPLY_ACK_PARAM_LEN];
+
+    put_le16(params, conn_handle);
+    ble_hs_test_util_hci_ack_set_params(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY),
+        status, params, sizeof params);
+}
+
+static void
+ble_sm_test_util_set_lt_key_req_reply_ack(uint8_t status, uint16_t conn_handle)
+{
+    static uint8_t params[BLE_HCI_LT_KEY_REQ_REPLY_ACK_PARAM_LEN];
+
+    put_le16(params, conn_handle);
+    ble_hs_test_util_hci_ack_set_params(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY),
+        status, params, sizeof params);
+}
+
+static void
+ble_sm_test_util_rx_enc_change(uint16_t conn_handle, uint8_t status,
+                                     uint8_t encryption_enabled)
+{
+    struct ble_hci_ev_enrypt_chg evt;
+
+    evt.status = status;
+    evt.enabled = encryption_enabled;
+    evt.connection_handle = htole16(conn_handle);
+
+    ble_sm_enc_change_rx(&evt);
+}
+
+static void
+ble_sm_test_util_verify_tx_start_enc(uint16_t conn_handle,
+                                     uint64_t random_number,
+                                     uint16_t ediv,
+                                     uint8_t *ltk)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_START_ENCRYPT,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_LE_START_ENCRYPT_LEN);
+    TEST_ASSERT(get_le16(param + 0) == conn_handle);
+    TEST_ASSERT(get_le64(param + 2) == random_number);
+    TEST_ASSERT(get_le16(param + 10) == ediv);
+    TEST_ASSERT(memcmp(param + 12, ltk, 16) == 0);
+}
+
+static void
+ble_sm_test_util_verify_tx_add_resolve_list(uint8_t peer_id_addr_type,
+                                            uint8_t *peer_id_addr,
+                                            uint8_t *peer_irk,
+                                            uint8_t *our_irk)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_SET_ADV_ENABLE,
+                                   NULL);
+
+    param = ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_ADD_RESOLV_LIST,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_ADD_TO_RESOLV_LIST_LEN);
+    TEST_ASSERT(param[0] == peer_id_addr_type);
+    TEST_ASSERT(memcmp(param + 1, peer_id_addr, 6) == 0);
+
+    /* Ensure IRKs are sent in little endian. */
+    TEST_ASSERT(memcmp(param + 7, peer_irk, 16) == 0);
+    TEST_ASSERT(memcmp(param + 23, our_irk, 16) == 0);
+}
+
+void
+ble_sm_test_util_io_inject(struct ble_sm_test_passkey_info *passkey_info,
+                           uint8_t cur_sm_state)
+{
+    uint8_t io_sm_state;
+    int rc;
+
+    io_sm_state = ble_sm_ioact_state(passkey_info->passkey.action);
+    if (io_sm_state != cur_sm_state) {
+        TEST_ASSERT(ble_sm_test_ioact.action == BLE_SM_IOACT_NONE);
+        return;
+    }
+
+    TEST_ASSERT(ble_sm_test_ioact.action == passkey_info->passkey.action);
+
+    if (passkey_info->passkey.action == BLE_SM_IOACT_NUMCMP) {
+        TEST_ASSERT(ble_sm_test_ioact.numcmp == passkey_info->exp_numcmp);
+    }
+
+    rc = ble_sm_inject_io(2, &passkey_info->passkey);
+    TEST_ASSERT(rc == 0);
+
+    ble_sm_test_ioact.action = BLE_SM_IOACT_NONE;
+}
+
+void
+ble_sm_test_util_io_inject_bad(uint16_t conn_handle, uint8_t correct_io_act)
+{
+    struct ble_sm_proc *proc;
+    struct ble_sm_io io;
+    uint8_t io_sm_state;
+    int already_injected;
+    int rc;
+    int i;
+
+    /* Lock mutex to prevent thread-safety assert from failing. */
+    ble_hs_lock();
+    proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL);
+    ble_hs_unlock();
+
+    TEST_ASSERT_FATAL(proc != NULL);
+
+    io_sm_state = ble_sm_ioact_state(correct_io_act);
+
+    for (i = 1; i < BLE_SM_IOACT_MAX_PLUS_ONE; i++) {
+        if (io_sm_state != proc->state  ||
+            i != correct_io_act         ||
+            proc->flags & BLE_SM_PROC_F_IO_INJECTED) {
+
+            already_injected = proc->flags & BLE_SM_PROC_F_IO_INJECTED;
+
+            io.action = i;
+            rc = ble_sm_inject_io(conn_handle, &io);
+
+            if (already_injected) {
+                TEST_ASSERT(rc == BLE_HS_EALREADY);
+            } else {
+                TEST_ASSERT(rc == BLE_HS_EINVAL);
+            }
+        }
+    }
+}
+
+void
+ble_sm_test_util_io_check_pre(struct ble_sm_test_passkey_info *passkey_info,
+                              uint8_t cur_sm_state)
+{
+    uint8_t io_sm_state;
+    int rc;
+
+    io_sm_state = ble_sm_ioact_state(passkey_info->passkey.action);
+    if (io_sm_state != cur_sm_state) {
+        return;
+    }
+
+    if (!passkey_info->io_before_rx) {
+        return;
+    }
+
+    if (passkey_info->passkey.action == BLE_SM_IOACT_NUMCMP) {
+        TEST_ASSERT(ble_sm_test_ioact.numcmp == passkey_info->exp_numcmp);
+    }
+
+    rc = ble_sm_inject_io(2, &passkey_info->passkey);
+    TEST_ASSERT(rc == 0);
+}
+
+void
+ble_sm_test_util_io_check_post(struct ble_sm_test_passkey_info *passkey_info,
+                               uint8_t cur_sm_state)
+{
+    uint8_t io_sm_state;
+    int rc;
+
+    io_sm_state = ble_sm_ioact_state(passkey_info->passkey.action);
+    if (io_sm_state != cur_sm_state) {
+        return;
+    }
+
+    if (passkey_info->io_before_rx) {
+        return;
+    }
+
+    if (passkey_info->passkey.action == BLE_SM_IOACT_NUMCMP) {
+        TEST_ASSERT(ble_sm_test_ioact.numcmp == passkey_info->exp_numcmp);
+    }
+
+    /* Ensure response not sent until user performs IO. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
+
+    rc = ble_sm_inject_io(2, &passkey_info->passkey);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+static void
+ble_sm_test_util_verify_persist(struct ble_sm_test_params *params,
+                                int we_are_initiator)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    struct ble_store_value_sec value_sec;
+    struct ble_store_key_sec key_sec;
+    int csrk_expected;
+    int ltk_expected;
+    int peer_irk_expected;
+    int our_irk_expected;
+    int bonding;
+    int sc;
+    int rc;
+
+    ble_sm_test_util_params_to_entities(params, we_are_initiator,
+                                        &our_entity, &peer_entity);
+
+    sc = params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_SC &&
+         params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_SC;
+
+    bonding = params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_BOND &&
+              params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_BOND;
+
+    memset(&key_sec, 0, sizeof key_sec);
+    key_sec.peer_addr = *BLE_ADDR_ANY;
+
+    rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+    if (!bonding) {
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+        peer_irk_expected = 0;
+    } else {
+        TEST_ASSERT_FATAL(rc == 0);
+
+        ltk_expected =
+            sc || !!(peer_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ENC);
+        peer_irk_expected =
+            !!(peer_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ID);
+        csrk_expected =
+            !!(peer_entity.key_dist & BLE_SM_PAIR_KEY_DIST_SIGN);
+
+        TEST_ASSERT(value_sec.peer_addr.type == peer_entity.id_addr_type);
+        TEST_ASSERT(
+            memcmp(value_sec.peer_addr.val, peer_entity.id_addr, 6) == 0);
+        TEST_ASSERT(value_sec.ediv == peer_entity.ediv);
+        TEST_ASSERT(value_sec.rand_num == peer_entity.rand_num);
+        TEST_ASSERT(value_sec.authenticated == params->authenticated);
+
+        TEST_ASSERT(value_sec.ltk_present == ltk_expected);
+        TEST_ASSERT(memcmp(value_sec.ltk, peer_entity.ltk, 16) == 0);
+
+        TEST_ASSERT(value_sec.irk_present == peer_irk_expected);
+        if (peer_irk_expected) {
+            TEST_ASSERT(memcmp(value_sec.irk,
+                               peer_entity.id_info->irk, 16) == 0);
+        }
+
+        TEST_ASSERT(value_sec.csrk_present == csrk_expected);
+        if (csrk_expected) {
+            TEST_ASSERT(memcmp(value_sec.csrk,
+                               peer_entity.sign_info->sig_key, 16) == 0);
+        }
+    }
+
+    rc = ble_store_read_our_sec(&key_sec, &value_sec);
+    if (!bonding) {
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+    } else {
+        TEST_ASSERT_FATAL(rc == 0);
+
+        ltk_expected =
+            sc || !!(our_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ENC);
+        our_irk_expected =
+            !!(our_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ID);
+        csrk_expected =
+            !!(our_entity.key_dist & BLE_SM_PAIR_KEY_DIST_SIGN);
+
+        TEST_ASSERT(value_sec.peer_addr.type == peer_entity.id_addr_type);
+        TEST_ASSERT(memcmp(value_sec.peer_addr.val, peer_entity.id_addr, 6) == 0);
+        TEST_ASSERT(value_sec.ediv == our_entity.ediv);
+        TEST_ASSERT(value_sec.rand_num == our_entity.rand_num);
+        TEST_ASSERT(value_sec.authenticated == params->authenticated);
+
+        TEST_ASSERT(value_sec.ltk_present == ltk_expected);
+        TEST_ASSERT(memcmp(value_sec.ltk, our_entity.ltk, 16) == 0);
+
+        TEST_ASSERT(value_sec.irk_present == our_irk_expected);
+        if (our_irk_expected) {
+            TEST_ASSERT(memcmp(value_sec.irk,
+                               our_entity.id_info->irk, 16) == 0);
+        }
+
+        TEST_ASSERT(value_sec.csrk_present == csrk_expected);
+        if (csrk_expected) {
+            TEST_ASSERT(memcmp(value_sec.csrk,
+                               our_entity.sign_info->sig_key, 16) == 0);
+        }
+    }
+
+    /* Verify no other keys were persisted. */
+    key_sec.idx++;
+    rc = ble_store_read_our_sec(&key_sec, &value_sec);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOENT);
+    rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+    TEST_ASSERT_FATAL(rc == BLE_HS_ENOENT);
+
+    /* Verify we sent the peer's IRK to the controller. */
+    if (peer_irk_expected) {
+        ble_sm_test_util_verify_tx_add_resolve_list(peer_entity.id_addr_type,
+                                                    peer_entity.id_addr,
+                                                    peer_entity.id_info->irk,
+                                                    our_entity.id_info->irk);
+    }
+}
+
+static void
+ble_sm_test_util_peer_bonding_good(int send_enc_req,
+                                   uint8_t our_addr_type,
+                                   uint8_t *our_rpa,
+                                   uint8_t peer_addr_type,
+                                   uint8_t *peer_id_addr,
+                                   uint8_t *peer_rpa,
+                                   uint8_t *ltk, int authenticated,
+                                   uint16_t ediv, uint64_t rand_num)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_hs_test_util_create_rpa_conn(2, our_addr_type, our_rpa, peer_addr_type,
+                                     peer_id_addr, peer_rpa,
+                                     BLE_HS_TEST_CONN_FEAT_ALL,
+                                     ble_sm_test_util_conn_cb, NULL);
+
+    /* This test inspects and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    if (send_enc_req) {
+        rc = ble_sm_slave_initiate(2);
+        TEST_ASSERT(rc == 0);
+    }
+
+    /* Receive a long term key request from the controller. */
+    ble_sm_test_util_set_lt_key_req_reply_ack(0, 2);
+    ble_sm_test_util_rx_lt_key_req(2, rand_num, ediv);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+
+    /* Ensure the LTK request event got sent to the application. */
+    TEST_ASSERT(ble_sm_test_store_obj_type == BLE_STORE_OBJ_TYPE_OUR_SEC);
+    TEST_ASSERT(ble_sm_test_store_key.sec.peer_addr.type ==
+                ble_hs_misc_peer_addr_type_to_id(peer_addr_type));
+    TEST_ASSERT(ble_sm_test_store_key.sec.ediv_rand_present);
+    TEST_ASSERT(ble_sm_test_store_key.sec.ediv == ediv);
+    TEST_ASSERT(ble_sm_test_store_key.sec.rand_num == rand_num);
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Ensure we sent the expected long term key request reply command. */
+    ble_sm_test_util_verify_tx_lt_key_req_reply(2, ltk);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Receive an encryption changed event. */
+    ble_sm_test_util_rx_enc_change(2, 0, 1);
+
+    /* Pairing should now be complete. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status == 0);
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                authenticated);
+
+    ble_hs_test_util_conn_disconnect(2);
+}
+
+void
+ble_sm_test_util_peer_bonding_bad(uint16_t ediv, uint64_t rand_num)
+{
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[6]){1,2,3,4,5,6}),
+                                 ble_sm_test_util_conn_cb,
+                                 NULL);
+
+    /* This test inspects and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Receive a long term key request from the controller. */
+    ble_sm_test_util_set_lt_key_req_neg_reply_ack(0, 2);
+    ble_sm_test_util_rx_lt_key_req(2, rand_num, ediv);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+
+    /* Ensure the LTK request event got sent to the application. */
+    TEST_ASSERT(ble_sm_test_store_obj_type ==
+                BLE_STORE_OBJ_TYPE_OUR_SEC);
+    TEST_ASSERT(ble_sm_test_store_key.sec.ediv_rand_present);
+    TEST_ASSERT(ble_sm_test_store_key.sec.ediv == ediv);
+    TEST_ASSERT(ble_sm_test_store_key.sec.rand_num == rand_num);
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+
+    /* Ensure we sent the expected long term key request neg reply command. */
+    ble_sm_test_util_verify_tx_lt_key_req_neg_reply(2);
+
+    /* Ensure the security procedure was aborted. */
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(!conn->bhc_sec_state.authenticated);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+}
+
+/**
+ * @param send_enc_req          Whether this procedure is initiated by a slave
+ *                                  security request;
+ *                                  1: Peer sends a security request at start.
+ *                                  0: No security request; we initiate.
+ */
+static void
+ble_sm_test_util_us_bonding_good(int send_enc_req, uint8_t our_addr_type,
+                                 uint8_t *our_rpa,
+                                 uint8_t peer_addr_type,
+                                 uint8_t *peer_id_addr, uint8_t *peer_rpa,
+                                 uint8_t *ltk, int authenticated,
+                                 uint16_t ediv, uint64_t rand_num)
+{
+    struct ble_sm_sec_req sec_req;
+    struct ble_hs_conn *conn;
+
+    ble_hs_test_util_create_rpa_conn(2, our_addr_type, our_rpa,
+                                     peer_addr_type, peer_id_addr,
+                                     peer_rpa, BLE_HS_TEST_CONN_FEAT_ALL,
+                                     ble_sm_test_util_conn_cb, NULL);
+
+    /* This test inspects and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_START_ENCRYPT),
+        0);
+
+    if (send_enc_req) {
+        sec_req.authreq = 0;
+        sec_req.authreq |= BLE_SM_PAIR_AUTHREQ_BOND;
+        if (authenticated) {
+            sec_req.authreq |= BLE_SM_PAIR_AUTHREQ_MITM;
+        }
+        ble_sm_test_util_rx_sec_req(2, &sec_req, 0);
+    } else {
+        ble_gap_security_initiate(2);
+    }
+
+    /* Ensure we sent the expected start encryption command. */
+    ble_sm_test_util_verify_tx_start_enc(2, rand_num, ediv, ltk);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Receive an encryption changed event. */
+    ble_sm_test_util_rx_enc_change(2, 0, 1);
+
+    /* Pairing should now be complete. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status == 0);
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                authenticated);
+
+    ble_hs_test_util_conn_disconnect(2);
+}
+
+void
+ble_sm_test_util_peer_fail_inval(
+    int we_are_master,
+    uint8_t *init_id_addr,
+    uint8_t *resp_addr,
+    struct ble_sm_pair_cmd *pair_req,
+    struct ble_sm_pair_fail *pair_fail)
+{
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init();
+    ble_hs_id_set_pub(resp_addr);
+
+    ble_hs_test_util_create_conn(2, init_id_addr, ble_sm_test_util_conn_cb,
+                                 NULL);
+
+    /* This test inspects and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    if (!we_are_master) {
+        conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    }
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Receive a pair request from the peer. */
+    ble_sm_test_util_rx_pair_req(2, pair_req,
+                                 BLE_HS_SM_US_ERR(pair_fail->reason));
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Ensure we sent the expected pair fail. */
+    ble_sm_test_util_verify_tx_pair_fail(pair_fail);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was not executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == -1);
+    TEST_ASSERT(ble_sm_test_gap_status == -1);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(!conn->bhc_sec_state.authenticated);
+}
+
+void
+ble_sm_test_util_peer_lgcy_fail_confirm(
+    uint8_t *init_id_addr,
+    uint8_t *resp_addr,
+    struct ble_sm_pair_cmd *pair_req,
+    struct ble_sm_pair_cmd *pair_rsp,
+    struct ble_sm_pair_confirm *confirm_req,
+    struct ble_sm_pair_confirm *confirm_rsp,
+    struct ble_sm_pair_random *random_req,
+    struct ble_sm_pair_random *random_rsp,
+    struct ble_sm_pair_fail *fail_rsp)
+{
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init();
+    ble_hs_id_set_pub(resp_addr);
+    ble_sm_dbg_set_next_pair_rand(random_rsp->value);
+
+    if (pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_SC) {
+        ble_hs_cfg.sm_sc = 1;
+    } else {
+        ble_hs_cfg.sm_sc = 0;
+    }
+
+    ble_hs_test_util_create_conn(2, init_id_addr, ble_sm_test_util_conn_cb,
+                                 NULL);
+
+    /* This test inspects and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    /* Peer is the initiator so we must be the slave. */
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Receive a pair request from the peer. */
+    ble_sm_test_util_rx_pair_req(2, pair_req, 0);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Ensure we sent the expected pair response. */
+    ble_sm_test_util_verify_tx_pair_rsp(pair_rsp);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Receive a pair confirm from the peer. */
+    ble_sm_test_util_rx_confirm(2, confirm_req);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Ensure we sent the expected pair confirm. */
+    ble_sm_test_util_verify_tx_pair_confirm(confirm_rsp);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, BLE_SM_IOACT_NONE);
+
+    /* Receive a pair random from the peer. */
+    ble_sm_test_util_rx_random(
+        2, random_req, BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH));
+
+    /* Ensure we sent the expected pair fail. */
+    ble_sm_test_util_verify_tx_pair_fail(fail_rsp);
+
+    /* The proc should now be freed. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status ==
+                BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH));
+    TEST_ASSERT(!ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(!ble_sm_test_sec_state.authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted ==
+                conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                conn->bhc_sec_state.authenticated);
+}
+
+static void
+ble_sm_test_util_bonding_all(struct ble_sm_test_params *params,
+                             int we_are_original_initiator)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    int sc;
+
+    if (!(params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_BOND) ||
+        !(params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_BOND)) {
+
+        /* Bonding not performed. */
+        return;
+    }
+
+    sc = params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_SC &&
+         params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_SC;
+
+    ble_sm_test_util_params_to_entities(params, we_are_original_initiator,
+                                        &our_entity, &peer_entity);
+
+    if (sc || peer_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+        /* We are master; we initiate procedure. */
+        ble_sm_test_util_us_bonding_good(0, our_entity.addr_type,
+                                         our_entity.rpa,
+                                         peer_entity.addr_type,
+                                         peer_entity.id_addr,
+                                         peer_entity.rpa,
+                                         peer_entity.ltk,
+                                         params->authenticated,
+                                         peer_entity.ediv,
+                                         peer_entity.rand_num);
+
+        /* We are master; peer initiates procedure via security request. */
+        ble_sm_test_util_us_bonding_good(1, our_entity.addr_type,
+                                         our_entity.rpa,
+                                         peer_entity.addr_type,
+                                         peer_entity.id_addr,
+                                         peer_entity.rpa,
+                                         peer_entity.ltk,
+                                         params->authenticated,
+                                         peer_entity.ediv,
+                                         peer_entity.rand_num);
+    }
+
+    if (sc || our_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+        /* Peer is master; peer initiates procedure. */
+        ble_sm_test_util_peer_bonding_good(0, our_entity.addr_type,
+                                           our_entity.rpa,
+                                           peer_entity.addr_type,
+                                           peer_entity.id_addr,
+                                           peer_entity.rpa,
+                                           our_entity.ltk,
+                                           params->authenticated,
+                                           our_entity.ediv,
+                                           our_entity.rand_num);
+
+        /* Peer is master; we initiate procedure via security request. */
+        ble_sm_test_util_peer_bonding_good(1, our_entity.addr_type,
+                                           our_entity.rpa,
+                                           peer_entity.addr_type,
+                                           peer_entity.id_addr,
+                                           peer_entity.rpa,
+                                           our_entity.ltk,
+                                           params->authenticated,
+                                           our_entity.ediv,
+                                           our_entity.rand_num);
+    }
+}
+
+static void
+ble_sm_test_util_rx_keys(struct ble_sm_test_params *params,
+                         int we_are_initiator)
+{
+    struct ble_sm_id_addr_info *peer_id_addr_info;
+    struct ble_sm_sign_info *peer_sign_info;
+    struct ble_sm_master_id *peer_master_id;
+    struct ble_sm_enc_info *peer_enc_info;
+    struct ble_sm_id_info *peer_id_info;
+    uint8_t peer_key_dist;
+    int sc;
+
+    if (we_are_initiator) {
+        peer_key_dist = params->pair_rsp.resp_key_dist;
+        peer_id_addr_info = &params->id_addr_info_req;
+        peer_sign_info = &params->sign_info_req;
+        peer_master_id = &params->master_id_req;
+        peer_enc_info = &params->enc_info_req;
+        peer_id_info = &params->id_info_req;
+    } else {
+        peer_key_dist = params->pair_rsp.init_key_dist;
+        peer_id_addr_info = &params->id_addr_info_rsp;
+        peer_sign_info = &params->sign_info_rsp;
+        peer_master_id = &params->master_id_rsp;
+        peer_enc_info = &params->enc_info_rsp;
+        peer_id_info = &params->id_info_rsp;
+    }
+
+    sc = params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_SC &&
+         params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_SC;
+
+    /* Receive key material from peer. */
+    if (!sc && (peer_key_dist & BLE_SM_PAIR_KEY_DIST_ENC)) {
+        ble_sm_test_util_rx_enc_info(2, peer_enc_info, 0);
+        ble_sm_test_util_rx_master_id(2, peer_master_id, 0);
+    }
+    if (peer_key_dist & BLE_SM_PAIR_KEY_DIST_ID) {
+
+        ble_hs_test_util_hci_ack_set_seq(((struct ble_hs_test_util_hci_ack[]) {
+            {
+                .opcode = ble_hs_hci_util_opcode_join(
+                                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE),
+            },
+            {
+                .opcode = ble_hs_hci_util_opcode_join(
+                                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
+            },
+            {
+                .opcode = ble_hs_hci_util_opcode_join(
+                                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
+            },
+            { 0 }
+        }));
+
+        ble_sm_test_util_rx_id_info(2, peer_id_info, 0);
+        ble_sm_test_util_rx_id_addr_info(2, peer_id_addr_info, 0);
+    }
+    if (peer_key_dist & BLE_SM_PAIR_KEY_DIST_SIGN) {
+        ble_sm_test_util_rx_sign_info(2, peer_sign_info, 0);
+    }
+}
+
+static void
+ble_sm_test_util_verify_tx_keys(struct ble_sm_test_params *params,
+                                int we_are_initiator)
+{
+    struct ble_sm_id_addr_info *our_id_addr_info;
+    struct ble_sm_sign_info *our_sign_info;
+    struct ble_sm_master_id *our_master_id;
+    struct ble_sm_enc_info *our_enc_info;
+    struct ble_sm_id_info *our_id_info;
+    uint8_t our_key_dist;
+    int sc;
+
+    if (we_are_initiator) {
+        our_key_dist = params->pair_rsp.init_key_dist;
+        our_id_addr_info = &params->id_addr_info_rsp;
+        our_sign_info = &params->sign_info_rsp;
+        our_master_id = &params->master_id_rsp;
+        our_enc_info = &params->enc_info_rsp;
+        our_id_info = &params->id_info_rsp;
+    } else {
+        our_key_dist = params->pair_rsp.resp_key_dist;
+        our_id_addr_info = &params->id_addr_info_req;
+        our_sign_info = &params->sign_info_req;
+        our_master_id = &params->master_id_req;
+        our_enc_info = &params->enc_info_req;
+        our_id_info = &params->id_info_req;
+    }
+
+    sc = params->pair_req.authreq & BLE_SM_PAIR_AUTHREQ_SC &&
+         params->pair_rsp.authreq & BLE_SM_PAIR_AUTHREQ_SC;
+
+    if (!sc && our_key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
+        ble_sm_test_util_verify_tx_enc_info(our_enc_info);
+        ble_sm_test_util_verify_tx_master_id(our_master_id);
+    }
+    if (our_key_dist & BLE_SM_PAIR_KEY_DIST_ID) {
+        ble_sm_test_util_verify_tx_id_info(our_id_info);
+        ble_sm_test_util_verify_tx_id_addr_info(our_id_addr_info);
+    }
+    if (our_key_dist & BLE_SM_PAIR_KEY_DIST_SIGN) {
+        ble_sm_test_util_verify_tx_sign_info(our_sign_info);
+    }
+}
+
+static void
+ble_sm_test_util_us_lgcy_good_once_no_init(
+    struct ble_sm_test_params *params,
+    struct ble_hs_conn *conn,
+    struct ble_sm_test_util_entity *our_entity,
+    struct ble_sm_test_util_entity *peer_entity)
+{
+    int rc;
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_START_ENCRYPT), 0);
+    if (params->sec_req.authreq != 0) {
+        ble_sm_test_util_rx_sec_req(2, &params->sec_req, 0);
+    } else {
+        /* Initiate the pairing procedure. */
+        rc = ble_gap_security_initiate(2);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    /* Ensure we sent the expected pair request. */
+    ble_sm_test_util_verify_tx_pair_req(our_entity->pair_cmd);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a pair response from the peer. */
+    ble_sm_test_util_rx_pair_rsp(2, peer_entity->pair_cmd, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    ble_sm_test_util_io_inject(&params->passkey_info,
+                               BLE_SM_PROC_STATE_CONFIRM);
+
+    /* Ensure we sent the expected pair confirm. */
+    ble_sm_test_util_verify_tx_pair_confirm(our_entity->confirms);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a pair confirm from the peer. */
+    ble_sm_test_util_rx_confirm(2, peer_entity->confirms);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected pair random. */
+    ble_sm_test_util_verify_tx_pair_random(our_entity->randoms);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a pair random from the peer. */
+    ble_sm_test_util_rx_random(2, peer_entity->randoms, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected start encryption command. */
+    ble_sm_test_util_verify_tx_start_enc(2, 0, 0, params->stk);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive an encryption changed event. */
+    ble_sm_test_util_rx_enc_change(2, 0, 1);
+
+    /* Receive key material from peer. */
+    ble_sm_test_util_rx_keys(params, 1);
+
+    /* Verify key material gets sent to peer. */
+    ble_sm_test_util_verify_tx_keys(params, 1);
+
+    /* Pairing should now be complete. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status == 0);
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated == params->authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted ==
+                conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                conn->bhc_sec_state.authenticated);
+
+    /* Verify the appropriate security material was persisted. */
+    ble_sm_test_util_verify_persist(params, 1);
+
+    ble_hs_test_util_conn_disconnect(2);
+}
+
+static void
+ble_sm_test_util_us_lgcy_good_once(struct ble_sm_test_params *params)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init_good(params, 1, &conn, &our_entity, &peer_entity);
+    ble_sm_test_util_us_lgcy_good_once_no_init(
+        params, conn, &our_entity, &peer_entity);
+}
+
+void
+ble_sm_test_util_us_lgcy_good(struct ble_sm_test_params *params)
+{
+    ble_addr_t peer_addr;
+    int rc;
+
+    /*** We are master. */
+
+    /* We initiate pairing. */
+    params->sec_req.authreq = 0;
+    ble_sm_test_util_us_lgcy_good_once(params);
+
+    /* Peer initiates with security request. */
+    params->sec_req.authreq = params->pair_rsp.authreq;
+    ble_sm_test_util_us_lgcy_good_once(params);
+
+    /* Verify link can be restored via the encryption procedure. */
+    ble_sm_test_util_bonding_all(params, 1);
+
+    /* Verify programmatic unbonding. */
+    peer_addr.type = ble_hs_misc_peer_addr_type_to_id(params->resp_addr_type);
+    memcpy(peer_addr.val, params->resp_id_addr, sizeof peer_addr.val);
+    rc = ble_store_util_delete_peer(&peer_addr);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_hs_test_util_num_our_secs() == 0);
+    TEST_ASSERT(ble_hs_test_util_num_peer_secs() == 0);
+}
+
+void
+ble_sm_test_util_peer_lgcy_good_once_no_init(
+    struct ble_sm_test_params *params,
+    struct ble_hs_conn *conn,
+    struct ble_sm_test_util_entity *our_entity,
+    struct ble_sm_test_util_entity *peer_entity)
+{
+    int rc;
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    if (params->sec_req.authreq != 0) {
+        rc = ble_sm_slave_initiate(2);
+        TEST_ASSERT(rc == 0);
+
+        /* Ensure we sent the expected security request. */
+        ble_sm_test_util_verify_tx_sec_req(&params->sec_req);
+    }
+
+    /* Receive a pair request from the peer. */
+    ble_sm_test_util_rx_pair_req(2, peer_entity->pair_cmd, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected pair response. */
+    ble_sm_test_util_verify_tx_pair_rsp(our_entity->pair_cmd);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    ble_sm_test_util_io_check_pre(&params->passkey_info,
+                                  BLE_SM_PROC_STATE_CONFIRM);
+
+    /* Receive a pair confirm from the peer. */
+    ble_sm_test_util_rx_confirm(2, peer_entity->confirms);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    ble_sm_test_util_io_check_post(&params->passkey_info,
+                                   BLE_SM_PROC_STATE_CONFIRM);
+
+    /* Ensure we sent the expected pair confirm. */
+    ble_sm_test_util_verify_tx_pair_confirm(our_entity->confirms);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a pair random from the peer. */
+    ble_sm_test_util_rx_random(2, peer_entity->randoms, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected pair random. */
+    ble_sm_test_util_verify_tx_pair_random(our_entity->randoms);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a long term key request from the controller. */
+    ble_sm_test_util_set_lt_key_req_reply_ack(0, 2);
+    ble_sm_test_util_rx_lt_key_req(2, 0, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected long term key request reply command. */
+    ble_sm_test_util_verify_tx_lt_key_req_reply(2, params->stk);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive an encryption changed event. */
+    ble_sm_test_util_rx_enc_change(2, 0, 1);
+
+    /* Verify key material gets sent to peer. */
+    ble_sm_test_util_verify_tx_keys(params, 0);
+
+    /* Receive key material from peer. */
+    ble_sm_test_util_rx_keys(params, 0);
+
+    /* Pairing should now be complete. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status == 0);
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                params->authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted ==
+                conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                conn->bhc_sec_state.authenticated);
+
+    /* Verify the appropriate security material was persisted. */
+    ble_sm_test_util_verify_persist(params, 0);
+
+    ble_hs_test_util_conn_disconnect(2);
+}
+
+void
+ble_sm_test_util_peer_lgcy_good_once(struct ble_sm_test_params *params)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init_good(params, 0, &conn, &our_entity, &peer_entity);
+    ble_sm_test_util_peer_lgcy_good_once_no_init(
+        params, conn, &our_entity, &peer_entity);
+}
+
+void
+ble_sm_test_util_peer_lgcy_good(struct ble_sm_test_params *params)
+{
+    ble_addr_t peer_addr;
+    int rc;
+
+    /*** Peer is master. */
+
+    /* Peer performs IO first; peer initiates pairing. */
+    params->passkey_info.io_before_rx = 0;
+    params->sec_req.authreq = 0;
+    ble_sm_test_util_peer_lgcy_good_once(params);
+
+    /* Peer performs IO first; we initiate with security request. */
+    params->passkey_info.io_before_rx = 0;
+    params->sec_req.authreq = params->pair_rsp.authreq;
+    ble_sm_test_util_peer_lgcy_good_once(params);
+
+    /* We perform IO first; peer initiates pairing. */
+    params->passkey_info.io_before_rx = 1;
+    params->sec_req.authreq = 0;
+    ble_sm_test_util_peer_lgcy_good_once(params);
+
+    /* We perform IO first; we initiate with security request. */
+    params->passkey_info.io_before_rx = 1;
+    params->sec_req.authreq = params->pair_rsp.authreq;
+    ble_sm_test_util_peer_lgcy_good_once(params);
+
+    /* Verify link can be restored via the encryption procedure. */
+    ble_sm_test_util_bonding_all(params, 0);
+
+    /* Verify repeating pairing event generated when peer attempts to pair
+     * while bonded.
+     */
+    ble_sm_test_util_repeat_pairing(params, 0);
+
+    /* Verify programmatic unbonding. */
+    peer_addr.type = ble_hs_misc_peer_addr_type_to_id(params->init_addr_type);
+    memcpy(peer_addr.val, params->init_id_addr, sizeof peer_addr.val);
+    rc = ble_store_util_delete_peer(&peer_addr);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_hs_test_util_num_our_secs() == 0);
+    TEST_ASSERT(ble_hs_test_util_num_peer_secs() == 0);
+}
+
+static void
+ble_sm_test_util_us_sc_good_once_no_init(
+    struct ble_sm_test_params *params,
+    struct ble_hs_conn *conn,
+    struct ble_sm_test_util_entity *our_entity,
+    struct ble_sm_test_util_entity *peer_entity)
+{
+    int num_iters;
+    int rc;
+    int i;
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    ble_hs_test_util_hci_ack_set(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_START_ENCRYPT), 0);
+    if (params->sec_req.authreq != 0) {
+        ble_sm_test_util_rx_sec_req(2, &params->sec_req, 0);
+    } else {
+        /* Initiate the pairing procedure. */
+        rc = ble_gap_security_initiate(2);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    /* Ensure we sent the expected pair request. */
+    ble_sm_test_util_verify_tx_pair_req(our_entity->pair_cmd);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a pair response from the peer. */
+    ble_sm_test_util_rx_pair_rsp(2, peer_entity->pair_cmd, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected public key. */
+    ble_sm_test_util_verify_tx_public_key(our_entity->public_key);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a public key from the peer. */
+    ble_sm_test_util_rx_public_key(2, peer_entity->public_key);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    switch (params->pair_alg) {
+    case BLE_SM_PAIR_ALG_PASSKEY:
+        num_iters = 20;
+        break;
+
+    default:
+        num_iters = 1;
+        break;
+    }
+
+    ble_sm_test_util_io_inject(&params->passkey_info,
+                               BLE_SM_PROC_STATE_CONFIRM);
+
+    for (i = 0; i < num_iters; i++) {
+        if (params->pair_alg != BLE_SM_PAIR_ALG_JW      &&
+            params->pair_alg != BLE_SM_PAIR_ALG_NUMCMP) {
+
+            if (i < num_iters - 1) {
+                ble_sm_dbg_set_next_pair_rand(
+                    our_entity->randoms[i + 1].value);
+            }
+
+            /* Ensure we sent the expected pair confirm. */
+            ble_sm_test_util_verify_tx_pair_confirm(our_entity->confirms + i);
+            TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+            TEST_ASSERT(ble_sm_num_procs() == 1);
+            ble_sm_test_util_io_inject_bad(
+                2, params->passkey_info.passkey.action);
+        }
+
+        /* Receive a pair confirm from the peer. */
+        ble_sm_test_util_rx_confirm(2, peer_entity->confirms + i);
+        TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+        TEST_ASSERT(ble_sm_num_procs() == 1);
+        ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+        /* Ensure we sent the expected pair random. */
+        ble_sm_test_util_verify_tx_pair_random(our_entity->randoms + i);
+        TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+        TEST_ASSERT(ble_sm_num_procs() == 1);
+        ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+        /* Receive a pair random from the peer. */
+        ble_sm_test_util_rx_random(2, peer_entity->randoms + i, 0);
+        TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+        TEST_ASSERT(ble_sm_num_procs() == 1);
+        ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+    }
+
+    ble_sm_test_util_io_inject(&params->passkey_info,
+                               BLE_SM_PROC_STATE_DHKEY_CHECK);
+
+    /* Ensure we sent the expected dhkey check. */
+    ble_sm_test_util_verify_tx_dhkey_check(our_entity->dhkey_check);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a dhkey check from the peer. */
+    ble_sm_test_util_rx_dhkey_check(2, peer_entity->dhkey_check, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected start encryption command. */
+    ble_sm_test_util_verify_tx_start_enc(2, 0, 0, params->ltk);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive an encryption changed event. */
+    ble_sm_test_util_rx_enc_change(2, 0, 1);
+
+    /* Receive key material from peer. */
+    ble_sm_test_util_rx_keys(params, 1);
+
+    /* Verify key material gets sent to peer. */
+    ble_sm_test_util_verify_tx_keys(params, 1);
+
+    /* Pairing should now be complete. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status == 0);
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                params->authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted ==
+                conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                conn->bhc_sec_state.authenticated);
+
+    /* Verify the appropriate security material was persisted. */
+    ble_sm_test_util_verify_persist(params, 1);
+
+    ble_hs_test_util_conn_disconnect(2);
+}
+
+static void
+ble_sm_test_util_us_sc_good_once(struct ble_sm_test_params *params)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init_good(params, 1, &conn, &our_entity, &peer_entity);
+    ble_sm_test_util_us_sc_good_once_no_init(
+        params, conn, &our_entity, &peer_entity);
+}
+
+void
+ble_sm_test_util_us_sc_good(struct ble_sm_test_params *params)
+{
+    ble_addr_t peer_addr;
+    int rc;
+
+    /*** We are master. */
+
+    /* We initiate pairing. */
+    params->passkey_info.io_before_rx = 0;
+    params->sec_req.authreq = 0;
+    ble_sm_test_util_us_sc_good_once(params);
+
+    /* Peer initiates with security request. */
+    params->passkey_info.io_before_rx = 0;
+    params->sec_req.authreq = params->pair_rsp.authreq;
+    ble_sm_test_util_us_sc_good_once(params);
+
+    /* Verify link can be restored via the encryption procedure. */
+    ble_sm_test_util_bonding_all(params, 1);
+
+    /* Verify programmatic unbonding. */
+    peer_addr.type = ble_hs_misc_peer_addr_type_to_id(params->resp_addr_type);
+    memcpy(peer_addr.val, params->resp_id_addr, sizeof peer_addr.val);
+    rc = ble_store_util_delete_peer(&peer_addr);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_hs_test_util_num_our_secs() == 0);
+    TEST_ASSERT(ble_hs_test_util_num_peer_secs() == 0);
+}
+
+static void
+ble_sm_test_util_peer_sc_good_once_no_init(
+    struct ble_sm_test_params *params,
+    struct ble_hs_conn *conn,
+    struct ble_sm_test_util_entity *our_entity,
+    struct ble_sm_test_util_entity *peer_entity)
+{
+    int num_iters;
+    int rc;
+    int i;
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    if (params->sec_req.authreq != 0) {
+        rc = ble_sm_slave_initiate(2);
+        TEST_ASSERT(rc == 0);
+
+        /* Ensure we sent the expected security request. */
+        ble_sm_test_util_verify_tx_sec_req(&params->sec_req);
+    }
+
+    /* Receive a pair request from the peer. */
+    ble_sm_test_util_rx_pair_req(2, peer_entity->pair_cmd, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected pair response. */
+    ble_sm_test_util_verify_tx_pair_rsp(our_entity->pair_cmd);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a public key from the peer. */
+    ble_sm_test_util_rx_public_key(2, peer_entity->public_key);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected public key. */
+    ble_sm_test_util_verify_tx_public_key(our_entity->public_key);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    switch (params->pair_alg) {
+    case BLE_SM_PAIR_ALG_PASSKEY:
+        num_iters = 20;
+        break;
+
+    default:
+        num_iters = 1;
+        break;
+    }
+
+    ble_sm_test_util_io_check_pre(&params->passkey_info,
+                                  BLE_SM_PROC_STATE_CONFIRM);
+
+    for (i = 0; i < num_iters; i++) {
+        if (params->pair_alg != BLE_SM_PAIR_ALG_JW      &&
+            params->pair_alg != BLE_SM_PAIR_ALG_NUMCMP) {
+
+            /* Receive a pair confirm from the peer. */
+            ble_sm_test_util_rx_confirm(2, peer_entity->confirms + i);
+            TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+            TEST_ASSERT(ble_sm_num_procs() == 1);
+            ble_sm_test_util_io_inject_bad(
+                2, params->passkey_info.passkey.action);
+
+            if (i < num_iters - 1) {
+                ble_sm_dbg_set_next_pair_rand(
+                    our_entity->randoms[i + 1].value);
+            }
+        }
+
+        if (i == 0) {
+            ble_sm_test_util_io_check_post(&params->passkey_info,
+                                           BLE_SM_PROC_STATE_CONFIRM);
+        }
+
+        /* Ensure we sent the expected pair confirm. */
+        ble_sm_test_util_verify_tx_pair_confirm(our_entity->confirms + i);
+        TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+        TEST_ASSERT(ble_sm_num_procs() == 1);
+        ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+        /* Receive a pair random from the peer. */
+        ble_sm_test_util_rx_random(2, peer_entity->randoms + i, 0);
+        TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+        TEST_ASSERT(ble_sm_num_procs() == 1);
+        ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+        /* Ensure we sent the expected pair random. */
+        ble_sm_test_util_verify_tx_pair_random(our_entity->randoms + i);
+        TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+        TEST_ASSERT(ble_sm_num_procs() == 1);
+        ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    }
+
+    ble_sm_test_util_io_check_pre(&params->passkey_info,
+                                  BLE_SM_PROC_STATE_DHKEY_CHECK);
+
+    /* Receive a dhkey check from the peer. */
+    ble_sm_test_util_rx_dhkey_check(2, peer_entity->dhkey_check, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    ble_sm_test_util_io_check_post(&params->passkey_info,
+                                   BLE_SM_PROC_STATE_DHKEY_CHECK);
+
+    /* Ensure we sent the expected dhkey check. */
+    ble_sm_test_util_verify_tx_dhkey_check(our_entity->dhkey_check);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a long term key request from the controller. */
+    ble_sm_test_util_set_lt_key_req_reply_ack(0, 2);
+    ble_sm_test_util_rx_lt_key_req(2, 0, 0);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Ensure we sent the expected long term key request reply command. */
+    ble_sm_test_util_verify_tx_lt_key_req_reply(2, params->ltk);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive an encryption changed event. */
+    ble_sm_test_util_rx_enc_change(2, 0, 1);
+
+    /* Verify key material gets sent to peer. */
+    ble_sm_test_util_verify_tx_keys(params, 0);
+
+    /* Receive key material from peer. */
+    ble_sm_test_util_rx_keys(params, 0);
+
+    /* Pairing should now be complete. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_status == 0);
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                params->authenticated);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(ble_sm_test_sec_state.encrypted ==
+                conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
+                conn->bhc_sec_state.authenticated);
+
+    /* Verify the appropriate security material was persisted. */
+    ble_sm_test_util_verify_persist(params, 0);
+
+    ble_hs_test_util_conn_disconnect(2);
+}
+
+static void
+ble_sm_test_util_peer_sc_good_once(struct ble_sm_test_params *params)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_init_good(params, 0, &conn, &our_entity, &peer_entity);
+    ble_sm_test_util_peer_sc_good_once_no_init(
+        params, conn, &our_entity, &peer_entity);
+}
+
+void
+ble_sm_test_util_peer_sc_good(struct ble_sm_test_params *params)
+{
+    ble_addr_t peer_addr;
+    int rc;
+
+    /*** Peer is master. */
+
+    /* Peer performs IO first; peer initiates pairing. */
+    params->passkey_info.io_before_rx = 0;
+    params->sec_req.authreq = 0;
+    ble_sm_test_util_peer_sc_good_once(params);
+
+    /* Peer performs IO first; we initiate with security request. */
+    params->passkey_info.io_before_rx = 0;
+    params->sec_req.authreq = params->pair_req.authreq;
+    ble_sm_test_util_peer_sc_good_once(params);
+
+    /* We perform IO first; peer initiates pairing. */
+    params->passkey_info.io_before_rx = 1;
+    params->sec_req.authreq = 0;
+    ble_sm_test_util_peer_sc_good_once(params);
+
+    /* We perform IO first; we initiate with security request. */
+    params->passkey_info.io_before_rx = 1;
+    params->sec_req.authreq = params->pair_req.authreq;
+    ble_sm_test_util_peer_sc_good_once(params);
+
+    /* Verify link can be restored via the encryption procedure. */
+    ble_sm_test_util_bonding_all(params, 0);
+
+    /* Verify repeating pairing event generated when peer attempts to pair
+     * while bonded.
+     */
+    ble_sm_test_util_repeat_pairing(params, 1);
+
+    /* Verify programmatic unbonding. */
+    peer_addr.type = ble_hs_misc_peer_addr_type_to_id(params->init_addr_type);
+    memcpy(peer_addr.val, params->init_id_addr, sizeof peer_addr.val);
+    rc = ble_store_util_delete_peer(&peer_addr);
+    TEST_ASSERT(rc == 0);
+
+    TEST_ASSERT(ble_hs_test_util_num_our_secs() == 0);
+    TEST_ASSERT(ble_hs_test_util_num_peer_secs() == 0);
+}
+
+void
+ble_sm_test_util_us_fail_inval(struct ble_sm_test_params *params)
+{
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_sm_test_util_init();
+    ble_hs_id_set_pub(params->resp_id_addr);
+
+    ble_sm_dbg_set_next_pair_rand(((uint8_t[16]){0}));
+
+    ble_hs_test_util_create_conn(2, params->init_id_addr,
+                                 ble_sm_test_util_conn_cb,
+                                 NULL);
+
+    /* This test inspects and modifies the connection object after unlocking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Initiate the pairing procedure. */
+    rc = ble_hs_test_util_security_initiate(2, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure we sent the expected pair request. */
+    ble_sm_test_util_verify_tx_pair_req(&params->pair_req);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 1);
+    ble_sm_test_util_io_inject_bad(2, params->passkey_info.passkey.action);
+
+    /* Receive a pair response from the peer. */
+    ble_sm_test_util_rx_pair_rsp(
+        2, &params->pair_rsp, BLE_HS_SM_US_ERR(params->pair_fail.reason));
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Ensure we sent the expected pair fail. */
+    ble_sm_test_util_verify_tx_pair_fail(&params->pair_fail);
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify that security callback was not executed. */
+    TEST_ASSERT(ble_sm_test_gap_event_type == -1);
+    TEST_ASSERT(ble_sm_test_gap_status == -1);
+
+    /* Verify that connection has correct security state. */
+    TEST_ASSERT(!conn->bhc_sec_state.encrypted);
+    TEST_ASSERT(!conn->bhc_sec_state.authenticated);
+}
+
+static void
+ble_sm_test_util_repeat_pairing(struct ble_sm_test_params *params, int sc)
+{
+    struct ble_sm_test_util_entity peer_entity;
+    struct ble_sm_test_util_entity our_entity;
+    struct ble_hs_conn *conn;
+
+    ble_sm_test_util_params_to_entities(params, 0, &our_entity, &peer_entity);
+
+    ble_sm_test_repeat_pairing.params = *params;
+    ble_hs_id_set_pub(our_entity.id_addr);
+    ble_sm_dbg_set_next_pair_rand(our_entity.randoms[0].value);
+    ble_sm_dbg_set_next_ediv(our_entity.ediv);
+    ble_sm_dbg_set_next_master_id_rand(our_entity.rand_num);
+    ble_sm_dbg_set_next_ltk(our_entity.ltk);
+    ble_hs_test_util_set_our_irk(our_entity.id_info->irk, 0, 0);
+    ble_sm_dbg_set_next_csrk(our_entity.sign_info->sig_key);
+
+    ble_hs_test_util_create_rpa_conn(2, our_entity.addr_type, our_entity.rpa,
+                                     peer_entity.addr_type,
+                                     peer_entity.id_addr, peer_entity.rpa,
+                                     BLE_HS_TEST_CONN_FEAT_ALL,
+                                     ble_sm_test_util_conn_cb,
+                                     NULL);
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    ble_hs_test_util_prev_tx_queue_clear();
+
+    /* First repeat pairing event:  retry;
+     * Second repeat pairing event: ignore.
+     */
+    ble_sm_test_repeat_pairing.rc = BLE_GAP_REPEAT_PAIRING_RETRY;
+    ble_sm_test_repeat_pairing.next_rc = BLE_GAP_REPEAT_PAIRING_IGNORE;
+
+    /* Receive a pair request from the peer. */
+    ble_sm_test_util_rx_pair_req(2, peer_entity.pair_cmd, BLE_HS_EALREADY);
+
+    /* Verify repeat pairing event got reported twice. */
+    TEST_ASSERT(ble_sm_test_repeat_pairing.num_calls == 2);
+
+    /* Verify no pairing procedures in progress. */
+    TEST_ASSERT(ble_sm_num_procs() == 0);
+
+    /* Verify no SM messages were sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+
+    /*** Receive another pairing request. */
+
+    ble_sm_test_repeat_pairing.num_calls = 0;
+
+    /* First repeat pairing event: erase and retry. */
+    ble_sm_test_repeat_pairing.rc = BLE_GAP_REPEAT_PAIRING_RETRY;
+    ble_sm_test_repeat_pairing.erase_on_retry = 1;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    /* Receive a pair request from the peer; verify pairing procedure completes
+     * successfully.
+     */
+    if (!sc) {
+        ble_sm_test_util_peer_lgcy_good_once_no_init(
+            params, conn, &our_entity, &peer_entity);
+    } else {
+        ble_sm_test_util_peer_sc_good_once_no_init(
+            params, conn, &our_entity, &peer_entity);
+    }
+
+    /* Verify repeat pairing event got reported once. */
+    TEST_ASSERT(ble_sm_test_repeat_pairing.num_calls == 1);
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test_util.h b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test_util.h
new file mode 100644
index 00000000..d8629b60
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_sm_test_util.h
@@ -0,0 +1,128 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SM_TEST_UTIL_
+#define H_BLE_SM_TEST_UTIL_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_sm_test_passkey_info {
+    struct ble_sm_io passkey;
+    uint32_t exp_numcmp;
+    unsigned io_before_rx:1;
+};
+
+struct ble_sm_test_params {
+    uint8_t init_addr_type;
+    uint8_t init_id_addr[6];
+    uint8_t init_rpa[6];
+    uint8_t resp_addr_type;
+    uint8_t resp_id_addr[6];
+    uint8_t resp_rpa[6];
+    struct ble_sm_test_passkey_info passkey_info;
+
+    struct ble_sm_sec_req sec_req;
+    struct ble_sm_pair_cmd pair_req;
+    struct ble_sm_pair_cmd pair_rsp;
+    struct ble_sm_pair_confirm confirm_req[20];
+    struct ble_sm_pair_confirm confirm_rsp[20];
+    struct ble_sm_pair_random random_req[20];
+    struct ble_sm_pair_random random_rsp[20];
+    struct ble_sm_id_info id_info_req;
+    struct ble_sm_id_info id_info_rsp;
+    struct ble_sm_id_addr_info id_addr_info_req;
+    struct ble_sm_id_addr_info id_addr_info_rsp;
+    struct ble_sm_sign_info sign_info_req;
+    struct ble_sm_sign_info sign_info_rsp;
+    struct ble_sm_pair_fail pair_fail;
+
+    int pair_alg;
+    unsigned authenticated:1;
+
+    /*** Secure connections fields. */
+    uint8_t ltk[16];
+    uint8_t our_priv_key[32];
+    struct ble_sm_public_key public_key_req;
+    struct ble_sm_public_key public_key_rsp;
+    struct ble_sm_dhkey_check dhkey_check_req;
+    struct ble_sm_dhkey_check dhkey_check_rsp;
+
+    /*** Legacy fields. */
+    uint8_t stk[16];
+    struct ble_sm_enc_info enc_info_req;
+    struct ble_sm_enc_info enc_info_rsp;
+    struct ble_sm_master_id master_id_req;
+    struct ble_sm_master_id master_id_rsp;
+};
+
+extern int ble_sm_test_gap_event;
+extern int ble_sm_test_gap_status;
+extern struct ble_gap_sec_state ble_sm_test_sec_state;
+
+extern int ble_sm_test_store_obj_type;
+extern union ble_store_key ble_sm_test_store_key;
+extern union ble_store_value ble_sm_test_store_value;
+
+void ble_sm_test_util_init(void);
+int ble_sm_test_util_conn_cb(struct ble_gap_event *ctxt, void *arg);
+void ble_sm_test_util_io_inject(struct ble_sm_test_passkey_info *passkey_info,
+                                uint8_t cur_sm_state);
+void ble_sm_test_util_io_inject_bad(uint16_t conn_handle,
+                                    uint8_t correct_io_act);
+void ble_sm_test_util_io_check_pre(
+    struct ble_sm_test_passkey_info *passkey_info,
+    uint8_t cur_sm_state);
+void ble_sm_test_util_io_check_post(
+    struct ble_sm_test_passkey_info *passkey_info,
+    uint8_t cur_sm_state);
+void ble_sm_test_util_rx_sec_req(uint16_t conn_handle,
+                                 struct ble_sm_sec_req *cmd,
+                                 int exp_status);
+void ble_sm_test_util_verify_tx_pair_fail(struct ble_sm_pair_fail *exp_cmd);
+void ble_sm_test_util_us_lgcy_good(struct ble_sm_test_params *params);
+void ble_sm_test_util_peer_fail_inval(int we_are_master,
+                                      uint8_t *init_addr,
+                                      uint8_t *resp_addr,
+                                      struct ble_sm_pair_cmd *pair_req,
+                                      struct ble_sm_pair_fail *pair_fail);
+void ble_sm_test_util_peer_lgcy_fail_confirm(
+    uint8_t *init_addr,
+    uint8_t *resp_addr,
+    struct ble_sm_pair_cmd *pair_req,
+    struct ble_sm_pair_cmd *pair_rsp,
+    struct ble_sm_pair_confirm *confirm_req,
+    struct ble_sm_pair_confirm *confirm_rsp,
+    struct ble_sm_pair_random *random_req,
+    struct ble_sm_pair_random *random_rsp,
+    struct ble_sm_pair_fail *fail_rsp);
+
+void ble_sm_test_util_peer_lgcy_good_once(struct ble_sm_test_params *params);
+void ble_sm_test_util_peer_lgcy_good(struct ble_sm_test_params *params);
+void ble_sm_test_util_peer_bonding_bad(uint16_t ediv, uint64_t rand_num);
+void ble_sm_test_util_peer_sc_good(struct ble_sm_test_params *params);
+void ble_sm_test_util_us_sc_good(struct ble_sm_test_params *params);
+void ble_sm_test_util_us_fail_inval(struct ble_sm_test_params *params);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_store_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_store_test.c
new file mode 100644
index 00000000..75d3a490
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_store_test.c
@@ -0,0 +1,435 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "testutil/testutil.h"
+#include "ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+static struct ble_store_status_event ble_store_test_status_event;
+
+static void
+ble_store_test_util_verify_peer_deleted(const ble_addr_t *addr)
+{
+    union ble_store_value value;
+    union ble_store_key key;
+    ble_addr_t addrs[64];
+    int num_addrs;
+    int rc;
+    int i;
+
+    memset(&key, 0, sizeof key);
+    key.sec.peer_addr = *addr;
+    rc = ble_store_read(BLE_STORE_OBJ_TYPE_OUR_SEC, &key, &value);
+    TEST_ASSERT(rc == BLE_HS_ENOENT);
+    rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_SEC, &key, &value);
+    TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+    memset(&key, 0, sizeof key);
+    key.cccd.peer_addr = *addr;
+    rc = ble_store_read(BLE_STORE_OBJ_TYPE_CCCD, &key, &value);
+    TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+    rc = ble_store_util_bonded_peers(addrs, &num_addrs,
+                                     sizeof addrs / sizeof addrs[0]);
+    TEST_ASSERT_FATAL(rc == 0);
+    for (i = 0; i < num_addrs; i++) {
+        TEST_ASSERT(ble_addr_cmp(addr, addrs + i) != 0);
+    }
+}
+
+static int
+ble_store_test_util_status_overflow(struct ble_store_status_event *event,
+                                    void *arg)
+{
+    int *status;
+
+    status = arg;
+
+    ble_store_test_status_event = *event;
+    return *status;
+}
+
+static void
+ble_store_test_util_overflow_sec(int is_our_sec)
+{
+    union ble_store_value val;
+    int obj_type;
+    int status;
+    int rc;
+    int i;
+
+    ble_hs_test_util_init();
+
+    ble_hs_cfg.store_status_cb = ble_store_test_util_status_overflow;
+    ble_hs_cfg.store_status_arg = &status;
+
+    if (is_our_sec) {
+        obj_type = BLE_STORE_OBJ_TYPE_OUR_SEC;
+    } else {
+        obj_type = BLE_STORE_OBJ_TYPE_PEER_SEC;
+    }
+
+    memset(&ble_store_test_status_event, 0,
+           sizeof ble_store_test_status_event);
+    memset(&val, 0, sizeof val);
+
+    val.sec.peer_addr =
+        (ble_addr_t){ BLE_ADDR_PUBLIC, { 1, 2, 3, 4, 5, 6 } };
+    val.sec.ltk_present = 1,
+
+    status = BLE_HS_ESTORE_CAP;
+    for (i = 0; ; i++) {
+        rc = ble_store_write(obj_type, &val);
+        if (i < MYNEWT_VAL(BLE_STORE_MAX_BONDS)) {
+            TEST_ASSERT_FATAL(rc == 0);
+        } else {
+            /* This record should have caused an overflow. */
+            TEST_ASSERT(rc == BLE_HS_ESTORE_CAP);
+            TEST_ASSERT(ble_store_test_status_event.event_code ==
+                        BLE_STORE_EVENT_OVERFLOW);
+            TEST_ASSERT(ble_store_test_status_event.overflow.obj_type ==
+                        obj_type);
+            TEST_ASSERT(ble_store_test_status_event.overflow.value == &val);
+            break;
+        }
+
+        val.sec.peer_addr.val[0]++;
+    }
+}
+
+static int
+ble_store_test_util_count(int obj_type)
+{
+    int count;
+    int rc;
+
+    rc = ble_store_util_count(obj_type, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    return count;
+}
+
+TEST_CASE_SELF(ble_store_test_peers)
+{
+    struct ble_store_value_sec secs[3] = {
+        {
+            .peer_addr = { BLE_ADDR_PUBLIC,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+        {
+            /* Address value is a duplicate of above, but type differs. */
+            .peer_addr = { BLE_ADDR_RANDOM,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+        {
+            .peer_addr = { BLE_ADDR_PUBLIC,     { 2, 3, 4, 5, 6, 7 } },
+            .ltk_present = 1,
+        },
+    };
+    ble_addr_t peer_addrs[3];
+    int num_addrs;
+    int rc;
+    int i;
+
+    ble_hs_test_util_init();
+
+    for (i = 0; i < sizeof secs / sizeof secs[0]; i++) {
+        rc = ble_store_write_our_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+        rc = ble_store_write_peer_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    rc = ble_store_util_bonded_peers(peer_addrs, &num_addrs,
+                                     sizeof peer_addrs / sizeof peer_addrs[0]);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    TEST_ASSERT(num_addrs == sizeof secs / sizeof secs[0]);
+    for (i = 0; i < num_addrs; i++) {
+        TEST_ASSERT(ble_addr_cmp(&peer_addrs[i], &secs[i].peer_addr) == 0);
+    }
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_store_test_delete_peer)
+{
+    struct ble_store_value_sec secs[2] = {
+        {
+            .peer_addr = { BLE_ADDR_PUBLIC,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+        {
+            /* Address value is a duplicate of above, but type differs. */
+            .peer_addr = { BLE_ADDR_RANDOM,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+    };
+    struct ble_store_value_cccd cccds[3] = {
+        /* First two belong to first peer. */
+        {
+            .peer_addr = secs[0].peer_addr,
+            .chr_val_handle = 5,
+        },
+        {
+            .peer_addr = secs[0].peer_addr,
+            .chr_val_handle = 8,
+        },
+
+        /* Last belongs to second peer. */
+        {
+            .peer_addr = secs[1].peer_addr,
+            .chr_val_handle = 5,
+        },
+    };
+    union ble_store_value value;
+    union ble_store_key key;
+    int count;
+    int rc;
+    int i;
+
+    ble_hs_test_util_init();
+
+    for (i = 0; i < sizeof secs / sizeof secs[0]; i++) {
+        rc = ble_store_write_our_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+        rc = ble_store_write_peer_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    for (i = 0; i < sizeof cccds / sizeof cccds[0]; i++) {
+        rc = ble_store_write_cccd(cccds + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    /* Delete first peer. */
+    rc = ble_store_util_delete_peer(&secs[0].peer_addr);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure all traces of first peer have been removed. */
+    ble_store_test_util_verify_peer_deleted(&secs[0].peer_addr);
+
+    /* Ensure second peer data is still intact. */
+    ble_store_key_from_value_sec(&key.sec, secs + 1);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_OUR_SEC, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 1);
+
+    rc = ble_store_read_our_sec(&key.sec, &value.sec);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(memcmp(&value.sec, secs + 1, sizeof value.sec) == 0);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 1);
+
+    rc = ble_store_read_peer_sec(&key.sec, &value.sec);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(memcmp(&value.sec, secs + 1, sizeof value.sec) == 0);
+
+    ble_store_key_from_value_cccd(&key.cccd, cccds + 2);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_CCCD, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 1);
+
+    rc = ble_store_read_cccd(&key.cccd, &value.cccd);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(memcmp(&value.cccd, cccds + 2, sizeof value.cccd) == 0);
+
+    /* Delete second peer. */
+    rc = ble_store_util_delete_peer(&secs[1].peer_addr);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure all traces of first peer have been removed. */
+    ble_store_test_util_verify_peer_deleted(&secs[1].peer_addr);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_store_test_count)
+{
+    struct ble_store_value_sec secs[4] = {
+        {
+            .peer_addr = { BLE_ADDR_PUBLIC,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+        {
+            .peer_addr = { BLE_ADDR_RANDOM,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+        {
+            .peer_addr = { BLE_ADDR_PUBLIC,     { 2, 3, 4, 5, 6, 7 } },
+            .ltk_present = 1,
+        },
+        {
+            .peer_addr = { BLE_ADDR_RANDOM,     { 3, 4, 5, 6, 7, 8 } },
+            .ltk_present = 1,
+        },
+    };
+    struct ble_store_value_cccd cccds[2] = {
+        {
+            .peer_addr = secs[0].peer_addr,
+            .chr_val_handle = 5,
+        },
+        {
+            .peer_addr = secs[0].peer_addr,
+            .chr_val_handle = 8,
+        },
+    };
+    int count;
+    int rc;
+    int i;
+
+    ble_hs_test_util_init();
+
+    /*** Verify initial counts are 0. */
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_OUR_SEC, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 0);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 0);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_CCCD, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 0);
+
+    /* Write some test data. */
+
+    for (i = 0; i < 3; i++) {
+        rc = ble_store_write_our_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+    for (i = 0; i < 2; i++) {
+        rc = ble_store_write_peer_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+    for (i = 0; i < 1; i++) {
+        rc = ble_store_write_cccd(cccds + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    /*** Verify counts after populating store. */
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_OUR_SEC, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 3);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 2);
+
+    rc = ble_store_util_count(BLE_STORE_OBJ_TYPE_CCCD, &count);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(count == 1);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_store_test_overflow)
+{
+    ble_store_test_util_overflow_sec(0);
+    ble_store_test_util_overflow_sec(1);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_CASE_SELF(ble_store_test_clear)
+{
+    const struct ble_store_value_sec secs[2] = {
+        {
+            .peer_addr = { BLE_ADDR_PUBLIC,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+        {
+            /* Address value is a duplicate of above, but type differs. */
+            .peer_addr = { BLE_ADDR_RANDOM,     { 1, 2, 3, 4, 5, 6 } },
+            .ltk_present = 1,
+        },
+    };
+    const struct ble_store_value_cccd cccds[3] = {
+        /* First two belong to first peer. */
+        {
+            .peer_addr = secs[0].peer_addr,
+            .chr_val_handle = 5,
+        },
+        {
+            .peer_addr = secs[0].peer_addr,
+            .chr_val_handle = 8,
+        },
+
+        /* Last belongs to second peer. */
+        {
+            .peer_addr = secs[1].peer_addr,
+            .chr_val_handle = 5,
+        },
+    };
+    int rc;
+    int i;
+
+    ble_hs_test_util_init();
+
+    for (i = 0; i < sizeof secs / sizeof secs[0]; i++) {
+        rc = ble_store_write_our_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+        rc = ble_store_write_peer_sec(secs + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    for (i = 0; i < sizeof cccds / sizeof cccds[0]; i++) {
+        rc = ble_store_write_cccd(cccds + i);
+        TEST_ASSERT_FATAL(rc == 0);
+    }
+
+    /* Sanity check. */
+    TEST_ASSERT_FATAL(
+        ble_store_test_util_count(BLE_STORE_OBJ_TYPE_OUR_SEC) == 2);
+    TEST_ASSERT_FATAL(
+        ble_store_test_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC) == 2);
+    TEST_ASSERT_FATAL(
+        ble_store_test_util_count(BLE_STORE_OBJ_TYPE_CCCD) == 3);
+
+    /* Ensure store is empty after clear gets called. */
+    rc = ble_store_clear();
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(ble_store_test_util_count(BLE_STORE_OBJ_TYPE_OUR_SEC) == 0);
+    TEST_ASSERT(ble_store_test_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC) == 0);
+    TEST_ASSERT(ble_store_test_util_count(BLE_STORE_OBJ_TYPE_CCCD) == 0);
+
+    /* Ensure second clear succeeds with no effect. */
+    rc = ble_store_clear();
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(ble_store_test_util_count(BLE_STORE_OBJ_TYPE_OUR_SEC) == 0);
+    TEST_ASSERT(ble_store_test_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC) == 0);
+    TEST_ASSERT(ble_store_test_util_count(BLE_STORE_OBJ_TYPE_CCCD) == 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_store_suite)
+{
+    ble_store_test_peers();
+    ble_store_test_delete_peer();
+    ble_store_test_count();
+    ble_store_test_overflow();
+    ble_store_test_clear();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/src/ble_uuid_test.c b/src/libs/mynewt-nimble/nimble/host/test/src/ble_uuid_test.c
new file mode 100644
index 00000000..786e371a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/src/ble_uuid_test.c
@@ -0,0 +1,76 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include "testutil/testutil.h"
+#include "ble_hs_test.h"
+#include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
+
+TEST_CASE_SELF(ble_uuid_test)
+{
+    uint8_t buf_16[2] = { 0x00, 0x18 };
+    uint8_t buf_128[16] = { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                            0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF };
+
+    const ble_uuid_t *uuid16_1 = BLE_UUID16_DECLARE(0x1800);
+    const ble_uuid_t *uuid16_2 = BLE_UUID16_DECLARE(0x1801);
+
+    const ble_uuid_t *uuid128_1 =
+        BLE_UUID128_DECLARE(0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                            0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF);
+    const ble_uuid_t *uuid128_2 =
+        BLE_UUID128_DECLARE(0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+                                0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xEE);
+
+    ble_uuid_any_t uuid;
+    int rc;
+
+    rc = ble_uuid_init_from_buf(&uuid, buf_16, 2);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_uuid_cmp(&uuid.u, uuid16_1);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_uuid_cmp(&uuid.u, uuid16_2);
+    TEST_ASSERT(rc != 0);
+
+    rc = ble_uuid_cmp(uuid16_1, uuid16_2);
+    TEST_ASSERT(rc != 0);
+
+    rc = ble_uuid_init_from_buf(&uuid, buf_128, 16);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_uuid_cmp(&uuid.u, uuid128_1);
+    TEST_ASSERT(rc == 0);
+
+    rc = ble_uuid_cmp(&uuid.u, uuid128_2);
+    TEST_ASSERT(rc != 0);
+
+    rc = ble_uuid_cmp(uuid128_1, uuid128_2);
+    TEST_ASSERT(rc != 0);
+
+    ble_hs_test_util_assert_mbufs_freed(NULL);
+}
+
+TEST_SUITE(ble_uuid_test_suite)
+{
+    ble_uuid_test();
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/test/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/test/syscfg.yml
new file mode 100644
index 00000000..6307398e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/test/syscfg.yml
@@ -0,0 +1,31 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.vals:
+    BLE_HS_DEBUG: 1
+    BLE_HS_PHONY_HCI_ACKS: 1
+    BLE_HS_REQUIRE_OS: 0
+    BLE_MAX_CONNECTIONS: 8
+    BLE_GATT_MAX_PROCS: 16
+    BLE_SM: 1
+    BLE_SM_SC: 1
+    MSYS_1_BLOCK_COUNT: 100
+    BLE_L2CAP_COC_MAX_NUM: 2
+    CONFIG_FCB: 1
+    BLE_VERSION: 52
+    BLE_L2CAP_ENHANCED_COC: 1
diff --git a/src/libs/mynewt-nimble/nimble/host/tools/log2smtest.rb b/src/libs/mynewt-nimble/nimble/host/tools/log2smtest.rb
new file mode 100755
index 00000000..e253e69f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/tools/log2smtest.rb
@@ -0,0 +1,1029 @@
+#!/usr/bin/env ruby
+
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+### This script converts a bletiny log into a security manager unit test.  The
+### input log must contain the connection establishment and complete pairing
+### procedure.
+###
+### Arguments: None
+### Stdin: bletiny log file
+
+$PAIR_ALG_STRINGS = {
+    0 => [ 'BLE_SM_PAIR_ALG_JW',        'just works',           'jw' ],
+    1 => [ 'BLE_SM_PAIR_ALG_PASSKEY',   'passkey entry',        'pk' ],
+    2 => [ 'BLE_SM_PAIR_ALG_OOB',       'out of band',          'ob' ],
+    3 => [ 'BLE_SM_PAIR_ALG_NUMCMP',    'numeric comparison',   'nc' ]
+}
+
+$ADDR_TYPE_STRINGS = {
+    0 => 'BLE_ADDR_TYPE_PUBLIC',
+    1 => 'BLE_ADDR_TYPE_RANDOM',
+    2 => 'BLE_ADDR_TYPE_RPA_PUB_DEFAULT',
+    3 => 'BLE_ADDR_TYPE_RPA_RND_DEFAULT',
+}
+
+$ACTION_STRINGS = {
+    0 => 'BLE_SM_IOACT_NONE',
+    1 => 'BLE_SM_IOACT_OOB',
+    2 => 'BLE_SM_IOACT_INPUT',
+    3 => 'BLE_SM_IOACT_DISP',
+    4 => 'BLE_SM_IOACT_NUMCMP',
+}
+
+$prev_idx = 0
+$ctxt = {}
+
+def test_case_name
+    type_str = $ctxt[:sc] ? "sc" : "lgcy"
+    init_str = $ctxt[:we_are_init] ? "us" : "peer"
+    alg_str = $PAIR_ALG_STRINGS[$ctxt[:pair_alg]][2]
+    iio_cap_str = "iio#{$ctxt[:pair_req][:io_cap]}"
+    rio_cap_str = "rio#{$ctxt[:pair_rsp][:io_cap]}"
+    bonding_str = "b#{$ctxt[:bonding] ? 1 : 0}"
+    iat_str = "iat#{$ctxt[:addrs][:init_type]}"
+    rat_str = "rat#{$ctxt[:addrs][:resp_type]}"
+    ikey_str = "ik#{$ctxt[:pair_rsp][:init_key_dist]}"
+    rkey_str = "rk#{$ctxt[:pair_rsp][:resp_key_dist]}"
+
+    "ble_sm_" +
+    "#{type_str}_#{init_str}_#{alg_str}_#{iio_cap_str}_#{rio_cap_str}_" +
+    "#{bonding_str}_#{iat_str}_#{rat_str}_#{ikey_str}_#{rkey_str}"
+end
+
+def test_case_comment
+<<-eos
+/**
+ * #{$ctxt[:sc] ? 'Secure connections' : 'Legacy'} pairing
+ * Master: #{$ctxt[:we_are_init] ? "us" : "peer"}
+ * Pair algorithm: #{$PAIR_ALG_STRINGS[$ctxt[:pair_alg]][1]}
+ * Initiator IO capabilities: #{$ctxt[:pair_req][:io_cap]}
+ * Responder IO capabilities: #{$ctxt[:pair_rsp][:io_cap]}
+ * Bonding: #{$ctxt[:bonding]}
+ * Initiator address type: #{$ADDR_TYPE_STRINGS[$ctxt[:addrs][:init_type]]}
+ * Responder address type: #{$ADDR_TYPE_STRINGS[$ctxt[:addrs][:resp_type]]}
+ * Initiator key distribution: #{$ctxt[:pair_rsp][:init_key_dist]}
+ * Responder key distribution: #{$ctxt[:pair_rsp][:resp_key_dist]}
+ */
+eos
+end
+
+def to_hex_s(byte)
+    if byte.is_a?(String)
+        byte = s_to_i(byte)
+    end
+
+    "0x#{byte.to_s(16).rjust(2, '0')}"
+end
+
+# to_i(0) but interpret leading zeros as decimal.
+def s_to_i(s)
+    if s[0..1] == "0x"
+        return s.to_i(16)
+    else
+        return s.to_i(10)
+    end
+end
+
+def invalid_byte_line(msg, line)
+    str = "invalid byte line"
+    if msg != nil
+        str += ": #{msg}"
+    end
+
+    str += "; line=#{line}"
+
+    raise str
+end
+
+def token_string_to_bytes(line, delim = ' ')
+    tokens = line.split(delim)
+    bytes = []
+    tokens.each do |token|
+        begin
+            byte = token.to_i(16)
+            bytes << byte
+        rescue
+            invalid_byte_line("token=#{token}", line)
+        end
+    end
+
+    return bytes
+end
+
+def txrx_prefix(is_tx)
+    if is_tx
+        return "tx"
+    else
+        return "rx"
+    end
+end
+
+def reqrsp_s(is_req)
+    reqrsp = nil
+    if is_req
+        return "req"
+    else
+        return "rsp"
+    end
+end
+
+def bytes_to_arr_body(bytes, indent)
+    lines = []
+
+    idx = 0
+    while idx < bytes.size
+        slice_len = nil
+        if bytes.size - idx >= 8
+            slice_len = 8
+        else
+            slice_len = bytes.size - idx
+        end
+
+        slice = bytes[idx...(idx + slice_len)]
+        line = ' ' * indent +
+            slice.map{|b| to_hex_s(b)}.join(", ") + ","
+        lines << line
+
+        idx += slice_len
+    end
+
+    return lines.join("\n") << "\n"
+end
+
+def bytes_to_arr(bytes, name, indent)
+    str = "#{' ' * indent}.#{name} = {\n"
+    str << bytes_to_arr_body(bytes, indent + 4)
+    str << "#{' ' * indent}},"
+
+    return str
+end
+
+def addr_string_to_bytes(addr_string)
+    token_string_to_bytes(addr_string, ':').reverse
+end
+
+def parse_pair_cmd(line, is_req)
+    suffix = reqrsp_s(is_req)
+    re = %r{
+        pair\s#{suffix};
+        \s
+        conn=\d+
+        \s
+        io_cap=(?<io_cap>\d+)
+        \s
+        oob_data_flag=(?<oob_data_flag>\d+)
+        \s
+        authreq=(?<authreq>0x[0-9a-f]+)
+        \s
+        mac_enc_key_size=(?<max_enc_key_size>\d+)
+        \s
+        init_key_dist=(?<init_key_dist>\d+)
+        \s
+        resp_key_dist=(?<resp_key_dist>\d+)
+    }x
+
+    m = re.match(line)
+    if m == nil
+        return nil
+    end
+
+    cmd = {}
+    cmd[:io_cap] = s_to_i(m[:io_cap])
+    cmd[:oob_data_flag] = s_to_i(m[:oob_data_flag])
+    cmd[:authreq] = s_to_i(m[:authreq])
+    cmd[:max_enc_key_size] = s_to_i(m[:max_enc_key_size])
+    cmd[:init_key_dist] = s_to_i(m[:init_key_dist])
+    cmd[:resp_key_dist] = s_to_i(m[:resp_key_dist])
+
+    return cmd
+end
+
+def parse_privkey(line)
+    if !(line =~ /our privkey=(.+)/)
+        return nil
+    end
+    return token_string_to_bytes($1)
+end
+
+def parse_public_key(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: public key; conn=\d+ x=(.+) y=(.+)/)
+        return nil
+    end
+
+    pubkey = {}
+    pubkey[:x] = token_string_to_bytes($1)
+    pubkey[:y] = token_string_to_bytes($2)
+
+    if pubkey[:x].size != 32
+        raise "invalid public key: x length incorrect; line=#{line}"
+    end
+
+    if pubkey[:y].size != 32
+        raise "invalid public key: y length incorrect; line=#{line}"
+    end
+
+    return pubkey
+end
+
+def parse_confirm(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: confirm; conn=\d+ value=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid confirm line (length mismatch): #{line}"
+    end
+
+    return { :value => bytes }
+end
+
+def parse_random(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: random; conn=\d+ value=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid random line (length mismatch): #{line}"
+    end
+
+    return { :value => bytes }
+end
+
+def parse_stk(line)
+    if !(line =~ /^    out=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid stk line (length mismatch): #{line}"
+    end
+
+    return bytes
+end
+
+def parse_dhkey_check(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: dhkey check; conn=\d+ value=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid dhkey_check line (length mismatch): #{line}"
+    end
+
+    return { :value => bytes }
+end
+
+def parse_ltk(line)
+    if !(line =~ /persisting.+ltk=([^ ]+)/)
+        return nil
+    end
+
+    bytes = $1.split(":")
+    if bytes.size != 16
+        raise "invalid ltk line (length mismatch): exp=16 got=#{bytes.size} " +
+              "line=#{line}"
+    end
+
+    return bytes
+end
+
+def parse_enc_info(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: enc info; conn=\d+ ltk=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid enc info line (length mismatch): #{line}"
+    end
+
+    return { :ltk => bytes }
+end
+
+def parse_master_id(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: master id; conn=\d+ ediv=(.+) rand=(.+)/)
+        return nil
+    end
+
+    return {
+        :ediv => s_to_i($1),
+        :rand => s_to_i($2),
+    }
+end
+
+def parse_id_info(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: id info; conn=\d+ irk=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid id info line (length mismatch): #{line}"
+    end
+
+    return { :irk => bytes }
+end
+
+def parse_id_addr_info(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: id addr info; conn=\d+ addr_type=(\d+) addr=(.+)/)
+        return nil
+    end
+
+    bytes = addr_string_to_bytes($2)
+    if bytes.size != 6
+        raise "invalid id addr info line (length mismatch): #{line}"
+    end
+
+    return {
+        :addr_type => s_to_i($1),
+        :addr => bytes,
+    }
+end
+
+def parse_sign_info(line, is_tx)
+    prefix = txrx_prefix(is_tx)
+    if !(line =~ /#{prefix}ed sm command: sign info; conn=\d+ sig_key=(.+)/)
+        return nil
+    end
+
+    bytes = token_string_to_bytes($1)
+    if bytes.size != 16
+        raise "invalid sign info line (length mismatch): #{line}"
+    end
+
+    return {
+        :sig_key => bytes,
+    }
+end
+
+def parse_passkey_info(line)
+    passkey_info = {}
+
+    case line
+    when /passkey action event; action=4 numcmp=(\d+)/
+        passkey_info[:action] = 4
+        passkey_info[:numcmp] = $1.to_i(10)
+    when /^b passkey conn=\d+ action=1 oob=(\S+)/
+        passkey_info[:action] = 1
+        passkey_info[:oob] = token_string_to_bytes($1, ':')
+    when /^b passkey conn=\d+ action=2 key=(\d+)/
+        passkey_info[:action] = 2
+        passkey_info[:key] = $1.to_i(10)
+    when /b passkey conn=\d+ action=3 key=(\d+)/
+        passkey_info[:action] = 3
+        passkey_info[:key] = $1.to_i(10)
+    else
+        return nil
+    end
+
+    return passkey_info
+end
+
+def parse_addrs(line)
+    if !(line =~ /our_ota_addr_type=(\d+) our_ota_addr=(\S+) our_id_addr_type=(\d+) our_id_addr=(\S+) peer_ota_addr_type=(\d+) peer_ota_addr=(\S+) peer_id_addr_type=(\d+) peer_id_addr=(\S+)/)
+        return nil
+    end
+
+    our_ota_addr_bytes = addr_string_to_bytes($2)
+    our_id_addr_bytes = addr_string_to_bytes($4)
+    peer_ota_addr_bytes = addr_string_to_bytes($6)
+    peer_id_addr_bytes = addr_string_to_bytes($8)
+
+    if $ctxt[:we_are_init]
+        init_id_bytes = our_id_addr_bytes
+        init_ota_bytes = our_ota_addr_bytes
+        resp_id_bytes = peer_id_addr_bytes
+        resp_ota_bytes = peer_ota_addr_bytes
+        init_addr_type = s_to_i($1)
+        resp_addr_type = s_to_i($5)
+    else
+        init_id_bytes = peer_id_addr_bytes
+        init_ota_bytes = peer_ota_addr_bytes
+        resp_id_bytes = our_id_addr_bytes
+        resp_ota_bytes = our_ota_addr_bytes
+        init_addr_type = s_to_i($5)
+        resp_addr_type = s_to_i($1)
+    end
+
+    if init_id_bytes == init_ota_bytes
+        init_ota_bytes = [0] * 6
+    end
+    if resp_id_bytes == resp_ota_bytes
+        resp_ota_bytes = [0] * 6
+    end
+
+    return {
+        :init_type => init_addr_type,
+        :resp_type => resp_addr_type,
+        :init_id_addr => init_id_bytes,
+        :resp_id_addr => resp_id_bytes,
+        :init_rpa => init_ota_bytes,
+        :resp_rpa => resp_ota_bytes,
+    }
+end
+
+def detect_initiator(lines)
+    lines.each do |line|
+        if line =~ /txed sm command: pair req/
+            $ctxt[:we_are_init] = true
+        elsif line =~ /txed sm command: pair rsp/
+            $ctxt[:we_are_init] = false
+        end
+    end
+
+    if $ctxt[:we_are_init] == nil
+        raise "could not detect which peer is the initiator"
+    end
+end
+
+def pair_cmd_to_s(cmd, is_req)
+    suffix = reqrsp_s(is_req)
+    return <<-eos
+        .pair_#{suffix} = {
+            .io_cap = #{to_hex_s(cmd[:io_cap])},
+            .oob_data_flag = #{to_hex_s(cmd[:oob_data_flag])},
+            .authreq = #{to_hex_s(cmd[:authreq])},
+            .max_enc_key_size = #{to_hex_s(cmd[:max_enc_key_size])},
+            .init_key_dist = #{to_hex_s(cmd[:init_key_dist])},
+            .resp_key_dist = #{to_hex_s(cmd[:resp_key_dist])},
+        },
+    eos
+end
+
+def privkey_to_s(privkey)
+    return bytes_to_arr(privkey, "our_priv_key", 8)
+end
+
+def public_key_to_s(public_key, is_req)
+    suffix = reqrsp_s(is_req)
+    return <<-eos
+        .public_key_#{suffix} = {
+#{bytes_to_arr(public_key[:x], "x", 12)}
+#{bytes_to_arr(public_key[:y], "y", 12)}
+        },
+    eos
+end
+
+def confirm_to_s(confirm, is_req, idx)
+    return <<-eos
+        .confirm_#{reqrsp_s(is_req)}[#{idx}] = {
+#{bytes_to_arr(confirm[:value], "value", 12)}
+        },
+    eos
+end
+
+def random_to_s(random, is_req, idx)
+    return <<-eos
+        .random_#{reqrsp_s(is_req)}[#{idx}] = {
+#{bytes_to_arr(random[:value], "value", 12)}
+        },
+    eos
+end
+
+def ltk_to_s(ltk)
+    return bytes_to_arr(ltk, "ltk", 8)
+end
+
+def stk_to_s(stk)
+    return bytes_to_arr(stk, "stk", 8)
+end
+
+def enc_info_to_s(id_info, is_req)
+    return <<-eos
+        .enc_info_#{reqrsp_s(is_req)} = {
+#{bytes_to_arr(id_info[:ltk], "ltk", 12)}
+        },
+    eos
+end
+
+def master_id_to_s(master_id, is_req)
+    return <<-eos
+        .master_id_#{reqrsp_s(is_req)} = {
+            .ediv = 0x#{master_id[:ediv].to_s(16)},
+            .rand_val = 0x#{master_id[:rand].to_s(16)},
+        },
+    eos
+end
+
+def id_info_to_s(id_info, is_req)
+    return <<-eos
+        .id_info_#{reqrsp_s(is_req)} = {
+#{bytes_to_arr(id_info[:irk], "irk", 12)}
+        },
+    eos
+end
+
+def id_addr_info_to_s(id_addr_info, is_req)
+    return <<-eos
+        .id_addr_info_#{reqrsp_s(is_req)} = {
+            .addr_type = #{id_addr_info[:addr_type]},
+#{bytes_to_arr(id_addr_info[:addr], "bd_addr", 12)}
+        },
+    eos
+end
+
+def sign_info_to_s(sign_info, is_req)
+    return <<-eos
+        .sign_info_#{reqrsp_s(is_req)} = {
+#{bytes_to_arr(sign_info[:sig_key], "sig_key", 12)}
+        },
+    eos
+end
+
+def passkey_info_fill(passkey_info)
+    case passkey_info[:action]
+    # None
+    when 0
+        $ctxt[:pair_alg] = 0
+        $ctxt[:authenticated] = false
+
+    # OOB
+    when 1
+        $ctxt[:pair_alg] = 2
+        $ctxt[:authenticated] = true
+
+    # Input
+    when 2
+        $ctxt[:pair_alg] = 1
+        $ctxt[:authenticated] = true
+
+    # Display
+    when 3
+        $ctxt[:pair_alg] = 1
+        $ctxt[:authenticated] = true
+
+    # Numeric comparison
+    when 4
+        $ctxt[:pair_alg] = 3
+        $ctxt[:authenticated] = true
+
+    else
+        raise "invalid MITM action: #{passkey_info[:action]}"
+    end
+end
+
+def passkey_info_s
+    passkey_info = $ctxt[:passkey_info]
+    action_str = $ACTION_STRINGS[passkey_info[:action]]
+
+    result = <<-eos
+        .pair_alg = #{$ctxt[:pair_alg]},
+        .authenticated = #{$ctxt[:authenticated]},
+        .passkey_info = {
+            .passkey = {
+                .action = #{action_str},
+    eos
+
+    if passkey_info[:key] != nil
+        result << <<-eos
+                .passkey = #{passkey_info[:key].to_i},
+        eos
+    end
+    if passkey_info[:oob] != nil
+        result << <<-eos
+#{bytes_to_arr(passkey_info[:oob], "oob", 16)}
+        eos
+    end
+    if passkey_info[:numcmp] != nil
+        result << <<-eos
+                .numcmp_accept = 1,
+        eos
+    end
+
+    result << <<-eos
+            },
+    eos
+
+    if passkey_info[:numcmp] != nil
+        result << <<-eos
+            .exp_numcmp = #{passkey_info[:numcmp].to_i},
+        eos
+    end
+
+    result << <<-eos
+        },
+    eos
+end
+
+def addrs_to_s(addrs)
+    s = ''
+
+    init_type = addrs[:init_type]
+    resp_type = addrs[:resp_type]
+
+    if init_type != 0
+        s += "        .init_addr_type = #{$ADDR_TYPE_STRINGS[init_type]},\n"
+    end
+    s += bytes_to_arr(addrs[:init_id_addr], "init_id_addr", 8) + "\n"
+    if init_type >= 2
+        s += bytes_to_arr(addrs[:init_rpa], "init_rpa", 8) + "\n"
+    end
+
+    if resp_type != 0
+        s += "        .resp_addr_type = #{$ADDR_TYPE_STRINGS[resp_type]},\n"
+    end
+    s += bytes_to_arr(addrs[:resp_id_addr], "resp_id_addr", 8) + "\n"
+    if resp_type >= 2
+        s += bytes_to_arr(addrs[:resp_rpa], "resp_rpa", 8) + "\n"
+    end
+
+    return s
+end
+
+def dhkey_check_to_s(dhkey_check, is_req)
+    return <<-eos
+        .dhkey_check_#{reqrsp_s(is_req)} = {
+#{bytes_to_arr(dhkey_check[:value], "value", 12)}
+        },
+    eos
+end
+
+def extract_one(lines, ignore_prev = false)
+    if ignore_prev
+        start = 0
+    else
+        start = $prev_idx
+    end
+
+    (start...lines.size).each do |idx|
+        line = lines[idx]
+        result = yield(line)
+        if result != nil
+            if !ignore_prev
+                $prev_idx = idx
+            end
+            return result
+        end
+    end
+
+    return nil
+end
+
+def extract_pair_req(lines)
+    return extract_one(lines) {|line| parse_pair_cmd(line, true)}
+end
+
+def extract_pair_rsp(lines)
+    return extract_one(lines) {|line| parse_pair_cmd(line, false)}
+end
+
+def extract_privkey(lines)
+    return extract_one(lines) {|line| parse_privkey(line)}
+end
+
+def extract_public_key_req(lines)
+    return extract_one(lines) do |line|
+        parse_public_key(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_public_key_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_public_key(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_confirm_req(lines)
+    return extract_one(lines) do |line|
+        parse_confirm(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_confirm_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_confirm(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_random_req(lines)
+    return extract_one(lines) do |line|
+        parse_random(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_random_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_random(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_confirm_random(lines)
+    confirm_reqs = []
+    confirm_rsps = []
+    random_reqs = []
+    random_rsps = []
+
+    idx = 0
+    loop do
+        confirm_req = extract_confirm_req(lines)
+        if confirm_req != nil
+            confirm_reqs << confirm_req
+        end
+
+        confirm_rsp = extract_confirm_rsp(lines)
+        break if confirm_rsp == nil
+        if idx >= 20
+            raise "too many confirm rsps (>20)"
+        end
+        confirm_rsps << confirm_rsp
+
+        random_req = extract_random_req(lines)
+        break if random_req == nil
+        random_reqs << random_req
+
+        random_rsp = extract_random_rsp(lines)
+        break if random_rsp == nil
+        random_rsps << random_rsp
+
+        idx += 1
+    end
+
+    return confirm_reqs, confirm_rsps, random_reqs, random_rsps
+end
+
+def extract_stk(lines)
+    return extract_one(lines, true) do |line|
+        parse_stk(line)
+    end
+end
+
+def extract_dhkey_check_req(lines)
+    return extract_one(lines) do |line|
+        parse_dhkey_check(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_dhkey_check_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_dhkey_check(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_enc_info_req(lines)
+    return extract_one(lines) do |line|
+        parse_enc_info(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_enc_info_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_enc_info(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_master_id_req(lines)
+    return extract_one(lines) do |line|
+        parse_master_id(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_master_id_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_master_id(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_id_info_req(lines)
+    return extract_one(lines) do |line|
+        parse_id_info(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_id_info_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_id_info(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_id_addr_info_req(lines)
+    return extract_one(lines) do |line|
+        parse_id_addr_info(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_id_addr_info_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_id_addr_info(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_sign_info_req(lines)
+    return extract_one(lines) do |line|
+        parse_sign_info(line, !$ctxt[:we_are_init])
+    end
+end
+
+def extract_sign_info_rsp(lines)
+    return extract_one(lines) do |line|
+        parse_sign_info(line, $ctxt[:we_are_init])
+    end
+end
+
+def extract_ltk(lines)
+    return extract_one(lines) do |line|
+        parse_ltk(line)
+    end
+end
+
+def extract_passkey_info(lines)
+    passkey_info = extract_one(lines, true) do |line|
+        parse_passkey_info(line)
+    end
+
+    if passkey_info == nil
+        passkey_info = { :action => 0 }
+    end
+
+    return passkey_info
+end
+
+def extract_addrs(lines)
+    return extract_one(lines) do |line|
+        parse_addrs(line)
+    end
+end
+
+
+lines = STDIN.readlines
+
+detect_initiator(lines)
+$ctxt[:pair_req] = extract_pair_req(lines)
+$ctxt[:pair_rsp] = extract_pair_rsp(lines)
+$ctxt[:privkey] = extract_privkey(lines)
+$ctxt[:public_key_req] = extract_public_key_req(lines)
+$ctxt[:public_key_rsp] = extract_public_key_rsp(lines)
+$ctxt[:confirm_reqs], $ctxt[:confirm_rsps], $ctxt[:random_reqs], $ctxt[:random_rsps] = extract_confirm_random(lines)
+$ctxt[:passkey_info] = extract_passkey_info(lines)
+$ctxt[:dhkey_check_req] = extract_dhkey_check_req(lines)
+$ctxt[:dhkey_check_rsp] = extract_dhkey_check_rsp(lines)
+$ctxt[:enc_info_req] = extract_enc_info_req(lines)
+$ctxt[:master_id_req] = extract_master_id_req(lines)
+$ctxt[:id_info_req] = extract_id_info_req(lines)
+$ctxt[:id_addr_info_req] = extract_id_addr_info_req(lines)
+$ctxt[:sign_info_req] = extract_sign_info_req(lines)
+$ctxt[:enc_info_rsp] = extract_enc_info_rsp(lines)
+$ctxt[:master_id_rsp] = extract_master_id_rsp(lines)
+$ctxt[:id_info_rsp] = extract_id_info_rsp(lines)
+$ctxt[:id_addr_info_rsp] = extract_id_addr_info_rsp(lines)
+$ctxt[:sign_info_rsp] = extract_sign_info_rsp(lines)
+$ctxt[:addrs] = extract_addrs(lines)
+$ctxt[:ltk] = extract_ltk(lines)
+$ctxt[:stk] = extract_stk(lines)
+
+expected_confirm_rsps = nil
+expected_random_reqs = nil
+expected_random_rsps = nil
+if $ctxt[:confirm_reqs].size == 0
+    expected_confirm_rsps = 1
+    expected_random_reqs = 1
+    expected_random_rsps = 1
+else
+    expected_confirm_rsps = $ctxt[:confirm_reqs].size
+    expected_random_reqs = $ctxt[:random_reqs].size
+    expected_random_rsps = $ctxt[:random_rsps].size
+end
+
+if $ctxt[:confirm_rsps].size != expected_confirm_rsps
+    raise "wrong number of confirm responses " +
+          "(exp=#{expected_confirm_rsps}; got=#{$ctxt[:confirm_rsps].size}"
+end
+
+if $ctxt[:random_reqs].size != expected_random_reqs
+    raise "wrong number of random requests " +
+          "(exp=#{expected_random_reqs}; got=#{$ctxt[:random_reqs].size}"
+end
+
+if $ctxt[:random_rsps].size != expected_random_rsps
+    raise "wrong number of random responses " +
+          "(exp=#{expected_random_rsps}; got=#{$ctxt[:random_rsps].size}"
+end
+
+passkey_info_fill($ctxt[:passkey_info])
+
+$ctxt[:sc] = $ctxt[:public_key_req] != nil
+$ctxt[:bonding] = $ctxt[:pair_req][:authreq] & 1 == 1 &&
+                  $ctxt[:pair_rsp][:authreq] & 1 == 1
+
+puts test_case_comment()
+puts <<-eos
+TEST_CASE(#{test_case_name()})
+{
+    struct ble_sm_test_params params;
+
+    params = (struct ble_sm_test_params) {
+eos
+
+puts addrs_to_s($ctxt[:addrs])
+
+puts pair_cmd_to_s($ctxt[:pair_req], true)
+puts pair_cmd_to_s($ctxt[:pair_rsp], false)
+
+if $ctxt[:sc]
+    puts privkey_to_s($ctxt[:privkey])
+    puts public_key_to_s($ctxt[:public_key_req], true)
+    puts public_key_to_s($ctxt[:public_key_req], false)
+end
+
+$ctxt[:confirm_rsps].size.times do |i|
+    confirm_req = $ctxt[:confirm_reqs][i]
+    confirm_rsp = $ctxt[:confirm_rsps][i]
+    random_req = $ctxt[:random_reqs][i]
+    random_rsp = $ctxt[:random_rsps][i]
+
+    if confirm_req != nil
+        puts confirm_to_s(confirm_req, true, i)
+    end
+
+    puts confirm_to_s(confirm_rsp, false, i)
+    puts random_to_s(random_req, true, i)
+    puts random_to_s(random_rsp, false, i)
+end
+
+if $ctxt[:sc]
+    puts dhkey_check_to_s($ctxt[:dhkey_check_req], true)
+    puts dhkey_check_to_s($ctxt[:dhkey_check_rsp], false)
+end
+
+if $ctxt[:enc_info_req] != nil
+    puts enc_info_to_s($ctxt[:enc_info_req], true)
+end
+if $ctxt[:master_id_req] != nil
+    puts master_id_to_s($ctxt[:master_id_req], true)
+end
+if $ctxt[:id_info_req] != nil
+    puts id_info_to_s($ctxt[:id_info_req], true)
+end
+if $ctxt[:id_addr_info_req] != nil
+    puts id_addr_info_to_s($ctxt[:id_addr_info_req], true)
+end
+if $ctxt[:sign_info_req] != nil
+    puts sign_info_to_s($ctxt[:sign_info_req], true)
+end
+if $ctxt[:enc_info_rsp] != nil
+    puts enc_info_to_s($ctxt[:enc_info_rsp], false)
+end
+if $ctxt[:master_id_rsp] != nil
+    puts master_id_to_s($ctxt[:master_id_rsp], false)
+end
+if $ctxt[:id_info_rsp] != nil
+    puts id_info_to_s($ctxt[:id_info_rsp], false)
+end
+if $ctxt[:id_addr_info_rsp] != nil
+    puts id_addr_info_to_s($ctxt[:id_addr_info_rsp], false)
+end
+if $ctxt[:sign_info_rsp] != nil
+    puts sign_info_to_s($ctxt[:sign_info_rsp], false)
+end
+if $ctxt[:sc]
+    puts ltk_to_s($ctxt[:ltk])
+else
+    puts stk_to_s($ctxt[:stk])
+end
+puts passkey_info_s()
+
+puts '    };'
+
+if $ctxt[:sc]
+    if $ctxt[:we_are_init]
+        puts '    ble_sm_test_util_us_sc_good(&params);'
+    else
+        puts '    ble_sm_test_util_peer_sc_good(&params);'
+    end
+else
+    if $ctxt[:we_are_init]
+        puts '    ble_sm_test_util_us_lgcy_good(&params);'
+    else
+        puts '    ble_sm_test_util_peer_lgcy_good(&params);'
+    end
+end
+puts '}'
diff --git a/src/libs/mynewt-nimble/nimble/host/util/include/host/util/util.h b/src/libs/mynewt-nimble/nimble/host/util/include/host/util/util.h
new file mode 100644
index 00000000..3f07c005
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/util/include/host/util/util.h
@@ -0,0 +1,46 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_HOST_UTIL_
+#define H_HOST_UTIL_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Tries to configure the device with at least one Bluetooth address.
+ * Addresses are restored in a hardware-specific fashion.
+ *
+ * @param prefer_random         Whether to attempt to restore a random address
+ *                                  before checking if a public address has
+ *                                  already been configured.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOADDR if the device does not have any
+ *                                  available addresses.
+ *                              Other BLE host core code on error.
+ */
+int ble_hs_util_ensure_addr(int prefer_random);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/host/util/pkg.yml b/src/libs/mynewt-nimble/nimble/host/util/pkg.yml
new file mode 100644
index 00000000..0f5f3a5d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/util/pkg.yml
@@ -0,0 +1,29 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: nimble/host/util
+pkg.description: Supplementary utilities for the NimBLE host
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - nimble/host
diff --git a/src/libs/mynewt-nimble/nimble/host/util/src/addr.c b/src/libs/mynewt-nimble/nimble/host/util/src/addr.c
new file mode 100644
index 00000000..9b43d237
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/util/src/addr.c
@@ -0,0 +1,95 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "host/ble_hs.h"
+#include "host/util/util.h"
+
+#if MYNEWT_VAL(BLE_CONTROLLER)
+#include "controller/ble_hw.h"
+#endif
+
+static int
+ble_hs_util_load_rand_addr(ble_addr_t *addr)
+{
+    /* XXX: It is unfortunate that the function to retrieve the random address
+     * is in the controller package.  A host-only device ought to be able to
+     * automically restore a random address.
+     */
+#if MYNEWT_VAL(BLE_CONTROLLER)
+    int rc;
+
+    rc = ble_hw_get_static_addr(addr);
+    if (rc == 0) {
+        return 0;
+    }
+#endif
+
+    return BLE_HS_ENOADDR;
+}
+
+static int
+ble_hs_util_ensure_rand_addr(void)
+{
+    ble_addr_t addr;
+    int rc;
+
+    /* If we already have a random address, then we are done. */
+    rc = ble_hs_id_copy_addr(BLE_ADDR_RANDOM, NULL, NULL);
+    if (rc == 0) {
+        return 0;
+    }
+
+    /* Otherwise, try to load a random address. */
+    rc = ble_hs_util_load_rand_addr(&addr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Configure nimble to use the random address. */
+    rc = ble_hs_id_set_rnd(addr.val);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+ble_hs_util_ensure_addr(int prefer_random)
+{
+    int rc;
+
+    if (prefer_random) {
+        /* Try to load a random address. */
+        rc = ble_hs_util_ensure_rand_addr();
+        if (rc == BLE_HS_ENOADDR) {
+            /* No random address; try to load a public address. */
+            rc = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, NULL, NULL);
+        }
+    } else {
+        /* Try to load a public address. */
+        rc = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, NULL, NULL);
+        if (rc == BLE_HS_ENOADDR) {
+            /* No public address; try to load a random address. */
+            rc = ble_hs_util_ensure_rand_addr();
+        }
+    }
+
+    return rc;
+}
diff --git a/src/libs/mynewt-nimble/nimble/host/util/syscfg.yml b/src/libs/mynewt-nimble/nimble/host/util/syscfg.yml
new file mode 100644
index 00000000..2cdd5746
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/host/util/syscfg.yml
@@ -0,0 +1,19 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+syscfg.defs: