333 files changed, 158458 insertions, 0 deletions

diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_hw.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_hw.h
new file mode 100644
index 00000000..cf293076
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_hw.h
@@ -0,0 +1,116 @@
+/*
+ * 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_HW_
+#define H_BLE_HW_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "syscfg/syscfg.h"
+
+#if defined(ARCH_sim)
+#define BLE_USES_HW_WHITELIST   (0)
+#else
+#define BLE_USES_HW_WHITELIST   MYNEWT_VAL(BLE_HW_WHITELIST_ENABLE)
+#endif
+
+/* Returns the number of hw whitelist elements */
+uint8_t ble_hw_whitelist_size(void);
+
+/* Clear the whitelist */
+void ble_hw_whitelist_clear(void);
+
+/* Remove a device from the hw whitelist */
+void ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type);
+
+/* Add a device to the hw whitelist */
+int ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type);
+
+/* Enable hw whitelisting */
+void ble_hw_whitelist_enable(void);
+
+/* Enable hw whitelisting */
+void ble_hw_whitelist_disable(void);
+
+/* Boolean function returning true if address matches a whitelist entry */
+int ble_hw_whitelist_match(void);
+
+/* Encrypt data */
+struct ble_encryption_block;
+int ble_hw_encrypt_block(struct ble_encryption_block *ecb);
+
+/* Random number generation */
+typedef void (*ble_rng_isr_cb_t)(uint8_t rnum);
+int ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias);
+
+/**
+ * Start the random number generator
+ *
+ * @return int
+ */
+int ble_hw_rng_start(void);
+
+/**
+ * Stop the random generator
+ *
+ * @return int
+ */
+int ble_hw_rng_stop(void);
+
+/**
+ * Read the random number generator.
+ *
+ * @return uint8_t
+ */
+uint8_t ble_hw_rng_read(void);
+
+/*  Clear the resolving list*/
+void ble_hw_resolv_list_clear(void);
+
+/* Add a device to the hw resolving list */
+int ble_hw_resolv_list_add(uint8_t *irk);
+
+/* Remove a device from the hw resolving list */
+void ble_hw_resolv_list_rmv(int index);
+
+/* Returns the size of the whitelist in HW */
+uint8_t ble_hw_resolv_list_size(void);
+
+/* Enable the resolving list */
+void ble_hw_resolv_list_enable(void);
+
+/* Disables resolving list devices */
+void ble_hw_resolv_list_disable(void);
+
+/* Returns index of resolved address; -1 if not resolved */
+int ble_hw_resolv_list_match(void);
+
+/* Returns public device address or -1 if not present */
+int ble_hw_get_public_addr(ble_addr_t *addr);
+
+/* Returns random static address or -1 if not present */
+int ble_hw_get_static_addr(ble_addr_t *addr);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_HW_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll.h
new file mode 100644
index 00000000..e515fb98
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll.h
@@ -0,0 +1,587 @@
+/*
+ * 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_LL_
+#define H_BLE_LL_
+
+#include "stats/stats.h"
+#include "os/os_cputime.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/nimble_npl.h"
+#include "controller/ble_phy.h"
+
+#ifdef MYNEWT
+#include "controller/ble_ll_ctrl.h"
+#include "hal/hal_system.h"
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if MYNEWT_VAL(OS_CPUTIME_FREQ) != 32768
+#error 32.768kHz clock required
+#endif
+
+#if defined(MYNEWT) && MYNEWT_VAL(BLE_LL_VND_EVENT_ON_ASSERT)
+#ifdef NDEBUG
+#define BLE_LL_ASSERT(cond) (void(0))
+#else
+#define BLE_LL_ASSERT(cond) \
+    if (!(cond)) { \
+        if (hal_debugger_connected()) { \
+            assert(0);\
+        } else {\
+            ble_ll_hci_ev_send_vendor_err(__FILE__, __LINE__); \
+            while(1) {}\
+        }\
+    }
+#endif
+#else
+#define BLE_LL_ASSERT(cond) assert(cond)
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#define BLE_LL_BT5_PHY_SUPPORTED    (1)
+#else
+#define BLE_LL_BT5_PHY_SUPPORTED    (0)
+#endif
+
+/* Controller revision. */
+#define BLE_LL_SUB_VERS_NR      (0x0000)
+
+/* Timing jitter as per spec is +/16 usecs */
+#define BLE_LL_JITTER_USECS         (16)
+
+/* Packet queue header definition */
+STAILQ_HEAD(ble_ll_pkt_q, os_mbuf_pkthdr);
+
+/*
+ * Global Link Layer data object. There is only one Link Layer data object
+ * per controller although there may be many instances of the link layer state
+ * machine running.
+ */
+struct ble_ll_obj
+{
+    /* Supported features */
+    uint64_t ll_supp_features;
+
+    /* Current Link Layer state */
+    uint8_t ll_state;
+
+    /* Number of ACL data packets supported */
+    uint8_t ll_num_acl_pkts;
+
+    /* ACL data packet size */
+    uint16_t ll_acl_pkt_size;
+
+    /* Preferred PHY's */
+    uint8_t ll_pref_tx_phys;
+    uint8_t ll_pref_rx_phys;
+
+    /* Task event queue */
+    struct ble_npl_eventq ll_evq;
+
+    /* Wait for response timer */
+    struct hal_timer ll_wfr_timer;
+
+    /* Packet receive queue (and event). Holds received packets from PHY */
+    struct ble_npl_event ll_rx_pkt_ev;
+    struct ble_ll_pkt_q ll_rx_pkt_q;
+
+    /* Packet transmit queue */
+    struct ble_npl_event ll_tx_pkt_ev;
+    struct ble_ll_pkt_q ll_tx_pkt_q;
+
+    /* Data buffer overflow event */
+    struct ble_npl_event ll_dbuf_overflow_ev;
+
+    /* Number of completed packets event */
+    struct ble_npl_event ll_comp_pkt_ev;
+
+    /* HW error callout */
+    struct ble_npl_callout ll_hw_err_timer;
+};
+extern struct ble_ll_obj g_ble_ll_data;
+
+/* Link layer statistics */
+STATS_SECT_START(ble_ll_stats)
+    STATS_SECT_ENTRY(hci_cmds)
+    STATS_SECT_ENTRY(hci_cmd_errs)
+    STATS_SECT_ENTRY(hci_events_sent)
+    STATS_SECT_ENTRY(bad_ll_state)
+    STATS_SECT_ENTRY(bad_acl_hdr)
+    STATS_SECT_ENTRY(no_bufs)
+    STATS_SECT_ENTRY(rx_adv_pdu_crc_ok)
+    STATS_SECT_ENTRY(rx_adv_pdu_crc_err)
+    STATS_SECT_ENTRY(rx_adv_bytes_crc_ok)
+    STATS_SECT_ENTRY(rx_adv_bytes_crc_err)
+    STATS_SECT_ENTRY(rx_data_pdu_crc_ok)
+    STATS_SECT_ENTRY(rx_data_pdu_crc_err)
+    STATS_SECT_ENTRY(rx_data_bytes_crc_ok)
+    STATS_SECT_ENTRY(rx_data_bytes_crc_err)
+    STATS_SECT_ENTRY(rx_adv_malformed_pkts)
+    STATS_SECT_ENTRY(rx_adv_ind)
+    STATS_SECT_ENTRY(rx_adv_direct_ind)
+    STATS_SECT_ENTRY(rx_adv_nonconn_ind)
+    STATS_SECT_ENTRY(rx_adv_ext_ind)
+    STATS_SECT_ENTRY(rx_scan_reqs)
+    STATS_SECT_ENTRY(rx_scan_rsps)
+    STATS_SECT_ENTRY(rx_connect_reqs)
+    STATS_SECT_ENTRY(rx_scan_ind)
+    STATS_SECT_ENTRY(rx_aux_connect_rsp)
+    STATS_SECT_ENTRY(adv_txg)
+    STATS_SECT_ENTRY(adv_late_starts)
+    STATS_SECT_ENTRY(adv_resched_pdu_fail)
+    STATS_SECT_ENTRY(adv_drop_event)
+    STATS_SECT_ENTRY(sched_state_conn_errs)
+    STATS_SECT_ENTRY(sched_state_adv_errs)
+    STATS_SECT_ENTRY(scan_starts)
+    STATS_SECT_ENTRY(scan_stops)
+    STATS_SECT_ENTRY(scan_req_txf)
+    STATS_SECT_ENTRY(scan_req_txg)
+    STATS_SECT_ENTRY(scan_rsp_txg)
+    STATS_SECT_ENTRY(aux_missed_adv)
+    STATS_SECT_ENTRY(aux_scheduled)
+    STATS_SECT_ENTRY(aux_received)
+    STATS_SECT_ENTRY(aux_fired_for_read)
+    STATS_SECT_ENTRY(aux_allocated)
+    STATS_SECT_ENTRY(aux_freed)
+    STATS_SECT_ENTRY(aux_sched_cb)
+    STATS_SECT_ENTRY(aux_conn_req_tx)
+    STATS_SECT_ENTRY(aux_conn_rsp_tx)
+    STATS_SECT_ENTRY(aux_conn_rsp_err)
+    STATS_SECT_ENTRY(aux_scan_req_tx)
+    STATS_SECT_ENTRY(aux_scan_rsp_err)
+    STATS_SECT_ENTRY(aux_chain_cnt)
+    STATS_SECT_ENTRY(aux_chain_err)
+    STATS_SECT_ENTRY(aux_scan_drop)
+    STATS_SECT_ENTRY(adv_evt_dropped)
+    STATS_SECT_ENTRY(scan_timer_stopped)
+    STATS_SECT_ENTRY(scan_timer_restarted)
+    STATS_SECT_ENTRY(periodic_adv_drop_event)
+    STATS_SECT_ENTRY(periodic_chain_drop_event)
+    STATS_SECT_ENTRY(sync_event_failed)
+    STATS_SECT_ENTRY(sync_received)
+    STATS_SECT_ENTRY(sync_chain_failed)
+    STATS_SECT_ENTRY(sync_missed_err)
+    STATS_SECT_ENTRY(sync_crc_err)
+    STATS_SECT_ENTRY(sync_rx_buf_err)
+    STATS_SECT_ENTRY(sync_scheduled)
+    STATS_SECT_ENTRY(sched_state_sync_errs)
+    STATS_SECT_ENTRY(sched_invalid_pdu)
+STATS_SECT_END
+extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
+
+/* States */
+#define BLE_LL_STATE_STANDBY        (0)
+#define BLE_LL_STATE_ADV            (1)
+#define BLE_LL_STATE_SCANNING       (2)
+#define BLE_LL_STATE_INITIATING     (3)
+#define BLE_LL_STATE_CONNECTION     (4)
+#define BLE_LL_STATE_DTM            (5)
+#define BLE_LL_STATE_SYNC           (6)
+
+/* LL Features */
+#define BLE_LL_FEAT_LE_ENCRYPTION    (0x0000000001)
+#define BLE_LL_FEAT_CONN_PARM_REQ    (0x0000000002)
+#define BLE_LL_FEAT_EXTENDED_REJ     (0x0000000004)
+#define BLE_LL_FEAT_SLAVE_INIT       (0x0000000008)
+#define BLE_LL_FEAT_LE_PING          (0x0000000010)
+#define BLE_LL_FEAT_DATA_LEN_EXT     (0x0000000020)
+#define BLE_LL_FEAT_LL_PRIVACY       (0x0000000040)
+#define BLE_LL_FEAT_EXT_SCAN_FILT    (0x0000000080)
+#define BLE_LL_FEAT_LE_2M_PHY        (0x0000000100)
+#define BLE_LL_FEAT_STABLE_MOD_ID_TX (0x0000000200)
+#define BLE_LL_FEAT_STABLE_MOD_ID_RX (0x0000000400)
+#define BLE_LL_FEAT_LE_CODED_PHY     (0x0000000800)
+#define BLE_LL_FEAT_EXT_ADV          (0x0000001000)
+#define BLE_LL_FEAT_PERIODIC_ADV     (0x0000002000)
+#define BLE_LL_FEAT_CSA2             (0x0000004000)
+#define BLE_LL_FEAT_LE_POWER_CLASS_1 (0x0000008000)
+#define BLE_LL_FEAT_MIN_USED_CHAN    (0x0000010000)
+#define BLE_LL_FEAT_CTE_REQ          (0x0000020000)
+#define BLE_LL_FEAT_CTE_RSP          (0x0000040000)
+#define BLE_LL_FEAT_CTE_TX           (0x0000080000)
+#define BLE_LL_FEAT_CTE_RX           (0x0000100000)
+#define BLE_LL_FEAT_CTE_AOD          (0x0000200000)
+#define BLE_LL_FEAT_CTE_AOA          (0x0000400000)
+#define BLE_LL_FEAT_CTE_RECV         (0x0000800000)
+#define BLE_LL_FEAT_SYNC_TRANS_SEND  (0x0001000000)
+#define BLE_LL_FEAT_SYNC_TRANS_RECV  (0x0002000000)
+#define BLE_LL_FEAT_SCA_UPDATE       (0x0004000000)
+#define BLE_LL_FEAT_REM_PKEY         (0x0008000000)
+#define BLE_LL_FEAT_CIS_MASTER       (0x0010000000)
+#define BLE_LL_FEAT_CIS_SLAVE        (0x0020000000)
+#define BLE_LL_FEAT_ISO_BROADCASTER  (0x0040000000)
+#define BLE_LL_FEAT_SYNC_RECV        (0x0080000000)
+#define BLE_LL_FEAT_ISO_HOST_SUPPORT (0x0100000000)
+#define BLE_LL_FEAT_POWER_CTRL_REQ   (0x0200000000)
+#define BLE_LL_FEAT_POWER_CHANGE_IND (0x0400000000)
+#define BLE_LL_FEAT_PATH_LOSS_MON    (0x0800000000)
+
+/* This is initial mask, so if feature exchange will not happen,
+ * but host will want to use this procedure, we will try. If not
+ * succeed, feature bit will be cleared.
+ * Look at LL Features above to find out what is allowed
+ */
+#define BLE_LL_CONN_INITIAL_FEATURES    (0x00000022)
+#define BLE_LL_CONN_CLEAR_FEATURE(connsm, feature)   (connsm->conn_features &= ~(feature))
+
+/* All the features which can be controlled by the Host */
+#define BLE_LL_HOST_CONTROLLED_FEATURES (BLE_LL_FEAT_ISO_HOST_SUPPORT)
+
+/* LL timing */
+#define BLE_LL_IFS                  (150)       /* usecs */
+#define BLE_LL_MAFS                 (300)       /* usecs */
+
+/*
+ * BLE LL device address. Note that element 0 of the array is the LSB and
+ * is sent over the air first. Byte 5 is the MSB and is the last one sent over
+ * the air.
+ */
+#define BLE_DEV_ADDR_LEN            (6)     /* bytes */
+
+struct ble_dev_addr
+{
+    uint8_t u8[BLE_DEV_ADDR_LEN];
+};
+
+#define BLE_IS_DEV_ADDR_STATIC(addr)        ((addr->u8[5] & 0xc0) == 0xc0)
+#define BLE_IS_DEV_ADDR_RESOLVABLE(addr)    ((addr->u8[5] & 0xc0) == 0x40)
+#define BLE_IS_DEV_ADDR_UNRESOLVABLE(addr)  ((addr->u8[5] & 0xc0) == 0x00)
+
+/*
+ * LL packet format
+ *
+ *  -> Preamble         (1/2 bytes)
+ *  -> Access Address   (4 bytes)
+ *  -> PDU              (2 to 257 octets)
+ *  -> CRC              (3 bytes)
+ */
+#define BLE_LL_PREAMBLE_LEN     (1)
+#define BLE_LL_ACC_ADDR_LEN     (4)
+#define BLE_LL_CRC_LEN          (3)
+#define BLE_LL_PDU_HDR_LEN      (2)
+#define BLE_LL_MAX_PAYLOAD_LEN  (255)
+#define BLE_LL_MIN_PDU_LEN      (BLE_LL_PDU_HDR_LEN)
+#define BLE_LL_MAX_PDU_LEN      ((BLE_LL_PDU_HDR_LEN) + (BLE_LL_MAX_PAYLOAD_LEN))
+#define BLE_LL_CRCINIT_ADV      (0x555555)
+
+/* Access address for advertising channels */
+#define BLE_ACCESS_ADDR_ADV             (0x8E89BED6)
+
+/*
+ * Advertising PDU format:
+ * -> 2 byte header
+ *      -> LSB contains pdu type, txadd and rxadd bits.
+ *      -> MSB contains length (6 bits). Length is length of payload. Does
+ *         not include the header length itself.
+ * -> Payload (max 37 bytes)
+ */
+#define BLE_ADV_PDU_HDR_TYPE_MASK           (0x0F)
+#define BLE_ADV_PDU_HDR_CHSEL_MASK          (0x20)
+#define BLE_ADV_PDU_HDR_TXADD_MASK          (0x40)
+#define BLE_ADV_PDU_HDR_RXADD_MASK          (0x80)
+
+/* Advertising channel PDU types */
+#define BLE_ADV_PDU_TYPE_ADV_IND            (0)
+#define BLE_ADV_PDU_TYPE_ADV_DIRECT_IND     (1)
+#define BLE_ADV_PDU_TYPE_ADV_NONCONN_IND    (2)
+#define BLE_ADV_PDU_TYPE_SCAN_REQ           (3)
+#define BLE_ADV_PDU_TYPE_SCAN_RSP           (4)
+#define BLE_ADV_PDU_TYPE_CONNECT_IND        (5)
+#define BLE_ADV_PDU_TYPE_ADV_SCAN_IND       (6)
+#define BLE_ADV_PDU_TYPE_ADV_EXT_IND        (7)
+#define BLE_ADV_PDU_TYPE_AUX_ADV_IND        BLE_ADV_PDU_TYPE_ADV_EXT_IND
+#define BLE_ADV_PDU_TYPE_AUX_SCAN_RSP       BLE_ADV_PDU_TYPE_ADV_EXT_IND
+#define BLE_ADV_PDU_TYPE_AUX_SYNC_IND       BLE_ADV_PDU_TYPE_ADV_EXT_IND
+#define BLE_ADV_PDU_TYPE_AUX_CHAIN_IND      BLE_ADV_PDU_TYPE_ADV_EXT_IND
+#define BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ    BLE_ADV_PDU_TYPE_CONNECT_IND
+#define BLE_ADV_PDU_TYPE_AUX_SCAN_REQ       BLE_ADV_PDU_TYPE_SCAN_REQ
+#define BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP    (8)
+
+/* Extended Header Length (6b) + AdvMode (2b) */
+#define BLE_LL_EXT_ADV_HDR_LEN          (1)
+
+#define BLE_LL_EXT_ADV_ADVA_BIT         (0)
+#define BLE_LL_EXT_ADV_TARGETA_BIT      (1)
+#define BLE_LL_EXT_ADV_CTE_INFO_BIT     (2)
+#define BLE_LL_EXT_ADV_DATA_INFO_BIT    (3)
+#define BLE_LL_EXT_ADV_AUX_PTR_BIT      (4)
+#define BLE_LL_EXT_ADV_SYNC_INFO_BIT    (5)
+#define BLE_LL_EXT_ADV_TX_POWER_BIT     (6)
+
+#define BLE_LL_EXT_ADV_FLAGS_SIZE       (1)
+#define BLE_LL_EXT_ADV_ADVA_SIZE        (6)
+#define BLE_LL_EXT_ADV_TARGETA_SIZE     (6)
+#define BLE_LL_EXT_ADV_DATA_INFO_SIZE   (2)
+#define BLE_LL_EXT_ADV_AUX_PTR_SIZE     (3)
+#define BLE_LL_EXT_ADV_SYNC_INFO_SIZE   (18)
+#define BLE_LL_EXT_ADV_TX_POWER_SIZE    (1)
+
+#define BLE_LL_EXT_ADV_MODE_NON_CONN    (0x00)
+#define BLE_LL_EXT_ADV_MODE_CONN        (0x01)
+#define BLE_LL_EXT_ADV_MODE_SCAN        (0x02)
+
+/* If Channel Selection Algorithm #2 is supported */
+#define BLE_ADV_PDU_HDR_CHSEL               (0x20)
+
+/*
+ * TxAdd and RxAdd bit definitions. A 0 is a public address; a 1 is a
+ * random address.
+ */
+#define BLE_ADV_PDU_HDR_TXADD_RAND          (0x40)
+#define BLE_ADV_PDU_HDR_RXADD_RAND          (0x80)
+
+/*
+ * Data Channel format
+ *
+ *  -> Header (2 bytes)
+ *      -> LSB contains llid, nesn, sn and md
+ *      -> MSB contains length (8 bits)
+ *  -> Payload (0 to 251)
+ *  -> MIC (0 or 4 bytes)
+ */
+#define BLE_LL_DATA_HDR_LLID_MASK       (0x03)
+#define BLE_LL_DATA_HDR_NESN_MASK       (0x04)
+#define BLE_LL_DATA_HDR_SN_MASK         (0x08)
+#define BLE_LL_DATA_HDR_MD_MASK         (0x10)
+#define BLE_LL_DATA_HDR_RSRVD_MASK      (0xE0)
+#define BLE_LL_DATA_PDU_MAX_PYLD        (251)
+#define BLE_LL_DATA_MIC_LEN             (4)
+
+/* LLID definitions */
+#define BLE_LL_LLID_RSRVD               (0)
+#define BLE_LL_LLID_DATA_FRAG           (1)
+#define BLE_LL_LLID_DATA_START          (2)
+#define BLE_LL_LLID_CTRL                (3)
+
+/*
+ * CONNECT_REQ
+ *      -> InitA        (6 bytes)
+ *      -> AdvA         (6 bytes)
+ *      -> LLData       (22 bytes)
+ *          -> Access address (4 bytes)
+ *          -> CRC init (3 bytes)
+ *          -> WinSize (1 byte)
+ *          -> WinOffset (2 bytes)
+ *          -> Interval (2 bytes)
+ *          -> Latency (2 bytes)
+ *          -> Timeout (2 bytes)
+ *          -> Channel Map (5 bytes)
+ *          -> Hop Increment (5 bits)
+ *          -> SCA (3 bits)
+ *
+ *  InitA is the initiators public (TxAdd=0) or random (TxAdd=1) address.
+ *  AdvaA is the advertisers public (RxAdd=0) or random (RxAdd=1) address.
+ *  LLData contains connection request data.
+ *      aa: Link Layer's access address
+ *      crc_init: The CRC initialization value used for CRC calculation.
+ *      winsize: The transmit window size = winsize * 1.25 msecs
+ *      winoffset: The transmit window offset =  winoffset * 1.25 msecs
+ *      interval: The connection interval = interval * 1.25 msecs.
+ *      latency: connection slave latency = latency
+ *      timeout: Connection supervision timeout = timeout * 10 msecs.
+ *      chanmap: contains channel mapping indicating used and unused data
+ *               channels. Only bits that are 1 are usable. LSB is channel 0.
+ *      hop_inc: Hop increment used for frequency hopping. Random value in
+ *               range of 5 to 16.
+ */
+#define BLE_CONNECT_REQ_LEN         (34)
+#define BLE_CONNECT_REQ_PDU_LEN     (BLE_CONNECT_REQ_LEN + BLE_LL_PDU_HDR_LEN)
+
+#define BLE_SCAN_REQ_LEN            (12)
+#define BLE_SCAN_RSP_MAX_LEN        (37)
+#define BLE_SCAN_RSP_MAX_EXT_LEN    (251)
+
+#define BLE_LL_ADDR_SUBTYPE_IDENTITY    (0)
+#define BLE_LL_ADDR_SUBTYPE_RPA         (1)
+#define BLE_LL_ADDR_SUBTYPE_NRPA        (2)
+
+/*--- External API ---*/
+/* Initialize the Link Layer */
+void ble_ll_init(void);
+
+/* Reset the Link Layer */
+int ble_ll_reset(void);
+
+int ble_ll_is_valid_public_addr(const uint8_t *addr);
+
+/* 'Boolean' function returning true if address is a valid random address */
+int ble_ll_is_valid_random_addr(const uint8_t *addr);
+
+/*
+ * Check if given own_addr_type is valid for current controller configuration
+ * given the random address provided (when applicable)
+ */
+int ble_ll_is_valid_own_addr_type(uint8_t own_addr_type,
+                                  const uint8_t *random_addr);
+
+/* Calculate the amount of time in microseconds a PDU with payload length of
+ * 'payload_len' will take to transmit on a PHY 'phy_mode'. */
+uint32_t ble_ll_pdu_tx_time_get(uint16_t payload_len, int phy_mode);
+
+/* Calculate maximum octets of PDU payload which can be transmitted during
+ * 'usecs' on a PHY 'phy_mode'. */
+uint16_t ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode);
+
+/* Is this address a resolvable private address? */
+int ble_ll_is_rpa(const uint8_t *addr, uint8_t addr_type);
+
+int ble_ll_addr_subtype(const uint8_t *addr, uint8_t addr_type);
+
+/* Is this address an identity address? */
+int ble_ll_addr_is_id(uint8_t *addr, uint8_t addr_type);
+
+/* Is 'addr' our device address? 'addr_type' is public (0) or random (!=0) */
+int ble_ll_is_our_devaddr(uint8_t *addr, int addr_type);
+
+/* Get identity address 'addr_type' is public (0) or random (!=0) */
+uint8_t *ble_ll_get_our_devaddr(uint8_t addr_type);
+
+/**
+ * Called to put a packet on the Link Layer transmit packet queue.
+ *
+ * @param txpdu Pointer to transmit packet
+ */
+void ble_ll_acl_data_in(struct os_mbuf *txpkt);
+
+/**
+ * Allocates mbuf for received PDU
+ *
+ * This allocated mbuf (may be chained if necessary) that has capacity large
+ * enough to store received PDU of given length. It does not set mbufs length
+ * as this has to be done by PHY when copying data.
+ *
+ * @param len  Length of PDU, including PDU header and excluding MIC (if encrypted)
+ *
+ * @return mbuf large enough to store received PDU on success
+ *         NULL on failure (oom)
+ */
+struct os_mbuf *ble_ll_rxpdu_alloc(uint16_t len);
+
+/* Tell the Link Layer there has been a data buffer overflow */
+void ble_ll_data_buffer_overflow(void);
+
+/* Tell the link layer there has been a hardware error */
+void ble_ll_hw_error(void);
+
+/*--- PHY interfaces ---*/
+struct ble_mbuf_hdr;
+
+/* Called by the PHY when a packet has started */
+int ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *hdr);
+
+/* Called by the PHY when a packet reception ends */
+int ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
+
+/* Helper callback to tx mbuf using ble_phy_tx() */
+uint8_t ble_ll_tx_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte);
+uint8_t ble_ll_tx_flat_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte);
+
+/*--- Controller API ---*/
+void ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr);
+
+/* Set the link layer state */
+void ble_ll_state_set(uint8_t ll_state);
+
+/* Get the link layer state */
+uint8_t ble_ll_state_get(void);
+
+/* Send an event to LL task */
+void ble_ll_event_send(struct ble_npl_event *ev);
+
+/* Hand received pdu's to LL task  */
+void ble_ll_rx_pdu_in(struct os_mbuf *rxpdu);
+
+/*
+ * Set public address
+ *
+ * This can be used to set controller public address from vendor specific storage,
+ * usually should be done in hal_bsp_init().
+ * Shall be *only* called before LL is initialized, i.e. before sysinit stage.
+ */
+int ble_ll_set_public_addr(const uint8_t *addr);
+
+/* Set random address */
+int ble_ll_set_random_addr(const uint8_t *cmdbuf, uint8_t len, bool hci_adv_ext);
+
+/* Wait for response timer expiration callback */
+void ble_ll_wfr_timer_exp(void *arg);
+
+/* Read set of features supported by the Link Layer */
+uint64_t ble_ll_read_supp_features(void);
+
+/* Set host supported features */
+int ble_ll_set_host_feat(const uint8_t *cmdbuf, uint8_t len);
+
+/* Read set of states supported by the Link Layer */
+uint64_t ble_ll_read_supp_states(void);
+
+/* Check if octets and time are valid. Returns 0 if not valid */
+int ble_ll_chk_txrx_octets(uint16_t octets);
+int ble_ll_chk_txrx_time(uint16_t time);
+
+/* Random numbers */
+int ble_ll_rand_init(void);
+void ble_ll_rand_sample(uint8_t rnum);
+int ble_ll_rand_data_get(uint8_t *buf, uint8_t len);
+void ble_ll_rand_prand_get(uint8_t *prand);
+int ble_ll_rand_start(void);
+
+// TODO added by JF, don't know why I need this?
+void ble_ll_task(void *arg);
+
+static inline int
+ble_ll_get_addr_type(uint8_t txrxflag)
+{
+    if (txrxflag) {
+        return BLE_HCI_ADV_OWN_ADDR_RANDOM;
+    }
+    return BLE_HCI_ADV_OWN_ADDR_PUBLIC;
+}
+
+/* Convert usecs to ticks and round up to nearest tick */
+static inline uint32_t
+ble_ll_usecs_to_ticks_round_up(uint32_t usecs)
+{
+    return os_cputime_usecs_to_ticks(usecs + 30);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/* LTK 0x4C68384139F574D836BCF34E9DFB01BF */
+extern const uint8_t g_bletest_LTK[];
+extern uint16_t g_bletest_EDIV;
+extern uint64_t g_bletest_RAND;
+extern uint64_t g_bletest_SKDm;
+extern uint64_t g_bletest_SKDs;
+extern uint32_t g_bletest_IVm;
+extern uint32_t g_bletest_IVs;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+void ble_ll_dtm_init(void);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_LL_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_adv.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_adv.h
new file mode 100644
index 00000000..4afaadd0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_adv.h
@@ -0,0 +1,209 @@
+/*
+ * 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_LL_ADV_
+#define H_BLE_LL_ADV_
+
+#include "syscfg/syscfg.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * ADV event timing
+ *      T_advEvent = advInterval + advDelay
+ *
+ *      advInterval: increments of 625 usecs
+ *      advDelay: RAND[0, 10] msecs
+ *
+ */
+#define BLE_LL_ADV_ITVL                 (625)           /* usecs */
+#define BLE_LL_ADV_ITVL_MIN             (32)            /* units */
+#define BLE_LL_ADV_ITVL_MAX             (16384)         /* units */
+#define BLE_LL_ADV_ITVL_MS_MIN          (20)            /* msecs */
+#define BLE_LL_ADV_ITVL_MS_MAX          (10240)         /* msecs */
+#define BLE_LL_ADV_ITVL_SCAN_MIN        (160)           /* units */
+#define BLE_LL_ADV_ITVL_SCAN_MS_MIN     (100)           /* msecs */
+#define BLE_LL_ADV_ITVL_NONCONN_MS_MIN  (100)           /* msecs */
+#define BLE_LL_ADV_DELAY_MS_MIN         (0)             /* msecs */
+#define BLE_LL_ADV_DELAY_MS_MAX         (10)            /* msecs */
+#define BLE_LL_ADV_PDU_ITVL_LD_MS_MAX   (10)            /* msecs */
+#define BLE_LL_ADV_PDU_ITVL_HD_MS_MAX   (3750)          /* usecs */
+#define BLE_LL_ADV_STATE_HD_MAX         (1280)          /* msecs */
+#define BLE_LL_ADV_PERIODIC_ITVL        (1250)          /* usecs */
+
+/* Maximum advertisement data length */
+#define BLE_ADV_LEGACY_DATA_MAX_LEN     (31)
+#define BLE_ADV_LEGACY_MAX_PKT_LEN      (37)
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_ADV_DATA_MAX_LEN            MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)
+#else
+#define BLE_ADV_DATA_MAX_LEN            BLE_ADV_LEGACY_DATA_MAX_LEN
+#endif
+
+/*
+ * ADV_IND
+ *      -> AdvA     (6 bytes)
+ *      -> AdvData  (0 - 31 bytes)
+ *
+ *  The advertising address (AdvA) is a public address (TxAdd=0) or random
+ *  address (TxAdd = 1)
+ */
+#define BLE_ADV_IND_MIN_LEN             (6)
+#define BLE_ADV_IND_MAX_LEN             (37)
+
+/*
+ * ADV_DIRECT_IND
+ *      -> AdvA     (6 bytes)
+ *      -> InitA    (6 bytes)
+ *
+ *  AdvA is the advertisers public address (TxAdd=0) or random address
+ *  (TxAdd = 1).
+ *
+ *  InitA is the initiators public or random address. This is the address
+ *  to which this packet is addressed.
+ *
+ */
+#define BLE_ADV_DIRECT_IND_LEN          (12)
+
+/*
+ * ADV_NONCONN_IND
+ *      -> AdvA     (6 bytes)
+ *      -> AdvData  (0 - 31 bytes)
+ *
+ *  The advertising address (AdvA) is a public address (TxAdd=0) or random
+ *  address (TxAdd = 1)
+ *
+ */
+#define BLE_ADV_NONCONN_IND_MIN_LEN     (6)
+#define BLE_ADV_NONCONN_IND_MAX_LEN     (37)
+
+/*
+ * ADV_SCAN_IND
+ *      -> AdvA     (6 bytes)
+ *      -> AdvData  (0 - 31 bytes)
+ *
+ *  The advertising address (AdvA) is a public address (TxAdd=0) or random
+ *  address (TxAdd = 1)
+ *
+ */
+#define BLE_ADV_SCAN_IND_MIN_LEN        (6)
+#define BLE_ADV_SCAN_IND_MAX_LEN        (37)
+
+/*---- HCI ----*/
+struct ble_ll_adv_sm;
+struct ble_ll_conn_sm;
+
+/* Start an advertiser */
+int ble_ll_adv_start_req(uint8_t adv_chanmask, uint8_t adv_type,
+                         uint8_t *init_addr, uint16_t adv_itvl, void *handle);
+
+/* Start or stop advertising */
+int ble_ll_hci_adv_set_enable(const uint8_t *cmdbuf, uint8_t len);
+
+/* Set legacy advertising data */
+int ble_ll_hci_set_adv_data(const uint8_t *cmdbuf, uint8_t len);
+
+/* Set scan response data */
+int ble_ll_hci_set_scan_rsp_data(const uint8_t *cmd, uint8_t cmd_len);
+
+/* Set advertising parameters */
+int ble_ll_adv_set_adv_params(const uint8_t *cmdbuf, uint8_t len);
+
+/* Read advertising channel power */
+int ble_ll_adv_read_txpwr(uint8_t *rspbuf, uint8_t *rsplen);
+
+/*---- API used by BLE LL ----*/
+/* Send the connection complete event */
+void ble_ll_adv_send_conn_comp_ev(struct ble_ll_conn_sm *connsm,
+                                  struct ble_mbuf_hdr *rxhdr);
+
+/* Returns local resolvable private address */
+uint8_t *ble_ll_adv_get_local_rpa(struct ble_ll_adv_sm *advsm);
+
+/* Returns peer resolvable private address */
+uint8_t *ble_ll_adv_get_peer_rpa(struct ble_ll_adv_sm *advsm);
+
+/* Called to initialize advertising functionality. */
+void ble_ll_adv_init(void);
+
+/* Called when LL wait for response timer expires in advertising state */
+void ble_ll_adv_wfr_timer_exp(void);
+
+/* Called to reset the advertiser. */
+void ble_ll_adv_reset(void);
+
+/* Called on rx pdu start when in advertising state */
+int ble_ll_adv_rx_isr_start(uint8_t pdu_type);
+
+/* Called on rx pdu end when in advertising state */
+int ble_ll_adv_rx_isr_end(uint8_t pdu_type, struct os_mbuf *rxpdu, int crcok);
+
+/* Processes received packets at the link layer task */
+void ble_ll_adv_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf,
+                          struct ble_mbuf_hdr *hdr);
+
+/* Boolean function denoting whether or not the whitelist can be changed */
+int ble_ll_adv_can_chg_whitelist(void);
+
+/*
+ * Called when an advertising event has been removed from the scheduler
+ * without being run.
+ */
+void ble_ll_adv_event_rmvd_from_sched(struct ble_ll_adv_sm *advsm);
+
+/*
+ * Called when a periodic event has been removed from the scheduler
+ * without being run.
+ */
+void ble_ll_adv_periodic_rmvd_from_sched(struct ble_ll_adv_sm *advsm);
+
+/* Called to halt currently running advertising event */
+void ble_ll_adv_halt(void);
+
+/* Called to determine if advertising is enabled */
+uint8_t ble_ll_adv_enabled(void);
+
+int ble_ll_adv_hci_set_random_addr(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_set_random_addr(const uint8_t *addr, uint8_t instance);
+int ble_ll_adv_remove(const uint8_t *addr, uint8_t len);
+int ble_ll_adv_clear_all(void);
+int ble_ll_adv_ext_set_param(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_adv_ext_set_adv_data(const uint8_t *cmdbuf, uint8_t cmdlen);
+int ble_ll_adv_ext_set_scan_rsp(const uint8_t *cmdbuf, uint8_t cmdlen);
+int ble_ll_adv_ext_set_enable(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_adv_periodic_set_param(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_periodic_set_data(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_adv_periodic_set_info_transfer(const uint8_t *cmdbuf, uint8_t len,
+                                          uint8_t *rspbuf, uint8_t *rsplen);
+
+/* Called to notify adv code about RPA rotation */
+void ble_ll_adv_rpa_timeout(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_ADV_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_conn.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_conn.h
new file mode 100644
index 00000000..26c99265
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_conn.h
@@ -0,0 +1,425 @@
+/*
+ * 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_LL_CONN_
+#define H_BLE_LL_CONN_
+
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "nimble/nimble_npl.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_ctrl.h"
+#include "controller/ble_phy.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Roles */
+#define BLE_LL_CONN_ROLE_NONE           (0)
+#define BLE_LL_CONN_ROLE_MASTER         (1)
+#define BLE_LL_CONN_ROLE_SLAVE          (2)
+
+/* Connection states */
+#define BLE_LL_CONN_STATE_IDLE          (0)
+#define BLE_LL_CONN_STATE_CREATED       (1)
+#define BLE_LL_CONN_STATE_ESTABLISHED   (2)
+
+/* Channel map size */
+#define BLE_LL_CONN_CHMAP_LEN           (5)
+
+/* Definitions for source clock accuracy */
+#define BLE_MASTER_SCA_251_500_PPM      (0)
+#define BLE_MASTER_SCA_151_250_PPM      (1)
+#define BLE_MASTER_SCA_101_150_PPM      (2)
+#define BLE_MASTER_SCA_76_100_PPM       (3)
+#define BLE_MASTER_SCA_51_75_PPM        (4)
+#define BLE_MASTER_SCA_31_50_PPM        (5)
+#define BLE_MASTER_SCA_21_30_PPM        (6)
+#define BLE_MASTER_SCA_0_20_PPM         (7)
+
+/* Definition for RSSI when the RSSI is unknown */
+#define BLE_LL_CONN_UNKNOWN_RSSI        (127)
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/*
+ * Encryption states for a connection
+ *
+ * NOTE: the states are ordered so that we can check to see if the state
+ * is greater than ENCRYPTED. If so, it means that the start or pause
+ * encryption procedure is running and we should not send data pdu's.
+ */
+enum conn_enc_state {
+    CONN_ENC_S_UNENCRYPTED = 1,
+    CONN_ENC_S_ENCRYPTED,
+    CONN_ENC_S_ENC_RSP_WAIT,
+    CONN_ENC_S_PAUSE_ENC_RSP_WAIT,
+    CONN_ENC_S_PAUSED,
+    CONN_ENC_S_START_ENC_REQ_WAIT,
+    CONN_ENC_S_START_ENC_RSP_WAIT,
+    CONN_ENC_S_LTK_REQ_WAIT,
+    CONN_ENC_S_LTK_NEG_REPLY
+};
+
+/*
+ * Note that the LTK is the key, the SDK is the plain text, and the
+ * session key is the cipher text portion of the encryption block.
+ *
+ * NOTE: we have intentionally violated the specification by making the
+ * transmit and receive packet counters 32-bits as opposed to 39 (as per the
+ * specification). We do this to save code space, ram and calculation time. The
+ * only drawback is that any encrypted connection that sends more than 2^32
+ * packets will suffer a MIC failure and thus be disconnected.
+ */
+struct ble_ll_conn_enc_data
+{
+    uint8_t enc_state;
+    uint8_t tx_encrypted;
+    uint16_t enc_div;
+    uint32_t tx_pkt_cntr;
+    uint32_t rx_pkt_cntr;
+    uint64_t host_rand_num;
+    uint8_t iv[8];
+    struct ble_encryption_block enc_block;
+};
+#endif
+
+/* Connection state machine flags. */
+union ble_ll_conn_sm_flags {
+    struct {
+        uint32_t pkt_rxd:1;
+        uint32_t terminate_ind_txd:1;
+        uint32_t terminate_ind_rxd:1;
+        uint32_t terminate_ind_rxd_acked:1;
+        uint32_t allow_slave_latency:1;
+        uint32_t slave_set_last_anchor:1;
+        uint32_t awaiting_host_reply:1;
+        uint32_t terminate_started:1;
+        uint32_t conn_update_sched:1;
+        uint32_t host_expects_upd_event:1;
+        uint32_t version_ind_sent:1;
+        uint32_t rxd_version_ind:1;
+        uint32_t chanmap_update_scheduled:1;
+        uint32_t conn_empty_pdu_txd:1;
+        uint32_t last_txd_md:1;
+        uint32_t conn_req_txd:1;
+        uint32_t send_ltk_req:1;
+        uint32_t encrypted:1;
+        uint32_t encrypt_chg_sent:1;
+        uint32_t le_ping_supp:1;
+        uint32_t csa2_supp:1;
+        uint32_t host_phy_update: 1;
+        uint32_t phy_update_sched: 1;
+        uint32_t ctrlr_phy_update: 1;
+        uint32_t phy_update_event: 1;
+        uint32_t peer_phy_update: 1; /* XXX:combine with ctrlr udpate bit? */
+        uint32_t aux_conn_req: 1;
+        uint32_t rxd_features:1;
+        uint32_t pending_hci_rd_features:1;
+        uint32_t pending_initiate_dle:1;
+    } cfbit;
+    uint32_t conn_flags;
+} __attribute__((packed));
+
+/**
+ * Structure used for PHY data inside a connection.
+ *
+ * NOTE: the new phy's are the phys we will change to when a phy update
+ * procedure is ongoing and the event counter hits the instant.
+ *
+ * tx_phy_mode: chip specific phy mode for tx
+ * rx_phy_mode: chip specific phy mode for rx
+ * cur_tx_phy: value denoting current tx_phy (not a bitmask!)
+ * cur_rx_phy: value denoting current rx phy (not a bitmask!)
+ * new_tx_phy: value denoting new tx_phy (not a bitmask!)
+ * new_rx_phy: value denoting new rx phy (not a bitmask!)
+ * req_pref_tx_phy: tx phy sent in a phy request (may be different than host)
+ * req_pref_rx_phy: rx phy sent in a phy request (may be different than host)
+ * host_pref_tx_phys: bitmask of preferred transmit PHYs sent by host
+ * host_pref_rx_phys: bitmask of preferred receive PHYs sent by host
+ * phy_options: preferred phy options for coded phy
+ */
+struct ble_ll_conn_phy_data
+{
+    uint32_t tx_phy_mode: 2;
+    uint32_t rx_phy_mode: 2;
+    uint32_t cur_tx_phy: 2;
+    uint32_t cur_rx_phy: 2;
+    uint32_t new_tx_phy: 2;
+    uint32_t new_rx_phy: 2;
+    uint32_t host_pref_tx_phys_mask: 3;
+    uint32_t host_pref_rx_phys_mask: 3;
+    uint32_t req_pref_tx_phys_mask: 3;
+    uint32_t req_pref_rx_phys_mask: 3;
+    uint32_t phy_options: 2;
+}  __attribute__((packed));
+
+#define CONN_CUR_TX_PHY_MASK(csm)   (1 << ((csm)->phy_data.cur_tx_phy - 1))
+#define CONN_CUR_RX_PHY_MASK(csm)   (1 << ((csm)->phy_data.cur_rx_phy - 1))
+
+struct hci_conn_update
+{
+    uint16_t handle;
+    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 hci_ext_conn_params
+{
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    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 hci_ext_create_conn
+{
+    uint8_t filter_policy;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+    uint8_t init_phy_mask;
+    struct hci_ext_conn_params params[3];
+};
+
+/* Connection state machine */
+struct ble_ll_conn_sm
+{
+    /* Connection state machine flags */
+    union ble_ll_conn_sm_flags csmflags;
+
+    /* Current connection handle, state and role */
+    uint16_t conn_handle;
+    uint8_t conn_state;
+    uint8_t conn_role;          /* Can possibly be 1 bit */
+
+    /* RSSI */
+    int8_t conn_rssi;
+
+    /* For privacy */
+    int8_t rpa_index;
+
+    /* Connection data length management */
+    uint8_t max_tx_octets;
+    uint8_t max_rx_octets;
+    uint8_t rem_max_tx_octets;
+    uint8_t rem_max_rx_octets;
+    uint8_t eff_max_tx_octets;
+    uint8_t eff_max_rx_octets;
+    uint16_t max_tx_time;
+    uint16_t max_rx_time;
+    uint16_t rem_max_tx_time;
+    uint16_t rem_max_rx_time;
+    uint16_t eff_max_tx_time;
+    uint16_t eff_max_rx_time;
+    uint8_t max_tx_octets_phy_mode[BLE_PHY_NUM_MODE];
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    uint16_t host_req_max_tx_time;
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    struct ble_ll_conn_phy_data phy_data;
+    uint16_t phy_instant;
+    uint8_t phy_tx_transition;
+#endif
+
+    /* Used to calculate data channel index for connection */
+    uint8_t chanmap[BLE_LL_CONN_CHMAP_LEN];
+    uint8_t req_chanmap[BLE_LL_CONN_CHMAP_LEN];
+    uint16_t chanmap_instant;
+    uint16_t channel_id; /* TODO could be union with hop and last chan used */
+    uint8_t hop_inc;
+    uint8_t data_chan_index;
+    uint8_t last_unmapped_chan;
+    uint8_t num_used_chans;
+
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+    uint8_t period_occ_mask;    /* mask: period 0 = 0x01, period 3 = 0x08 */
+#endif
+
+    /* Ack/Flow Control */
+    uint8_t tx_seqnum;          /* note: can be 1 bit */
+    uint8_t next_exp_seqnum;    /* note: can be 1 bit */
+    uint8_t cons_rxd_bad_crc;   /* note: can be 1 bit */
+    uint8_t last_rxd_sn;        /* note: cant be 1 bit given current code */
+    uint8_t last_rxd_hdr_byte;  /* note: possibly can make 1 bit since we
+                                   only use the MD bit now */
+
+    /* connection event mgmt */
+    uint8_t reject_reason;
+    uint8_t host_reply_opcode;
+    uint8_t master_sca;
+    uint8_t tx_win_size;
+    uint8_t cur_ctrl_proc;
+    uint8_t disconnect_reason;
+    uint8_t rxd_disconnect_reason;
+    uint8_t vers_nr;
+    uint8_t conn_features;
+    uint8_t remote_features[7];
+    uint16_t pending_ctrl_procs;
+    uint16_t event_cntr;
+    uint16_t completed_pkts;
+    uint16_t comp_id;
+    uint16_t sub_vers_nr;
+    uint16_t auth_pyld_tmo;         /* could be ifdef'd. 10 msec units */
+
+    uint32_t access_addr;
+    uint32_t crcinit;               /* only low 24 bits used */
+    /* XXX: do we need ce_end_time? Cant this be sched end time? */
+    uint32_t ce_end_time;   /* cputime at which connection event should end */
+    uint32_t terminate_timeout;
+    uint32_t last_scheduled;
+
+    /* Connection timing */
+    uint16_t conn_itvl;
+    uint16_t slave_latency;
+    uint16_t supervision_tmo;
+    uint16_t min_ce_len;
+    uint16_t max_ce_len;
+    uint16_t tx_win_off;
+    uint32_t anchor_point;
+    uint8_t anchor_point_usecs;     /* XXX: can this be uint8_t ?*/
+    uint8_t conn_itvl_usecs;
+    uint32_t conn_itvl_ticks;
+    uint32_t last_anchor_point;     /* Slave only */
+    uint32_t slave_cur_tx_win_usecs;
+    uint32_t slave_cur_window_widening;
+    uint32_t last_rxd_pdu_cputime;  /* Used exclusively for supervision timer */
+
+    /*
+     * Used to mark that identity address was used as InitA
+     */
+    uint8_t inita_identity_used;
+
+    /* address information */
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+
+    /*
+     * XXX: TODO. Could save memory. Have single event at LL and put these
+     * on a singly linked list. Only would need list pointer here.
+     */
+    /* Connection end event */
+    struct ble_npl_event conn_ev_end;
+
+    /* Packet transmit queue */
+    struct os_mbuf *cur_tx_pdu;
+    STAILQ_HEAD(conn_txq_head, os_mbuf_pkthdr) conn_txq;
+
+    /* List entry for active/free connection pools */
+    union {
+        SLIST_ENTRY(ble_ll_conn_sm) act_sle;
+        STAILQ_ENTRY(ble_ll_conn_sm) free_stqe;
+    };
+
+    /* LL control procedure response timer */
+    struct ble_npl_callout ctrl_proc_rsp_timer;
+
+    /* For scheduling connections */
+    struct ble_ll_sched_item conn_sch;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+    struct ble_npl_callout auth_pyld_timer;
+#endif
+
+    /*
+     * XXX: a note on all these structures for control procedures. First off,
+     * all of these need to be ifdef'd to save memory. Another thing to
+     * consider is this: since most control procedures can only run when no
+     * others are running, can I use just one structure (a union)? Should I
+     * allocate these from a pool? Not sure what to do. For now, I just use
+     * a large chunk of memory per connection.
+     */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    struct ble_ll_conn_enc_data enc_data;
+#endif
+    /*
+     * For connection update procedure. XXX: can make this a pointer and
+     * malloc it if we want to save space.
+     */
+    struct hci_conn_update conn_param_req;
+
+    /* For connection update procedure */
+    struct ble_ll_conn_upd_req conn_update_req;
+
+    /* XXX: for now, just store them all */
+    struct ble_ll_conn_params conn_cp;
+
+    struct ble_ll_scan_sm *scansm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct hci_ext_create_conn initial_params;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    uint8_t  sync_transfer_mode;
+    uint16_t sync_transfer_skip;
+    uint32_t sync_transfer_sync_timeout;
+#endif
+};
+
+/* Flags */
+#define CONN_F_UPDATE_SCHED(csm)    ((csm)->csmflags.cfbit.conn_update_sched)
+#define CONN_F_EMPTY_PDU_TXD(csm)   ((csm)->csmflags.cfbit.conn_empty_pdu_txd)
+#define CONN_F_LAST_TXD_MD(csm)     ((csm)->csmflags.cfbit.last_txd_md)
+#define CONN_F_CONN_REQ_TXD(csm)    ((csm)->csmflags.cfbit.conn_req_txd)
+#define CONN_F_ENCRYPTED(csm)       ((csm)->csmflags.cfbit.encrypted)
+#define CONN_F_ENC_CHANGE_SENT(csm) ((csm)->csmflags.cfbit.encrypt_chg_sent)
+#define CONN_F_LE_PING_SUPP(csm)    ((csm)->csmflags.cfbit.le_ping_supp)
+#define CONN_F_TERMINATE_STARTED(csm) ((csm)->csmflags.cfbit.terminate_started)
+#define CONN_F_CSA2_SUPP(csm)       ((csm)->csmflags.cfbit.csa2_supp)
+#define CONN_F_HOST_PHY_UPDATE(csm) ((csm)->csmflags.cfbit.host_phy_update)
+#define CONN_F_PHY_UPDATE_SCHED(csm) ((csm)->csmflags.cfbit.phy_update_sched)
+#define CONN_F_CTRLR_PHY_UPDATE(csm) ((csm)->csmflags.cfbit.ctrlr_phy_update)
+#define CONN_F_PHY_UPDATE_EVENT(csm) ((csm)->csmflags.cfbit.phy_update_event)
+#define CONN_F_PEER_PHY_UPDATE(csm)  ((csm)->csmflags.cfbit.peer_phy_update)
+#define CONN_F_AUX_CONN_REQ(csm)  ((csm)->csmflags.cfbit.aux_conn_req)
+
+/* Role */
+#define CONN_IS_MASTER(csm)         (csm->conn_role == BLE_LL_CONN_ROLE_MASTER)
+#define CONN_IS_SLAVE(csm)          (csm->conn_role == BLE_LL_CONN_ROLE_SLAVE)
+
+/*
+ * Given a handle, returns an active connection state machine (or NULL if the
+ * handle does not exist
+ *
+ */
+struct ble_ll_conn_sm *ble_ll_conn_find_active_conn(uint16_t handle);
+
+/* required for unit testing */
+uint8_t ble_ll_conn_calc_dci(struct ble_ll_conn_sm *conn, uint16_t latency);
+
+/* used to get anchor point for connection event specified */
+void ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event,
+                            uint32_t *anchor, uint8_t *anchor_usecs);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_CONN_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_ctrl.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_ctrl.h
new file mode 100644
index 00000000..b0da1e73
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -0,0 +1,313 @@
+/*
+ * 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_LL_CTRL_
+#define H_BLE_LL_CTRL_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * LL control procedures. This "enumeration" is not in the specification;
+ * It is used to determine which LL control procedure is currently running
+ * in a connection and which ones may be pending.
+ */
+#define BLE_LL_CTRL_PROC_CONN_UPDATE    (0)
+#define BLE_LL_CTRL_PROC_CHAN_MAP_UPD   (1)
+#define BLE_LL_CTRL_PROC_ENCRYPT        (2)
+#define BLE_LL_CTRL_PROC_FEATURE_XCHG   (3)
+#define BLE_LL_CTRL_PROC_VERSION_XCHG   (4)
+#define BLE_LL_CTRL_PROC_TERMINATE      (5)
+#define BLE_LL_CTRL_PROC_CONN_PARAM_REQ (6)
+#define BLE_LL_CTRL_PROC_LE_PING        (7)
+#define BLE_LL_CTRL_PROC_DATA_LEN_UPD   (8)
+#define BLE_LL_CTRL_PROC_PHY_UPDATE     (9)
+#define BLE_LL_CTRL_PROC_NUM            (10)
+#define BLE_LL_CTRL_PROC_IDLE           (255)
+
+/* Checks if a particular control procedure is running */
+#define IS_PENDING_CTRL_PROC(sm, proc)  (sm->pending_ctrl_procs & (1 << proc))
+#define CLR_PENDING_CTRL_PROC(sm, proc) (sm->pending_ctrl_procs &= ~(1 << proc))
+
+/* LL control procedure timeout */
+#define BLE_LL_CTRL_PROC_TIMEOUT_MS     (40000) /* ms */
+
+/*
+ * LL CTRL PDU format
+ *  -> Opcode   (1 byte)
+ *  -> Data     (0 - 26 bytes)
+ */
+#define BLE_LL_CTRL_CONN_UPDATE_IND     (0)
+#define BLE_LL_CTRL_CHANNEL_MAP_REQ     (1)
+#define BLE_LL_CTRL_TERMINATE_IND       (2)
+#define BLE_LL_CTRL_ENC_REQ             (3)
+#define BLE_LL_CTRL_ENC_RSP             (4)
+#define BLE_LL_CTRL_START_ENC_REQ       (5)
+#define BLE_LL_CTRL_START_ENC_RSP       (6)
+#define BLE_LL_CTRL_UNKNOWN_RSP         (7)
+#define BLE_LL_CTRL_FEATURE_REQ         (8)
+#define BLE_LL_CTRL_FEATURE_RSP         (9)
+#define BLE_LL_CTRL_PAUSE_ENC_REQ       (10)
+#define BLE_LL_CTRL_PAUSE_ENC_RSP       (11)
+#define BLE_LL_CTRL_VERSION_IND         (12)
+#define BLE_LL_CTRL_REJECT_IND          (13)
+#define BLE_LL_CTRL_SLAVE_FEATURE_REQ   (14)
+#define BLE_LL_CTRL_CONN_PARM_REQ       (15)
+#define BLE_LL_CTRL_CONN_PARM_RSP       (16)
+#define BLE_LL_CTRL_REJECT_IND_EXT      (17)
+#define BLE_LL_CTRL_PING_REQ            (18)
+#define BLE_LL_CTRL_PING_RSP            (19)
+#define BLE_LL_CTRL_LENGTH_REQ          (20)
+#define BLE_LL_CTRL_LENGTH_RSP          (21)
+#define BLE_LL_CTRL_PHY_REQ             (22)
+#define BLE_LL_CTRL_PHY_RSP             (23)
+#define BLE_LL_CTRL_PHY_UPDATE_IND      (24)
+#define BLE_LL_CTRL_MIN_USED_CHAN_IND   (25)
+#define BLE_LL_CTRL_CTE_REQ             (26)
+#define BLE_LL_CTRL_CTE_RSP             (27)
+#define BLE_LL_CTRL_PERIODIC_SYNC_IND   (28)
+#define BLE_LL_CTRL_CLOCK_ACCURACY_REQ  (29)
+#define BLE_LL_CTRL_CLOCK_ACCURACY_RSP  (30)
+
+/* Maximum opcode value */
+#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_CLOCK_ACCURACY_RSP + 1)
+
+extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES];
+
+/* Maximum LL control PDU size */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+#define BLE_LL_CTRL_MAX_PDU_LEN         (35)
+#else
+#define BLE_LL_CTRL_MAX_PDU_LEN         (27)
+#endif
+
+/* LL control connection update request */
+struct ble_ll_conn_upd_req
+{
+    uint8_t winsize;
+    uint16_t winoffset;
+    uint16_t interval;
+    uint16_t latency;
+    uint16_t timeout;
+    uint16_t instant;
+};
+#define BLE_LL_CTRL_CONN_UPD_REQ_LEN        (11)
+
+/* LL control channel map request */
+struct ble_ll_chan_map_req
+{
+    uint8_t chmap[5];
+    uint16_t instant;
+};
+#define BLE_LL_CTRL_CHAN_MAP_LEN            (7)
+
+/*
+ * LL control terminate ind
+ *  -> error code (1 byte)
+ */
+#define BLE_LL_CTRL_TERMINATE_IND_LEN      (1)
+
+/* LL control enc req */
+struct ble_ll_enc_req
+{
+    uint8_t rand[8];
+    uint16_t ediv;
+    uint8_t skdm[8];
+    uint32_t ivm;
+};
+
+#define BLE_LL_CTRL_ENC_REQ_LEN             (22)
+
+/* LL control enc rsp */
+struct ble_ll_enc_rsp
+{
+    uint8_t skds[8];
+    uint32_t ivs;
+};
+
+#define BLE_LL_CTRL_ENC_RSP_LEN             (12)
+
+/* LL control start/pause enc request and response */
+#define BLE_LL_CTRL_START_ENC_REQ_LEN       (0)
+#define BLE_LL_CTRL_START_ENC_RSP_LEN       (0)
+#define BLE_LL_CTRL_PAUSE_ENC_REQ_LEN       (0)
+#define BLE_LL_CTRL_PAUSE_ENC_RSP_LEN       (0)
+
+/*
+ * LL control unknown response
+ *  -> 1 byte which contains the unknown or un-supported opcode.
+ */
+#define BLE_LL_CTRL_UNK_RSP_LEN             (1)
+
+/*
+ * LL control feature req and LL control feature rsp
+ *  -> 8 bytes of data containing features supported by device.
+ */
+#define BLE_LL_CTRL_FEATURE_LEN             (8)
+
+/*
+ * LL control version ind
+ *  -> version (1 byte):
+ *      Contains the version number of the bluetooth controller specification.
+ *  -> comp_id (2 bytes)
+ *      Contains the company identifier of the manufacturer of the controller.
+ *  -> sub_ver_num: Contains a unique value for implementation or revision of
+ *      the bluetooth controller.
+ */
+struct ble_ll_version_ind
+{
+    uint8_t ble_ctrlr_ver;
+    uint16_t company_id;
+    uint16_t sub_ver_num;
+};
+
+#define BLE_LL_CTRL_VERSION_IND_LEN         (5)
+
+/*
+ * LL control reject ind
+ *  -> error code (1 byte): contains reason why request was rejected.
+ */
+#define BLE_LL_CTRL_REJ_IND_LEN             (1)
+
+/*
+ * LL control slave feature req
+ *  -> 8 bytes of data containing features supported by device.
+ */
+#define BLE_LL_CTRL_SLAVE_FEATURE_REQ_LEN   (8)
+
+/* LL control connection param req and connection param rsp */
+struct ble_ll_conn_params
+{
+    uint16_t interval_min;
+    uint16_t interval_max;
+    uint16_t latency;
+    uint16_t timeout;
+    uint8_t pref_periodicity;
+    uint16_t ref_conn_event_cnt;
+    uint16_t offset0;
+    uint16_t offset1;
+    uint16_t offset2;
+    uint16_t offset3;
+    uint16_t offset4;
+    uint16_t offset5;
+};
+
+#define BLE_LL_CTRL_CONN_PARAMS_LEN     (23)
+
+/* LL control reject ind ext */
+struct ble_ll_reject_ind_ext
+{
+    uint8_t reject_opcode;
+    uint8_t err_code;
+};
+
+#define BLE_LL_CTRL_REJECT_IND_EXT_LEN  (2)
+
+/* LL control ping req and ping rsp (contain no data) */
+#define BLE_LL_CTRL_PING_LEN            (0)
+
+/*
+ * LL control length req and length rsp
+ *  -> max_rx_bytes (2 bytes): defines connMaxRxOctets. Range 27 to 251
+ *  -> max_rx_time (2 bytes): defines connMaxRxTime. Range 328 to 2120 usecs.
+ *  -> max_tx_bytes (2 bytes): defines connMaxTxOctets. Range 27 to 251
+ *  -> max_tx_time (2 bytes): defines connMaxTxTime. Range 328 to 2120 usecs.
+ */
+struct ble_ll_len_req
+{
+    uint16_t max_rx_bytes;
+    uint16_t max_rx_time;
+    uint16_t max_tx_bytes;
+    uint16_t max_tx_time;
+};
+
+#define BLE_LL_CTRL_LENGTH_REQ_LEN      (8)
+
+/* PHY request/response */
+#define BLE_LL_CTRL_PHY_REQ_LEN         (2)
+#define BLE_LL_CTRL_PHY_RSP_LEN         (2)
+#define BLE_LL_CTRL_PHY_UPD_IND_LEN     (4)
+
+/* Min used channels */
+#define BLE_LL_CTRL_MIN_USED_CHAN_LEN   (2)
+
+/* CTE REQ */
+#define BLE_LL_CTRL_CTE_REQ_LEN         (1)
+
+/* CTE RSP (contains no data) */
+#define BLE_LL_CTRL_CTE_RSP_LEN     (0)
+
+/* Periodic Sync Transfer IND */
+#define BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN   (34)
+
+/* Clock accuracy request/response */
+#define BLE_LL_CTRL_CLOCK_ACCURACY_REQ_LEN  (1)
+#define BLE_LL_CTRL_CLOCK_ACCURACY_RSP_LEN  (1)
+
+/* API */
+struct ble_ll_conn_sm;
+void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc);
+void ble_ll_ctrl_proc_stop(struct ble_ll_conn_sm *connsm, int ctrl_proc);
+int ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om);
+void ble_ll_ctrl_chk_proc_start(struct ble_ll_conn_sm *connsm);
+void ble_ll_ctrl_terminate_start(struct ble_ll_conn_sm *connsm);
+int ble_ll_ctrl_is_terminate_ind(uint8_t hdr, uint8_t opcode);
+uint8_t ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm,
+                                     uint8_t *rsp,
+                                     struct ble_ll_conn_params *req);
+int ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm,
+                                uint8_t rej_opcode, uint8_t err);
+int ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm);
+int ble_ll_ctrl_enc_allowed_pdu_rx(struct os_mbuf *rxpdu);
+int ble_ll_ctrl_enc_allowed_pdu_tx(struct os_mbuf_pkthdr *pkthdr);
+int ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm);
+int ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu);
+
+void ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm);
+void ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm,
+                                     struct ble_ll_conn_params *cp);
+void ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status);
+void ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm,
+                                      uint8_t status);
+void ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status);
+void ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status);
+int ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm);
+int ble_ll_hci_ev_hw_err(uint8_t hw_err);
+void ble_ll_hci_ev_databuf_overflow(void);
+void ble_ll_hci_ev_le_csa(struct ble_ll_conn_sm *connsm);
+void ble_ll_hci_ev_send_scan_req_recv(uint8_t adv_handle, const uint8_t *peer,
+                                      uint8_t peer_addr_type);
+void ble_ll_hci_ev_send_scan_timeout(void);
+void ble_ll_hci_ev_send_adv_set_terminated(uint8_t status, uint8_t adv_handle,
+                                           uint16_t conn_handle, uint8_t events);
+int ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status);
+void ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm);
+void ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm);
+void ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm);
+void ble_ll_hci_ev_send_vendor_err(const char *file, uint32_t line);
+
+uint8_t ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask);
+uint8_t ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_CTRL_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_hci.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_hci.h
new file mode 100644
index 00000000..abef8746
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_hci.h
@@ -0,0 +1,75 @@
+/*
+ * 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_LL_HCI_
+#define H_BLE_LL_HCI_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "nimble/hci_common.h"
+
+/* For supported commands */
+#define BLE_LL_SUPP_CMD_LEN (42)
+extern const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN];
+
+/* The largest event the controller will send. */
+#define BLE_LL_MAX_EVT_LEN  MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)
+
+/*
+ * This determines the number of outstanding commands allowed from the
+ * host to the controller. NOTE: you cannot change this without modifying
+ * other portions of the code as we currently use a global os event for
+ * the command; you would need to allocate a pool of these.
+ */
+#define BLE_LL_CFG_NUM_HCI_CMD_PKTS     (1)
+
+typedef void (*ble_ll_hci_post_cmd_complete_cb)(void);
+
+/* Initialize LL HCI */
+void ble_ll_hci_init(void);
+
+/* Used to determine if the LE event is enabled/disabled */
+bool ble_ll_hci_is_le_event_enabled(unsigned int subev);
+
+/* Used to determine if event is enabled/disabled */
+bool ble_ll_hci_is_event_enabled(unsigned int evcode);
+
+/* Send event from controller to host */
+int ble_ll_hci_event_send(struct ble_hci_ev *hci_ev);
+
+/* Sends a command complete with a no-op opcode to host */
+void ble_ll_hci_send_noop(void);
+
+/* Checks the preferref phy masks from set default phy and set phy commands */
+int ble_ll_hci_chk_phy_masks(uint8_t all_phys, uint8_t tx_phys, uint8_t rx_phys,
+                             uint8_t *txphy, uint8_t *rxphy);
+
+/* Returns true if Extended Advertising HCI commands are in use */
+bool ble_ll_hci_adv_mode_ext(void);
+
+/* Get TX power compensation rounded to integer dB */
+int8_t ble_ll_get_tx_pwr_compensation(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_HCI_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_resolv.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_resolv.h
new file mode 100644
index 00000000..228e0a37
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_resolv.h
@@ -0,0 +1,116 @@
+/*
+ * 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_LL_RESOLV_
+#define H_BLE_LL_RESOLV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * An entry in the resolving list.
+ *      The identity address is stored in little endian format.
+ *      The local rpa is stored in little endian format.
+ *      The IRKs are stored in big endian format.
+ *
+ *  Note:
+ *  rl_local_irk and rl_peer_irk need to be word aligned
+ */
+struct ble_ll_resolv_entry
+{
+    uint8_t rl_addr_type;
+    uint8_t rl_priv_mode;
+    uint8_t rl_has_local;
+    uint8_t rl_has_peer;
+    uint8_t rl_local_irk[16];
+    uint8_t rl_peer_irk[16];
+    uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN];
+    uint8_t rl_local_rpa[BLE_DEV_ADDR_LEN];
+    uint8_t rl_peer_rpa[BLE_DEV_ADDR_LEN];
+};
+
+extern struct ble_ll_resolv_entry g_ble_ll_resolv_list[];
+
+/* Clear the resolving list */
+int ble_ll_resolv_list_clr(void);
+
+/* Read the size of the resolving list */
+int ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen);
+
+/* Add a device to the resolving list */
+int ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len);
+
+/* Remove a device from the resolving list */
+int ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len);
+
+/* Address resolution enable command */
+int ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                               uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                                uint8_t *rspbuf, uint8_t *rsplen);
+
+/* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */
+struct ble_ll_resolv_entry *
+ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type);
+
+/* Returns true if address resolution is enabled */
+uint8_t ble_ll_resolv_enabled(void);
+
+/* Reset private address resolution */
+void ble_ll_resolv_list_reset(void);
+
+/* Generate local or peer RPA. It is up to caller to make sure required IRK
+ * is present on RL
+ */
+void ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local,
+                                 uint8_t *addr);
+
+void ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa);
+void ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa);
+
+/* Generate a resolvable private address. */
+int ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa,
+                          int local);
+
+/* Set the resolvable private address timeout */
+int ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len);
+
+/* Set the privacy mode */
+int ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len);
+
+/* Get the RPA timeout, in seconds */
+uint32_t ble_ll_resolv_get_rpa_tmo(void);
+
+/* Resolve a resolvable private address */
+int ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk);
+
+/* Try to resolve peer RPA and return index on RL if matched */
+int ble_ll_resolv_peer_rpa_any(const uint8_t *rpa);
+
+/* Initialize resolv*/
+void ble_ll_resolv_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_rfmgmt.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_rfmgmt.h
new file mode 100644
index 00000000..37b81a88
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_rfmgmt.h
@@ -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.
+ */
+
+#ifndef H_BLE_LL_RFMGMT_
+#define H_BLE_LL_RFMGMT_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_ll_rfmgmt_init(void);
+
+#if MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME) > 0
+
+void ble_ll_rfmgmt_reset(void);
+
+/* Notify rfmgmt that scan window has changed (only called from ble_ll_scan) */
+void ble_ll_rfmgmt_scan_changed(bool enabled, uint32_t next_window);
+
+/* Notify rfmgmt that 1st scheduled item has changed (only called from ble_ll_sched) */
+void ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *first);
+
+/* Notify rfmgmt that RF is no longer needed by current event */
+void ble_ll_rfmgmt_release(void);
+
+/* Enables RF immediately and returns tick at which RF will be fully enabled */
+uint32_t ble_ll_rfmgmt_enable_now(void);
+
+/* Returns true only if RF is currently fully enabled (i.e. not off or enabling) */
+bool ble_ll_rfmgmt_is_enabled(void);
+
+#else
+
+static inline void ble_ll_rfmgmt_reset(void) { }
+static inline void ble_ll_rfmgmt_scan_changed(bool e, uint32_t n) { }
+static inline void ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *f) { }
+static inline void ble_ll_rfmgmt_release(void) { }
+static inline uint32_t ble_ll_rfmgmt_enable_now(void) { return 0; }
+static inline bool ble_ll_rfmgmt_is_enabled(void) { return true; }
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_RFMGMT_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_scan.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_scan.h
new file mode 100644
index 00000000..139ad5e1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_scan.h
@@ -0,0 +1,293 @@
+/*
+ * 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_LL_SCAN_
+#define H_BLE_LL_SCAN_
+
+#include "controller/ble_ll_sched.h"
+#include "hal/hal_timer.h"
+#include "syscfg/syscfg.h"
+#include "nimble/nimble_npl.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * SCAN_REQ
+ *      -> ScanA    (6 bytes)
+ *      -> AdvA     (6 bytes)
+ *
+ *  ScanA is the scanners public (TxAdd=0) or random (TxAdd = 1) address
+ *  AdvaA is the advertisers public (RxAdd=0) or random (RxAdd=1) address.
+ *
+ * Sent by the LL in the Scanning state; received by the LL in the advertising
+ * state. The advertising address is the intended recipient of this frame.
+ */
+#define BLE_SCAN_REQ_LEN                (12)
+
+/*
+ * SCAN_RSP
+ *      -> AdvA         (6 bytes)
+ *      -> ScanRspData  (0 - 31 bytes)
+ *
+ *  AdvaA is the advertisers public (TxAdd=0) or random (TxAdd=1) address.
+ *  ScanRspData may contain any data from the advertisers host.
+ *
+ * Sent by the LL in the advertising state; received by the LL in the
+ * scanning state.
+ */
+#define BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN       (31)
+#define BLE_SCAN_LEGACY_MAX_PKT_LEN            (37)
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_SCAN_RSP_DATA_MAX_LEN       MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)
+
+/* For Bluetooth 5.0 we need state machine for two PHYs*/
+#define BLE_LL_SCAN_PHY_NUMBER          (2)
+#else
+#define BLE_LL_SCAN_PHY_NUMBER          (1)
+#define BLE_SCAN_RSP_DATA_MAX_LEN       BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN
+#endif
+
+#define PHY_UNCODED                    (0)
+#define PHY_CODED                      (1)
+
+#define BLE_LL_EXT_ADV_MODE_NON_CONN    (0x00)
+#define BLE_LL_EXT_ADV_MODE_CONN        (0x01)
+#define BLE_LL_EXT_ADV_MODE_SCAN        (0x02)
+
+/* All values are stored as ticks */
+struct ble_ll_scan_timing {
+    uint32_t interval;
+    uint32_t window;
+    uint32_t start_time;
+};
+
+struct ble_ll_scan_params
+{
+    uint8_t phy;
+    uint8_t own_addr_type;
+    uint8_t scan_filt_policy;
+    uint8_t configured;
+    uint8_t scan_type;
+    uint8_t scan_chan;
+    struct ble_ll_scan_timing timing;
+};
+
+#define BLE_LL_AUX_HAS_ADVA                     0x01
+#define BLE_LL_AUX_HAS_TARGETA                  0x02
+#define BLE_LL_AUX_HAS_ADI                      0x04
+#define BLE_LL_AUX_IS_MATCHED                   0x08
+#define BLE_LL_AUX_IS_TARGETA_RESOLVED          0x10
+
+#define BLE_LL_AUX_FLAG_HCI_SENT_ANY            0x02
+#define BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED      0x04
+#define BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED      0x08
+#define BLE_LL_AUX_FLAG_SCAN_COMPLETE           0x10
+#define BLE_LL_AUX_FLAG_SCAN_ERROR              0x20
+#define BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED        0x40
+#define BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED      0x80
+
+struct ble_ll_aux_data {
+    uint8_t flags;
+
+    /*
+     * Since aux_data can be accessed from ISR and LL, we have separate copies
+     * of flags to make sure that ISR does not modify flags while LL uses them.
+     * ISR updates 'flags_isr' and LL adds these to 'flags_ll' which it then
+     * uses for further processing allowing to update 'flags_isr' if another
+     * scan for given 'aux_data' is scheduled. Note that flags must not be unset
+     * while aux_data is valid.
+     */
+    uint8_t flags_isr;
+    uint8_t flags_ll;
+
+    uint8_t ref_cnt;
+    uint8_t chan;
+    uint8_t aux_phy;
+    uint8_t aux_primary_phy;
+    uint8_t mode;
+    uint8_t scanning;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int8_t rpa_index;
+#endif
+    uint16_t adi;
+    uint32_t offset;
+    uint8_t offset_units;
+    uint8_t adva[6];
+    uint8_t adva_type;
+    uint8_t targeta[6];
+    uint8_t targeta_type;
+    uint16_t evt_type;
+    struct ble_ll_sched_item sch;
+    struct ble_hci_ev *evt;
+    struct ble_npl_event ev;
+};
+
+struct ble_ll_scan_pdu_data {
+    uint8_t hdr_byte;
+    /* ScanA for SCAN_REQ and InitA for CONNECT_IND */
+    union {
+        uint8_t scana[BLE_DEV_ADDR_LEN];
+        uint8_t inita[BLE_DEV_ADDR_LEN];
+    };
+    uint8_t adva[BLE_DEV_ADDR_LEN];
+};
+
+struct ble_ll_scan_sm
+{
+    uint8_t scan_enabled;
+    uint8_t own_addr_type;
+    uint8_t scan_filt_dups;
+    uint8_t scan_rsp_pending;
+    uint8_t scan_rsp_cons_fails;
+    uint8_t scan_rsp_cons_ok;
+    uint8_t scan_peer_rpa[BLE_DEV_ADDR_LEN];
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_npl_time_t scan_nrpa_timer;
+    uint8_t scan_nrpa[BLE_DEV_ADDR_LEN];
+#endif
+    struct ble_ll_scan_pdu_data pdu_data;
+
+    /* XXX: Shall we count backoff per phy? */
+    uint16_t upper_limit;
+    uint16_t backoff_count;
+    uint32_t scan_win_start_time;
+    struct ble_npl_event scan_sched_ev;
+    struct hal_timer scan_timer;
+    struct ble_npl_event scan_interrupted_ev;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct hal_timer duration_timer;
+    struct hal_timer period_timer;
+    uint32_t duration_ticks;
+    uint32_t period_ticks;
+    uint8_t ext_scanning;
+#endif
+
+    uint8_t restart_timer_needed;
+    struct ble_ll_aux_data *cur_aux_data;
+
+    struct ble_ll_scan_params *scanp;
+    struct ble_ll_scan_params *scanp_next;
+    struct ble_ll_scan_params scanp_phys[BLE_LL_SCAN_PHY_NUMBER];
+};
+
+/* Scan types */
+#define BLE_SCAN_TYPE_PASSIVE   (BLE_HCI_SCAN_TYPE_PASSIVE)
+#define BLE_SCAN_TYPE_ACTIVE    (BLE_HCI_SCAN_TYPE_ACTIVE)
+#define BLE_SCAN_TYPE_INITIATE  (2)
+
+/*---- HCI ----*/
+/* Set scanning parameters */
+int ble_ll_scan_set_scan_params(const uint8_t *cmdbuf, uint8_t len);
+
+/* Turn scanning on/off */
+int ble_ll_hci_scan_set_enable(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_hci_ext_scan_set_enable(const uint8_t *cmdbuf, uint8_t len);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+int ble_ll_set_ext_scan_params(const uint8_t *cmdbuf, uint8_t len);
+#endif
+
+/*--- Controller Internal API ---*/
+/* Initialize the scanner */
+void ble_ll_scan_init(void);
+
+/* Reset the scanner */
+void ble_ll_scan_reset(void);
+
+/* Called when Link Layer starts to receive a PDU and is in scanning state */
+int ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags);
+
+/* Called when Link Layer has finished receiving a PDU while scanning */
+int ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok);
+
+/* Process a scan response PDU */
+void ble_ll_scan_rx_pkt_in(uint8_t pdu_type, struct os_mbuf *om,
+                           struct ble_mbuf_hdr *hdr);
+
+/* Boolean function denoting whether or not the whitelist can be changed */
+int ble_ll_scan_can_chg_whitelist(void);
+
+/* Boolean function returning true if scanning enabled */
+int ble_ll_scan_enabled(void);
+
+/* Boolean function returns true if whitelist is enabled for scanning */
+int ble_ll_scan_whitelist_enabled(void);
+
+/* Initialize the scanner when we start initiating */
+struct hci_create_conn;
+int ble_ll_scan_initiator_start(struct hci_create_conn *hcc,
+                                struct ble_ll_scan_sm **sm);
+
+/* Returns storage for PDU data (for SCAN_REQ or CONNECT_IND) */
+struct ble_ll_scan_pdu_data *ble_ll_scan_get_pdu_data(void);
+
+/* Called to set the resolvable private address of the last connected peer */
+void ble_ll_scan_set_peer_rpa(uint8_t *rpa);
+
+/* Returns peer RPA of last connection made */
+uint8_t *ble_ll_scan_get_peer_rpa(void);
+
+/* Returns the local RPA used by the scanner/initiator */
+uint8_t *ble_ll_scan_get_local_rpa(void);
+
+/* Stop the scanning state machine */
+void ble_ll_scan_sm_stop(int chk_disable);
+
+/* Resume scanning */
+void ble_ll_scan_chk_resume(void);
+
+/* Called when wait for response timer expires in scanning mode */
+void ble_ll_scan_wfr_timer_exp(void);
+
+/* Called when scan could be interrupted  */
+void ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm);
+
+int ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf,
+                                struct ble_mbuf_hdr *ble_hdr,
+                                uint8_t **addr, uint8_t *addr_type,
+                                uint8_t **inita, uint8_t *init_addr_type,
+                                int *ext_mode);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+int ble_ll_scan_update_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf,
+                                bool *adva_present);
+
+/* Initialize the extended scanner when we start initiating */
+struct hci_ext_create_conn;
+int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc,
+                                    struct ble_ll_scan_sm **sm);
+
+/* Called to parse extended advertising*/
+struct ble_ll_aux_data *ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_scan);
+void ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_scan);
+void ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data);
+#endif
+
+/* Called to halt currently running scan */
+void ble_ll_scan_halt(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_SCAN_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sched.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sched.h
new file mode 100644
index 00000000..a614cf09
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sched.h
@@ -0,0 +1,216 @@
+/*
+ * 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_LL_SCHED_
+#define H_BLE_LL_SCHED_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Time per BLE scheduler slot */
+#define BLE_LL_SCHED_USECS_PER_SLOT         (1250)
+#define BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT   (41)    /* 1 tick = 30.517 usecs */
+
+/*
+ * Worst case time needed for scheduled advertising item. This is the longest
+ * possible time to receive a scan request and send a scan response (with the
+ * appropriate IFS time between them). This number is calculated using the
+ * following formula: IFS + SCAN_REQ + IFS + SCAN_RSP = 150 + 176 + 150 + 376.
+ * Note: worst case time to tx adv, rx scan req and send scan rsp is 1228 usecs.
+ * This assumes maximum sized advertising PDU and scan response PDU.
+ *
+ * For connectable advertising events no scan request is allowed. In this case
+ * we just need to receive a connect request PDU: IFS + CONNECT_REQ = 150 + 352.
+ * Note: worst-case is 376 + 150 + 352 = 878 usecs
+ *
+ * NOTE: The advertising PDU transmit time is NOT included here since we know
+ * how long that will take (worst-case is 376 usecs).
+ */
+#define BLE_LL_SCHED_ADV_MAX_USECS          (852)
+#define BLE_LL_SCHED_DIRECT_ADV_MAX_USECS   (502)
+#define BLE_LL_SCHED_MAX_ADV_PDU_USECS      (376)
+
+/*
+ * This is the offset from the start of the scheduled item until the actual
+ * tx/rx should occur, in ticks.
+ */
+extern uint8_t g_ble_ll_sched_offset_ticks;
+
+/*
+ * This is the number of slots needed to transmit and receive a maximum
+ * size PDU, including an IFS time before each. The actual time is
+ * 2120 usecs for tx/rx and 150 for IFS = 4540 usecs.
+ */
+#define BLE_LL_SCHED_MAX_TXRX_SLOT  (4 * BLE_LL_SCHED_USECS_PER_SLOT)
+
+/* BLE scheduler errors */
+#define BLE_LL_SCHED_ERR_OVERLAP    (1)
+
+/* Types of scheduler events */
+#define BLE_LL_SCHED_TYPE_ADV       (1)
+#define BLE_LL_SCHED_TYPE_SCAN      (2)
+#define BLE_LL_SCHED_TYPE_CONN      (3)
+#define BLE_LL_SCHED_TYPE_AUX_SCAN  (4)
+#define BLE_LL_SCHED_TYPE_DTM       (5)
+#define BLE_LL_SCHED_TYPE_PERIODIC  (6)
+#define BLE_LL_SCHED_TYPE_SYNC      (7)
+
+/* Return values for schedule callback. */
+#define BLE_LL_SCHED_STATE_RUNNING  (0)
+#define BLE_LL_SCHED_STATE_DONE     (1)
+
+/* Callback function */
+struct ble_ll_sched_item;
+typedef int (*sched_cb_func)(struct ble_ll_sched_item *sch);
+typedef void (*sched_remove_cb_func)(struct ble_ll_sched_item *sch);
+/*
+ * Strict connection scheduling (for the master) is different than how
+ * connections are normally scheduled. With strict connection scheduling we
+ * introduce the concept of a "period". A period is a collection of slots. Each
+ * slot is 1.25 msecs in length. The number of slots in a period is determined
+ * by the syscfg value BLE_LL_CONN_INIT_SLOTS. A collection of periods is called
+ * an epoch. The length of an epoch is determined by the number of connections
+ * (BLE_MAX_CONNECTIONS plus BLE_LL_ADD_STRICT_SCHED_PERIODS). Connections
+ * will be scheduled at period boundaries. Any scanning/initiating/advertising
+ * will be done in unused periods, if possible.
+ */
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+#define BLE_LL_SCHED_PERIODS    (MYNEWT_VAL(BLE_MAX_CONNECTIONS) + \
+                                 MYNEWT_VAL(BLE_LL_ADD_STRICT_SCHED_PERIODS))
+
+struct ble_ll_sched_obj
+{
+    uint8_t sch_num_occ_periods;
+    uint32_t sch_occ_period_mask;
+    uint32_t sch_ticks_per_period;
+    uint32_t sch_ticks_per_epoch;
+    uint32_t sch_epoch_start;
+};
+
+extern struct ble_ll_sched_obj g_ble_ll_sched_data;
+
+/*
+ * XXX: TODO:
+ * -> How do we know epoch start is up to date? Not wrapped?
+ * -> for now, only do this with no more than 32 connections.
+ * -> Do not let initiating occur if no empty sched slots
+ */
+#endif
+
+/*
+ * Schedule item
+ *  sched_type: This is the type of the schedule item.
+ *  enqueued: Flag denoting if item is on the scheduler list. 0: no, 1:yes
+ *  remainder: # of usecs from offset till tx/rx should occur
+ *  txrx_offset: Number of ticks from start time until tx/rx should occur.
+ *
+ */
+struct ble_ll_sched_item
+{
+    uint8_t         sched_type;
+    uint8_t         enqueued;
+    uint8_t         remainder;
+    uint32_t        start_time;
+    uint32_t        end_time;
+    void            *cb_arg;
+    sched_cb_func   sched_cb;
+    TAILQ_ENTRY(ble_ll_sched_item) link;
+};
+
+/* Initialize the scheduler */
+int ble_ll_sched_init(void);
+
+/* Remove item(s) from schedule */
+int ble_ll_sched_rmv_elem(struct ble_ll_sched_item *sch);
+
+void ble_ll_sched_rmv_elem_type(uint8_t type, sched_remove_cb_func remove_cb);
+
+/* Schedule a new master connection */
+struct ble_ll_conn_sm;
+int ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm,
+                            struct ble_mbuf_hdr *ble_hdr, uint8_t pyld_len);
+
+/* Schedule a new slave connection */
+int ble_ll_sched_slave_new(struct ble_ll_conn_sm *connsm);
+
+struct ble_ll_adv_sm;
+typedef void ble_ll_sched_adv_new_cb(struct ble_ll_adv_sm *advsm,
+                                     uint32_t sch_start, void *arg);
+
+/* Schedule a new advertising event */
+int ble_ll_sched_adv_new(struct ble_ll_sched_item *sch,
+                         ble_ll_sched_adv_new_cb cb, void *arg);
+
+/* Schedule periodic advertising event */
+int ble_ll_sched_periodic_adv(struct ble_ll_sched_item *sch, uint32_t *start,
+                              bool after_overlap);
+
+int ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch,
+                                 uint32_t anchor_point,
+                                 uint8_t anchor_point_usecs,
+                                 uint32_t window_widening, int8_t phy_mode);
+int ble_ll_sched_sync(struct ble_ll_sched_item *sch,
+                      uint32_t beg_cputime, uint32_t rem_usecs, uint32_t offset,
+                      int8_t phy_mode);
+
+/* Reschedule an advertising event */
+int ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch, uint32_t *start,
+                                uint32_t max_delay_ticks);
+
+/* Reschedule and advertising pdu */
+int ble_ll_sched_adv_resched_pdu(struct ble_ll_sched_item *sch);
+
+/* Reschedule a connection that had previously been scheduled or that is over */
+int ble_ll_sched_conn_reschedule(struct ble_ll_conn_sm * connsm);
+
+/**
+ * Called to determine when the next scheduled event will occur.
+ *
+ * If there are not scheduled events this function returns 0; otherwise it
+ * returns 1 and *next_event_time is set to the start time of the next event.
+ *
+ * @param next_event_time cputime at which next scheduled event will occur
+ *
+ * @return int 0: No events are scheduled 1: there is an upcoming event
+ */
+int ble_ll_sched_next_time(uint32_t *next_event_time);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+struct ble_ll_scan_sm;
+struct ble_ll_aux_data;
+int ble_ll_sched_aux_scan(struct ble_mbuf_hdr *ble_hdr,
+                          struct ble_ll_scan_sm *scansm,
+                          struct ble_ll_aux_data *aux_scan);
+
+int ble_ll_sched_scan_req_over_aux_ptr(uint32_t chan, uint8_t phy_mode);
+#endif
+
+/* Stop the scheduler */
+void ble_ll_sched_stop(void);
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+int ble_ll_sched_dtm(struct ble_ll_sched_item *sch);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_LL_SCHED_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sync.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sync.h
new file mode 100644
index 00000000..712af6df
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sync.h
@@ -0,0 +1,74 @@
+/*
+ * 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_LL_SYNC_
+#define H_BLE_LL_SYNC_
+
+#include <stdint.h>
+
+#include "nimble/ble.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_conn.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_ll_sync_sm;
+
+int ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb);
+int ble_ll_sync_terminate(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_list_add(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_list_remove(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_list_clear(void);
+int ble_ll_sync_list_size(uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len,
+                         uint8_t *rspbuf, uint8_t *rsplen);
+
+void ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
+                              const uint8_t *sync_ind, bool reports_disabled,
+                              uint16_t max_skip, uint32_t sync_timeout);
+void ble_ll_sync_transfer_disconnected(struct ble_ll_conn_sm *connsm);
+
+void ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type,
+                            int rpa_index, uint8_t sid,
+                            struct ble_mbuf_hdr *rxhdr,
+                            const uint8_t *syncinfo);
+
+int ble_ll_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr);
+int ble_ll_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
+void ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
+void ble_ll_sync_wfr_timer_exp(void);
+void ble_ll_sync_halt(void);
+void ble_ll_sync_rmvd_from_sched(struct ble_ll_sync_sm *sm);
+
+uint32_t ble_ll_sync_get_event_end_time(void);
+
+bool ble_ll_sync_enabled(void);
+
+void ble_ll_sync_reset(void);
+void ble_ll_sync_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_SYNC_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_test.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_test.h
new file mode 100644
index 00000000..32984c6b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_test.h
@@ -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.
+ */
+
+#ifndef H_LL_TEST_
+#define H_LL_TEST_
+
+#include <stddef.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_ll_csa2_test_all(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_trace.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_trace.h
new file mode 100644
index 00000000..7545b570
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_trace.h
@@ -0,0 +1,96 @@
+/*
+ * 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_LL_TRACE_
+#define H_BLE_LL_TRACE_
+
+#include "os/os_trace_api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_LL_TRACE_ID_SCHED                   0
+#define BLE_LL_TRACE_ID_RX_START                1
+#define BLE_LL_TRACE_ID_RX_END                  2
+#define BLE_LL_TRACE_ID_WFR_EXP                 3
+#define BLE_LL_TRACE_ID_CTRL_RX                 4
+#define BLE_LL_TRACE_ID_CONN_EV_START           5
+#define BLE_LL_TRACE_ID_CONN_EV_END             6
+#define BLE_LL_TRACE_ID_CONN_END                7
+#define BLE_LL_TRACE_ID_CONN_TX                 8
+#define BLE_LL_TRACE_ID_CONN_RX                 9
+#define BLE_LL_TRACE_ID_ADV_TXDONE              10
+#define BLE_LL_TRACE_ID_ADV_HALT                11
+#define BLE_LL_TRACE_ID_AUX_REF                 12
+#define BLE_LL_TRACE_ID_AUX_UNREF               13
+
+#if MYNEWT_VAL(BLE_LL_SYSVIEW)
+
+extern uint32_t ble_ll_trace_off;
+
+void ble_ll_trace_init(void);
+
+static inline void
+ble_ll_trace_u32(unsigned id, uint32_t p1)
+{
+    os_trace_api_u32(ble_ll_trace_off + id, p1);
+}
+
+static inline void
+ble_ll_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2)
+{
+    os_trace_api_u32x2(ble_ll_trace_off + id, p1, p2);
+}
+
+static inline void
+ble_ll_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3)
+{
+    os_trace_api_u32x3(ble_ll_trace_off + id, p1, p2, p3);
+}
+
+#else
+
+static inline void
+ble_ll_trace_init(void)
+{
+}
+
+static inline void
+ble_ll_trace_u32(unsigned id, uint32_t p1)
+{
+}
+
+static inline void
+ble_ll_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2)
+{
+}
+
+static inline void
+ble_ll_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3)
+{
+}
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_TRACE_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_utils.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_utils.h
new file mode 100644
index 00000000..24830900
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_utils.h
@@ -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.
+ */
+
+#include <stdint.h>
+
+uint32_t ble_ll_utils_calc_access_addr(void);
+uint8_t ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap);
+uint8_t ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id,
+                                   uint8_t num_used_chans, const uint8_t *chanmap);
+uint8_t ble_ll_utils_calc_num_used_chans(const uint8_t *chanmap);
+uint32_t ble_ll_utils_calc_window_widening(uint32_t anchor_point,
+                                           uint32_t last_anchor_point,
+                                           uint8_t master_sca);
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_whitelist.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_whitelist.h
new file mode 100644
index 00000000..2d3b6c5d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_whitelist.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_LL_WHITELIST_
+#define H_BLE_LL_WHITELIST_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Clear the whitelist */
+int ble_ll_whitelist_clear(void);
+
+/* Read the size of the whitelist */
+int ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen);
+
+/* Add a device to the whitelist */
+int ble_ll_whitelist_add(const uint8_t *cmdbuf, uint8_t len);
+
+/* Remove a device fromthe whitelist */
+int ble_ll_whitelist_rmv(const uint8_t *cmdbuf, uint8_t len);
+
+/* Enable whitelisting */
+void ble_ll_whitelist_enable(void);
+
+/* Disable whitelisting */
+void ble_ll_whitelist_disable(void);
+
+/* Boolean function returning true if address matches a whitelist entry */
+int ble_ll_whitelist_match(uint8_t *addr, uint8_t addr_type, int is_ident);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_WHITELIST_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy.h
new file mode 100644
index 00000000..cabb0adb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy.h
@@ -0,0 +1,242 @@
+/*
+ * 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_PHY_
+#define H_BLE_PHY_
+
+#include "nimble/hci_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Forward declarations */
+struct os_mbuf;
+
+/* Channel/Frequency defintions */
+#define BLE_PHY_NUM_CHANS           (40)
+#define BLE_PHY_NUM_DATA_CHANS      (37)
+#define BLE_PHY_CHAN0_FREQ_MHZ      (2402)
+#define BLE_PHY_DATA_CHAN0_FREQ_MHZ (2404)
+#define BLE_PHY_CHAN_SPACING_MHZ    (2)
+#define BLE_PHY_NUM_ADV_CHANS       (3)
+#define BLE_PHY_ADV_CHAN_START      (37)
+
+/* Power */
+#define BLE_PHY_MAX_PWR_DBM         (10)
+
+/* Deviation */
+#define BLE_PHY_DEV_KHZ             (185)
+#define BLE_PHY_BINARY_ZERO         (-BLE_PHY_DEV)
+#define BLE_PHY_BINARY_ONE          (BLE_PHY_DEV)
+
+/* Max. clock drift */
+#define BLE_PHY_MAX_DRIFT_PPM       (50)
+
+/* Data rate */
+#define BLE_PHY_BIT_RATE_BPS        (1000000)
+
+/* Macros */
+#define BLE_IS_ADV_CHAN(chan)       (chan >= BLE_PHY_ADV_CHAN_START)
+#define BLE_IS_DATA_CHAN(chan)      (chan < BLE_PHY_ADV_CHAN_START)
+
+/* PHY states */
+#define BLE_PHY_STATE_IDLE          (0)
+#define BLE_PHY_STATE_RX            (1)
+#define BLE_PHY_STATE_TX            (2)
+
+/* BLE PHY transitions */
+#define BLE_PHY_TRANSITION_NONE     (0)
+#define BLE_PHY_TRANSITION_RX_TX    (1)
+#define BLE_PHY_TRANSITION_TX_RX    (2)
+
+/* PHY error codes */
+#define BLE_PHY_ERR_RADIO_STATE     (1)
+#define BLE_PHY_ERR_INIT            (2)
+#define BLE_PHY_ERR_INV_PARAM       (3)
+#define BLE_PHY_ERR_NO_BUFS         (4)
+#define BLE_PHY_ERR_TX_LATE         (5)
+#define BLE_PHY_ERR_RX_LATE         (6)
+
+/* Maximun PDU length. Includes LL header of 2 bytes and 255 bytes payload. */
+#define BLE_PHY_MAX_PDU_LEN         (257)
+
+/* Wait for response timer */
+typedef void (*ble_phy_tx_end_func)(void *arg);
+
+/* Initialize the PHY */
+int ble_phy_init(void);
+
+/* Reset the PHY */
+int ble_phy_reset(void);
+
+/* Set the PHY channel */
+int ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit);
+
+/* Set transmit start time */
+int ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs);
+
+/* Set receive start time */
+int ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs);
+
+/* Set the transmit end callback and argument */
+void ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg);
+
+typedef uint8_t (*ble_phy_tx_pducb_t)(uint8_t *dptr, void *pducb_arg,
+                                      uint8_t *hdr_byte);
+
+/* Place the PHY into transmit mode */
+int ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans);
+
+/* Place the PHY into receive mode */
+int ble_phy_rx(void);
+
+/* Copies the received PHY buffer into the allocated pdu */
+void ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu);
+
+/* Set the transmit power */
+int ble_phy_txpwr_set(int dbm);
+
+/* Get highest allowed power from range */
+int ble_phy_txpower_round(int dbm);
+
+/* Get the transmit power */
+int ble_phy_txpwr_get(void);
+
+/* Set RX path power compensation value rounded to integer dB */
+void ble_phy_set_rx_pwr_compensation(int8_t compensation);
+
+/* Disable the PHY */
+void ble_phy_disable(void);
+
+#define BLE_PHY_WFR_ENABLE_RX       (0)
+#define BLE_PHY_WFR_ENABLE_TXRX     (1)
+
+void ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs);
+
+/* Starts rf clock */
+void ble_phy_rfclk_enable(void);
+
+/* Stops rf clock */
+void ble_phy_rfclk_disable(void);
+
+/*
+ * Used to restart reception on same channel after wfr timer expiration or
+ * frame received.
+ */
+void ble_phy_restart_rx(void);
+
+/* Gets the current state of the PHY */
+int ble_phy_state_get(void);
+
+/* Gets current state of transceiver */
+uint8_t ble_phy_xcvr_state_get(void);
+
+/* Returns 'true' if a reception has started */
+int ble_phy_rx_started(void);
+
+/*
+ * Returns the maximum supported tx/rx PDU payload size, in bytes, for data
+ * channel PDUs (this does not apply to advertising channel PDUs). Note
+ * that the data channel PDU is composed of a 2-byte header, the payload, and
+ * an optional MIC. The maximum payload is 251 bytes.
+ */
+uint8_t ble_phy_max_data_pdu_pyld(void);
+
+/* Gets the current access address */
+uint32_t ble_phy_access_addr_get(void);
+
+/* Enable encryption */
+void ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+                            uint8_t is_master);
+
+/* Disable encryption */
+void ble_phy_encrypt_disable(void);
+
+/* Set the packet counters and dir used by LE encyption */
+void ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir);
+
+/* Enable phy resolving list */
+void ble_phy_resolv_list_enable(void);
+
+/* Disable phy resolving list */
+void ble_phy_resolv_list_disable(void);
+
+/*
+ * PHY mode values for 1M, 2M and Coded S=8 are the same as corresponding values
+ * of PHY. This makes conversion between 'phy' and 'phy_mode' easier and it also
+ * means that default coding for Coded will be S=8, unless explicitly translated
+ * to S=2.
+ */
+#define BLE_PHY_MODE_CODED_500KBPS  (0)
+#define BLE_PHY_MODE_1M             (1)
+#define BLE_PHY_MODE_2M             (2)
+#define BLE_PHY_MODE_CODED_125KBPS  (3)
+
+/* The number of different modes */
+#define BLE_PHY_NUM_MODE            (4)
+
+/* PHY numbers (compatible with HCI) */
+#define BLE_PHY_1M                  (BLE_HCI_LE_PHY_1M)
+#define BLE_PHY_2M                  (BLE_HCI_LE_PHY_2M)
+#define BLE_PHY_CODED               (BLE_HCI_LE_PHY_CODED)
+
+/* PHY bitmasks (compatible with HCI) */
+#define BLE_PHY_MASK_1M             (BLE_HCI_LE_PHY_1M_PREF_MASK)
+#define BLE_PHY_MASK_2M             (BLE_HCI_LE_PHY_2M_PREF_MASK)
+#define BLE_PHY_MASK_CODED          (BLE_HCI_LE_PHY_CODED_PREF_MASK)
+
+#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY))
+uint32_t ble_phy_mode_pdu_start_off(int phy);
+void ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode);
+#else
+#define ble_phy_mode_pdu_start_off(phy)     (40)
+
+#endif
+
+int ble_phy_get_cur_phy(void);
+static inline int ble_ll_phy_to_phy_mode(int phy, int phy_options)
+{
+    int phy_mode;
+
+    /*
+     * 'phy' value can be used as 'phy_mode' value unless S=2 coding is explicitly
+     * required. By default we'll use S=2 for Coded.
+     */
+    phy_mode = phy;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if (phy == BLE_PHY_CODED && phy_options == BLE_HCI_LE_PHY_CODED_S2_PREF) {
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+    }
+#endif
+
+    return phy_mode;
+}
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+void ble_phy_enable_dtm(void);
+void ble_phy_disable_dtm(void);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_PHY_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy_trace.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy_trace.h
new file mode 100644
index 00000000..64e55118
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy_trace.h
@@ -0,0 +1,96 @@
+/*
+ * 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_PHY_TRACE_
+#define H_BLE_PHY_TRACE_
+
+#include "os/os_trace_api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_PHY_TRACE_ID_START_TX       0
+#define BLE_PHY_TRACE_ID_START_RX       1
+#define BLE_PHY_TRACE_ID_DISABLE        2
+
+#if MYNEWT_VAL(BLE_PHY_SYSVIEW)
+
+extern uint32_t ble_phy_trace_off;
+
+void ble_phy_trace_init(void);
+
+static inline void
+ble_phy_trace_void(unsigned id)
+{
+    os_trace_api_void(ble_phy_trace_off + id);
+}
+
+static inline void
+ble_phy_trace_u32(unsigned id, uint32_t p1)
+{
+    os_trace_api_u32(ble_phy_trace_off + id, p1);
+}
+
+static inline void
+ble_phy_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2)
+{
+    os_trace_api_u32x2(ble_phy_trace_off + id, p1, p2);
+}
+
+static inline void
+ble_phy_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3)
+{
+    os_trace_api_u32x3(ble_phy_trace_off + id, p1, p2, p3);
+}
+
+#else
+
+static inline void
+ble_phy_trace_init(void)
+{
+}
+
+static inline void
+ble_phy_trace_void(unsigned id)
+{
+}
+
+static inline void
+ble_phy_trace_u32(unsigned id, uint32_t p1)
+{
+}
+
+static inline void
+ble_phy_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2)
+{
+}
+
+static inline void
+ble_phy_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3)
+{
+}
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_PHY_TRACE_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/pkg.yml b/src/libs/mynewt-nimble/nimble/controller/pkg.yml
new file mode 100644
index 00000000..96c63679
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/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/controller
+pkg.description: Controller 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.req_apis:
+    - ble_driver
+    - ble_transport
+    - stats
+
+pkg.deps:
+    - "@apache-mynewt-core/kernel/os"
+    - nimble
+
+pkg.init:
+    ble_ll_init: 'MYNEWT_VAL(BLE_LL_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll.c
new file mode 100644
index 00000000..996ad9c3
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll.c
@@ -0,0 +1,1714 @@
+/*
+ * 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 <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "os/os_cputime.h"
+#include "stats/stats.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_phy_trace.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "controller/ble_ll_trace.h"
+#include "controller/ble_ll_sync.h"
+#include "ble_ll_conn_priv.h"
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+#include "ble_ll_dtm_priv.h"
+#endif
+
+/* XXX:
+ *
+ * 1) use the sanity task!
+ * 2) Need to figure out what to do with packets that we hand up that did
+ * not pass the filter policy for the given state. Currently I count all
+ * packets I think. Need to figure out what to do with this.
+ * 3) For the features defined, we need to conditionally compile code.
+ * 4) Should look into always disabled the wfr interrupt if we receive the
+ * start of a frame. Need to look at the various states to see if this is the
+ * right thing to do.
+ */
+
+/* Supported states */
+#define BLE_LL_S_NCA                    (0x00000000001)
+#define BLE_LL_S_SA                     (0x00000000002)
+#define BLE_LL_S_CA                     (0x00000000004)
+#define BLE_LL_S_HDCA                   (0x00000000008)
+#define BLE_LL_S_PS                     (0x00000000010)
+#define BLE_LL_S_AS                     (0x00000000020)
+#define BLE_LL_S_INIT                   (0x00000000040)
+#define BLE_LL_S_SLAVE                  (0x00000000080)
+#define BLE_LL_S_NCA_PS                 (0x00000000100)
+#define BLE_LL_S_SA_PS                  (0x00000000200)
+#define BLE_LL_S_CA_PS                  (0x00000000400)
+#define BLE_LL_S_HDCA_PS                (0x00000000800)
+#define BLE_LL_S_NCA_AS                 (0x00000001000)
+#define BLE_LL_S_SA_AS                  (0x00000002000)
+#define BLE_LL_S_CA_AS                  (0x00000004000)
+#define BLE_LL_S_HDCA_AS                (0x00000008000)
+#define BLE_LL_S_NCA_INIT               (0x00000010000)
+#define BLE_LL_S_SA_INIT                (0x00000020000)
+#define BLE_LL_S_NCA_MASTER             (0x00000040000)
+#define BLE_LL_S_SA_MASTER              (0x00000080000)
+#define BLE_LL_S_NCA_SLAVE              (0x00000100000)
+#define BLE_LL_S_SA_SLAVE               (0x00000200000)
+#define BLE_LL_S_PS_INIT                (0x00000400000)
+#define BLE_LL_S_AS_INIT                (0x00000800000)
+#define BLE_LL_S_PS_MASTER              (0x00001000000)
+#define BLE_LL_S_AS_MASTER              (0x00002000000)
+#define BLE_LL_S_PS_SLAVE               (0x00004000000)
+#define BLE_LL_S_AS_SLAVE               (0x00008000000)
+#define BLE_LL_S_INIT_MASTER            (0x00010000000)
+#define BLE_LL_S_LDCA                   (0x00020000000)
+#define BLE_LL_S_LDCA_PS                (0x00040000000)
+#define BLE_LL_S_LDCA_AS                (0x00080000000)
+#define BLE_LL_S_CA_INIT                (0x00100000000)
+#define BLE_LL_S_HDCA_INIT              (0x00200000000)
+#define BLE_LL_S_LDCA_INIT              (0x00400000000)
+#define BLE_LL_S_CA_MASTER              (0x00800000000)
+#define BLE_LL_S_HDCA_MASTER            (0x01000000000)
+#define BLE_LL_S_LDCA_MASTER            (0x02000000000)
+#define BLE_LL_S_CA_SLAVE               (0x04000000000)
+#define BLE_LL_S_HDCA_SLAVE             (0x08000000000)
+#define BLE_LL_S_LDCA_SLAVE             (0x10000000000)
+#define BLE_LL_S_INIT_SLAVE             (0x20000000000)
+
+#define BLE_LL_SUPPORTED_STATES             \
+(                                           \
+    BLE_LL_S_NCA                    |       \
+    BLE_LL_S_SA                     |       \
+    BLE_LL_S_CA                     |       \
+    BLE_LL_S_HDCA                   |       \
+    BLE_LL_S_PS                     |       \
+    BLE_LL_S_AS                     |       \
+    BLE_LL_S_INIT                   |       \
+    BLE_LL_S_SLAVE                  |       \
+    BLE_LL_S_NCA_PS                 |       \
+    BLE_LL_S_SA_PS                  |       \
+    BLE_LL_S_CA_PS                  |       \
+    BLE_LL_S_HDCA_PS                |       \
+    BLE_LL_S_NCA_AS                 |       \
+    BLE_LL_S_SA_AS                  |       \
+    BLE_LL_S_CA_AS                  |       \
+    BLE_LL_S_HDCA_AS                |       \
+    BLE_LL_S_NCA_INIT               |       \
+    BLE_LL_S_SA_INIT                |       \
+    BLE_LL_S_NCA_MASTER             |       \
+    BLE_LL_S_SA_MASTER              |       \
+    BLE_LL_S_NCA_SLAVE              |       \
+    BLE_LL_S_SA_SLAVE               |       \
+    BLE_LL_S_PS_INIT                |       \
+    BLE_LL_S_AS_INIT                |       \
+    BLE_LL_S_PS_MASTER              |       \
+    BLE_LL_S_AS_MASTER              |       \
+    BLE_LL_S_PS_SLAVE               |       \
+    BLE_LL_S_AS_SLAVE               |       \
+    BLE_LL_S_INIT_MASTER            |       \
+    BLE_LL_S_LDCA                   |       \
+    BLE_LL_S_LDCA_PS                |       \
+    BLE_LL_S_LDCA_AS                |       \
+    BLE_LL_S_CA_INIT                |       \
+    BLE_LL_S_HDCA_INIT              |       \
+    BLE_LL_S_LDCA_INIT              |       \
+    BLE_LL_S_CA_MASTER              |       \
+    BLE_LL_S_HDCA_MASTER            |       \
+    BLE_LL_S_LDCA_MASTER            |       \
+    BLE_LL_S_CA_SLAVE               |       \
+    BLE_LL_S_HDCA_SLAVE             |       \
+    BLE_LL_S_LDCA_SLAVE             |       \
+    BLE_LL_S_INIT_SLAVE)
+
+/* The global BLE LL data object */
+struct ble_ll_obj g_ble_ll_data;
+
+/* Global link layer statistics */
+STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
+STATS_NAME_START(ble_ll_stats)
+    STATS_NAME(ble_ll_stats, hci_cmds)
+    STATS_NAME(ble_ll_stats, hci_cmd_errs)
+    STATS_NAME(ble_ll_stats, hci_events_sent)
+    STATS_NAME(ble_ll_stats, bad_ll_state)
+    STATS_NAME(ble_ll_stats, bad_acl_hdr)
+    STATS_NAME(ble_ll_stats, no_bufs)
+    STATS_NAME(ble_ll_stats, rx_adv_pdu_crc_ok)
+    STATS_NAME(ble_ll_stats, rx_adv_pdu_crc_err)
+    STATS_NAME(ble_ll_stats, rx_adv_bytes_crc_ok)
+    STATS_NAME(ble_ll_stats, rx_adv_bytes_crc_err)
+    STATS_NAME(ble_ll_stats, rx_data_pdu_crc_ok)
+    STATS_NAME(ble_ll_stats, rx_data_pdu_crc_err)
+    STATS_NAME(ble_ll_stats, rx_data_bytes_crc_ok)
+    STATS_NAME(ble_ll_stats, rx_data_bytes_crc_err)
+    STATS_NAME(ble_ll_stats, rx_adv_malformed_pkts)
+    STATS_NAME(ble_ll_stats, rx_adv_ind)
+    STATS_NAME(ble_ll_stats, rx_adv_direct_ind)
+    STATS_NAME(ble_ll_stats, rx_adv_nonconn_ind)
+    STATS_NAME(ble_ll_stats, rx_adv_ext_ind)
+    STATS_NAME(ble_ll_stats, rx_scan_reqs)
+    STATS_NAME(ble_ll_stats, rx_scan_rsps)
+    STATS_NAME(ble_ll_stats, rx_connect_reqs)
+    STATS_NAME(ble_ll_stats, rx_scan_ind)
+    STATS_NAME(ble_ll_stats, rx_aux_connect_rsp)
+    STATS_NAME(ble_ll_stats, adv_txg)
+    STATS_NAME(ble_ll_stats, adv_late_starts)
+    STATS_NAME(ble_ll_stats, adv_resched_pdu_fail)
+    STATS_NAME(ble_ll_stats, adv_drop_event)
+    STATS_NAME(ble_ll_stats, sched_state_conn_errs)
+    STATS_NAME(ble_ll_stats, sched_state_adv_errs)
+    STATS_NAME(ble_ll_stats, scan_starts)
+    STATS_NAME(ble_ll_stats, scan_stops)
+    STATS_NAME(ble_ll_stats, scan_req_txf)
+    STATS_NAME(ble_ll_stats, scan_req_txg)
+    STATS_NAME(ble_ll_stats, scan_rsp_txg)
+    STATS_NAME(ble_ll_stats, aux_missed_adv)
+    STATS_NAME(ble_ll_stats, aux_scheduled)
+    STATS_NAME(ble_ll_stats, aux_received)
+    STATS_NAME(ble_ll_stats, aux_fired_for_read)
+    STATS_NAME(ble_ll_stats, aux_allocated)
+    STATS_NAME(ble_ll_stats, aux_freed)
+    STATS_NAME(ble_ll_stats, aux_sched_cb)
+    STATS_NAME(ble_ll_stats, aux_conn_req_tx)
+    STATS_NAME(ble_ll_stats, aux_conn_rsp_tx)
+    STATS_NAME(ble_ll_stats, aux_conn_rsp_err)
+    STATS_NAME(ble_ll_stats, aux_scan_req_tx)
+    STATS_NAME(ble_ll_stats, aux_scan_rsp_err)
+    STATS_NAME(ble_ll_stats, aux_chain_cnt)
+    STATS_NAME(ble_ll_stats, aux_chain_err)
+    STATS_NAME(ble_ll_stats, aux_scan_drop)
+    STATS_NAME(ble_ll_stats, adv_evt_dropped)
+    STATS_NAME(ble_ll_stats, scan_timer_stopped)
+    STATS_NAME(ble_ll_stats, scan_timer_restarted)
+    STATS_NAME(ble_ll_stats, periodic_adv_drop_event)
+    STATS_NAME(ble_ll_stats, periodic_chain_drop_event)
+    STATS_NAME(ble_ll_stats, sync_event_failed)
+    STATS_NAME(ble_ll_stats, sync_received)
+    STATS_NAME(ble_ll_stats, sync_chain_failed)
+    STATS_NAME(ble_ll_stats, sync_missed_err)
+    STATS_NAME(ble_ll_stats, sync_crc_err)
+    STATS_NAME(ble_ll_stats, sync_rx_buf_err)
+    STATS_NAME(ble_ll_stats, sync_scheduled)
+    STATS_NAME(ble_ll_stats, sched_state_sync_errs)
+    STATS_NAME(ble_ll_stats, sched_invalid_pdu)
+STATS_NAME_END(ble_ll_stats)
+
+static void ble_ll_event_rx_pkt(struct ble_npl_event *ev);
+static void ble_ll_event_tx_pkt(struct ble_npl_event *ev);
+static void ble_ll_event_dbuf_overflow(struct ble_npl_event *ev);
+
+#if MYNEWT
+
+/* The BLE LL task data structure */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_LL_STACK_SIZE   (120)
+#else
+#define BLE_LL_STACK_SIZE   (90)
+#endif
+
+struct os_task g_ble_ll_task;
+
+OS_TASK_STACK_DEFINE(g_ble_ll_stack, BLE_LL_STACK_SIZE);
+
+#endif /* MYNEWT */
+
+/** Our global device address (public) */
+uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
+
+/** Our random address */
+uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
+
+/** Our supported features which can be controller by the host */
+uint64_t g_ble_ll_supported_host_features = 0;
+
+static const uint16_t g_ble_ll_pdu_header_tx_time[BLE_PHY_NUM_MODE] =
+{
+    [BLE_PHY_MODE_1M] =
+            (BLE_LL_PREAMBLE_LEN + BLE_LL_ACC_ADDR_LEN + BLE_LL_CRC_LEN +
+                    BLE_LL_PDU_HDR_LEN) << 3,
+    [BLE_PHY_MODE_2M] =
+            (BLE_LL_PREAMBLE_LEN * 2 + BLE_LL_ACC_ADDR_LEN + BLE_LL_CRC_LEN +
+                    BLE_LL_PDU_HDR_LEN) << 2,
+    /* For Coded PHY we have exact TX times provided by specification:
+     * - Preamble, Access Address, CI, TERM1 (always coded as S=8)
+     * - PDU, CRC, TERM2 (coded as S=2 or S=8)
+     * (Vol 6, Part B, 2.2).
+     */
+    [BLE_PHY_MODE_CODED_125KBPS] =
+            (80 + 256 + 16 + 24 + 8 * (BLE_LL_PDU_HDR_LEN * 8 + 24 + 3)),
+    [BLE_PHY_MODE_CODED_500KBPS] =
+            (80 + 256 + 16 + 24 + 2 * (BLE_LL_PDU_HDR_LEN * 8 + 24 + 3)),
+};
+
+/**
+ * Counts the number of advertising PDU's received, by type. For advertising
+ * PDU's that contain a destination address, we still count these packets even
+ * if they are not for us.
+ *
+ * @param pdu_type
+ */
+static void
+ble_ll_count_rx_adv_pdus(uint8_t pdu_type)
+{
+    /* Count received packet types  */
+    switch (pdu_type) {
+    case BLE_ADV_PDU_TYPE_ADV_EXT_IND:
+        STATS_INC(ble_ll_stats, rx_adv_ext_ind);
+        break;
+    case BLE_ADV_PDU_TYPE_ADV_IND:
+        STATS_INC(ble_ll_stats, rx_adv_ind);
+        break;
+    case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND:
+        STATS_INC(ble_ll_stats, rx_adv_direct_ind);
+        break;
+    case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND:
+        STATS_INC(ble_ll_stats, rx_adv_nonconn_ind);
+        break;
+    case BLE_ADV_PDU_TYPE_SCAN_REQ:
+        STATS_INC(ble_ll_stats, rx_scan_reqs);
+        break;
+    case BLE_ADV_PDU_TYPE_SCAN_RSP:
+        STATS_INC(ble_ll_stats, rx_scan_rsps);
+        break;
+    case BLE_ADV_PDU_TYPE_CONNECT_IND:
+        STATS_INC(ble_ll_stats, rx_connect_reqs);
+        break;
+    case BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP:
+        STATS_INC(ble_ll_stats, rx_aux_connect_rsp);
+        break;
+    case BLE_ADV_PDU_TYPE_ADV_SCAN_IND:
+        STATS_INC(ble_ll_stats, rx_scan_ind);
+        break;
+    default:
+        break;
+    }
+}
+
+struct os_mbuf *
+ble_ll_rxpdu_alloc(uint16_t len)
+{
+    struct os_mbuf *om_ret;
+    struct os_mbuf *om_next;
+    struct os_mbuf *om;
+    struct os_mbuf_pkthdr *pkthdr;
+    uint16_t databuf_len;
+    int rem_len;
+
+    /*
+     * Make sure that data in mbuf are word-aligned with and without packet
+     * header. This is essential for proper and quick copying of received PDUs
+     * into mbufs.
+     */
+    _Static_assert((offsetof(struct os_mbuf, om_data) & 3) == 0,
+                   "Unaligned om_data");
+    _Static_assert(((offsetof(struct os_mbuf, om_data) +
+                     sizeof(struct os_mbuf_pkthdr) +
+                     sizeof(struct ble_mbuf_hdr)) & 3) == 0,
+                   "Unaligned data trailing packet header");
+
+    om_ret = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
+    if (!om_ret) {
+        goto rxpdu_alloc_fail;
+    }
+
+    /* Set complete PDU length in packet header */
+    pkthdr = OS_MBUF_PKTHDR(om_ret);
+    pkthdr->omp_len = len;
+
+    rem_len = len;
+
+    /*
+     * Calculate length of data in memory block. We assume length is rounded
+     * down to word size so PHY can do word-size aligned data copy to mbufs
+     * (except for last one) and leave remainder unused.
+     *
+     * Note that there likely won't be any remainder here since all pools have
+     * block size aligned to word size anyway.
+     */
+    databuf_len = om_ret->om_omp->omp_databuf_len & ~3;
+
+    /*
+     * First mbuf can store less data due to packet header. Also we reserve one
+     * word for leading space to prepend header when necessary (like for data
+     * PDU before handing over to HCI)
+     */
+    om_ret->om_data += 4;
+    rem_len -= databuf_len - om_ret->om_pkthdr_len - 4;
+
+    /* Allocate and chain mbufs until there's enough space to store complete PDU */
+    om = om_ret;
+    while (rem_len > 0) {
+        om_next = os_msys_get(rem_len, 0);
+        if (!om_next) {
+            os_mbuf_free_chain(om_ret);
+            goto rxpdu_alloc_fail;
+        }
+
+        SLIST_NEXT(om, om_next) = om_next;
+        om = om_next;
+
+        rem_len -= databuf_len;
+    }
+
+    return om_ret;
+
+rxpdu_alloc_fail:
+    STATS_INC(ble_ll_stats, no_bufs);
+    return NULL;
+}
+
+int
+ble_ll_chk_txrx_octets(uint16_t octets)
+{
+    int rc;
+
+    if ((octets < BLE_LL_CONN_SUPP_BYTES_MIN) ||
+        (octets > BLE_LL_CONN_SUPP_BYTES_MAX)) {
+        rc = 0;
+    } else {
+        rc = 1;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_chk_txrx_time(uint16_t time)
+{
+    int rc;
+
+    if ((time < BLE_LL_CONN_SUPP_TIME_MIN) ||
+        (time > BLE_LL_CONN_SUPP_TIME_MAX)) {
+        rc = 0;
+    } else {
+        rc = 1;
+    }
+
+    return rc;
+}
+
+/**
+ * Checks to see if the address is a resolvable private address.
+ *
+ * NOTE: the addr_type parameter will be 0 if the address is public;
+ * any other value is random (all non-zero values).
+ *
+ * @param addr
+ * @param addr_type Public (zero) or Random (non-zero) address
+ *
+ * @return int
+ */
+int
+ble_ll_is_rpa(const uint8_t *addr, uint8_t addr_type)
+{
+    int rc;
+
+    if (addr_type && ((addr[5] & 0xc0) == 0x40)) {
+        rc = 1;
+    } else {
+        rc = 0;
+    }
+    return rc;
+}
+
+int
+ble_ll_addr_is_id(uint8_t *addr, uint8_t addr_type)
+{
+    return !addr_type || ((addr[5] & 0xc0) == 0xc0);
+}
+
+int
+ble_ll_addr_subtype(const uint8_t *addr, uint8_t addr_type)
+{
+    if (!addr_type) {
+        return BLE_LL_ADDR_SUBTYPE_IDENTITY;
+    }
+
+    switch (addr[5] >> 6) {
+    case 0:
+        return BLE_LL_ADDR_SUBTYPE_NRPA; /* NRPA */
+    case 1:
+        return BLE_LL_ADDR_SUBTYPE_RPA; /* RPA */
+    default:
+        return BLE_LL_ADDR_SUBTYPE_IDENTITY; /* static random */
+    }
+}
+
+int
+ble_ll_is_valid_public_addr(const uint8_t *addr)
+{
+    int i;
+
+    for (i = 0; i < BLE_DEV_ADDR_LEN; ++i) {
+        if (addr[i]) {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+/* Checks to see that the device is a valid random address */
+int
+ble_ll_is_valid_random_addr(const uint8_t *addr)
+{
+    int i;
+    int rc;
+    uint16_t sum;
+    uint8_t addr_type;
+
+    /* Make sure all bits are neither one nor zero */
+    sum = 0;
+    for (i = 0; i < (BLE_DEV_ADDR_LEN -1); ++i) {
+        sum += addr[i];
+    }
+    sum += addr[5] & 0x3f;
+
+    if ((sum == 0) || (sum == ((5*255) + 0x3f))) {
+        return 0;
+    }
+
+    /* Get the upper two bits of the address */
+    rc = 1;
+    addr_type = addr[5] & 0xc0;
+    if (addr_type == 0xc0) {
+        /* Static random address. No other checks needed */
+    } else if (addr_type == 0x40) {
+        /* Resolvable */
+        sum = addr[3] + addr[4] + (addr[5] & 0x3f);
+        if ((sum == 0) || (sum == (255 + 255 + 0x3f))) {
+            rc = 0;
+        }
+    } else if (addr_type == 0) {
+        /* non-resolvable. Cant be equal to public */
+        if (!memcmp(g_dev_addr, addr, BLE_DEV_ADDR_LEN)) {
+            rc = 0;
+        }
+    } else {
+        /* Invalid upper two bits */
+        rc = 0;
+    }
+
+    return rc;
+}
+int
+ble_ll_is_valid_own_addr_type(uint8_t own_addr_type, const uint8_t *random_addr)
+{
+    int rc;
+
+    switch (own_addr_type) {
+    case BLE_HCI_ADV_OWN_ADDR_PUBLIC:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_ADV_OWN_ADDR_PRIV_PUB:
+#endif
+        rc = ble_ll_is_valid_public_addr(g_dev_addr);
+        break;
+    case BLE_HCI_ADV_OWN_ADDR_RANDOM:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_ADV_OWN_ADDR_PRIV_RAND:
+#endif
+        rc = ble_ll_is_valid_random_addr(random_addr);
+        break;
+    default:
+        rc = 0;
+        break;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_set_public_addr(const uint8_t *addr)
+{
+    memcpy(g_dev_addr, addr, BLE_DEV_ADDR_LEN);
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Called from the HCI command parser when the set random address command
+ * is received.
+ *
+ * Context: Link Layer task (HCI command parser)
+ *
+ * @param addr Pointer to address
+ *
+ * @return int 0: success
+ */
+int
+ble_ll_set_random_addr(const uint8_t *cmdbuf, uint8_t len, bool hci_adv_ext)
+{
+    const struct ble_hci_le_set_rand_addr_cp *cmd = (const void *) cmdbuf;
+
+    if (len < sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If the Host issues this command when scanning or legacy advertising is
+     * enabled, the Controller shall return the error code Command Disallowed.
+     *
+     * Test specification extends this also to initiating.
+     */
+
+    if (g_ble_ll_conn_create_sm || ble_ll_scan_enabled() ||
+                                (!hci_adv_ext && ble_ll_adv_enabled())) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (!ble_ll_is_valid_random_addr(cmd->addr)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    memcpy(g_random_addr, cmd->addr, BLE_DEV_ADDR_LEN);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* For instance 0 we need same address if legacy advertising might be
+     * used. If extended advertising is in use than this command doesn't
+     * affect instance 0.
+     */
+    if (!hci_adv_ext)
+        ble_ll_adv_set_random_addr(cmd->addr, 0);
+#endif
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Checks to see if an address is our device address (either public or
+ * random)
+ *
+ * @param addr
+ * @param addr_type
+ *
+ * @return int 0: not our device address. 1: is our device address
+ */
+int
+ble_ll_is_our_devaddr(uint8_t *addr, int addr_type)
+{
+    int rc;
+    uint8_t *our_addr;
+
+    if (addr_type) {
+        our_addr = g_random_addr;
+    } else {
+        our_addr = g_dev_addr;
+    }
+
+    rc = 0;
+    if (!memcmp(our_addr, addr, BLE_DEV_ADDR_LEN)) {
+        rc = 1;
+    }
+
+    return rc;
+}
+
+/**
+ * Get identity address
+ *
+ * @param addr_type Random (1). Public(0)
+ *
+ * @return pointer to identity address of given type.
+ */
+uint8_t*
+ble_ll_get_our_devaddr(uint8_t addr_type)
+{
+    if (addr_type) {
+        return g_random_addr;
+    }
+
+    return g_dev_addr;
+}
+
+/**
+ * Wait for response timeout function
+ *
+ * Context: interrupt (ble scheduler)
+ *
+ * @param arg
+ */
+void
+ble_ll_wfr_timer_exp(void *arg)
+{
+    int rx_start;
+    uint8_t lls;
+
+    rx_start = ble_phy_rx_started();
+    lls = g_ble_ll_data.ll_state;
+
+    ble_ll_trace_u32x3(BLE_LL_TRACE_ID_WFR_EXP, lls, ble_phy_xcvr_state_get(),
+                       (uint32_t)rx_start);
+
+    /* If we have started a reception, there is nothing to do here */
+    if (!rx_start) {
+        switch (lls) {
+        case BLE_LL_STATE_ADV:
+            ble_ll_adv_wfr_timer_exp();
+            break;
+        case BLE_LL_STATE_CONNECTION:
+            ble_ll_conn_wfr_timer_exp();
+            break;
+        case BLE_LL_STATE_SCANNING:
+            ble_ll_scan_wfr_timer_exp();
+            break;
+        case BLE_LL_STATE_INITIATING:
+            ble_ll_conn_init_wfr_timer_exp();
+            break;
+#if MYNEWT_VAL(BLE_LL_DTM)
+        case BLE_LL_STATE_DTM:
+            ble_ll_dtm_wfr_timer_exp();
+            break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        case BLE_LL_STATE_SYNC:
+            ble_ll_sync_wfr_timer_exp();
+            break;
+#endif
+        default:
+            break;
+        }
+    }
+}
+
+/**
+ * ll tx pkt in proc
+ *
+ * Process ACL data packet input from host
+ *
+ * Context: Link layer task
+ *
+ */
+static void
+ble_ll_tx_pkt_in(void)
+{
+    uint16_t handle;
+    uint16_t length;
+    uint16_t pb;
+    struct os_mbuf_pkthdr *pkthdr;
+    struct os_mbuf *om;
+
+    /* Drain all packets off the queue */
+    while (STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q)) {
+        /* Get mbuf pointer from packet header pointer */
+        pkthdr = STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q);
+        om = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf));
+
+        /* Remove from queue */
+        STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_tx_pkt_q, omp_next);
+
+        /* Strip HCI ACL header to get handle and length */
+        handle = get_le16(om->om_data);
+        length = get_le16(om->om_data + 2);
+        os_mbuf_adj(om, sizeof(struct hci_data_hdr));
+
+        /* Do some basic error checking */
+        pb = handle & 0x3000;
+        if ((pkthdr->omp_len != length) || (pb > 0x1000) || (length == 0)) {
+            /* This is a bad ACL packet. Count a stat and free it */
+            STATS_INC(ble_ll_stats, bad_acl_hdr);
+            os_mbuf_free_chain(om);
+            continue;
+        }
+
+        /* Hand to connection state machine */
+        ble_ll_conn_tx_pkt_in(om, handle, length);
+    }
+}
+
+/**
+ * Count Link Layer statistics for received PDUs
+ *
+ * Context: Link layer task
+ *
+ * @param hdr
+ * @param len
+ */
+static void
+ble_ll_count_rx_stats(struct ble_mbuf_hdr *hdr, uint16_t len, uint8_t pdu_type)
+{
+    uint8_t crcok;
+    bool connection_data;
+
+    crcok = BLE_MBUF_HDR_CRC_OK(hdr);
+    connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_CONNECTION);
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+    /* Reuse connection stats for DTM */
+    if (!connection_data) {
+        connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_DTM);
+    }
+#endif
+
+    if (crcok) {
+        if (connection_data) {
+            STATS_INC(ble_ll_stats, rx_data_pdu_crc_ok);
+            STATS_INCN(ble_ll_stats, rx_data_bytes_crc_ok, len);
+        } else {
+            STATS_INC(ble_ll_stats, rx_adv_pdu_crc_ok);
+            STATS_INCN(ble_ll_stats, rx_adv_bytes_crc_ok, len);
+            ble_ll_count_rx_adv_pdus(pdu_type);
+        }
+    } else {
+        if (connection_data) {
+            STATS_INC(ble_ll_stats, rx_data_pdu_crc_err);
+            STATS_INCN(ble_ll_stats, rx_data_bytes_crc_err, len);
+        } else {
+            STATS_INC(ble_ll_stats, rx_adv_pdu_crc_err);
+            STATS_INCN(ble_ll_stats, rx_adv_bytes_crc_err, len);
+        }
+    }
+}
+
+/**
+ * ll rx pkt in
+ *
+ * Process received packet from PHY.
+ *
+ * Context: Link layer task
+ *
+ */
+static void
+ble_ll_rx_pkt_in(void)
+{
+    os_sr_t sr;
+    uint8_t pdu_type;
+    uint8_t *rxbuf;
+    struct os_mbuf_pkthdr *pkthdr;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf *m;
+
+    /* Drain all packets off the queue */
+    while (STAILQ_FIRST(&g_ble_ll_data.ll_rx_pkt_q)) {
+        /* Get mbuf pointer from packet header pointer */
+        pkthdr = STAILQ_FIRST(&g_ble_ll_data.ll_rx_pkt_q);
+        m = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf));
+
+        /* Remove from queue */
+        OS_ENTER_CRITICAL(sr);
+        STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_rx_pkt_q, omp_next);
+        OS_EXIT_CRITICAL(sr);
+
+        /* Note: pdu type wont get used unless this is an advertising pdu */
+        ble_hdr = BLE_MBUF_HDR_PTR(m);
+        rxbuf = m->om_data;
+        pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
+        ble_ll_count_rx_stats(ble_hdr, pkthdr->omp_len, pdu_type);
+
+        /* Process the data or advertising pdu */
+        /* Process the PDU */
+        switch (BLE_MBUF_HDR_RX_STATE(ble_hdr)) {
+        case BLE_LL_STATE_CONNECTION:
+            ble_ll_conn_rx_data_pdu(m, ble_hdr);
+            /* m is going to be free by function above */
+            m = NULL;
+            break;
+        case BLE_LL_STATE_ADV:
+            ble_ll_adv_rx_pkt_in(pdu_type, rxbuf, ble_hdr);
+            break;
+        case BLE_LL_STATE_SCANNING:
+            ble_ll_scan_rx_pkt_in(pdu_type, m, ble_hdr);
+            break;
+        case BLE_LL_STATE_INITIATING:
+            ble_ll_init_rx_pkt_in(pdu_type, rxbuf, ble_hdr);
+            break;
+#if MYNEWT_VAL(BLE_LL_DTM)
+        case BLE_LL_STATE_DTM:
+            ble_ll_dtm_rx_pkt_in(m, ble_hdr);
+            break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        case BLE_LL_STATE_SYNC:
+            ble_ll_sync_rx_pkt_in(m, ble_hdr);
+            break;
+#endif
+        default:
+            /* Any other state should never occur */
+            STATS_INC(ble_ll_stats, bad_ll_state);
+            break;
+        }
+        if (m) {
+            /* Free the packet buffer */
+            os_mbuf_free_chain(m);
+        }
+    }
+}
+
+/**
+ * Called to put a packet on the Link Layer receive packet queue.
+ *
+ * @param rxpdu Pointer to received PDU
+ */
+void
+ble_ll_rx_pdu_in(struct os_mbuf *rxpdu)
+{
+    struct os_mbuf_pkthdr *pkthdr;
+
+    pkthdr = OS_MBUF_PKTHDR(rxpdu);
+    STAILQ_INSERT_TAIL(&g_ble_ll_data.ll_rx_pkt_q, pkthdr, omp_next);
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_rx_pkt_ev);
+}
+
+/**
+ * Called to put a packet on the Link Layer transmit packet queue.
+ *
+ * @param txpdu Pointer to transmit packet
+ */
+void
+ble_ll_acl_data_in(struct os_mbuf *txpkt)
+{
+    os_sr_t sr;
+    struct os_mbuf_pkthdr *pkthdr;
+
+    pkthdr = OS_MBUF_PKTHDR(txpkt);
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&g_ble_ll_data.ll_tx_pkt_q, pkthdr, omp_next);
+    OS_EXIT_CRITICAL(sr);
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_tx_pkt_ev);
+}
+
+/**
+ * Called to post event to Link Layer when a data buffer overflow has
+ * occurred.
+ *
+ * Context: Interrupt
+ *
+ */
+void
+ble_ll_data_buffer_overflow(void)
+{
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_dbuf_overflow_ev);
+}
+
+/**
+ * Called when a HW error occurs.
+ *
+ * Context: Interrupt
+ */
+void
+ble_ll_hw_error(void)
+{
+    ble_npl_callout_reset(&g_ble_ll_data.ll_hw_err_timer, 0);
+}
+
+/**
+ * Called when the HW error timer expires.
+ *
+ * @param arg
+ */
+static void
+ble_ll_hw_err_timer_cb(struct ble_npl_event *ev)
+{
+    if (ble_ll_hci_ev_hw_err(BLE_HW_ERR_HCI_SYNC_LOSS)) {
+        /*
+         * Restart callout if failed to allocate event. Try to allocate an
+         * event every 50 milliseconds (or each OS tick if a tick is longer
+         * than 100 msecs).
+         */
+        ble_npl_callout_reset(&g_ble_ll_data.ll_hw_err_timer,
+                         ble_npl_time_ms_to_ticks32(50));
+    }
+}
+
+/**
+ * Called upon start of received PDU
+ *
+ * Context: Interrupt
+ *
+ * @param rxpdu
+ *        chan
+ *
+ * @return int
+ *   < 0: A frame we dont want to receive.
+ *   = 0: Continue to receive frame. Dont go from rx to tx
+ *   > 0: Continue to receive frame and go from rx to tx when done
+ */
+int
+ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr)
+{
+    int rc;
+    uint8_t pdu_type;
+
+    /* Advertising channel PDU */
+    pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
+
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_RX_START, g_ble_ll_data.ll_state,
+                       pdu_type);
+
+    switch (g_ble_ll_data.ll_state) {
+    case BLE_LL_STATE_CONNECTION:
+        rc = ble_ll_conn_rx_isr_start(rxhdr, ble_phy_access_addr_get());
+        break;
+    case BLE_LL_STATE_ADV:
+        rc = ble_ll_adv_rx_isr_start(pdu_type);
+        break;
+    case BLE_LL_STATE_INITIATING:
+        rc = ble_ll_init_rx_isr_start(pdu_type, rxhdr);
+        break;
+    case BLE_LL_STATE_SCANNING:
+        rc = ble_ll_scan_rx_isr_start(pdu_type, &rxhdr->rxinfo.flags);
+        break;
+#if MYNEWT_VAL(BLE_LL_DTM)
+    case BLE_LL_STATE_DTM:
+        rc = ble_ll_dtm_rx_isr_start(rxhdr, ble_phy_access_addr_get());
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    case BLE_LL_STATE_SYNC:
+        rc = ble_ll_sync_rx_isr_start(pdu_type, rxhdr);
+        break;
+#endif
+    default:
+        /* Should not be in this state! */
+        rc = -1;
+        STATS_INC(ble_ll_stats, bad_ll_state);
+        break;
+    }
+
+    return rc;
+}
+
+/**
+ * Called by the PHY when a receive packet has ended.
+ *
+ * NOTE: Called from interrupt context!
+ *
+ * @param rxbuf Pointer to received PDU data
+ *        rxhdr Pointer to BLE header of received mbuf
+ *
+ * @return int
+ *       < 0: Disable the phy after reception.
+ *      == 0: Success. Do not disable the PHY.
+ *       > 0: Do not disable PHY as that has already been done.
+ */
+int
+ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    int rc;
+    int badpkt;
+    uint8_t pdu_type;
+    uint8_t len;
+    uint8_t crcok;
+    struct os_mbuf *rxpdu;
+
+    /* Get CRC status from BLE header */
+    crcok = BLE_MBUF_HDR_CRC_OK(rxhdr);
+
+    /* Get advertising PDU type and length */
+    pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
+    len = rxbuf[1];
+
+    ble_ll_trace_u32x3(BLE_LL_TRACE_ID_RX_END, pdu_type, len,
+                       rxhdr->rxinfo.flags);
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+    if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_DTM) {
+        rc = ble_ll_dtm_rx_isr_end(rxbuf, rxhdr);
+        return rc;
+    }
+#endif
+
+    if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_CONNECTION) {
+        rc = ble_ll_conn_rx_isr_end(rxbuf, rxhdr);
+        return rc;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_SYNC) {
+        rc = ble_ll_sync_rx_isr_end(rxbuf, rxhdr);
+        return rc;
+    }
+#endif
+
+    /* If the CRC checks, make sure lengths check! */
+    badpkt = 0;
+    if (crcok) {
+        switch (pdu_type) {
+        case BLE_ADV_PDU_TYPE_SCAN_REQ:
+        case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND:
+            if (len != BLE_SCAN_REQ_LEN) {
+                badpkt = 1;
+            }
+            break;
+        case BLE_ADV_PDU_TYPE_SCAN_RSP:
+        case BLE_ADV_PDU_TYPE_ADV_IND:
+        case BLE_ADV_PDU_TYPE_ADV_SCAN_IND:
+        case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND:
+            if ((len < BLE_DEV_ADDR_LEN) || (len > BLE_ADV_SCAN_IND_MAX_LEN)) {
+                badpkt = 1;
+            }
+            break;
+        case BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP:
+            break;
+        case BLE_ADV_PDU_TYPE_ADV_EXT_IND:
+            break;
+        case BLE_ADV_PDU_TYPE_CONNECT_IND:
+            if (len != BLE_CONNECT_REQ_LEN) {
+                badpkt = 1;
+            }
+            break;
+        default:
+            badpkt = 1;
+            break;
+        }
+
+        /* If this is a malformed packet, just kill it here */
+        if (badpkt) {
+            STATS_INC(ble_ll_stats, rx_adv_malformed_pkts);
+        }
+    }
+
+    /* Hand packet to the appropriate state machine (if crc ok) */
+    rxpdu = NULL;
+    switch (BLE_MBUF_HDR_RX_STATE(rxhdr)) {
+    case BLE_LL_STATE_ADV:
+        if (!badpkt) {
+            rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN);
+            if (rxpdu) {
+                ble_phy_rxpdu_copy(rxbuf, rxpdu);
+            }
+        }
+        rc = ble_ll_adv_rx_isr_end(pdu_type, rxpdu, crcok);
+        break;
+    case BLE_LL_STATE_SCANNING:
+        if (!badpkt) {
+            rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN);
+            if (rxpdu) {
+                ble_phy_rxpdu_copy(rxbuf, rxpdu);
+            }
+        }
+        rc = ble_ll_scan_rx_isr_end(rxpdu, crcok);
+        break;
+    case BLE_LL_STATE_INITIATING:
+        rc = ble_ll_init_rx_isr_end(rxbuf, crcok, rxhdr);
+        break;
+    default:
+        rc = -1;
+        STATS_INC(ble_ll_stats, bad_ll_state);
+        break;
+    }
+
+    /* Hand packet up to higher layer (regardless of CRC failure) */
+    if (rxpdu) {
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
+    return rc;
+}
+
+uint8_t
+ble_ll_tx_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct os_mbuf *txpdu;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    txpdu = pducb_arg;
+    BLE_LL_ASSERT(txpdu);
+    ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+
+    os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, ble_hdr->txinfo.pyld_len,
+                     dptr);
+
+    *hdr_byte = ble_hdr->txinfo.hdr_byte;
+
+    return ble_hdr->txinfo.pyld_len;
+}
+
+uint8_t
+ble_ll_tx_flat_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct os_mbuf *txpdu;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    txpdu = pducb_arg;
+    BLE_LL_ASSERT(txpdu);
+    ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+
+    memcpy(dptr, txpdu->om_data, ble_hdr->txinfo.pyld_len);
+
+    *hdr_byte = ble_hdr->txinfo.hdr_byte;
+
+    return ble_hdr->txinfo.pyld_len;
+}
+
+static void
+ble_ll_event_rx_pkt(struct ble_npl_event *ev)
+{
+    ble_ll_rx_pkt_in();
+}
+
+static void
+ble_ll_event_tx_pkt(struct ble_npl_event *ev)
+{
+    ble_ll_tx_pkt_in();
+}
+
+static void
+ble_ll_event_dbuf_overflow(struct ble_npl_event *ev)
+{
+    ble_ll_hci_ev_databuf_overflow();
+}
+
+static void
+ble_ll_event_comp_pkts(struct ble_npl_event *ev)
+{
+    ble_ll_conn_num_comp_pkts_event_send(NULL);
+}
+
+/**
+ * Link Layer task.
+ *
+ * This is the task that runs the Link Layer.
+ *
+ * @param arg
+ */
+void
+ble_ll_task(void *arg)
+{
+    struct ble_npl_event *ev;
+
+    /* Init ble phy */
+    ble_phy_init();
+
+    /* Set output power to 1mW (0 dBm) */
+    ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+
+    /* Register callback for transport */
+    ble_hci_trans_cfg_ll(ble_ll_hci_cmd_rx, NULL, ble_ll_hci_acl_rx, NULL);
+
+    /* Tell the host that we are ready to receive packets */
+    ble_ll_hci_send_noop();
+
+    ble_ll_rand_start();
+
+    while (1) {
+        ev = ble_npl_eventq_get(&g_ble_ll_data.ll_evq, BLE_NPL_TIME_FOREVER);
+        assert(ev);
+        ble_npl_event_run(ev);
+    }
+}
+
+/**
+ * ble ll state set
+ *
+ * Called to set the current link layer state.
+ *
+ * Context: Interrupt and Link Layer task
+ *
+ * @param ll_state
+ */
+void
+ble_ll_state_set(uint8_t ll_state)
+{
+    g_ble_ll_data.ll_state = ll_state;
+}
+
+/**
+ * ble ll state get
+ *
+ * Called to get the current link layer state.
+ *
+ * Context: Link Layer task (can be called from interrupt context though).
+ *
+ * @return ll_state
+ */
+uint8_t
+ble_ll_state_get(void)
+{
+    return g_ble_ll_data.ll_state;
+}
+
+/**
+ * ble ll event send
+ *
+ * Send an event to the Link Layer task
+ *
+ * @param ev Event to add to the Link Layer event queue.
+ */
+void
+ble_ll_event_send(struct ble_npl_event *ev)
+{
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, ev);
+}
+
+/**
+ * Returns the features supported by the link layer
+ *
+ * @return uint8_t bitmask of supported features.
+ */
+uint64_t
+ble_ll_read_supp_states(void)
+{
+    return BLE_LL_SUPPORTED_STATES;
+}
+
+/**
+ * Returns the features supported by the link layer
+ *
+ * @return uint64_t bitmask of supported features.
+ */
+uint64_t
+ble_ll_read_supp_features(void)
+{
+    return g_ble_ll_data.ll_supp_features;
+}
+
+/**
+ * Sets the features controlled by the host.
+ *
+ * @return HCI command status
+ */
+int
+ble_ll_set_host_feat(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_host_feat_cp *cmd = (const void *) cmdbuf;
+    uint64_t mask;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (!SLIST_EMPTY(&g_ble_ll_conn_active_list)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if ((cmd->bit_num > 0x3F) || (cmd->val > 1)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    mask = (uint64_t)1 << (cmd->bit_num);
+    if (!(mask & BLE_LL_HOST_CONTROLLED_FEATURES)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (!(mask & g_ble_ll_supported_host_features)) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+
+    if (cmd->val == 0) {
+        g_ble_ll_data.ll_supp_features &= ~(mask);
+    } else {
+        g_ble_ll_data.ll_supp_features |= mask;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+/**
+ * Flush a link layer packet queue.
+ *
+ * @param pktq
+ */
+static void
+ble_ll_flush_pkt_queue(struct ble_ll_pkt_q *pktq)
+{
+    struct os_mbuf_pkthdr *pkthdr;
+    struct os_mbuf *om;
+
+    /* FLush all packets from Link layer queues */
+    while (STAILQ_FIRST(pktq)) {
+        /* Get mbuf pointer from packet header pointer */
+        pkthdr = STAILQ_FIRST(pktq);
+        om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+
+        /* Remove from queue and free the mbuf */
+        STAILQ_REMOVE_HEAD(pktq, omp_next);
+        os_mbuf_free_chain(om);
+    }
+}
+
+/**
+ * Called to initialize a mbuf used by the controller
+ *
+ * NOTE: this is only used when the mbuf is created by the controller;
+ * it should not be used for data packets (ACL data packets) that come from
+ * the host. This routine assumes that the entire pdu length can fit in
+ * one mbuf contiguously.
+ *
+ * @param m
+ * @param pdulen
+ * @param hdr
+ */
+void
+ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr)
+{
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* Set mbuf length and packet length */
+    m->om_len = pdulen;
+    OS_MBUF_PKTHDR(m)->omp_len = pdulen;
+
+    /* Set BLE transmit header */
+    ble_hdr = BLE_MBUF_HDR_PTR(m);
+    ble_hdr->txinfo.flags = 0;
+    ble_hdr->txinfo.offset = 0;
+    ble_hdr->txinfo.pyld_len = pdulen;
+    ble_hdr->txinfo.hdr_byte = hdr;
+}
+
+/**
+ * Called to reset the controller. This performs a "software reset" of the link
+ * layer; it does not perform a HW reset of the controller nor does it reset
+ * the HCI interface.
+ *
+ * Context: Link Layer task (HCI command)
+ *
+ * @return int The ble error code to place in the command complete event that
+ * is returned when this command is issued.
+ */
+int
+ble_ll_reset(void)
+{
+    int rc;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    ble_phy_disable();
+    ble_ll_sched_stop();
+    ble_ll_scan_reset();
+    ble_ll_rfmgmt_reset();
+    OS_EXIT_CRITICAL(sr);
+
+    /* Stop any advertising */
+    ble_ll_adv_reset();
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+    ble_ll_dtm_reset();
+#endif
+
+    /* Stop sync */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    ble_ll_sync_reset();
+#endif
+
+    /* FLush all packets from Link layer queues */
+    ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_tx_pkt_q);
+    ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_rx_pkt_q);
+
+    /* Reset LL stats */
+    STATS_RESET(ble_ll_stats);
+
+    /* Reset any preferred PHYs */
+    g_ble_ll_data.ll_pref_tx_phys = 0;
+    g_ble_ll_data.ll_pref_rx_phys = 0;
+
+    /* Reset connection module */
+    ble_ll_conn_module_reset();
+
+    /* All this does is re-initialize the event masks so call the hci init */
+    ble_ll_hci_init();
+
+    /* Reset scheduler */
+    ble_ll_sched_init();
+
+    /* Set state to standby */
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+
+    /* Reset our random address */
+    memset(g_random_addr, 0, BLE_DEV_ADDR_LEN);
+
+    /* Clear the whitelist */
+    ble_ll_whitelist_clear();
+
+    /* Reset resolving list */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_ll_resolv_list_reset();
+#endif
+
+    /* Re-initialize the PHY */
+    rc = ble_phy_init();
+
+    return rc;
+}
+
+static void
+ble_ll_seed_prng(void)
+{
+    uint32_t seed;
+    int i;
+
+    /* Seed random number generator with least significant bytes of device
+     * address.
+     */
+    seed = 0;
+    for (i = 0; i < 4; ++i) {
+        seed |= g_dev_addr[i];
+        seed <<= 8;
+    }
+    srand(seed);
+}
+
+uint32_t
+ble_ll_pdu_tx_time_get(uint16_t payload_len, int phy_mode)
+{
+    uint32_t usecs;
+
+#if (BLE_LL_BT5_PHY_SUPPORTED)
+    if (phy_mode == BLE_PHY_MODE_1M) {
+        /* 8 usecs per byte */
+        usecs = payload_len << 3;
+    } else if (phy_mode == BLE_PHY_MODE_2M) {
+        /* 4 usecs per byte */
+        usecs = payload_len << 2;
+    } else if (phy_mode == BLE_PHY_MODE_CODED_125KBPS) {
+        /* S=8 => 8 * 8 = 64 usecs per byte */
+        usecs = payload_len << 6;
+    } else if (phy_mode == BLE_PHY_MODE_CODED_500KBPS) {
+        /* S=2 => 2 * 8 = 16 usecs per byte */
+        usecs = payload_len << 4;
+    } else {
+        BLE_LL_ASSERT(0);
+    }
+
+    usecs += g_ble_ll_pdu_header_tx_time[phy_mode];
+#else
+    usecs = (((payload_len) + BLE_LL_PDU_HDR_LEN + BLE_LL_ACC_ADDR_LEN
+            + BLE_LL_PREAMBLE_LEN + BLE_LL_CRC_LEN) << 3);
+#endif
+
+    return usecs;
+}
+
+uint16_t
+ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode)
+{
+    uint32_t header_tx_time;
+    uint16_t octets = 0;
+
+    BLE_LL_ASSERT(phy_mode < BLE_PHY_NUM_MODE);
+
+    header_tx_time = g_ble_ll_pdu_header_tx_time[phy_mode];
+
+    /*
+     * Current conn max tx time can be too short to even send a packet header
+     * and this can happen if we changed connection form uncoded to coded phy.
+     * However, the lower bound for conn max tx time (all of them) depends on
+     * current phy (uncoded/coded) but it always allows to send at least 27
+     * bytes of payload thus we alwyas return at least 27 from here.
+     *
+     * Reference:
+     * Core v5.0, Vol 6, Part B, section 4.5.10
+     * see connEffectiveMaxTxTime and connEffectiveMaxRxTime definitions
+     */
+
+    if (usecs < header_tx_time) {
+        return 27;
+    }
+
+    usecs -= header_tx_time;
+
+    if (phy_mode == BLE_PHY_MODE_1M) {
+        /* 8 usecs per byte */
+        octets = usecs >> 3;
+    } else if (phy_mode == BLE_PHY_MODE_2M) {
+        /* 4 usecs per byte */
+        octets = usecs >> 2;
+    } else if (phy_mode == BLE_PHY_MODE_CODED_125KBPS) {
+        /* S=8 => 8 * 8 = 64 usecs per byte */
+        octets = usecs >> 6;
+    } else if (phy_mode == BLE_PHY_MODE_CODED_500KBPS) {
+        /* S=2 => 2 * 8 = 16 usecs per byte */
+        octets = usecs >> 4;
+    } else {
+        BLE_LL_ASSERT(0);
+    }
+
+    /* see comment at the beginning */
+    return max(27, octets);
+}
+
+static inline bool
+ble_ll_is_addr_empty(const uint8_t *addr)
+{
+    return memcmp(addr, BLE_ADDR_ANY, BLE_DEV_ADDR_LEN) == 0;
+}
+
+/**
+ * Initialize the Link Layer. Should be called only once
+ *
+ * @return int
+ */
+void
+ble_ll_init(void)
+{
+    int rc;
+    uint64_t features;
+    ble_addr_t addr;
+    struct ble_ll_obj *lldata;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    ble_ll_trace_init();
+    ble_phy_trace_init();
+
+    /* Set public device address if not already set */
+    if (ble_ll_is_addr_empty(g_dev_addr)) {
+        /* Use sycfg address if configured, otherwise try to read from HW */
+        if (!ble_ll_is_addr_empty(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR))) {
+            memcpy(g_dev_addr, MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR), BLE_DEV_ADDR_LEN);
+        } else {
+            rc = ble_hw_get_public_addr(&addr);
+            if (!rc) {
+                memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
+            }
+        }
+    }
+
+    ble_ll_rfmgmt_init();
+
+    /* Get pointer to global data object */
+    lldata = &g_ble_ll_data;
+
+    /* Set acl pkt size and number */
+    lldata->ll_num_acl_pkts = MYNEWT_VAL(BLE_ACL_BUF_COUNT);
+    lldata->ll_acl_pkt_size = MYNEWT_VAL(BLE_ACL_BUF_SIZE);
+
+    /* Initialize eventq */
+    ble_npl_eventq_init(&lldata->ll_evq);
+
+    /* Initialize the transmit (from host) and receive (from phy) queues */
+    STAILQ_INIT(&lldata->ll_tx_pkt_q);
+    STAILQ_INIT(&lldata->ll_rx_pkt_q);
+
+    /* Initialize transmit (from host) and receive packet (from phy) event */
+    ble_npl_event_init(&lldata->ll_rx_pkt_ev, ble_ll_event_rx_pkt, NULL);
+    ble_npl_event_init(&lldata->ll_tx_pkt_ev, ble_ll_event_tx_pkt, NULL);
+
+    /* Initialize data buffer overflow event and completed packets */
+    ble_npl_event_init(&lldata->ll_dbuf_overflow_ev, ble_ll_event_dbuf_overflow, NULL);
+    ble_npl_event_init(&lldata->ll_comp_pkt_ev, ble_ll_event_comp_pkts, NULL);
+
+    /* Initialize the HW error timer */
+    ble_npl_callout_init(&g_ble_ll_data.ll_hw_err_timer,
+                         &g_ble_ll_data.ll_evq,
+                         ble_ll_hw_err_timer_cb,
+                         NULL);
+
+    /* Initialize LL HCI */
+    ble_ll_hci_init();
+
+    /* Init the scheduler */
+    ble_ll_sched_init();
+
+    /* Initialize advertiser */
+    ble_ll_adv_init();
+
+    /* Initialize a scanner */
+    ble_ll_scan_init();
+
+    /* Initialize the connection module */
+    ble_ll_conn_module_init();
+
+    /* Set the supported features. NOTE: we always support extended reject. */
+    features = BLE_LL_FEAT_EXTENDED_REJ;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+    features |= BLE_LL_FEAT_DATA_LEN_EXT;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_CONN_PARAM_REQ)
+    features |= BLE_LL_FEAT_CONN_PARM_REQ;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_SLAVE_INIT_FEAT_XCHG)
+    features |= BLE_LL_FEAT_SLAVE_INIT;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    features |= BLE_LL_FEAT_LE_ENCRYPTION;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    features |= (BLE_LL_FEAT_LL_PRIVACY | BLE_LL_FEAT_EXT_SCAN_FILT);
+    ble_ll_resolv_init();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+    features |= BLE_LL_FEAT_LE_PING;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    features |= BLE_LL_FEAT_EXT_ADV;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    /* CSA2 */
+    features |= BLE_LL_FEAT_CSA2;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    features |= BLE_LL_FEAT_LE_2M_PHY;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    features |= BLE_LL_FEAT_LE_CODED_PHY;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    features |= BLE_LL_FEAT_PERIODIC_ADV;
+    ble_ll_sync_init();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    features |= BLE_LL_FEAT_SYNC_TRANS_RECV;
+    features |= BLE_LL_FEAT_SYNC_TRANS_SEND;
+#endif
+
+    /* Initialize random number generation */
+    ble_ll_rand_init();
+
+    /* XXX: This really doesn't belong here, as the address probably has not
+     * been set yet.
+     */
+    ble_ll_seed_prng();
+
+    lldata->ll_supp_features = features;
+
+    rc = stats_init_and_reg(STATS_HDR(ble_ll_stats),
+                            STATS_SIZE_INIT_PARMS(ble_ll_stats, STATS_SIZE_32),
+                            STATS_NAME_INIT_PARMS(ble_ll_stats),
+                            "ble_ll");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+    ble_ll_dtm_init();
+#endif
+
+#if MYNEWT
+    /* Initialize the LL task */
+    os_task_init(&g_ble_ll_task, "ble_ll", ble_ll_task, NULL,
+                 MYNEWT_VAL(BLE_LL_PRIO), OS_WAIT_FOREVER, g_ble_ll_stack,
+                 BLE_LL_STACK_SIZE);
+#else
+
+/*
+ * For non-Mynewt OS it is required that OS creates task for LL and run LL
+ * routine which is wrapped by nimble_port_ll_task_func().
+ */
+
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_adv.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_adv.c
new file mode 100644
index 00000000..8b661f8c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_adv.c
@@ -0,0 +1,5136 @@
+/*
+ * 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 <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "os/os_cputime.h"
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_trace.h"
+#include "controller/ble_ll_utils.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "ble_ll_conn_priv.h"
+
+/* XXX: TODO
+ * 1) Need to look at advertising and scan request PDUs. Do I allocate these
+ * once? Do I use a different pool for smaller ones? Do I statically declare
+ * them?
+ * 3) How do features get supported? What happens if device does not support
+ * advertising? (for example)
+ * 4) How to determine the advertising interval we will actually use. As of
+ * now, we set it to max.
+ */
+
+/* Scheduling data for secondary channel */
+struct ble_ll_adv_aux {
+    struct ble_ll_sched_item sch;
+    uint32_t start_time;
+    uint16_t aux_data_offset;
+    uint8_t chan;
+    uint8_t ext_hdr;
+    uint8_t aux_data_len;
+    uint8_t payload_len;
+};
+
+/* Scheduling data for sync PDUs */
+struct ble_ll_adv_sync {
+    struct ble_ll_sched_item sch;
+    uint32_t start_time;
+    uint16_t sync_data_offset;
+    uint8_t chan;
+    uint8_t ext_hdr;
+    uint8_t sync_data_len;
+    uint8_t payload_len;
+};
+
+/*
+ * Advertising state machine
+ *
+ * The advertising state machine data structure.
+ *
+ *  adv_pdu_len
+ *      The length of the advertising PDU that will be sent. This does not
+ *      include the preamble, access address and CRC.
+ *
+ *  initiator_addr:
+ *      This is the address that we send in directed advertisements (the
+ *      INITA field). If we are using Privacy this is a RPA that we need to
+ *      generate. We reserve space in the advsm to save time when creating
+ *      the ADV_DIRECT_IND. If own address type is not 2 or 3, this is simply
+ *      the peer address from the set advertising parameters.
+ */
+struct ble_ll_adv_sm
+{
+    uint8_t adv_enabled;
+    uint8_t adv_instance;
+    uint8_t adv_chanmask;
+    uint8_t adv_filter_policy;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t adv_chan;
+    uint8_t adv_pdu_len;
+    int8_t adv_rpa_index;
+    int8_t adv_txpwr;
+    uint16_t flags;
+    uint16_t props;
+    uint16_t adv_itvl_min;
+    uint16_t adv_itvl_max;
+    uint32_t adv_itvl_usecs;
+    uint32_t adv_event_start_time;
+    uint32_t adv_pdu_start_time;
+    uint32_t adv_end_time;
+    uint32_t adv_rpa_timer;
+    uint8_t adva[BLE_DEV_ADDR_LEN];
+    uint8_t adv_rpa[BLE_DEV_ADDR_LEN];
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+    uint8_t initiator_addr[BLE_DEV_ADDR_LEN];
+    struct os_mbuf *adv_data;
+    struct os_mbuf *new_adv_data;
+    struct os_mbuf *scan_rsp_data;
+    struct os_mbuf *new_scan_rsp_data;
+    uint8_t *conn_comp_ev;
+    struct ble_npl_event adv_txdone_ev;
+    struct ble_ll_sched_item adv_sch;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    uint16_t channel_id;
+    uint16_t event_cntr;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    uint8_t aux_active : 1;
+    uint8_t aux_index : 1;
+    uint8_t aux_first_pdu : 1;
+    uint8_t aux_not_scanned : 1;
+    uint8_t aux_dropped : 1;
+    struct ble_mbuf_hdr *rx_ble_hdr;
+    struct os_mbuf **aux_data;
+    struct ble_ll_adv_aux aux[2];
+    struct ble_npl_event adv_sec_txdone_ev;
+    uint16_t duration;
+    uint16_t adi;
+    uint8_t adv_random_addr[BLE_DEV_ADDR_LEN];
+    uint8_t events_max;
+    uint8_t events;
+    uint8_t pri_phy;
+    uint8_t sec_phy;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    struct os_mbuf *periodic_adv_data;
+    struct os_mbuf *periodic_new_data;
+    uint32_t periodic_crcinit; /* only 3 bytes are used */
+    uint32_t periodic_access_addr;
+    uint16_t periodic_adv_itvl_min;
+    uint16_t periodic_adv_itvl_max;
+    uint16_t periodic_adv_props;
+    uint16_t periodic_channel_id;
+    uint16_t periodic_event_cntr;
+    uint16_t periodic_chain_event_cntr;
+    uint8_t periodic_adv_enabled : 1;
+    uint8_t periodic_adv_active : 1;
+    uint8_t periodic_sync_active : 1;
+    uint8_t periodic_sync_index : 1;
+    uint8_t periodic_num_used_chans;
+    uint8_t periodic_chanmap[BLE_LL_CONN_CHMAP_LEN];
+    uint32_t periodic_adv_itvl_ticks;
+    uint8_t periodic_adv_itvl_rem_usec;
+    uint8_t periodic_adv_event_start_time_remainder;
+    uint32_t periodic_adv_event_start_time;
+    struct ble_ll_adv_sync periodic_sync[2];
+    struct ble_npl_event adv_periodic_txdone_ev;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    uint16_t periodic_event_cntr_last_sent;
+#endif
+#endif
+#endif
+};
+
+#define BLE_LL_ADV_SM_FLAG_TX_ADD                   0x0001
+#define BLE_LL_ADV_SM_FLAG_RX_ADD                   0x0002
+#define BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF           0x0004
+#define BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD             0x0008
+#define BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK      0x0030 /* use helpers! */
+#define BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE      0x0040
+#define BLE_LL_ADV_SM_FLAG_CONFIGURED               0x0080
+#define BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO              0x0100
+#define BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA             0x0200
+#define BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA        0x0400
+#define BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED      0x0800
+#define BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE 0x1000
+#define BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING    0x2000
+#define BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA        0x4000
+
+#define ADV_DATA_LEN(_advsm) \
+                ((_advsm->adv_data) ? OS_MBUF_PKTLEN(advsm->adv_data) : 0)
+#define SCAN_RSP_DATA_LEN(_advsm) \
+                ((_advsm->scan_rsp_data) ? OS_MBUF_PKTLEN(advsm->scan_rsp_data) : 0)
+#define AUX_DATA_LEN(_advsm) \
+                (*(_advsm->aux_data) ? OS_MBUF_PKTLEN(*advsm->aux_data) : 0)
+
+#define AUX_CURRENT(_advsm)     (&(_advsm->aux[_advsm->aux_index]))
+#define AUX_NEXT(_advsm)        (&(_advsm->aux[_advsm->aux_index ^ 1]))
+
+#define SYNC_CURRENT(_advsm) (&(_advsm->periodic_sync[_advsm->periodic_sync_index]))
+#define SYNC_NEXT(_advsm)    (&(_advsm->periodic_sync[_advsm->periodic_sync_index ^ 1]))
+#define SYNC_DATA_LEN(_advsm) \
+                (_advsm->periodic_adv_data ? OS_MBUF_PKTLEN(advsm->periodic_adv_data) : 0)
+
+/* The advertising state machine global object */
+struct ble_ll_adv_sm g_ble_ll_adv_sm[BLE_ADV_INSTANCES];
+struct ble_ll_adv_sm *g_ble_ll_cur_adv_sm;
+
+static struct ble_ll_adv_sm *
+ble_ll_adv_sm_find_configured(uint8_t instance)
+{
+    struct ble_ll_adv_sm *advsm;
+    int i;
+
+    /* in legacy mode we only allow instance 0 */
+    if (!ble_ll_hci_adv_mode_ext()) {
+        BLE_LL_ASSERT(instance == 0);
+        return &g_ble_ll_adv_sm[0];
+    }
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_adv_sm); i++) {
+        advsm = &g_ble_ll_adv_sm[i];
+
+        if ((advsm->flags & BLE_LL_ADV_SM_FLAG_CONFIGURED) &&
+                (advsm->adv_instance == instance)) {
+            return advsm;
+        }
+    }
+
+    return NULL;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static int
+ble_ll_adv_active_chanset_is_pri(struct ble_ll_adv_sm *advsm)
+{
+    return (advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0x10;
+}
+
+static int
+ble_ll_adv_active_chanset_is_sec(struct ble_ll_adv_sm *advsm)
+{
+    return (advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0x20;
+}
+#endif
+
+static void
+ble_ll_adv_active_chanset_clear(struct ble_ll_adv_sm *advsm)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    advsm->flags &= ~BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK;
+    OS_EXIT_CRITICAL(sr);
+}
+
+static void
+ble_ll_adv_active_chanset_set_pri(struct ble_ll_adv_sm *advsm)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    assert((advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0);
+    advsm->flags &= ~BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK;
+    advsm->flags |= 0x10;
+    OS_EXIT_CRITICAL(sr);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_adv_active_chanset_set_sec(struct ble_ll_adv_sm *advsm)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    assert((advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0);
+    advsm->flags &= ~BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK;
+    advsm->flags |= 0x20;
+    OS_EXIT_CRITICAL(sr);
+}
+#endif
+
+static void
+ble_ll_adv_flags_set(struct ble_ll_adv_sm *advsm, uint16_t flags)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    advsm->flags |= flags;
+    OS_EXIT_CRITICAL(sr);
+}
+
+static void
+ble_ll_adv_flags_clear(struct ble_ll_adv_sm *advsm, uint16_t flags)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    advsm->flags &= ~flags;
+    OS_EXIT_CRITICAL(sr);
+}
+
+static void ble_ll_adv_make_done(struct ble_ll_adv_sm *advsm, struct ble_mbuf_hdr *hdr);
+static void ble_ll_adv_sm_init(struct ble_ll_adv_sm *advsm);
+static void ble_ll_adv_sm_stop_timeout(struct ble_ll_adv_sm *advsm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+static void
+ble_ll_adv_rpa_update(struct ble_ll_adv_sm *advsm)
+{
+    if (ble_ll_resolv_gen_rpa(advsm->peer_addr, advsm->peer_addr_type,
+                          advsm->adva, 1)) {
+        ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD);
+    } else {
+        if (advsm->own_addr_type & 1) {
+            ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD);
+        } else {
+            ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD);
+        }
+    }
+
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        if (ble_ll_resolv_gen_rpa(advsm->peer_addr, advsm->peer_addr_type,
+                              advsm->initiator_addr, 0)) {
+            ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_RX_ADD);
+        } else {
+            if (advsm->peer_addr_type & 1) {
+                ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_RX_ADD);
+            } else {
+                ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_RX_ADD);
+            }
+        }
+    }
+}
+
+/**
+ * Called to change advertisers ADVA and INITA (for directed advertisements)
+ * as an advertiser needs to adhere to the resolvable private address generation
+ * timer.
+ *
+ * NOTE: the resolvable private address code uses its own timer to regenerate
+ * local resolvable private addresses. The advertising code uses its own
+ * timer to reset the INITA (for directed advertisements). This code also sets
+ * the appropriate txadd and rxadd bits that will go into the advertisement.
+ *
+ * Another thing to note: it is possible that an IRK is all zeroes in the
+ * resolving list. That is why we need to check if the generated address is
+ * in fact a RPA as a resolving list entry with all zeroes will use the
+ * identity address (which may be a private address or public).
+ *
+ * @param advsm
+ */
+void
+ble_ll_adv_chk_rpa_timeout(struct ble_ll_adv_sm *advsm)
+{
+    if (advsm->own_addr_type < BLE_HCI_ADV_OWN_ADDR_PRIV_PUB) {
+        return;
+    }
+
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO) {
+        ble_ll_adv_rpa_update(advsm);
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO);
+    }
+}
+
+void
+ble_ll_adv_rpa_timeout(void)
+{
+    struct ble_ll_adv_sm *advsm;
+    int i;
+
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        advsm = &g_ble_ll_adv_sm[i];
+
+        if (advsm->adv_enabled &&
+                advsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+            /* Mark RPA as timed out so we get a new RPA */
+            ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO);
+        }
+    }
+}
+#endif
+
+/**
+ * Calculate the first channel that we should advertise upon when we start
+ * an advertising event.
+ *
+ * @param advsm
+ *
+ * @return uint8_t The number of the first channel usable for advertising.
+ */
+static uint8_t
+ble_ll_adv_first_chan(struct ble_ll_adv_sm *advsm)
+{
+    uint8_t adv_chan;
+
+    /* Set first advertising channel */
+    if (advsm->adv_chanmask & 0x01) {
+        adv_chan = BLE_PHY_ADV_CHAN_START;
+    } else if (advsm->adv_chanmask & 0x02) {
+        adv_chan = BLE_PHY_ADV_CHAN_START + 1;
+    } else {
+        adv_chan = BLE_PHY_ADV_CHAN_START + 2;
+    }
+
+    return adv_chan;
+}
+
+/**
+ * Calculate the final channel that we should advertise upon when we start
+ * an advertising event.
+ *
+ * @param advsm
+ *
+ * @return uint8_t The number of the final channel usable for advertising.
+ */
+static uint8_t
+ble_ll_adv_final_chan(struct ble_ll_adv_sm *advsm)
+{
+    uint8_t adv_chan;
+
+    if (advsm->adv_chanmask & 0x04) {
+        adv_chan = BLE_PHY_ADV_CHAN_START + 2;
+    } else if (advsm->adv_chanmask & 0x02) {
+        adv_chan = BLE_PHY_ADV_CHAN_START + 1;
+    } else {
+        adv_chan = BLE_PHY_ADV_CHAN_START;
+    }
+
+    return adv_chan;
+}
+
+/**
+ * Create the advertising legacy PDU
+ *
+ * @param advsm Pointer to advertisement state machine
+ */
+static uint8_t
+ble_ll_adv_legacy_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    uint8_t     adv_data_len;
+    uint8_t     pdulen;
+    uint8_t     pdu_type;
+
+    advsm = pducb_arg;
+
+    /* assume this is not a direct ind */
+    adv_data_len = ADV_DATA_LEN(advsm);
+    pdulen = BLE_DEV_ADDR_LEN + adv_data_len;
+
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        pdu_type = BLE_ADV_PDU_TYPE_ADV_DIRECT_IND;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+        pdu_type |= BLE_ADV_PDU_HDR_CHSEL;
+#endif
+
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_RX_ADD) {
+            pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND;
+        }
+
+        adv_data_len = 0;
+        pdulen = BLE_ADV_DIRECT_IND_LEN;
+    } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        pdu_type = BLE_ADV_PDU_TYPE_ADV_IND;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+        pdu_type |= BLE_ADV_PDU_HDR_CHSEL;
+#endif
+    } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+        pdu_type = BLE_ADV_PDU_TYPE_ADV_SCAN_IND;
+    } else {
+        pdu_type = BLE_ADV_PDU_TYPE_ADV_NONCONN_IND;
+    }
+
+    /* An invalid advertising data length indicates a memory overwrite */
+    assert(adv_data_len <= BLE_ADV_LEGACY_DATA_MAX_LEN);
+
+    /* Set the PDU length in the state machine (includes header) */
+    advsm->adv_pdu_len = pdulen + BLE_LL_PDU_HDR_LEN;
+
+    /* Set TxAdd to random if needed. */
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+        pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
+    }
+
+    *hdr_byte = pdu_type;
+
+    /* Construct advertisement */
+    memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
+    dptr += BLE_DEV_ADDR_LEN;
+
+    /* For ADV_DIRECT_IND add inita */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        memcpy(dptr, advsm->initiator_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    /* Copy in advertising data, if any */
+    if (adv_data_len != 0) {
+        os_mbuf_copydata(advsm->adv_data, 0, adv_data_len, dptr);
+    }
+
+    return pdulen;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_adv_put_aux_ptr(uint8_t chan, uint8_t phy, uint32_t offset,
+                       uint8_t *dptr)
+{
+    dptr[0] = chan;
+
+    if (offset > 245700) {
+        dptr[0] |= 0x80;
+        offset = offset / 300;
+    } else {
+        offset = offset / 30;
+    }
+
+    if (offset > 0x1fff) {
+        offset = 0;
+    }
+
+    /* offset is 13bits and PHY 3 bits */
+    dptr[1] = (offset & 0x000000ff);
+    dptr[2] = ((offset >> 8) & 0x0000001f) | (phy - 1) << 5;
+}
+
+/**
+ * Create the advertising PDU
+ */
+static uint8_t
+ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    uint8_t pdu_type;
+    uint8_t adv_mode;
+    uint8_t ext_hdr_len;
+    uint8_t ext_hdr_flags;
+    uint32_t offset;
+
+    advsm = pducb_arg;
+
+    assert(ble_ll_adv_active_chanset_is_pri(advsm));
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        return ble_ll_adv_legacy_pdu_make(dptr, advsm, hdr_byte);
+    }
+
+    /* only ADV_EXT_IND goes on primary advertising channels */
+    pdu_type = BLE_ADV_PDU_TYPE_ADV_EXT_IND;
+
+    /* Set TxAdd to random if needed. */
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+        pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
+    }
+
+    *hdr_byte = pdu_type;
+
+    adv_mode = 0;
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        adv_mode |= BLE_LL_EXT_ADV_MODE_CONN;
+    }
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+        adv_mode |= BLE_LL_EXT_ADV_MODE_SCAN;
+    }
+
+    ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_DATA_INFO_SIZE +
+                  BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    ext_hdr_flags = (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT) |
+                    (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT);
+
+    /* ext hdr len and adv mode */
+    dptr[0] = ext_hdr_len | (adv_mode << 6);
+    dptr += 1;
+
+    /* ext hdr flags */
+    dptr[0] = ext_hdr_flags;
+    dptr += 1;
+
+    /* ADI */
+    dptr[0] = advsm->adi & 0x00ff;
+    dptr[1] = advsm->adi >> 8;
+    dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    /* AuxPtr */
+    if (AUX_CURRENT(advsm)->sch.enqueued) {
+        offset = os_cputime_ticks_to_usecs(AUX_CURRENT(advsm)->start_time - advsm->adv_pdu_start_time);
+    } else {
+        offset = 0;
+    }
+    /* Always use channel from 1st AUX */
+    ble_ll_adv_put_aux_ptr(AUX_CURRENT(advsm)->chan, advsm->sec_phy,
+                           offset, dptr);
+
+    return BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+static void
+ble_ll_adv_put_syncinfo(struct ble_ll_adv_sm *advsm,
+                        struct ble_ll_conn_sm *connsm, uint8_t *conn_event_cnt,
+                        uint8_t *dptr)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    uint8_t anchor_usecs;
+    uint16_t conn_cnt;
+#endif
+    unsigned int event_cnt_off = 0;
+    uint32_t offset = 0;
+    uint32_t anchor;
+    uint8_t units;
+
+    if (connsm) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+        anchor = connsm->anchor_point;
+        anchor_usecs = connsm->anchor_point_usecs;
+        conn_cnt = connsm->event_cntr;
+
+        /* get anchor for conn event that is before periodic_adv_event_start_time */
+        while (CPUTIME_GT(anchor, advsm->periodic_adv_event_start_time)) {
+            ble_ll_conn_get_anchor(connsm, --conn_cnt, &anchor, &anchor_usecs);
+        }
+
+        offset = os_cputime_ticks_to_usecs(advsm->periodic_adv_event_start_time - anchor);
+        offset -= anchor_usecs;
+        offset += advsm->periodic_adv_event_start_time_remainder;
+
+        /* connEventCount */
+        put_le16(conn_event_cnt, conn_cnt);
+#endif
+    } else {
+        anchor = advsm->periodic_adv_event_start_time;
+
+        /* Get periodic event that is past AUX start time (so that we always
+         * provide valid offset if it is not too far in future). This can
+         * happen if advertising event is interleaved with periodic advertising
+         * event (when chaining).
+         */
+        while (CPUTIME_GT(AUX_CURRENT(advsm)->start_time, anchor)) {
+            anchor += advsm->periodic_adv_itvl_ticks;
+            event_cnt_off++;
+        }
+
+        offset = os_cputime_ticks_to_usecs(anchor - AUX_CURRENT(advsm)->start_time);
+        offset += advsm->periodic_adv_event_start_time_remainder;
+        offset += advsm->periodic_adv_itvl_rem_usec;
+    }
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit),
+     * RFU (1 bit)
+     */
+    if (offset > 245700) {
+        units = 0x20;
+        offset = offset / 300;
+
+        if (offset >= 0x2000) {
+            if (connsm) {
+                offset -= 0x2000;
+                units |= 0x40;
+            } else {
+                /* not able to represent time in offset */
+                offset = 0;
+                units = 0x00;
+                event_cnt_off = 0;
+            }
+        }
+
+    } else {
+        units = 0x00;
+        offset = offset / 30;
+    }
+
+    dptr[0] = (offset & 0x000000ff);
+    dptr[1] = ((offset >> 8) & 0x0000001f) | units;
+
+    /* Interval (2 bytes) */
+    put_le16(&dptr[2], advsm->periodic_adv_itvl_max);
+
+    /* Channels Mask (37 bits) */
+    dptr[4] = advsm->periodic_chanmap[0];
+    dptr[5] = advsm->periodic_chanmap[1];
+    dptr[6] = advsm->periodic_chanmap[2];
+    dptr[7] = advsm->periodic_chanmap[3];
+    dptr[8] = advsm->periodic_chanmap[4] & 0x1f;
+
+    /* SCA (3 bits) */
+    dptr[8] |= MYNEWT_VAL(BLE_LL_MASTER_SCA) << 5;
+
+    /* AA (4 bytes) */
+    put_le32(&dptr[9], advsm->periodic_access_addr);
+
+    /* CRCInit (3 bytes) */
+    dptr[13] = (uint8_t)advsm->periodic_crcinit;
+    dptr[14] = (uint8_t)(advsm->periodic_crcinit >> 8);
+    dptr[15] = (uint8_t)(advsm->periodic_crcinit >> 16);
+
+    /* Event Counter (2 bytes) */
+    put_le16(&dptr[16], advsm->periodic_event_cntr + event_cnt_off);
+}
+#endif
+
+/**
+ * Create the AUX PDU
+ */
+static uint8_t
+ble_ll_adv_aux_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    struct ble_ll_adv_aux *aux;
+    uint8_t adv_mode;
+    uint8_t pdu_type;
+    uint8_t ext_hdr_len;
+    uint32_t offset;
+
+    advsm = pducb_arg;
+    aux = AUX_CURRENT(advsm);
+
+    assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY));
+    assert(ble_ll_adv_active_chanset_is_sec(advsm));
+
+    /* It's the same for AUX_ADV_IND and AUX_CHAIN_IND */
+    pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND;
+
+    /* We do not create scannable PDUs here - this is handled separately */
+    adv_mode = 0;
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        adv_mode |= BLE_LL_EXT_ADV_MODE_CONN;
+    }
+
+    ext_hdr_len = aux->payload_len - BLE_LL_EXT_ADV_HDR_LEN - aux->aux_data_len;
+    dptr[0] = (adv_mode << 6) | ext_hdr_len;
+    dptr += 1;
+
+    /* only put flags if needed */
+    if (aux->ext_hdr) {
+        dptr[0] = aux->ext_hdr;
+        dptr += 1;
+    }
+
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+
+        /* Set TxAdd to random if needed. */
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+            pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
+        }
+
+        memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE);
+        dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        memcpy(dptr, advsm->initiator_addr, BLE_LL_EXT_ADV_TARGETA_SIZE);
+        dptr += BLE_LL_EXT_ADV_TARGETA_SIZE;
+
+        /* Set RxAdd to random if needed. */
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_RX_ADD) {
+            pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND;
+        }
+    }
+
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        dptr[0] = advsm->adi & 0x00ff;
+        dptr[1] = advsm->adi >> 8;
+        dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    }
+
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        if (!AUX_NEXT(advsm)->sch.enqueued) {
+            /*
+             * Trim data here in case we do not have next aux scheduled. This
+             * can happen if next aux was outside advertising set period and
+             * was removed from scheduler.
+             */
+            offset = 0;
+        } else if (advsm->rx_ble_hdr) {
+            offset = os_cputime_ticks_to_usecs(AUX_NEXT(advsm)->start_time - advsm->rx_ble_hdr->beg_cputime);
+            offset -= (advsm->rx_ble_hdr->rem_usecs + ble_ll_pdu_tx_time_get(12, advsm->sec_phy) + BLE_LL_IFS);
+        } else {
+            offset = os_cputime_ticks_to_usecs(AUX_NEXT(advsm)->start_time - aux->start_time);
+        }
+
+        ble_ll_adv_put_aux_ptr(AUX_NEXT(advsm)->chan, advsm->sec_phy,
+                               offset, dptr);
+
+        dptr += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+        ble_ll_adv_put_syncinfo(advsm, NULL, NULL, dptr);
+        dptr += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+    }
+#endif
+
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+        dptr[0] = advsm->adv_txpwr + ble_ll_get_tx_pwr_compensation();
+        dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    if (aux->aux_data_len) {
+        os_mbuf_copydata(*advsm->aux_data, aux->aux_data_offset,
+                         aux->aux_data_len, dptr);
+    }
+
+    *hdr_byte = pdu_type;
+
+    return aux->payload_len;
+}
+
+static uint8_t
+ble_ll_adv_aux_scannable_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    uint8_t pdu_type;
+    uint8_t *ext_hdr_len;
+    uint8_t *ext_hdr;
+    uint8_t pdulen;
+
+    advsm = pducb_arg;
+
+    assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY));
+    assert(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE);
+    assert(advsm->aux_first_pdu);
+    assert(ble_ll_adv_active_chanset_is_sec(advsm));
+
+    pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND;
+
+    ext_hdr_len = &dptr[0];
+    ext_hdr = &dptr[1];
+    dptr += 2;
+
+    /* Flags always */
+    *ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE;
+    *ext_hdr = 0;
+
+    /* AdvA when non anonymous */
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) {
+        /* Set TxAdd to random if needed. */
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+            pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
+        }
+
+        *ext_hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE;
+        *ext_hdr |= (1 << BLE_LL_EXT_ADV_ADVA_BIT);
+        memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE);
+        dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    /* TargetA only for directed */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        *ext_hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE;
+        *ext_hdr |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT);
+        memcpy(dptr, advsm->initiator_addr, BLE_LL_EXT_ADV_TARGETA_SIZE);
+        dptr += BLE_LL_EXT_ADV_TARGETA_SIZE;
+
+        /* Set RxAdd to random if needed. */
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_RX_ADD) {
+            pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND;
+        }
+    }
+
+    /* ADI always */
+    *ext_hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    *ext_hdr |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT);
+    dptr[0] = advsm->adi & 0x00ff;
+    dptr[1] = advsm->adi >> 8;
+    dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    /* TxPower if configured */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
+        *ext_hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+        *ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
+        dptr[0] = advsm->adv_txpwr + ble_ll_get_tx_pwr_compensation();
+        dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    pdulen = BLE_LL_EXT_ADV_HDR_LEN + *ext_hdr_len;
+
+    *hdr_byte = pdu_type;
+    *ext_hdr_len |= (BLE_LL_EXT_ADV_MODE_SCAN << 6);
+
+    return pdulen;
+}
+#endif
+
+static uint8_t
+ble_ll_adv_scan_rsp_legacy_pdu_make(uint8_t *dptr, void *pducb_arg,
+                                    uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    uint8_t     scan_rsp_len;
+    uint8_t     pdulen;
+    uint8_t     hdr;
+
+    advsm = pducb_arg;
+
+    /* Make sure that the length is valid */
+    scan_rsp_len = SCAN_RSP_DATA_LEN(advsm);
+    assert(scan_rsp_len <= BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN);
+
+    /* Set BLE transmit header */
+    pdulen = BLE_DEV_ADDR_LEN + scan_rsp_len;
+    hdr = BLE_ADV_PDU_TYPE_SCAN_RSP;
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+        hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+    }
+
+    *hdr_byte = hdr;
+
+    /*
+     * The adva in this packet will be the same one that was being advertised
+     * and is based on the peer identity address in the set advertising
+     * parameters. If a different peer sends us a scan request (for some reason)
+     * we will reply with an adva that was not generated based on the local irk
+     * of the peer sending the scan request.
+     */
+
+    /* Construct scan response */
+    memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
+    if (scan_rsp_len != 0) {
+        os_mbuf_copydata(advsm->scan_rsp_data, 0, scan_rsp_len,
+                         dptr + BLE_DEV_ADDR_LEN);
+    }
+
+    return pdulen;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/**
+ * Create a scan response PDU
+ *
+ * @param advsm
+ */
+static uint8_t
+ble_ll_adv_scan_rsp_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+
+    advsm = pducb_arg;
+
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        return ble_ll_adv_scan_rsp_legacy_pdu_make(dptr, pducb_arg, hdr_byte);
+    }
+
+    return ble_ll_adv_aux_pdu_make(dptr, pducb_arg, hdr_byte);
+}
+
+struct aux_conn_rsp_data {
+    struct ble_ll_adv_sm *advsm;
+    uint8_t *peer;
+    uint8_t rxadd;
+};
+
+/**
+ * Create a AUX connect response PDU
+ *
+ * @param advsm
+ */
+static uint8_t
+ble_ll_adv_aux_conn_rsp_pdu_make(uint8_t *dptr, void *pducb_arg,
+                                 uint8_t *hdr_byte)
+{
+    struct aux_conn_rsp_data *rsp_data;
+    uint8_t     pdulen;
+    uint8_t     ext_hdr_len;
+    uint8_t     ext_hdr_flags;
+    uint8_t     hdr;
+
+    rsp_data = pducb_arg;
+
+    /* flags,AdvA and TargetA */
+    ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_ADVA_SIZE +
+                  BLE_LL_EXT_ADV_TARGETA_SIZE;
+    ext_hdr_flags = (1 << BLE_LL_EXT_ADV_ADVA_BIT);
+    ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT);
+
+    pdulen = BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len;
+
+    /* Set BLE transmit header */
+    hdr = BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP;
+    if (rsp_data->rxadd) {
+        hdr |= BLE_ADV_PDU_HDR_RXADD_MASK;
+    }
+    if (rsp_data->advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+        hdr |= BLE_ADV_PDU_HDR_TXADD_MASK;
+    }
+
+    *hdr_byte = hdr;
+
+    /* ext hdr len and adv mode (00b) */
+    dptr[0] = ext_hdr_len;
+    dptr += 1;
+
+    /* ext hdr flags */
+    dptr[0] = ext_hdr_flags;
+    dptr += 1;
+
+    memcpy(dptr, rsp_data->advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE);
+    dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
+
+    memcpy(dptr, rsp_data->peer, BLE_LL_EXT_ADV_TARGETA_SIZE);
+    dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
+
+    return pdulen;
+}
+#endif
+
+/**
+ * Called to indicate the advertising event is over.
+ *
+ * Context: Interrupt
+ *
+ * @param advsm
+ *
+ */
+static void
+ble_ll_adv_tx_done(void *arg)
+{
+    struct ble_ll_adv_sm *advsm;
+
+    /* reset power to max after advertising */
+    ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+
+    advsm = (struct ble_ll_adv_sm *)arg;
+
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_ADV_TXDONE, advsm->adv_instance,
+                       advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (ble_ll_adv_active_chanset_is_pri(advsm)) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+    } else if (ble_ll_adv_active_chanset_is_sec(advsm)) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev);
+    } else {
+        assert(0);
+    }
+#else
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+#endif
+
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+
+    ble_ll_adv_active_chanset_clear(advsm);
+
+    /* We no longer have a current state machine */
+    g_ble_ll_cur_adv_sm = NULL;
+}
+
+/*
+ * Called when an advertising event has been removed from the scheduler
+ * without being run.
+ */
+void
+ble_ll_adv_event_rmvd_from_sched(struct ble_ll_adv_sm *advsm)
+{
+    /*
+     * Need to set advertising channel to final chan so new event gets
+     * scheduled.
+     */
+    advsm->adv_chan = ble_ll_adv_final_chan(advsm);
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+/*
+ * Called when a periodic event has been removed from the scheduler
+ * without being run.
+ */
+void
+ble_ll_adv_periodic_rmvd_from_sched(struct ble_ll_adv_sm *advsm)
+{
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_periodic_txdone_ev);
+}
+#endif
+
+/**
+ * This is the scheduler callback (called from interrupt context) which
+ * transmits an advertisement.
+ *
+ * Context: Interrupt (scheduler)
+ *
+ * @param sch
+ *
+ * @return int
+ */
+static int
+ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    uint8_t end_trans;
+    uint32_t txstart;
+    struct ble_ll_adv_sm *advsm;
+
+    /* Get the state machine for the event */
+    advsm = (struct ble_ll_adv_sm *)sch->cb_arg;
+
+    /* Set the current advertiser */
+    g_ble_ll_cur_adv_sm = advsm;
+
+    ble_ll_adv_active_chanset_set_pri(advsm);
+
+    if ((advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA) ||
+        (advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA)) {
+        goto adv_tx_done;
+    }
+
+    /* Set the power */
+    ble_phy_txpwr_set(advsm->adv_txpwr);
+
+    /* Set channel */
+    rc = ble_phy_setchan(advsm->adv_chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV);
+    assert(rc == 0);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    /* Set phy mode */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        ble_phy_mode_set(BLE_PHY_MODE_1M, BLE_PHY_MODE_1M);
+    } else {
+        ble_phy_mode_set(advsm->pri_phy, advsm->pri_phy);
+    }
+#else
+    ble_phy_mode_set(BLE_PHY_MODE_1M, BLE_PHY_MODE_1M);
+#endif
+#endif
+
+    /* Set transmit start time. */
+    txstart = sch->start_time + g_ble_ll_sched_offset_ticks;
+    rc = ble_phy_tx_set_start_time(txstart, sch->remainder);
+    if (rc) {
+        STATS_INC(ble_ll_stats, adv_late_starts);
+        goto adv_tx_done;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    advsm->adv_rpa_index = -1;
+    if (ble_ll_resolv_enabled()) {
+        ble_phy_resolv_list_enable();
+    } else {
+        ble_phy_resolv_list_disable();
+    }
+#endif
+
+    /* We switch to RX after connectable or scannable legacy packets. */
+    if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
+            ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) ||
+             (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE))) {
+        end_trans = BLE_PHY_TRANSITION_TX_RX;
+        ble_phy_set_txend_cb(NULL, NULL);
+    } else {
+        end_trans = BLE_PHY_TRANSITION_NONE;
+        ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
+    }
+
+    /* Transmit advertisement */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    rc = ble_phy_tx(ble_ll_adv_pdu_make, advsm, end_trans);
+#else
+    rc = ble_phy_tx(ble_ll_adv_legacy_pdu_make, advsm, end_trans);
+#endif
+    if (rc) {
+        goto adv_tx_done;
+    }
+
+    /* Enable/disable whitelisting based on filter policy */
+    if (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE) {
+        ble_ll_whitelist_enable();
+    } else {
+        ble_ll_whitelist_disable();
+    }
+
+    /* Set link layer state to advertising */
+    ble_ll_state_set(BLE_LL_STATE_ADV);
+
+    /* Count # of adv. sent */
+    STATS_INC(ble_ll_stats, adv_txg);
+
+    return BLE_LL_SCHED_STATE_RUNNING;
+
+adv_tx_done:
+    ble_ll_adv_tx_done(advsm);
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static void
+ble_ll_adv_set_sched(struct ble_ll_adv_sm *advsm)
+{
+    uint32_t max_usecs;
+    struct ble_ll_sched_item *sch;
+
+    sch = &advsm->adv_sch;
+    sch->cb_arg = advsm;
+    sch->sched_cb = ble_ll_adv_tx_start_cb;
+    sch->sched_type = BLE_LL_SCHED_TYPE_ADV;
+
+    /* Set end time to maximum time this schedule item may take */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        max_usecs = ble_ll_pdu_tx_time_get(advsm->adv_pdu_len, BLE_PHY_MODE_1M);
+
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+            max_usecs += BLE_LL_SCHED_DIRECT_ADV_MAX_USECS;
+        } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+            max_usecs += BLE_LL_SCHED_ADV_MAX_USECS;
+        }
+    } else {
+        /*
+         * In ADV_EXT_IND we always set only ADI and AUX so the payload length
+         * is always 7 bytes.
+         */
+        max_usecs = ble_ll_pdu_tx_time_get(7, advsm->pri_phy);
+    }
+#else
+    max_usecs = ble_ll_pdu_tx_time_get(advsm->adv_pdu_len, BLE_PHY_MODE_1M);
+
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        max_usecs += BLE_LL_SCHED_DIRECT_ADV_MAX_USECS;
+    } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        max_usecs += BLE_LL_SCHED_ADV_MAX_USECS;
+    }
+#endif
+
+    sch->start_time = advsm->adv_pdu_start_time - g_ble_ll_sched_offset_ticks;
+    sch->remainder = 0;
+    sch->end_time = advsm->adv_pdu_start_time +
+                    ble_ll_usecs_to_ticks_round_up(max_usecs);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static int
+ble_ll_adv_secondary_tx_start_cb(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    uint8_t end_trans;
+    uint32_t txstart;
+    struct ble_ll_adv_sm *advsm;
+    ble_phy_tx_pducb_t pducb;
+    struct ble_ll_adv_aux *aux;
+
+    /* Get the state machine for the event */
+    advsm = (struct ble_ll_adv_sm *)sch->cb_arg;
+
+    /* Set the current advertiser */
+    g_ble_ll_cur_adv_sm = advsm;
+
+    ble_ll_adv_active_chanset_set_sec(advsm);
+
+    /* Set the power */
+    ble_phy_txpwr_set(advsm->adv_txpwr);
+
+    /* Set channel */
+    aux = AUX_CURRENT(advsm);
+    rc = ble_phy_setchan(aux->chan, BLE_ACCESS_ADDR_ADV,
+                         BLE_LL_CRCINIT_ADV);
+    assert(rc == 0);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    /* Set phy mode */
+     ble_phy_mode_set(advsm->sec_phy, advsm->sec_phy);
+#endif
+
+    /* Set transmit start time. */
+    txstart = sch->start_time + g_ble_ll_sched_offset_ticks;
+    rc = ble_phy_tx_set_start_time(txstart, sch->remainder);
+    if (rc) {
+        STATS_INC(ble_ll_stats, adv_late_starts);
+        goto adv_aux_dropped;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    advsm->adv_rpa_index = -1;
+    if (ble_ll_resolv_enabled()) {
+        ble_phy_resolv_list_enable();
+    } else {
+        ble_phy_resolv_list_disable();
+    }
+#endif
+
+    /* Set phy mode based on type of advertisement */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        end_trans = BLE_PHY_TRANSITION_TX_RX;
+        ble_phy_set_txend_cb(NULL, NULL);
+        pducb = ble_ll_adv_aux_pdu_make;
+    } else if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) &&
+               advsm->aux_first_pdu) {
+        end_trans = BLE_PHY_TRANSITION_TX_RX;
+        ble_phy_set_txend_cb(NULL, NULL);
+        pducb = ble_ll_adv_aux_scannable_pdu_make;
+    } else {
+        end_trans = BLE_PHY_TRANSITION_NONE;
+        ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
+        pducb = ble_ll_adv_aux_pdu_make;
+    }
+
+    /* Transmit advertisement */
+    rc = ble_phy_tx(pducb, advsm, end_trans);
+    if (rc) {
+        goto adv_aux_dropped;
+    }
+
+    /* Enable/disable whitelisting based on filter policy */
+    if (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE) {
+        ble_ll_whitelist_enable();
+    } else {
+        ble_ll_whitelist_disable();
+    }
+
+    /* Set link layer state to advertising */
+    ble_ll_state_set(BLE_LL_STATE_ADV);
+
+    /* Count # of adv. sent */
+    STATS_INC(ble_ll_stats, adv_txg);
+
+    return BLE_LL_SCHED_STATE_RUNNING;
+
+adv_aux_dropped:
+    advsm->aux_dropped = 1;
+    ble_ll_adv_tx_done(advsm);
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static uint8_t
+ble_ll_adv_aux_scannable_pdu_payload_len(struct ble_ll_adv_sm *advsm)
+{
+    uint8_t len;
+
+    /* Flags, ADI always */
+    len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE
+            + BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    /* AdvA if not anonymous */
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) {
+        len += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    /* TargetA only for directed */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        len += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    /* TxPower if configured */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
+        len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    return len;
+}
+
+static void
+ble_ll_adv_aux_calculate(struct ble_ll_adv_sm *advsm,
+                         struct ble_ll_adv_aux *aux, uint16_t aux_data_offset)
+{
+    uint16_t rem_aux_data_len;
+    uint8_t hdr_len;
+    bool chainable;
+
+    assert(!aux->sch.enqueued);
+    assert((AUX_DATA_LEN(advsm) > aux_data_offset) ||
+           (AUX_DATA_LEN(advsm) == 0 && aux_data_offset == 0));
+
+    aux->aux_data_offset = aux_data_offset;
+    aux->aux_data_len = 0;
+    aux->payload_len = 0;
+    aux->ext_hdr = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    aux->chan = ble_ll_utils_calc_dci_csa2(advsm->event_cntr++,
+                                           advsm->channel_id,
+                                           g_ble_ll_conn_params.num_used_chans,
+                                           g_ble_ll_conn_params.master_chan_map);
+#else
+    aux->chan = ble_ll_utils_remapped_channel(rand() % BLE_PHY_NUM_DATA_CHANS,
+                                              g_ble_ll_conn_params.master_chan_map);
+#endif
+
+    rem_aux_data_len = AUX_DATA_LEN(advsm) - aux_data_offset;
+    chainable = !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE);
+
+    hdr_len = BLE_LL_EXT_ADV_HDR_LEN;
+
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) {
+        /* ADI */
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT);
+        hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    }
+
+    /* AdvA for 1st PDU in chain (i.e. AUX_ADV_IND or AUX_SCAN_RSP) */
+    if (aux_data_offset == 0 &&
+        !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_ADVA_BIT);
+        hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    /* Note: this function does not calculate AUX_ADV_IND when advertising is
+     * scannable. Instead it is calculated in ble_ll_adv_aux_schedule_first().
+     *
+     * However this function calculates length of AUX_SCAN_RSP and according
+     * to BT 5.0 Vol 6 Part B, 2.3.2.3, TargetA shall not be include there.
+     *
+     * This is why TargetA is added to all directed advertising here unless it
+     * is scannable one.
+     *
+     * Note. TargetA shall not be also in AUX_CHAIN_IND
+     */
+    if (aux_data_offset == 0  &&
+        (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) &&
+            !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT);
+        hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    /* TxPower if configured.
+     * Note: TxPower should not be be present in AUX_CHAIN_IND
+     */
+    if (aux_data_offset == 0 &&
+        (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR)) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
+        hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    /* SyncInfo for 1st PDU in chain (i.e. AUX_ADV_IND only) if periodic
+     * advertising is enabled
+     */
+    if (aux_data_offset == 0 && advsm->periodic_adv_active) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT);
+        hdr_len += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+    }
+#endif
+
+    /* if we have any fields in ext header we need to add flags, note that Aux
+     * PTR is handled later and it will account for flags if needed
+     */
+    if (aux->ext_hdr) {
+        hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE;
+    }
+
+    /* AdvData always */
+    aux->aux_data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, rem_aux_data_len);
+
+    /* AuxPtr if there are more AdvData remaining that we can fit here */
+    if (chainable && (rem_aux_data_len > aux->aux_data_len)) {
+            /* adjust for flags that needs to be added if AuxPtr is only field
+             * in Extended Header
+             */
+            if (!aux->ext_hdr) {
+                hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE;
+                aux->aux_data_len -= BLE_LL_EXT_ADV_FLAGS_SIZE;
+            }
+
+            aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT);
+            hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+            aux->aux_data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+
+            /* PDU payload should be full if chained */
+            assert(hdr_len + aux->aux_data_len == BLE_LL_MAX_PAYLOAD_LEN);
+    }
+
+    aux->payload_len = hdr_len + aux->aux_data_len;
+}
+
+static void
+ble_ll_adv_aux_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start,
+                         void *arg)
+{
+    struct ble_ll_adv_aux *aux = arg;
+
+    aux->start_time = sch_start + g_ble_ll_sched_offset_ticks;
+}
+
+static void
+ble_ll_adv_aux_schedule_next(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_aux *aux;
+    struct ble_ll_adv_aux *aux_next;
+    struct ble_ll_sched_item *sch;
+    uint16_t rem_aux_data_len;
+    uint16_t next_aux_data_offset;
+    uint32_t max_usecs;
+
+    assert(advsm->aux_active);
+
+    aux = AUX_CURRENT(advsm);
+    aux_next = AUX_NEXT(advsm);
+
+    assert(!aux_next->sch.enqueued);
+
+    /*
+     * Do not schedule next aux if current aux is no longer scheduled since we
+     * do not have reference time for scheduling.
+     */
+    if (!aux->sch.enqueued) {
+        return;
+    }
+
+    /*
+     * Do not schedule next aux if current aux does not have AuxPtr in extended
+     * header as this means we do not need subsequent ADV_CHAIN_IND to be sent.
+     */
+    if (!(aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT))) {
+        return;
+    }
+
+    next_aux_data_offset = aux->aux_data_offset + aux->aux_data_len;
+
+    assert(AUX_DATA_LEN(advsm) >= next_aux_data_offset);
+
+    rem_aux_data_len = AUX_DATA_LEN(advsm) - next_aux_data_offset;
+    assert(rem_aux_data_len > 0);
+
+    ble_ll_adv_aux_calculate(advsm, aux_next, next_aux_data_offset);
+    max_usecs = ble_ll_pdu_tx_time_get(aux_next->payload_len, advsm->sec_phy);
+
+    aux_next->start_time = aux->sch.end_time +
+                           ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + MYNEWT_VAL(BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY));
+
+    sch = &aux_next->sch;
+    sch->start_time = aux_next->start_time - g_ble_ll_sched_offset_ticks;
+    sch->remainder = 0;
+    sch->end_time = aux_next->start_time +
+                    ble_ll_usecs_to_ticks_round_up(max_usecs);
+    ble_ll_sched_adv_new(&aux_next->sch, ble_ll_adv_aux_scheduled, aux_next);
+
+    /*
+     * In case duration is set for advertising set we need to check if newly
+     * scheduled aux will fit inside duration. If not, remove it from scheduler
+     * so advertising will stop after current aux.
+     */
+    if (advsm->duration && (aux_next->sch.end_time > advsm->adv_end_time)) {
+        ble_ll_sched_rmv_elem(&aux_next->sch);
+    }
+}
+
+static void
+ble_ll_adv_aux_schedule_first(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_aux *aux;
+    struct ble_ll_sched_item *sch;
+    uint32_t max_usecs;
+
+    assert(!advsm->aux_active);
+    assert(!advsm->aux[0].sch.enqueued);
+    assert(!advsm->aux[1].sch.enqueued);
+
+    advsm->aux_active = 1;
+    advsm->aux_index = 0;
+    advsm->aux_first_pdu = 1;
+    advsm->aux_not_scanned = 0;
+    advsm->aux_dropped = 0;
+
+    aux = AUX_CURRENT(advsm);
+    ble_ll_adv_aux_calculate(advsm, aux, 0);
+
+    /* Set end time to maximum time this schedule item may take */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        max_usecs = ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy) +
+                    BLE_LL_IFS +
+                    /* AUX_CONN_REQ */
+                    ble_ll_pdu_tx_time_get(34 + 14, advsm->sec_phy)  +
+                    BLE_LL_IFS +
+                    /* AUX_CONN_RSP */
+                    ble_ll_pdu_tx_time_get(14, advsm->sec_phy);
+    } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+        /* For scannable advertising we need to calculate how much time we
+         * need for AUX_ADV_IND along with AUX_SCAN_REQ, AUX_SCAN_RSP and
+         * IFS in between.
+         *
+         * Note:
+         *  1. aux->payload_len, which calculated by above ble_ll_adv_aux_calulcate(),
+         * contains AUX_SCAN_RSP length.
+         *  2. length of AUX_ADV_IND is calculated by special function:
+         *      ble_ll_adv_aux_scannable_pdu_payload_len()
+         */
+        max_usecs = ble_ll_pdu_tx_time_get(ble_ll_adv_aux_scannable_pdu_payload_len(advsm),
+                                           advsm->sec_phy) +
+                    BLE_LL_IFS +
+                    /* AUX_SCAN_REQ */
+                    ble_ll_pdu_tx_time_get(12, advsm->sec_phy)  +
+                    BLE_LL_IFS +
+                    /* AUX_SCAN_RSP */
+                    ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy);
+    } else {
+        max_usecs = ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy);
+    }
+
+    sch = &aux->sch;
+    sch->start_time = aux->start_time - g_ble_ll_sched_offset_ticks;
+    sch->remainder = 0;
+    sch->end_time = aux->start_time + ble_ll_usecs_to_ticks_round_up(max_usecs);
+    ble_ll_sched_adv_new(sch, ble_ll_adv_aux_scheduled, aux);
+}
+
+static void
+ble_ll_adv_aux_set_start_time(struct ble_ll_adv_sm *advsm)
+{
+    static const uint8_t bits[8] = {0, 1, 1, 2, 1, 2, 2, 3};
+    struct ble_ll_sched_item *sched = &advsm->adv_sch;
+    uint32_t adv_pdu_dur;
+    uint32_t adv_event_dur;
+    uint8_t chans;
+
+    assert(!advsm->aux_active);
+    assert(!advsm->aux[0].sch.enqueued);
+    assert(!advsm->aux[1].sch.enqueued);
+
+    assert(advsm->adv_chanmask > 0 &&
+           advsm->adv_chanmask <= BLE_HCI_ADV_CHANMASK_DEF);
+
+    chans = bits[advsm->adv_chanmask];
+
+    /*
+     * We want to schedule auxiliary packet as soon as possible after the end
+     * of advertising event, but no sooner than T_MAFS. The interval between
+     * advertising packets is 250 usecs (8.19 ticks) on LE Coded and a bit less
+     * on 1M, but it can vary a bit due to scheduling which we can't really
+     * control. Since we round ticks up for both interval and T_MAFS, we still
+     * have some margin here. The worst thing that can happen is that we skip
+     * last advertising packet which is not a bit problem so leave it as-is, no
+     * need to make code more complicated.
+     */
+
+    /*
+     * XXX: this could be improved if phy has TX-TX transition with controlled
+     *      or predefined interval, but since it makes advertising code even
+     *      more complicated let's skip it for now...
+     */
+
+    adv_pdu_dur = (int32_t)(sched->end_time - sched->start_time) -
+                  g_ble_ll_sched_offset_ticks;
+
+    /* 9 is 8.19 ticks rounded up - see comment above */
+    adv_event_dur = (adv_pdu_dur * chans) + (9 * (chans - 1));
+
+    advsm->aux[0].start_time = advsm->adv_event_start_time + adv_event_dur +
+                               ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + MYNEWT_VAL(BLE_LL_SCHED_AUX_MAFS_DELAY));
+}
+
+static void
+ble_ll_adv_aux_schedule(struct ble_ll_adv_sm *advsm)
+{
+    /*
+     * For secondary channel we always start by scheduling two consecutive
+     * auxiliary packets at once. Then, after sending one packet we try to
+     * schedule another one as long as there are some data left to send. This
+     * is to make sure we can always calculate AuxPtr to subsequent packet
+     * without need to scheduled it in an interrupt.
+     */
+
+    ble_ll_adv_aux_set_start_time(advsm);
+    ble_ll_adv_aux_schedule_first(advsm);
+    ble_ll_adv_aux_schedule_next(advsm);
+
+    /*
+     * In case duration is set for advertising set we need to check if at least
+     * 1st aux will fit inside duration. If not, stop advertising now so we do
+     * not start extended advertising event which we cannot finish in time.
+     */
+    if (advsm->duration &&
+            (AUX_CURRENT(advsm)->sch.end_time > advsm->adv_end_time)) {
+        ble_ll_adv_sm_stop_timeout(advsm);
+    }
+}
+#endif
+
+/**
+ * Called when advertising need to be halted. This normally should not be called
+ * and is only called when a scheduled item executes but advertising is still
+ * running.
+ *
+ * Context: Interrupt
+ */
+void
+ble_ll_adv_halt(void)
+{
+    struct ble_ll_adv_sm *advsm;
+
+    if (g_ble_ll_cur_adv_sm != NULL) {
+        advsm = g_ble_ll_cur_adv_sm;
+
+        ble_ll_trace_u32(BLE_LL_TRACE_ID_ADV_HALT, advsm->adv_instance);
+
+        ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING) {
+            ble_ll_adv_flags_clear(advsm,
+                                   BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING);
+            ble_npl_eventq_put(&g_ble_ll_data.ll_evq,
+                               &advsm->adv_periodic_txdone_ev);
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+            g_ble_ll_cur_adv_sm = NULL;
+            return;
+        }
+#endif
+
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) {
+            ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev);
+        }
+#endif
+
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        ble_ll_adv_active_chanset_clear(g_ble_ll_cur_adv_sm);
+        g_ble_ll_cur_adv_sm = NULL;
+    } else {
+        ble_ll_trace_u32(BLE_LL_TRACE_ID_ADV_HALT, UINT32_MAX);
+    }
+}
+
+/**
+ * Called by the HCI command parser when a set advertising parameters command
+ * has been received.
+ *
+ * Context: Link Layer task (HCI command parser)
+ *
+ * @param cmd
+ *
+ * @return int
+ */
+int
+ble_ll_adv_set_adv_params(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_adv_params_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_adv_sm *advsm;
+    uint8_t adv_filter_policy;
+    uint16_t adv_itvl_min;
+    uint16_t adv_itvl_max;
+    uint16_t props;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    advsm = &g_ble_ll_adv_sm[0];
+    if (advsm->adv_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Make sure intervals are OK (along with advertising type */
+    adv_itvl_min = le16toh(cmd->min_interval);
+    adv_itvl_max = le16toh(cmd->max_interval);
+
+    /*
+     * Get the filter policy now since we will ignore it if we are doing
+     * directed advertising
+     */
+    adv_filter_policy = cmd->filter_policy;
+
+    switch (cmd->type) {
+    case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD:
+        adv_filter_policy = BLE_HCI_ADV_FILT_NONE;
+        memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+
+        /* Ignore min/max interval */
+        adv_itvl_min = 0;
+        adv_itvl_max = 0;
+
+        props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_HD_DIR ;
+        break;
+    case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD:
+        adv_filter_policy = BLE_HCI_ADV_FILT_NONE;
+        memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+
+        props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_LD_DIR ;
+        break;
+    case BLE_HCI_ADV_TYPE_ADV_IND:
+        props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_IND;
+        break;
+    case BLE_HCI_ADV_TYPE_ADV_NONCONN_IND:
+        props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_NONCONN;
+        break;
+    case BLE_HCI_ADV_TYPE_ADV_SCAN_IND:
+        props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_SCAN;
+        break;
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Make sure intervals values are valid
+     * (HD directed advertising ignores those parameters)
+     */
+    if (!(props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED)) {
+        if ((adv_itvl_min > adv_itvl_max) ||
+                (adv_itvl_min < BLE_HCI_ADV_ITVL_MIN) ||
+                (adv_itvl_min > BLE_HCI_ADV_ITVL_MAX) ||
+                (adv_itvl_max < BLE_HCI_ADV_ITVL_MIN) ||
+                (adv_itvl_max > BLE_HCI_ADV_ITVL_MAX)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+
+    if ((cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) ||
+        (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    advsm->adv_txpwr = MYNEWT_VAL(BLE_LL_TX_PWR_DBM);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+        /* Copy peer address */
+        memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+    }
+#else
+    /* If we dont support privacy some address types wont work */
+    if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+#endif
+
+    /* There are only three adv channels, so check for any outside the range */
+    if (((cmd->chan_map & 0xF8) != 0) || (cmd->chan_map == 0)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check for valid filter policy */
+    if (adv_filter_policy > BLE_HCI_ADV_FILT_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Fill out rest of advertising state machine */
+    advsm->own_addr_type = cmd->own_addr_type;
+    advsm->peer_addr_type = cmd->peer_addr_type;
+    advsm->adv_filter_policy = adv_filter_policy;
+    advsm->adv_chanmask = cmd->chan_map;
+    advsm->adv_itvl_min = adv_itvl_min;
+    advsm->adv_itvl_max = adv_itvl_max;
+    advsm->props = props;
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_adv_update_did(struct ble_ll_adv_sm *advsm)
+{
+    uint16_t old_adi = advsm->adi;
+
+    /*
+     * The Advertising DID for a given advertising set shall be initialized
+     * with a randomly chosen value. Whenever the Host provides new advertising
+     * data or scan response data for a given advertising set (whether it is the
+     * same as the previous data or not), the Advertising DID shall be updated.
+     * The new value shall be a randomly chosen value that is not the same as
+     * the previously used value.
+     */
+    do {
+        advsm->adi = (advsm->adi & 0xf000) | (rand() & 0x0fff);
+    } while (old_adi == advsm->adi);
+}
+#endif
+
+static void
+ble_ll_adv_update_adv_scan_rsp_data(struct ble_ll_adv_sm *advsm)
+{
+    if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA) &&
+                    !(advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA)) {
+        return;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (advsm->aux_active) {
+        return;
+    }
+#endif
+
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA) {
+        if (advsm->new_adv_data) {
+            os_mbuf_free_chain(advsm->adv_data);
+            advsm->adv_data = advsm->new_adv_data;
+            advsm->new_adv_data = NULL;
+        }
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA);
+    } else if (advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA) {
+        os_mbuf_free_chain(advsm->scan_rsp_data);
+        advsm->scan_rsp_data = advsm->new_scan_rsp_data;
+        advsm->new_scan_rsp_data = NULL;
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA);
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* DID shall be updated when host provides new advertising data */
+    ble_ll_adv_update_did(advsm);
+#endif
+}
+
+/**
+ * Stop advertising state machine
+ *
+ * Context: Link Layer task.
+ *
+ * @param advsm
+ */
+static void
+ble_ll_adv_sm_stop(struct ble_ll_adv_sm *advsm)
+{
+    os_sr_t sr;
+
+    if (advsm->adv_enabled) {
+        ble_ll_rfmgmt_release();
+
+        /* Remove any scheduled advertising items */
+        ble_ll_sched_rmv_elem(&advsm->adv_sch);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        advsm->aux_active = 0;
+        ble_ll_sched_rmv_elem(&advsm->aux[0].sch);
+        ble_ll_sched_rmv_elem(&advsm->aux[1].sch);
+#endif
+
+        /* Set to standby if we are no longer advertising */
+        OS_ENTER_CRITICAL(sr);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if ((g_ble_ll_cur_adv_sm == advsm) &&
+               !(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING)) {
+            ble_phy_disable();
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+            g_ble_ll_cur_adv_sm = NULL;
+            ble_ll_scan_chk_resume();
+        }
+#else
+        if (ble_ll_state_get() == BLE_LL_STATE_ADV) {
+            ble_phy_disable();
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+            g_ble_ll_cur_adv_sm = NULL;
+            ble_ll_scan_chk_resume();
+        }
+#endif
+        OS_EXIT_CRITICAL(sr);
+
+        ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev);
+#endif
+
+        /* If there is an event buf we need to free it */
+        if (advsm->conn_comp_ev) {
+            ble_hci_trans_buf_free(advsm->conn_comp_ev);
+            advsm->conn_comp_ev = NULL;
+        }
+
+        ble_ll_adv_active_chanset_clear(advsm);
+
+        /* Disable advertising */
+        advsm->adv_enabled = 0;
+
+        /* Check if there is outstanding update */
+        ble_ll_adv_update_adv_scan_rsp_data(advsm);
+    }
+}
+
+static void
+ble_ll_adv_sm_stop_timeout(struct ble_ll_adv_sm *advsm)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (ble_ll_hci_adv_mode_ext()) {
+        ble_ll_hci_ev_send_adv_set_terminated(BLE_ERR_DIR_ADV_TMO,
+                                                        advsm->adv_instance, 0,
+                                                        advsm->events);
+    }
+#endif
+
+    /*
+     * For high duty directed advertising we need to send connection
+     * complete event with proper status
+     */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+        ble_ll_conn_comp_event_send(NULL, BLE_ERR_DIR_ADV_TMO,
+                                    advsm->conn_comp_ev, advsm);
+        advsm->conn_comp_ev = NULL;
+    }
+
+    /* Disable advertising */
+    ble_ll_adv_sm_stop(advsm);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_adv_sm_stop_limit_reached(struct ble_ll_adv_sm *advsm)
+{
+    ble_ll_hci_ev_send_adv_set_terminated(BLE_ERR_LIMIT_REACHED,
+                                          advsm->adv_instance, 0,
+                                          advsm->events);
+
+    /*
+     * For high duty directed advertising we need to send connection
+     * complete event with proper status
+     *
+     * Spec is a bit unambiguous here since it doesn't define what code should
+     * be used if HD directed advertising was terminated before timeout due to
+     * events count limit. For now just use same code as with duration timeout.
+     */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+        ble_ll_conn_comp_event_send(NULL, BLE_ERR_DIR_ADV_TMO,
+                                    advsm->conn_comp_ev, advsm);
+        advsm->conn_comp_ev = NULL;
+    }
+
+    /* Disable advertising */
+    ble_ll_adv_sm_stop(advsm);
+}
+#endif
+
+static void
+ble_ll_adv_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start, void *arg)
+{
+    /* The event start time is when we start transmission of the adv PDU */
+    advsm->adv_event_start_time = sch_start + g_ble_ll_sched_offset_ticks;
+    advsm->adv_pdu_start_time = advsm->adv_event_start_time;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* this is validated for HD adv so no need to do additional checks here
+     * duration is in 10ms units
+     */
+    if (advsm->duration) {
+        advsm->adv_end_time = advsm->adv_event_start_time +
+                             os_cputime_usecs_to_ticks(advsm->duration * 10000);
+    }
+#else
+    /* Set the time at which we must end directed, high-duty cycle advertising.
+     */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+        advsm->adv_end_time = advsm->adv_event_start_time +
+                     os_cputime_usecs_to_ticks(BLE_LL_ADV_STATE_HD_MAX * 1000);
+    }
+#endif
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+static uint8_t
+ble_ll_adv_sync_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    struct ble_ll_adv_sync *sync;
+    uint8_t adv_mode;
+    uint8_t pdu_type;
+    uint8_t ext_hdr_len;
+    uint32_t offset;
+
+    advsm = pducb_arg;
+    sync = SYNC_CURRENT(advsm);
+
+    assert(!ble_ll_adv_active_chanset_is_sec(advsm));
+    assert(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING);
+
+    /* It's the same for AUX_SYNC_IND and AUX_CHAIN_IND */
+    pdu_type = BLE_ADV_PDU_TYPE_AUX_SYNC_IND;
+
+    /* non-connectable and non-scannable */
+    adv_mode = 0;
+
+    ext_hdr_len = sync->payload_len - BLE_LL_EXT_ADV_HDR_LEN - sync->sync_data_len;
+    dptr[0] = (adv_mode << 6) | ext_hdr_len;
+    dptr += 1;
+
+    /* only put flags if needed */
+    if (sync->ext_hdr) {
+        dptr[0] = sync->ext_hdr;
+        dptr += 1;
+    }
+
+    if (sync->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        if (!SYNC_NEXT(advsm)->sch.enqueued) {
+            /*
+             * Trim data here in case we do not have next sync scheduled. This
+             * can happen if next sync was outside advertising set period and
+             * was removed from scheduler.
+             */
+            offset = 0;
+        } else {
+            offset = os_cputime_ticks_to_usecs(SYNC_NEXT(advsm)->start_time - sync->start_time);
+        }
+
+        ble_ll_adv_put_aux_ptr(SYNC_NEXT(advsm)->chan, advsm->sec_phy,
+                               offset, dptr);
+
+        dptr += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    }
+
+    if (sync->ext_hdr & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+        dptr[0] = advsm->adv_txpwr + ble_ll_get_tx_pwr_compensation();
+        dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    if (sync->sync_data_len) {
+        os_mbuf_copydata(advsm->periodic_adv_data, sync->sync_data_offset,
+                         sync->sync_data_len, dptr);
+    }
+
+    *hdr_byte = pdu_type;
+
+    return sync->payload_len;
+}
+
+
+static void
+ble_ll_adv_sync_tx_done(struct ble_ll_adv_sm *advsm)
+{
+    /* reset power to max after advertising */
+    ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+
+    /* for sync we trace a no pri nor sec set */
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_ADV_TXDONE, advsm->adv_instance, 0);
+
+    assert(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING);
+    assert(!ble_ll_adv_active_chanset_is_sec(advsm));
+
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_periodic_txdone_ev);
+
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING);
+
+    /* We no longer have a current state machine */
+    g_ble_ll_cur_adv_sm = NULL;
+}
+
+/**
+ * Called to indicate the advertising sync event is over.
+ *
+ * Context: Interrupt
+ *
+ * @param advsm
+ *
+ */
+static void
+ble_ll_adv_sync_tx_end(void *arg)
+{
+    struct ble_ll_adv_sm *advsm = arg;
+
+    ble_ll_adv_sync_tx_done(advsm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    /* store last sent periodic counter */
+    advsm->periodic_event_cntr_last_sent = advsm->periodic_event_cntr;
+#endif
+}
+
+static int
+ble_ll_adv_sync_tx_start_cb(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    uint32_t txstart;
+    struct ble_ll_adv_sm *advsm;
+    struct ble_ll_adv_sync *sync;
+
+    /* Get the state machine for the event */
+    advsm = (struct ble_ll_adv_sm *)sch->cb_arg;
+
+    /* Set the current advertiser */
+    g_ble_ll_cur_adv_sm = advsm;
+
+    ble_ll_adv_active_chanset_clear(advsm);
+    ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING);
+
+    /* Set the power */
+    ble_phy_txpwr_set(advsm->adv_txpwr);
+
+    /* Set channel */
+    sync = SYNC_CURRENT(advsm);
+    rc = ble_phy_setchan(sync->chan, advsm->periodic_access_addr,
+                         advsm->periodic_crcinit);
+
+    assert(rc == 0);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    /* Set phy mode */
+     ble_phy_mode_set(advsm->sec_phy, advsm->sec_phy);
+#endif
+
+    /* Set transmit start time. */
+    txstart = sch->start_time + g_ble_ll_sched_offset_ticks;
+    rc = ble_phy_tx_set_start_time(txstart, sch->remainder);
+    if (rc) {
+        STATS_INC(ble_ll_stats, adv_late_starts);
+        goto adv_tx_done;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_phy_resolv_list_disable();
+#endif
+
+    /* Transmit advertisement */
+    ble_phy_set_txend_cb(ble_ll_adv_sync_tx_end, advsm);
+    rc = ble_phy_tx(ble_ll_adv_sync_pdu_make, advsm, BLE_PHY_TRANSITION_NONE);
+    if (rc) {
+        goto adv_tx_done;
+    }
+
+    /* disable whitelisting, we are always non-connectable non-scannable */
+    ble_ll_whitelist_disable();
+
+    /* Set link layer state to advertising */
+    ble_ll_state_set(BLE_LL_STATE_ADV);
+
+    /* Count # of adv. sent */
+    STATS_INC(ble_ll_stats, adv_txg);
+
+    return BLE_LL_SCHED_STATE_RUNNING;
+
+adv_tx_done:
+    ble_ll_adv_sync_tx_done(advsm);
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static void
+ble_ll_adv_sync_calculate(struct ble_ll_adv_sm *advsm,
+                          struct ble_ll_adv_sync *sync,
+                          uint16_t sync_data_offset,
+                          uint8_t chan)
+{
+    uint16_t rem_sync_data_len;
+    uint8_t hdr_len;
+
+    assert(!sync->sch.enqueued);
+    assert((SYNC_DATA_LEN(advsm) > sync_data_offset) ||
+           (SYNC_DATA_LEN(advsm) == 0 && sync_data_offset == 0));
+
+    sync->sync_data_offset = sync_data_offset;
+    sync->sync_data_len = 0;
+    sync->payload_len = 0;
+    sync->ext_hdr = 0;
+    sync->chan = chan;
+
+    rem_sync_data_len = SYNC_DATA_LEN(advsm) - sync_data_offset;
+
+    hdr_len = BLE_LL_EXT_ADV_HDR_LEN;
+
+    /* TxPower if configured
+     * Note: TxPower shall not be present in chain PDU for SYNC
+     */
+    if (sync_data_offset == 0 &&
+        (advsm->periodic_adv_props & BLE_HCI_LE_SET_PERIODIC_ADV_PROP_INC_TX_PWR)) {
+        sync->ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
+        hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    /* if we have any fields in ext header we need to add flags, note that Aux
+     * PTR is handled later and it will account for flags if needed
+     *
+     * This could be handled inside TxPower but lets keep code consistent with
+     * how Aux calculate works and this also make it easier to add more fields
+     * into flags if needed in future
+     */
+    if (sync->ext_hdr) {
+        hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE;
+    }
+
+    /* AdvData always */
+    sync->sync_data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, rem_sync_data_len);
+
+    /* AuxPtr if there are more AdvData remaining that we can fit here */
+    if ((rem_sync_data_len > sync->sync_data_len)) {
+            /* adjust for flags that needs to be added if AuxPtr is only field
+             * in Extended Header
+             */
+            if (!sync->ext_hdr) {
+                hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE;
+                sync->sync_data_len -= BLE_LL_EXT_ADV_FLAGS_SIZE;
+            }
+
+            sync->ext_hdr |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT);
+            hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+            sync->sync_data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+
+            /* PDU payload should be full if chained */
+            assert(hdr_len + sync->sync_data_len == BLE_LL_MAX_PAYLOAD_LEN);
+    }
+
+    sync->payload_len = hdr_len + sync->sync_data_len;
+}
+
+static void
+ble_ll_adv_periodic_schedule_first(struct ble_ll_adv_sm *advsm,
+                                   bool first_pdu)
+{
+    struct ble_ll_adv_sync *sync;
+    struct ble_ll_sched_item *sch;
+    uint32_t sch_start;
+    uint32_t max_usecs;
+    uint8_t chan;
+    int rc;
+
+    assert(!advsm->periodic_sync_active);
+    assert(!advsm->periodic_sync[0].sch.enqueued);
+    assert(!advsm->periodic_sync[1].sch.enqueued);
+
+    advsm->periodic_sync_active = 1;
+    advsm->periodic_sync_index = 0;
+
+    sync = SYNC_CURRENT(advsm);
+
+    /* For first SYNC packet in chain we use separate CSA#2 state to maintain
+     * freq hopping as advertised in SyncInfo
+     *
+     * Preincrement event counter as we later send this in PDU so make sure
+     * same values are used
+     */
+    chan = ble_ll_utils_calc_dci_csa2(++advsm->periodic_event_cntr,
+                                      advsm->periodic_channel_id,
+                                      advsm->periodic_num_used_chans,
+                                      advsm->periodic_chanmap);
+
+    ble_ll_adv_sync_calculate(advsm, sync, 0, chan);
+
+    /* sync is always non-connectable and non-scannable*/
+    max_usecs = ble_ll_pdu_tx_time_get(sync->payload_len, advsm->sec_phy);
+
+    sch = &sync->sch;
+
+    advsm->periodic_adv_event_start_time_remainder += advsm->periodic_adv_itvl_rem_usec;
+    if (advsm->periodic_adv_event_start_time_remainder >= 31) {
+        advsm->periodic_adv_event_start_time++;
+        advsm->periodic_adv_event_start_time_remainder -= 31;
+    }
+
+    sch->start_time = advsm->periodic_adv_event_start_time;
+    sch->remainder = advsm->periodic_adv_event_start_time_remainder;
+    sch->end_time = sch->start_time + ble_ll_usecs_to_ticks_round_up(max_usecs);
+    sch->start_time -= g_ble_ll_sched_offset_ticks;
+
+    rc = ble_ll_sched_periodic_adv(sch, &sch_start, first_pdu);
+    if (rc) {
+        STATS_INC(ble_ll_stats, periodic_adv_drop_event);
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq,
+                           &advsm->adv_periodic_txdone_ev);
+        return;
+    }
+
+    sync->start_time = sch_start + g_ble_ll_sched_offset_ticks;
+
+    assert(first_pdu ||
+           (sync->start_time == advsm->periodic_adv_event_start_time));
+
+    /* The event start time is when we start transmission of the SYNC PDU */
+    advsm->periodic_adv_event_start_time = sync->start_time;
+}
+
+static void
+ble_ll_adv_sync_next_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start,
+                         void *arg)
+{
+    struct ble_ll_adv_sync *sync = arg;
+
+    sync->start_time = sch_start + g_ble_ll_sched_offset_ticks;
+}
+
+static void
+ble_ll_adv_periodic_schedule_next(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_sync *sync;
+    struct ble_ll_adv_sync *sync_next;
+    struct ble_ll_sched_item *sch;
+    uint16_t rem_sync_data_len;
+    uint16_t next_sync_data_offset;
+    uint32_t max_usecs;
+    uint8_t chan;
+
+    assert(advsm->periodic_sync_active);
+
+    sync = SYNC_CURRENT(advsm);
+    sync_next = SYNC_NEXT(advsm);
+
+    assert(!sync_next->sch.enqueued);
+
+    /*
+     * Do not schedule next sync if current sync is no longer scheduled since we
+     * do not have reference time for scheduling.
+     */
+    if (!sync->sch.enqueued) {
+        return;
+    }
+
+    /*
+     * Do not schedule next sync if current sync does not have AuxPtr in extended
+     * header as this means we do not need subsequent ADV_CHAIN_IND to be sent.
+     */
+    if (!(sync->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT))) {
+        return;
+    }
+
+    next_sync_data_offset = sync->sync_data_offset + sync->sync_data_len;
+
+    assert(SYNC_DATA_LEN(advsm) >= next_sync_data_offset);
+
+    rem_sync_data_len = SYNC_DATA_LEN(advsm) - next_sync_data_offset;
+    assert(rem_sync_data_len > 0);
+
+    /* we use separate counter for chaining */
+    chan = ble_ll_utils_calc_dci_csa2(advsm->periodic_chain_event_cntr++,
+                                      advsm->periodic_channel_id,
+                                      advsm->periodic_num_used_chans,
+                                      advsm->periodic_chanmap);
+
+    ble_ll_adv_sync_calculate(advsm, sync_next, next_sync_data_offset, chan);
+    max_usecs = ble_ll_pdu_tx_time_get(sync_next->payload_len, advsm->sec_phy);
+
+    sync_next->start_time = sync->sch.end_time +
+                            ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + MYNEWT_VAL(BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY));
+
+    sch = &sync_next->sch;
+    sch->start_time = sync_next->start_time - g_ble_ll_sched_offset_ticks;
+
+    /* adjust for previous packets remainder */
+    sch->remainder = sync->sch.remainder;
+    sch->end_time = sync_next->start_time +
+                    ble_ll_usecs_to_ticks_round_up(max_usecs);
+
+    /* here we can use ble_ll_sched_adv_new as we don't care about timing */
+    ble_ll_sched_adv_new(&sync_next->sch, ble_ll_adv_sync_next_scheduled,
+                         sync_next);
+
+    /* if we are pass advertising interval, drop chain */
+    if (sch->end_time > advsm->periodic_adv_event_start_time +
+            advsm->periodic_adv_itvl_ticks) {
+        STATS_INC(ble_ll_stats, periodic_chain_drop_event);
+        ble_ll_sched_rmv_elem(&sync->sch);
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq,
+                           &advsm->adv_periodic_txdone_ev);
+    }
+}
+
+static void
+ble_ll_adv_sync_schedule(struct ble_ll_adv_sm *advsm, bool first_pdu)
+{
+    /*
+     * For secondary channel we always start by scheduling two consecutive
+     * auxiliary packets at once. Then, after sending one packet we try to
+     * schedule another one as long as there are some data left to send. This
+     * is to make sure we can always calculate AuxPtr to subsequent packet
+     * without need to scheduled it in an interrupt.
+     */
+
+    ble_ll_adv_periodic_schedule_first(advsm, first_pdu);
+    ble_ll_adv_periodic_schedule_next(advsm);
+}
+
+static void
+ble_ll_adv_reschedule_periodic_event(struct ble_ll_adv_sm *advsm)
+{
+    advsm->periodic_adv_event_start_time += advsm->periodic_adv_itvl_ticks;
+    ble_ll_adv_sync_schedule(advsm, false);
+}
+
+static void
+ble_ll_adv_update_periodic_data(struct ble_ll_adv_sm *advsm)
+{
+    if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA)) {
+        return;
+    }
+
+    if (advsm->periodic_sync_active) {
+        return;
+    }
+
+    if (advsm->periodic_new_data) {
+        os_mbuf_free_chain(advsm->periodic_adv_data);
+        advsm->periodic_adv_data = advsm->periodic_new_data;
+        advsm->periodic_new_data = NULL;
+    }
+
+    ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA);
+}
+
+/**
+ * Called when periodic packet is txd on secondary channel
+ *
+ * Context: Link Layer task.
+ *
+ * @param ev
+ */
+static void
+ble_ll_adv_periodic_done(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_sync *sync;
+    struct ble_ll_adv_sync *sync_next;
+
+    assert(advsm->periodic_adv_enabled);
+    assert(advsm->periodic_adv_active);
+    assert(advsm->periodic_sync_active);
+
+    ble_ll_rfmgmt_release();
+
+    sync = SYNC_CURRENT(advsm);
+    sync_next = SYNC_NEXT(advsm);
+
+    /* Remove anything else scheduled for periodic */
+    ble_ll_sched_rmv_elem(&sync->sch);
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_periodic_txdone_ev);
+
+    /* If we have next SYNC scheduled, try to schedule another one */
+    if (sync_next->sch.enqueued) {
+        advsm->periodic_sync_index ^= 1;
+        ble_ll_adv_periodic_schedule_next(advsm);
+        return;
+    }
+
+    /* Check if we need to resume scanning */
+    ble_ll_scan_chk_resume();
+
+    advsm->periodic_sync_active = 0;
+    ble_ll_adv_update_periodic_data(advsm);
+    ble_ll_adv_reschedule_periodic_event(advsm);
+}
+
+static void
+ble_ll_adv_periodic_event_done(struct ble_npl_event *ev)
+{
+    ble_ll_adv_periodic_done(ble_npl_event_get_arg(ev));
+}
+
+static void
+ble_ll_adv_sm_start_periodic(struct ble_ll_adv_sm *advsm)
+{
+    uint32_t usecs;
+    uint32_t ticks;
+
+    /*
+     * The Advertising DID is not required to change when a SyncInfo field is
+     * added to or removed from an advertising set. However, if it does not
+     * change, then scanners may fail to synchronize to periodic advertising
+     * because entries in the Advertising DID cache (see Section 4.3.3) mean
+     * they ignore the advertisements containing the SyncInfo field. Therefore,
+     * advertisers should update the Advertising DID when a periodic advertising
+     * train is enabled.
+     */
+    ble_ll_adv_update_did(advsm);
+
+    advsm->periodic_adv_active = 1;
+
+    /* keep channel map since we cannot change it later on */
+    memcpy(advsm->periodic_chanmap, g_ble_ll_conn_params.master_chan_map,
+           BLE_LL_CONN_CHMAP_LEN);
+    advsm->periodic_num_used_chans = g_ble_ll_conn_params.num_used_chans;
+    advsm->periodic_event_cntr = 0;
+    /* for chaining we start with random counter as we share access addr */
+    advsm->periodic_chain_event_cntr = rand();
+    advsm->periodic_access_addr = ble_ll_utils_calc_access_addr();
+    advsm->periodic_channel_id = ((advsm->periodic_access_addr & 0xffff0000) >> 16) ^
+                                 (advsm->periodic_access_addr & 0x0000ffff);
+    advsm->periodic_crcinit = rand() & 0xffffff;
+
+    usecs = (uint32_t)advsm->periodic_adv_itvl_max * BLE_LL_ADV_PERIODIC_ITVL;
+    ticks = os_cputime_usecs_to_ticks(usecs);
+
+    advsm->periodic_adv_itvl_rem_usec = (usecs - os_cputime_ticks_to_usecs(ticks));
+    if (advsm->periodic_adv_itvl_rem_usec == 31) {
+        advsm->periodic_adv_itvl_rem_usec = 0;
+        ticks++;
+    }
+    advsm->periodic_adv_itvl_ticks = ticks;
+
+    /* There is no point in starting periodic advertising until next advertising
+     * event since SyncInfo is needed for synchronization
+     */
+    advsm->periodic_adv_event_start_time_remainder = 0;
+    advsm->periodic_adv_event_start_time = advsm->adv_pdu_start_time +
+                      os_cputime_usecs_to_ticks(advsm->adv_itvl_usecs + 5000);
+
+    ble_ll_adv_sync_schedule(advsm, true);
+}
+
+static void
+ble_ll_adv_sm_stop_periodic(struct ble_ll_adv_sm *advsm)
+{
+    os_sr_t sr;
+
+    ble_ll_rfmgmt_release();
+
+    if (!advsm->periodic_adv_active) {
+        return;
+    }
+
+    /*
+     * The Advertising DID is not required to change when a SyncInfo field is
+     * added to or removed from an advertising set. However, if it does not
+     * change, then scanners may unnecessary try to synchronize to instance that
+     * no longer has periodic advertising enabled  because entries in the
+     * Advertising DID cache (see Section 4.3.3) mean they ignore the
+     * advertisements no longer containing the SyncInfo field. Therefore,
+     * advertisers should update the Advertising DID when a periodic advertising
+     * train is disabled.
+     */
+    ble_ll_adv_update_did(advsm);
+
+    /* Remove any scheduled advertising items */
+    advsm->periodic_adv_active = 0;
+    advsm->periodic_sync_active = 0;
+    ble_ll_sched_rmv_elem(&advsm->periodic_sync[0].sch);
+    ble_ll_sched_rmv_elem(&advsm->periodic_sync[1].sch);
+
+    /* Set to standby if we are no longer advertising */
+    OS_ENTER_CRITICAL(sr);
+    if ((g_ble_ll_cur_adv_sm == advsm) &&
+            (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING)) {
+        ble_phy_disable();
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        g_ble_ll_cur_adv_sm = NULL;
+        ble_ll_scan_chk_resume();
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq,
+                          &advsm->adv_periodic_txdone_ev);
+
+    ble_ll_adv_update_periodic_data(advsm);
+}
+#endif
+
+/**
+ * Start the advertising state machine. This is called when the host sends
+ * the "enable advertising" command and is not called again while in the
+ * advertising state.
+ *
+ * Context: Link-layer task.
+ *
+ * @param advsm Pointer to advertising state machine
+ *
+ * @return int
+ */
+static int
+ble_ll_adv_sm_start(struct ble_ll_adv_sm *advsm)
+{
+    uint8_t adv_chan;
+    uint8_t *addr;
+    uint8_t *evbuf;
+    uint32_t start_delay_us;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    uint32_t access_addr;
+#endif
+    const uint8_t *random_addr;
+    uint32_t earliest_start_time;
+    int32_t delta;
+
+    /* only clear flags that are not set from HCI */
+    ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD |
+                                  BLE_LL_ADV_SM_FLAG_RX_ADD |
+                                  BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    random_addr = advsm->adv_random_addr;
+#else
+    random_addr = g_random_addr;
+#endif
+
+    if (!ble_ll_is_valid_own_addr_type(advsm->own_addr_type, random_addr)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /*
+     * Get an event with which to send the connection complete event if
+     * this is connectable
+     */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        /* We expect this to be NULL but if not we wont allocate one... */
+        if (advsm->conn_comp_ev == NULL) {
+            evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (!evbuf) {
+                return BLE_ERR_MEM_CAPACITY;
+            }
+            advsm->conn_comp_ev = evbuf;
+        }
+    }
+
+    /* Set advertising address */
+    if ((advsm->own_addr_type & 1) == 0) {
+        addr = g_dev_addr;
+    } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        addr = advsm->adv_random_addr;
+#else
+        addr = g_random_addr;
+#endif
+        advsm->flags |= BLE_LL_ADV_SM_FLAG_TX_ADD;
+    }
+    memcpy(advsm->adva, addr, BLE_DEV_ADDR_LEN);
+
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        memcpy(advsm->initiator_addr, advsm->peer_addr, BLE_DEV_ADDR_LEN);
+        if (advsm->peer_addr_type & 1) {
+            advsm->flags |= BLE_LL_ADV_SM_FLAG_RX_ADD;
+        }
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* This will generate an RPA for both initiator addr and adva */
+    if (advsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+        ble_ll_adv_rpa_update(advsm);
+    }
+#endif
+
+    /* Set flag telling us that advertising is enabled */
+    advsm->adv_enabled = 1;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    advsm->event_cntr = 0;
+    access_addr = ble_ll_utils_calc_access_addr();
+    advsm->channel_id = ((access_addr & 0xffff0000) >> 16) ^
+                         (access_addr & 0x0000ffff);
+#endif
+
+    /* Determine the advertising interval we will use */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+        /* Set it to max. allowed for high duty cycle advertising */
+        advsm->adv_itvl_usecs = BLE_LL_ADV_PDU_ITVL_HD_MS_MAX;
+    } else {
+        advsm->adv_itvl_usecs = (uint32_t)advsm->adv_itvl_max;
+        advsm->adv_itvl_usecs *= BLE_LL_ADV_ITVL;
+    }
+
+    /* Set first advertising channel */
+    adv_chan = ble_ll_adv_first_chan(advsm);
+    advsm->adv_chan = adv_chan;
+
+    /*
+     * Scheduling 1st PDU is a bit tricky.
+     * Earliest possible start time is after RF is enabled so just force RF to
+     * start here to see when if will be fully enabled - it will be too early,
+     * but this is the only reliable way to have it enabled on time.
+     * Next we calculate expected start time (randomize it a bit) and this is
+     * used to setup start time for scheduler item.
+     * Then we check if start time for scheduler item (which includes scheduler
+     * overhead) is no earlier than calculated earliest possible start time and
+     * adjust scheduler item if necessary.
+     */
+    earliest_start_time = ble_ll_rfmgmt_enable_now();
+
+    start_delay_us = rand() % (BLE_LL_ADV_DELAY_MS_MAX * 1000);
+    advsm->adv_pdu_start_time = os_cputime_get32() +
+                                os_cputime_usecs_to_ticks(start_delay_us);
+
+    ble_ll_adv_set_sched(advsm);
+
+    delta = (int32_t)(advsm->adv_sch.start_time - earliest_start_time);
+    if (delta < 0) {
+        advsm->adv_sch.start_time -= delta;
+        advsm->adv_sch.end_time -= delta;
+    }
+
+    /* This does actual scheduling */
+    ble_ll_sched_adv_new(&advsm->adv_sch, ble_ll_adv_scheduled, NULL);
+
+    /* we start periodic before AE since we need PDU start time in SyncInfo */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (advsm->periodic_adv_enabled && !advsm->periodic_adv_active) {
+        ble_ll_adv_sm_start_periodic(advsm);
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) {
+        ble_ll_adv_aux_schedule(advsm);
+    }
+#endif
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Called when the LE HCI command read advertising channel tx power command
+ * has been received. Returns the current advertising transmit power.
+ *
+ * Context: Link Layer task (HCI command parser)
+ *
+ * @return int
+ */
+int
+ble_ll_adv_read_txpwr(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_adv_chan_txpwr_rp *rsp = (void *) rspbuf;
+
+    rsp->power_level = MYNEWT_VAL(BLE_LL_TX_PWR_DBM);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Turn advertising on/off.
+ *
+ * Context: Link Layer task
+ *
+ * @param cmd
+ *
+ * @return int
+ */
+static int
+ble_ll_adv_set_enable(uint8_t instance, uint8_t enable, int duration,
+                      uint8_t events)
+{
+    int rc;
+    struct ble_ll_adv_sm *advsm;
+
+    advsm = ble_ll_adv_sm_find_configured(instance);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    rc = BLE_ERR_SUCCESS;
+    if (enable == 1) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        if (ble_ll_hci_adv_mode_ext() &&
+                (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) &&
+                !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
+                SCAN_RSP_DATA_LEN(advsm) == 0) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        /* handle specifics of HD dir adv enabled in legacy way */
+        if (duration < 0) {
+            if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+                duration = BLE_LL_ADV_STATE_HD_MAX / 10;
+            } else {
+                duration = 0;
+            }
+        }
+        advsm->duration = duration;
+        advsm->events_max = events;
+        advsm->events = 0;
+#endif
+
+        /* If already enabled, do nothing */
+        if (!advsm->adv_enabled) {
+            /* Start the advertising state machine */
+            rc = ble_ll_adv_sm_start(advsm);
+        }
+    } else if (enable == 0) {
+        ble_ll_adv_sm_stop(advsm);
+    } else {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_hci_adv_set_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_adv_enable_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_adv_set_enable(0, cmd->enable, -1, 0);
+}
+
+static void
+ble_ll_adv_update_data_mbuf(struct os_mbuf **omp, bool new_data, uint16_t maxlen,
+                            const void *data, uint16_t datalen)
+{
+    struct os_mbuf *om;
+    int ret;
+
+    om = *omp;
+
+    if (new_data) {
+        if (om) {
+            os_mbuf_free_chain(om);
+        }
+
+        om = os_msys_get_pkthdr(datalen, 0);
+        if (!om) {
+            goto done;
+        }
+    }
+
+    assert(om);
+
+    if (OS_MBUF_PKTLEN(om) + datalen > maxlen) {
+        os_mbuf_free_chain(om);
+        om = NULL;
+        goto done;
+    }
+
+    ret = os_mbuf_append(om, data, datalen);
+    if (ret) {
+        os_mbuf_free_chain(om);
+        om = NULL;
+    }
+
+done:
+    *omp = om;
+}
+
+/**
+ * Set the scan response data that the controller will send.
+ *
+ * @param cmd
+ * @param len
+ *
+ * @return int
+ */
+static int
+ble_ll_adv_set_scan_rsp_data(const uint8_t *data, uint8_t datalen,
+                             uint8_t instance, uint8_t operation)
+{
+    struct ble_ll_adv_sm *advsm;
+    bool new_data;
+
+    advsm = ble_ll_adv_sm_find_configured(instance);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    /* check if type of advertising support scan rsp */
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) {
+        if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+
+    switch (operation) {
+    case BLE_HCI_LE_SET_DATA_OPER_COMPLETE:
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            if (datalen > BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN) {
+                return BLE_ERR_INV_HCI_CMD_PARMS;
+            }
+        }
+
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_HCI_LE_SET_DATA_OPER_LAST:
+        /* TODO mark scan rsp as complete? */
+        /* fall through */
+    case BLE_HCI_LE_SET_DATA_OPER_INT:
+        if (!advsm->scan_rsp_data) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        if (!datalen) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+        break;
+    case BLE_HCI_LE_SET_DATA_OPER_FIRST:
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        if (!datalen) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        break;
+#endif
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    new_data = (operation == BLE_HCI_LE_SET_DATA_OPER_COMPLETE) ||
+               (operation == BLE_HCI_LE_SET_DATA_OPER_FIRST);
+
+    if (advsm->adv_enabled) {
+        if (advsm->new_scan_rsp_data) {
+            ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA);
+            os_mbuf_free_chain(advsm->new_scan_rsp_data);
+            advsm->new_scan_rsp_data = NULL;
+        }
+
+        ble_ll_adv_update_data_mbuf(&advsm->new_scan_rsp_data, new_data,
+                                    BLE_ADV_DATA_MAX_LEN, data, datalen);
+        if (!advsm->new_scan_rsp_data) {
+            return BLE_ERR_MEM_CAPACITY;
+        }
+        ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA);
+    } else {
+        ble_ll_adv_update_data_mbuf(&advsm->scan_rsp_data, new_data,
+                                    BLE_SCAN_RSP_DATA_MAX_LEN, data, datalen);
+        if (!advsm->scan_rsp_data) {
+            return BLE_ERR_MEM_CAPACITY;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        /* DID shall be updated when host provides new scan response data */
+        ble_ll_adv_update_did(advsm);
+#endif
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_hci_set_scan_rsp_data(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_scan_rsp_data_cp *cmd = (const void *) cmdbuf;
+
+    if ((len != sizeof(*cmd)) || (cmd->scan_rsp_len > sizeof(cmd->scan_rsp))) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_adv_set_scan_rsp_data(cmd->scan_rsp, cmd->scan_rsp_len, 0,
+                                        BLE_HCI_LE_SET_DATA_OPER_COMPLETE);
+}
+/**
+ * Called by the LL HCI command parser when a set advertising
+ * data command has been sent from the host to the controller.
+ *
+ * @param cmd Pointer to command data
+ * @param len Length of command data
+ *
+ * @return int 0: success; BLE_ERR_INV_HCI_CMD_PARMS otherwise.
+ */
+static int
+ble_ll_adv_set_adv_data(const uint8_t *data, uint8_t datalen, uint8_t instance,
+                        uint8_t operation)
+{
+    struct ble_ll_adv_sm *advsm;
+    bool new_data;
+
+    advsm = ble_ll_adv_sm_find_configured(instance);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    /* check if type of advertising support adv data */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+            if (ble_ll_hci_adv_mode_ext()) {
+                return BLE_ERR_INV_HCI_CMD_PARMS;
+            }
+        }
+    } else {
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+
+    switch (operation) {
+    case BLE_HCI_LE_SET_DATA_OPER_COMPLETE:
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            if (datalen > BLE_ADV_LEGACY_DATA_MAX_LEN) {
+                return BLE_ERR_INV_HCI_CMD_PARMS;
+            }
+        }
+
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE);
+
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_HCI_LE_SET_DATA_OPER_UNCHANGED:
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (!advsm->adv_enabled || !ADV_DATA_LEN(advsm) || datalen) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        /* update DID only */
+        ble_ll_adv_update_did(advsm);
+        return BLE_ERR_SUCCESS;
+    case BLE_HCI_LE_SET_DATA_OPER_LAST:
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE);
+        /* fall through */
+    case BLE_HCI_LE_SET_DATA_OPER_INT:
+        if (!advsm->adv_data) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (!datalen) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+        break;
+    case BLE_HCI_LE_SET_DATA_OPER_FIRST:
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        if (!datalen) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE);
+        break;
+#endif
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    new_data = (operation == BLE_HCI_LE_SET_DATA_OPER_COMPLETE) ||
+               (operation == BLE_HCI_LE_SET_DATA_OPER_FIRST);
+
+    if (advsm->adv_enabled) {
+        if (advsm->new_adv_data) {
+            ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA);
+            os_mbuf_free_chain(advsm->new_adv_data);
+            advsm->new_adv_data = NULL;
+        }
+
+        ble_ll_adv_update_data_mbuf(&advsm->new_adv_data, new_data,
+                                    BLE_ADV_DATA_MAX_LEN, data, datalen);
+        if (!advsm->new_adv_data) {
+            return BLE_ERR_MEM_CAPACITY;
+        }
+        ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA);
+    } else {
+        ble_ll_adv_update_data_mbuf(&advsm->adv_data, new_data,
+                                    BLE_ADV_DATA_MAX_LEN, data, datalen);
+        if (!advsm->adv_data) {
+            return BLE_ERR_MEM_CAPACITY;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        /* DID shall be updated when host provides new advertising data */
+        ble_ll_adv_update_did(advsm);
+#endif
+        }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_hci_set_adv_data(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_adv_data_cp *cmd = (const void *) cmdbuf;
+
+    if ((len != sizeof(*cmd)) || (cmd->adv_data_len > sizeof(cmd->adv_data))) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_adv_set_adv_data(cmd->adv_data, cmd->adv_data_len, 0,
+                                   BLE_HCI_LE_SET_DATA_OPER_COMPLETE);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static bool
+pri_phy_valid(uint8_t phy)
+{
+    switch (phy) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_HCI_LE_PHY_CODED:
+#endif
+    case BLE_HCI_LE_PHY_1M:
+        return true;
+    default:
+        return false;
+    }
+}
+
+static bool
+sec_phy_valid(uint8_t phy)
+{
+    switch (phy) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_HCI_LE_PHY_CODED:
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_HCI_LE_PHY_2M:
+#endif
+    case BLE_HCI_LE_PHY_1M:
+        return true;
+    default:
+        return false;
+    }
+}
+
+static struct ble_ll_adv_sm *
+ble_ll_adv_sm_get(uint8_t instance)
+{
+    struct ble_ll_adv_sm *advsm;
+    int i;
+
+    advsm = ble_ll_adv_sm_find_configured(instance);
+    if (advsm) {
+        return advsm;
+    }
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_adv_sm); i++) {
+        advsm = &g_ble_ll_adv_sm[i];
+
+        if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONFIGURED)) {
+            ble_ll_adv_sm_init(advsm);
+
+           /* configured flag is set by caller on success config */
+           advsm->adv_instance = instance;
+           return advsm;
+        }
+    }
+
+    return NULL;
+}
+
+int
+ble_ll_adv_ext_set_param(const uint8_t *cmdbuf, uint8_t len,
+                         uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_set_ext_adv_params_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_set_ext_adv_params_rp *rsp = (void *) rspbuf;
+    struct ble_ll_adv_sm *advsm;
+    uint32_t adv_itvl_min;
+    uint32_t adv_itvl_max;
+    uint16_t props;
+    int rc;
+
+    if (len != sizeof(*cmd )) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    advsm = ble_ll_adv_sm_get(cmd->adv_handle);
+    if (!advsm) {
+        rc = BLE_ERR_MEM_CAPACITY;
+        goto done;
+    }
+
+    if (advsm->adv_enabled) {
+        rc = BLE_ERR_CMD_DISALLOWED;
+        goto done;
+    }
+
+    props = le16toh(cmd->props);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    /* If the Host issues this command when periodic advertising is enabled for
+     * the specified advertising set and connectable, scannable, legacy, or
+     * anonymous advertising is specified, the Controller shall return the
+     * error code Invalid HCI Command Parameters (0x12).
+     */
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED) {
+        if (advsm->periodic_adv_enabled) {
+            if (props & (BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE |
+                         BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE |
+                         BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY |
+                         BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) {
+                rc = BLE_ERR_INV_HCI_CMD_PARMS;
+                goto done;
+            }
+        }
+    }
+#endif
+
+    adv_itvl_min = cmd->pri_itvl_min[2] << 16 | cmd->pri_itvl_min[1] << 8 |
+                   cmd->pri_itvl_min[0];
+    adv_itvl_max = cmd->pri_itvl_max[2] << 16 | cmd->pri_itvl_max[1] << 8 |
+                   cmd->pri_itvl_max[0];
+
+    if (props & ~BLE_HCI_LE_SET_EXT_ADV_PROP_MASK) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        if (ADV_DATA_LEN(advsm) > BLE_ADV_LEGACY_DATA_MAX_LEN ||
+            SCAN_RSP_DATA_LEN(advsm) > BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN) {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+            goto done;
+        }
+
+        /* if legacy bit is set possible values are limited */
+        switch (props) {
+        case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_IND:
+        case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_LD_DIR:
+        case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_HD_DIR:
+        case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_SCAN:
+        case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_NONCONN:
+            break;
+        default:
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+            goto done;
+        }
+    } else {
+        /* HD directed advertising allowed only on legacy PDUs */
+        if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+            goto done;
+        }
+
+        /* if ext advertising PDUs are used then it shall not be both
+         * connectable and scanable
+         */
+        if ((props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) &&
+            (props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+            goto done;
+        }
+    }
+
+    /* High Duty Directed advertising is special */
+    if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+        if (ADV_DATA_LEN(advsm) || SCAN_RSP_DATA_LEN(advsm)) {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+            goto done;
+        }
+
+        /* Ignore min/max interval */
+        adv_itvl_min = 0;
+        adv_itvl_max = 0;
+    } else {
+        /* validate intervals for non HD-directed advertising */
+        if ((adv_itvl_min > adv_itvl_max) ||
+                (adv_itvl_min < BLE_HCI_ADV_ITVL_MIN) ||
+                (adv_itvl_max < BLE_HCI_ADV_ITVL_MIN)) {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+            goto done;
+        }
+
+        /* TODO for now limit those to values from legacy advertising
+         *
+         * If the primary advertising interval range is outside the advertising
+         * interval range supported by the Controller, then the Controller shall
+         * return the error code Unsupported Feature or Parameter Value (0x11).
+         */
+        if ((adv_itvl_min > BLE_HCI_ADV_ITVL_MAX) ||
+                (adv_itvl_max > BLE_HCI_ADV_ITVL_MAX)) {
+            rc = BLE_ERR_UNSUPPORTED;
+            goto done;
+        }
+    }
+
+    /* There are only three adv channels, so check for any outside the range */
+    if (((cmd->pri_chan_map & 0xF8) != 0) || (cmd->pri_chan_map == 0)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* If we dont support privacy some address types wont work */
+    if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+        rc = BLE_ERR_UNSUPPORTED;
+        goto done;
+    }
+#endif
+
+    /* peer address type is only valid for directed */
+    if ((props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) &&
+            (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    /* Check filter policy (valid only for undirected) */
+    if (!(props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) &&
+         cmd->filter_policy > BLE_HCI_ADV_FILT_MAX) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (!pri_phy_valid(cmd->pri_phy)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    /* check secondary phy only if not using legacy PDUs */
+    if (!(props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
+            !sec_phy_valid(cmd->sec_phy)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (cmd->sid > 0x0f) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (cmd->scan_req_notif > 0x01) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    rc = BLE_ERR_SUCCESS;
+
+    if (cmd->tx_power == 127) {
+        /* no preference */
+        advsm->adv_txpwr = MYNEWT_VAL(BLE_LL_TX_PWR_DBM);
+    } else {
+        advsm->adv_txpwr = ble_phy_txpower_round(cmd->tx_power);
+    }
+
+    /* we can always store as those are validated and used only when needed */
+    advsm->peer_addr_type = cmd->peer_addr_type;
+    memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+    advsm->own_addr_type = cmd->own_addr_type;
+    advsm->adv_filter_policy = cmd->filter_policy;
+    advsm->adv_chanmask = cmd->pri_chan_map;
+    advsm->adv_itvl_min = adv_itvl_min;
+    advsm->adv_itvl_max = adv_itvl_max;
+    advsm->pri_phy = cmd->pri_phy;
+    advsm->sec_phy = cmd->sec_phy;
+    /* Update SID only */
+    advsm->adi = (advsm->adi & 0x0fff) | ((cmd->sid << 12));
+
+    advsm->props = props;
+
+    /* Set proper mbuf chain for aux data */
+    if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        advsm->aux_data = NULL;
+    } else if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+        advsm->aux_data = &advsm->scan_rsp_data;
+    } else {
+        advsm->aux_data = &advsm->adv_data;
+    }
+
+    if (cmd->scan_req_notif) {
+        ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF);
+    } else {
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF);
+    }
+
+    ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_CONFIGURED);
+
+done:
+    /* Update TX power */
+    rsp->tx_power = rc ? 0 : advsm->adv_txpwr;
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+int
+ble_ll_adv_ext_set_adv_data(const uint8_t *cmdbuf, uint8_t cmdlen)
+{
+    const struct ble_hci_le_set_ext_adv_data_cp *cmd = (const void *) cmdbuf;
+
+    if (cmdlen < sizeof(*cmd )) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->adv_data_len > BLE_HCI_MAX_EXT_ADV_DATA_LEN ||
+            cmd->adv_data_len > cmdlen - sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* TODO fragment preference ignored for now */
+
+    return ble_ll_adv_set_adv_data(cmd->adv_data, cmd->adv_data_len,
+                                   cmd->adv_handle, cmd->operation);
+}
+
+int
+ble_ll_adv_ext_set_scan_rsp(const uint8_t *cmdbuf, uint8_t cmdlen)
+{
+    const struct ble_hci_le_set_ext_scan_rsp_data_cp *cmd = (const void *) cmdbuf;
+
+    if (cmdlen < sizeof(*cmd )) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->scan_rsp_len > BLE_HCI_MAX_EXT_ADV_DATA_LEN ||
+            cmd->scan_rsp_len > cmdlen - sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* TODO fragment preference ignored for now */
+
+    return ble_ll_adv_set_scan_rsp_data(cmd->scan_rsp, cmd->scan_rsp_len,
+                                        cmd->adv_handle, cmd->operation);
+}
+
+/**
+ * HCI LE extended advertising enable command
+ *
+ * @param cmd Pointer to command data
+ * @param len Command data length
+ *
+ * @return int BLE error code
+ */
+int
+ble_ll_adv_ext_set_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_ext_adv_enable_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_adv_sm *advsm;
+    int i, j, rc;
+
+    if (len < sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* check if length is correct */
+    if (len != 2 + (cmd->num_sets * sizeof(cmd->sets[0]))) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->num_sets > BLE_ADV_INSTANCES) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->num_sets == 0) {
+        if (cmd->enable) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        /* disable all instances */
+        for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+            ble_ll_adv_set_enable(i, 0, 0, 0);
+        }
+
+        return BLE_ERR_SUCCESS;
+    }
+
+    /* validate instances */
+    for (i = 0; i < cmd->num_sets; i++) {
+        /* validate duplicated sets */
+        for (j = i + 1; j < cmd->num_sets; j++) {
+            if (cmd->sets[i].adv_handle == cmd->sets[j].adv_handle) {
+                return BLE_ERR_INV_HCI_CMD_PARMS;
+            }
+        }
+
+        advsm = ble_ll_adv_sm_find_configured(cmd->sets[i].adv_handle);
+        if (!advsm) {
+            return BLE_ERR_UNK_ADV_INDENT;
+        }
+
+        if (cmd->enable) {
+            if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+                if (cmd->sets[i].duration == 0 ||
+                        le16toh(cmd->sets[i].duration) > 128) {
+                    return BLE_ERR_INV_HCI_CMD_PARMS;
+                }
+            }
+        }
+    }
+
+    for (i = 0; i < cmd->num_sets; i++) {
+        rc = ble_ll_adv_set_enable(cmd->sets[i].adv_handle, cmd->enable,
+                                   le16toh(cmd->sets[i].duration),
+                                   cmd->sets[i].max_events);
+        if (rc) {
+            return rc;
+        }
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_adv_set_random_addr(const uint8_t *addr, uint8_t instance)
+{
+    struct ble_ll_adv_sm *advsm;
+
+    advsm = ble_ll_adv_sm_find_configured(instance);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    /*
+     * Reject if connectable advertising is on
+     * Core Spec Vol. 2 Part E 7.8.52
+     */
+    if (advsm->adv_enabled &&
+            (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    memcpy(advsm->adv_random_addr, addr, BLE_DEV_ADDR_LEN);
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_adv_hci_set_random_addr(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_adv_set_rnd_addr_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_adv_set_random_addr(cmd->addr, cmd->adv_handle);
+}
+
+/**
+ * HCI LE extended advertising remove command
+ *
+ * @return int BLE error code
+ */
+int
+ble_ll_adv_remove(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_remove_adv_set_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_adv_sm *advsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    if (advsm->adv_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (advsm->periodic_adv_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (advsm->periodic_adv_data) {
+        os_mbuf_free_chain(advsm->periodic_adv_data);
+    }
+#endif
+
+    if (advsm->adv_data) {
+        os_mbuf_free_chain(advsm->adv_data);
+    }
+    if (advsm->scan_rsp_data) {
+        os_mbuf_free_chain(advsm->scan_rsp_data);
+    }
+
+    ble_ll_adv_sm_init(advsm);
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * HCI LE extended advertising clear command
+ *
+ * @return int BLE error code
+ */
+int
+ble_ll_adv_clear_all(void)
+{
+    int i;
+
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        if (g_ble_ll_adv_sm[i].adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (g_ble_ll_adv_sm[i].periodic_adv_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+#endif
+    }
+
+    ble_ll_adv_reset();
+
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+static uint16_t
+ble_ll_adv_sync_get_pdu_len(uint16_t data_len, uint16_t *data_offset,
+                            uint16_t props)
+{
+    uint16_t rem_data_len = data_len - *data_offset;
+    uint8_t hdr_len = BLE_LL_EXT_ADV_HDR_LEN;
+    uint8_t ext_hdr = 0;
+
+    /* TxPower if configured
+     * Note: TxPower shall not be present in chain PDU for SYNC
+     */
+    if (*data_offset == 0 &&
+        (props & BLE_HCI_LE_SET_PERIODIC_ADV_PROP_INC_TX_PWR)) {
+        ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
+        hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    /* if we have any fields in ext header we need to add flags, note that Aux
+     * PTR is handled later and it will account for flags if needed
+     *
+     * This could be handled inside TxPower but lets keep code consistent with
+     * how Aux calculate works and this also make it easier to add more fields
+     * into flags if needed in future
+     */
+    if (ext_hdr) {
+        hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE;
+    }
+
+    /* AdvData always */
+    data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, rem_data_len);
+
+    /* AuxPtr if there are more AdvData remaining that we can fit here */
+    if (rem_data_len > data_len) {
+            /* adjust for flags that needs to be added if AuxPtr is only field
+             * in Extended Header
+             */
+            if (!ext_hdr) {
+                hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE;
+                data_len -= BLE_LL_EXT_ADV_FLAGS_SIZE;
+            }
+
+            hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+            data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+
+            /* PDU payload should be full if chained */
+            BLE_LL_ASSERT(hdr_len + data_len == BLE_LL_MAX_PAYLOAD_LEN);
+    }
+
+    *data_offset += data_len;
+
+    return hdr_len + data_len;
+}
+
+static bool
+ble_ll_adv_periodic_check_data_itvl(uint16_t payload_len, uint16_t props,
+                                    uint16_t itvl, uint8_t phy)
+{
+    uint32_t max_usecs = 0;
+    uint32_t itvl_usecs;
+    uint16_t offset = 0;
+    uint16_t pdu_len;
+
+    while (offset < payload_len) {
+        pdu_len = ble_ll_adv_sync_get_pdu_len(payload_len, &offset, props);
+
+        max_usecs += ble_ll_pdu_tx_time_get(pdu_len, phy);
+        max_usecs += ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS +
+                                MYNEWT_VAL(BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY));
+    }
+
+    itvl_usecs = (uint32_t)itvl * BLE_LL_ADV_PERIODIC_ITVL;
+
+    return max_usecs < itvl_usecs;
+}
+
+int
+ble_ll_adv_periodic_set_param(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_periodic_adv_params_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_adv_sm *advsm;
+    uint16_t adv_itvl_min;
+    uint16_t adv_itvl_max;
+    uint16_t props;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    adv_itvl_min = le16toh(cmd->min_itvl);
+    adv_itvl_max = le16toh(cmd->max_itvl);
+    props = le16toh(cmd->props);
+
+    advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    /* If the advertising set identified by the Advertising_Handle specified
+     * scannable, connectable, legacy, or anonymous advertising, the Controller
+     * shall return the error code Invalid HCI Command Parameters (0x12).
+     */
+    if (advsm->props & (BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV |
+                        BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE |
+                        BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE |
+                        BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If the Host issues this command when periodic advertising is enabled for
+     * the specified advertising set, the Controller shall return the error code
+     * Command Disallowed (0x0C).
+     */
+    if (advsm->periodic_adv_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* validate intervals */
+    if ((adv_itvl_min < 0x0006) || (adv_itvl_max < 0x006) ||
+            (adv_itvl_min > adv_itvl_max)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* validate properties */
+    if (props & ~BLE_HCI_LE_SET_PERIODIC_ADV_PROP_MASK) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If the advertising set already contains periodic advertising data and the
+     * length of the data is greater than the maximum that the Controller can
+     * transmit within a periodic advertising interval of
+     * Periodic_Advertising_Interval_Max, the Controller shall return the error
+     * code Packet Too Long (0x45).
+     */
+    if (!ble_ll_adv_periodic_check_data_itvl(SYNC_DATA_LEN(advsm), props,
+                                             adv_itvl_max, advsm->sec_phy)) {
+        return BLE_ERR_PACKET_TOO_LONG;
+    }
+
+    advsm->periodic_adv_itvl_min = adv_itvl_min;
+    advsm->periodic_adv_itvl_max = adv_itvl_max;
+    advsm->periodic_adv_props = props;
+
+    ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_adv_periodic_set_data(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_periodic_adv_data_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_adv_sm *advsm;
+    uint16_t payload_total_len;
+    bool new_data = false;
+
+    if (len < sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->adv_data_len > BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN ||
+            cmd->adv_data_len != len - sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    switch (cmd->operation) {
+    case BLE_HCI_LE_SET_DATA_OPER_LAST:
+    case BLE_HCI_LE_SET_DATA_OPER_INT:
+        if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (!advsm->periodic_adv_data || !cmd->adv_data_len) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (advsm->periodic_adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+        break;
+    case BLE_HCI_LE_SET_DATA_OPER_FIRST:
+        if (advsm->periodic_adv_enabled) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        if (!cmd->adv_data_len) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+        new_data = true;
+        break;
+    case BLE_HCI_LE_SET_DATA_OPER_COMPLETE:
+        new_data = true;
+        break;
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    payload_total_len = cmd->adv_data_len;
+    if (!new_data) {
+        payload_total_len += SYNC_DATA_LEN(advsm);
+    }
+
+    /* If the combined length of the data is greater than the maximum that the
+     * Controller can transmit within the current periodic advertising interval
+     * (if periodic advertising is currently enabled) or the
+     * Periodic_Advertising_Interval_Max for the advertising set (if currently
+     * disabled), all the data shall be discarded and the Controller shall
+     * return the error code Packet Too Long (0x45).
+     */
+    if (!ble_ll_adv_periodic_check_data_itvl(payload_total_len,
+                                             advsm->periodic_adv_props,
+                                             advsm->periodic_adv_itvl_max,
+                                             advsm->sec_phy)) {
+        return BLE_ERR_PACKET_TOO_LONG;
+    }
+
+    if (advsm->periodic_adv_active) {
+        ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA);
+
+        ble_ll_adv_update_data_mbuf(&advsm->periodic_new_data, true,
+                                    BLE_ADV_DATA_MAX_LEN,
+                                    cmd->adv_data, cmd->adv_data_len);
+        if (!advsm->periodic_new_data) {
+            return BLE_ERR_MEM_CAPACITY;
+        }
+
+        ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA);
+    } else {
+        ble_ll_adv_update_data_mbuf(&advsm->periodic_adv_data, new_data,
+                                    BLE_ADV_DATA_MAX_LEN, cmd->adv_data,
+                                    cmd->adv_data_len);
+        if (!advsm->periodic_adv_data) {
+            return BLE_ERR_MEM_CAPACITY;
+        }
+    }
+
+    /* set/clear incomplete data flag only on success */
+    switch (cmd->operation) {
+    case BLE_HCI_LE_SET_DATA_OPER_LAST:
+    case BLE_HCI_LE_SET_DATA_OPER_COMPLETE:
+        ble_ll_adv_flags_clear(advsm,
+                               BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE);
+        break;
+    case BLE_HCI_LE_SET_DATA_OPER_INT:
+    case BLE_HCI_LE_SET_DATA_OPER_FIRST:
+    default:
+        ble_ll_adv_flags_set(advsm,
+                             BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE);
+        break;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_periodic_adv_enable_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_adv_sm *advsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle);
+    if (!advsm) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    if (cmd->enable) {
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        /* If Enable is set to 0x01 and the length of the periodic advertising
+         * data is greater than the maximum that the Controller can transmit
+         * within the chosen periodicadvertising interval, the Controller shall
+         * return the error code Packet Too Long (0x45).
+         */
+        if (!ble_ll_adv_periodic_check_data_itvl(SYNC_DATA_LEN(advsm),
+                                                 advsm->periodic_adv_props,
+                                                 advsm->periodic_adv_itvl_max,
+                                                 advsm->sec_phy)) {
+            return BLE_ERR_PACKET_TOO_LONG;
+        }
+
+        /* If the advertising set is not currently enabled (see the
+         * LE_Set_Extended_Advertising_Enable command), the periodic advertising
+         * is not started until the advertising set is enabled.
+         */
+        if (advsm->adv_enabled && !advsm->periodic_adv_active) {
+            /* Start the periodic advertising state machine */
+            ble_ll_adv_sm_start_periodic(advsm);
+        }
+    } else {
+        /* Stop the periodic advertising state machine */
+        ble_ll_adv_sm_stop_periodic(advsm);
+    }
+
+    advsm->periodic_adv_enabled = cmd->enable;
+
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+static int
+ble_ll_adv_periodic_send_sync_ind(struct ble_ll_adv_sm *advsm,
+                                  struct ble_ll_conn_sm *connsm,
+                                  uint16_t service_data)
+{
+    struct os_mbuf *om;
+    uint8_t *sync_ind;
+
+    om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+                            sizeof(struct ble_mbuf_hdr));
+    if (!om) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    om->om_data[0] = BLE_LL_CTRL_PERIODIC_SYNC_IND;
+
+    sync_ind = om->om_data + 1;
+
+    /* ID (service_data), already in LE order */
+    memcpy(sync_ind, &service_data, sizeof(service_data));
+
+    /* fill in syncinfo */
+    ble_ll_adv_put_syncinfo(advsm, connsm, sync_ind + 20, sync_ind + 2);
+
+    /* lastPaEventCounter */
+    put_le16(sync_ind + 22, advsm->periodic_event_cntr_last_sent);
+
+    /* SID, AType, SCA */
+    sync_ind[24] = (advsm->adi >> 12);
+    sync_ind[24] |= !!(advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) << 4 ;
+    sync_ind[24] |= MYNEWT_VAL(BLE_LL_MASTER_SCA) << 5;
+
+    /* PHY */
+    sync_ind[25] = (0x01 << (advsm->sec_phy - 1));
+
+    /* AdvA */
+    memcpy(sync_ind + 26, advsm->adva, BLE_DEV_ADDR_LEN);
+
+    /* syncConnEventCount */
+    put_le16(sync_ind + 32, connsm->event_cntr);
+
+    ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL,
+                            BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN + 1);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_adv_periodic_set_info_transfer(const uint8_t *cmdbuf, uint8_t len,
+                                      uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_periodic_adv_set_info_transfer_cp *cmd = (const void *)cmdbuf;
+     struct ble_hci_le_periodic_adv_set_info_transfer_rp *rsp = (void *) rspbuf;
+     struct ble_ll_conn_sm *connsm;
+     struct ble_ll_adv_sm *advsm;
+     uint16_t handle;
+     int rc;
+
+     if (len != sizeof(*cmd)) {
+         rc = BLE_ERR_INV_HCI_CMD_PARMS;
+         goto done;
+     }
+
+     advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle);
+     if (!advsm) {
+         rc = BLE_ERR_UNK_ADV_INDENT;
+         goto done;
+     }
+
+     if (!advsm->periodic_adv_active) {
+         rc = BLE_ERR_CMD_DISALLOWED;
+         goto done;
+     }
+
+     handle = le16toh(cmd->conn_handle);
+     if (handle > 0xeff) {
+         rc = BLE_ERR_INV_HCI_CMD_PARMS;
+         goto done;
+     }
+
+     connsm = ble_ll_conn_find_active_conn(handle);
+     if (!connsm) {
+         rc = BLE_ERR_UNK_CONN_ID;
+         goto done;
+     }
+
+     /* TODO should not need to shift
+      * byte 3 (0 byte is conn_feature) , bit 1
+      *
+      * Allow initiate LL procedure only if remote supports it.
+      */
+     if (!(connsm->remote_features[2] & (BLE_LL_FEAT_SYNC_TRANS_RECV >> (8 * 3)))) {
+         rc = BLE_ERR_UNSUPP_REM_FEATURE;
+         goto done;
+     }
+
+     rc = ble_ll_adv_periodic_send_sync_ind(advsm, connsm, cmd->service_data);
+ done:
+     rsp->conn_handle = cmd->conn_handle;
+     *rsplen = sizeof(*rsp);
+     return rc;
+}
+#endif
+#endif
+#endif
+
+/**
+ * Says whether the specified address is already connected or not.
+ * @param   [in]    addr        The peer address.
+ * @param   [in]    addr_type   Public address (0) or random address (1).
+ * @return  Return 1 if already connected, 0 otherwise.
+ */
+static int
+ble_ll_adv_already_connected(const uint8_t* addr, uint8_t addr_type)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    /* extracted from ble_ll_conn_slave_start function */
+    SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) {
+        if (!memcmp(&connsm->peer_addr, addr, BLE_DEV_ADDR_LEN)) {
+            if (addr_type == BLE_ADDR_RANDOM) {
+                if (connsm->peer_addr_type & 1) {
+                    return 1;
+                }
+            } else {
+                if ((connsm->peer_addr_type & 1) == 0) {
+                    return 1;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Called when the LL receives a scan request or connection request
+ *
+ * Context: Called from interrupt context.
+ *
+ * @param rxbuf
+ *
+ * @return -1: request not for us or is a connect request.
+ *          0: request (scan) is for us and we successfully went from rx to tx.
+ *        > 0: PHY error attempting to go from rx to tx.
+ */
+static int
+ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu)
+{
+    int rc;
+    int resolved;
+    uint8_t chk_wl;
+    uint8_t txadd;
+    uint8_t peer_addr_type;
+    uint8_t *rxbuf;
+    uint8_t *adva;
+    uint8_t *peer;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct ble_ll_adv_sm *advsm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct aux_conn_rsp_data rsp_data;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl;
+#endif
+
+    /* See if adva in the request (scan or connect) matches what we sent */
+    advsm = g_ble_ll_cur_adv_sm;
+    rxbuf = rxpdu->om_data;
+    adva = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN;
+    if (memcmp(advsm->adva, adva, BLE_DEV_ADDR_LEN)) {
+        return -1;
+    }
+
+    /* Set device match bit if we are whitelisting */
+    if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) {
+        chk_wl = advsm->adv_filter_policy & 1;
+    } else {
+        chk_wl = advsm->adv_filter_policy & 2;
+    }
+
+    /* Get the peer address type */
+    if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) {
+        txadd = BLE_ADDR_RANDOM;
+    } else {
+        txadd = BLE_ADDR_PUBLIC;
+    }
+
+    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+    peer = rxbuf + BLE_LL_PDU_HDR_LEN;
+    peer_addr_type = txadd;
+    resolved = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    rl = NULL;
+    if (ble_ll_resolv_enabled()) {
+        if (ble_ll_is_rpa(peer, txadd)) {
+            advsm->adv_rpa_index = ble_hw_resolv_list_match();
+            if (advsm->adv_rpa_index >= 0) {
+                ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_RESOLVED;
+                rl = &g_ble_ll_resolv_list[advsm->adv_rpa_index];
+                if (chk_wl) {
+                    peer = rl->rl_identity_addr;
+                    peer_addr_type = rl->rl_addr_type;
+                    resolved = 1;
+                }
+            } else {
+                if (chk_wl) {
+                    return -1;
+                }
+            }
+        } else {
+            /* Verify privacy mode */
+            rl = ble_ll_resolv_list_find(peer, peer_addr_type);
+            if (rl && (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) &&
+                rl->rl_has_peer) {
+                return -1;
+            }
+        }
+    }
+#endif
+
+    /* Set device match bit if we are whitelisting */
+    if (chk_wl && !ble_ll_whitelist_match(peer, peer_addr_type, resolved)) {
+        return -1;
+    }
+
+    /*
+     * We set the device match bit to tell the upper layer that we will
+     * accept the request
+     */
+    ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH;
+
+    /* Setup to transmit the scan response if appropriate */
+    rc = -1;
+
+    if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) {
+        /* PHY used for scan requests shall be the same as the PHY used for the
+         * PDU that they reply to so no need to change PHY mode.
+         */
+        ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF) {
+            ble_ll_hci_ev_send_scan_req_recv(advsm->adv_instance, peer,
+                                             peer_addr_type);
+        }
+
+        /*
+         * We need to store current rxed packet header temporarily so AuxPtr
+         * can be calculated (if necessary) relative to AUX_SCAN_RSP instead of
+         * AUX_ADV_IND.
+         */
+
+        advsm->rx_ble_hdr = ble_hdr;
+        rc = ble_phy_tx(ble_ll_adv_scan_rsp_pdu_make, advsm,
+                        BLE_PHY_TRANSITION_NONE);
+        advsm->rx_ble_hdr = NULL;
+#else
+        rc = ble_phy_tx(ble_ll_adv_scan_rsp_legacy_pdu_make, advsm,
+                        BLE_PHY_TRANSITION_NONE);
+#endif
+
+        if (!rc) {
+            ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_TXD;
+            STATS_INC(ble_ll_stats, scan_rsp_txg);
+        }
+    } else if (pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ) {
+        /* See if the device is already connected */
+        if (ble_ll_adv_already_connected(peer, peer_addr_type)) {
+            return -1;
+        }
+
+        /*
+         * Only accept connect requests from the desired address if we
+         * are doing directed advertising
+         */
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+            if (memcmp(advsm->initiator_addr, peer, BLE_DEV_ADDR_LEN)) {
+                return -1;
+            }
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+            return -1;
+        }
+
+        /* use remote address used over the air */
+        rsp_data.advsm = advsm;
+        rsp_data.peer = rxbuf + BLE_LL_PDU_HDR_LEN;
+        rsp_data.rxadd = rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK;
+
+        ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
+        rc = ble_phy_tx(ble_ll_adv_aux_conn_rsp_pdu_make, &rsp_data,
+                        BLE_PHY_TRANSITION_NONE);
+        if (!rc) {
+            ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD);
+            STATS_INC(ble_ll_stats, aux_conn_rsp_tx);
+        }
+#endif
+    }
+
+    return rc;
+}
+
+/**
+ * Called when a connect request has been received.
+ *
+ * Context: Link Layer
+ *
+ * @param rxbuf
+ * @param flags
+ *
+ * @return 0: no connection started. 1: connection started
+ */
+static int
+ble_ll_adv_conn_req_rxd(uint8_t *rxbuf, struct ble_mbuf_hdr *hdr,
+                        struct ble_ll_adv_sm *advsm)
+{
+    int valid;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    uint8_t resolved;
+#endif
+    uint8_t addr_type;
+    uint8_t *inita;
+    uint8_t *ident_addr;
+
+    /* Don't create connection if AUX_CONNECT_RSP was not send */
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) {
+        if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD)) {
+            return 0;
+        }
+    }
+
+    /* Check filter policy. */
+    valid = 0;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    resolved = BLE_MBUF_HDR_RESOLVED(hdr);
+#endif
+    inita = rxbuf + BLE_LL_PDU_HDR_LEN;
+    if (hdr->rxinfo.flags & BLE_MBUF_HDR_F_DEVMATCH) {
+
+        valid = 1;
+        if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) {
+            addr_type = BLE_ADDR_RANDOM;
+        } else {
+            addr_type = BLE_ADDR_PUBLIC;
+        }
+
+        /*
+         * Only accept connect requests from the desired address if we
+         * are doing directed advertising
+         */
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+            ident_addr = inita;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+            if (resolved) {
+                ident_addr = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_identity_addr;
+                addr_type = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_addr_type;
+            }
+#endif
+            if ((addr_type != advsm->peer_addr_type) ||
+                memcmp(advsm->peer_addr, ident_addr, BLE_DEV_ADDR_LEN)) {
+                valid = 0;
+            }
+        }
+    }
+
+    if (valid) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (resolved) {
+            /* Retain the resolvable private address that we received. */
+            memcpy(advsm->adv_rpa, inita, BLE_DEV_ADDR_LEN);
+
+            /* Update resolving list with current peer RPA */
+            ble_ll_resolv_set_peer_rpa(advsm->adv_rpa_index, inita);
+
+            /*
+             * Overwrite received inita with identity address since that
+             * is used from now on.
+             */
+            memcpy(inita,
+                   g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_identity_addr,
+                   BLE_DEV_ADDR_LEN);
+
+            /* Peer address type is an identity address */
+            addr_type = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_addr_type;
+            addr_type += 2;
+        }
+#endif
+
+        /* Try to start slave connection. If successful, stop advertising */
+        valid = ble_ll_conn_slave_start(rxbuf, addr_type, hdr,
+                          !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY));
+        if (valid) {
+            /* stop advertising only if not transmitting connection response */
+            if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD)) {
+                ble_ll_adv_sm_stop(advsm);
+            }
+        }
+    }
+
+    return valid;
+}
+
+/**
+ * Called on phy rx pdu end when in advertising state.
+ *
+ * There are only two pdu types we care about in this state: scan requests
+ * and connection requests. When we receive a scan request we must determine if
+ * we need to send a scan response and that needs to be acted on within T_IFS.
+ *
+ * When we receive a connection request, we need to determine if we will allow
+ * this device to start a connection with us. However, no immediate response is
+ * sent so we handle this at the link layer task.
+ *
+ * Context: Interrupt
+ *
+ * @param pdu_type Type of pdu received.
+ * @param rxpdu Pointer to received PDU
+ *
+ * @return int
+ *       < 0: Disable the phy after reception.
+ *      == 0: Do not disable the PHY
+ *       > 0: Do not disable PHY as that has already been done.
+ */
+int
+ble_ll_adv_rx_isr_end(uint8_t pdu_type, struct os_mbuf *rxpdu, int crcok)
+{
+    int rc;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_mbuf_hdr *rxhdr;
+#endif
+
+    rc = -1;
+    if (rxpdu == NULL) {
+        ble_ll_adv_tx_done(g_ble_ll_cur_adv_sm);
+    } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        rxhdr = BLE_MBUF_HDR_PTR(rxpdu);
+        rxhdr->rxinfo.user_data = g_ble_ll_cur_adv_sm;
+        if (ble_ll_adv_active_chanset_is_sec(g_ble_ll_cur_adv_sm)) {
+            rxhdr->rxinfo.flags |= BLE_MBUF_HDR_F_EXT_ADV_SEC;
+        } else {
+            assert(ble_ll_adv_active_chanset_is_pri(g_ble_ll_cur_adv_sm));
+        }
+#endif
+        if (crcok) {
+            if ((pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) ||
+                (pdu_type == BLE_ADV_PDU_TYPE_CONNECT_IND)) {
+                /* Process request */
+                rc = ble_ll_adv_rx_req(pdu_type, rxpdu);
+            }
+        }
+
+        if (rc) {
+            /* We no longer have a current state machine */
+            g_ble_ll_cur_adv_sm = NULL;
+        }
+    }
+
+    if (rc) {
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    }
+
+    return rc;
+}
+
+/**
+ * Process a received packet at the link layer task when in the advertising
+ * state
+ *
+ * Context: Link Layer
+ *
+ *
+ * @param ptype
+ * @param rxbuf
+ * @param hdr
+ *
+ * @return int
+ */
+void
+ble_ll_adv_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf, struct ble_mbuf_hdr *hdr)
+{
+    int adv_event_over;
+    struct ble_ll_adv_sm *advsm;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    advsm = (struct ble_ll_adv_sm *)hdr->rxinfo.user_data;
+#else
+    advsm = &g_ble_ll_adv_sm[0];
+#endif
+
+    /*
+     * It is possible that advertising was stopped and a packet plcaed on the
+     * LL receive packet queue. In this case, just ignore the received packet
+     * as the advertising state machine is no longer "valid"
+     */
+    if (!advsm->adv_enabled) {
+        return;
+    }
+
+    /*
+     * If we have received a scan request and we are transmitting a response
+     * or we have received a valid connect request, dont "end" the advertising
+     * event. In the case of a connect request we will stop advertising. In
+     * the case of the scan response transmission we will get a transmit
+     * end callback.
+     */
+    adv_event_over = 1;
+    if (BLE_MBUF_HDR_CRC_OK(hdr)) {
+        if (ptype == BLE_ADV_PDU_TYPE_CONNECT_IND) {
+            if (ble_ll_adv_conn_req_rxd(rxbuf, hdr, advsm)) {
+                adv_event_over = 0;
+            }
+        } else {
+            if ((ptype == BLE_ADV_PDU_TYPE_SCAN_REQ) &&
+                (hdr->rxinfo.flags & BLE_MBUF_HDR_F_SCAN_RSP_TXD)) {
+                adv_event_over = 0;
+            }
+        }
+    }
+
+    if (adv_event_over) {
+        ble_ll_adv_make_done(advsm, hdr);
+    }
+}
+
+/**
+ * Called when a receive PDU has started and we are advertising.
+ *
+ * Context: interrupt
+ *
+ * @param pdu_type
+ * @param rxpdu
+ *
+ * @return int
+ *   < 0: A frame we dont want to receive.
+ *   = 0: Continue to receive frame. Dont go from rx to tx
+ *   > 0: Continue to receive frame and go from rx to tx when done
+ */
+int
+ble_ll_adv_rx_isr_start(uint8_t pdu_type)
+{
+    int rc;
+    struct ble_ll_adv_sm *advsm;
+
+    /* Assume we will abort the frame */
+    rc = -1;
+
+    /* If we get a scan request we must tell the phy to go from rx to tx */
+    advsm = g_ble_ll_cur_adv_sm;
+    if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) {
+        /* Only accept scan requests if we are indirect adv or scan adv */
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+            rc = 1;
+        }
+    } else {
+        /* Only accept connect requests if connectable advertising event */
+        if (pdu_type == BLE_ADV_PDU_TYPE_CONNECT_IND) {
+            if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+                /* Need transition to TX if extended adv */
+                rc = !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY);
+            }
+        }
+    }
+
+    /*
+     * If we abort the frame, we need to post the LL task to check if the
+     * advertising event is over.
+     */
+    if (rc < 0) {
+        ble_ll_adv_tx_done(advsm);
+    }
+
+    return rc;
+}
+
+static void
+ble_ll_adv_drop_event(struct ble_ll_adv_sm *advsm)
+{
+    STATS_INC(ble_ll_stats, adv_drop_event);
+
+    ble_ll_sched_rmv_elem(&advsm->adv_sch);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_ll_sched_rmv_elem(&advsm->aux[0].sch);
+    ble_ll_sched_rmv_elem(&advsm->aux[1].sch);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev);
+    advsm->aux_active = 0;
+#endif
+
+    advsm->adv_chan = ble_ll_adv_final_chan(advsm);
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+}
+
+static void
+ble_ll_adv_reschedule_event(struct ble_ll_adv_sm *advsm)
+{
+    int rc;
+    uint32_t start_time;
+    uint32_t max_delay_ticks;
+
+    assert(advsm->adv_enabled);
+
+    if (!advsm->adv_sch.enqueued) {
+        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
+            max_delay_ticks = 0;
+        } else {
+            max_delay_ticks =
+                    os_cputime_usecs_to_ticks(BLE_LL_ADV_DELAY_MS_MAX * 1000);
+        }
+
+        rc = ble_ll_sched_adv_reschedule(&advsm->adv_sch, &start_time,
+                                         max_delay_ticks);
+        if (rc) {
+            ble_ll_adv_drop_event(advsm);
+            return;
+        }
+
+        start_time += g_ble_ll_sched_offset_ticks;
+        advsm->adv_event_start_time = start_time;
+        advsm->adv_pdu_start_time = start_time;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
+                                                        !advsm->aux_active) {
+        ble_ll_adv_aux_schedule(advsm);
+    }
+#endif
+}
+
+/**
+ * Called when an advertising event is over.
+ *
+ * Context: Link Layer task.
+ *
+ * @param arg Pointer to advertising state machine.
+ */
+static void
+ble_ll_adv_done(struct ble_ll_adv_sm *advsm)
+
+{
+    int rc;
+    int resched_pdu;
+    uint8_t mask;
+    uint8_t final_adv_chan;
+    int32_t delta_t;
+    uint32_t itvl;
+    uint32_t tick_itvl;
+    uint32_t start_time;
+
+    assert(advsm->adv_enabled);
+
+    ble_ll_rfmgmt_release();
+
+    ble_ll_adv_update_adv_scan_rsp_data(advsm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        /* stop advertising this was due to transmitting connection response */
+        if (advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD) {
+            ble_ll_adv_sm_stop(advsm);
+            return;
+        }
+    }
+#endif
+
+    /* Remove the element from the schedule if it is still there. */
+    ble_ll_sched_rmv_elem(&advsm->adv_sch);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+
+    /*
+     * Check if we have ended our advertising event. If our last advertising
+     * packet was sent on the last channel, it means we are done with this
+     * event.
+     */
+    final_adv_chan = ble_ll_adv_final_chan(advsm);
+
+    if (advsm->adv_chan == final_adv_chan) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (advsm->events_max) {
+            advsm->events++;
+        }
+#endif
+
+        ble_ll_scan_chk_resume();
+
+        /* This event is over. Set adv channel to first one */
+        advsm->adv_chan = ble_ll_adv_first_chan(advsm);
+
+        /*
+         * Calculate start time of next advertising event. NOTE: we do not
+         * add the random advDelay as the scheduling code will do that.
+         */
+        itvl = advsm->adv_itvl_usecs;
+        tick_itvl = os_cputime_usecs_to_ticks(itvl);
+        advsm->adv_event_start_time += tick_itvl;
+        advsm->adv_pdu_start_time = advsm->adv_event_start_time;
+
+        /*
+         * The scheduled time better be in the future! If it is not, we will
+         * just keep advancing until we the time is in the future
+         */
+        start_time = advsm->adv_pdu_start_time - g_ble_ll_sched_offset_ticks;
+
+        delta_t = (int32_t)(start_time - os_cputime_get32());
+        if (delta_t < 0) {
+            /*
+             * NOTE: we just the same interval that we calculated earlier.
+             * No real need to keep recalculating a new interval.
+             */
+            while (delta_t < 0) {
+                advsm->adv_event_start_time += tick_itvl;
+                advsm->adv_pdu_start_time = advsm->adv_event_start_time;
+                delta_t += (int32_t)tick_itvl;
+            }
+        }
+        resched_pdu = 0;
+    } else {
+        /*
+         * Move to next advertising channel. If not in the mask, just
+         * increment by 1. We can do this because we already checked if we
+         * just transmitted on the last advertising channel
+         */
+        ++advsm->adv_chan;
+        mask = 1 << (advsm->adv_chan - BLE_PHY_ADV_CHAN_START);
+        if ((mask & advsm->adv_chanmask) == 0) {
+            ++advsm->adv_chan;
+        }
+
+        /*
+         * We will transmit right away. Set next pdu start time to now
+         * plus a xcvr start delay just so we dont count late adv starts
+         */
+        advsm->adv_pdu_start_time = os_cputime_get32() +
+                                    g_ble_ll_sched_offset_ticks;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        /* If we're past aux (unlikely, but can happen), just drop an event */
+        if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
+                advsm->aux_active &&
+                advsm->adv_pdu_start_time > AUX_CURRENT(advsm)->start_time) {
+            ble_ll_adv_drop_event(advsm);
+            return;
+        }
+#endif
+
+        resched_pdu = 1;
+    }
+
+    /* check if advertising timed out */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (advsm->duration &&
+        advsm->adv_pdu_start_time >= advsm->adv_end_time) {
+        /* Legacy PDUs need to be stop here.
+         * For ext adv it will be stopped when AUX is done (unless it was
+         * dropped so check if AUX is active here as well).
+         */
+        if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) ||
+                !advsm->aux_active) {
+            ble_ll_adv_sm_stop_timeout(advsm);
+        }
+
+        return;
+    }
+#else
+    if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) &&
+            (advsm->adv_pdu_start_time >= advsm->adv_end_time)) {
+        ble_ll_adv_sm_stop_timeout(advsm);
+        return;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (advsm->events_max && (advsm->events >= advsm->events_max)) {
+        /* Legacy PDUs need to be stop here.
+         * For ext adv it will be stopped when AUX is done (unless it was
+         * dropped so check if AUX is active here as well).
+         */
+        if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) ||
+                !advsm->aux_active) {
+            ble_ll_adv_sm_stop_limit_reached(advsm);
+        }
+
+        return;
+    }
+#endif
+
+    /* We need to regenerate our RPA's if we have passed timeout */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_ll_adv_chk_rpa_timeout(advsm);
+#endif
+
+    /* Schedule advertising transmit */
+    ble_ll_adv_set_sched(advsm);
+
+    if (!resched_pdu) {
+        ble_ll_adv_reschedule_event(advsm);
+        return;
+    }
+
+    /*
+     * In the unlikely event we can't reschedule this, just post a done event
+     * and we will reschedule the next advertising PDU.
+     */
+    rc = ble_ll_sched_adv_resched_pdu(&advsm->adv_sch);
+    if (rc) {
+        STATS_INC(ble_ll_stats, adv_resched_pdu_fail);
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev);
+    }
+}
+
+static void
+ble_ll_adv_event_done(struct ble_npl_event *ev)
+{
+    ble_ll_adv_done(ble_npl_event_get_arg(ev));
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/**
+ * Called when auxiliary packet is txd on secondary channel
+ *
+ * Context: Link Layer task.
+ *
+ * @param ev
+ */
+static void
+ble_ll_adv_sec_done(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_aux *aux;
+    struct ble_ll_adv_aux *aux_next;
+
+    assert(advsm->adv_enabled);
+    assert(advsm->aux_active);
+
+    aux = AUX_CURRENT(advsm);
+    aux_next = AUX_NEXT(advsm);
+
+    /* We don't need RF anymore */
+    ble_ll_rfmgmt_release();
+
+    if (advsm->aux_dropped) {
+        ble_ll_adv_drop_event(advsm);
+        return;
+    }
+
+    if (advsm->aux_not_scanned) {
+        ble_ll_sched_rmv_elem(&aux_next->sch);
+    }
+
+    /* Remove anything else scheduled for secondary channel */
+    ble_ll_sched_rmv_elem(&aux->sch);
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev);
+
+    /* Stop advertising due to transmitting connection response */
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD) {
+        ble_ll_adv_sm_stop(advsm);
+        return;
+    }
+
+    /* If we have next AUX scheduled, try to schedule another one */
+    if (aux_next->sch.enqueued) {
+        advsm->aux_index ^= 1;
+        advsm->aux_first_pdu = 0;
+        ble_ll_adv_aux_schedule_next(advsm);
+        return;
+    }
+
+    ble_ll_scan_chk_resume();
+
+    /* Check if advertising timed out */
+    if (advsm->duration && (advsm->adv_pdu_start_time >= advsm->adv_end_time)) {
+        ble_ll_adv_sm_stop_timeout(advsm);
+        return;
+    }
+
+    if (advsm->events_max && (advsm->events >= advsm->events_max)) {
+        ble_ll_adv_sm_stop_limit_reached(advsm);
+        return;
+    }
+
+    advsm->aux_active = 0;
+    ble_ll_adv_update_adv_scan_rsp_data(advsm);
+    ble_ll_adv_reschedule_event(advsm);
+}
+
+static void
+ble_ll_adv_sec_event_done(struct ble_npl_event *ev)
+{
+    ble_ll_adv_sec_done(ble_npl_event_get_arg(ev));
+}
+#endif
+
+static void
+ble_ll_adv_make_done(struct ble_ll_adv_sm *advsm, struct ble_mbuf_hdr *hdr)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (BLE_MBUF_HDR_EXT_ADV_SEC(hdr)) {
+        assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY));
+        assert(ble_ll_adv_active_chanset_is_sec(advsm));
+        ble_ll_adv_active_chanset_clear(advsm);
+        ble_ll_adv_sec_done(advsm);
+    } else {
+        assert(ble_ll_adv_active_chanset_is_pri(advsm));
+        ble_ll_adv_active_chanset_clear(advsm);
+        ble_ll_adv_done(advsm);
+    }
+#else
+    ble_ll_adv_active_chanset_clear(advsm);
+    ble_ll_adv_done(advsm);
+#endif
+}
+
+/**
+ * Checks if the controller can change the whitelist. If advertising is enabled
+ * and is using the whitelist the controller is not allowed to change the
+ * whitelist.
+ *
+ * @return int 0: not allowed to change whitelist; 1: change allowed.
+ */
+int
+ble_ll_adv_can_chg_whitelist(void)
+{
+    struct ble_ll_adv_sm *advsm;
+    int rc;
+    int i;
+
+    rc = 1;
+    for (i = 0; i < BLE_ADV_INSTANCES; ++i) {
+        advsm = &g_ble_ll_adv_sm[i];
+        if (advsm->adv_enabled &&
+            (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE)) {
+            rc = 0;
+            break;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Sends the connection complete event when advertising a connection starts.
+ *
+ * @return uint8_t* Pointer to event buffer
+ */
+void
+ble_ll_adv_send_conn_comp_ev(struct ble_ll_conn_sm *connsm,
+                             struct ble_mbuf_hdr *rxhdr)
+{
+    uint8_t *evbuf;
+    struct ble_ll_adv_sm *advsm;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    advsm = (struct ble_ll_adv_sm *)rxhdr->rxinfo.user_data;
+#else
+    advsm = &g_ble_ll_adv_sm[0];
+#endif
+
+    evbuf = advsm->conn_comp_ev;
+    assert(evbuf != NULL);
+    advsm->conn_comp_ev = NULL;
+
+    ble_ll_conn_comp_event_send(connsm, BLE_ERR_SUCCESS, evbuf, advsm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    ble_ll_hci_ev_le_csa(connsm);
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (ble_ll_hci_adv_mode_ext()) {
+        ble_ll_hci_ev_send_adv_set_terminated(0, advsm->adv_instance,
+                                          connsm->conn_handle, advsm->events);
+    }
+#endif
+}
+
+/**
+ * Returns the local resolvable private address currently being using by
+ * the advertiser
+ *
+ * @return uint8_t*
+ */
+uint8_t *
+ble_ll_adv_get_local_rpa(struct ble_ll_adv_sm *advsm)
+{
+    uint8_t *rpa = NULL;
+
+    if (advsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+        if ((advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) &&
+                                    ble_ll_is_rpa(advsm->adva, 1)) {
+            rpa = advsm->adva;
+        }
+    }
+
+    return rpa;
+}
+
+/**
+ * Returns the peer resolvable private address of last device connecting to us
+ *
+ * @return uint8_t*
+ */
+uint8_t *
+ble_ll_adv_get_peer_rpa(struct ble_ll_adv_sm *advsm)
+{
+    /* XXX: should this go into IRK list or connection? */
+    return advsm->adv_rpa;
+}
+
+/**
+ * Called when the LL wait for response timer expires while in the advertising
+ * state. Disables the phy and
+ *
+ */
+void
+ble_ll_adv_wfr_timer_exp(void)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    g_ble_ll_cur_adv_sm->aux_not_scanned = 1;
+#endif
+
+    ble_phy_disable();
+    ble_ll_adv_tx_done(g_ble_ll_cur_adv_sm);
+}
+
+/**
+ * Reset the advertising state machine.
+ *
+ * Context: Link Layer task
+ *
+ */
+void
+ble_ll_adv_reset(void)
+{
+    int i;
+    struct ble_ll_adv_sm *advsm;
+
+    for (i = 0; i < BLE_ADV_INSTANCES; ++i) {
+        advsm = &g_ble_ll_adv_sm[i];
+
+        /* Stop advertising state machine */
+        ble_ll_adv_sm_stop(advsm);
+
+        /* clear any data present */
+        os_mbuf_free_chain(advsm->adv_data);
+        os_mbuf_free_chain(advsm->scan_rsp_data);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        /* Stop periodic advertising state machine */
+        ble_ll_adv_sm_stop_periodic(advsm);
+
+        /* clear any periodic data present */
+        os_mbuf_free_chain(advsm->periodic_adv_data);
+#endif
+
+        /* re-initialize the advertiser state machine */
+        ble_ll_adv_sm_init(advsm);
+    }
+}
+
+/* Called to determine if advertising is enabled.
+ */
+uint8_t
+ble_ll_adv_enabled(void)
+{
+    int i;
+
+    for (i = 0; i < BLE_ADV_INSTANCES; i++) {
+        if (g_ble_ll_adv_sm[i].adv_enabled) {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+static void
+ble_ll_adv_sm_init(struct ble_ll_adv_sm *advsm)
+{
+    memset(advsm, 0, sizeof(struct ble_ll_adv_sm));
+
+    advsm->adv_itvl_min = BLE_HCI_ADV_ITVL_DEF;
+    advsm->adv_itvl_max = BLE_HCI_ADV_ITVL_DEF;
+    advsm->adv_chanmask = BLE_HCI_ADV_CHANMASK_DEF;
+
+    /* Initialize advertising tx done event */
+    ble_npl_event_init(&advsm->adv_txdone_ev, ble_ll_adv_event_done, advsm);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_npl_event_init(&advsm->adv_sec_txdone_ev, ble_ll_adv_sec_event_done, advsm);
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    ble_npl_event_init(&advsm->adv_periodic_txdone_ev,
+                       ble_ll_adv_periodic_event_done, advsm);
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* Initialize aux schedulers */
+    advsm->aux_active = 0;
+    advsm->aux[0].sch.cb_arg = advsm;
+    advsm->aux[0].sch.sched_cb = ble_ll_adv_secondary_tx_start_cb;
+    advsm->aux[0].sch.sched_type = BLE_LL_SCHED_TYPE_ADV;
+    advsm->aux[1].sch.cb_arg = advsm;
+    advsm->aux[1].sch.sched_cb = ble_ll_adv_secondary_tx_start_cb;
+    advsm->aux[1].sch.sched_type = BLE_LL_SCHED_TYPE_ADV;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    /* Initialize sync schedulers */
+    advsm->periodic_sync_active = 0;
+    advsm->periodic_sync[0].sch.cb_arg = advsm;
+    advsm->periodic_sync[0].sch.sched_cb = ble_ll_adv_sync_tx_start_cb;
+    advsm->periodic_sync[0].sch.sched_type = BLE_LL_SCHED_TYPE_PERIODIC;
+    advsm->periodic_sync[1].sch.cb_arg = advsm;
+    advsm->periodic_sync[1].sch.sched_cb = ble_ll_adv_sync_tx_start_cb;
+    advsm->periodic_sync[1].sch.sched_type = BLE_LL_SCHED_TYPE_PERIODIC;
+#endif
+#endif
+
+    /* Configure instances to be legacy on start */
+    advsm->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE;
+    advsm->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY;
+}
+
+/**
+ * Initialize the advertising functionality of a BLE device. This should
+ * be called once on initialization
+ */
+void
+ble_ll_adv_init(void)
+{
+    int i;
+
+    /* Set default advertising parameters */
+    for (i = 0; i < BLE_ADV_INSTANCES; ++i) {
+        ble_ll_adv_sm_init(&g_ble_ll_adv_sm[i]);
+    }
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn.c
new file mode 100644
index 00000000..1b17a0d2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn.c
@@ -0,0 +1,4270 @@
+/*
+ * 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 <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "os/os_cputime.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble/xcvr.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_ctrl.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_trace.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll_utils.h"
+#include "ble_ll_conn_priv.h"
+
+#if (BLETEST_THROUGHPUT_TEST == 1)
+extern void bletest_completed_pkt(uint16_t handle);
+#endif
+
+/* XXX TODO
+ * 1) I think if we are initiating and we already have a connection with
+ * a device that we will still try and connect to it. Fix this.
+ *  -> This is true. There are a couple things to do
+ *      i) When a connection create is issued, if we already are connected
+ *      deny it. BLE ERROR = 0x0B (ACL connection exists).
+ *      ii) If we receive an advertisement while initiating and want to send
+ *      a connect request to the device, make sure we dont have it.
+ *      iii) I think I need to do something like this: I am initiating and
+ *      advertising. Suppose the device I want to connect to sends me a connect
+ *      request because I am advertising? What happens to connection? Deal
+ *      with this!
+ *
+ * 2) Make sure we check incoming data packets for size and all that. You
+ * know, supported octets and all that. For both rx and tx.
+ *
+ * 3) Make sure we are setting the schedule end time properly for both slave
+ * and master. We should just set this to the end of the connection event.
+ * We might want to guarantee a IFS time as well since the next event needs
+ * to be scheduled prior to the start of the event to account for the time it
+ * takes to get a frame ready (which is pretty much the IFS time).
+ *
+ * 4) looks like the current code will allow the 1st packet in a
+ * connection to extend past the end of the allocated connection end
+ * time. That is not good. Need to deal with that. Need to extend connection
+ * end time.
+ *
+ * 6) Use error code 0x3E correctly! Connection failed to establish. If you
+ * read the LE connection complete event, it says that if the connection
+ * fails to be established that the connection complete event gets sent to
+ * the host that issued the create connection. Need to resolve this.
+ *
+ * 7) How does peer address get set if we are using whitelist? Look at filter
+ * policy and make sure you are doing this correctly.
+ *
+ * 8) Right now I use a fixed definition for required slots. CHange this.
+ *
+ * 10) See what connection state machine elements are purely master and
+ * purely slave. We can make a union of them.
+ *
+ * 11) Not sure I am dealing with the connection terminate timeout perfectly.
+ * I may extend a connection event too long although if it is always in terms
+ * of connection events I am probably fine. Checking at end that the next
+ * connection event will occur past terminate timeould would be fine.
+ *
+ * 12) When a slave receives a data packet in a connection it has to send a
+ * response. Well, it should. If this packet will overrun the next scheduled
+ * event, what should we do? Transmit anyway? Not transmit? For now, we just
+ * transmit.
+ *
+ * 32kHz crystal
+ * 1) When scheduling, I need to make sure I have time between
+ * this one and the next. Should I deal with this in the sched. Or
+ * is this basically accounted for given a slot? I really just need to
+ * make sure everything is over N ticks before the next sched start!
+ * Just add to end time?
+ *
+ * 2) I think one way to handle the problem of losing up to a microsecond
+ * every time we call ble_ll_conn_next_event in a loop is to do everything by
+ * keeping track of last anchor point. Would need last anchor usecs too. I guess
+ * we could also keep last anchor usecs as a uint32 or something and when we
+ * do the next event keep track of the residual using a different ticks to
+ * usecs calculation. Not sure.
+ */
+
+/*
+ * XXX: How should we deal with a late connection event? We need to determine
+ * what we want to do under the following cases:
+ *  1) The current connection event has not ended but a schedule item starts
+ */
+
+/* This is a dummy structure we use for the empty PDU */
+struct ble_ll_empty_pdu
+{
+    struct os_mbuf om;
+    struct os_mbuf_pkthdr pkt_hdr;
+    struct ble_mbuf_hdr ble_hdr;
+};
+
+/* We cannot have more than 254 connections given our current implementation */
+#if (MYNEWT_VAL(BLE_MAX_CONNECTIONS) >= 255)
+    #error "Maximum # of connections is 254"
+#endif
+
+/* Global connection complete event. Used when initiating */
+uint8_t *g_ble_ll_conn_comp_ev;
+
+/* Global LL connection parameters */
+struct ble_ll_conn_global_params g_ble_ll_conn_params;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+/* Global default sync transfer params */
+struct ble_ll_conn_sync_transfer_params g_ble_ll_conn_sync_transfer_params;
+#endif
+
+/* Pointer to connection state machine we are trying to create */
+struct ble_ll_conn_sm *g_ble_ll_conn_create_sm;
+
+/* Pointer to current connection */
+struct ble_ll_conn_sm *g_ble_ll_conn_cur_sm;
+
+/* Connection state machine array */
+struct ble_ll_conn_sm g_ble_ll_conn_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+
+/* List of active connections */
+struct ble_ll_conn_active_list g_ble_ll_conn_active_list;
+
+/* List of free connections */
+struct ble_ll_conn_free_list g_ble_ll_conn_free_list;
+
+STATS_SECT_START(ble_ll_conn_stats)
+    STATS_SECT_ENTRY(cant_set_sched)
+    STATS_SECT_ENTRY(conn_ev_late)
+    STATS_SECT_ENTRY(wfr_expirations)
+    STATS_SECT_ENTRY(handle_not_found)
+    STATS_SECT_ENTRY(no_conn_sm)
+    STATS_SECT_ENTRY(no_free_conn_sm)
+    STATS_SECT_ENTRY(rx_data_pdu_no_conn)
+    STATS_SECT_ENTRY(rx_data_pdu_bad_aa)
+    STATS_SECT_ENTRY(slave_rxd_bad_conn_req_params)
+    STATS_SECT_ENTRY(slave_ce_failures)
+    STATS_SECT_ENTRY(data_pdu_rx_dup)
+    STATS_SECT_ENTRY(data_pdu_txg)
+    STATS_SECT_ENTRY(data_pdu_txf)
+    STATS_SECT_ENTRY(conn_req_txd)
+    STATS_SECT_ENTRY(l2cap_enqueued)
+    STATS_SECT_ENTRY(rx_ctrl_pdus)
+    STATS_SECT_ENTRY(rx_l2cap_pdus)
+    STATS_SECT_ENTRY(rx_l2cap_bytes)
+    STATS_SECT_ENTRY(rx_malformed_ctrl_pdus)
+    STATS_SECT_ENTRY(rx_bad_llid)
+    STATS_SECT_ENTRY(tx_ctrl_pdus)
+    STATS_SECT_ENTRY(tx_ctrl_bytes)
+    STATS_SECT_ENTRY(tx_l2cap_pdus)
+    STATS_SECT_ENTRY(tx_l2cap_bytes)
+    STATS_SECT_ENTRY(tx_empty_pdus)
+    STATS_SECT_ENTRY(mic_failures)
+    STATS_SECT_ENTRY(sched_start_in_idle)
+    STATS_SECT_ENTRY(sched_end_in_idle)
+    STATS_SECT_ENTRY(conn_event_while_tmo)
+STATS_SECT_END
+STATS_SECT_DECL(ble_ll_conn_stats) ble_ll_conn_stats;
+
+STATS_NAME_START(ble_ll_conn_stats)
+    STATS_NAME(ble_ll_conn_stats, cant_set_sched)
+    STATS_NAME(ble_ll_conn_stats, conn_ev_late)
+    STATS_NAME(ble_ll_conn_stats, wfr_expirations)
+    STATS_NAME(ble_ll_conn_stats, handle_not_found)
+    STATS_NAME(ble_ll_conn_stats, no_conn_sm)
+    STATS_NAME(ble_ll_conn_stats, no_free_conn_sm)
+    STATS_NAME(ble_ll_conn_stats, rx_data_pdu_no_conn)
+    STATS_NAME(ble_ll_conn_stats, rx_data_pdu_bad_aa)
+    STATS_NAME(ble_ll_conn_stats, slave_rxd_bad_conn_req_params)
+    STATS_NAME(ble_ll_conn_stats, slave_ce_failures)
+    STATS_NAME(ble_ll_conn_stats, data_pdu_rx_dup)
+    STATS_NAME(ble_ll_conn_stats, data_pdu_txg)
+    STATS_NAME(ble_ll_conn_stats, data_pdu_txf)
+    STATS_NAME(ble_ll_conn_stats, conn_req_txd)
+    STATS_NAME(ble_ll_conn_stats, l2cap_enqueued)
+    STATS_NAME(ble_ll_conn_stats, rx_ctrl_pdus)
+    STATS_NAME(ble_ll_conn_stats, rx_l2cap_pdus)
+    STATS_NAME(ble_ll_conn_stats, rx_l2cap_bytes)
+    STATS_NAME(ble_ll_conn_stats, rx_malformed_ctrl_pdus)
+    STATS_NAME(ble_ll_conn_stats, rx_bad_llid)
+    STATS_NAME(ble_ll_conn_stats, tx_ctrl_pdus)
+    STATS_NAME(ble_ll_conn_stats, tx_ctrl_bytes)
+    STATS_NAME(ble_ll_conn_stats, tx_l2cap_pdus)
+    STATS_NAME(ble_ll_conn_stats, tx_l2cap_bytes)
+    STATS_NAME(ble_ll_conn_stats, tx_empty_pdus)
+    STATS_NAME(ble_ll_conn_stats, mic_failures)
+    STATS_NAME(ble_ll_conn_stats, sched_start_in_idle)
+    STATS_NAME(ble_ll_conn_stats, sched_end_in_idle)
+    STATS_NAME(ble_ll_conn_stats, conn_event_while_tmo)
+STATS_NAME_END(ble_ll_conn_stats)
+
+static void ble_ll_conn_event_end(struct ble_npl_event *ev);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+/**
+ * Checks to see if we should start a PHY update procedure
+ *
+ * If current phy is not one of the preferred we need to start control
+ * procedure.
+ *
+ * XXX: we could also decide to change the PHY if RSSI is really good
+ * and we are currently at 1Mbps or lower data rate and we could use
+ * a higher data rate.
+ *
+ * @param connsm
+ * @return 0: success; -1: no phy update procedure started
+ */
+int
+ble_ll_conn_chk_phy_upd_start(struct ble_ll_conn_sm *csm)
+{
+    int rc;
+
+    /* If no host preferences or  */
+    if (((csm->phy_data.host_pref_tx_phys_mask == 0) &&
+         (csm->phy_data.host_pref_rx_phys_mask == 0)) ||
+        ((csm->phy_data.host_pref_tx_phys_mask & CONN_CUR_TX_PHY_MASK(csm)) &&
+         (csm->phy_data.host_pref_rx_phys_mask & CONN_CUR_RX_PHY_MASK(csm)))) {
+        rc = -1;
+    } else {
+        csm->phy_data.req_pref_tx_phys_mask = csm->phy_data.host_pref_tx_phys_mask;
+        csm->phy_data.req_pref_rx_phys_mask = csm->phy_data.host_pref_rx_phys_mask;
+        ble_ll_ctrl_proc_start(csm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+        rc = 0;
+    }
+
+    return rc;
+}
+#endif
+
+static void
+ble_ll_conn_calc_itvl_ticks(struct ble_ll_conn_sm *connsm)
+{
+    uint32_t ticks;
+    uint32_t usecs;
+
+    /*
+     * Precalculate the number of ticks and remaining microseconds for
+     * the connection interval
+     */
+    usecs = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS;
+    ticks = os_cputime_usecs_to_ticks(usecs);
+    connsm->conn_itvl_usecs = (uint8_t)(usecs -
+                                        os_cputime_ticks_to_usecs(ticks));
+    if (connsm->conn_itvl_usecs == 31) {
+        connsm->conn_itvl_usecs = 0;
+        ++ticks;
+    }
+    connsm->conn_itvl_ticks = ticks;
+}
+
+/**
+ * Get the event buffer allocated to send the connection complete event
+ * when we are initiating.
+ *
+ * @return uint8_t*
+ */
+static uint8_t *
+ble_ll_init_get_conn_comp_ev(void)
+{
+    uint8_t *evbuf;
+
+    evbuf = g_ble_ll_conn_comp_ev;
+    BLE_LL_ASSERT(evbuf != NULL);
+    g_ble_ll_conn_comp_ev = NULL;
+
+    return evbuf;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * Called to determine if the received PDU is an empty PDU or not.
+ */
+static int
+ble_ll_conn_is_empty_pdu(uint8_t *rxbuf)
+{
+    int rc;
+    uint8_t llid;
+
+    llid = rxbuf[0] & BLE_LL_DATA_HDR_LLID_MASK;
+    if ((llid == BLE_LL_LLID_DATA_FRAG) && (rxbuf[1] == 0)) {
+        rc = 1;
+    } else {
+        rc = 0;
+    }
+    return rc;
+}
+#endif
+
+/**
+ * Called to return the currently running connection state machine end time.
+ * Always called when interrupts are disabled.
+ *
+ * @return int 0: s1 is not least recently used. 1: s1 is least recently used
+ */
+int
+ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2)
+{
+    int rc;
+
+    /* Set time that we last serviced the schedule */
+    if ((int32_t)(s1->last_scheduled - s2->last_scheduled) < 0) {
+        rc = 1;
+    } else {
+        rc = 0;
+    }
+
+    return rc;
+}
+
+/**
+ * Called to return the currently running connection state machine end time.
+ * Always called when interrupts are disabled.
+ *
+ * @return uint32_t
+ */
+uint32_t
+ble_ll_conn_get_ce_end_time(void)
+{
+    uint32_t ce_end_time;
+
+    if (g_ble_ll_conn_cur_sm) {
+        ce_end_time = g_ble_ll_conn_cur_sm->ce_end_time;
+    } else {
+        ce_end_time = os_cputime_get32();
+    }
+    return ce_end_time;
+}
+
+/**
+ * Called when connection state machine needs to halt. This function will:
+ *  -> Disable the PHY, which will prevent any transmit/receive interrupts.
+ *  -> Disable the wait for response timer, if running.
+ *  -> Remove the connection state machine from the scheduler.
+ *  -> Sets the Link Layer state to standby.
+ *  -> Sets the current state machine to NULL.
+ *
+ *  NOTE: the ordering of these function calls is important! We have to stop
+ *  the PHY and remove the schedule item before we can set the state to
+ *  standby and set the current state machine pointer to NULL.
+ */
+static void
+ble_ll_conn_halt(void)
+{
+    ble_phy_disable();
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    g_ble_ll_conn_cur_sm = NULL;
+}
+
+/**
+ * Called when the current connection state machine is no longer being used.
+ */
+static void
+ble_ll_conn_current_sm_over(struct ble_ll_conn_sm *connsm)
+{
+
+    ble_ll_conn_halt();
+
+    /*
+     * NOTE: the connection state machine may be NULL if we are calling
+     * this when we are ending the connection. In that case, there is no
+     * need to post to the LL the connection event end event
+     */
+    if (connsm) {
+        ble_ll_event_send(&connsm->conn_ev_end);
+    }
+}
+
+/**
+ * Given a handle, find an active connection matching the handle
+ *
+ * @param handle
+ *
+ * @return struct ble_ll_conn_sm*
+ */
+struct ble_ll_conn_sm *
+ble_ll_conn_find_active_conn(uint16_t handle)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = NULL;
+    if ((handle != 0) && (handle <= MYNEWT_VAL(BLE_MAX_CONNECTIONS))) {
+        connsm = &g_ble_ll_conn_sm[handle - 1];
+        if (connsm->conn_state == BLE_LL_CONN_STATE_IDLE) {
+            connsm = NULL;
+        }
+    }
+    return connsm;
+}
+
+/**
+ * Get a connection state machine.
+ */
+struct ble_ll_conn_sm *
+ble_ll_conn_sm_get(void)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = STAILQ_FIRST(&g_ble_ll_conn_free_list);
+    if (connsm) {
+        STAILQ_REMOVE_HEAD(&g_ble_ll_conn_free_list, free_stqe);
+    } else {
+        STATS_INC(ble_ll_conn_stats, no_free_conn_sm);
+    }
+
+    return connsm;
+}
+
+static uint8_t
+ble_ll_conn_calc_dci_csa1(struct ble_ll_conn_sm *conn)
+{
+    uint8_t curchan;
+    uint8_t remap_index;
+    uint8_t bitpos;
+
+    /* Get next unmapped channel */
+    curchan = conn->last_unmapped_chan + conn->hop_inc;
+    if (curchan > BLE_PHY_NUM_DATA_CHANS) {
+        curchan -= BLE_PHY_NUM_DATA_CHANS;
+    }
+
+    /* Save unmapped channel */
+    conn->last_unmapped_chan = curchan;
+
+    /* Is this a valid channel? */
+    bitpos = 1 << (curchan & 0x07);
+    if (conn->chanmap[curchan >> 3] & bitpos) {
+        return curchan;
+    }
+
+    /* Calculate remap index */
+    remap_index = curchan % conn->num_used_chans;
+
+    return ble_ll_utils_remapped_channel(remap_index, conn->chanmap);
+}
+
+/**
+ * Determine data channel index to be used for the upcoming/current
+ * connection event
+ *
+ * @param conn
+ * @param latency Used only for CSA #1
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_ll_conn_calc_dci(struct ble_ll_conn_sm *conn, uint16_t latency)
+{
+    uint8_t index;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    if (CONN_F_CSA2_SUPP(conn)) {
+        return ble_ll_utils_calc_dci_csa2(conn->event_cntr, conn->channel_id,
+                                          conn->num_used_chans, conn->chanmap);
+    }
+#endif
+
+    index = conn->data_chan_index;
+
+    while (latency > 0) {
+        index = ble_ll_conn_calc_dci_csa1(conn);
+        latency--;
+    }
+
+    return index;
+}
+
+/**
+ * Called when we are in the connection state and the wait for response timer
+ * fires off.
+ *
+ * Context: Interrupt
+ */
+void
+ble_ll_conn_wfr_timer_exp(void)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = g_ble_ll_conn_cur_sm;
+    ble_ll_conn_current_sm_over(connsm);
+    STATS_INC(ble_ll_conn_stats, wfr_expirations);
+}
+
+void
+ble_ll_conn_reset_pending_aux_conn_rsp(void)
+{
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    return;
+#endif
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = g_ble_ll_conn_create_sm;
+    if (!connsm) {
+        return;
+    }
+
+    if (CONN_F_AUX_CONN_REQ(connsm)) {
+        STATS_INC(ble_ll_stats, aux_conn_rsp_err);
+        CONN_F_CONN_REQ_TXD(connsm) = 0;
+        CONN_F_AUX_CONN_REQ(connsm) = 0;
+        ble_ll_sched_rmv_elem(&connsm->conn_sch);
+        return;
+    }
+
+    return;
+}
+
+bool
+ble_ll_conn_init_pending_aux_conn_rsp(void)
+{
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    return false;
+#endif
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = g_ble_ll_conn_create_sm;
+    if (!connsm) {
+        return false;
+    }
+
+    return CONN_F_AUX_CONN_REQ(connsm);
+}
+
+void
+ble_ll_conn_init_wfr_timer_exp(void)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = g_ble_ll_conn_create_sm;
+    if (!connsm) {
+        return;
+    }
+
+    ble_ll_conn_reset_pending_aux_conn_rsp();
+    connsm->inita_identity_used = 0;
+
+    ble_ll_scan_interrupted(connsm->scansm);
+
+#endif
+}
+/**
+ * Callback for slave when it transmits a data pdu and the connection event
+ * ends after the transmission.
+ *
+ * Context: Interrupt
+ *
+ * @param sch
+ *
+ */
+static void
+ble_ll_conn_wait_txend(void *arg)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)arg;
+    ble_ll_conn_current_sm_over(connsm);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+static void
+ble_ll_conn_start_rx_encrypt(void *arg)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)arg;
+    CONN_F_ENCRYPTED(connsm) = 1;
+    ble_phy_encrypt_enable(connsm->enc_data.rx_pkt_cntr,
+                           connsm->enc_data.iv,
+                           connsm->enc_data.enc_block.cipher_text,
+                           !CONN_IS_MASTER(connsm));
+}
+
+static void
+ble_ll_conn_start_rx_unencrypt(void *arg)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)arg;
+    CONN_F_ENCRYPTED(connsm) = 0;
+    ble_phy_encrypt_disable();
+}
+
+static void
+ble_ll_conn_txend_encrypt(void *arg)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)arg;
+    CONN_F_ENCRYPTED(connsm) = 1;
+    ble_ll_conn_current_sm_over(connsm);
+}
+
+static void
+ble_ll_conn_rxend_unencrypt(void *arg)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)arg;
+    CONN_F_ENCRYPTED(connsm) = 0;
+    ble_ll_conn_current_sm_over(connsm);
+}
+
+static void
+ble_ll_conn_continue_rx_encrypt(void *arg)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)arg;
+    ble_phy_encrypt_set_pkt_cntr(connsm->enc_data.rx_pkt_cntr,
+                                 !CONN_IS_MASTER(connsm));
+}
+#endif
+
+/**
+ * Returns the cputime of the next scheduled item on the scheduler list or
+ * when the current connection will start its next interval (whichever is
+ * earlier). This API is called when determining at what time we should end
+ * the current connection event. The current connection event must end before
+ * the next scheduled item. However, the current connection itself is not
+ * in the scheduler list! Thus, we need to calculate the time at which the
+ * next connection will start (the schedule start time; not the anchor point)
+ * and not overrun it.
+ *
+ * Context: Interrupt
+ *
+ * @param connsm
+ *
+ * @return uint32_t
+ */
+static uint32_t
+ble_ll_conn_get_next_sched_time(struct ble_ll_conn_sm *connsm)
+{
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+    uint32_t ce_end;
+    ce_end = connsm->ce_end_time;
+#else
+    uint32_t ce_end;
+    uint32_t next_sched_time;
+
+    /* Calculate time at which next connection event will start */
+    /* NOTE: We dont care if this time is tick short. */
+    ce_end = connsm->anchor_point + connsm->conn_itvl_ticks -
+        g_ble_ll_sched_offset_ticks;
+    if ((connsm->anchor_point_usecs + connsm->conn_itvl_usecs) >= 31) {
+        ++ce_end;
+    }
+
+    if (ble_ll_sched_next_time(&next_sched_time)) {
+        if (CPUTIME_LT(next_sched_time, ce_end)) {
+            ce_end = next_sched_time;
+        }
+    }
+#endif
+
+    return ce_end;
+}
+
+/**
+ * Called to check if certain connection state machine flags have been
+ * set.
+ *
+ * @param connsm
+ */
+static void
+ble_ll_conn_chk_csm_flags(struct ble_ll_conn_sm *connsm)
+{
+    uint8_t update_status;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (connsm->csmflags.cfbit.send_ltk_req) {
+        /*
+         * Send Long term key request event to host. If masked, we need to
+         * send a REJECT_IND.
+         */
+        if (ble_ll_hci_ev_ltk_req(connsm)) {
+            ble_ll_ctrl_reject_ind_send(connsm, BLE_LL_CTRL_ENC_REQ,
+                                        BLE_ERR_PINKEY_MISSING);
+        }
+        connsm->csmflags.cfbit.send_ltk_req = 0;
+    }
+#endif
+
+    /*
+     * There are two cases where this flag gets set:
+     * 1) A connection update procedure was started and the event counter
+     * has passed the instant.
+     * 2) We successfully sent the reject reason.
+     */
+    if (connsm->csmflags.cfbit.host_expects_upd_event) {
+        update_status = BLE_ERR_SUCCESS;
+        if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE)) {
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE);
+        } else {
+            if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) {
+                ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
+                update_status = connsm->reject_reason;
+            }
+        }
+        ble_ll_hci_ev_conn_update(connsm, update_status);
+        connsm->csmflags.cfbit.host_expects_upd_event = 0;
+    }
+
+    /* Check if we need to send PHY update complete event */
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    if (CONN_F_PHY_UPDATE_EVENT(connsm)) {
+        if (!ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS)) {
+            /* Sent event. Clear flag */
+            CONN_F_PHY_UPDATE_EVENT(connsm) = 0;
+        }
+    }
+#endif
+}
+
+/**
+ * Called when we want to send a data channel pdu inside a connection event.
+ *
+ * Context: interrupt
+ *
+ * @param connsm
+ *
+ * @return int 0: success; otherwise failure to transmit
+ */
+static uint16_t
+ble_ll_conn_adjust_pyld_len(struct ble_ll_conn_sm *connsm, uint16_t pyld_len)
+{
+    uint16_t phy_max_tx_octets;
+    uint16_t ret;
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    uint8_t phy_mode;
+
+    if (connsm->phy_tx_transition) {
+        phy_mode = ble_ll_phy_to_phy_mode(connsm->phy_tx_transition,
+                                          connsm->phy_data.phy_options);
+    } else {
+        phy_mode = connsm->phy_data.tx_phy_mode;
+    }
+
+    phy_max_tx_octets = ble_ll_pdu_max_tx_octets_get(connsm->eff_max_tx_time,
+                                                     phy_mode);
+
+#else
+    phy_max_tx_octets = ble_ll_pdu_max_tx_octets_get(connsm->eff_max_tx_time,
+                                                     BLE_PHY_MODE_1M);
+#endif
+
+    ret = pyld_len;
+
+    if (ret > connsm->eff_max_tx_octets) {
+        ret = connsm->eff_max_tx_octets;
+    }
+
+    if (ret > phy_max_tx_octets) {
+        ret = phy_max_tx_octets;
+    }
+
+    return ret;
+}
+
+static int
+ble_ll_conn_tx_pdu(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+    uint8_t md;
+    uint8_t hdr_byte;
+    uint8_t end_transition;
+    uint8_t cur_txlen;
+    uint8_t next_txlen;
+    uint8_t cur_offset;
+    uint16_t pktlen;
+    uint32_t next_event_time;
+    uint32_t ticks;
+    struct os_mbuf *m;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf_pkthdr *pkthdr = NULL;
+    struct os_mbuf_pkthdr *nextpkthdr;
+    struct ble_ll_empty_pdu empty_pdu;
+    ble_phy_tx_end_func txend_func;
+    int tx_phy_mode;
+    uint8_t llid;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    int is_ctrl;
+    uint8_t opcode;
+#endif
+
+    /* For compiler warnings... */
+    ble_hdr = NULL;
+    m = NULL;
+    md = 0;
+    hdr_byte = BLE_LL_LLID_DATA_FRAG;
+
+    if (connsm->csmflags.cfbit.terminate_ind_rxd) {
+        /* We just received terminate indication.
+         * Just send empty packet as an ACK
+         */
+        CONN_F_EMPTY_PDU_TXD(connsm) = 1;
+        goto conn_tx_pdu;
+    }
+
+    /*
+     * We need to check if we are retrying a pdu or if there is a pdu on
+     * the transmit queue.
+     */
+    pkthdr = STAILQ_FIRST(&connsm->conn_txq);
+    if (!connsm->cur_tx_pdu && !CONN_F_EMPTY_PDU_TXD(connsm) && !pkthdr) {
+        CONN_F_EMPTY_PDU_TXD(connsm) = 1;
+        goto conn_tx_pdu;
+    }
+
+    /*
+     * If we dont have a pdu we have previously transmitted, take it off
+     * the connection transmit queue
+     */
+    cur_offset = 0;
+    if (!connsm->cur_tx_pdu && !CONN_F_EMPTY_PDU_TXD(connsm)) {
+        /* Convert packet header to mbuf */
+        m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+        nextpkthdr = STAILQ_NEXT(pkthdr, omp_next);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        /*
+         * If we are encrypting, we are only allowed to send certain
+         * kinds of LL control PDU's. If none is enqueued, send empty pdu!
+         */
+        if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+            if (!ble_ll_ctrl_enc_allowed_pdu_tx(pkthdr)) {
+                CONN_F_EMPTY_PDU_TXD(connsm) = 1;
+                goto conn_tx_pdu;
+            }
+
+            /*
+             * We will allow a next packet if it itself is allowed or we are
+             * a slave and we are sending the START_ENC_RSP. The master has
+             * to wait to receive the START_ENC_RSP from the slave before
+             * packets can be let go.
+             */
+            if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu_tx(nextpkthdr)
+                && ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) ||
+                    !ble_ll_ctrl_is_start_enc_rsp(m))) {
+                nextpkthdr = NULL;
+            }
+        }
+#endif
+        /* Take packet off queue*/
+        STAILQ_REMOVE_HEAD(&connsm->conn_txq, omp_next);
+        ble_hdr = BLE_MBUF_HDR_PTR(m);
+
+        /*
+         * We dequeued new packet for transmission.
+         * If this is a data PDU we need to calculate payload length we can send
+         * over current PHY. Effectively, this determines fragmentation of packet
+         * into PDUs.
+         * If this is a control PDU we send complete PDU as only data PDU can be
+         * fragmented. We assume that checks (i.e. if remote supports such PDU)
+         * were already performed before putting packet on queue.
+         */
+        llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+        pktlen = pkthdr->omp_len;
+        if (llid == BLE_LL_LLID_CTRL) {
+            cur_txlen = pktlen;
+        } else {
+            cur_txlen = ble_ll_conn_adjust_pyld_len(connsm, pktlen);
+        }
+        ble_hdr->txinfo.pyld_len = cur_txlen;
+
+        /* NOTE: header was set when first enqueued */
+        hdr_byte = ble_hdr->txinfo.hdr_byte;
+        connsm->cur_tx_pdu = m;
+    } else {
+        nextpkthdr = pkthdr;
+        if (connsm->cur_tx_pdu) {
+            m = connsm->cur_tx_pdu;
+            ble_hdr = BLE_MBUF_HDR_PTR(m);
+            pktlen = OS_MBUF_PKTLEN(m);
+            cur_txlen = ble_hdr->txinfo.pyld_len;
+            cur_offset = ble_hdr->txinfo.offset;
+            if (cur_offset == 0) {
+                hdr_byte = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+            }
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+            if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+                /* We will allow a next packet if it itself is allowed */
+                pkthdr = OS_MBUF_PKTHDR(connsm->cur_tx_pdu);
+                if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu_tx(nextpkthdr)
+                    && ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) ||
+                        !ble_ll_ctrl_is_start_enc_rsp(connsm->cur_tx_pdu))) {
+                    nextpkthdr = NULL;
+                }
+            }
+#endif
+        } else {
+            /* Empty PDU here. NOTE: header byte gets set later */
+            pktlen = 0;
+            cur_txlen = 0;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+            if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+                /* We will allow a next packet if it itself is allowed */
+                if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu_tx(nextpkthdr)) {
+                    nextpkthdr = NULL;
+                }
+            }
+#endif
+        }
+    }
+
+    /*
+     * Set the more data data flag if we have more data to send and we
+     * have not been asked to terminate
+     */
+    if (nextpkthdr || ((cur_offset + cur_txlen) < pktlen)) {
+        /* Get next event time */
+        next_event_time = ble_ll_conn_get_next_sched_time(connsm);
+
+        /* XXX: TODO: need to check this with phy update procedure. There are
+           limitations if we have started update */
+
+        /*
+         * Dont bother to set the MD bit if we cannot do the following:
+         *  -> wait IFS, send the current frame.
+         *  -> wait IFS, receive a maximum size frame.
+         *  -> wait IFS, send the next frame.
+         *  -> wait IFS, receive a maximum size frame.
+         *
+         *  For slave:
+         *  -> wait IFS, send current frame.
+         *  -> wait IFS, receive maximum size frame.
+         *  -> wait IFS, send next frame.
+         */
+        if ((cur_offset + cur_txlen) < pktlen) {
+            next_txlen = pktlen - (cur_offset + cur_txlen);
+        } else {
+            if (nextpkthdr->omp_len > connsm->eff_max_tx_octets) {
+                next_txlen = connsm->eff_max_tx_octets;
+            } else {
+                next_txlen = nextpkthdr->omp_len;
+            }
+        }
+
+        /*
+         * XXX: this calculation is based on using the current time
+         * and assuming the transmission will occur an IFS time from
+         * now. This is not the most accurate especially if we have
+         * received a frame and we are replying to it.
+         */
+#if BLE_LL_BT5_PHY_SUPPORTED
+        tx_phy_mode = connsm->phy_data.tx_phy_mode;
+#else
+        tx_phy_mode = BLE_PHY_MODE_1M;
+#endif
+
+        ticks = (BLE_LL_IFS * 3) + connsm->eff_max_rx_time +
+            ble_ll_pdu_tx_time_get(next_txlen, tx_phy_mode) +
+            ble_ll_pdu_tx_time_get(cur_txlen, tx_phy_mode);
+
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+            ticks += (BLE_LL_IFS + connsm->eff_max_rx_time);
+        }
+
+        ticks = os_cputime_usecs_to_ticks(ticks);
+        if ((int32_t)((os_cputime_get32() + ticks) - next_event_time) < 0) {
+            md = 1;
+        }
+     }
+
+    /* If we send an empty PDU we need to initialize the header */
+conn_tx_pdu:
+    if (CONN_F_EMPTY_PDU_TXD(connsm)) {
+        /*
+         * This looks strange, but we dont use the data pointer in the mbuf
+         * when we have an empty pdu.
+         */
+        m = (struct os_mbuf *)&empty_pdu;
+        m->om_data = (uint8_t *)&empty_pdu;
+        m->om_data += BLE_MBUF_MEMBLOCK_OVERHEAD;
+        ble_hdr = &empty_pdu.ble_hdr;
+        ble_hdr->txinfo.flags = 0;
+        ble_hdr->txinfo.offset = 0;
+        ble_hdr->txinfo.pyld_len = 0;
+    }
+
+    /* Set tx seqnum */
+    if (connsm->tx_seqnum) {
+        hdr_byte |= BLE_LL_DATA_HDR_SN_MASK;
+    }
+
+    /* If we have more data, set the bit */
+    if (md) {
+        hdr_byte |= BLE_LL_DATA_HDR_MD_MASK;
+    }
+
+    /* Set NESN (next expected sequence number) bit */
+    if (connsm->next_exp_seqnum) {
+        hdr_byte |= BLE_LL_DATA_HDR_NESN_MASK;
+    }
+
+    /* Set the header byte in the outgoing frame */
+    ble_hdr->txinfo.hdr_byte = hdr_byte;
+
+    /*
+     * If we are a slave, check to see if this transmission will end the
+     * connection event. We will end the connection event if we have
+     * received a valid frame with the more data bit set to 0 and we dont
+     * have more data.
+     *
+     * XXX: for a slave, we dont check to see if we can:
+     *  -> wait IFS, rx frame from master (either big or small).
+     *  -> wait IFS, send empty pdu or next pdu.
+     *
+     *  We could do this. Now, we just keep going and hope that we dont
+     *  overrun next scheduled item.
+     */
+    if ((connsm->csmflags.cfbit.terminate_ind_rxd) ||
+        ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) && (md == 0) &&
+         (connsm->cons_rxd_bad_crc == 0) &&
+         ((connsm->last_rxd_hdr_byte & BLE_LL_DATA_HDR_MD_MASK) == 0) &&
+         !ble_ll_ctrl_is_terminate_ind(hdr_byte, m->om_data[0]))) {
+        /* We will end the connection event */
+        end_transition = BLE_PHY_TRANSITION_NONE;
+        txend_func = ble_ll_conn_wait_txend;
+    } else {
+        /* Wait for a response here */
+        end_transition = BLE_PHY_TRANSITION_TX_RX;
+        txend_func = NULL;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+    if (llid == BLE_LL_LLID_CTRL) {
+        is_ctrl = 1;
+        opcode = m->om_data[0];
+    } else {
+        is_ctrl = 0;
+        opcode = 0;
+    }
+
+    if (is_ctrl && (opcode == BLE_LL_CTRL_START_ENC_RSP)) {
+        /*
+         * Both master and slave send the START_ENC_RSP encrypted and receive
+         * encrypted
+         */
+        CONN_F_ENCRYPTED(connsm) = 1;
+        connsm->enc_data.tx_encrypted = 1;
+        ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr,
+                               connsm->enc_data.iv,
+                               connsm->enc_data.enc_block.cipher_text,
+                               CONN_IS_MASTER(connsm));
+    } else if (is_ctrl && (opcode == BLE_LL_CTRL_START_ENC_REQ)) {
+        /*
+         * Only the slave sends this and it gets sent unencrypted but
+         * we receive encrypted
+         */
+        CONN_F_ENCRYPTED(connsm) = 0;
+        connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_RSP_WAIT;
+        connsm->enc_data.tx_encrypted = 0;
+        ble_phy_encrypt_disable();
+        if (txend_func == NULL) {
+            txend_func = ble_ll_conn_start_rx_encrypt;
+        } else {
+            txend_func = ble_ll_conn_txend_encrypt;
+        }
+    } else if (is_ctrl && (opcode == BLE_LL_CTRL_PAUSE_ENC_RSP)) {
+        /*
+         * The slave sends the PAUSE_ENC_RSP encrypted. The master sends
+         * it unencrypted (note that link was already set unencrypted).
+         */
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            CONN_F_ENCRYPTED(connsm) = 1;
+            connsm->enc_data.tx_encrypted = 1;
+            ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr,
+                                   connsm->enc_data.iv,
+                                   connsm->enc_data.enc_block.cipher_text,
+                                   CONN_IS_MASTER(connsm));
+            if (txend_func == NULL) {
+                txend_func = ble_ll_conn_start_rx_unencrypt;
+            } else {
+                txend_func = ble_ll_conn_rxend_unencrypt;
+            }
+        } else {
+            CONN_F_ENCRYPTED(connsm) = 0;
+            connsm->enc_data.enc_state = CONN_ENC_S_PAUSED;
+            connsm->enc_data.tx_encrypted = 0;
+            ble_phy_encrypt_disable();
+        }
+    } else {
+        /* If encrypted set packet counter */
+        if (CONN_F_ENCRYPTED(connsm)) {
+            connsm->enc_data.tx_encrypted = 1;
+            ble_phy_encrypt_set_pkt_cntr(connsm->enc_data.tx_pkt_cntr,
+                                         CONN_IS_MASTER(connsm));
+            if (txend_func == NULL) {
+                txend_func = ble_ll_conn_continue_rx_encrypt;
+            }
+        }
+    }
+#endif
+
+    /* Set transmit end callback */
+    ble_phy_set_txend_cb(txend_func, connsm);
+    rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, m, end_transition);
+    if (!rc) {
+        /* Log transmit on connection state */
+        cur_txlen = ble_hdr->txinfo.pyld_len;
+        ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CONN_TX, cur_txlen,
+                           ble_hdr->txinfo.offset);
+
+        /* Set last transmitted MD bit */
+        CONN_F_LAST_TXD_MD(connsm) = md;
+
+        /* Increment packets transmitted */
+        if (CONN_F_EMPTY_PDU_TXD(connsm)) {
+            if (connsm->csmflags.cfbit.terminate_ind_rxd) {
+                connsm->csmflags.cfbit.terminate_ind_rxd_acked = 1;
+            }
+            STATS_INC(ble_ll_conn_stats, tx_empty_pdus);
+        } else if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) {
+            STATS_INC(ble_ll_conn_stats, tx_ctrl_pdus);
+            STATS_INCN(ble_ll_conn_stats, tx_ctrl_bytes, cur_txlen);
+        } else {
+            STATS_INC(ble_ll_conn_stats, tx_l2cap_pdus);
+            STATS_INCN(ble_ll_conn_stats, tx_l2cap_bytes, cur_txlen);
+        }
+    }
+    return rc;
+}
+
+/**
+ * Schedule callback for start of connection event.
+ *
+ * Context: Interrupt
+ *
+ * @param sch
+ *
+ * @return int 0: scheduled item is still running. 1: schedule item is done.
+ */
+static int
+ble_ll_conn_event_start_cb(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    uint32_t usecs;
+    uint32_t start;
+    struct ble_ll_conn_sm *connsm;
+
+    /* XXX: note that we can extend end time here if we want. Look at this */
+
+    /* Set current connection state machine */
+    connsm = (struct ble_ll_conn_sm *)sch->cb_arg;
+    g_ble_ll_conn_cur_sm = connsm;
+    BLE_LL_ASSERT(connsm);
+    if (connsm->conn_state == BLE_LL_CONN_STATE_IDLE) {
+        /* That should not happen. If it does it means connection
+         * is already closed
+         */
+        STATS_INC(ble_ll_conn_stats, sched_start_in_idle);
+        BLE_LL_ASSERT(0);
+        ble_ll_conn_current_sm_over(connsm);
+        return BLE_LL_SCHED_STATE_DONE;
+    }
+
+    /* Log connection event start */
+    ble_ll_trace_u32(BLE_LL_TRACE_ID_CONN_EV_START, connsm->conn_handle);
+
+    /* Disable whitelisting as connections do not use it */
+    ble_ll_whitelist_disable();
+
+    /* Set LL state */
+    ble_ll_state_set(BLE_LL_STATE_CONNECTION);
+
+    /* Set channel */
+    ble_phy_setchan(connsm->data_chan_index, connsm->access_addr,
+                    connsm->crcinit);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_phy_resolv_list_disable();
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_set(connsm->phy_data.tx_phy_mode, connsm->phy_data.rx_phy_mode);
+#endif
+
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        /* Set start time of transmission */
+        start = sch->start_time + g_ble_ll_sched_offset_ticks;
+        rc = ble_phy_tx_set_start_time(start, sch->remainder);
+        if (!rc) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+            if (CONN_F_ENCRYPTED(connsm)) {
+                ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr,
+                                       connsm->enc_data.iv,
+                                       connsm->enc_data.enc_block.cipher_text,
+                                       1);
+            } else {
+                ble_phy_encrypt_disable();
+            }
+#endif
+            rc = ble_ll_conn_tx_pdu(connsm);
+            if (!rc) {
+                rc = BLE_LL_SCHED_STATE_RUNNING;
+            } else {
+                /* Inform LL task of connection event end */
+                rc = BLE_LL_SCHED_STATE_DONE;
+            }
+        } else {
+            STATS_INC(ble_ll_conn_stats, conn_ev_late);
+            rc = BLE_LL_SCHED_STATE_DONE;
+        }
+    } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (CONN_F_ENCRYPTED(connsm)) {
+            ble_phy_encrypt_enable(connsm->enc_data.rx_pkt_cntr,
+                                   connsm->enc_data.iv,
+                                   connsm->enc_data.enc_block.cipher_text,
+                                   1);
+        } else {
+            ble_phy_encrypt_disable();
+        }
+#endif
+
+        /* XXX: what is this really for the slave? */
+        start = sch->start_time + g_ble_ll_sched_offset_ticks;
+        rc = ble_phy_rx_set_start_time(start, sch->remainder);
+        if (rc) {
+            /* End the connection event as we have no more buffers */
+            STATS_INC(ble_ll_conn_stats, slave_ce_failures);
+            rc = BLE_LL_SCHED_STATE_DONE;
+        } else {
+            /*
+             * Set flag that tells slave to set last anchor point if a packet
+             * has been received.
+             */
+            connsm->csmflags.cfbit.slave_set_last_anchor = 1;
+
+            /*
+             * Set the wait for response time. The anchor point is when we
+             * expect the master to start transmitting. Worst-case, we expect
+             * to hear a reply within the anchor point plus:
+             *  -> current tx window size
+             *  -> current window widening amount (includes +/- 16 usec jitter)
+             *  -> Amount of time it takes to detect packet start.
+             *  -> Some extra time (16 usec) to insure timing is OK
+             */
+
+            /*
+             * For the 32 kHz crystal, the amount of usecs we have to wait
+             * is not from the anchor point; we have to account for the time
+             * from when the receiver is enabled until the anchor point. The
+             * time we start before the anchor point is this:
+             *   -> current window widening.
+             *   -> up to one 32 kHz tick since we discard remainder.
+             *   -> Up to one tick since the usecs to ticks calc can be off
+             *   by up to one tick.
+             * NOTES:
+             * 1) the 61 we add is for the two ticks mentioned above.
+             * 2) The address rx time and jitter is accounted for in the
+             * phy function
+             */
+            usecs = connsm->slave_cur_tx_win_usecs + 61 +
+                (2 * connsm->slave_cur_window_widening);
+            ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, usecs);
+            /* Set next wakeup time to connection event end time */
+            rc = BLE_LL_SCHED_STATE_RUNNING;
+        }
+    }
+
+    if (rc == BLE_LL_SCHED_STATE_DONE) {
+        ble_ll_event_send(&connsm->conn_ev_end);
+        ble_phy_disable();
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        g_ble_ll_conn_cur_sm = NULL;
+    }
+
+    /* Set time that we last serviced the schedule */
+    connsm->last_scheduled = os_cputime_get32();
+    return rc;
+}
+
+/**
+ * Called to determine if the device is allowed to send the next pdu in the
+ * connection event. This will always return 'true' if we are a slave. If we
+ * are a master, we must be able to send the next fragment and get a minimum
+ * sized response from the slave.
+ *
+ * Context: Interrupt context (rx end isr).
+ *
+ * @param connsm
+ * @param begtime   Time at which IFS before pdu transmission starts
+ *
+ * @return int 0: not allowed to send 1: allowed to send
+ */
+static int
+ble_ll_conn_can_send_next_pdu(struct ble_ll_conn_sm *connsm, uint32_t begtime,
+                              uint32_t add_usecs)
+{
+    int rc;
+    uint8_t rem_bytes;
+    uint32_t ticks;
+    uint32_t usecs;
+    uint32_t next_sched_time;
+    struct os_mbuf *txpdu;
+    struct os_mbuf_pkthdr *pkthdr;
+    struct ble_mbuf_hdr *txhdr;
+    uint32_t allowed_usecs;
+    int tx_phy_mode;
+
+#if BLE_LL_BT5_PHY_SUPPORTED
+    tx_phy_mode = connsm->phy_data.tx_phy_mode;
+#else
+    tx_phy_mode = BLE_PHY_MODE_1M;
+#endif
+
+    rc = 1;
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        /* Get next scheduled item time */
+        next_sched_time = ble_ll_conn_get_next_sched_time(connsm);
+
+        txpdu = connsm->cur_tx_pdu;
+        if (!txpdu) {
+            pkthdr = STAILQ_FIRST(&connsm->conn_txq);
+            if (pkthdr) {
+                txpdu = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+            }
+        } else {
+            pkthdr = OS_MBUF_PKTHDR(txpdu);
+        }
+
+        /* XXX: TODO: need to check this with phy update procedure. There are
+           limitations if we have started update */
+        if (txpdu) {
+            txhdr = BLE_MBUF_HDR_PTR(txpdu);
+            rem_bytes = pkthdr->omp_len - txhdr->txinfo.offset;
+            if (rem_bytes > connsm->eff_max_tx_octets) {
+                rem_bytes = connsm->eff_max_tx_octets;
+            }
+            usecs = ble_ll_pdu_tx_time_get(rem_bytes, tx_phy_mode);
+        } else {
+            /* We will send empty pdu (just a LL header) */
+            usecs = ble_ll_pdu_tx_time_get(0, tx_phy_mode);
+        }
+        usecs += (BLE_LL_IFS * 2) + connsm->eff_max_rx_time;
+
+        ticks = (uint32_t)(next_sched_time - begtime);
+        allowed_usecs = os_cputime_ticks_to_usecs(ticks);
+        if ((usecs + add_usecs) >= allowed_usecs) {
+            rc = 0;
+        }
+    }
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+/**
+ * Callback for the Authenticated payload timer. This function is called
+ * when the authenticated payload timer expires. When the authenticated
+ * payload timeout expires, we should
+ *  -> Send the authenticated payload timeout event.
+ *  -> Start the LE ping procedure.
+ *  -> Restart the timer.
+ *
+ * @param arg
+ */
+void
+ble_ll_conn_auth_pyld_timer_cb(struct ble_npl_event *ev)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev);
+    ble_ll_auth_pyld_tmo_event_send(connsm);
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_LE_PING);
+    ble_ll_conn_auth_pyld_timer_start(connsm);
+}
+
+void
+ble_ll_conn_rd_features_timer_cb(struct ble_npl_event *ev)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = (struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev);
+
+    if (!connsm->csmflags.cfbit.pending_hci_rd_features ||
+                                        !connsm->csmflags.cfbit.rxd_features) {
+        return;
+    }
+
+    ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS);
+    connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+}
+
+/**
+ * Start (or restart) the authenticated payload timer
+ *
+ * @param connsm
+ */
+void
+ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm)
+{
+    int32_t tmo;
+
+    /* Timeout in is in 10 msec units */
+    tmo = (int32_t)BLE_LL_CONN_AUTH_PYLD_OS_TMO(connsm->auth_pyld_tmo);
+    ble_npl_callout_reset(&connsm->auth_pyld_timer, tmo);
+}
+#endif
+
+static void
+ble_ll_conn_master_common_init(struct ble_ll_conn_sm *connsm)
+{
+
+    /* Set master role */
+    connsm->conn_role = BLE_LL_CONN_ROLE_MASTER;
+
+    /* Set default ce parameters */
+
+    /*
+     * XXX: for now, we need twice the transmit window as our calculations
+     * for the transmit window offset could be off.
+     */
+    connsm->tx_win_size = BLE_LL_CONN_TX_WIN_MIN + 1;
+    connsm->tx_win_off = 0;
+    connsm->master_sca = MYNEWT_VAL(BLE_LL_MASTER_SCA);
+
+    /* Hop increment is a random value between 5 and 16. */
+    connsm->hop_inc = (rand() % 12) + 5;
+
+    /* Set channel map to map requested by host */
+    connsm->num_used_chans = g_ble_ll_conn_params.num_used_chans;
+    memcpy(connsm->chanmap, g_ble_ll_conn_params.master_chan_map,
+           BLE_LL_CONN_CHMAP_LEN);
+
+    /*  Calculate random access address and crc initialization value */
+    connsm->access_addr = ble_ll_utils_calc_access_addr();
+    connsm->crcinit = rand() & 0xffffff;
+
+    /* Set initial schedule callback */
+    connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb;
+}
+/**
+ * Called when a create connection command has been received. This initializes
+ * a connection state machine in the master role.
+ *
+ * NOTE: Must be called before the state machine is started
+ *
+ * @param connsm
+ * @param hcc
+ */
+void
+ble_ll_conn_master_init(struct ble_ll_conn_sm *connsm,
+                        struct hci_create_conn *hcc)
+{
+
+    ble_ll_conn_master_common_init(connsm);
+
+    /* Set slave latency and supervision timeout */
+    connsm->slave_latency = hcc->conn_latency;
+    connsm->supervision_tmo = hcc->supervision_timeout;
+
+    /* Set own address type and peer address if needed */
+    connsm->own_addr_type = hcc->own_addr_type;
+    if (hcc->filter_policy == 0) {
+        memcpy(&connsm->peer_addr, &hcc->peer_addr, BLE_DEV_ADDR_LEN);
+        connsm->peer_addr_type = hcc->peer_addr_type;
+    }
+
+    /* XXX: for now, just make connection interval equal to max */
+    connsm->conn_itvl = hcc->conn_itvl_max;
+
+    /* Check the min/max CE lengths are less than connection interval */
+    if (hcc->min_ce_len > (connsm->conn_itvl * 2)) {
+        connsm->min_ce_len = connsm->conn_itvl * 2;
+    } else {
+        connsm->min_ce_len = hcc->min_ce_len;
+    }
+
+    if (hcc->max_ce_len > (connsm->conn_itvl * 2)) {
+        connsm->max_ce_len = connsm->conn_itvl * 2;
+    } else {
+        connsm->max_ce_len = hcc->max_ce_len;
+    }
+}
+
+static void
+ble_ll_update_max_tx_octets_phy_mode(struct ble_ll_conn_sm *connsm)
+{
+    uint32_t usecs;
+
+    usecs = connsm->eff_max_tx_time;
+
+    connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_1M] =
+            ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_1M);
+    connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_2M] =
+            ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_2M);
+    connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_CODED_125KBPS] =
+            ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_CODED_125KBPS);
+    connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_CODED_500KBPS] =
+            ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_CODED_500KBPS);
+}
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+
+static void
+ble_ll_conn_set_phy(struct ble_ll_conn_sm *connsm, int tx_phy, int rx_phy)
+{
+
+    struct ble_ll_conn_phy_data *phy_data = &connsm->phy_data;
+
+    phy_data->rx_phy_mode = ble_ll_phy_to_phy_mode(rx_phy,
+                                                   BLE_HCI_LE_PHY_CODED_ANY);
+    phy_data->cur_rx_phy = rx_phy;
+
+    phy_data->tx_phy_mode = ble_ll_phy_to_phy_mode(tx_phy,
+                                                   BLE_HCI_LE_PHY_CODED_ANY);
+    phy_data->cur_tx_phy = tx_phy;
+
+}
+
+static void
+ble_ll_conn_init_phy(struct ble_ll_conn_sm *connsm, int phy)
+{
+    struct ble_ll_conn_global_params *conngp;
+
+    /* Always initialize symmetric PHY - controller can change this later */
+    ble_ll_conn_set_phy(connsm, phy, phy);
+
+    /* Update data length management to match initial PHY */
+    conngp = &g_ble_ll_conn_params;
+    connsm->max_tx_octets = conngp->conn_init_max_tx_octets;
+    connsm->max_rx_octets = conngp->supp_max_rx_octets;
+    if (phy == BLE_PHY_CODED) {
+        connsm->max_tx_time = conngp->conn_init_max_tx_time_coded;
+        connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED;
+        connsm->rem_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN_CODED;
+        connsm->rem_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN_CODED;
+        /* Assume peer does support coded */
+        connsm->remote_features[0] |= (BLE_LL_FEAT_LE_CODED_PHY >> 8);
+    } else {
+        connsm->max_tx_time = conngp->conn_init_max_tx_time_uncoded;
+        connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_UNCODED;
+        connsm->rem_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN_UNCODED;
+        connsm->rem_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN_UNCODED;
+    }
+    connsm->eff_max_tx_time = connsm->rem_max_tx_time;
+    connsm->eff_max_rx_time = connsm->rem_max_rx_time;
+    connsm->rem_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    connsm->rem_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    connsm->eff_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    connsm->eff_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+
+    ble_ll_update_max_tx_octets_phy_mode(connsm);
+}
+
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+
+void
+ble_ll_conn_ext_master_init(struct ble_ll_conn_sm *connsm,
+                        struct hci_ext_create_conn *hcc)
+{
+
+    ble_ll_conn_master_common_init(connsm);
+
+    /* Set own address type and peer address if needed */
+    connsm->own_addr_type = hcc->own_addr_type;
+    if (hcc->filter_policy == 0) {
+        memcpy(&connsm->peer_addr, &hcc->peer_addr, BLE_DEV_ADDR_LEN);
+        connsm->peer_addr_type = hcc->peer_addr_type;
+    }
+
+    connsm->initial_params = *hcc;
+}
+
+void
+ble_ll_conn_ext_set_params(struct ble_ll_conn_sm *connsm,
+                           struct hci_ext_conn_params *hcc_params, int phy)
+{
+    /* Set slave latency and supervision timeout */
+    connsm->slave_latency = hcc_params->conn_latency;
+    connsm->supervision_tmo = hcc_params->supervision_timeout;
+
+    /* XXX: for now, just make connection interval equal to max */
+    connsm->conn_itvl = hcc_params->conn_itvl_max;
+
+
+    /* Check the min/max CE lengths are less than connection interval */
+    if (hcc_params->min_ce_len > (connsm->conn_itvl * 2)) {
+        connsm->min_ce_len = connsm->conn_itvl * 2;
+    } else {
+        connsm->min_ce_len = hcc_params->min_ce_len;
+    }
+
+    if (hcc_params->max_ce_len > (connsm->conn_itvl * 2)) {
+        connsm->max_ce_len = connsm->conn_itvl * 2;
+    } else {
+        connsm->max_ce_len = hcc_params->max_ce_len;
+    }
+
+    ble_ll_conn_calc_itvl_ticks(connsm);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_ll_conn_init_phy(connsm, phy);
+#endif
+}
+
+
+#endif
+
+static void
+ble_ll_conn_set_csa(struct ble_ll_conn_sm *connsm, bool chsel)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    if (chsel) {
+        CONN_F_CSA2_SUPP(connsm) = 1;
+        connsm->channel_id = ((connsm->access_addr & 0xffff0000) >> 16) ^
+                              (connsm->access_addr & 0x0000ffff);
+
+        /* calculate the next data channel */
+        connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 0);
+        return;
+    }
+#endif
+
+    connsm->last_unmapped_chan = 0;
+
+    /* calculate the next data channel */
+    connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 1);
+}
+
+/**
+ * Create a new connection state machine. This is done once per
+ * connection when the HCI command "create connection" is issued to the
+ * controller or when a slave receives a connect request.
+ *
+ * Context: Link Layer task
+ *
+ * @param connsm
+ */
+void
+ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm)
+{
+    struct ble_ll_conn_global_params *conn_params;
+
+    /* Reset following elements */
+    connsm->csmflags.conn_flags = 0;
+    connsm->event_cntr = 0;
+    connsm->conn_state = BLE_LL_CONN_STATE_IDLE;
+    connsm->disconnect_reason = 0;
+    connsm->rxd_disconnect_reason = 0;
+    connsm->conn_features = BLE_LL_CONN_INITIAL_FEATURES;
+    memset(connsm->remote_features, 0, sizeof(connsm->remote_features));
+    connsm->vers_nr = 0;
+    connsm->comp_id = 0;
+    connsm->sub_vers_nr = 0;
+    connsm->reject_reason = BLE_ERR_SUCCESS;
+    connsm->conn_rssi = BLE_LL_CONN_UNKNOWN_RSSI;
+    connsm->rpa_index = -1;
+    connsm->inita_identity_used = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    connsm->sync_transfer_sync_timeout = g_ble_ll_conn_sync_transfer_params.sync_timeout_us;
+    connsm->sync_transfer_mode = g_ble_ll_conn_sync_transfer_params.mode;
+    connsm->sync_transfer_skip = g_ble_ll_conn_sync_transfer_params.max_skip;
+#endif
+
+    /* XXX: TODO set these based on PHY that started connection */
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    connsm->phy_data.cur_tx_phy = BLE_PHY_1M;
+    connsm->phy_data.cur_rx_phy = BLE_PHY_1M;
+    connsm->phy_data.tx_phy_mode = BLE_PHY_MODE_1M;
+    connsm->phy_data.rx_phy_mode = BLE_PHY_MODE_1M;
+    connsm->phy_data.req_pref_tx_phys_mask = 0;
+    connsm->phy_data.req_pref_rx_phys_mask = 0;
+    connsm->phy_data.host_pref_tx_phys_mask = g_ble_ll_data.ll_pref_tx_phys;
+    connsm->phy_data.host_pref_rx_phys_mask = g_ble_ll_data.ll_pref_rx_phys;
+    connsm->phy_data.phy_options = 0;
+    connsm->phy_tx_transition = 0;
+#endif
+
+    /* Reset current control procedure */
+    connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE;
+    connsm->pending_ctrl_procs = 0;
+
+    /*
+     * Set handle in connection update procedure to 0. If the handle
+     * is non-zero it means that the host initiated the connection
+     * parameter update request and the rest of the parameters are valid.
+     */
+    connsm->conn_param_req.handle = 0;
+
+    /* Connection end event */
+    ble_npl_event_init(&connsm->conn_ev_end, ble_ll_conn_event_end, connsm);
+
+    /* Initialize transmit queue and ack/flow control elements */
+    STAILQ_INIT(&connsm->conn_txq);
+    connsm->cur_tx_pdu = NULL;
+    connsm->tx_seqnum = 0;
+    connsm->next_exp_seqnum = 0;
+    connsm->cons_rxd_bad_crc = 0;
+    connsm->last_rxd_sn = 1;
+    connsm->completed_pkts = 0;
+
+    /* initialize data length mgmt */
+    conn_params = &g_ble_ll_conn_params;
+    connsm->max_tx_octets = conn_params->conn_init_max_tx_octets;
+    connsm->max_rx_octets = conn_params->supp_max_rx_octets;
+    connsm->max_tx_time = conn_params->conn_init_max_tx_time;
+    connsm->max_rx_time = conn_params->supp_max_rx_time;
+    connsm->rem_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN;
+    connsm->rem_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN;
+    connsm->eff_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN;
+    connsm->eff_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN;
+    connsm->rem_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    connsm->rem_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    connsm->eff_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    connsm->eff_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    connsm->host_req_max_tx_time = 0;
+#endif
+
+    ble_ll_update_max_tx_octets_phy_mode(connsm);
+
+    /* Reset encryption data */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    memset(&connsm->enc_data, 0, sizeof(struct ble_ll_conn_enc_data));
+    connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+    connsm->auth_pyld_tmo = BLE_LL_CONN_DEF_AUTH_PYLD_TMO;
+    CONN_F_LE_PING_SUPP(connsm) = 1;
+    ble_npl_callout_init(&connsm->auth_pyld_timer,
+                    &g_ble_ll_data.ll_evq,
+                    ble_ll_conn_auth_pyld_timer_cb,
+                    connsm);
+#endif
+
+    ble_ll_conn_calc_itvl_ticks(connsm);
+
+    /* Add to list of active connections */
+    SLIST_INSERT_HEAD(&g_ble_ll_conn_active_list, connsm, act_sle);
+}
+
+void
+ble_ll_conn_update_eff_data_len(struct ble_ll_conn_sm *connsm)
+{
+    int send_event;
+    uint16_t eff_time;
+    uint16_t eff_bytes;
+
+    /* Assume no event sent */
+    send_event = 0;
+
+    /* See if effective times have changed */
+    eff_time = min(connsm->rem_max_tx_time, connsm->max_rx_time);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if (connsm->phy_data.cur_rx_phy == BLE_PHY_CODED) {
+        eff_time = max(eff_time, BLE_LL_CONN_SUPP_TIME_MIN_CODED);
+    }
+#endif
+    if (eff_time != connsm->eff_max_rx_time) {
+        connsm->eff_max_rx_time = eff_time;
+        send_event = 1;
+    }
+    eff_time = min(connsm->rem_max_rx_time, connsm->max_tx_time);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if (connsm->phy_data.cur_tx_phy == BLE_PHY_CODED) {
+        eff_time = max(eff_time, BLE_LL_CONN_SUPP_TIME_MIN_CODED);
+    }
+#endif
+    if (eff_time != connsm->eff_max_tx_time) {
+        connsm->eff_max_tx_time = eff_time;
+        send_event = 1;
+
+        ble_ll_update_max_tx_octets_phy_mode(connsm);
+    }
+    eff_bytes = min(connsm->rem_max_tx_octets, connsm->max_rx_octets);
+    if (eff_bytes != connsm->eff_max_rx_octets) {
+        connsm->eff_max_rx_octets = eff_bytes;
+        send_event = 1;
+    }
+    eff_bytes = min(connsm->rem_max_rx_octets, connsm->max_tx_octets);
+    if (eff_bytes != connsm->eff_max_tx_octets) {
+        connsm->eff_max_tx_octets = eff_bytes;
+        send_event = 1;
+    }
+
+    if (send_event) {
+        ble_ll_hci_ev_datalen_chg(connsm);
+    }
+}
+
+/**
+ * Called when a connection is terminated
+ *
+ * Context: Link Layer task.
+ *
+ * @param connsm
+ * @param ble_err
+ */
+void
+ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err)
+{
+    struct os_mbuf *m;
+    struct os_mbuf_pkthdr *pkthdr;
+    os_sr_t sr;
+
+    /* Remove scheduler events just in case */
+    ble_ll_sched_rmv_elem(&connsm->conn_sch);
+
+    /* In case of the supervision timeout we shall make sure
+     * that there is no ongoing connection event. It could happen
+     * because we scheduled connection event before checking connection timeout.
+     * If connection event managed to start, let us drop it.
+     */
+    OS_ENTER_CRITICAL(sr);
+    if (g_ble_ll_conn_cur_sm == connsm) {
+        ble_ll_conn_halt();
+        STATS_INC(ble_ll_conn_stats, conn_event_while_tmo);
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    /* Stop any control procedures that might be running */
+    ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+    ble_npl_callout_stop(&connsm->auth_pyld_timer);
+#endif
+
+    /* Remove from the active connection list */
+    SLIST_REMOVE(&g_ble_ll_conn_active_list, connsm, ble_ll_conn_sm, act_sle);
+
+    /* Free the current transmit pdu if there is one. */
+    if (connsm->cur_tx_pdu) {
+        os_mbuf_free_chain(connsm->cur_tx_pdu);
+        connsm->cur_tx_pdu = NULL;
+    }
+
+    /* Free all packets on transmit queue */
+    while (1) {
+        /* Get mbuf pointer from packet header pointer */
+        pkthdr = STAILQ_FIRST(&connsm->conn_txq);
+        if (!pkthdr) {
+            break;
+        }
+        STAILQ_REMOVE_HEAD(&connsm->conn_txq, omp_next);
+
+        m = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf));
+        os_mbuf_free_chain(m);
+    }
+
+    /* Make sure events off queue */
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &connsm->conn_ev_end);
+
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+    /* Remove from occupied periods */
+    OS_ENTER_CRITICAL(sr);
+    BLE_LL_ASSERT(g_ble_ll_sched_data.sch_num_occ_periods > 0);
+    BLE_LL_ASSERT(g_ble_ll_sched_data.sch_occ_period_mask & connsm->period_occ_mask);
+    --g_ble_ll_sched_data.sch_num_occ_periods;
+    g_ble_ll_sched_data.sch_occ_period_mask &= ~connsm->period_occ_mask;
+    OS_EXIT_CRITICAL(sr);
+#endif
+
+    /* Connection state machine is now idle */
+    connsm->conn_state = BLE_LL_CONN_STATE_IDLE;
+
+    /*
+     * If we have features and there's pending HCI command, send an event before
+     * disconnection event so it does make sense to host.
+     */
+    if (connsm->csmflags.cfbit.pending_hci_rd_features &&
+                                        connsm->csmflags.cfbit.rxd_features) {
+        ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS);
+        connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+    }
+
+    /*
+     * If there is still pending read features request HCI command, send an
+     * event to complete it.
+     */
+    if (connsm->csmflags.cfbit.pending_hci_rd_features) {
+        ble_ll_hci_ev_rd_rem_used_feat(connsm, ble_err);
+        connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+    }
+
+    /*
+     * We need to send a disconnection complete event. Connection Complete for
+     * canceling connection creation is sent from LE Create Connection Cancel
+     * Command handler.
+     *
+     * If the ble error is "success" it means that the reset command was
+     * received and we should not send an event.
+     */
+    if (ble_err && (ble_err != BLE_ERR_UNK_CONN_ID ||
+                                connsm->csmflags.cfbit.terminate_ind_rxd)) {
+        ble_ll_disconn_comp_event_send(connsm, ble_err);
+    }
+
+    /* Put connection state machine back on free list */
+    STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe);
+
+    /* Log connection end */
+    ble_ll_trace_u32x3(BLE_LL_TRACE_ID_CONN_END, connsm->conn_handle,
+                       connsm->event_cntr, (uint32_t)ble_err);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+void
+ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event,
+                       uint32_t *anchor, uint8_t *anchor_usecs)
+{
+    uint32_t ticks;
+    uint32_t itvl;
+
+    itvl = (connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS);
+
+    if ((int16_t)(conn_event - connsm->event_cntr) < 0) {
+        itvl *= connsm->event_cntr - conn_event;
+        ticks = os_cputime_usecs_to_ticks(itvl);
+        *anchor = connsm->anchor_point - ticks;
+    } else {
+        itvl *= conn_event - connsm->event_cntr;
+        ticks = os_cputime_usecs_to_ticks(itvl);
+        *anchor = connsm->anchor_point + ticks;
+    }
+
+    *anchor_usecs = connsm->anchor_point_usecs;
+    *anchor_usecs += (itvl - os_cputime_ticks_to_usecs(ticks));
+    if (*anchor_usecs >= 31) {
+        (*anchor)++;
+        *anchor_usecs -= 31;
+    }
+}
+#endif
+
+/**
+ * Called to move to the next connection event.
+ *
+ * Context: Link Layer task.
+ *
+ * @param connsm
+ *
+ * @return int
+ */
+static int
+ble_ll_conn_next_event(struct ble_ll_conn_sm *connsm)
+{
+    uint16_t latency;
+    uint32_t itvl;
+    uint32_t cur_ww;
+    uint32_t max_ww;
+    struct ble_ll_conn_upd_req *upd;
+    uint32_t ticks;
+    uint32_t usecs;
+
+    /* XXX: deal with connection request procedure here as well */
+    ble_ll_conn_chk_csm_flags(connsm);
+
+    /* If unable to start terminate procedure, start it now */
+    if (connsm->disconnect_reason && !CONN_F_TERMINATE_STARTED(connsm)) {
+        ble_ll_ctrl_terminate_start(connsm);
+    }
+
+    if (CONN_F_TERMINATE_STARTED(connsm) && (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE)) {
+        /* Some of the devices waits whole connection interval to ACK our
+         * TERMINATE_IND sent as a Slave. Since we are here it means we are still waiting for ACK.
+         * Make sure we catch it in next connection event.
+         */
+        connsm->slave_latency = 0;
+    }
+
+    /*
+     * XXX: TODO Probably want to add checks to see if we need to start
+     * a control procedure here as an instant may have prevented us from
+     * starting one.
+     */
+
+    /*
+     * XXX TODO: I think this is technically incorrect. We can allow slave
+     * latency if we are doing one of these updates as long as we
+     * know that the master has received the ACK to the PDU that set
+     * the instant
+     */
+    /* Set event counter to the next connection event that we will tx/rx in */
+    itvl = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS;
+    latency = 1;
+    if (connsm->csmflags.cfbit.allow_slave_latency      &&
+        !connsm->csmflags.cfbit.conn_update_sched       &&
+        !CONN_F_PHY_UPDATE_SCHED(connsm)                &&
+        !connsm->csmflags.cfbit.chanmap_update_scheduled) {
+        if (connsm->csmflags.cfbit.pkt_rxd) {
+            latency += connsm->slave_latency;
+            itvl = itvl * latency;
+        }
+    }
+    connsm->event_cntr += latency;
+
+    /* Set next connection event start time */
+    /* We can use pre-calculated values for one interval if latency is 1. */
+    if (latency == 1) {
+        connsm->anchor_point += connsm->conn_itvl_ticks;
+        connsm->anchor_point_usecs += connsm->conn_itvl_usecs;
+    } else {
+        uint32_t ticks;
+        ticks = os_cputime_usecs_to_ticks(itvl);
+        connsm->anchor_point += ticks;
+        connsm->anchor_point_usecs += (itvl - os_cputime_ticks_to_usecs(ticks));
+    }
+    if (connsm->anchor_point_usecs >= 31) {
+        ++connsm->anchor_point;
+        connsm->anchor_point_usecs -= 31;
+    }
+
+    /*
+     * If a connection update has been scheduled and the event counter
+     * is now equal to the instant, we need to adjust the start of the
+     * connection by the the transmit window offset. We also copy in the
+     * update parameters as they now should take effect.
+     */
+    if (connsm->csmflags.cfbit.conn_update_sched &&
+        (connsm->event_cntr == connsm->conn_update_req.instant)) {
+
+        /* Set flag so we send connection update event */
+        upd = &connsm->conn_update_req;
+        if ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER)  ||
+            ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) &&
+             IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) ||
+            (connsm->conn_itvl != upd->interval)            ||
+            (connsm->slave_latency != upd->latency)         ||
+            (connsm->supervision_tmo != upd->timeout)) {
+            connsm->csmflags.cfbit.host_expects_upd_event = 1;
+        }
+
+        connsm->supervision_tmo = upd->timeout;
+        connsm->slave_latency = upd->latency;
+        connsm->tx_win_size = upd->winsize;
+        connsm->slave_cur_tx_win_usecs =
+            connsm->tx_win_size * BLE_LL_CONN_TX_WIN_USECS;
+        connsm->tx_win_off = upd->winoffset;
+        connsm->conn_itvl = upd->interval;
+        ble_ll_conn_calc_itvl_ticks(connsm);
+        if (upd->winoffset != 0) {
+            usecs = upd->winoffset * BLE_LL_CONN_ITVL_USECS;
+            ticks = os_cputime_usecs_to_ticks(usecs);
+            connsm->anchor_point += ticks;
+            usecs = usecs - os_cputime_ticks_to_usecs(ticks);
+            connsm->anchor_point_usecs += usecs;
+            if (connsm->anchor_point_usecs >= 31) {
+                ++connsm->anchor_point;
+                connsm->anchor_point_usecs -= 31;
+            }
+        }
+
+        /* Reset the starting point of the connection supervision timeout */
+        connsm->last_rxd_pdu_cputime = connsm->anchor_point;
+
+        /* Reset update scheduled flag */
+        connsm->csmflags.cfbit.conn_update_sched = 0;
+    }
+
+    /*
+     * If there is a channel map request pending and we have reached the
+     * instant, change to new channel map. Note there is a special case here.
+     * If we received a channel map update with an instant equal to the event
+     * counter, when we get here the event counter has already been
+     * incremented by 1. That is why we do a signed comparison and change to
+     * new channel map once the event counter equals or has passed channel
+     * map update instant.
+     */
+    if (connsm->csmflags.cfbit.chanmap_update_scheduled &&
+        ((int16_t)(connsm->chanmap_instant - connsm->event_cntr) <= 0)) {
+
+        /* XXX: there is a chance that the control packet is still on
+         * the queue of the master. This means that we never successfully
+         * transmitted update request. Would end up killing connection
+           on slave side. Could ignore it or see if still enqueued. */
+        connsm->num_used_chans =
+            ble_ll_utils_calc_num_used_chans(connsm->req_chanmap);
+        memcpy(connsm->chanmap, connsm->req_chanmap, BLE_LL_CONN_CHMAP_LEN);
+
+        connsm->csmflags.cfbit.chanmap_update_scheduled = 0;
+
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CHAN_MAP_UPD);
+
+        /* XXX: host could have resent channel map command. Need to
+           check to make sure we dont have to restart! */
+    }
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    if (CONN_F_PHY_UPDATE_SCHED(connsm) &&
+        (connsm->event_cntr == connsm->phy_instant)) {
+
+        /* Set cur phy to new phy */
+        if (connsm->phy_data.new_tx_phy) {
+            connsm->phy_data.cur_tx_phy = connsm->phy_data.new_tx_phy;
+            connsm->phy_data.tx_phy_mode =
+                                ble_ll_phy_to_phy_mode(connsm->phy_data.cur_tx_phy,
+                                                   connsm->phy_data.phy_options);
+        }
+
+        if (connsm->phy_data.new_rx_phy) {
+            connsm->phy_data.cur_rx_phy = connsm->phy_data.new_rx_phy;
+            connsm->phy_data.rx_phy_mode =
+                                ble_ll_phy_to_phy_mode(connsm->phy_data.cur_rx_phy,
+                                                   connsm->phy_data.phy_options);
+        }
+
+        /* Clear flags and set flag to send event at next instant */
+        CONN_F_PHY_UPDATE_SCHED(connsm) = 0;
+        CONN_F_PHY_UPDATE_EVENT(connsm) = 1;
+
+        ble_ll_ctrl_phy_update_proc_complete(connsm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+        /* Recalculate effective connection parameters */
+        ble_ll_conn_update_eff_data_len(connsm);
+
+        /*
+         * If PHY in either direction was changed to coded, we assume that peer
+         * does support LE Coded PHY even if features were not exchanged yet.
+         * This means that MaxRxTime can be updated to supported max and we need
+         * initiate DLE to notify peer about the change.
+         */
+        if (((connsm->phy_data.cur_tx_phy == BLE_PHY_CODED) ||
+             (connsm->phy_data.cur_rx_phy == BLE_PHY_CODED)) &&
+            !(connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8))) {
+            connsm->remote_features[0] |= (BLE_LL_FEAT_LE_CODED_PHY >> 8);
+            connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED;
+            ble_ll_ctrl_initiate_dle(connsm);
+        }
+#endif
+    }
+#endif
+
+    /* Calculate data channel index of next connection event */
+    connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, latency);
+
+    /*
+     * If we are trying to terminate connection, check if next wake time is
+     * passed the termination timeout. If so, no need to continue with
+     * connection as we will time out anyway.
+     */
+    if (CONN_F_TERMINATE_STARTED(connsm)) {
+        if ((int32_t)(connsm->terminate_timeout - connsm->anchor_point) <= 0) {
+            return -1;
+        }
+    }
+
+    /*
+     * Calculate ce end time. For a slave, we need to add window widening and
+     * the transmit window if we still have one.
+     */
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+    itvl = g_ble_ll_sched_data.sch_ticks_per_period;
+#else
+    itvl = MYNEWT_VAL(BLE_LL_CONN_INIT_SLOTS) * BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT;
+#endif
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+
+        cur_ww = ble_ll_utils_calc_window_widening(connsm->anchor_point,
+                                                   connsm->last_anchor_point,
+                                                   connsm->master_sca);
+        max_ww = (connsm->conn_itvl * (BLE_LL_CONN_ITVL_USECS/2)) - BLE_LL_IFS;
+        if (cur_ww >= max_ww) {
+            return -1;
+        }
+        cur_ww += BLE_LL_JITTER_USECS;
+        connsm->slave_cur_window_widening = cur_ww;
+        itvl += os_cputime_usecs_to_ticks(cur_ww + connsm->slave_cur_tx_win_usecs);
+    }
+    itvl -= g_ble_ll_sched_offset_ticks;
+    connsm->ce_end_time = connsm->anchor_point + itvl;
+
+    return 0;
+}
+
+/**
+ * Called when a connection has been created. This function will
+ *  -> Set the connection state to created.
+ *  -> Start the connection supervision timer
+ *  -> Set the Link Layer state to connection.
+ *  -> Send a connection complete event.
+ *
+ *  See Section 4.5.2 Vol 6 Part B
+ *
+ *  Context: Link Layer
+ *
+ * @param connsm
+ *
+ * @ return 0: connection NOT created. 1: connection created
+ */
+static int
+ble_ll_conn_created(struct ble_ll_conn_sm *connsm, struct ble_mbuf_hdr *rxhdr)
+{
+    int rc;
+    uint8_t *evbuf;
+    uint32_t endtime;
+    uint32_t usecs;
+
+    /* XXX: TODO this assumes we received in 1M phy */
+
+    /* Set state to created */
+    connsm->conn_state = BLE_LL_CONN_STATE_CREATED;
+
+    /* Clear packet received flag */
+    connsm->csmflags.cfbit.pkt_rxd = 0;
+
+    /* Consider time created the last scheduled time */
+    connsm->last_scheduled = os_cputime_get32();
+
+    /*
+     * Set the last rxd pdu time since this is where we want to start the
+     * supervision timer from.
+     */
+    connsm->last_rxd_pdu_cputime = connsm->last_scheduled;
+
+    /*
+     * Set first connection event time. If slave the endtime is the receive end
+     * time of the connect request. The actual connection starts 1.25 msecs plus
+     * the transmit window offset from the end of the connection request.
+     */
+    rc = 1;
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+        /*
+         * With a 32.768 kHz crystal we dont care about the remaining usecs
+         * when setting last anchor point. The only thing last anchor is used
+         * for is to calculate window widening. The effect of this is
+         * negligible.
+         */
+        connsm->last_anchor_point = rxhdr->beg_cputime;
+
+        usecs = rxhdr->rem_usecs + 1250 +
+                (connsm->tx_win_off * BLE_LL_CONN_TX_WIN_USECS) +
+                ble_ll_pdu_tx_time_get(BLE_CONNECT_REQ_LEN,
+                                       rxhdr->rxinfo.phy_mode);
+
+        if (rxhdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) {
+            switch (rxhdr->rxinfo.phy) {
+            case BLE_PHY_1M:
+            case BLE_PHY_2M:
+                usecs += 1250;
+                break;
+            case BLE_PHY_CODED:
+                usecs += 2500;
+                break;
+            default:
+                BLE_LL_ASSERT(0);
+                break;
+            }
+        }
+
+        /* Anchor point is cputime. */
+        endtime = os_cputime_usecs_to_ticks(usecs);
+        connsm->anchor_point = rxhdr->beg_cputime + endtime;
+        connsm->anchor_point_usecs = usecs - os_cputime_ticks_to_usecs(endtime);
+        if (connsm->anchor_point_usecs == 31) {
+            ++connsm->anchor_point;
+            connsm->anchor_point_usecs = 0;
+        }
+
+        connsm->slave_cur_tx_win_usecs =
+            connsm->tx_win_size * BLE_LL_CONN_TX_WIN_USECS;
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+        connsm->ce_end_time = connsm->anchor_point +
+            g_ble_ll_sched_data.sch_ticks_per_period +
+            os_cputime_usecs_to_ticks(connsm->slave_cur_tx_win_usecs) + 1;
+
+#else
+        connsm->ce_end_time = connsm->anchor_point +
+            (MYNEWT_VAL(BLE_LL_CONN_INIT_SLOTS) * BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT)
+            + os_cputime_usecs_to_ticks(connsm->slave_cur_tx_win_usecs) + 1;
+#endif
+        connsm->slave_cur_window_widening = BLE_LL_JITTER_USECS;
+
+        /* Start the scheduler for the first connection event */
+        while (ble_ll_sched_slave_new(connsm)) {
+            if (ble_ll_conn_next_event(connsm)) {
+                STATS_INC(ble_ll_conn_stats, cant_set_sched);
+                rc = 0;
+                break;
+            }
+        }
+    }
+
+    /* Send connection complete event to inform host of connection */
+    if (rc) {
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        /*
+         * If we have default phy preferences and they are different than
+         * the current PHY's in use, start update procedure.
+         */
+        /*
+         * XXX: should we attempt to start this without knowing if
+         * the other side can support it?
+         */
+        if (!ble_ll_conn_chk_phy_upd_start(connsm)) {
+            CONN_F_CTRLR_PHY_UPDATE(connsm) = 1;
+        }
+#endif
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            ble_ll_adv_send_conn_comp_ev(connsm, rxhdr);
+        } else {
+            evbuf = ble_ll_init_get_conn_comp_ev();
+            ble_ll_conn_comp_event_send(connsm, BLE_ERR_SUCCESS, evbuf, NULL);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+            ble_ll_hci_ev_le_csa(connsm);
+#endif
+
+            /*
+             * Initiate features exchange
+             *
+             * XXX we do this only as a master as it was observed that sending
+             * LL_SLAVE_FEATURE_REQ after connection breaks some recent iPhone
+             * models; for slave just assume master will initiate features xchg
+             * if it has some additional features to use.
+             */
+            ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG);
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Called upon end of connection event
+ *
+ * Context: Link-layer task
+ *
+ * @param void *arg Pointer to connection state machine
+ *
+ */
+static void
+ble_ll_conn_event_end(struct ble_npl_event *ev)
+{
+    uint8_t ble_err;
+    uint32_t tmo;
+    struct ble_ll_conn_sm *connsm;
+
+    ble_ll_rfmgmt_release();
+
+    /* Better be a connection state machine! */
+    connsm = (struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev);
+    BLE_LL_ASSERT(connsm);
+    if (connsm->conn_state == BLE_LL_CONN_STATE_IDLE) {
+        /* That should not happen. If it does it means connection
+         * is already closed.
+         * Make sure LL state machine is in idle
+         */
+        STATS_INC(ble_ll_conn_stats, sched_end_in_idle);
+        BLE_LL_ASSERT(0);
+
+        /* Just in case */
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+
+        ble_ll_scan_chk_resume();
+        return;
+    }
+
+    /* Log event end */
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CONN_EV_END, connsm->conn_handle,
+                       connsm->event_cntr);
+
+    ble_ll_scan_chk_resume();
+
+    /* If we have transmitted the terminate IND successfully, we are done */
+    if ((connsm->csmflags.cfbit.terminate_ind_txd) ||
+                    (connsm->csmflags.cfbit.terminate_ind_rxd &&
+                     connsm->csmflags.cfbit.terminate_ind_rxd_acked)) {
+        if (connsm->csmflags.cfbit.terminate_ind_txd) {
+            ble_err = BLE_ERR_CONN_TERM_LOCAL;
+        } else {
+            /* Make sure the disconnect reason is valid! */
+            ble_err = connsm->rxd_disconnect_reason;
+            if (ble_err == 0) {
+                ble_err = BLE_ERR_REM_USER_CONN_TERM;
+            }
+        }
+        ble_ll_conn_end(connsm, ble_err);
+        return;
+    }
+
+    /* Remove any connection end events that might be enqueued */
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &connsm->conn_ev_end);
+
+    /*
+     * If we have received a packet, we can set the current transmit window
+     * usecs to 0 since we dont need to listen in the transmit window.
+     */
+    if (connsm->csmflags.cfbit.pkt_rxd) {
+        connsm->slave_cur_tx_win_usecs = 0;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+    /*
+     * If we are encrypted and have passed the authenticated payload timeout
+     * we need to send an event to tell the host. Unfortunately, I think we
+     * need one of these per connection and we have to set this timer
+     * fairly accurately. So we need to another event in the connection.
+     * This sucks.
+     *
+     * The way this works is that whenever the timer expires it just gets reset
+     * and we send the autheticated payload timeout event. Note that this timer
+     * should run even when encryption is paused.
+     * XXX: what should be here? Was there code here that got deleted?
+     */
+#endif
+
+    /* Move to next connection event */
+    if (ble_ll_conn_next_event(connsm)) {
+        ble_ll_conn_end(connsm, BLE_ERR_CONN_TERM_LOCAL);
+        return;
+    }
+
+    /* Reset "per connection event" variables */
+    connsm->cons_rxd_bad_crc = 0;
+    connsm->csmflags.cfbit.pkt_rxd = 0;
+
+    /* See if we need to start any control procedures */
+    ble_ll_ctrl_chk_proc_start(connsm);
+
+    /* Set initial schedule callback */
+    connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb;
+
+    /* XXX: I think all this fine for when we do connection updates, but
+       we may want to force the first event to be scheduled. Not sure */
+    /* Schedule the next connection event */
+    while (ble_ll_sched_conn_reschedule(connsm)) {
+        if (ble_ll_conn_next_event(connsm)) {
+            ble_ll_conn_end(connsm, BLE_ERR_CONN_TERM_LOCAL);
+            return;
+        }
+    }
+
+    /*
+     * This is definitely not perfect but hopefully will be fine in regards to
+     * the specification. We check the supervision timer at connection event
+     * end. If the next connection event is going to start past the supervision
+     * timeout we end the connection here. I guess this goes against the spec
+     * in two ways:
+     * 1) We are actually causing a supervision timeout before the time
+     * specified. However, this is really a moot point because the supervision
+     * timeout would have expired before we could possibly receive a packet.
+     * 2) We may end the supervision timeout a bit later than specified as
+     * we only check this at event end and a bad CRC could cause us to continue
+     * the connection event longer than the supervision timeout. Given that two
+     * bad CRC's consecutively ends the connection event, I dont regard this as
+     * a big deal but it could cause a slightly longer supervision timeout.
+     */
+    if (connsm->conn_state == BLE_LL_CONN_STATE_CREATED) {
+        tmo = (uint32_t)connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS * 6UL;
+        ble_err = BLE_ERR_CONN_ESTABLISHMENT;
+    } else {
+        tmo = connsm->supervision_tmo * BLE_HCI_CONN_SPVN_TMO_UNITS * 1000UL;
+        ble_err = BLE_ERR_CONN_SPVN_TMO;
+    }
+    /* XXX: Convert to ticks to usecs calculation instead??? */
+    tmo = os_cputime_usecs_to_ticks(tmo);
+    if ((int32_t)(connsm->anchor_point - connsm->last_rxd_pdu_cputime) >= tmo) {
+        ble_ll_conn_end(connsm, ble_err);
+        return;
+    }
+
+    /* If we have completed packets, send an event */
+    ble_ll_conn_num_comp_pkts_event_send(connsm);
+
+    /* If we have features and there's pending HCI command, send an event */
+    if (connsm->csmflags.cfbit.pending_hci_rd_features &&
+                                        connsm->csmflags.cfbit.rxd_features) {
+        ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS);
+        connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+    }
+}
+
+/**
+ * Update the connection request PDU with the address type and address of
+ * advertiser we are going to send connect request to.
+ *
+ * @param m
+ * @param adva
+ * @param addr_type     Address type of ADVA from received advertisement.
+ * @param inita
+ * @param inita_type     Address type of INITA from received advertisement.
+
+ * @param txoffset      The tx window offset for this connection
+ */
+static void
+ble_ll_conn_connect_ind_prepare(struct ble_ll_conn_sm *connsm,
+                                struct ble_ll_scan_pdu_data *pdu_data,
+                                uint8_t adva_type, uint8_t *adva,
+                                uint8_t inita_type, uint8_t *inita,
+                                int rpa_index, uint8_t channel)
+{
+    uint8_t hdr;
+    uint8_t *addr;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int is_rpa;
+    struct ble_ll_resolv_entry *rl;
+#endif
+
+    hdr = BLE_ADV_PDU_TYPE_CONNECT_IND;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+    /* We need CSA2 bit only for legacy connect */
+    if (channel >= BLE_PHY_NUM_DATA_CHANS) {
+        hdr |= BLE_ADV_PDU_HDR_CHSEL;
+    }
+#endif
+
+    if (adva_type) {
+        /* Set random address */
+        hdr |= BLE_ADV_PDU_HDR_RXADD_MASK;
+    }
+
+    if (inita) {
+        memcpy(pdu_data->inita, inita, BLE_DEV_ADDR_LEN);
+        if (inita_type) {
+            hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+        }
+    } else {
+        /* Get pointer to our device address */
+        connsm = g_ble_ll_conn_create_sm;
+        if ((connsm->own_addr_type & 1) == 0) {
+            addr = g_dev_addr;
+        } else {
+            hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+            addr = g_random_addr;
+        }
+
+    /* XXX: do this ahead of time? Calculate the local rpa I mean */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+            rl = NULL;
+            is_rpa = ble_ll_is_rpa(adva, adva_type);
+            if (is_rpa) {
+                if (rpa_index >= 0) {
+                    rl = &g_ble_ll_resolv_list[rpa_index];
+                }
+            } else {
+                /* we look for RL entry to generate local RPA regardless if
+                 * resolving is enabled or not (as this is is for local RPA
+                 * not peer RPA)
+                 */
+                 rl = ble_ll_resolv_list_find(adva, adva_type);
+            }
+
+            /*
+             * If peer in on resolving list, we use RPA generated with Local IRK
+             * from resolving list entry. In other case, we need to use our identity
+             * address (see  Core 5.0, Vol 6, Part B, section 6.4).
+             */
+            if (rl && rl->rl_has_local) {
+                hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+                ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita);
+                addr = NULL;
+            }
+        }
+#endif
+
+        if (addr) {
+            memcpy(pdu_data->inita, addr, BLE_DEV_ADDR_LEN);
+            /* Identity address used */
+            connsm->inita_identity_used = 1;
+        }
+    }
+
+    memcpy(pdu_data->adva, adva, BLE_DEV_ADDR_LEN);
+
+    pdu_data->hdr_byte = hdr;
+}
+
+/* Returns true if the address matches the connection peer address having in
+ * mind privacy mode
+ */
+static int
+ble_ll_conn_is_peer_adv(uint8_t addr_type, uint8_t *adva, int index)
+{
+    int rc;
+    uint8_t *peer_addr = NULL;
+    struct ble_ll_conn_sm *connsm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl;
+#endif
+
+    /* XXX: Deal with different types of random addresses here! */
+    connsm = g_ble_ll_conn_create_sm;
+    if (!connsm) {
+        return 0;
+    }
+
+    switch (connsm->peer_addr_type) {
+    /* Fall-through intentional */
+    case BLE_HCI_CONN_PEER_ADDR_PUBLIC:
+    case BLE_HCI_CONN_PEER_ADDR_RANDOM:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (ble_ll_addr_is_id(adva, addr_type)) {
+                /* Peer uses its identity address. Let's verify privacy mode.
+                 *
+                 * Note: Core Spec 5.0 Vol 6, Part B
+                 * If the Host has added the peer device to the resolving list
+                 * with an all-zero peer IRK, the Controller shall only accept
+                 * the peer's identity address.
+                 */
+            if (ble_ll_resolv_enabled()) {
+                rl = ble_ll_resolv_list_find(adva, addr_type);
+                if (rl && (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) &&
+                    rl->rl_has_peer) {
+                    return 0;
+                }
+            }
+        }
+
+        /* Check if peer uses RPA. If so and it match, use it as controller
+         * supports privacy mode
+         */
+        if ((index >= 0) &&
+                (g_ble_ll_resolv_list[index].rl_addr_type == connsm->peer_addr_type)) {
+            peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr;
+        }
+#endif
+        /*
+         * If we are here it means we don't know the device, lets
+         * check if type is what we are looking for and later
+         * if address matches
+         */
+        if ((connsm->peer_addr_type == addr_type) && !peer_addr) {
+            peer_addr = adva;
+        }
+
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT:
+        if ((index < 0) ||
+            (g_ble_ll_resolv_list[index].rl_addr_type != 0)) {
+            return 0;
+        }
+        peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr;
+        break;
+    case BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT:
+        if ((index < 0) ||
+            (g_ble_ll_resolv_list[index].rl_addr_type != 1)) {
+            return 0;
+        }
+        peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr;
+        break;
+#endif
+    default:
+        peer_addr = NULL;
+        break;
+    }
+
+    rc = 0;
+    if (peer_addr) {
+        if (!memcmp(peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN)) {
+            rc = 1;
+        }
+    }
+
+    return rc;
+}
+
+static void
+ble_ll_conn_connect_ind_txend_to_standby(void *arg)
+{
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+}
+
+static void
+ble_ll_conn_connect_ind_txend_to_init(void *arg)
+{
+    ble_ll_state_set(BLE_LL_STATE_INITIATING);
+}
+
+static uint8_t
+ble_ll_conn_connect_ind_tx_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_scan_pdu_data *pdu_data;
+
+    connsm = pducb_arg;
+    /*
+     * pdu_data was prepared just before starting TX and is expected to be
+     * still valid here
+     */
+    pdu_data = ble_ll_scan_get_pdu_data();
+
+    memcpy(dptr, pdu_data->inita, BLE_DEV_ADDR_LEN);
+    memcpy(dptr + BLE_DEV_ADDR_LEN, pdu_data->adva, BLE_DEV_ADDR_LEN);
+
+    dptr += 2 * BLE_DEV_ADDR_LEN;
+
+    put_le32(dptr, connsm->access_addr);
+    dptr[4] = (uint8_t)connsm->crcinit;
+    dptr[5] = (uint8_t)(connsm->crcinit >> 8);
+    dptr[6] = (uint8_t)(connsm->crcinit >> 16);
+    dptr[7] = connsm->tx_win_size;
+    put_le16(dptr + 8, connsm->tx_win_off);
+    put_le16(dptr + 10, connsm->conn_itvl);
+    put_le16(dptr + 12, connsm->slave_latency);
+    put_le16(dptr + 14, connsm->supervision_tmo);
+    memcpy(dptr + 16, &connsm->chanmap, BLE_LL_CONN_CHMAP_LEN);
+    dptr[21] = connsm->hop_inc | (connsm->master_sca << 5);
+
+    *hdr_byte = pdu_data->hdr_byte;
+
+    return 34;
+}
+
+/**
+ * Send a connection requestion to an advertiser
+ *
+ * Context: Interrupt
+ *
+ * @param addr_type Address type of advertiser
+ * @param adva Address of advertiser
+ */
+int
+ble_ll_conn_connect_ind_send(struct ble_ll_conn_sm *connsm, uint8_t end_trans)
+{
+    int rc;
+
+    if (end_trans == BLE_PHY_TRANSITION_NONE) {
+        ble_phy_set_txend_cb(ble_ll_conn_connect_ind_txend_to_standby, NULL);
+    } else {
+        ble_phy_set_txend_cb(ble_ll_conn_connect_ind_txend_to_init, NULL);
+    }
+
+    rc = ble_phy_tx(ble_ll_conn_connect_ind_tx_pducb, connsm, end_trans);
+
+    return rc;
+}
+
+/**
+ * Called when a schedule item overlaps the currently running connection
+ * event. This generally should not happen, but if it does we stop the
+ * current connection event to let the schedule item run.
+ *
+ * NOTE: the phy has been disabled as well as the wfr timer before this is
+ * called.
+ */
+void
+ble_ll_conn_event_halt(void)
+{
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    if (g_ble_ll_conn_cur_sm) {
+        g_ble_ll_conn_cur_sm->csmflags.cfbit.pkt_rxd = 0;
+        ble_ll_event_send(&g_ble_ll_conn_cur_sm->conn_ev_end);
+        g_ble_ll_conn_cur_sm = NULL;
+    }
+}
+
+/**
+ * Process a received PDU while in the initiating state.
+ *
+ * Context: Link Layer task.
+ *
+ * @param pdu_type
+ * @param rxbuf
+ * @param ble_hdr
+ */
+void
+ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf,
+                      struct ble_mbuf_hdr *ble_hdr)
+{
+    uint8_t addr_type;
+    uint8_t *addr;
+    uint8_t *adv_addr;
+    uint8_t *inita;
+    uint8_t inita_type;
+    struct ble_ll_conn_sm *connsm;
+    int ext_adv_mode = -1;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_aux_data *aux_data = NULL;
+
+     if (ble_hdr->rxinfo.user_data) {
+         /* aux_data just a local helper, no need to ref
+          * as ble_hdr->rxinfo.user_data is unref in the end of this function
+          */
+         aux_data = ble_hdr->rxinfo.user_data;
+     }
+#endif
+
+    /* Get the connection state machine we are trying to create */
+    connsm = g_ble_ll_conn_create_sm;
+    if (!connsm) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (aux_data) {
+            ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data);
+            ble_hdr->rxinfo.user_data = NULL;
+        }
+#endif
+        return;
+    }
+
+    if (!BLE_MBUF_HDR_CRC_OK(ble_hdr)) {
+        goto scan_continue;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (BLE_MBUF_HDR_AUX_INVALID(ble_hdr)) {
+        goto scan_continue;
+    }
+
+    if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) {
+        if (BLE_MBUF_HDR_WAIT_AUX(ble_hdr)) {
+            /* Just continue scanning. We are waiting for AUX */
+            if (!ble_ll_sched_aux_scan(ble_hdr, connsm->scansm, aux_data)) {
+                /* ref for aux ptr in the scheduler */
+                ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data);
+                ble_hdr->rxinfo.user_data = NULL;
+                ble_ll_scan_chk_resume();
+                return;
+            }
+            goto scan_continue;
+        }
+    }
+
+    if (CONN_F_AUX_CONN_REQ(connsm)) {
+        if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) {
+            /* Wait for connection response, in this point of time aux is NULL */
+            BLE_LL_ASSERT(ble_hdr->rxinfo.user_data == NULL);
+            return;
+        }
+    }
+#endif
+
+    /* If we have sent a connect request, we need to enter CONNECTION state */
+    if (connsm && CONN_F_CONN_REQ_TXD(connsm)) {
+        /* Set address of advertiser to which we are connecting. */
+
+        if (ble_ll_scan_adv_decode_addr(pdu_type, rxbuf, ble_hdr,
+                                        &adv_addr, &addr_type,
+                                        &inita, &inita_type, &ext_adv_mode)) {
+            /* Something got wrong, keep trying to connect */
+            goto scan_continue;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        /*
+         * Did we resolve this address? If so, set correct peer address
+         * and peer address type.
+         */
+        if (connsm->rpa_index >= 0) {
+            addr_type = g_ble_ll_resolv_list[connsm->rpa_index].rl_addr_type + 2;
+            addr = g_ble_ll_resolv_list[connsm->rpa_index].rl_identity_addr;
+        } else {
+            addr = adv_addr;
+        }
+#else
+        addr = adv_addr;
+#endif
+
+        if (connsm->rpa_index >= 0) {
+            connsm->peer_addr_type = addr_type;
+            memcpy(connsm->peer_addr, addr, BLE_DEV_ADDR_LEN);
+
+            ble_ll_scan_set_peer_rpa(adv_addr);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+            /* Update resolving list with current peer RPA */
+            ble_ll_resolv_set_peer_rpa(connsm->rpa_index, rxbuf + BLE_LL_PDU_HDR_LEN);
+            if (ble_ll_is_rpa(inita, inita_type)) {
+                ble_ll_resolv_set_local_rpa(connsm->rpa_index, inita);
+            }
+
+#endif
+        } else if (ble_ll_scan_whitelist_enabled()) {
+            /* if WL is used we need to store peer addr also if it was not
+             * resolved
+             */
+            connsm->peer_addr_type = addr_type;
+            memcpy(connsm->peer_addr, addr, BLE_DEV_ADDR_LEN);
+        }
+
+        /* Connection has been created. Stop scanning */
+        g_ble_ll_conn_create_sm = NULL;
+        ble_ll_scan_sm_stop(0);
+
+        /* For AUX Connect CSA2 is mandatory. Otherwise we need to check bit
+         * mask
+         */
+        if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) {
+            ble_ll_conn_set_csa(connsm, 1);
+        } else {
+            ble_ll_conn_set_csa(connsm, rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK);
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        /* Lets take last used phy */
+        ble_ll_conn_init_phy(connsm, ble_hdr->rxinfo.phy);
+#endif
+        if (aux_data) {
+            ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data);
+            ble_hdr->rxinfo.user_data = NULL;
+        }
+#endif
+        ble_ll_conn_created(connsm, NULL);
+        return;
+    }
+
+scan_continue:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* Drop last reference and keep continue to connect */
+    if (aux_data) {
+        ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data);
+        ble_hdr->rxinfo.user_data = NULL;
+    }
+#endif
+    ble_ll_scan_chk_resume();
+}
+
+/**
+ * Called when a receive PDU has started and we are in the initiating state.
+ *
+ * Context: Interrupt
+ *
+ * @param pdu_type
+ * @param ble_hdr
+ *
+ * @return int
+ *  0: we will not attempt to reply to this frame
+ *  1: we may send a response to this frame.
+ */
+int
+ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    connsm = g_ble_ll_conn_create_sm;
+    if (!connsm) {
+        return 0;
+    }
+
+    if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) ||
+        (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND ||
+         pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP)) {
+        return 1;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND &&
+                                                connsm->scansm->ext_scanning) {
+        if (connsm->scansm->cur_aux_data) {
+            STATS_INC(ble_ll_stats, aux_received);
+        }
+
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_EXT_ADV;
+        return 1;
+    }
+#endif
+
+    return 0;
+}
+
+/**
+ * Called when a receive PDU has ended and we are in the initiating state.
+ *
+ * Context: Interrupt
+ *
+ * @param rxpdu
+ * @param crcok
+ * @param ble_hdr
+ *
+ * @return int
+ *       < 0: Disable the phy after reception.
+ *      == 0: Success. Do not disable the PHY.
+ *       > 0: Do not disable PHY as that has already been done.
+ */
+int
+ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
+                       struct ble_mbuf_hdr *ble_hdr)
+{
+    int rc;
+    int resolved;
+    int chk_wl;
+    int index;
+    uint8_t pdu_type;
+    uint8_t adv_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t *adv_addr = NULL;
+    uint8_t *peer;
+    uint8_t *init_addr = NULL;
+    uint8_t init_addr_type;
+    uint8_t pyld_len;
+    uint8_t inita_is_rpa;
+    uint8_t conn_req_end_trans;
+    struct os_mbuf *rxpdu;
+    struct ble_ll_conn_sm *connsm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_scan_sm *scansm;
+    uint8_t phy;
+#endif
+    int ext_adv_mode = -1;
+
+    /* Get connection state machine to use if connection to be established */
+    connsm = g_ble_ll_conn_create_sm;
+    /* This could happen if connection init was cancelled while isr end was
+     * already pending
+     */
+    if (!connsm) {
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        return -1;
+    }
+
+    rc = -1;
+    pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
+    pyld_len = rxbuf[1];
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    scansm = connsm->scansm;
+    if (scansm->cur_aux_data) {
+        ble_hdr->rxinfo.user_data = scansm->cur_aux_data;
+        scansm->cur_aux_data = NULL;
+    }
+#endif
+
+    if (!crcok) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        /* Invalid packet - make sure we do not wait for AUX_CONNECT_RSP */
+        ble_ll_conn_reset_pending_aux_conn_rsp();
+#endif
+
+        /* Ignore this packet */
+        goto init_rx_isr_exit;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* If we sent AUX_CONNECT_REQ, we only expect AUX_CONNECT_RSP here */
+    if (CONN_F_AUX_CONN_REQ(connsm)) {
+        if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) {
+            STATS_INC(ble_ll_stats, aux_conn_rsp_err);
+            CONN_F_CONN_REQ_TXD(connsm) = 0;
+            CONN_F_AUX_CONN_REQ(connsm) = 0;
+            ble_ll_sched_rmv_elem(&connsm->conn_sch);
+        }
+        goto init_rx_isr_exit;
+    }
+#endif
+
+    inita_is_rpa = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) {
+        if (!scansm->ext_scanning) {
+            goto init_rx_isr_exit;
+        }
+
+        rc = ble_ll_scan_update_aux_data(ble_hdr, rxbuf, NULL);
+        if (rc < 0) {
+            /* No memory or broken packet */
+            ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_INVALID;
+            goto init_rx_isr_exit;
+        }
+    }
+#endif
+
+    /* Lets get addresses from advertising report*/
+    if (ble_ll_scan_adv_decode_addr(pdu_type, rxbuf, ble_hdr,
+                                    &adv_addr, &adv_addr_type,
+                                    &init_addr, &init_addr_type,
+                                    &ext_adv_mode)) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_INVALID;
+#endif
+        goto init_rx_isr_exit;
+    }
+
+    switch (pdu_type) {
+    case BLE_ADV_PDU_TYPE_ADV_IND:
+        break;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_ADV_PDU_TYPE_ADV_EXT_IND:
+        rc = -1;
+
+        /* If this is not connectable adv mode, lets skip it */
+        if (!(ext_adv_mode & BLE_LL_EXT_ADV_MODE_CONN)) {
+            goto init_rx_isr_exit;
+        }
+
+        if (!adv_addr) {
+            ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_PTR_WAIT;
+            goto init_rx_isr_exit;
+        }
+
+        if (!init_addr) {
+            break;
+        }
+        /* if there is direct address lets fall down and check it.*/
+        // no break
+#endif
+    case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND:
+        inita_is_rpa = (uint8_t)ble_ll_is_rpa(init_addr, init_addr_type);
+        if (!inita_is_rpa) {
+
+            /* Resolving will be done later. Check if identity InitA matches */
+            if (!ble_ll_is_our_devaddr(init_addr, init_addr_type)) {
+                goto init_rx_isr_exit;
+            }
+        }
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        else {
+            /* If privacy is off - reject RPA InitA*/
+            goto init_rx_isr_exit;
+        }
+#endif
+
+        break;
+    default:
+        goto init_rx_isr_exit;
+    }
+
+    /* Should we send a connect request? */
+    index = -1;
+    peer = adv_addr;
+    peer_addr_type = adv_addr_type;
+
+    resolved = 0;
+    chk_wl = ble_ll_scan_whitelist_enabled();
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (ble_ll_is_rpa(adv_addr, adv_addr_type) && ble_ll_resolv_enabled()) {
+        index = ble_hw_resolv_list_match();
+        if (index >= 0) {
+            rl = &g_ble_ll_resolv_list[index];
+
+            ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_RESOLVED;
+            connsm->rpa_index = index;
+            peer = rl->rl_identity_addr;
+            peer_addr_type = rl->rl_addr_type;
+            resolved = 1;
+
+            /* Assure privacy */
+            if ((rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && init_addr &&
+                !inita_is_rpa && rl->rl_has_local) {
+                goto init_rx_isr_exit;
+            }
+
+            /*
+             * If the InitA is a RPA, we must see if it resolves based on the
+             * identity address of the resolved ADVA.
+             */
+            if (init_addr && inita_is_rpa) {
+                if (!ble_ll_resolv_rpa(init_addr,
+                                       g_ble_ll_resolv_list[index].rl_local_irk)) {
+                    goto init_rx_isr_exit;
+                }
+
+                /* Core Specification Vol 6, Part B, Section 6.4:
+                 * "The Link Layer should not set the InitA field to the same
+                 * value as the TargetA field in the received advertising PDU."
+                 *
+                 * We update the received PDU directly here, so ble_ll_init_rx_pkt_in
+                 * can process it as is.
+                 */
+                memcpy(init_addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
+            }
+
+        } else {
+            if (chk_wl) {
+                goto init_rx_isr_exit;
+            }
+
+            /* Could not resolved InitA */
+            if (init_addr && inita_is_rpa) {
+                goto init_rx_isr_exit;
+            }
+        }
+    } else if (init_addr) {
+
+        /* If resolving is off and InitA is RPA we reject advertising */
+        if (inita_is_rpa && !ble_ll_resolv_enabled()) {
+            goto init_rx_isr_exit;
+        }
+
+        /* Let's see if we have IRK with that peer.*/
+        rl = ble_ll_resolv_list_find(adv_addr, adv_addr_type);
+
+        /* Lets make sure privacy mode is correct together with InitA in case it
+         * is identity address
+         */
+        if (rl && !inita_is_rpa &&
+           (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) &&
+           rl->rl_has_local) {
+            goto init_rx_isr_exit;
+        }
+
+        /*
+         * If the InitA is a RPA, we must see if it resolves based on the
+         * identity address of the resolved ADVA.
+         */
+        if (inita_is_rpa) {
+            if (!rl || !ble_ll_resolv_rpa(init_addr, rl->rl_local_irk)) {
+                goto init_rx_isr_exit;
+            }
+
+            /* Core Specification Vol 6, Part B, Section 6.4:
+             * "The Link Layer should not set the InitA field to the same
+             * value as the TargetA field in the received advertising PDU."
+             *
+             * We update the received PDU directly here, so ble_ll_init_rx_pkt_in
+             * can process it as is.
+             */
+            memcpy(init_addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
+        }
+    }
+#endif
+
+    /* Check filter policy */
+    if (chk_wl) {
+        if (!ble_ll_whitelist_match(peer, peer_addr_type, resolved)) {
+            goto init_rx_isr_exit;
+        }
+    } else {
+        /* Must match the connection address */
+        if (!ble_ll_conn_is_peer_adv(adv_addr_type, adv_addr, index)) {
+            goto init_rx_isr_exit;
+        }
+    }
+    ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH;
+
+    /* For CONNECT_IND we don't go into RX state */
+    conn_req_end_trans = BLE_PHY_TRANSITION_NONE;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* Check if we should send AUX_CONNECT_REQ and wait for AUX_CONNECT_RSP */
+    if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) {
+        conn_req_end_trans = BLE_PHY_TRANSITION_TX_RX;
+    }
+
+    if (connsm->scansm->ext_scanning) {
+            phy = ble_hdr->rxinfo.phy;
+
+            /* Update connection state machine with appropriate parameters for
+             * certain PHY
+             */
+            ble_ll_conn_ext_set_params(connsm,
+                                       &connsm->initial_params.params[phy - 1],
+                                       phy);
+
+    }
+#endif
+
+    /* Schedule new connection */
+    if (ble_ll_sched_master_new(connsm, ble_hdr, pyld_len)) {
+        STATS_INC(ble_ll_conn_stats, cant_set_sched);
+        goto init_rx_isr_exit;
+    }
+
+    /* Prepare data for connect request */
+    ble_ll_conn_connect_ind_prepare(connsm,
+                                    ble_ll_scan_get_pdu_data(),
+                                    adv_addr_type, adv_addr,
+                                    init_addr_type, init_addr,
+                                    index, ble_hdr->rxinfo.channel);
+
+    /* Setup to transmit the connect request */
+    rc = ble_ll_conn_connect_ind_send(connsm, conn_req_end_trans);
+    if (rc) {
+        ble_ll_sched_rmv_elem(&connsm->conn_sch);
+        goto init_rx_isr_exit;
+    }
+
+    if (init_addr && !inita_is_rpa) {
+        connsm->inita_identity_used = 1;
+    }
+
+    CONN_F_CONN_REQ_TXD(connsm) = 1;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) {
+        /* Lets wait for AUX_CONNECT_RSP */
+        CONN_F_AUX_CONN_REQ(connsm) = 1;
+        /* Keep aux data until we get scan response */
+        scansm->cur_aux_data = ble_hdr->rxinfo.user_data;
+        ble_hdr->rxinfo.user_data = NULL;
+        STATS_INC(ble_ll_stats, aux_conn_req_tx);
+    }
+#endif
+
+    STATS_INC(ble_ll_conn_stats, conn_req_txd);
+
+init_rx_isr_exit:
+
+    /*
+     * We have to restart receive if we cant hand up pdu. We return 0 so that
+     * the phy does not get disabled.
+     */
+    rxpdu = ble_ll_rxpdu_alloc(pyld_len + BLE_LL_PDU_HDR_LEN);
+    if (rxpdu == NULL) {
+        /*
+         * XXX: possible allocate the PDU when we start initiating?
+         * I cannot say I like this solution, but if we cannot allocate a PDU
+         * to hand up to the LL, we need to remove the connection we just
+         * scheduled since the connection state machine will not get processed
+         * by link layer properly. For now, just remove it from the scheduler
+         */
+        if (CONN_F_CONN_REQ_TXD(connsm) == 1) {
+            CONN_F_CONN_REQ_TXD(connsm) = 0;
+            CONN_F_AUX_CONN_REQ(connsm) = 0;
+            ble_ll_sched_rmv_elem(&connsm->conn_sch);
+        }
+        ble_phy_restart_rx();
+        rc = 0;
+    } else {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
+    if (rc) {
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    }
+
+    return rc;
+}
+
+/**
+ * Function called when a timeout has occurred for a connection. There are
+ * two types of timeouts: a connection supervision timeout and control
+ * procedure timeout.
+ *
+ * Context: Link Layer task
+ *
+ * @param connsm
+ * @param ble_err
+ */
+void
+ble_ll_conn_timeout(struct ble_ll_conn_sm *connsm, uint8_t ble_err)
+{
+    int was_current;
+    os_sr_t sr;
+
+    was_current = 0;
+    OS_ENTER_CRITICAL(sr);
+    if (g_ble_ll_conn_cur_sm == connsm) {
+        ble_ll_conn_current_sm_over(NULL);
+        was_current = 1;
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    /* Check if we need to resume scanning */
+    if (was_current) {
+        ble_ll_scan_chk_resume();
+    }
+
+    ble_ll_conn_end(connsm, ble_err);
+}
+
+/**
+ * Called when a data channel PDU has started that matches the access
+ * address of the current connection. Note that the CRC of the PDU has not
+ * been checked yet.
+ *
+ * Context: Interrupt
+ *
+ * @param rxhdr
+ */
+int
+ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa)
+{
+    struct ble_ll_conn_sm *connsm;
+
+    /*
+     * Disable wait for response timer since we receive a response. We dont
+     * care if this is the response we were waiting for or not; the code
+     * called at receive end will deal with ending the connection event
+     * if needed
+     */
+    connsm = g_ble_ll_conn_cur_sm;
+    if (connsm) {
+        /* Double check access address. Better match connection state machine */
+        if (aa != connsm->access_addr) {
+            STATS_INC(ble_ll_conn_stats, rx_data_pdu_bad_aa);
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+            ble_ll_event_send(&connsm->conn_ev_end);
+            g_ble_ll_conn_cur_sm = NULL;
+            return -1;
+        }
+
+        /* Set connection handle in mbuf header */
+        rxhdr->rxinfo.handle = connsm->conn_handle;
+
+        /* Set flag denoting we have received a packet in connection event */
+        connsm->csmflags.cfbit.pkt_rxd = 1;
+
+        /* Connection is established */
+        connsm->conn_state = BLE_LL_CONN_STATE_ESTABLISHED;
+
+        /* Set anchor point (and last) if 1st rxd frame in connection event */
+        if (connsm->csmflags.cfbit.slave_set_last_anchor) {
+            connsm->csmflags.cfbit.slave_set_last_anchor = 0;
+            connsm->last_anchor_point = rxhdr->beg_cputime;
+            connsm->anchor_point = connsm->last_anchor_point;
+            connsm->anchor_point_usecs = rxhdr->rem_usecs;
+        }
+    }
+    return 1;
+}
+
+/**
+ * Called from the Link Layer task when a data PDU has been received
+ *
+ * Context: Link layer task
+ *
+ * @param rxpdu Pointer to received pdu
+ * @param rxpdu Pointer to ble mbuf header of received pdu
+ */
+void
+ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
+{
+    uint8_t hdr_byte;
+    uint8_t rxd_sn;
+    uint8_t *rxbuf;
+    uint8_t llid;
+    uint16_t acl_len;
+    uint16_t acl_hdr;
+    struct ble_ll_conn_sm *connsm;
+
+    if (BLE_MBUF_HDR_CRC_OK(hdr)) {
+        /* XXX: there is a chance that the connection was thrown away and
+           re-used before processing packets here. Fix this. */
+        /* We better have a connection state machine */
+        connsm = ble_ll_conn_find_active_conn(hdr->rxinfo.handle);
+        if (connsm) {
+            /* Check state machine */
+            ble_ll_conn_chk_csm_flags(connsm);
+
+            /* Validate rx data pdu */
+            rxbuf = rxpdu->om_data;
+            hdr_byte = rxbuf[0];
+            acl_len = rxbuf[1];
+            llid = hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+
+            /*
+             * Check that the LLID and payload length are reasonable.
+             * Empty payload is only allowed for LLID == 01b.
+             *  */
+            if ((llid == 0) ||
+                ((acl_len == 0) && (llid != BLE_LL_LLID_DATA_FRAG))) {
+                STATS_INC(ble_ll_conn_stats, rx_bad_llid);
+                goto conn_rx_data_pdu_end;
+            }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+            /* Check if PDU is allowed when encryption is started. If not,
+             * terminate connection.
+             *
+             * Reference: Core 5.0, Vol 6, Part B, 5.1.3.1
+             */
+            if ((connsm->enc_data.enc_state > CONN_ENC_S_PAUSE_ENC_RSP_WAIT) &&
+                    !ble_ll_ctrl_enc_allowed_pdu_rx(rxpdu)) {
+                ble_ll_conn_timeout(connsm, BLE_ERR_CONN_TERM_MIC);
+                goto conn_rx_data_pdu_end;
+            }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+            /*
+             * Reset authenticated payload timeout if valid MIC. NOTE: we dont
+             * check the MIC failure bit as that would have terminated the
+             * connection
+             */
+            if ((connsm->enc_data.enc_state == CONN_ENC_S_ENCRYPTED) &&
+                CONN_F_LE_PING_SUPP(connsm) && (acl_len != 0)) {
+                ble_ll_conn_auth_pyld_timer_start(connsm);
+            }
+#endif
+
+            /* Update RSSI */
+            connsm->conn_rssi = hdr->rxinfo.rssi;
+
+            /*
+             * If we are a slave, we can only start to use slave latency
+             * once we have received a NESN of 1 from the master
+             */
+            if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+                if (hdr_byte & BLE_LL_DATA_HDR_NESN_MASK) {
+                    connsm->csmflags.cfbit.allow_slave_latency = 1;
+                }
+            }
+
+            /*
+             * Discard the received PDU if the sequence number is the same
+             * as the last received sequence number
+             */
+            rxd_sn = hdr_byte & BLE_LL_DATA_HDR_SN_MASK;
+            if (rxd_sn != connsm->last_rxd_sn) {
+                /* Update last rxd sn */
+                connsm->last_rxd_sn = rxd_sn;
+
+                /* No need to do anything if empty pdu */
+                if ((llid == BLE_LL_LLID_DATA_FRAG) && (acl_len == 0)) {
+                    goto conn_rx_data_pdu_end;
+                }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+                /*
+                 * XXX: should we check to see if we are in a state where we
+                 * might expect to get an encrypted PDU?
+                 */
+                if (BLE_MBUF_HDR_MIC_FAILURE(hdr)) {
+                    STATS_INC(ble_ll_conn_stats, mic_failures);
+                    ble_ll_conn_timeout(connsm, BLE_ERR_CONN_TERM_MIC);
+                    goto conn_rx_data_pdu_end;
+                }
+#endif
+
+                if (llid == BLE_LL_LLID_CTRL) {
+                    /* Process control frame */
+                    STATS_INC(ble_ll_conn_stats, rx_ctrl_pdus);
+                    if (ble_ll_ctrl_rx_pdu(connsm, rxpdu)) {
+                        STATS_INC(ble_ll_conn_stats, rx_malformed_ctrl_pdus);
+                    }
+                } else {
+                    /* Count # of received l2cap frames and byes */
+                    STATS_INC(ble_ll_conn_stats, rx_l2cap_pdus);
+                    STATS_INCN(ble_ll_conn_stats, rx_l2cap_bytes, acl_len);
+
+                    /* NOTE: there should be at least two bytes available */
+                    BLE_LL_ASSERT(OS_MBUF_LEADINGSPACE(rxpdu) >= 2);
+                    os_mbuf_prepend(rxpdu, 2);
+                    rxbuf = rxpdu->om_data;
+
+                    acl_hdr = (llid << 12) | connsm->conn_handle;
+                    put_le16(rxbuf, acl_hdr);
+                    put_le16(rxbuf + 2, acl_len);
+                    ble_hci_trans_ll_acl_tx(rxpdu);
+                }
+
+                /* NOTE: we dont free the mbuf since we handed it off! */
+                return;
+            } else {
+                STATS_INC(ble_ll_conn_stats, data_pdu_rx_dup);
+            }
+        } else {
+            STATS_INC(ble_ll_conn_stats, no_conn_sm);
+        }
+    }
+
+    /* Free buffer */
+conn_rx_data_pdu_end:
+    os_mbuf_free_chain(rxpdu);
+}
+
+/**
+ * Called when a packet has been received while in the connection state.
+ *
+ * Context: Interrupt
+ *
+ * @param rxpdu
+ * @param crcok
+ *
+ * @return int
+ *       < 0: Disable the phy after reception.
+ *      == 0: Success. Do not disable the PHY.
+ *       > 0: Do not disable PHY as that has already been done.
+ */
+int
+ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    int rc;
+    int is_ctrl;
+    uint8_t hdr_byte;
+    uint8_t hdr_sn;
+    uint8_t hdr_nesn;
+    uint8_t conn_sn;
+    uint8_t conn_nesn;
+    uint8_t reply;
+    uint8_t rem_bytes;
+    uint8_t opcode = 0;
+    uint8_t rx_pyld_len;
+    uint32_t begtime;
+    uint32_t add_usecs;
+    struct os_mbuf *txpdu;
+    struct ble_ll_conn_sm *connsm;
+    struct os_mbuf *rxpdu;
+    struct ble_mbuf_hdr *txhdr;
+    int rx_phy_mode;
+
+    /* Retrieve the header and payload length */
+    hdr_byte = rxbuf[0];
+    rx_pyld_len = rxbuf[1];
+
+    /*
+     * We need to attempt to allocate a buffer here. The reason we do this
+     * now is that we should not ack the packet if we have no receive
+     * buffers available. We want to free up our transmit PDU if it was
+     * acked, but we should not ack the received frame if we cant hand it up.
+     * NOTE: we hand up empty pdu's to the LL task!
+     */
+    rxpdu = ble_ll_rxpdu_alloc(rx_pyld_len + BLE_LL_PDU_HDR_LEN);
+
+    /*
+     * We should have a current connection state machine. If we dont, we just
+     * hand the packet to the higher layer to count it.
+     */
+    rc = -1;
+    connsm = g_ble_ll_conn_cur_sm;
+    if (!connsm) {
+        STATS_INC(ble_ll_conn_stats, rx_data_pdu_no_conn);
+        goto conn_exit;
+    }
+
+    /*
+     * Calculate the end time of the received PDU. NOTE: this looks strange
+     * but for the 32768 crystal we add the time it takes to send the packet
+     * to the 'additional usecs' field to save some calculations.
+     */
+    begtime = rxhdr->beg_cputime;
+#if BLE_LL_BT5_PHY_SUPPORTED
+    rx_phy_mode = connsm->phy_data.rx_phy_mode;
+#else
+    rx_phy_mode = BLE_PHY_MODE_1M;
+#endif
+    add_usecs = rxhdr->rem_usecs +
+            ble_ll_pdu_tx_time_get(rx_pyld_len, rx_phy_mode);
+
+    /*
+     * Check the packet CRC. A connection event can continue even if the
+     * received PDU does not pass the CRC check. If we receive two consecutive
+     * CRC errors we end the conection event.
+     */
+    if (!BLE_MBUF_HDR_CRC_OK(rxhdr)) {
+        /*
+         * Increment # of consecutively received CRC errors. If more than
+         * one we will end the connection event.
+         */
+        ++connsm->cons_rxd_bad_crc;
+        if (connsm->cons_rxd_bad_crc >= 2) {
+            reply = 0;
+        } else {
+            if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+                reply = CONN_F_LAST_TXD_MD(connsm);
+            } else {
+                /* A slave always responds with a packet */
+                reply = 1;
+            }
+        }
+    } else {
+        /* Reset consecutively received bad crcs (since this one was good!) */
+        connsm->cons_rxd_bad_crc = 0;
+
+        /* Set last valid received pdu time (resets supervision timer) */
+        connsm->last_rxd_pdu_cputime = begtime +
+                                        os_cputime_usecs_to_ticks(add_usecs);
+
+        /*
+         * Check for valid LLID before proceeding. We have seen some weird
+         * things with the PHY where the CRC is OK but we dont have a valid
+         * LLID. This should really never happen but if it does we will just
+         * bail. An error stat will get incremented at the LL.
+         */
+        if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == 0) {
+            goto conn_exit;
+        }
+
+        /* Set last received header byte */
+        connsm->last_rxd_hdr_byte = hdr_byte;
+
+        is_ctrl = 0;
+        if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) {
+            is_ctrl = 1;
+            opcode = rxbuf[2];
+        }
+
+        /*
+         * If SN bit from header does not match NESN in connection, this is
+         * a resent PDU and should be ignored.
+         */
+        hdr_sn = hdr_byte & BLE_LL_DATA_HDR_SN_MASK;
+        conn_nesn = connsm->next_exp_seqnum;
+        if (rxpdu && ((hdr_sn && conn_nesn) || (!hdr_sn && !conn_nesn))) {
+            connsm->next_exp_seqnum ^= 1;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+            if (CONN_F_ENCRYPTED(connsm) && !ble_ll_conn_is_empty_pdu(rxbuf)) {
+                ++connsm->enc_data.rx_pkt_cntr;
+            }
+#endif
+        }
+
+        ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CONN_RX, connsm->tx_seqnum,
+                           !!(hdr_byte & BLE_LL_DATA_HDR_NESN_MASK));
+
+        /*
+         * Check NESN bit from header. If same as tx seq num, the transmission
+         * is acknowledged. Otherwise we need to resend this PDU.
+         */
+        if (CONN_F_EMPTY_PDU_TXD(connsm) || connsm->cur_tx_pdu) {
+            hdr_nesn = hdr_byte & BLE_LL_DATA_HDR_NESN_MASK;
+            conn_sn = connsm->tx_seqnum;
+            if ((hdr_nesn && conn_sn) || (!hdr_nesn && !conn_sn)) {
+                /* We did not get an ACK. Must retry the PDU */
+                STATS_INC(ble_ll_conn_stats, data_pdu_txf);
+            } else {
+                /* Transmit success */
+                connsm->tx_seqnum ^= 1;
+                STATS_INC(ble_ll_conn_stats, data_pdu_txg);
+
+                /* If we transmitted the empty pdu, clear flag */
+                if (CONN_F_EMPTY_PDU_TXD(connsm)) {
+                    CONN_F_EMPTY_PDU_TXD(connsm) = 0;
+                    goto chk_rx_terminate_ind;
+                }
+
+                /*
+                 * Determine if we should remove packet from queue or if there
+                 * are more fragments to send.
+                 */
+                txpdu = connsm->cur_tx_pdu;
+                if (txpdu) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+                    if (connsm->enc_data.tx_encrypted) {
+                        ++connsm->enc_data.tx_pkt_cntr;
+                    }
+#endif
+                    txhdr = BLE_MBUF_HDR_PTR(txpdu);
+                    if ((txhdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK)
+                        == BLE_LL_LLID_CTRL) {
+                        connsm->cur_tx_pdu = NULL;
+                        /* Note: the mbuf is freed by this call */
+                        rc = ble_ll_ctrl_tx_done(txpdu, connsm);
+                        if (rc) {
+                            /* Means we transmitted a TERMINATE_IND */
+                            goto conn_exit;
+                        } else {
+                            goto chk_rx_terminate_ind;
+                        }
+                    }
+
+                    /* Increment offset based on number of bytes sent */
+                    txhdr->txinfo.offset += txhdr->txinfo.pyld_len;
+                    if (txhdr->txinfo.offset >= OS_MBUF_PKTLEN(txpdu)) {
+                        /* If l2cap pdu, increment # of completed packets */
+                        if (txhdr->txinfo.pyld_len != 0) {
+#if (BLETEST_THROUGHPUT_TEST == 1)
+                            bletest_completed_pkt(connsm->conn_handle);
+#endif
+                            ++connsm->completed_pkts;
+                            if (connsm->completed_pkts > 2) {
+                                ble_npl_eventq_put(&g_ble_ll_data.ll_evq,
+                                                   &g_ble_ll_data.ll_comp_pkt_ev);
+                            }
+                        }
+                        os_mbuf_free_chain(txpdu);
+                        connsm->cur_tx_pdu = NULL;
+                    } else {
+                        rem_bytes = OS_MBUF_PKTLEN(txpdu) - txhdr->txinfo.offset;
+                        /* Adjust payload for max TX time and octets */
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+                        if (is_ctrl &&
+                            (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) &&
+                            (opcode == BLE_LL_CTRL_PHY_UPDATE_IND)) {
+                            connsm->phy_tx_transition =
+                                    ble_ll_ctrl_phy_tx_transition_get(rxbuf[3]);
+                        }
+#endif
+
+                        rem_bytes = ble_ll_conn_adjust_pyld_len(connsm, rem_bytes);
+                        txhdr->txinfo.pyld_len = rem_bytes;
+                    }
+                }
+            }
+        }
+
+        /* Should we continue connection event? */
+        /* If this is a TERMINATE_IND, we have to reply */
+chk_rx_terminate_ind:
+        /* If we received a terminate IND, we must set some flags */
+        if (is_ctrl && (opcode == BLE_LL_CTRL_TERMINATE_IND)
+                    && (rx_pyld_len == (1 + BLE_LL_CTRL_TERMINATE_IND_LEN))) {
+            connsm->csmflags.cfbit.terminate_ind_rxd = 1;
+            connsm->rxd_disconnect_reason = rxbuf[3];
+        }
+
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+            reply = CONN_F_LAST_TXD_MD(connsm) || (hdr_byte & BLE_LL_DATA_HDR_MD_MASK);
+        } else {
+            /* A slave always replies */
+            reply = 1;
+        }
+    }
+
+    /* If reply flag set, send data pdu and continue connection event */
+    rc = -1;
+    if (rx_pyld_len && CONN_F_ENCRYPTED(connsm)) {
+        rx_pyld_len += BLE_LL_DATA_MIC_LEN;
+    }
+    if (reply && ble_ll_conn_can_send_next_pdu(connsm, begtime, add_usecs)) {
+        rc = ble_ll_conn_tx_pdu(connsm);
+    }
+
+conn_exit:
+    /* Copy the received pdu and hand it up */
+    if (rxpdu) {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
+    /* Send link layer a connection end event if over */
+    if (rc) {
+        ble_ll_conn_current_sm_over(connsm);
+    }
+
+    return rc;
+}
+
+/**
+ * Called to adjust payload length to fit into max effective octets and TX time
+ * on current PHY.
+ */
+/**
+ * Called to enqueue a packet on the transmit queue of a connection. Should
+ * only be called by the controller.
+ *
+ * Context: Link Layer
+ *
+ *
+ * @param connsm
+ * @param om
+ */
+void
+ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
+                        uint8_t hdr_byte, uint8_t length)
+{
+    os_sr_t sr;
+    struct os_mbuf_pkthdr *pkthdr;
+    struct ble_mbuf_hdr *ble_hdr;
+    int lifo;
+
+    /* Set mbuf length and packet length if a control PDU */
+    if (hdr_byte == BLE_LL_LLID_CTRL) {
+        om->om_len = length;
+        OS_MBUF_PKTHDR(om)->omp_len = length;
+    }
+
+    /* Set BLE transmit header */
+    ble_hdr = BLE_MBUF_HDR_PTR(om);
+    ble_hdr->txinfo.flags = 0;
+    ble_hdr->txinfo.offset = 0;
+    ble_hdr->txinfo.hdr_byte = hdr_byte;
+
+    /*
+     * Initial payload length is calculate when packet is dequeued, there's no
+     * need to do this now.
+     */
+
+    lifo = 0;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+        uint8_t llid;
+
+        /*
+         * If this is one of the following types we need to insert it at
+         * head of queue.
+         */
+        llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+        if (llid == BLE_LL_LLID_CTRL) {
+            switch (om->om_data[0]) {
+            case BLE_LL_CTRL_TERMINATE_IND:
+            case BLE_LL_CTRL_REJECT_IND:
+            case BLE_LL_CTRL_REJECT_IND_EXT:
+            case BLE_LL_CTRL_START_ENC_REQ:
+            case BLE_LL_CTRL_START_ENC_RSP:
+                lifo = 1;
+                break;
+            case BLE_LL_CTRL_PAUSE_ENC_RSP:
+                if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+                    lifo = 1;
+                }
+                break;
+            case BLE_LL_CTRL_ENC_REQ:
+            case BLE_LL_CTRL_ENC_RSP:
+                /* If encryption has been paused, we don't want to send any packets from the
+                 * TX queue, as they would go unencrypted.
+                 */
+                if (connsm->enc_data.enc_state == CONN_ENC_S_PAUSED) {
+                    lifo = 1;
+                }
+                break;
+            default:
+                break;
+            }
+        }
+    }
+#endif
+
+    /* Add to transmit queue for the connection */
+    pkthdr = OS_MBUF_PKTHDR(om);
+    OS_ENTER_CRITICAL(sr);
+    if (lifo) {
+        STAILQ_INSERT_HEAD(&connsm->conn_txq, pkthdr, omp_next);
+    } else {
+        STAILQ_INSERT_TAIL(&connsm->conn_txq, pkthdr, omp_next);
+    }
+    OS_EXIT_CRITICAL(sr);
+}
+
+/**
+ * Data packet from host.
+ *
+ * Context: Link Layer task
+ *
+ * @param om
+ * @param handle
+ * @param length
+ *
+ * @return int
+ */
+void
+ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t length)
+{
+    uint8_t hdr_byte;
+    uint16_t conn_handle;
+    uint16_t pb;
+    struct ble_ll_conn_sm *connsm;
+
+    /* See if we have an active matching connection handle */
+    conn_handle = handle & 0x0FFF;
+    connsm = ble_ll_conn_find_active_conn(conn_handle);
+    if (connsm) {
+        /* Construct LL header in buffer (NOTE: pb already checked) */
+        pb = handle & 0x3000;
+        if (pb == 0) {
+            hdr_byte = BLE_LL_LLID_DATA_START;
+        } else {
+            hdr_byte = BLE_LL_LLID_DATA_FRAG;
+        }
+
+        /* Add to total l2cap pdus enqueue */
+        STATS_INC(ble_ll_conn_stats, l2cap_enqueued);
+
+        /* Clear flags field in BLE header */
+        ble_ll_conn_enqueue_pkt(connsm, om, hdr_byte, length);
+    } else {
+        /* No connection found! */
+        STATS_INC(ble_ll_conn_stats, handle_not_found);
+        os_mbuf_free_chain(om);
+    }
+}
+
+/**
+ * Called to set the global channel mask that we use for all connections.
+ *
+ * @param num_used_chans
+ * @param chanmap
+ */
+void
+ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, const uint8_t *chanmap)
+{
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_conn_global_params *conn_params;
+
+    /* Do nothing if same channel map */
+    conn_params = &g_ble_ll_conn_params;
+    if (!memcmp(conn_params->master_chan_map, chanmap, BLE_LL_CONN_CHMAP_LEN)) {
+        return;
+    }
+
+    /* Change channel map and cause channel map update procedure to start */
+    conn_params->num_used_chans = num_used_chans;
+    memcpy(conn_params->master_chan_map, chanmap, BLE_LL_CONN_CHMAP_LEN);
+
+    /* Perform channel map update */
+    SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) {
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+            ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CHAN_MAP_UPD);
+        }
+    }
+}
+
+/**
+ * Called when a device has received a connect request while advertising and
+ * the connect request has passed the advertising filter policy and is for
+ * us. This will start a connection in the slave role assuming that we dont
+ * already have a connection with this device and that the connect request
+ * parameters are valid.
+ *
+ * Context: Link Layer
+ *
+ * @param rxbuf Pointer to received Connect Request PDU
+ *
+ * @return 0: connection not started; 1 connecton started
+ */
+int
+ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, struct ble_mbuf_hdr *rxhdr,
+                        bool force_csa2)
+{
+    int rc;
+    uint32_t temp;
+    uint32_t crcinit;
+    uint8_t *inita;
+    uint8_t *dptr;
+    struct ble_ll_conn_sm *connsm;
+
+    /* Ignore the connection request if we are already connected*/
+    inita = rxbuf + BLE_LL_PDU_HDR_LEN;
+    SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) {
+        if (!memcmp(&connsm->peer_addr, inita, BLE_DEV_ADDR_LEN)) {
+            if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) {
+                if (connsm->peer_addr_type & 1) {
+                    return 0;
+                }
+            } else {
+                if ((connsm->peer_addr_type & 1) == 0) {
+                    return 0;
+                }
+            }
+        }
+    }
+
+    /* Allocate a connection. If none available, dont do anything */
+    connsm = ble_ll_conn_sm_get();
+    if (connsm == NULL) {
+        return 0;
+    }
+
+    /* Set the pointer at the start of the connection data */
+    dptr = rxbuf + BLE_LL_CONN_REQ_ADVA_OFF + BLE_DEV_ADDR_LEN;
+
+    /* Set connection state machine information */
+    connsm->access_addr = get_le32(dptr);
+    crcinit = dptr[6];
+    crcinit = (crcinit << 8) | dptr[5];
+    crcinit = (crcinit << 8) | dptr[4];
+    connsm->crcinit = crcinit;
+    connsm->tx_win_size = dptr[7];
+    connsm->tx_win_off = get_le16(dptr + 8);
+    connsm->conn_itvl = get_le16(dptr + 10);
+    connsm->slave_latency = get_le16(dptr + 12);
+    connsm->supervision_tmo = get_le16(dptr + 14);
+    memcpy(&connsm->chanmap, dptr + 16, BLE_LL_CONN_CHMAP_LEN);
+    connsm->hop_inc = dptr[21] & 0x1F;
+    connsm->master_sca = dptr[21] >> 5;
+
+    /* Error check parameters */
+    if ((connsm->tx_win_off > connsm->conn_itvl) ||
+        (connsm->conn_itvl < BLE_HCI_CONN_ITVL_MIN) ||
+        (connsm->conn_itvl > BLE_HCI_CONN_ITVL_MAX) ||
+        (connsm->tx_win_size < BLE_LL_CONN_TX_WIN_MIN) ||
+        (connsm->slave_latency > BLE_LL_CONN_SLAVE_LATENCY_MAX)) {
+        goto err_slave_start;
+    }
+
+    /* Slave latency cannot cause a supervision timeout */
+    temp = (connsm->slave_latency + 1) * (connsm->conn_itvl * 2) *
+            BLE_LL_CONN_ITVL_USECS;
+    if ((connsm->supervision_tmo * 10000) <= temp ) {
+        goto err_slave_start;
+    }
+
+    /*
+     * The transmit window must be less than or equal to the lesser of 10
+     * msecs or the connection interval minus 1.25 msecs.
+     */
+    temp = connsm->conn_itvl - 1;
+    if (temp > 8) {
+        temp = 8;
+    }
+    if (connsm->tx_win_size > temp) {
+        goto err_slave_start;
+    }
+
+    /* Set the address of device that we are connecting with */
+    memcpy(&connsm->peer_addr, inita, BLE_DEV_ADDR_LEN);
+    connsm->peer_addr_type = pat;
+
+    /* Calculate number of used channels; make sure it meets min requirement */
+    connsm->num_used_chans = ble_ll_utils_calc_num_used_chans(connsm->chanmap);
+    if (connsm->num_used_chans < 2) {
+        goto err_slave_start;
+    }
+
+    /* Start the connection state machine */
+    connsm->conn_role = BLE_LL_CONN_ROLE_SLAVE;
+    ble_ll_conn_sm_new(connsm);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    /* Use the same PHY as we received CONNECT_REQ on */
+    ble_ll_conn_init_phy(connsm, rxhdr->rxinfo.phy);
+#endif
+
+    ble_ll_conn_set_csa(connsm,
+                        force_csa2 || (rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK));
+
+    /* Set initial schedule callback */
+    connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb;
+    rc = ble_ll_conn_created(connsm, rxhdr);
+    if (!rc) {
+        SLIST_REMOVE(&g_ble_ll_conn_active_list, connsm, ble_ll_conn_sm, act_sle);
+        STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe);
+    }
+    return rc;
+
+err_slave_start:
+    STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe);
+    STATS_INC(ble_ll_conn_stats, slave_rxd_bad_conn_req_params);
+    return 0;
+}
+
+#define MAX_TIME_UNCODED(_maxbytes) \
+        ble_ll_pdu_tx_time_get(_maxbytes + BLE_LL_DATA_MIC_LEN, \
+                               BLE_PHY_MODE_1M);
+#define MAX_TIME_CODED(_maxbytes) \
+        ble_ll_pdu_tx_time_get(_maxbytes + BLE_LL_DATA_MIC_LEN, \
+                               BLE_PHY_MODE_CODED_125KBPS);
+
+/**
+ * Called to reset the connection module. When this function is called the
+ * scheduler has been stopped and the phy has been disabled. The LL should
+ * be in the standby state.
+ *
+ * Context: Link Layer task
+ */
+void
+ble_ll_conn_module_reset(void)
+{
+    uint8_t max_phy_pyld;
+    uint16_t maxbytes;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_conn_global_params *conn_params;
+
+    /* Kill the current one first (if one is running) */
+    if (g_ble_ll_conn_cur_sm) {
+        connsm = g_ble_ll_conn_cur_sm;
+        g_ble_ll_conn_cur_sm = NULL;
+        ble_ll_conn_end(connsm, BLE_ERR_SUCCESS);
+    }
+
+    /* Free the global connection complete event if there is one */
+    if (g_ble_ll_conn_comp_ev) {
+        ble_hci_trans_buf_free(g_ble_ll_conn_comp_ev);
+        g_ble_ll_conn_comp_ev = NULL;
+    }
+
+    /* Reset connection we are attempting to create */
+    g_ble_ll_conn_create_sm = NULL;
+
+    /* Now go through and end all the connections */
+    while (1) {
+        connsm = SLIST_FIRST(&g_ble_ll_conn_active_list);
+        if (!connsm) {
+            break;
+        }
+        ble_ll_conn_end(connsm, BLE_ERR_SUCCESS);
+    }
+
+    /* Get the maximum supported PHY PDU size from the PHY */
+    max_phy_pyld = ble_phy_max_data_pdu_pyld();
+
+    /* Configure the global LL parameters */
+    conn_params = &g_ble_ll_conn_params;
+
+    maxbytes = min(MYNEWT_VAL(BLE_LL_SUPP_MAX_RX_BYTES), max_phy_pyld);
+    conn_params->supp_max_rx_octets = maxbytes;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    conn_params->supp_max_rx_time = MAX_TIME_CODED(maxbytes);
+#else
+    conn_params->supp_max_rx_time = MAX_TIME_UNCODED(maxbytes);
+#endif
+
+    maxbytes = min(MYNEWT_VAL(BLE_LL_SUPP_MAX_TX_BYTES), max_phy_pyld);
+    conn_params->supp_max_tx_octets = maxbytes;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    conn_params->supp_max_tx_time = MAX_TIME_CODED(maxbytes);
+#else
+    conn_params->supp_max_tx_time = MAX_TIME_UNCODED(maxbytes);
+#endif
+
+    maxbytes = min(MYNEWT_VAL(BLE_LL_CONN_INIT_MAX_TX_BYTES), max_phy_pyld);
+    conn_params->conn_init_max_tx_octets = maxbytes;
+    conn_params->conn_init_max_tx_time = MAX_TIME_UNCODED(maxbytes);
+    conn_params->conn_init_max_tx_time_uncoded = MAX_TIME_UNCODED(maxbytes);
+    conn_params->conn_init_max_tx_time_coded = MAX_TIME_CODED(maxbytes);
+
+    conn_params->sugg_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+    conn_params->sugg_tx_time = BLE_LL_CONN_SUPP_TIME_MIN;
+
+    /* Mask in all channels by default */
+    conn_params->num_used_chans = BLE_PHY_NUM_DATA_CHANS;
+    memset(conn_params->master_chan_map, 0xff, BLE_LL_CONN_CHMAP_LEN - 1);
+    conn_params->master_chan_map[4] = 0x1f;
+
+    /* Reset statistics */
+    STATS_RESET(ble_ll_conn_stats);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    /* reset default sync transfer params */
+    g_ble_ll_conn_sync_transfer_params.max_skip = 0;
+    g_ble_ll_conn_sync_transfer_params.mode = 0;
+    g_ble_ll_conn_sync_transfer_params.sync_timeout_us = 0;
+#endif
+}
+
+/* Initialize the connection module */
+void
+ble_ll_conn_module_init(void)
+{
+    int rc;
+    uint16_t i;
+    struct ble_ll_conn_sm *connsm;
+
+    /* Initialize list of active conections */
+    SLIST_INIT(&g_ble_ll_conn_active_list);
+    STAILQ_INIT(&g_ble_ll_conn_free_list);
+
+    /*
+     * Take all the connections off the free memory pool and add them to
+     * the free connection list, assigning handles in linear order. Note:
+     * the specification allows a handle of zero; we just avoid using it.
+     */
+    connsm = &g_ble_ll_conn_sm[0];
+    for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); ++i) {
+
+        memset(connsm, 0, sizeof(struct ble_ll_conn_sm));
+        connsm->conn_handle = i + 1;
+        STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe);
+
+        /* Initialize fixed schedule elements */
+        connsm->conn_sch.sched_type = BLE_LL_SCHED_TYPE_CONN;
+        connsm->conn_sch.cb_arg = connsm;
+        ++connsm;
+    }
+
+    /* Register connection statistics */
+    rc = stats_init_and_reg(STATS_HDR(ble_ll_conn_stats),
+                            STATS_SIZE_INIT_PARMS(ble_ll_conn_stats, STATS_SIZE_32),
+                            STATS_NAME_INIT_PARMS(ble_ll_conn_stats),
+                            "ble_ll_conn");
+    BLE_LL_ASSERT(rc == 0);
+
+    /* Call reset to finish reset of initialization */
+    ble_ll_conn_module_reset();
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_hci.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_hci.c
new file mode 100644
index 00000000..1350fdc0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_hci.c
@@ -0,0 +1,1896 @@
+/*
+ * 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 <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_utils.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_conn.h"
+#include "controller/ble_ll_ctrl.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_adv.h"
+#include "ble_ll_conn_priv.h"
+
+/*
+ * Used to limit the rate at which we send the number of completed packets
+ * event to the host. This is the os time at which we can send an event.
+ */
+static ble_npl_time_t g_ble_ll_last_num_comp_pkt_evt;
+extern uint8_t *g_ble_ll_conn_comp_ev;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static const uint8_t ble_ll_valid_conn_phy_mask = (BLE_HCI_LE_PHY_1M_PREF_MASK
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+                                | BLE_HCI_LE_PHY_2M_PREF_MASK
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+                                | BLE_HCI_LE_PHY_CODED_PREF_MASK
+#endif
+                              );
+static const uint8_t ble_ll_conn_required_phy_mask = (BLE_HCI_LE_PHY_1M_PREF_MASK
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+                            | BLE_HCI_LE_PHY_CODED_PREF_MASK
+#endif
+                            );
+#endif
+
+/**
+ * Allocate an event to send a connection complete event when initiating
+ *
+ * @return int 0: success -1: failure
+ */
+static int
+ble_ll_init_alloc_conn_comp_ev(void)
+{
+    int rc;
+    uint8_t *evbuf;
+
+    rc = 0;
+    evbuf = g_ble_ll_conn_comp_ev;
+    if (evbuf == NULL) {
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (!evbuf) {
+            rc = -1;
+        } else {
+            g_ble_ll_conn_comp_ev = evbuf;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Called to check that the connection parameters are within range
+ *
+ * @param itvl_min
+ * @param itvl_max
+ * @param latency
+ * @param spvn_tmo
+ *
+ * @return int BLE_ERR_INV_HCI_CMD_PARMS if invalid parameters, 0 otherwise
+ */
+int
+ble_ll_conn_hci_chk_conn_params(uint16_t itvl_min, uint16_t itvl_max,
+                                uint16_t latency, uint16_t spvn_tmo)
+{
+    uint32_t spvn_tmo_usecs;
+    uint32_t min_spvn_tmo_usecs;
+
+    if ((itvl_min > itvl_max) ||
+        (itvl_min < BLE_HCI_CONN_ITVL_MIN) ||
+        (itvl_max > BLE_HCI_CONN_ITVL_MAX) ||
+        (latency > BLE_HCI_CONN_LATENCY_MAX) ||
+        (spvn_tmo < BLE_HCI_CONN_SPVN_TIMEOUT_MIN) ||
+        (spvn_tmo > BLE_HCI_CONN_SPVN_TIMEOUT_MAX)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /*
+    * Supervision timeout (in msecs) must be more than:
+    *  (1 + connLatency) * connIntervalMax * 1.25 msecs * 2.
+    */
+    spvn_tmo_usecs = spvn_tmo;
+    spvn_tmo_usecs *= (BLE_HCI_CONN_SPVN_TMO_UNITS * 1000);
+    min_spvn_tmo_usecs = (uint32_t)itvl_max * 2 * BLE_LL_CONN_ITVL_USECS;
+    min_spvn_tmo_usecs *= (1 + latency);
+    if (spvn_tmo_usecs <= min_spvn_tmo_usecs) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Send a connection complete event
+ *
+ * @param status The BLE error code associated with the event
+ */
+void
+ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status,
+                            uint8_t *evbuf, struct ble_ll_adv_sm *advsm)
+{
+    struct ble_hci_ev_le_subev_enh_conn_complete *enh_ev;
+    struct ble_hci_ev_le_subev_conn_complete *ev;
+    struct ble_hci_ev *hci_ev = (void *) evbuf;
+    uint8_t *rpa;
+
+    BLE_LL_ASSERT(evbuf);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE)) {
+        hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+        hci_ev->length = sizeof(*enh_ev);
+        enh_ev = (void *) hci_ev->data;
+
+        memset(enh_ev, 0, sizeof(*enh_ev));
+
+        enh_ev->subev_code = BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE;
+        enh_ev->status = status;
+
+        if (connsm) {
+            enh_ev->conn_handle = htole16(connsm->conn_handle);
+            enh_ev->role = connsm->conn_role - 1;
+            enh_ev->peer_addr_type = connsm->peer_addr_type;
+            memcpy(enh_ev->peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN);
+
+            if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+                if (connsm->inita_identity_used) {
+                    /* We used identity address in CONNECT_IND which can be just
+                     * fine if
+                     * a) it was direct advertising we replied to and remote uses
+                     *    its identity address in device privacy mode or IRK is all
+                     *    zeros.
+                     * b) peer uses RPA and this is first time we connect to him
+                     */
+                    rpa = NULL;
+                } else  if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+                    rpa = ble_ll_scan_get_local_rpa();
+                } else {
+                    rpa = NULL;
+                }
+            } else {
+                rpa = ble_ll_adv_get_local_rpa(advsm);
+            }
+
+            if (rpa) {
+                memcpy(enh_ev->local_rpa, rpa, BLE_DEV_ADDR_LEN);
+            }
+
+            /* We need to adjust peer type if device connected using RPA
+             * and was resolved since RPA needs to be added to HCI event.
+             */
+             if (connsm->peer_addr_type < BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT
+                     && (connsm->rpa_index > -1)) {
+                 enh_ev->peer_addr_type += 2;
+             }
+
+             if (enh_ev->peer_addr_type > BLE_HCI_CONN_PEER_ADDR_RANDOM) {
+                 if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+                     rpa = ble_ll_scan_get_peer_rpa();
+                 } else {
+                     rpa = ble_ll_adv_get_peer_rpa(advsm);
+                 }
+                 memcpy(enh_ev->peer_rpa, rpa, BLE_DEV_ADDR_LEN);
+             }
+
+            enh_ev->conn_itvl = htole16(connsm->conn_itvl);
+            enh_ev->conn_latency = htole16(connsm->slave_latency);
+            enh_ev->supervision_timeout = htole16(connsm->supervision_tmo);
+            enh_ev->mca = connsm->master_sca;
+        }
+
+        ble_ll_hci_event_send(hci_ev);
+        return;
+    }
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CONN_COMPLETE)) {
+        hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+        hci_ev->length = sizeof(*ev);
+        ev = (void *) hci_ev->data;
+
+        memset(ev, 0, sizeof(*ev));
+
+        ev->subev_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
+        ev->status = status;
+
+        if (connsm) {
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->role = connsm->conn_role - 1;
+            ev->peer_addr_type = connsm->peer_addr_type;
+
+            if (ev->peer_addr_type > BLE_HCI_CONN_PEER_ADDR_RANDOM) {
+                ev->peer_addr_type -= 2;
+            }
+            memcpy(ev->peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN);
+            ev->conn_itvl = htole16(connsm->conn_itvl);
+            ev->conn_latency = htole16(connsm->slave_latency);
+            ev->supervision_timeout = htole16(connsm->supervision_tmo);
+            ev->mca = connsm->master_sca;
+        }
+
+        ble_ll_hci_event_send(hci_ev);
+        return;
+    }
+
+    ble_hci_trans_buf_free(evbuf);
+}
+
+/**
+ * Called to create and send the number of completed packets event to the
+ * host.
+ */
+void
+ble_ll_conn_num_comp_pkts_event_send(struct ble_ll_conn_sm *connsm)
+{
+    /** The maximum number of handles that will fit in an event buffer. */
+    static const int max_handles =
+            (BLE_LL_MAX_EVT_LEN - sizeof(struct ble_hci_ev_num_comp_pkts) - 1) / 4;
+    struct ble_hci_ev_num_comp_pkts *ev;
+    struct ble_hci_ev *hci_ev;
+    int event_sent;
+
+    if (connsm == NULL) {
+        goto skip_conn;
+    }
+
+    /*
+     * At some periodic rate, make sure we go through all active connections
+     * and send the number of completed packet events. We do this mainly
+     * because the spec says we must update the host even though no packets
+     * have completed but there are data packets in the controller buffers
+     * (i.e. enqueued in a connection state machine).
+     */
+    if ((ble_npl_stime_t)(ble_npl_time_get() - g_ble_ll_last_num_comp_pkt_evt) <
+        ble_npl_time_ms_to_ticks32(MYNEWT_VAL(BLE_LL_NUM_COMP_PKT_ITVL_MS))) {
+        /*
+         * If this connection has completed packets, send an event right away.
+         * We do this to increase throughput but we dont want to search the
+         * entire active list every time.
+         */
+        if (connsm->completed_pkts) {
+            hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (hci_ev) {
+                hci_ev->opcode = BLE_HCI_EVCODE_NUM_COMP_PKTS;
+                hci_ev->length = sizeof(*ev);
+                ev = (void *)hci_ev->data;
+
+                ev->count = 1;
+                ev->completed[0].handle = htole16(connsm->conn_handle);
+                ev->completed[0].packets = htole16(connsm->completed_pkts);
+                hci_ev->length += sizeof(ev->completed[0]);
+
+                connsm->completed_pkts = 0;
+
+                ble_ll_hci_event_send(hci_ev);
+            }
+        }
+        return;
+    }
+
+    /* Iterate through all the active, created connections */
+skip_conn:
+    hci_ev = NULL;
+    ev = NULL;
+
+    event_sent = 0;
+    SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) {
+        /*
+         * Only look at connections that we have sent a connection complete
+         * event and that either has packets enqueued or has completed packets.
+         */
+        if ((connsm->conn_state != BLE_LL_CONN_STATE_IDLE) &&
+            (connsm->completed_pkts || !STAILQ_EMPTY(&connsm->conn_txq))) {
+            /* If no buffer, get one, If cant get one, leave. */
+            if (!hci_ev) {
+                hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+                if (!hci_ev) {
+                    break;
+                }
+
+                hci_ev->opcode = BLE_HCI_EVCODE_NUM_COMP_PKTS;
+                hci_ev->length = sizeof(*ev);
+                ev = (void *)hci_ev->data;
+
+                ev->count = 0;
+            }
+
+            /* Add handle and complete packets */
+            ev->completed[ev->count].handle = htole16(connsm->conn_handle);
+            ev->completed[ev->count].packets = htole16(connsm->completed_pkts);
+            hci_ev->length += sizeof(ev->completed[ev->count]);
+            ev->count++;
+
+            connsm->completed_pkts = 0;
+
+            /* Send now if the buffer is full. */
+            if (ev->count == max_handles) {
+                ble_ll_hci_event_send(hci_ev);
+                hci_ev = NULL;
+                event_sent = 1;
+            }
+        }
+    }
+
+    /* Send event if there is an event to send */
+    if (hci_ev) {
+        ble_ll_hci_event_send(hci_ev);
+        event_sent = 1;
+    }
+
+    if (event_sent) {
+        g_ble_ll_last_num_comp_pkt_evt = ble_npl_time_get();
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+/**
+ * Send a authenticated payload timeout event
+ *
+ * NOTE: we currently only send this event when we have a reason to send it;
+ * not when it fails.
+ *
+ * @param reason The BLE error code to send as a disconnect reason
+ */
+void
+ble_ll_auth_pyld_tmo_event_send(struct ble_ll_conn_sm *connsm)
+{
+    struct ble_hci_ev_auth_pyld_tmo *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_AUTH_PYLD_TMO)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_AUTH_PYLD_TMO;
+            hci_ev->length = sizeof(*ev);
+
+            ev = (void *) hci_ev->data;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+#endif
+
+/**
+ * Send a disconnection complete event.
+ *
+ * NOTE: we currently only send this event when we have a reason to send it;
+ * not when it fails.
+ *
+ * @param reason The BLE error code to send as a disconnect reason
+ */
+void
+ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t reason)
+{
+    struct ble_hci_ev_disconn_cmp *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_DISCONN_CMP)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_DISCONN_CMP;
+            hci_ev->length = sizeof(*ev);
+
+            ev = (void *) hci_ev->data;
+
+            ev->status = BLE_ERR_SUCCESS;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->reason = reason;
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+static int
+ble_ll_conn_hci_chk_scan_params(uint16_t itvl, uint16_t window)
+{
+    /* Check interval and window */
+    if ((itvl < BLE_HCI_SCAN_ITVL_MIN) ||
+        (itvl > BLE_HCI_SCAN_ITVL_MAX) ||
+        (window < BLE_HCI_SCAN_WINDOW_MIN) ||
+        (window > BLE_HCI_SCAN_WINDOW_MAX) ||
+        (itvl < window)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return 0;
+}
+
+/**
+ * Process the HCI command to create a connection.
+ *
+ * Context: Link Layer task (HCI command processing)
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_create(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_create_conn_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+    struct hci_create_conn hcc = { 0 };
+    int rc;
+
+    if (len < sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If we are already creating a connection we should leave */
+    if (g_ble_ll_conn_create_sm) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* If already enabled, we return an error */
+    if (ble_ll_scan_enabled()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Retrieve command data */
+    hcc.scan_itvl = le16toh(cmd->scan_itvl);
+    hcc.scan_window = le16toh(cmd->scan_window);
+
+    rc = ble_ll_conn_hci_chk_scan_params(hcc.scan_itvl, hcc.scan_window);
+    if (rc) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check filter policy */
+    hcc.filter_policy = cmd->filter_policy;
+    if (hcc.filter_policy > BLE_HCI_INITIATOR_FILT_POLICY_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Get peer address type and address only if no whitelist used */
+    if (hcc.filter_policy == 0) {
+        hcc.peer_addr_type = cmd->peer_addr_type;
+        if (hcc.peer_addr_type > BLE_HCI_CONN_PEER_ADDR_MAX) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        memcpy(&hcc.peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    /* Get own address type (used in connection request) */
+    hcc.own_addr_type = cmd->own_addr_type;
+    if (hcc.own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check connection interval, latency and supervision timeoout */
+    hcc.conn_itvl_min = le16toh(cmd->min_conn_itvl);
+    hcc.conn_itvl_max = le16toh(cmd->max_conn_itvl);
+    hcc.conn_latency = le16toh(cmd->conn_latency);
+    hcc.supervision_timeout = le16toh(cmd->tmo);
+    rc = ble_ll_conn_hci_chk_conn_params(hcc.conn_itvl_min,
+                                         hcc.conn_itvl_max,
+                                         hcc.conn_latency,
+                                         hcc.supervision_timeout);
+    if (rc) {
+        return rc;
+    }
+
+    /* Min/max connection event lengths */
+    hcc.min_ce_len = le16toh(cmd->min_ce);
+    hcc.max_ce_len = le16toh(cmd->max_ce);
+    if (hcc.min_ce_len > hcc.max_ce_len) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Make sure we can allocate an event to send the connection complete */
+    if (ble_ll_init_alloc_conn_comp_ev()) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* Make sure we can accept a connection! */
+    connsm = ble_ll_conn_sm_get();
+    if (connsm == NULL) {
+        return BLE_ERR_CONN_LIMIT;
+    }
+
+    /* Initialize state machine in master role and start state machine */
+    ble_ll_conn_master_init(connsm, &hcc);
+    ble_ll_conn_sm_new(connsm);
+    /* CSA will be selected when advertising is received */
+
+    /* Start scanning */
+    rc = ble_ll_scan_initiator_start(&hcc, &connsm->scansm);
+    if (rc) {
+        SLIST_REMOVE(&g_ble_ll_conn_active_list,connsm,ble_ll_conn_sm,act_sle);
+        STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe);
+    } else {
+        /* Set the connection state machine we are trying to create. */
+        g_ble_ll_conn_create_sm = connsm;
+    }
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_conn_hcc_params_set_fallback(struct hci_ext_create_conn *hcc,
+                                    const struct hci_ext_conn_params *fallback)
+{
+    BLE_LL_ASSERT(fallback);
+
+    if (!(hcc->init_phy_mask & BLE_PHY_MASK_1M)) {
+        hcc->params[0] = *fallback;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    if (!(hcc->init_phy_mask & BLE_PHY_MASK_2M)) {
+        hcc->params[1] = *fallback;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if (!(hcc->init_phy_mask & BLE_PHY_MASK_CODED)) {
+        hcc->params[2] = *fallback;
+    }
+#endif
+}
+
+int
+ble_ll_ext_conn_create(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_ext_create_conn_cp *cmd = (const void *) cmdbuf;
+    const struct conn_params *params = cmd->conn_params;
+    const struct hci_ext_conn_params *fallback_params = NULL;
+    struct hci_ext_create_conn hcc = { 0 };
+    struct ble_ll_conn_sm *connsm;
+    int rc;
+
+    /* validate length */
+    if (len < sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    len -= sizeof(*cmd);
+
+    /* If we are already creating a connection we should leave */
+    if (g_ble_ll_conn_create_sm) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* If already enabled, we return an error */
+    if (ble_ll_scan_enabled()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    hcc.filter_policy = cmd->filter_policy;
+    if (hcc.filter_policy > BLE_HCI_INITIATOR_FILT_POLICY_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    hcc.own_addr_type = cmd->own_addr_type;
+    if (hcc.own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Validate peer address type only if no whitelist used */
+    if (hcc.filter_policy == 0) {
+        hcc.peer_addr_type = cmd->peer_addr_type;
+
+        if (hcc.peer_addr_type > BLE_HCI_CONN_PEER_ADDR_MAX) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        memcpy(hcc.peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    hcc.init_phy_mask = cmd->init_phy_mask;
+    if (hcc.init_phy_mask & ~ble_ll_valid_conn_phy_mask) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (!(hcc.init_phy_mask & ble_ll_conn_required_phy_mask)) {
+        /* At least one of those need to be set */
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (hcc.init_phy_mask & BLE_PHY_MASK_1M) {
+        if (len < sizeof(*params)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+        len -= sizeof(*params);
+
+        hcc.params[0].scan_itvl = le16toh(params->scan_itvl);
+        hcc.params[0].scan_window = le16toh(params->scan_window);
+
+        rc = ble_ll_conn_hci_chk_scan_params(hcc.params[0].scan_itvl,
+                                             hcc.params[0].scan_window);
+        if (rc) {
+            return rc;
+        }
+
+        hcc.params[0].conn_itvl_min = le16toh(params->conn_min_itvl);
+        hcc.params[0].conn_itvl_max = le16toh(params->conn_min_itvl);
+        hcc.params[0].conn_latency = le16toh(params->conn_latency);
+        hcc.params[0].supervision_timeout = le16toh(params->supervision_timeout);
+
+        rc = ble_ll_conn_hci_chk_conn_params(hcc.params[0].conn_itvl_min,
+                                             hcc.params[0].conn_itvl_max,
+                                             hcc.params[0].conn_latency,
+                                             hcc.params[0].supervision_timeout);
+        if (rc) {
+            return rc;
+        }
+
+        /* Min/max connection event lengths */
+        hcc.params[0].min_ce_len = le16toh(params->min_ce);
+        hcc.params[0].max_ce_len = le16toh(params->max_ce);
+        if (hcc.params[0].min_ce_len > hcc.params[0].max_ce_len) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        fallback_params = &hcc.params[0];
+        params++;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    if (hcc.init_phy_mask & BLE_PHY_MASK_2M) {
+        if (len < sizeof(*params)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+        len -= sizeof(*params);
+
+        hcc.params[1].conn_itvl_min = le16toh(params->conn_min_itvl);
+        hcc.params[1].conn_itvl_max = le16toh(params->conn_min_itvl);
+        hcc.params[1].conn_latency = le16toh(params->conn_latency);
+        hcc.params[1].supervision_timeout = le16toh(params->supervision_timeout);
+
+        rc = ble_ll_conn_hci_chk_conn_params(hcc.params[1].conn_itvl_min,
+                                             hcc.params[1].conn_itvl_max,
+                                             hcc.params[1].conn_latency,
+                                             hcc.params[1].supervision_timeout);
+        if (rc) {
+            return rc;
+        }
+
+        /* Min/max connection event lengths */
+        hcc.params[1].min_ce_len = le16toh(params->min_ce);
+        hcc.params[1].max_ce_len = le16toh(params->max_ce);
+        if (hcc.params[1].min_ce_len > hcc.params[1].max_ce_len) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        params++;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if (hcc.init_phy_mask & BLE_PHY_MASK_CODED) {
+        if (len < sizeof(*params)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+        len -= sizeof(*params);
+
+        hcc.params[2].scan_itvl = le16toh(params->scan_itvl);
+        hcc.params[2].scan_window = le16toh(params->scan_window);
+
+        rc = ble_ll_conn_hci_chk_scan_params(hcc.params[2].scan_itvl,
+                                             hcc.params[2].scan_window);
+        if (rc) {
+            return rc;
+        }
+
+        hcc.params[2].conn_itvl_min = le16toh(params->conn_min_itvl);
+        hcc.params[2].conn_itvl_max = le16toh(params->conn_min_itvl);
+        hcc.params[2].conn_latency = le16toh(params->conn_latency);
+        hcc.params[2].supervision_timeout = le16toh(params->supervision_timeout);
+
+        rc = ble_ll_conn_hci_chk_conn_params(hcc.params[2].conn_itvl_min,
+                                             hcc.params[2].conn_itvl_max,
+                                             hcc.params[2].conn_latency,
+                                             hcc.params[2].supervision_timeout);
+        if (rc) {
+            return rc;
+        }
+
+        /* Min/max connection event lengths */
+        hcc.params[2].min_ce_len = le16toh(params->min_ce);
+        hcc.params[2].max_ce_len = le16toh(params->max_ce);
+        if (hcc.params[2].min_ce_len > hcc.params[2].max_ce_len) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (!fallback_params) {
+            fallback_params = &hcc.params[2];
+        }
+        params++;
+    }
+#endif
+
+    /* Make sure we can allocate an event to send the connection complete */
+    if (ble_ll_init_alloc_conn_comp_ev()) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* Make sure we can accept a connection! */
+    connsm = ble_ll_conn_sm_get();
+    if (connsm == NULL) {
+        return BLE_ERR_CONN_LIMIT;
+    }
+
+    ble_ll_conn_hcc_params_set_fallback(&hcc, fallback_params);
+
+    /* Initialize state machine in master role and start state machine */
+    ble_ll_conn_ext_master_init(connsm, &hcc);
+    ble_ll_conn_sm_new(connsm);
+
+    /* CSA will be selected when advertising is received */
+
+    /* Start scanning */
+    rc = ble_ll_scan_ext_initiator_start(&hcc, &connsm->scansm);
+    if (rc) {
+        SLIST_REMOVE(&g_ble_ll_conn_active_list,connsm,ble_ll_conn_sm,act_sle);
+        STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe);
+    } else {
+        /* Set the connection state machine we are trying to create. */
+        g_ble_ll_conn_create_sm = connsm;
+    }
+
+    return rc;
+}
+#endif
+
+static int
+ble_ll_conn_process_conn_params(const struct ble_hci_le_rem_conn_param_rr_cp *cmd,
+                                struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+    struct hci_conn_update *hcu;
+
+    /* Retrieve command data */
+    hcu = &connsm->conn_param_req;
+    hcu->handle = connsm->conn_handle;
+
+    BLE_LL_ASSERT(connsm->conn_handle == le16toh(cmd->conn_handle));
+
+    hcu->conn_itvl_min = le16toh(cmd->conn_itvl_min);
+    hcu->conn_itvl_max = le16toh(cmd->conn_itvl_max);
+    hcu->conn_latency = le16toh(cmd->conn_latency);
+    hcu->supervision_timeout = le16toh(cmd->supervision_timeout);
+    hcu->min_ce_len = le16toh(cmd->min_ce);
+    hcu->max_ce_len = le16toh(cmd->max_ce);
+
+    /* Check that parameter values are in range */
+    rc = ble_ll_conn_hci_chk_conn_params(hcu->conn_itvl_min,
+                                         hcu->conn_itvl_max,
+                                         hcu->conn_latency,
+                                         hcu->supervision_timeout);
+
+    /* Check valid min/max ce length */
+    if (rc || (hcu->min_ce_len > hcu->max_ce_len)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+    return rc;
+}
+
+/**
+ * Called when the host issues the read remote features command
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_read_rem_features(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rd_rem_feat_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If no connection handle exit with error */
+    connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    /* If already pending exit with error */
+    if (connsm->csmflags.cfbit.pending_hci_rd_features) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /*
+     * Start control procedure if we did not receive peer's features and did not
+     * start procedure already.
+     */
+    if (!connsm->csmflags.cfbit.rxd_features &&
+                !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) {
+        if ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) &&
+            !(ble_ll_read_supp_features() & BLE_LL_FEAT_SLAVE_INIT)) {
+                return BLE_ERR_CMD_DISALLOWED;
+        }
+
+        ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG);
+    }
+
+    connsm->csmflags.cfbit.pending_hci_rd_features = 1;
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Called to process a connection update command.
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_update(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_conn_update_cp *cmd = (const void *) cmdbuf;
+    int rc;
+    uint8_t ctrl_proc;
+    uint16_t handle;
+    struct ble_ll_conn_sm *connsm;
+    struct hci_conn_update *hcu;
+
+    /*
+     * XXX: must deal with slave not supporting this feature and using
+     * conn update! Right now, we only check if WE support the connection
+     * parameters request procedure. We dont check if the remote does.
+     * We should also be able to deal with sending the parameter request,
+     * getting an UNKOWN_RSP ctrl pdu and resorting to use normal
+     * connection update procedure.
+     */
+
+    /* If no connection handle exit with error */
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    /* Better not have this procedure ongoing! */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ) ||
+        IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* See if this feature is supported on both sides */
+    if ((connsm->conn_features & BLE_LL_FEAT_CONN_PARM_REQ) == 0) {
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            return BLE_ERR_UNSUPP_REM_FEATURE;
+        }
+        ctrl_proc = BLE_LL_CTRL_PROC_CONN_UPDATE;
+    } else {
+        ctrl_proc = BLE_LL_CTRL_PROC_CONN_PARAM_REQ;
+    }
+
+    /*
+     * If we are a slave and the master has initiated the procedure already
+     * we should deny the slave request for now. If we are a master and the
+     * slave has initiated the procedure, we need to send a reject to the
+     * slave.
+     */
+    if (connsm->csmflags.cfbit.awaiting_host_reply) {
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            return BLE_ERR_LMP_COLLISION;
+        } else {
+            connsm->csmflags.cfbit.awaiting_host_reply = 0;
+
+            /* XXX: If this fails no reject ind will be sent! */
+            ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode,
+                                        BLE_ERR_LMP_COLLISION);
+        }
+    }
+
+    /*
+     * If we are a slave and the master has initiated the channel map
+     * update procedure we should deny the slave request for now.
+     */
+    if (connsm->csmflags.cfbit.chanmap_update_scheduled) {
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            return BLE_ERR_DIFF_TRANS_COLL;
+        }
+    }
+
+    /* Retrieve command data */
+    hcu = &connsm->conn_param_req;
+    hcu->conn_itvl_min = le16toh(cmd->conn_itvl_min);
+    hcu->conn_itvl_max = le16toh(cmd->conn_itvl_max);
+    hcu->conn_latency = le16toh(cmd->conn_latency);
+    hcu->supervision_timeout = le16toh(cmd->supervision_timeout);
+    hcu->min_ce_len = le16toh(cmd->min_ce_len);
+    hcu->max_ce_len = le16toh(cmd->max_ce_len);
+    if (hcu->min_ce_len > hcu->max_ce_len) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check that parameter values are in range */
+    rc = ble_ll_conn_hci_chk_conn_params(hcu->conn_itvl_min,
+                                         hcu->conn_itvl_max,
+                                         hcu->conn_latency,
+                                         hcu->supervision_timeout);
+    if (!rc) {
+        hcu->handle = handle;
+
+        /* Start the control procedure */
+        ble_ll_ctrl_proc_start(connsm, ctrl_proc);
+    }
+
+    return rc;
+}
+
+int
+ble_ll_conn_hci_param_rr(const uint8_t *cmdbuf, uint8_t len,
+                         uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_rem_conn_param_rr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rem_conn_param_rr_rp *rsp = (void *) rspbuf;
+    int rc;
+    uint8_t *dptr;
+    uint8_t rsp_opcode;
+    uint16_t handle;
+    struct os_mbuf *om;
+    struct ble_ll_conn_sm *connsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+
+    /* See if we support this feature */
+    if ((ble_ll_read_supp_features() & BLE_LL_FEAT_CONN_PARM_REQ) == 0) {
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        goto done;
+    }
+
+    /* If we dont have a handle we cant do anything */
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto done;
+    }
+
+    /* Make sure connection parameters are valid */
+    rc = ble_ll_conn_process_conn_params(cmd, connsm);
+
+    /* The connection should be awaiting a reply. If not, just discard */
+    if (connsm->csmflags.cfbit.awaiting_host_reply) {
+        /* Get a control packet buffer */
+        if (rc == BLE_ERR_SUCCESS) {
+            om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+                                    sizeof(struct ble_mbuf_hdr));
+            if (om) {
+                dptr = om->om_data;
+                rsp_opcode = ble_ll_ctrl_conn_param_reply(connsm, dptr,
+                                                          &connsm->conn_cp);
+                dptr[0] = rsp_opcode;
+                len = g_ble_ll_ctrl_pkt_lengths[rsp_opcode] + 1;
+                ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len);
+            }
+        } else {
+            /* XXX: check return code and deal */
+            ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode,
+                                        BLE_ERR_CONN_PARMS);
+        }
+        connsm->csmflags.cfbit.awaiting_host_reply = 0;
+
+        /* XXX: if we cant get a buffer, what do we do? We need to remember
+         * reason if it was a negative reply. We also would need to have
+         * some state to tell us this happened
+         */
+    }
+
+done:
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+int
+ble_ll_conn_hci_param_nrr(const uint8_t *cmdbuf, uint8_t len,
+                          uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_rem_conn_params_nrr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rem_conn_params_nrr_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t handle;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+
+    /* See if we support this feature */
+    if ((ble_ll_read_supp_features() & BLE_LL_FEAT_CONN_PARM_REQ) == 0) {
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        goto done;
+    }
+
+    /* If we dont have a handle we cant do anything */
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto done;
+    }
+
+    rc = BLE_ERR_SUCCESS;
+
+    /* The connection should be awaiting a reply. If not, just discard */
+    if (connsm->csmflags.cfbit.awaiting_host_reply) {
+        /* XXX: check return code and deal */
+        ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode,
+                                    cmd->reason);
+        connsm->csmflags.cfbit.awaiting_host_reply = 0;
+
+        /* XXX: if we cant get a buffer, what do we do? We need to remember
+         * reason if it was a negative reply. We also would need to have
+         * some state to tell us this happened
+         */
+    }
+
+done:
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/* this is called from same context after cmd complete is send so it is
+ * safe to use g_ble_ll_conn_comp_ev
+ */
+static void
+ble_ll_conn_hci_cancel_conn_complete_event(void)
+{
+    BLE_LL_ASSERT(g_ble_ll_conn_comp_ev);
+
+    ble_ll_conn_comp_event_send(NULL, BLE_ERR_UNK_CONN_ID,
+                                g_ble_ll_conn_comp_ev, NULL);
+    g_ble_ll_conn_comp_ev = NULL;
+}
+
+/**
+ * Called when HCI command to cancel a create connection command has been
+ * received.
+ *
+ * Context: Link Layer (HCI command parser)
+ *
+ * @return int
+ */
+int
+ble_ll_conn_create_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb)
+{
+    int rc;
+    struct ble_ll_conn_sm *connsm;
+    os_sr_t sr;
+
+    /*
+     * If we receive this command and we have not got a connection
+     * create command, we have to return disallowed. The spec does not say
+     * what happens if the connection has already been established. We
+     * return disallowed as well
+     */
+    OS_ENTER_CRITICAL(sr);
+    connsm = g_ble_ll_conn_create_sm;
+    if (connsm && (connsm->conn_state == BLE_LL_CONN_STATE_IDLE)) {
+        /* stop scanning and end the connection event */
+        g_ble_ll_conn_create_sm = NULL;
+        ble_ll_scan_sm_stop(1);
+        ble_ll_conn_end(connsm, BLE_ERR_UNK_CONN_ID);
+
+        *post_cmd_cb = ble_ll_conn_hci_cancel_conn_complete_event;
+
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        /* If we are not attempting to create a connection*/
+        rc = BLE_ERR_CMD_DISALLOWED;
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    return rc;
+}
+
+/**
+ * Called to process a HCI disconnect command
+ *
+ * Context: Link Layer task (HCI command parser).
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_disconnect_cmd(const uint8_t *cmdbuf, uint8_t len)
+{
+    int rc;
+    uint16_t handle;
+    struct ble_ll_conn_sm *connsm;
+    const struct ble_hci_lc_disconnect_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof (*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check for valid parameters */
+    handle = le16toh(cmd->conn_handle);
+
+    rc = BLE_ERR_INV_HCI_CMD_PARMS;
+    if (handle <= BLE_LL_CONN_MAX_CONN_HANDLE) {
+        /* Make sure reason is valid */
+        switch (cmd->reason) {
+        case BLE_ERR_AUTH_FAIL:
+        case BLE_ERR_REM_USER_CONN_TERM:
+        case BLE_ERR_RD_CONN_TERM_RESRCS:
+        case BLE_ERR_RD_CONN_TERM_PWROFF:
+        case BLE_ERR_UNSUPP_REM_FEATURE:
+        case BLE_ERR_UNIT_KEY_PAIRING:
+        case BLE_ERR_CONN_PARMS:
+            connsm = ble_ll_conn_find_active_conn(handle);
+            if (connsm) {
+                /* Do not allow command if we are in process of disconnecting */
+                if (connsm->disconnect_reason) {
+                    rc = BLE_ERR_CMD_DISALLOWED;
+                } else {
+                    /* This control procedure better not be pending! */
+                    BLE_LL_ASSERT(CONN_F_TERMINATE_STARTED(connsm) == 0);
+
+                    /* Record the disconnect reason */
+                    connsm->disconnect_reason = cmd->reason;
+
+                    /* Start this control procedure */
+                    ble_ll_ctrl_terminate_start(connsm);
+
+                    rc = BLE_ERR_SUCCESS;
+                }
+            } else {
+                rc = BLE_ERR_UNK_CONN_ID;
+            }
+            break;
+        default:
+            break;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Called to process a HCI disconnect command
+ *
+ * Context: Link Layer task (HCI command parser).
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_rd_rem_ver_cmd(const uint8_t *cmdbuf, uint8_t len)
+{
+    struct ble_ll_conn_sm *connsm;
+    const struct ble_hci_rd_rem_ver_info_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check for valid parameters */
+    connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    /* Return error if in progress */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /*
+     * Start this control procedure. If we have already done this control
+     * procedure we set the pending bit so that the host gets an event because
+     * it is obviously expecting one (or would not have sent the command).
+     * NOTE: we cant just send the event here. That would cause the event to
+     * be queued before the command status.
+     */
+    if (!connsm->csmflags.cfbit.version_ind_sent) {
+        ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG);
+    } else {
+        connsm->pending_ctrl_procs |= (1 << BLE_LL_CTRL_PROC_VERSION_XCHG);
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Called to read the RSSI for a given connection handle
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_rd_rssi(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen)
+{
+
+    const struct ble_hci_rd_rssi_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_rd_rssi_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rsp->handle = cmd->handle;
+
+    connsm = ble_ll_conn_find_active_conn(le16toh(cmd->handle));
+    if (!connsm) {
+        rsp->rssi = 127;
+        rc = BLE_ERR_UNK_CONN_ID;
+    } else {
+        rsp->rssi = connsm->conn_rssi;
+        rc = BLE_ERR_SUCCESS;
+    }
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/**
+ * Called to read the current channel map of a connection
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_rd_chan_map(const uint8_t *cmdbuf, uint8_t len,
+                            uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_rd_chan_map_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rd_chan_map_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t handle;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        memset(rsp->chan_map, 0, sizeof(rsp->chan_map));
+    } else {
+        if (connsm->csmflags.cfbit.chanmap_update_scheduled) {
+            memcpy(rsp->chan_map, connsm->req_chanmap, BLE_LL_CONN_CHMAP_LEN);
+        } else {
+            memcpy(rsp->chan_map, connsm->chanmap, BLE_LL_CONN_CHMAP_LEN);
+        }
+        rc = BLE_ERR_SUCCESS;
+    }
+
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/**
+ * Called when the host issues the LE command "set host channel classification"
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_host_chan_class_cp *cmd = (const void *) cmdbuf;
+    uint8_t num_used_chans;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /*
+     * The HCI command states that the host is allowed to mask in just one
+     * channel but the Link Layer needs minimum two channels to operate. So
+     * I will not allow this command if there are less than 2 channels masked.
+     */
+    num_used_chans = ble_ll_utils_calc_num_used_chans(cmd->chan_map);
+    if ((num_used_chans < 2) || ((cmd->chan_map[4] & 0xe0) != 0)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Set the host channel mask */
+    ble_ll_conn_set_global_chanmap(num_used_chans, cmd->chan_map);
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+int
+ble_ll_conn_hci_set_data_len(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_set_data_len_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_set_data_len_rp *rsp = (void *) rspbuf;
+    int rc;
+    uint16_t handle;
+    uint16_t txoctets;
+    uint16_t txtime;
+    struct ble_ll_conn_sm *connsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Find connection */
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto done;
+    }
+
+    txoctets = le16toh(cmd->tx_octets);
+    txtime = le16toh(cmd->tx_time);
+
+    /* Make sure it is valid */
+    if (!ble_ll_chk_txrx_octets(txoctets) ||
+        !ble_ll_chk_txrx_time(txtime)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    /*
+     * Keep original value requested by host since we may want to recalculate
+     * MaxTxTime after PHY changes between coded and uncoded.
+     */
+    connsm->host_req_max_tx_time = txtime;
+
+    /* If peer does not support coded, we cannot use value larger than 2120us */
+    if (!(connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8))) {
+        txtime = min(txtime, BLE_LL_CONN_SUPP_TIME_MAX_UNCODED);
+    }
+#endif
+
+    rc = BLE_ERR_SUCCESS;
+    if (connsm->max_tx_time != txtime ||
+        connsm->max_tx_octets != txoctets) {
+
+        connsm->max_tx_time = txtime;
+        connsm->max_tx_octets = txoctets;
+
+        ble_ll_ctrl_initiate_dle(connsm);
+    }
+
+done:
+    rsp->conn_handle = htole16(handle);
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * LE start encrypt command
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_start_encrypt(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_start_encrypt_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+    } else if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+        rc = BLE_ERR_UNSPECIFIED;
+    } else if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) {
+        /*
+         * The specification does not say what to do here but the host should
+         * not be telling us to start encryption while we are in the process
+         * of honoring a previous start encrypt.
+         */
+        rc = BLE_ERR_CMD_DISALLOWED;
+    } else {
+        /* Start the control procedure */
+        connsm->enc_data.host_rand_num = le64toh(cmd->rand);
+        connsm->enc_data.enc_div = le16toh(cmd->div);
+        swap_buf(connsm->enc_data.enc_block.key, cmd->ltk, 16);
+        ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+        rc = BLE_ERR_SUCCESS;
+    }
+
+    return rc;
+}
+
+/**
+ * Called to process the LE long term key reply.
+ *
+ * Context: Link Layer Task.
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param ocf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_ltk_reply(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_lt_key_req_reply_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_lt_key_req_reply_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t handle;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Find connection handle */
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto ltk_key_cmd_complete;
+    }
+
+    /* Should never get this if we are a master! */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        rc = BLE_ERR_UNSPECIFIED;
+        goto ltk_key_cmd_complete;
+    }
+
+    /* The connection should be awaiting a reply. If not, just discard */
+    if (connsm->enc_data.enc_state != CONN_ENC_S_LTK_REQ_WAIT) {
+        rc = BLE_ERR_CMD_DISALLOWED;
+        goto ltk_key_cmd_complete;
+    }
+
+    swap_buf(connsm->enc_data.enc_block.key, cmd->ltk, 16);
+    ble_ll_calc_session_key(connsm);
+    ble_ll_ctrl_start_enc_send(connsm);
+    rc = BLE_ERR_SUCCESS;
+
+ltk_key_cmd_complete:
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/**
+ * Called to process the LE long term key negative reply.
+ *
+ * Context: Link Layer Task.
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param ocf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_ltk_neg_reply(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_lt_key_req_neg_reply_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_lt_key_req_neg_reply_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t handle;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Find connection handle */
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto ltk_key_cmd_complete;
+    }
+
+    /* Should never get this if we are a master! */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        rc = BLE_ERR_UNSPECIFIED;
+        goto ltk_key_cmd_complete;
+    }
+
+    /* The connection should be awaiting a reply. If not, just discard */
+    if (connsm->enc_data.enc_state != CONN_ENC_S_LTK_REQ_WAIT) {
+        rc = BLE_ERR_CMD_DISALLOWED;
+        goto ltk_key_cmd_complete;
+    }
+
+    /* We received a negative reply! Send REJECT_IND */
+    ble_ll_ctrl_reject_ind_send(connsm, BLE_LL_CTRL_ENC_REQ,
+                                BLE_ERR_PINKEY_MISSING);
+    connsm->enc_data.enc_state = CONN_ENC_S_LTK_NEG_REPLY;
+
+    rc = BLE_ERR_SUCCESS;
+
+ltk_key_cmd_complete:
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+/**
+ * Read authenticated payload timeout (OGF=3, OCF==0x007B)
+ *
+ * @param cmdbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_rd_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_cb_rd_auth_pyld_tmo_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_cb_rd_auth_pyld_tmo_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t handle;
+    int rc;
+
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        rsp->tmo = 0;
+    } else {
+        rc = BLE_ERR_SUCCESS;
+        rsp->tmo = htole16(connsm->auth_pyld_tmo);
+    }
+
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/**
+ * Write authenticated payload timeout (OGF=3, OCF=00x7C)
+ *
+ * @param cmdbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_wr_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_cb_wr_auth_pyld_tmo_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_cb_wr_auth_pyld_tmo_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint32_t min_tmo;
+    uint16_t handle;
+    uint16_t tmo;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rc = BLE_ERR_SUCCESS;
+
+    handle = le16toh(cmd->conn_handle);
+
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+    } else {
+        /*
+         * The timeout is in units of 10 msecs. We need to make sure that the
+         * timeout is greater than or equal to connItvl * (1 + slaveLatency)
+         */
+        tmo = le16toh(cmd->tmo);
+        min_tmo = (uint32_t)connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS;
+        min_tmo *= (connsm->slave_latency + 1);
+        min_tmo /= 10000;
+
+        if (tmo < min_tmo) {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        } else {
+            connsm->auth_pyld_tmo = tmo;
+            if (ble_npl_callout_is_active(&connsm->auth_pyld_timer)) {
+                ble_ll_conn_auth_pyld_timer_start(connsm);
+            }
+        }
+    }
+
+    rsp->conn_handle = htole16(handle);
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+/**
+ * Read current phy for connection (OGF=8, OCF==0x0030)
+ *
+ * @param cmdbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_rd_phy(const uint8_t *cmdbuf, uint8_t len,
+                          uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_rd_phy_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rd_phy_rp *rsp = (void *) rspbuf;
+    int rc;
+    uint16_t handle;
+    struct ble_ll_conn_sm *connsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rsp->tx_phy = 0;
+        rsp->rx_phy = 0;
+        rc = BLE_ERR_UNK_CONN_ID;
+    } else {
+        rsp->tx_phy = connsm->phy_data.cur_tx_phy;
+        rsp->rx_phy = connsm->phy_data.cur_rx_phy;
+        rc = BLE_ERR_SUCCESS;
+    }
+
+    rsp->conn_handle = htole16(handle);
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/**
+ * Set PHY preferences for connection
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_set_phy(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_phy_cp *cmd = (const void *) cmdbuf;
+    int rc;
+    uint16_t phy_options;
+    uint8_t tx_phys;
+    uint8_t rx_phys;
+    uint16_t handle;
+    struct ble_ll_conn_sm *connsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    /*
+     * If host has requested a PHY update and we are not finished do
+     * not allow another one
+     */
+    if (CONN_F_HOST_PHY_UPDATE(connsm)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    phy_options = le16toh(cmd->phy_options);
+    if (phy_options > BLE_HCI_LE_PHY_CODED_S8_PREF) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check valid parameters */
+    rc = ble_ll_hci_chk_phy_masks(cmd->all_phys, cmd->tx_phys, cmd->rx_phys,
+                                  &tx_phys, &rx_phys);
+    if (rc) {
+        goto phy_cmd_param_err;
+    }
+
+    connsm->phy_data.phy_options = phy_options & 0x03;
+    connsm->phy_data.host_pref_tx_phys_mask = tx_phys,
+    connsm->phy_data.host_pref_rx_phys_mask = rx_phys;
+
+    /*
+     * The host preferences override the default phy preferences. Currently,
+     * the only reason the controller will initiate a procedure on its own
+     * is due to the fact that the host set default preferences. So if there
+     * is a pending control procedure and it has not yet started, we do not
+     * need to perform the default controller procedure.
+     */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE)) {
+        if (connsm->cur_ctrl_proc != BLE_LL_CTRL_PROC_PHY_UPDATE) {
+            CONN_F_CTRLR_PHY_UPDATE(connsm) = 0;
+        }
+        CONN_F_HOST_PHY_UPDATE(connsm) = 1;
+    } else {
+        /*
+         * We could be doing a peer-initiated PHY update procedure. If this
+         * is the case the requested phy preferences will not both be 0. If
+         * we are not done with a peer-initiated procedure we just set the
+         * pending bit but do not start the control procedure.
+         */
+        if (CONN_F_PEER_PHY_UPDATE(connsm)) {
+            connsm->pending_ctrl_procs |= (1 << BLE_LL_CTRL_PROC_PHY_UPDATE);
+            CONN_F_HOST_PHY_UPDATE(connsm) = 1;
+        } else {
+            /* Check if we should start phy update procedure */
+            if (!ble_ll_conn_chk_phy_upd_start(connsm)) {
+                CONN_F_HOST_PHY_UPDATE(connsm) = 1;
+            } else {
+                /*
+                 * Set flag to send a PHY update complete event. We set flag
+                 * even if we do not do an update procedure since we have to
+                 * inform the host even if we decide not to change anything.
+                 */
+                CONN_F_PHY_UPDATE_EVENT(connsm) = 1;
+            }
+        }
+    }
+
+phy_cmd_param_err:
+    return rc;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+int
+ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len,
+                                uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_periodic_adv_sync_transfer_params_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_periodic_adv_sync_transfer_params_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint16_t sync_timeout;
+    uint16_t skip;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (cmd->mode > 0x02) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    skip = le16toh(cmd->skip);
+    if (skip > 0x01f3) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    sync_timeout = le16toh(cmd->sync_timeout);
+    if ((sync_timeout < 0x000a) || (sync_timeout > 0x4000)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    /* we don't support any CTE yet */
+    if (cmd->sync_cte_type) {
+        rc = BLE_ERR_UNSUPPORTED;
+        goto done;
+    }
+
+    connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto done;
+    }
+
+    /* timeout in 10ms units */
+    connsm->sync_transfer_sync_timeout = sync_timeout * 10000;
+    connsm->sync_transfer_mode = cmd->mode;
+    connsm->sync_transfer_skip = skip;
+
+    rc = BLE_ERR_SUCCESS;
+
+done:
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+int
+ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_default_periodic_sync_transfer_params_cp *cmd = (const void *)cmdbuf;
+    uint16_t sync_timeout;
+    uint16_t skip;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->mode > 0x02) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    skip = le16toh(cmd->skip);
+    if (skip > 0x01f3) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    sync_timeout = le16toh(cmd->sync_timeout);
+    if ((sync_timeout < 0x000a) || (sync_timeout > 0x4000)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* we don't support any CTE yet */
+    if (cmd->sync_cte_type) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+
+    /* timeout in 10ms units */
+    g_ble_ll_conn_sync_transfer_params.sync_timeout_us = sync_timeout * 10000;
+    g_ble_ll_conn_sync_transfer_params.mode = cmd->mode;
+    g_ble_ll_conn_sync_transfer_params.max_skip = skip;
+
+    return BLE_ERR_SUCCESS;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_priv.h b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_priv.h
new file mode 100644
index 00000000..f2f72d17
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_priv.h
@@ -0,0 +1,226 @@
+/*
+ * 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_LL_CONN_PRIV_
+#define H_BLE_LL_CONN_PRIV_
+
+#include "controller/ble_ll_conn.h"
+#include "controller/ble_ll_hci.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * Definitions for min/max RX/TX time/bytes values allowed for connections.
+ * Source: Core 5.0 specification, Vol 6, Part B, section 4.5.10
+ */
+#define BLE_LL_CONN_SUPP_TIME_MIN           (328)   /* usecs */
+#define BLE_LL_CONN_SUPP_TIME_MAX           (17040) /* usecs */
+#define BLE_LL_CONN_SUPP_TIME_MIN_UNCODED   (328)   /* usecs */
+#define BLE_LL_CONN_SUPP_TIME_MAX_UNCODED   (2120)  /* usecs */
+#define BLE_LL_CONN_SUPP_TIME_MIN_CODED     (2704)  /* usecs */
+#define BLE_LL_CONN_SUPP_TIME_MAX_CODED     (17040) /* usecs */
+#define BLE_LL_CONN_SUPP_BYTES_MIN          (27)    /* bytes */
+#define BLE_LL_CONN_SUPP_BYTES_MAX          (251)   /* bytes */
+
+/* Connection event timing */
+#define BLE_LL_CONN_INITIAL_OFFSET          (1250)
+#define BLE_LL_CONN_ITVL_USECS              (1250)
+#define BLE_LL_CONN_TX_WIN_USECS            (1250)
+#define BLE_LL_CONN_TX_OFF_USECS            (1250)
+#define BLE_LL_CONN_CE_USECS                (625)
+#define BLE_LL_CONN_TX_WIN_MIN              (1)         /* in tx win units */
+#define BLE_LL_CONN_SLAVE_LATENCY_MAX       (499)
+
+/* Connection handle range */
+#define BLE_LL_CONN_MAX_CONN_HANDLE         (0x0EFF)
+
+/* Offset (in bytes) of advertising address in connect request */
+#define BLE_LL_CONN_REQ_ADVA_OFF    (BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN)
+
+/* Default authenticated payload timeout (30 seconds; in 10 msecs increments) */
+#define BLE_LL_CONN_DEF_AUTH_PYLD_TMO       (3000)
+#define BLE_LL_CONN_AUTH_PYLD_OS_TMO(x)     ble_npl_time_ms_to_ticks32((x) * 10)
+
+/* Global Link Layer connection parameters */
+struct ble_ll_conn_global_params
+{
+    uint8_t master_chan_map[BLE_LL_CONN_CHMAP_LEN];
+    uint8_t num_used_chans;
+    uint8_t supp_max_tx_octets;
+    uint8_t supp_max_rx_octets;
+    uint8_t conn_init_max_tx_octets;
+    uint8_t sugg_tx_octets;
+    uint16_t sugg_tx_time;
+    uint16_t conn_init_max_tx_time;
+    uint16_t conn_init_max_tx_time_uncoded;
+    uint16_t conn_init_max_tx_time_coded;
+    uint16_t supp_max_tx_time;
+    uint16_t supp_max_rx_time;
+};
+extern struct ble_ll_conn_global_params g_ble_ll_conn_params;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+struct ble_ll_conn_sync_transfer_params
+{
+    uint32_t sync_timeout_us;
+    uint16_t max_skip;
+    uint8_t  mode;
+};
+extern struct ble_ll_conn_sync_transfer_params g_ble_ll_conn_sync_transfer_params;
+#endif
+
+/* Some data structures used by other LL routines */
+SLIST_HEAD(ble_ll_conn_active_list, ble_ll_conn_sm);
+STAILQ_HEAD(ble_ll_conn_free_list, ble_ll_conn_sm);
+extern struct ble_ll_conn_active_list g_ble_ll_conn_active_list;
+extern struct ble_ll_conn_free_list g_ble_ll_conn_free_list;
+
+/* Pointer to connection state machine we are trying to create */
+extern struct ble_ll_conn_sm *g_ble_ll_conn_create_sm;
+
+/* Generic interface */
+struct ble_ll_len_req;
+struct ble_mbuf_hdr;
+struct ble_ll_adv_sm;
+
+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;
+};
+
+void ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm);
+void ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err);
+void ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
+                             uint8_t hdr_byte, uint8_t length);
+struct ble_ll_conn_sm *ble_ll_conn_sm_get(void);
+void ble_ll_conn_master_init(struct ble_ll_conn_sm *connsm,
+                             struct hci_create_conn *hcc);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+void ble_ll_conn_ext_master_init(struct ble_ll_conn_sm *connsm,
+                                 struct hci_ext_create_conn *hcc);
+
+void ble_ll_conn_ext_set_params(struct ble_ll_conn_sm *connsm,
+                                struct hci_ext_conn_params *hcc_params,
+                                int phy);
+#endif
+
+struct ble_ll_conn_sm *ble_ll_conn_find_active_conn(uint16_t handle);
+void ble_ll_conn_update_eff_data_len(struct ble_ll_conn_sm *connsm);
+
+/* Advertising interface */
+int ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat,
+                            struct ble_mbuf_hdr *rxhdr, bool force_csa2);
+
+/* Link Layer interface */
+void ble_ll_conn_module_init(void);
+void ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, const uint8_t *chanmap);
+void ble_ll_conn_module_reset(void);
+void ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t len);
+int ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa);
+int ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
+void ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
+void ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf,
+                           struct ble_mbuf_hdr *ble_hdr);
+int ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr);
+int ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
+                           struct ble_mbuf_hdr *ble_hdr);
+void ble_ll_conn_wfr_timer_exp(void);
+void ble_ll_conn_init_wfr_timer_exp(void);
+int ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2);
+uint32_t ble_ll_conn_get_ce_end_time(void);
+void ble_ll_conn_event_halt(void);
+void ble_ll_conn_reset_pending_aux_conn_rsp(void);
+bool ble_ll_conn_init_pending_aux_conn_rsp(void);
+/* HCI */
+void ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm,
+                                    uint8_t reason);
+void ble_ll_auth_pyld_tmo_event_send(struct ble_ll_conn_sm *connsm);
+int ble_ll_conn_hci_disconnect_cmd(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_rd_rem_ver_cmd(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_create(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_update(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_param_rr(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_param_nrr(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_create_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb);
+void ble_ll_conn_num_comp_pkts_event_send(struct ble_ll_conn_sm *connsm);
+void ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status,
+                                 uint8_t *evbuf, struct ble_ll_adv_sm *advsm);
+void ble_ll_conn_timeout(struct ble_ll_conn_sm *connsm, uint8_t ble_err);
+int ble_ll_conn_hci_chk_conn_params(uint16_t itvl_min, uint16_t itvl_max,
+                                    uint16_t latency, uint16_t spvn_tmo);
+int ble_ll_conn_hci_read_rem_features(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_rd_rssi(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf,
+                            uint8_t *rsplen);
+int ble_ll_conn_hci_rd_chan_map(const uint8_t *cmdbuf, uint8_t len,
+                                uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_set_data_len(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_le_start_encrypt(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_le_ltk_reply(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_le_ltk_neg_reply(const uint8_t *cmdbuf, uint8_t len,
+                                     uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_wr_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len,
+                                     uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_rd_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len,
+                                     uint8_t *rspbuf, uint8_t *rsplen);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+void ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm);
+#else
+#define ble_ll_conn_auth_pyld_timer_start(x)
+#endif
+
+int ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg);
+int ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg);
+
+int ble_ll_conn_hci_le_rd_phy(const uint8_t *cmdbuf, uint8_t len,
+                              uint8_t *rsp, uint8_t *rsplen);
+int ble_ll_conn_hci_le_set_phy(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_chk_phy_upd_start(struct ble_ll_conn_sm *connsm);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+int ble_ll_ext_conn_create(const uint8_t *cmdbuf, uint8_t cmdlen);
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+int ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len,
+                                    uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_CONN_PRIV_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_ctrl.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_ctrl.c
new file mode 100644
index 00000000..ea2ba834
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_ctrl.c
@@ -0,0 +1,2744 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_ctrl.h"
+#include "controller/ble_ll_trace.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll_sync.h"
+#include "ble_ll_conn_priv.h"
+
+/* To use spec sample data for testing */
+#undef BLE_LL_ENCRYPT_USE_TEST_DATA
+
+/*
+ * For console debug to show session key calculation. NOTE: if you define
+ * this the stack requirements for the LL task go up considerably. The
+ * default stack will not be enough and must be increased.
+ */
+#undef BLE_LL_ENCRYPT_DEBUG
+#ifdef BLE_LL_ENCRYPT_DEBUG
+#include "console/console.h"
+#endif
+
+/*
+ * XXX:
+ *  1) Do I need to keep track of which procedures have already been done?
+ *     Do I need to worry about repeating procedures?
+ *  2) Should we create pool of control pdu's?. Dont need more
+ *  than the # of connections and can probably deal with quite a few less
+ *  if we have lots of connections.
+ *  3) What about procedures that have been completed but try to restart?
+ *  4) NOTE: there is a supported features procedure. However, in the case
+ *  of data length extension, if the receiving device does not understand
+ *  the pdu or it does not support data length extension, the LL_UNKNOWN_RSP
+ *  pdu is sent. That needs to be processed...
+ *  5) We are supposed to remember when we do the data length update proc if
+ *  the device sent us an unknown rsp. We should not send it another len req.
+ *  Implement this how? Through remote supported features?
+ *  8) How to count control pdus sent. DO we count enqueued + sent, or only
+ *  sent (actually attempted to tx). Do we count failures? How?
+ */
+
+/*
+ * XXX: I definitely have an issue with control procedures and connection
+ * param request procedure and connection update procedure. This was
+ * noted when receiving an unknown response. Right now, I am getting confused
+ * with connection parameter request and updates regarding which procedures
+ * are running. So I need to go look through all the code and see where I
+ * used the request procedure and the update procedure and make sure I am doing
+ * the correct thing.
+ */
+
+/*
+ * This array contains the length of the CtrData field in LL control PDU's.
+ * Note that there is a one byte opcode which precedes this field in the LL
+ * control PDU, so total data channel payload length for the control pdu is
+ * one greater.
+ */
+const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] =
+{
+    BLE_LL_CTRL_CONN_UPD_REQ_LEN,
+    BLE_LL_CTRL_CHAN_MAP_LEN,
+    BLE_LL_CTRL_TERMINATE_IND_LEN,
+    BLE_LL_CTRL_ENC_REQ_LEN,
+    BLE_LL_CTRL_ENC_RSP_LEN,
+    BLE_LL_CTRL_START_ENC_REQ_LEN,
+    BLE_LL_CTRL_START_ENC_RSP_LEN,
+    BLE_LL_CTRL_UNK_RSP_LEN,
+    BLE_LL_CTRL_FEATURE_LEN,
+    BLE_LL_CTRL_FEATURE_LEN,
+    BLE_LL_CTRL_PAUSE_ENC_REQ_LEN,
+    BLE_LL_CTRL_PAUSE_ENC_RSP_LEN,
+    BLE_LL_CTRL_VERSION_IND_LEN,
+    BLE_LL_CTRL_REJ_IND_LEN,
+    BLE_LL_CTRL_SLAVE_FEATURE_REQ_LEN,
+    BLE_LL_CTRL_CONN_PARAMS_LEN,
+    BLE_LL_CTRL_CONN_PARAMS_LEN,
+    BLE_LL_CTRL_REJECT_IND_EXT_LEN,
+    BLE_LL_CTRL_PING_LEN,
+    BLE_LL_CTRL_PING_LEN,
+    BLE_LL_CTRL_LENGTH_REQ_LEN,
+    BLE_LL_CTRL_LENGTH_REQ_LEN,
+    BLE_LL_CTRL_PHY_REQ_LEN,
+    BLE_LL_CTRL_PHY_RSP_LEN,
+    BLE_LL_CTRL_PHY_UPD_IND_LEN,
+    BLE_LL_CTRL_MIN_USED_CHAN_LEN,
+    BLE_LL_CTRL_CTE_REQ_LEN,
+    BLE_LL_CTRL_CTE_RSP_LEN,
+    BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN,
+    BLE_LL_CTRL_CLOCK_ACCURACY_REQ_LEN,
+    BLE_LL_CTRL_CLOCK_ACCURACY_RSP_LEN,
+};
+
+/**
+ * Called to determine if a LL control procedure with an instant has
+ * been initiated.
+ *
+ * If the function returns a 0 it means no conflicting procedure has
+ * been initiated. Otherwise it returns the appropriate BLE error code to
+ * send.
+ *
+ * @param connsm Pointer to connection state machine.
+ * @param req_ctrl_proc The procedure that the peer is trying to initiate
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_ll_ctrl_proc_with_instant_initiated(struct ble_ll_conn_sm *connsm,
+                                        uint8_t req_ctrl_proc)
+{
+    uint8_t err;
+
+    switch (connsm->cur_ctrl_proc) {
+    case BLE_LL_CTRL_PROC_PHY_UPDATE:
+    case BLE_LL_CTRL_PROC_CONN_UPDATE:
+    case BLE_LL_CTRL_PROC_CONN_PARAM_REQ:
+    case BLE_LL_CTRL_PROC_CHAN_MAP_UPD:
+        if (req_ctrl_proc == connsm->cur_ctrl_proc) {
+            err = BLE_ERR_LMP_COLLISION;
+        } else if ((connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_UPDATE) &&
+                   (req_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) {
+            err = BLE_ERR_LMP_COLLISION;
+        } else {
+            err = BLE_ERR_DIFF_TRANS_COLL;
+        }
+        break;
+    default:
+        err = 0;
+    }
+
+    return err;
+}
+
+/**
+ * Create a LL_REJECT_EXT_IND pdu.
+ *
+ * @param rej_opcode Opcode to be rejected.
+ * @param err: error response
+ * @param ctrdata: Pointer to where CtrData starts in pdu
+ */
+void
+ble_ll_ctrl_rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata)
+{
+    ctrdata[0] = rej_opcode;
+    ctrdata[1] = err;
+}
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+/**
+ * Called to cancel a phy update procedure.
+ *
+ * @param connsm
+ * @param ble_err
+ */
+void
+ble_ll_ctrl_phy_update_cancel(struct ble_ll_conn_sm *connsm, uint8_t ble_err)
+{
+    /* cancel any pending phy update procedures */
+    CLR_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+
+    /* Check if the host wants an event */
+    if (CONN_F_HOST_PHY_UPDATE(connsm)) {
+        ble_ll_hci_ev_phy_update(connsm, ble_err);
+        CONN_F_HOST_PHY_UPDATE(connsm) = 0;
+    }
+
+    /* Clear any bits for phy updates that might be in progress */
+    CONN_F_CTRLR_PHY_UPDATE(connsm) = 0;
+}
+#endif
+
+static int
+ble_ll_ctrl_len_proc(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    int rc;
+    struct ble_ll_len_req ctrl_req;
+
+    /* Extract parameters and check if valid */
+    ctrl_req.max_rx_bytes = get_le16(dptr);
+    ctrl_req.max_rx_time = get_le16(dptr + 2);
+    ctrl_req.max_tx_bytes = get_le16(dptr + 4);
+    ctrl_req.max_tx_time = get_le16(dptr + 6);
+
+    if ((ctrl_req.max_rx_bytes < BLE_LL_CONN_SUPP_BYTES_MIN) ||
+        (ctrl_req.max_rx_time < BLE_LL_CONN_SUPP_TIME_MIN) ||
+        (ctrl_req.max_tx_bytes < BLE_LL_CONN_SUPP_BYTES_MIN) ||
+        (ctrl_req.max_tx_time < BLE_LL_CONN_SUPP_TIME_MIN)) {
+        rc = 1;
+    } else {
+        /* Update parameters */
+        connsm->rem_max_rx_time = ctrl_req.max_rx_time;
+        connsm->rem_max_tx_time = ctrl_req.max_tx_time;
+        connsm->rem_max_rx_octets = ctrl_req.max_rx_bytes;
+        connsm->rem_max_tx_octets = ctrl_req.max_tx_bytes;
+
+        /* Recalculate effective connection parameters */
+        ble_ll_conn_update_eff_data_len(connsm);
+        rc = 0;
+    }
+
+    return rc;
+}
+
+/**
+ * Process a received LL_PING_RSP control pdu.
+ *
+ * NOTE: we dont have to reset the callout since this packet will have had a
+ * valid MIC and that will restart the authenticated payload timer
+ *
+ * @param connsm
+ */
+static void
+ble_ll_ctrl_rx_ping_rsp(struct ble_ll_conn_sm *connsm)
+{
+    /* Stop the control procedure */
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_LE_PING);
+}
+
+/**
+ * Called when we receive either a connection parameter request or response.
+ *
+ * @param connsm
+ * @param dptr
+ * @param rspbuf
+ * @param opcode
+ *
+ * @return int
+ */
+static int
+ble_ll_ctrl_conn_param_pdu_proc(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                                uint8_t *rspbuf, uint8_t opcode)
+{
+    int rc;
+    int indicate;
+    uint8_t rsp_opcode;
+    uint8_t ble_err;
+    struct ble_ll_conn_params *req;
+    struct hci_conn_update *hcu;
+
+    /* Extract parameters and check if valid */
+    req = &connsm->conn_cp;
+    req->interval_min = get_le16(dptr);
+    req->interval_max = get_le16(dptr + 2);
+    req->latency = get_le16(dptr + 4);
+    req->timeout = get_le16(dptr + 6);
+    req->pref_periodicity = dptr[8];
+    req->ref_conn_event_cnt  = get_le16(dptr + 9);
+    req->offset0 = get_le16(dptr + 11);
+    req->offset1 = get_le16(dptr + 13);
+    req->offset2 = get_le16(dptr + 15);
+    req->offset3 = get_le16(dptr + 17);
+    req->offset4 = get_le16(dptr + 19);
+    req->offset5 = get_le16(dptr + 21);
+
+    /* Check if parameters are valid */
+    ble_err = BLE_ERR_SUCCESS;
+    rc = ble_ll_conn_hci_chk_conn_params(req->interval_min,
+                                         req->interval_max,
+                                         req->latency,
+                                         req->timeout);
+    if (rc) {
+        ble_err = BLE_ERR_INV_LMP_LL_PARM;
+        goto conn_param_pdu_exit;
+    }
+
+    /*
+     * Check if there is a requested change to either the interval, timeout
+     * or latency. If not, this may just be an anchor point change and we do
+     * not have to notify the host.
+     *  XXX: what if we dont like the parameters? When do we check that out?
+     */
+    indicate = 1;
+    if (opcode == BLE_LL_CTRL_CONN_PARM_REQ) {
+        if ((connsm->conn_itvl >= req->interval_min) &&
+            (connsm->conn_itvl <= req->interval_max) &&
+            (connsm->supervision_tmo == req->timeout) &&
+            (connsm->slave_latency == req->latency)) {
+            indicate = 0;
+            goto conn_parm_req_do_indicate;
+        }
+    }
+
+    /*
+     * A change has been requested. Is it within the values specified by
+     * the host? Note that for a master we will not be processing a
+     * connect param request from a slave if we are currently trying to
+     * update the connection parameters. This means that the previous
+     * check is all we need for a master (when receiving a request).
+     */
+    if ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) ||
+        (opcode == BLE_LL_CTRL_CONN_PARM_RSP)) {
+        /*
+         * Not sure what to do about the slave. It is possible that the
+         * current connection parameters are not the same ones as the local host
+         * has provided? Not sure what to do here. Do we need to remember what
+         * host sent us? For now, I will assume that we need to remember what
+         * the host sent us and check it out.
+         */
+        hcu = &connsm->conn_param_req;
+        if (hcu->handle != 0) {
+            if (!((req->interval_min < hcu->conn_itvl_min) ||
+                  (req->interval_min > hcu->conn_itvl_max) ||
+                  (req->interval_max < hcu->conn_itvl_min) ||
+                  (req->interval_max > hcu->conn_itvl_max) ||
+                  (req->latency != hcu->conn_latency) ||
+                  (req->timeout != hcu->supervision_timeout))) {
+                indicate = 0;
+            }
+        }
+    }
+
+conn_parm_req_do_indicate:
+    /*
+     * XXX: are the connection update parameters acceptable? If not, we will
+     * need to know before we indicate to the host that they are acceptable.
+     */
+    if (indicate) {
+        /* If Host masked out Remote Connection Parameter Request Event, we need to
+         * send Reject back to the remote device
+         */
+        if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ)){
+            ble_err = BLE_ERR_UNSUPP_REM_FEATURE;
+            goto conn_param_pdu_exit;
+        }
+
+        /*
+         * Send event to host. At this point we leave and wait to get
+         * an answer.
+         */
+        ble_ll_hci_ev_rem_conn_parm_req(connsm, req);
+        connsm->host_reply_opcode = opcode;
+        connsm->csmflags.cfbit.awaiting_host_reply = 1;
+        rsp_opcode = 255;
+    } else {
+        /* Create reply to connection request */
+        rsp_opcode = ble_ll_ctrl_conn_param_reply(connsm, rspbuf, req);
+    }
+
+conn_param_pdu_exit:
+    if (ble_err) {
+        rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+        rspbuf[1] = opcode;
+        rspbuf[2] = ble_err;
+    }
+    return rsp_opcode;
+}
+
+/**
+ * Called to make a connection update request LL control PDU
+ *
+ * Context: Link Layer
+ *
+ * @param connsm
+ * @param rsp
+ */
+static void
+ble_ll_ctrl_conn_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld,
+                          struct ble_ll_conn_params *cp)
+{
+    uint16_t instant;
+    uint32_t dt;
+    uint32_t num_old_ce;
+    uint32_t new_itvl_usecs;
+    uint32_t old_itvl_usecs;
+    struct hci_conn_update *hcu;
+    struct ble_ll_conn_upd_req *req;
+
+    /*
+     * Set instant. We set the instant to the current event counter plus
+     * the amount of slave latency as the slave may not be listening
+     * at every connection interval and we are not sure when the connect
+     * request will actually get sent. We add one more event plus the
+     * minimum as per the spec of 6 connection events.
+     */
+    instant = connsm->event_cntr + connsm->slave_latency + 6 + 1;
+
+    /*
+     * XXX: This should change in the future, but for now we will just
+     * start the new instant at the same anchor using win offset 0.
+     */
+    /* Copy parameters in connection update structure */
+    hcu = &connsm->conn_param_req;
+    req = &connsm->conn_update_req;
+    if (cp) {
+        /* XXX: so we need to make the new anchor point some time away
+         * from txwinoffset by some amount of msecs. Not sure how to do
+           that here. We dont need to, but we should. */
+        /* Calculate offset from requested offsets (if any) */
+        if (cp->offset0 != 0xFFFF) {
+            new_itvl_usecs = cp->interval_max * BLE_LL_CONN_ITVL_USECS;
+            old_itvl_usecs = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS;
+            if ((int16_t)(cp->ref_conn_event_cnt - instant) >= 0) {
+                num_old_ce = cp->ref_conn_event_cnt - instant;
+                dt = old_itvl_usecs * num_old_ce;
+                dt += (cp->offset0 * BLE_LL_CONN_ITVL_USECS);
+                dt = dt % new_itvl_usecs;
+            } else {
+                num_old_ce = instant - cp->ref_conn_event_cnt;
+                dt = old_itvl_usecs * num_old_ce;
+                dt -= (cp->offset0 * BLE_LL_CONN_ITVL_USECS);
+                dt = dt % new_itvl_usecs;
+                dt = new_itvl_usecs - dt;
+            }
+            req->winoffset = dt / BLE_LL_CONN_TX_WIN_USECS;
+        } else {
+            req->winoffset = 0;
+        }
+        req->interval = cp->interval_max;
+        req->timeout = cp->timeout;
+        req->latency = cp->latency;
+        req->winsize = 1;
+    } else {
+        req->interval = hcu->conn_itvl_max;
+        req->timeout = hcu->supervision_timeout;
+        req->latency = hcu->conn_latency;
+        req->winoffset = 0;
+        req->winsize = connsm->tx_win_size;
+    }
+    req->instant = instant;
+
+    /* XXX: make sure this works for the connection parameter request proc. */
+    pyld[0] = req->winsize;
+    put_le16(pyld + 1, req->winoffset);
+    put_le16(pyld + 3, req->interval);
+    put_le16(pyld + 5, req->latency);
+    put_le16(pyld + 7, req->timeout);
+    put_le16(pyld + 9, instant);
+
+    /* Set flag in state machine to denote we have scheduled an update */
+    connsm->csmflags.cfbit.conn_update_sched = 1;
+}
+
+/**
+ * Called to process and UNKNOWN_RSP LL control packet.
+ *
+ * Context: Link Layer Task
+ *
+ * @param dptr
+ */
+static int
+ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspdata)
+{
+    uint8_t ctrl_proc;
+    uint8_t opcode;
+
+    /* Get opcode of unknown LL control frame */
+    opcode = dptr[0];
+
+    /* Convert opcode to control procedure id */
+    switch (opcode) {
+    case BLE_LL_CTRL_LENGTH_REQ:
+        ctrl_proc = BLE_LL_CTRL_PROC_DATA_LEN_UPD;
+        BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_DATA_LEN_EXT);
+        break;
+    case BLE_LL_CTRL_CONN_UPDATE_IND:
+        ctrl_proc = BLE_LL_CTRL_PROC_CONN_UPDATE;
+        break;
+    case BLE_LL_CTRL_SLAVE_FEATURE_REQ:
+        ctrl_proc = BLE_LL_CTRL_PROC_FEATURE_XCHG;
+        BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_SLAVE_INIT);
+        break;
+    case BLE_LL_CTRL_CONN_PARM_REQ:
+        BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_CONN_PARM_REQ);
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+            ble_ll_ctrl_conn_upd_make(connsm, rspdata, NULL);
+            connsm->reject_reason = BLE_ERR_SUCCESS;
+            return BLE_LL_CTRL_CONN_UPDATE_IND;
+        }
+        /* note: fall-through intentional */
+    case BLE_LL_CTRL_CONN_PARM_RSP:
+        ctrl_proc = BLE_LL_CTRL_PROC_CONN_PARAM_REQ;
+        break;
+    case BLE_LL_CTRL_PING_REQ:
+        /* LL can authenticate remote device even if remote device does not
+         * support LE Ping feature.
+         */
+        ctrl_proc = BLE_LL_CTRL_PROC_LE_PING;
+        BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_LE_PING);
+        break;
+#if (BLE_LL_BT5_PHY_SUPPORTED ==1)
+    case BLE_LL_CTRL_PHY_REQ:
+        ble_ll_ctrl_phy_update_cancel(connsm, BLE_ERR_UNSUPP_REM_FEATURE);
+        ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE;
+        break;
+#endif
+    default:
+        ctrl_proc = BLE_LL_CTRL_PROC_NUM;
+        break;
+    }
+
+    /* If we are running this one currently, stop it */
+    if (connsm->cur_ctrl_proc == ctrl_proc) {
+        /* Stop the control procedure */
+        ble_ll_ctrl_proc_stop(connsm, ctrl_proc);
+        if (ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) {
+            ble_ll_hci_ev_conn_update(connsm, BLE_ERR_UNSUPP_REM_FEATURE);
+        } else if (ctrl_proc == BLE_LL_CTRL_PROC_FEATURE_XCHG) {
+            if (connsm->csmflags.cfbit.pending_hci_rd_features) {
+                ble_ll_hci_ev_rd_rem_used_feat(connsm,
+                                                   BLE_ERR_UNSUPP_REM_FEATURE);
+            }
+            connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+        }
+    }
+
+    return BLE_ERR_MAX;
+}
+
+/**
+ * Callback when LL control procedure times out (for a given connection). If
+ * this is called, it means that we need to end the connection because it
+ * has not responded to a LL control request.
+ *
+ * Context: Link Layer
+ *
+ * @param arg Pointer to connection state machine.
+ */
+static void
+ble_ll_ctrl_proc_rsp_timer_cb(struct ble_npl_event *ev)
+{
+    /* Control procedure has timed out. Kill the connection */
+    ble_ll_conn_timeout((struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev),
+                        BLE_ERR_LMP_LL_RSP_TMO);
+}
+
+static void
+ble_ll_ctrl_start_rsp_timer(struct ble_ll_conn_sm *connsm)
+{
+    ble_npl_callout_init(&connsm->ctrl_proc_rsp_timer,
+                    &g_ble_ll_data.ll_evq,
+                    ble_ll_ctrl_proc_rsp_timer_cb,
+                    connsm);
+
+    /* Re-start timer. Control procedure timeout is 40 seconds */
+    ble_npl_callout_reset(&connsm->ctrl_proc_rsp_timer,
+                     ble_npl_time_ms_to_ticks32(BLE_LL_CTRL_PROC_TIMEOUT_MS));
+}
+
+/**
+ * Convert a phy mask to a numeric phy value.
+ *
+ * NOTE: only one bit should be set here and there should be at least one.
+ * If this function returns a 0 it is an error!
+ *
+ * @param phy_mask Bitmask of phy
+ *
+ * @return uint8_t The numeric value associated with the phy mask
+ *
+ * BLE_HCI_LE_PHY_1M                    (1)
+ * BLE_HCI_LE_PHY_2M                    (2)
+ * BLE_HCI_LE_PHY_CODED                 (3)
+ */
+uint8_t
+ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask)
+{
+    uint8_t phy;
+
+    /*
+     * NOTE: wipe out unsupported PHYs. There should not be an unsupported
+     * in this mask if the other side is working correctly.
+     */
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    phy_mask &= ~BLE_HCI_LE_PHY_2M_PREF_MASK;
+#endif
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    phy_mask &= ~BLE_HCI_LE_PHY_CODED_PREF_MASK;
+#endif
+
+    if (phy_mask & BLE_PHY_MASK_1M) {
+        phy = BLE_PHY_1M;
+        phy_mask &= ~BLE_PHY_MASK_1M;
+    } else if (phy_mask & BLE_PHY_MASK_2M) {
+        phy = BLE_PHY_2M;
+        phy_mask &= ~BLE_PHY_MASK_2M;
+    } else if (phy_mask & BLE_PHY_MASK_CODED) {
+        phy = BLE_PHY_CODED;
+        phy_mask &= ~BLE_PHY_MASK_CODED;
+    } else {
+        phy = 0;
+    }
+
+    if (phy_mask != 0) {
+        phy = 0;
+    }
+
+    return phy;
+}
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+uint8_t
+ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask)
+{
+    /*
+     * Evaluate PHYs in transition starting from the one with longest TX time
+     * so we select the one that allows shortest payload to be sent. This is
+     * to make sure we do not violate timing restriction on new PHY.
+     */
+    if (phy_mask & BLE_PHY_MASK_CODED) {
+        return BLE_PHY_CODED;
+    } else if (phy_mask & BLE_PHY_MASK_1M) {
+        return BLE_PHY_1M;
+    } else if (phy_mask & BLE_PHY_MASK_2M) {
+        return BLE_PHY_2M;
+    }
+
+    return 0;
+}
+
+void
+ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm)
+{
+    int chk_proc_stop;
+    int chk_host_phy;
+
+    chk_proc_stop = 1;
+    chk_host_phy = 1;
+
+    connsm->phy_tx_transition = 0;
+
+    if (CONN_F_PEER_PHY_UPDATE(connsm)) {
+        CONN_F_PEER_PHY_UPDATE(connsm) = 0;
+    } else if (CONN_F_CTRLR_PHY_UPDATE(connsm)) {
+        CONN_F_CTRLR_PHY_UPDATE(connsm) = 0;
+    } else {
+        /* Must be a host-initiated update */
+        CONN_F_HOST_PHY_UPDATE(connsm) = 0;
+        chk_host_phy = 0;
+        if (CONN_F_PHY_UPDATE_EVENT(connsm) == 0) {
+            ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS);
+        }
+    }
+
+    /* Must check if we need to start host procedure */
+    if (chk_host_phy) {
+        if (CONN_F_HOST_PHY_UPDATE(connsm)) {
+            if (ble_ll_conn_chk_phy_upd_start(connsm)) {
+                CONN_F_HOST_PHY_UPDATE(connsm) = 0;
+            } else {
+                chk_proc_stop = 0;
+            }
+        }
+    }
+
+    if (chk_proc_stop) {
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+    }
+}
+
+/**
+ *
+ *  There is probably a better way for the controller to choose which PHY use.
+ *  There are no BER metrics and RSSI does not give you S/N so for now we will
+ *  choose this heirarchy:
+ *     -> if 2Mbps available, use it.
+ *     -> If 1Mbps available, use it.
+ *     -> otherwise use coded phy.
+ *
+ * @param prefs The mask of preferred phys
+ * @return uint8_t The phy to use (not a mask)
+ */
+static uint8_t
+ble_ll_ctrl_find_new_phy(uint8_t phy_mask_prefs)
+{
+    uint8_t new_phy;
+
+    new_phy = phy_mask_prefs;
+    if (new_phy) {
+        if (new_phy & BLE_PHY_MASK_2M) {
+            new_phy = BLE_PHY_2M;
+        } else if (new_phy & BLE_PHY_MASK_1M) {
+            new_phy = BLE_PHY_1M;
+        } else {
+            new_phy = BLE_PHY_CODED;
+        }
+    }
+
+    return new_phy;
+}
+
+/**
+ * Create a LL_PHY_UPDATE_IND pdu
+ *
+ * @param connsm Pointer to connection state machine
+ * @param dptr Pointer to PHY_REQ or PHY_RSP data.
+ * @param ctrdata: Pointer to where CtrData of UPDATE_IND pdu starts
+ * @param slave_req flag denoting if slave requested this. 0: no 1:yes
+ */
+static void
+ble_ll_ctrl_phy_update_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                                uint8_t *ctrdata, int slave_req)
+{
+    uint8_t m_to_s;
+    uint8_t s_to_m;
+    uint8_t tx_phys;
+    uint8_t rx_phys;
+    uint16_t instant;
+    uint8_t is_slave_sym = 0;
+
+    /* Get preferences from PDU */
+    tx_phys = dptr[0];
+    rx_phys = dptr[1];
+
+    /* If we are master, check if slave requested symmetric PHY */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        is_slave_sym = tx_phys == rx_phys;
+        is_slave_sym &= __builtin_popcount(tx_phys) == 1;
+    }
+
+    /* Get m_to_s and s_to_m masks */
+    if (slave_req) {
+        m_to_s = connsm->phy_data.host_pref_tx_phys_mask & rx_phys;
+        s_to_m = connsm->phy_data.host_pref_rx_phys_mask & tx_phys;
+    } else {
+        m_to_s = connsm->phy_data.req_pref_tx_phys_mask & rx_phys;
+        s_to_m = connsm->phy_data.req_pref_rx_phys_mask & tx_phys;
+    }
+
+    if (is_slave_sym) {
+        /*
+         * If either s_to_m or m_to_s is 0, it means for at least one direction
+         * requested PHY is not our preferred one so make sure we keep current
+         * PHY in both directions
+         *
+         * Core 5.2, Vol 6, PartB, 5.1.10
+         *     If the slave specified a single PHY in both the TX_PHYS and
+         *     RX_PHYS fields and both fields are the same, the master shall
+         *     either select the PHY specified by the slave for both directions
+         *     or shall leave both directions unchanged.
+         */
+        if ((s_to_m == 0) || (m_to_s == 0)) {
+            s_to_m = 0;
+            m_to_s = 0;
+        } else {
+            BLE_LL_ASSERT(s_to_m == m_to_s);
+        }
+    }
+
+    /* Calculate new PHYs to use */
+    m_to_s = ble_ll_ctrl_find_new_phy(m_to_s);
+    s_to_m = ble_ll_ctrl_find_new_phy(s_to_m);
+
+    /* Make sure we do not indicate PHY change if the same as current one */
+    if (m_to_s == connsm->phy_data.cur_tx_phy) {
+        m_to_s = 0;
+    }
+    if (s_to_m == connsm->phy_data.cur_rx_phy) {
+        s_to_m = 0;
+    }
+
+    /* At this point, m_to_s and s_to_m are not masks; they are numeric */
+
+    /*
+     * If not changing we still send update ind. Check if hosts expects
+     * the event and if so send it. Stop control procedure if it is the
+     * one running.
+     */
+    if ((m_to_s == 0) && (s_to_m == 0)) {
+        if (CONN_F_PEER_PHY_UPDATE(connsm)) {
+            CONN_F_PEER_PHY_UPDATE(connsm) = 0;
+        } else if (CONN_F_CTRLR_PHY_UPDATE(connsm)) {
+            CONN_F_CTRLR_PHY_UPDATE(connsm) = 0;
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+        } else {
+            ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS);
+            CONN_F_HOST_PHY_UPDATE(connsm) = 0;
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+        }
+        instant = 0;
+    } else {
+        /* Determine instant we will use. 6 more is minimum */
+        instant = connsm->event_cntr + connsm->slave_latency + 6 + 1;
+        connsm->phy_instant = instant;
+        CONN_F_PHY_UPDATE_SCHED(connsm) = 1;
+
+        /* Set new phys to use when instant occurs */
+        connsm->phy_data.new_tx_phy = m_to_s;
+        connsm->phy_data.new_rx_phy = s_to_m;
+
+        /* Convert m_to_s and s_to_m to masks */
+        if (m_to_s) {
+            m_to_s = 1 << (m_to_s - 1);
+        }
+
+        if (s_to_m) {
+            s_to_m = 1 << (s_to_m - 1);
+        }
+    }
+
+    ctrdata[0] = m_to_s;
+    ctrdata[1] = s_to_m;
+    put_le16(ctrdata + 2, instant);
+}
+
+/**
+ * Create a LL_PHY_REQ or LL_PHY_RSP pdu
+ *
+ * @param connsm Pointer to connection state machine
+ * @param ctrdata: Pointer to where CtrData starts in pdu
+ */
+static void
+ble_ll_ctrl_phy_req_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *ctrdata)
+{
+    /* If no preference we use current phy */
+    if (connsm->phy_data.host_pref_tx_phys_mask == 0) {
+        ctrdata[0] = CONN_CUR_TX_PHY_MASK(connsm);
+    } else {
+        ctrdata[0] = connsm->phy_data.host_pref_tx_phys_mask;
+    }
+    if (connsm->phy_data.host_pref_rx_phys_mask == 0) {
+        ctrdata[1] = CONN_CUR_RX_PHY_MASK(connsm);
+    } else {
+        ctrdata[1] = connsm->phy_data.host_pref_rx_phys_mask;
+    }
+}
+
+static uint8_t
+ble_ll_ctrl_rx_phy_req(struct ble_ll_conn_sm *connsm, uint8_t *req,
+                       uint8_t *rsp)
+{
+    uint8_t rsp_opcode;
+    uint8_t err;
+
+    /*
+     * XXX: TODO if we have an instant in progress we should end connection.
+     * At least it seems that is the case. Need to figure out more from
+     * the spec here.
+     */
+
+    /* Check if we have already initiated a procedure with an instant */
+    err = ble_ll_ctrl_proc_with_instant_initiated(connsm,
+                                                  BLE_LL_CTRL_PROC_PHY_UPDATE);
+
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        if (err) {
+            ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_PHY_REQ, err, rsp);
+            rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+        } else {
+            /*
+             * NOTE: do not change order of these two lines as the call to
+             * make the LL_PHY_UPDATE_IND pdu might clear the flag.
+             */
+            CONN_F_PEER_PHY_UPDATE(connsm) = 1;
+            ble_ll_ctrl_phy_update_ind_make(connsm, req, rsp, 1);
+            rsp_opcode = BLE_LL_CTRL_PHY_UPDATE_IND;
+        }
+    } else {
+        /* XXX: deal with other control procedures that we need to stop */
+        if (err) {
+            if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_PHY_UPDATE) {
+                ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer);
+                connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE;
+            }
+
+            /* If there is a PHY update procedure pending cancel it */
+            ble_ll_ctrl_phy_update_cancel(connsm, err);
+
+            /* XXX: ? Should not be any phy update events */
+            CONN_F_PHY_UPDATE_EVENT(connsm) = 0;
+        }
+
+        /* XXX: TODO: if we started another procedure with an instant
+         * why are we doing this? Need to look into this.*/
+
+        /* Respond to master's phy update procedure */
+        CONN_F_PEER_PHY_UPDATE(connsm) = 1;
+        ble_ll_ctrl_phy_req_rsp_make(connsm, rsp);
+        rsp_opcode = BLE_LL_CTRL_PHY_RSP;
+
+        connsm->phy_tx_transition = ble_ll_ctrl_phy_tx_transition_get(req[1] | rsp[0]);
+
+        /* Start response timer */
+        connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE;
+        ble_ll_ctrl_start_rsp_timer(connsm);
+    }
+    return rsp_opcode;
+}
+
+/**
+ * Process a received LL_PHY_RSP pdu
+ *
+ * @param connsm
+ * @param dptr Pointer to LL_PHY_RSP ctrdata
+ * @param rsp Pointer to CtrData of PHY_UPDATE_IND.
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_phy_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                       uint8_t *rsp)
+{
+    uint8_t rsp_opcode;
+
+    rsp_opcode = BLE_ERR_MAX;
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_PHY_UPDATE) {
+            ble_ll_ctrl_phy_update_ind_make(connsm, dptr, rsp, 0);
+            ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer);
+            rsp_opcode = BLE_LL_CTRL_PHY_UPDATE_IND;
+        }
+
+        /*
+         * If not in the process of doing this control procedure something
+         * is wrong. End connection? Assert?
+         *
+         * XXX: TODO count some stat?
+         */
+    } else {
+        rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    /* NOTE: slave should never receive one of these */
+
+    return rsp_opcode;
+}
+
+/**
+ * Called when a LL_PHY_UPDATE_IND pdu is received
+ *
+ * NOTE: slave is the only device that should receive this.
+ *
+ * @param connsm
+ * @param dptr
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_phy_update_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    int no_change;
+    uint8_t new_m_to_s_mask;
+    uint8_t new_s_to_m_mask;
+    uint8_t new_tx_phy;
+    uint8_t new_rx_phy;
+    uint16_t instant;
+    uint16_t delta;
+
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        return BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    /*
+     * Reception stops the procedure response timer but does not
+     * complete the procedure
+     */
+    if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_PHY_UPDATE) {
+        ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer);
+    }
+
+    /*
+     * XXX: Should we check to see if we are expecting to receive one
+     * of these, and if not, kill connection? Meaning we better be
+     * doing either a PEER, CTRLR, or HOST phy update.
+     */
+    /* get the new phy masks and see if we need to change */
+    new_m_to_s_mask = dptr[0];
+    new_s_to_m_mask = dptr[1];
+    instant = get_le16(dptr + 2);
+
+    if ((new_m_to_s_mask == 0) && (new_s_to_m_mask == 0)) {
+        /* No change in phy */
+        no_change = 1;
+    } else {
+        no_change = 0;
+        /*
+         * NOTE: from the slaves perspective, the m to s phy is the one
+         * that the slave will receive on; s to m is the one it will
+         * transmit on
+         */
+        new_rx_phy = ble_ll_ctrl_phy_from_phy_mask(new_m_to_s_mask);
+        new_tx_phy = ble_ll_ctrl_phy_from_phy_mask(new_s_to_m_mask);
+
+        if ((new_tx_phy == 0) && (new_rx_phy == 0)) {
+            /* XXX: this is an error! What to do??? */
+            no_change = 1;
+        }
+
+        if ((new_tx_phy == connsm->phy_data.cur_tx_phy) &&
+            (new_rx_phy == connsm->phy_data.cur_rx_phy)) {
+            no_change = 1;
+        }
+    }
+
+    if (!no_change) {
+        /* If instant is in the past, we have to end the connection */
+        delta = (instant - connsm->event_cntr) & 0xFFFF;
+        if (delta >= 32767) {
+            ble_ll_conn_timeout(connsm, BLE_ERR_INSTANT_PASSED);
+        } else {
+            connsm->phy_data.new_tx_phy = new_tx_phy;
+            connsm->phy_data.new_rx_phy = new_rx_phy;
+            connsm->phy_instant = instant;
+            CONN_F_PHY_UPDATE_SCHED(connsm) = 1;
+        }
+        return BLE_ERR_MAX;
+    }
+
+    ble_ll_ctrl_phy_update_proc_complete(connsm);
+
+    return BLE_ERR_MAX;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+/**
+ * Called when a BLE_LL_CTRL_PERIODIC_SYNC_IND PDU is received
+ *
+ * @param connsm
+ * @param dptr
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_periodic_sync_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    if (connsm->sync_transfer_mode) {
+        ble_ll_sync_periodic_ind(connsm, dptr, connsm->sync_transfer_mode == 1,
+                                 connsm->sync_transfer_skip,
+                                 connsm->sync_transfer_sync_timeout);
+    }
+
+    return BLE_ERR_MAX;
+}
+#endif
+
+/**
+ * Create a link layer length request or length response PDU.
+ *
+ * NOTE: this function does not set the LL data pdu header nor does it
+ * set the opcode in the buffer.
+ *
+ * @param connsm
+ * @param dptr: Pointer to where control pdu payload starts
+ */
+static void
+ble_ll_ctrl_datalen_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    put_le16(dptr + 1, connsm->max_rx_octets);
+    put_le16(dptr + 3, connsm->max_rx_time);
+    put_le16(dptr + 5, connsm->max_tx_octets);
+    put_le16(dptr + 7, connsm->max_tx_time);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+void
+ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm)
+{
+#ifdef BLE_LL_ENCRYPT_DEBUG
+    int cnt;
+#endif
+
+    /* XXX: possibly have some way out of this if this locks up */
+    while (1) {
+        if (!ble_hw_encrypt_block(&connsm->enc_data.enc_block)) {
+            break;
+        }
+    }
+
+#ifdef BLE_LL_ENCRYPT_DEBUG
+    console_printf("Calculating Session Key for handle=%u",
+                   connsm->conn_handle);
+
+    console_printf("\nLTK:");
+    for (cnt = 0; cnt < 16; ++cnt) {
+        console_printf("%02x", connsm->enc_data.enc_block.key[cnt]);
+    }
+    console_printf("\nSKD:");
+    for (cnt = 0; cnt < 16; ++cnt) {
+        console_printf("%02x", connsm->enc_data.enc_block.plain_text[cnt]);
+    }
+    console_printf("\nSession Key:");
+    for (cnt = 0; cnt < 16; ++cnt) {
+        console_printf("%02x", connsm->enc_data.enc_block.cipher_text[cnt]);
+    }
+    console_printf("\nIV:");
+    for (cnt = 0; cnt < 8; ++ cnt) {
+        console_printf("%02x", connsm->enc_data.iv[cnt]);
+    }
+    console_printf("\n");
+#endif
+}
+
+/**
+ * Called to determine if this is a control PDU we are allowed to send. This
+ * is called when a link is being encrypted, as only certain control PDU's
+ * area lowed to be sent.
+ *
+ * XXX: the current code may actually allow some control pdu's to be sent
+ * in states where they shouldnt. I dont expect those states to occur so I
+ * dont try to check for them but we could do more... for example there are
+ * different PDUs allowed for master/slave and TX/RX
+ *
+ * @param llid
+ * @param opcode
+ * @param len
+ *
+ * @return int
+ */
+static int
+ble_ll_ctrl_enc_allowed_pdu(uint8_t llid, uint8_t len, uint8_t opcode)
+{
+    int allowed;
+
+    allowed = 0;
+
+    switch (llid) {
+    case BLE_LL_LLID_CTRL:
+        switch (opcode) {
+        case BLE_LL_CTRL_REJECT_IND:
+        case BLE_LL_CTRL_REJECT_IND_EXT:
+        case BLE_LL_CTRL_START_ENC_RSP:
+        case BLE_LL_CTRL_START_ENC_REQ:
+        case BLE_LL_CTRL_ENC_REQ:
+        case BLE_LL_CTRL_ENC_RSP:
+        case BLE_LL_CTRL_PAUSE_ENC_REQ:
+        case BLE_LL_CTRL_PAUSE_ENC_RSP:
+        case BLE_LL_CTRL_TERMINATE_IND:
+            allowed = 1;
+            break;
+        }
+        break;
+    case BLE_LL_LLID_DATA_FRAG:
+        if (len == 0) {
+            /* Empty PDUs are allowed */
+            allowed = 1;
+        }
+        break;
+    }
+
+    return allowed;
+}
+
+int
+ble_ll_ctrl_enc_allowed_pdu_rx(struct os_mbuf *rxpdu)
+{
+    uint8_t llid;
+    uint8_t len;
+    uint8_t opcode;
+
+    llid = rxpdu->om_data[0] & BLE_LL_DATA_HDR_LLID_MASK;
+    len = rxpdu->om_data[1];
+    if (llid == BLE_LL_LLID_CTRL) {
+        opcode = rxpdu->om_data[2];
+    } else {
+        opcode = 0;
+    }
+
+    return ble_ll_ctrl_enc_allowed_pdu(llid, len, opcode);
+}
+
+int
+ble_ll_ctrl_enc_allowed_pdu_tx(struct os_mbuf_pkthdr *pkthdr)
+{
+    struct os_mbuf *m;
+    struct ble_mbuf_hdr *ble_hdr;
+    uint8_t llid;
+    uint8_t len;
+    uint8_t opcode;
+
+    m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+    ble_hdr = BLE_MBUF_HDR_PTR(m);
+
+    llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+    len = ble_hdr->txinfo.pyld_len;
+    if (llid == BLE_LL_LLID_CTRL) {
+        opcode = m->om_data[0];
+    } else {
+        opcode = 0;
+    }
+
+    return ble_ll_ctrl_enc_allowed_pdu(llid, len, opcode);
+}
+
+int
+ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu)
+{
+    int is_start_enc_rsp;
+    uint8_t opcode;
+    uint8_t llid;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    is_start_enc_rsp = 0;
+    ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+
+    llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+    if (llid == BLE_LL_LLID_CTRL) {
+        opcode = txpdu->om_data[0];
+        if (opcode == BLE_LL_CTRL_START_ENC_RSP) {
+            is_start_enc_rsp = 1;
+        }
+    }
+
+    return is_start_enc_rsp;
+}
+
+/**
+ * Called to create and send a LL_START_ENC_REQ
+ *
+ * @param connsm
+ * @param err
+ *
+ * @return int
+ */
+int
+ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+    struct os_mbuf *om;
+
+    om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+                            sizeof(struct ble_mbuf_hdr));
+    if (om) {
+        om->om_data[0] = BLE_LL_CTRL_START_ENC_REQ;
+        ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, 1);
+
+        /* Wait for LL_START_ENC_RSP. If there is already procedure in progress,
+         * LL response timer is already running.
+         */
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) {
+            connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_ENCRYPT;
+            ble_ll_ctrl_start_rsp_timer(connsm);
+        }
+
+        rc = 0;
+    } else {
+        rc = -1;
+    }
+    return rc;
+}
+
+/**
+ * Create a link layer control "encrypt request" PDU.
+ *
+ * The LL_ENC_REQ PDU format is:
+ *      Rand    (8)
+ *      EDIV    (2)
+ *      SKDm    (8)
+ *      IVm     (4)
+ *
+ * The random number and encrypted diversifier come from the host command.
+ * Controller generates master portion of SDK and IV.
+ *
+ * NOTE: this function does not set the LL data pdu header nor does it
+ * set the opcode in the buffer.
+ *
+ * @param connsm
+ * @param dptr: Pointer to where control pdu payload starts
+ */
+static void
+ble_ll_ctrl_enc_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    put_le64(dptr, connsm->enc_data.host_rand_num);
+    put_le16(dptr + 8, connsm->enc_data.enc_div);
+
+#ifdef BLE_LL_ENCRYPT_USE_TEST_DATA
+    /* IV stored LSB to MSB, IVm is LSB, IVs is MSB */
+    put_le64(dptr + 10, g_bletest_SKDm);
+    swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8);
+    put_le32(dptr + 18, g_bletest_IVm);
+    memcpy(connsm->enc_data.iv, dptr + 18, 4);
+    return;
+#endif
+
+    ble_ll_rand_data_get(connsm->enc_data.enc_block.plain_text + 8, 8);
+    swap_buf(dptr + 10, connsm->enc_data.enc_block.plain_text + 8, 8);
+    ble_ll_rand_data_get(connsm->enc_data.iv, 4);
+    memcpy(dptr + 18, connsm->enc_data.iv, 4);
+}
+
+/**
+ * Called when LL_ENC_RSP is received by the master.
+ *
+ * Context: Link Layer Task.
+ *
+ * Format of the LL_ENC_RSP is:
+ *      SKDs (8)
+ *      IVs  (4)
+ *
+ *  The master now has the long term key (from the start encrypt command)
+ *  and the SKD (stored in the plain text encryption block). From this the
+ *  sessionKey is generated.
+ *
+ * @param connsm
+ * @param dptr
+ */
+static void
+ble_ll_ctrl_rx_enc_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    /* Calculate session key now that we have received the ENC_RSP */
+    if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) {
+        /* In case we were already encrypted we need to reset packet counters */
+        connsm->enc_data.rx_pkt_cntr = 0;
+        connsm->enc_data.tx_pkt_cntr = 0;
+        connsm->enc_data.tx_encrypted = 0;
+
+        swap_buf(connsm->enc_data.enc_block.plain_text, dptr, 8);
+        memcpy(connsm->enc_data.iv + 4, dptr + 8, 4);
+        ble_ll_calc_session_key(connsm);
+        connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_REQ_WAIT;
+    }
+}
+
+/**
+ * Called when we have received a LL control encryption request PDU. This
+ * should only be received by a slave.
+ *
+ * The LL_ENC_REQ PDU format is:
+ *      Rand    (8)
+ *      EDIV    (2)
+ *      SKDm    (8)
+ *      IVm     (4)
+ *
+ * This function returns the response opcode. Typically this will be ENC_RSP
+ * but it could be a reject ind. Note that the caller of this function
+ * will send the REJECT_IND_EXT if supported by remote.
+ *
+ * NOTE: if this is received by a master we will silently discard the PDU
+ * (denoted by return BLE_ERR_MAX).
+ *
+ * @param connsm
+ * @param dptr      Pointer to start of encrypt request data.
+ * @param rspbuf
+ */
+static uint8_t
+ble_ll_ctrl_rx_enc_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                       uint8_t *rspdata)
+{
+    if (connsm->conn_role != BLE_LL_CONN_ROLE_SLAVE) {
+        return BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    connsm->enc_data.enc_state = CONN_ENC_S_LTK_REQ_WAIT;
+
+    /* In case we were already encrypted we need to reset packet counters */
+    connsm->enc_data.rx_pkt_cntr = 0;
+    connsm->enc_data.tx_pkt_cntr = 0;
+    connsm->enc_data.tx_encrypted = 0;
+
+    /* Extract information from request */
+    connsm->enc_data.host_rand_num = get_le64(dptr);
+    connsm->enc_data.enc_div = get_le16(dptr + 8);
+
+#if BLE_LL_ENCRYPT_USE_TEST_DATA
+    swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8);
+    memcpy(connsm->enc_data.iv, dptr + 18, 4);
+
+    put_le64(rspdata, g_bletest_SKDs);
+    swap_buf(connsm->enc_data.enc_block.plain_text, rspdata, 8);
+    put_le32(rspdata + 8, g_bletest_IVs);
+    memcpy(connsm->enc_data.iv + 4, rspdata + 8, 4);
+    return BLE_LL_CTRL_ENC_RSP;
+#endif
+
+    swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8);
+    memcpy(connsm->enc_data.iv, dptr + 18, 4);
+
+    /* Create the ENC_RSP. Concatenate our SKD and IV */
+    ble_ll_rand_data_get(connsm->enc_data.enc_block.plain_text, 8);
+    swap_buf(rspdata, connsm->enc_data.enc_block.plain_text, 8);
+    ble_ll_rand_data_get(connsm->enc_data.iv + 4, 4);
+    memcpy(rspdata + 8, connsm->enc_data.iv + 4, 4);
+
+    return BLE_LL_CTRL_ENC_RSP;
+}
+
+static uint8_t
+ble_ll_ctrl_rx_start_enc_req(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+
+    /* Only master should receive start enc request */
+    rc = BLE_ERR_MAX;
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        /* We only want to send a START_ENC_RSP if we havent yet */
+        if (connsm->enc_data.enc_state == CONN_ENC_S_START_ENC_REQ_WAIT) {
+            connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_RSP_WAIT;
+            rc = BLE_LL_CTRL_START_ENC_RSP;
+        }
+    } else {
+        rc = BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+    return rc;
+}
+
+static uint8_t
+ble_ll_ctrl_rx_pause_enc_req(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+
+    /*
+     * The spec does not say what to do here, but if we receive a pause
+     * encryption request and we are not encrypted, what do we do? We
+     * ignore it...
+     */
+    rc = BLE_ERR_MAX;
+    if ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) &&
+        (connsm->enc_data.enc_state == CONN_ENC_S_ENCRYPTED)) {
+        rc = BLE_LL_CTRL_PAUSE_ENC_RSP;
+    } else {
+        rc = BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    return rc;
+}
+
+/**
+ * Called when a LL control pdu with opcode PAUSE_ENC_RSP is received.
+ *
+ *
+ * @param connsm
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_pause_enc_rsp(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        rc = BLE_LL_CTRL_PAUSE_ENC_RSP;
+    } else if (connsm->enc_data.enc_state == CONN_ENC_S_PAUSE_ENC_RSP_WAIT) {
+        /* Master sends back unencrypted LL_PAUSE_ENC_RSP.
+         * From this moment encryption is paused.
+         */
+        rc = BLE_ERR_MAX;
+        connsm->enc_data.enc_state = CONN_ENC_S_PAUSED;
+    } else {
+        rc = BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    return rc;
+}
+
+/**
+ * Called when we have received a LL_CTRL_START_ENC_RSP.
+ *
+ * Context: Link-layer task
+ *
+ * @param connsm
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_start_enc_rsp(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+
+    /* Not in proper state. Discard */
+    if (connsm->enc_data.enc_state != CONN_ENC_S_START_ENC_RSP_WAIT) {
+        return BLE_ERR_MAX;
+    }
+
+    /* If master, we are done. Stop control procedure and sent event to host */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+
+        /* We are encrypted */
+        connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+        ble_ll_conn_auth_pyld_timer_start(connsm);
+#endif
+        rc = BLE_ERR_MAX;
+    } else {
+        /* Procedure has completed but slave needs to send START_ENC_RSP */
+        rc = BLE_LL_CTRL_START_ENC_RSP;
+
+        /* Stop timer if it was started when sending START_ENC_REQ */
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) {
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+        }
+    }
+
+    /*
+     * XXX: for now, a Slave sends this event when it receivest the
+     * START_ENC_RSP from the master. It might be technically incorrect
+     * to send it before we transmit our own START_ENC_RSP.
+     */
+    ble_ll_hci_ev_encrypt_chg(connsm, BLE_ERR_SUCCESS);
+
+    return rc;
+}
+
+#endif
+
+/**
+ * Called to make a connection parameter request or response control pdu.
+ *
+ * @param connsm
+ * @param dptr Pointer to start of data. NOTE: the opcode is not part
+ *             of the data.
+ */
+static void
+ble_ll_ctrl_conn_param_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                                struct ble_ll_conn_params *req)
+{
+    uint16_t offset;
+    struct hci_conn_update *hcu;
+
+    /* If we were passed in a request, we use the parameters from the request */
+    if (req) {
+        put_le16(dptr, req->interval_min);
+        put_le16(dptr + 2, req->interval_max);
+        put_le16(dptr + 4, req->latency);
+        put_le16(dptr + 6, req->timeout);
+    } else {
+        hcu = &connsm->conn_param_req;
+        /* The host should have provided the parameters! */
+        BLE_LL_ASSERT(hcu->handle != 0);
+        put_le16(dptr, hcu->conn_itvl_min);
+        put_le16(dptr + 2, hcu->conn_itvl_max);
+        put_le16(dptr + 4, hcu->conn_latency);
+        put_le16(dptr + 6, hcu->supervision_timeout);
+    }
+
+    /* XXX: NOTE: if interval min and interval max are != to each
+     * other this value should be set to non-zero. I think this
+     * applies only when an offset field is set. See section 5.1.7.1 pg 103
+     * Vol 6 Part B.
+     */
+    /* XXX: for now, set periodicity to 0 */
+    dptr[8] = 0;
+
+    /* XXX: deal with reference event count. what to put here? */
+    put_le16(dptr + 9, connsm->event_cntr);
+
+    /* XXX: For now, dont use offsets */
+    offset = 0xFFFF;
+    put_le16(dptr + 11, offset);
+    put_le16(dptr + 13, offset);
+    put_le16(dptr + 15, offset);
+    put_le16(dptr + 17, offset);
+    put_le16(dptr + 19, offset);
+    put_le16(dptr + 21, offset);
+}
+
+static void
+ble_ll_ctrl_version_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld)
+{
+    /* Set flag to denote we have sent/received this */
+    connsm->csmflags.cfbit.version_ind_sent = 1;
+
+    /* Fill out response */
+    pyld[0] = BLE_HCI_VER_BCS;
+    put_le16(pyld + 1, MYNEWT_VAL(BLE_LL_MFRG_ID));
+    put_le16(pyld + 3, BLE_LL_SUB_VERS_NR);
+}
+
+/**
+ * Called to make a LL control channel map request PDU.
+ *
+ * @param connsm    Pointer to connection state machine
+ * @param pyld      Pointer to payload of LL control PDU
+ */
+static void
+ble_ll_ctrl_chanmap_req_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld)
+{
+    /* Copy channel map that host desires into request */
+    memcpy(pyld, g_ble_ll_conn_params.master_chan_map, BLE_LL_CONN_CHMAP_LEN);
+    memcpy(connsm->req_chanmap, pyld, BLE_LL_CONN_CHMAP_LEN);
+
+    /* Place instant into request */
+    connsm->chanmap_instant = connsm->event_cntr + connsm->slave_latency + 6 + 1;
+    put_le16(pyld + BLE_LL_CONN_CHMAP_LEN, connsm->chanmap_instant);
+
+    /* Set scheduled flag */
+    connsm->csmflags.cfbit.chanmap_update_scheduled = 1;
+}
+
+/**
+ * Called to respond to a LL control PDU connection parameter request or
+ * response.
+ *
+ * @param connsm
+ * @param rsp
+ * @param req
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm, uint8_t *rsp,
+                             struct ble_ll_conn_params *req)
+{
+    uint8_t rsp_opcode;
+
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+        /* Create a connection parameter response */
+        ble_ll_ctrl_conn_param_pdu_make(connsm, rsp + 1, req);
+        rsp_opcode = BLE_LL_CTRL_CONN_PARM_RSP;
+    } else {
+        /* Create a connection update pdu */
+        ble_ll_ctrl_conn_upd_make(connsm, rsp + 1, req);
+        rsp_opcode = BLE_LL_CTRL_CONN_UPDATE_IND;
+    }
+
+    return rsp_opcode;
+}
+
+/**
+ * Called when we have received a LL_REJECT_IND or LL_REJECT_IND_EXT link
+ * layer control Data Channel pdu.
+ *
+ * @param connsm
+ * @param dptr
+ * @param opcode
+ */
+static int
+ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                          uint8_t opcode, uint8_t *rspdata)
+{
+    uint8_t ble_error;
+    uint8_t rsp_opcode = BLE_ERR_MAX;
+
+    /* Get error out of received PDU */
+    if (opcode == BLE_LL_CTRL_REJECT_IND) {
+        ble_error = dptr[0];
+    } else {
+        ble_error = dptr[1];
+    }
+
+    /* XXX: should I check to make sure the rejected opcode is sane
+       if we receive ind ext? */
+    switch (connsm->cur_ctrl_proc) {
+    case BLE_LL_CTRL_PROC_CONN_PARAM_REQ:
+        if (opcode == BLE_LL_CTRL_REJECT_IND_EXT) {
+            /* As a master we should send connection update indication in this point */
+            if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+                rsp_opcode = BLE_LL_CTRL_CONN_UPDATE_IND;
+                ble_ll_ctrl_conn_upd_make(connsm, rspdata, NULL);
+                connsm->reject_reason = BLE_ERR_SUCCESS;
+            } else {
+                ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
+                ble_ll_hci_ev_conn_update(connsm, ble_error);
+            }
+        }
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    case BLE_LL_CTRL_PROC_ENCRYPT:
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+        ble_ll_hci_ev_encrypt_chg(connsm, ble_error);
+        connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+        break;
+#endif
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    case BLE_LL_CTRL_PROC_PHY_UPDATE:
+        ble_ll_ctrl_phy_update_cancel(connsm, ble_error);
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+        break;
+#endif
+    case BLE_LL_CTRL_PROC_DATA_LEN_UPD:
+        /* That should not happen according to Bluetooth 5.0 Vol6 Part B, 5.1.9
+         * However we need this workaround as there are devices on the market
+         * which do send LL_REJECT on LL_LENGTH_REQ when collision happens
+         */
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD);
+        break;
+    default:
+        break;
+    }
+
+    return rsp_opcode;
+}
+
+/**
+ * Called when we receive a connection update event
+ *
+ * @param connsm
+ * @param dptr
+ *
+ * @return int
+ */
+static int
+ble_ll_ctrl_rx_conn_update(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    uint8_t rsp_opcode;
+    uint16_t conn_events;
+    struct ble_ll_conn_upd_req *reqdata;
+
+    /* Only a slave should receive this */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        return BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    /* Retrieve parameters */
+    reqdata = &connsm->conn_update_req;
+    reqdata->winsize = dptr[0];
+    reqdata->winoffset = get_le16(dptr + 1);
+    reqdata->interval = get_le16(dptr + 3);
+    reqdata->latency = get_le16(dptr + 5);
+    reqdata->timeout = get_le16(dptr + 7);
+    reqdata->instant = get_le16(dptr + 9);
+
+    /* XXX: validate them at some point. If they dont check out, we
+       return the unknown response */
+    rsp_opcode = BLE_ERR_MAX;
+
+    /* If instant is in the past, we have to end the connection */
+    conn_events = (reqdata->instant - connsm->event_cntr) & 0xFFFF;
+    if (conn_events >= 32767) {
+        ble_ll_conn_timeout(connsm, BLE_ERR_INSTANT_PASSED);
+    } else {
+        connsm->csmflags.cfbit.conn_update_sched = 1;
+
+        /*
+         * Errata says that receiving a connection update when the event
+         * counter is equal to the instant means wesimply ignore the window
+         * offset and window size. Anchor point has already been set based on
+         * first packet received in connection event. Given that we increment
+         * the event counter BEFORE checking to see if the instant is equal to
+         * the event counter what we do here is increment the instant and set
+         * the window offset and size to 0.
+         */
+        if (conn_events == 0) {
+            reqdata->winoffset = 0;
+            reqdata->winsize = 0;
+            reqdata->instant += 1;
+        }
+    }
+
+    return rsp_opcode;
+}
+
+void
+ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm)
+{
+    if (!(connsm->conn_features & BLE_LL_FEAT_DATA_LEN_EXT)) {
+        return;
+    }
+
+    /*
+     * Section 4.5.10 Vol 6 PART B. If the max tx/rx time or octets
+     * exceeds the minimum, data length procedure needs to occur
+     */
+    if ((connsm->max_tx_octets <= BLE_LL_CONN_SUPP_BYTES_MIN) &&
+        (connsm->max_rx_octets <= BLE_LL_CONN_SUPP_BYTES_MIN) &&
+        (connsm->max_tx_time <= BLE_LL_CONN_SUPP_TIME_MIN) &&
+        (connsm->max_rx_time <= BLE_LL_CONN_SUPP_TIME_MIN)) {
+        return;
+    }
+
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD);
+}
+
+static void
+ble_ll_ctrl_update_features(struct ble_ll_conn_sm *connsm, uint8_t *feat)
+{
+    connsm->conn_features = feat[0];
+    memcpy(connsm->remote_features, feat + 1, 7);
+
+    /* If we received peer's features for the 1st time, we should try DLE */
+    if (!connsm->csmflags.cfbit.rxd_features) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+        /*
+         * If connection was established on uncoded PHY, by default we use
+         * MaxTxTime and MaxRxTime applicable for that PHY since we are not
+         * allowed to indicate longer supported time if peer does not support
+         * LE Coded PHY. However, once we know that peer does support it we can
+         * update those values to ones applicable for coded PHY.
+         */
+        if (connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8)) {
+            if (connsm->host_req_max_tx_time) {
+                connsm->max_tx_time = max(connsm->max_tx_time,
+                                          connsm->host_req_max_tx_time);
+            } else {
+                connsm->max_tx_time = g_ble_ll_conn_params.conn_init_max_tx_time_coded;
+            }
+            connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED;
+        }
+#endif
+
+        connsm->csmflags.cfbit.pending_initiate_dle = 1;
+        connsm->csmflags.cfbit.rxd_features = 1;
+    }
+}
+
+/**
+ * Called when we receive a feature request or a slave initiated feature
+ * request.
+ *
+ *
+ * @param connsm
+ * @param dptr
+ * @param rspbuf
+ * @param opcode
+ * @param new_features
+ *
+ * @return int
+ */
+static int
+ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                           uint8_t *rspbuf, uint8_t opcode)
+{
+    uint8_t rsp_opcode;
+    uint64_t our_feat;
+
+    /*
+     * Only accept slave feature requests if we are a master and feature
+     * requests if we are a slave.
+     */
+    if (opcode ==  BLE_LL_CTRL_SLAVE_FEATURE_REQ) {
+        if (connsm->conn_role != BLE_LL_CONN_ROLE_MASTER) {
+            return BLE_LL_CTRL_UNKNOWN_RSP;
+        }
+    } else {
+        /* XXX: not sure this is correct but do it anyway */
+        if (connsm->conn_role != BLE_LL_CONN_ROLE_SLAVE) {
+            return BLE_LL_CTRL_UNKNOWN_RSP;
+        }
+    }
+
+    our_feat = ble_ll_read_supp_features();
+
+    rsp_opcode = BLE_LL_CTRL_FEATURE_RSP;
+
+    ble_ll_ctrl_update_features(connsm, dptr);
+
+    /*
+     * 1st octet of features should be common features of local and remote
+     * controller - we call this 'connection features'
+     * remaining octets are features of controller which sends PDU, in this case
+     * it's our controller
+     *
+     * See: Vol 6, Part B, section 2.4.2.10
+     */
+    connsm->conn_features &= our_feat;
+
+    put_le64(rspbuf + 1, our_feat);
+    rspbuf[1] = connsm->conn_features;
+
+    return rsp_opcode;
+}
+
+/**
+ * Called when we receive a feature response
+ *
+ * @param connsm
+ * @param dptr
+ * @param new_features
+ *
+ */
+static void
+ble_ll_ctrl_rx_feature_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    ble_ll_ctrl_update_features(connsm, dptr);
+
+    /* Stop the control procedure */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) {
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG);
+    }
+
+    /* Send event to host if pending features read */
+    if (connsm->csmflags.cfbit.pending_hci_rd_features) {
+        ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS);
+        connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+    }
+}
+
+/**
+ *
+ *
+ * Context: Link Layer task
+ *
+ * @param connsm
+ * @param dptr
+ * @param rspbuf
+ *
+ * @return int
+ */
+static int
+ble_ll_ctrl_rx_conn_param_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                              uint8_t *rspbuf)
+{
+    uint8_t rsp_opcode;
+
+    /*
+     * This is not in the specification per se but it simplifies the
+     * implementation. If we get a connection parameter request and we
+     * are awaiting a reply from the host, simply ignore the request. This
+     * might not be a good idea if the parameters are different, but oh
+     * well. This is not expected to happen anyway. A return of BLE_ERR_MAX
+     * means that we will simply discard the connection parameter request
+     */
+    if (connsm->csmflags.cfbit.awaiting_host_reply) {
+        return BLE_ERR_MAX;
+    }
+
+    /* XXX: remember to deal with this on the master: if the slave has
+     * initiated a procedure we may have received its connection parameter
+     * update request and have signaled the host with an event. If that
+     * is the case, we will need to drop the host command when we get it
+       and also clear any applicable states. */
+
+    /* XXX: Read 5.3 again. There are multiple control procedures that might
+     * be pending (a connection update) that will cause collisions and the
+       behavior below. */
+    /*
+     * Check for procedure collision (Vol 6 PartB 5.3). If we are a slave
+     * and we receive a request we "consider the slave initiated
+     * procedure as complete". This means send a connection update complete
+     * event (with error).
+     *
+     * If a master, we send reject with a
+     * transaction collision error code.
+     */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) {
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
+            ble_ll_hci_ev_conn_update(connsm, BLE_ERR_LMP_COLLISION);
+        } else {
+            /* The master sends reject ind ext w/error code 0x23 */
+            rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+            rspbuf[1] = BLE_LL_CTRL_CONN_PARM_REQ;
+            rspbuf[2] = BLE_ERR_LMP_COLLISION;
+            return rsp_opcode;
+        }
+    }
+
+    /*
+     * If we are a master and we currently performing a channel map
+     * update procedure we need to return an error
+     */
+    if ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) &&
+        (connsm->csmflags.cfbit.chanmap_update_scheduled)) {
+        rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+        rspbuf[1] = BLE_LL_CTRL_CONN_PARM_REQ;
+        rspbuf[2] = BLE_ERR_DIFF_TRANS_COLL;
+        return rsp_opcode;
+    }
+
+    /* Process the received connection parameter request */
+    rsp_opcode = ble_ll_ctrl_conn_param_pdu_proc(connsm, dptr, rspbuf,
+                                                 BLE_LL_CTRL_CONN_PARM_REQ);
+    return rsp_opcode;
+}
+
+static int
+ble_ll_ctrl_rx_conn_param_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                              uint8_t *rspbuf)
+{
+    uint8_t rsp_opcode;
+
+    /* A slave should never receive this response */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+        return BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    /*
+     * This case should never happen! It means that the slave initiated a
+     * procedure and the master initiated one as well. If we do get in this
+     * state just clear the awaiting reply. The slave will hopefully stop its
+     * procedure when we reply.
+     */
+    if (connsm->csmflags.cfbit.awaiting_host_reply) {
+        connsm->csmflags.cfbit.awaiting_host_reply = 0;
+    }
+
+    /* If we receive a response and no procedure is pending, just leave */
+    if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) {
+        return BLE_ERR_MAX;
+    }
+
+    /* Process the received connection parameter response */
+    rsp_opcode = ble_ll_ctrl_conn_param_pdu_proc(connsm, dptr, rspbuf,
+                                                 BLE_LL_CTRL_CONN_PARM_RSP);
+    return rsp_opcode;
+}
+
+/**
+ * Called to process the LL control PDU VERSION_IND
+ *
+ * Context: Link Layer task
+ *
+ * @param connsm
+ * @param dptr
+ * @param rspbuf
+ *
+ * @return int
+ */
+static int
+ble_ll_ctrl_rx_version_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                           uint8_t *rspbuf)
+{
+    uint8_t rsp_opcode;
+
+    /* Process the packet */
+    connsm->vers_nr = dptr[0];
+    connsm->comp_id = get_le16(dptr + 1);
+    connsm->sub_vers_nr = get_le16(dptr + 3);
+    connsm->csmflags.cfbit.rxd_version_ind = 1;
+
+    rsp_opcode = BLE_ERR_MAX;
+    if (!connsm->csmflags.cfbit.version_ind_sent) {
+        rsp_opcode = BLE_LL_CTRL_VERSION_IND;
+        ble_ll_ctrl_version_ind_make(connsm, rspbuf);
+    }
+
+    /* Stop the control procedure */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG)) {
+        ble_ll_hci_ev_rd_rem_ver(connsm, BLE_ERR_SUCCESS);
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG);
+    }
+    return rsp_opcode;
+}
+
+/**
+ * Called to process a received channel map request control pdu.
+ *
+ * Context: Link Layer task
+ *
+ * @param connsm
+ * @param dptr
+ */
+static int
+ble_ll_ctrl_rx_chanmap_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    uint16_t instant;
+    uint16_t conn_events;
+
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        return BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+
+    /* If instant is in the past, we have to end the connection */
+    instant = get_le16(dptr + BLE_LL_CONN_CHMAP_LEN);
+    conn_events = (instant - connsm->event_cntr) & 0xFFFF;
+    if (conn_events >= 32767) {
+        ble_ll_conn_timeout(connsm, BLE_ERR_INSTANT_PASSED);
+    } else {
+        connsm->chanmap_instant = instant;
+        memcpy(connsm->req_chanmap, dptr, BLE_LL_CONN_CHMAP_LEN);
+        connsm->csmflags.cfbit.chanmap_update_scheduled = 1;
+    }
+
+    return BLE_ERR_MAX;
+}
+
+/**
+ * Initiate LL control procedure.
+ *
+ * This function is called to obtain a mbuf to send a LL control PDU. The data
+ * channel PDU header is not part of the mbuf data; it is part of the BLE
+ * header (which is part of the mbuf).
+ *
+ * Context: LL task.
+ *
+ * @param connsm
+ * @param ctrl_proc
+ */
+static struct os_mbuf *
+ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc)
+{
+    uint8_t len;
+    uint8_t opcode;
+    uint8_t *dptr;
+    uint8_t *ctrdata;
+    struct os_mbuf *om;
+
+    /* Get an mbuf for the control pdu */
+    om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, sizeof(struct ble_mbuf_hdr));
+
+    if (om) {
+        /* The control data starts after the opcode (1 byte) */
+        dptr = om->om_data;
+        ctrdata = dptr + 1;
+
+        switch (ctrl_proc) {
+        case BLE_LL_CTRL_PROC_CONN_UPDATE:
+            opcode = BLE_LL_CTRL_CONN_UPDATE_IND;
+            ble_ll_ctrl_conn_upd_make(connsm, ctrdata, NULL);
+            break;
+        case BLE_LL_CTRL_PROC_CHAN_MAP_UPD:
+            opcode = BLE_LL_CTRL_CHANNEL_MAP_REQ;
+            ble_ll_ctrl_chanmap_req_make(connsm, ctrdata);
+            break;
+        case BLE_LL_CTRL_PROC_FEATURE_XCHG:
+            if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+                opcode = BLE_LL_CTRL_FEATURE_REQ;
+            } else {
+                opcode = BLE_LL_CTRL_SLAVE_FEATURE_REQ;
+            }
+            put_le64(ctrdata, ble_ll_read_supp_features());
+            break;
+        case BLE_LL_CTRL_PROC_VERSION_XCHG:
+            opcode = BLE_LL_CTRL_VERSION_IND;
+            ble_ll_ctrl_version_ind_make(connsm, ctrdata);
+            break;
+        case BLE_LL_CTRL_PROC_TERMINATE:
+            opcode = BLE_LL_CTRL_TERMINATE_IND;
+            ctrdata[0] = connsm->disconnect_reason;
+            break;
+        case BLE_LL_CTRL_PROC_CONN_PARAM_REQ:
+            opcode = BLE_LL_CTRL_CONN_PARM_REQ;
+            ble_ll_ctrl_conn_param_pdu_make(connsm, ctrdata, NULL);
+            break;
+        case BLE_LL_CTRL_PROC_LE_PING:
+            opcode = BLE_LL_CTRL_PING_REQ;
+            break;
+        case BLE_LL_CTRL_PROC_DATA_LEN_UPD:
+            opcode = BLE_LL_CTRL_LENGTH_REQ;
+            ble_ll_ctrl_datalen_upd_make(connsm, dptr);
+            break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        /* XXX: deal with already encrypted connection.*/
+        case BLE_LL_CTRL_PROC_ENCRYPT:
+            /* If we are already encrypted we do pause procedure */
+            if (connsm->enc_data.enc_state == CONN_ENC_S_ENCRYPTED) {
+                opcode = BLE_LL_CTRL_PAUSE_ENC_REQ;
+            } else {
+                opcode = BLE_LL_CTRL_ENC_REQ;
+                ble_ll_ctrl_enc_req_make(connsm, ctrdata);
+            }
+            break;
+#endif
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        case BLE_LL_CTRL_PROC_PHY_UPDATE:
+            opcode = BLE_LL_CTRL_PHY_REQ;
+            ble_ll_ctrl_phy_req_rsp_make(connsm, ctrdata);
+            break;
+#endif
+        default:
+            BLE_LL_ASSERT(0);
+            break;
+        }
+
+        /* Set llid, length and opcode */
+        dptr[0] = opcode;
+        len = g_ble_ll_ctrl_pkt_lengths[opcode] + 1;
+
+        /* Add packet to transmit queue of connection */
+        ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len);
+    }
+
+    return om;
+}
+
+/**
+ * Called to determine if the pdu is a TERMINATE_IND
+ *
+ * @param hdr
+ * @param opcode
+ *
+ * @return int
+ */
+int
+ble_ll_ctrl_is_terminate_ind(uint8_t hdr, uint8_t opcode)
+{
+    int rc;
+
+    rc = 0;
+    if ((hdr & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) {
+        if (opcode == BLE_LL_CTRL_TERMINATE_IND) {
+            rc = 1;
+        }
+    }
+    return rc;
+}
+
+/**
+ * Stops the LL control procedure indicated by 'ctrl_proc'.
+ *
+ * Context: Link Layer task
+ *
+ * @param connsm
+ * @param ctrl_proc
+ */
+void
+ble_ll_ctrl_proc_stop(struct ble_ll_conn_sm *connsm, int ctrl_proc)
+{
+    if (connsm->cur_ctrl_proc == ctrl_proc) {
+        ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer);
+        connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE;
+    }
+    CLR_PENDING_CTRL_PROC(connsm, ctrl_proc);
+
+    /* If there are others, start them */
+    ble_ll_ctrl_chk_proc_start(connsm);
+}
+
+/**
+ * Called to start the terminate procedure.
+ *
+ * Context: Link Layer task.
+ *
+ * @param connsm
+ */
+void
+ble_ll_ctrl_terminate_start(struct ble_ll_conn_sm *connsm)
+{
+    int ctrl_proc;
+    uint32_t usecs;
+    struct os_mbuf *om;
+
+    BLE_LL_ASSERT(connsm->disconnect_reason != 0);
+
+    ctrl_proc = BLE_LL_CTRL_PROC_TERMINATE;
+    om = ble_ll_ctrl_proc_init(connsm, ctrl_proc);
+    if (om) {
+        CONN_F_TERMINATE_STARTED(connsm) = 1;
+
+        /* Set terminate "timeout" */
+        usecs = connsm->supervision_tmo * BLE_HCI_CONN_SPVN_TMO_UNITS * 1000;
+        connsm->terminate_timeout = os_cputime_get32() +
+            os_cputime_usecs_to_ticks(usecs);
+    }
+}
+
+/**
+ * Called to start a LL control procedure except for the terminate procedure. We
+ * always set the control procedure pending bit even if the control procedure
+ * has been initiated.
+ *
+ * Context: Link Layer task.
+ *
+ * @param connsm Pointer to connection state machine.
+ */
+void
+ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc)
+{
+    struct os_mbuf *om;
+
+    BLE_LL_ASSERT(ctrl_proc != BLE_LL_CTRL_PROC_TERMINATE);
+
+    om = NULL;
+    if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) {
+        /* Initiate the control procedure. */
+        om = ble_ll_ctrl_proc_init(connsm, ctrl_proc);
+        if (om) {
+            /* Set the current control procedure */
+            connsm->cur_ctrl_proc = ctrl_proc;
+
+            /* Initialize the procedure response timeout */
+            if (ctrl_proc != BLE_LL_CTRL_PROC_CHAN_MAP_UPD) {
+                ble_ll_ctrl_start_rsp_timer(connsm);
+            }
+        }
+    }
+
+    /* Set bitmask denoting control procedure is pending */
+    connsm->pending_ctrl_procs |= (1 << ctrl_proc);
+}
+
+/**
+ * Called to determine if we need to start a LL control procedure for the given
+ * connection.
+ *
+ * Context: Link Layer
+ *
+ * @param connsm Pointer to connection state machine.
+ */
+void
+ble_ll_ctrl_chk_proc_start(struct ble_ll_conn_sm *connsm)
+{
+    int i;
+
+    /* XXX: TODO new rules! Cannot start certain control procedures if other
+     * ones are peer initiated. We need to wait. Deal with this.
+     */
+
+    /*
+     * If we are terminating, dont start any new procedures but start
+     * terminate if needed
+     */
+    if (connsm->disconnect_reason) {
+        if (!CONN_F_TERMINATE_STARTED(connsm)) {
+            /*
+             * If the terminate procedure has not started it means we were not
+             * able to start it right away (no control pdu was available).
+             * Start it now. No need to start any other procedures.
+             */
+            ble_ll_ctrl_terminate_start(connsm);
+        }
+        return;
+    }
+
+    /* If there is a running procedure or no pending, do nothing */
+    if ((connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) &&
+        (connsm->pending_ctrl_procs != 0)) {
+        /*
+         * The specification says there is no priority to control procedures
+         * so just start from the first one for now.
+         */
+        for (i = 0; i < BLE_LL_CTRL_PROC_NUM; ++i) {
+            if (IS_PENDING_CTRL_PROC(connsm, i)) {
+                /*
+                 * The version exchange is a special case. If we have already
+                 * received the information dont start it.
+                 */
+                if ((i == BLE_LL_CTRL_PROC_VERSION_XCHG) &&
+                    (connsm->csmflags.cfbit.rxd_version_ind)) {
+                    ble_ll_hci_ev_rd_rem_ver(connsm, BLE_ERR_SUCCESS);
+                    CLR_PENDING_CTRL_PROC(connsm, i);
+                } else {
+                    ble_ll_ctrl_proc_start(connsm, i);
+                    break;
+                }
+            }
+        }
+    }
+}
+
+/**
+ * Called when the Link Layer receives a LL control PDU.
+ *
+ * NOTE: this function uses the received PDU for the response in some cases. If
+ * the received PDU is not used it needs to be freed here.
+ *
+ * XXX: may want to check, for both master and slave, whether the control
+ * pdu should be received by that role. Might make for less code...
+ * Context: Link Layer
+ *
+ * @param om
+ * @param connsm
+ */
+int
+ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
+{
+    uint32_t features;
+    uint32_t feature;
+    uint8_t len;
+    uint8_t opcode;
+    uint8_t rsp_opcode;
+    uint8_t *dptr;
+    uint8_t *rspbuf;
+    uint8_t *rspdata;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    int restart_encryption;
+#endif
+    int rc = 0;
+
+    /* XXX: where do we validate length received and packet header length?
+     * do this in LL task when received. Someplace!!! What I mean
+     * is we should validate the over the air length with the mbuf length.
+       Should the PHY do that???? */
+
+    /*
+     * dptr points to om_data pointer. The first byte of om_data is the
+     * first byte of the Data Channel PDU header. Get length from header and
+     * opcode from LL control PDU.
+     */
+    dptr = om->om_data;
+    len = dptr[1];
+    opcode = dptr[2];
+
+    /*
+     * rspbuf points to first byte of response. The response buffer does not
+     * contain the Data Channel PDU. Thus, the first byte of rspbuf is the
+     * LL control PDU payload (the opcode of the control PDU). rspdata
+     * points to CtrData in the control PDU.
+     */
+    rspbuf = dptr;
+    rspdata = rspbuf + 1;
+
+    /* Move data pointer to start of control data (2 byte PDU hdr + opcode) */
+    dptr += (BLE_LL_PDU_HDR_LEN + 1);
+
+    /*
+     * Subtract the opcode from the length. Note that if the length was zero,
+     * which would be an error, we will fail the check against the length
+     * of the control packet.
+     */
+    --len;
+
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CTRL_RX, opcode, len);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    restart_encryption = 0;
+#endif
+
+    /* If opcode comes from reserved value or CtrlData fields is invalid
+     * we shall respond with LL_UNKNOWN_RSP
+     */
+    if ((opcode >= BLE_LL_CTRL_OPCODES) ||
+        (len != g_ble_ll_ctrl_pkt_lengths[opcode])) {
+        rc = -1;
+        rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+        goto ll_ctrl_send_rsp;
+    }
+
+    /* Check if the feature is supported. */
+    switch (opcode) {
+    case BLE_LL_CTRL_LENGTH_REQ:
+        feature = BLE_LL_FEAT_DATA_LEN_EXT;
+        break;
+    case BLE_LL_CTRL_SLAVE_FEATURE_REQ:
+        feature = BLE_LL_FEAT_SLAVE_INIT;
+        break;
+    case BLE_LL_CTRL_CONN_PARM_REQ:
+    case BLE_LL_CTRL_CONN_PARM_RSP:
+        feature = BLE_LL_FEAT_CONN_PARM_REQ;
+        break;
+    case BLE_LL_CTRL_ENC_REQ:
+    case BLE_LL_CTRL_START_ENC_REQ:
+    case BLE_LL_CTRL_PAUSE_ENC_REQ:
+        feature = BLE_LL_FEAT_LE_ENCRYPTION;
+        break;
+    case BLE_LL_CTRL_PING_REQ:
+        feature = BLE_LL_FEAT_LE_PING;
+        break;
+    case BLE_LL_CTRL_PHY_REQ:
+        feature = BLE_LL_FEAT_LE_2M_PHY | BLE_LL_FEAT_LE_CODED_PHY;
+        break;
+    case BLE_LL_CTRL_MIN_USED_CHAN_IND:
+        feature = BLE_LL_FEAT_MIN_USED_CHAN;
+        break;
+    case BLE_LL_CTRL_PERIODIC_SYNC_IND:
+        feature = BLE_LL_FEAT_SYNC_TRANS_RECV;
+        break;
+    default:
+        feature = 0;
+        break;
+    }
+
+    if (feature) {
+        features = ble_ll_read_supp_features();
+        if ((features & feature) == 0) {
+            if (opcode == BLE_LL_CTRL_ENC_REQ) {
+                if (connsm->conn_features & BLE_LL_FEAT_EXTENDED_REJ) {
+                    rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+                    rspbuf[1] = opcode;
+                    rspbuf[2] = BLE_ERR_UNSUPP_REM_FEATURE;
+
+                } else {
+                    rsp_opcode = BLE_LL_CTRL_REJECT_IND;
+                    rspbuf[1] = BLE_ERR_UNSUPP_REM_FEATURE;
+                }
+            } else {
+                /* Construct unknown rsp pdu */
+                rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+            }
+            goto ll_ctrl_send_rsp;
+        }
+    }
+
+    /* Process opcode */
+    rsp_opcode = BLE_ERR_MAX;
+    switch (opcode) {
+    case BLE_LL_CTRL_CONN_UPDATE_IND:
+        rsp_opcode = ble_ll_ctrl_rx_conn_update(connsm, dptr);
+        break;
+    case BLE_LL_CTRL_CHANNEL_MAP_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_chanmap_req(connsm, dptr);
+        break;
+    case BLE_LL_CTRL_LENGTH_REQ:
+        /* Extract parameters and check if valid */
+        if (ble_ll_ctrl_len_proc(connsm, dptr)) {
+            rc  = -1;
+            rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+            goto ll_ctrl_send_rsp;
+        }
+
+        /*
+         * If we have not started this procedure ourselves and it is
+         * pending, no need to perform it.
+         */
+        if ((connsm->cur_ctrl_proc != BLE_LL_CTRL_PROC_DATA_LEN_UPD) &&
+            IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD)) {
+            CLR_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD);
+        }
+
+        /* Send a response */
+        rsp_opcode = BLE_LL_CTRL_LENGTH_RSP;
+        ble_ll_ctrl_datalen_upd_make(connsm, rspbuf);
+        break;
+    case BLE_LL_CTRL_LENGTH_RSP:
+        /* According to specification, process this only if we asked for it. */
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_DATA_LEN_UPD) {
+            /*
+             * Process the received data. If received data is invalid, we'll
+             * reply with LL_UNKNOWN_RSP as per spec, but we still need to stop
+             * control procedure to avoid timeout.
+             */
+            if (ble_ll_ctrl_len_proc(connsm, dptr)) {
+                rc = -1;
+                rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+            }
+
+            /* Stop the control procedure */
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD);
+        }
+        break;
+    case BLE_LL_CTRL_UNKNOWN_RSP:
+        rsp_opcode = ble_ll_ctrl_proc_unk_rsp(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_FEATURE_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode);
+        break;
+    /* XXX: check to see if ctrl procedure was running? Do we care? */
+    case BLE_LL_CTRL_FEATURE_RSP:
+        ble_ll_ctrl_rx_feature_rsp(connsm, dptr);
+        break;
+    case BLE_LL_CTRL_VERSION_IND:
+        rsp_opcode = ble_ll_ctrl_rx_version_ind(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_SLAVE_FEATURE_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode);
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    case BLE_LL_CTRL_ENC_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_enc_req(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_ENC_RSP:
+        ble_ll_ctrl_rx_enc_rsp(connsm, dptr);
+        break;
+    case BLE_LL_CTRL_START_ENC_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_start_enc_req(connsm);
+        break;
+    case BLE_LL_CTRL_START_ENC_RSP:
+        rsp_opcode = ble_ll_ctrl_rx_start_enc_rsp(connsm);
+        break;
+    case BLE_LL_CTRL_PAUSE_ENC_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_pause_enc_req(connsm);
+        break;
+    case BLE_LL_CTRL_PAUSE_ENC_RSP:
+        rsp_opcode = ble_ll_ctrl_rx_pause_enc_rsp(connsm);
+        if (rsp_opcode == BLE_LL_CTRL_PAUSE_ENC_RSP) {
+            restart_encryption = 1;
+        }
+        break;
+#endif
+    case BLE_LL_CTRL_PING_REQ:
+        rsp_opcode = BLE_LL_CTRL_PING_RSP;
+        break;
+    case BLE_LL_CTRL_PING_RSP:
+        ble_ll_ctrl_rx_ping_rsp(connsm);
+        break;
+    case BLE_LL_CTRL_CONN_PARM_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_conn_param_req(connsm, dptr, rspbuf);
+        break;
+    case BLE_LL_CTRL_CONN_PARM_RSP:
+        rsp_opcode = ble_ll_ctrl_rx_conn_param_rsp(connsm, dptr, rspbuf);
+        break;
+    /* Fall-through intentional... */
+    case BLE_LL_CTRL_REJECT_IND:
+    case BLE_LL_CTRL_REJECT_IND_EXT:
+        /* Sometimes reject triggers sending other LL CTRL msg */
+        rsp_opcode = ble_ll_ctrl_rx_reject_ind(connsm, dptr, opcode, rspdata);
+        break;
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    case BLE_LL_CTRL_PHY_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_phy_req(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_PHY_RSP:
+        rsp_opcode = ble_ll_ctrl_rx_phy_rsp(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_PHY_UPDATE_IND:
+        rsp_opcode = ble_ll_ctrl_rx_phy_update_ind(connsm, dptr);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    case BLE_LL_CTRL_PERIODIC_SYNC_IND:
+        rsp_opcode = ble_ll_ctrl_rx_periodic_sync_ind(connsm, dptr);
+        break;
+#endif
+    default:
+        /* Nothing to do here */
+        break;
+    }
+
+    /* Free mbuf or send response */
+ll_ctrl_send_rsp:
+    if (rsp_opcode == BLE_ERR_MAX) {
+        os_mbuf_free_chain(om);
+    } else {
+        /*
+         * Write the response opcode into the buffer. If this is an unknown
+         * response, put opcode of unknown pdu into buffer.
+         */
+        rspbuf[0] = rsp_opcode;
+        if (rsp_opcode == BLE_LL_CTRL_UNKNOWN_RSP) {
+            rspbuf[1] = opcode;
+        }
+        len = g_ble_ll_ctrl_pkt_lengths[rsp_opcode] + 1;
+        ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (restart_encryption) {
+            /* XXX: what happens if this fails? Meaning we cant allocate
+               mbuf? */
+            ble_ll_ctrl_proc_init(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+        }
+#endif
+    }
+
+    if (connsm->csmflags.cfbit.pending_initiate_dle) {
+        connsm->csmflags.cfbit.pending_initiate_dle = 0;
+        ble_ll_ctrl_initiate_dle(connsm);
+    }
+
+    return rc;
+}
+
+/**
+ * Called to create and send a REJECT_IND_EXT control PDU or a REJECT_IND
+ *
+ * @param connsm
+ * @param rej_opcode
+ * @param err
+ *
+ * @return int
+ */
+int
+ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm, uint8_t rej_opcode,
+                            uint8_t err)
+{
+    int rc;
+    uint8_t len;
+    uint8_t opcode;
+    uint8_t *rspbuf;
+    struct os_mbuf *om;
+
+    om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+                            sizeof(struct ble_mbuf_hdr));
+    if (om) {
+        rspbuf = om->om_data;
+        opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+        if (rej_opcode == BLE_LL_CTRL_ENC_REQ) {
+            if ((connsm->conn_features & BLE_LL_FEAT_EXTENDED_REJ) == 0) {
+                opcode = BLE_LL_CTRL_REJECT_IND;
+            }
+        }
+        rspbuf[0] = opcode;
+        if (opcode == BLE_LL_CTRL_REJECT_IND) {
+            rspbuf[1] = err;
+            len = BLE_LL_CTRL_REJ_IND_LEN + 1;
+        } else {
+            rspbuf[1] = rej_opcode;
+            rspbuf[2] = err;
+            len = BLE_LL_CTRL_REJECT_IND_EXT_LEN + 1;
+        }
+        ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len);
+        rc = 0;
+    } else {
+        rc = 1;
+    }
+    return rc;
+}
+
+/**
+ * Called when a Link Layer Control pdu has been transmitted successfully.
+ * This is called when we have a received a PDU during the ISR.
+ *
+ * Context: ISR
+ *
+ * @param txpdu
+ *
+ * @return int
+ */
+int
+ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+    uint8_t opcode;
+
+    rc = 0;
+    opcode = txpdu->om_data[0];
+    switch (opcode) {
+    case BLE_LL_CTRL_TERMINATE_IND:
+        connsm->csmflags.cfbit.terminate_ind_txd = 1;
+        rc = -1;
+        break;
+    case BLE_LL_CTRL_REJECT_IND_EXT:
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) {
+            /* If rejecting opcode is BLE_LL_CTRL_PROC_CONN_PARAM_REQ and
+             * reason is LMP collision that means we are master on the link and
+             * peer wanted to start procedure which we already started.
+             * Let's wait for response and do not close procedure. */
+            if (txpdu->om_data[1] == BLE_LL_CTRL_CONN_PARM_REQ &&
+                            txpdu->om_data[2] != BLE_ERR_LMP_COLLISION) {
+                connsm->reject_reason = txpdu->om_data[2];
+                connsm->csmflags.cfbit.host_expects_upd_event = 1;
+            }
+        }
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+            connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+        }
+#endif
+        break;
+    case BLE_LL_CTRL_REJECT_IND:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+#endif
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    case BLE_LL_CTRL_PAUSE_ENC_REQ:
+        /* note: fall-through intentional */
+    case BLE_LL_CTRL_ENC_REQ:
+        connsm->enc_data.enc_state = CONN_ENC_S_ENC_RSP_WAIT;
+        break;
+    case BLE_LL_CTRL_ENC_RSP:
+        connsm->csmflags.cfbit.send_ltk_req = 1;
+        break;
+    case BLE_LL_CTRL_START_ENC_RSP:
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED;
+            if (CONN_F_LE_PING_SUPP(connsm)) {
+                ble_ll_conn_auth_pyld_timer_start(connsm);
+            }
+        }
+        break;
+    case BLE_LL_CTRL_PAUSE_ENC_RSP:
+        if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+            connsm->enc_data.enc_state = CONN_ENC_S_PAUSE_ENC_RSP_WAIT;
+        }
+        break;
+#endif
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    case BLE_LL_CTRL_PHY_REQ:
+        connsm->phy_tx_transition =
+                    ble_ll_ctrl_phy_tx_transition_get(connsm->phy_data.req_pref_tx_phys_mask);
+        break;
+    case BLE_LL_CTRL_PHY_UPDATE_IND:
+        connsm->phy_tx_transition =
+                    ble_ll_ctrl_phy_tx_transition_get(txpdu->om_data[2]);
+        break;
+#endif
+    default:
+        break;
+    }
+
+    os_mbuf_free_chain(txpdu);
+    return rc;
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm.c
new file mode 100644
index 00000000..de3b168b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm.c
@@ -0,0 +1,726 @@
+/*
+ * 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 "sysinit/sysinit.h"
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+
+#include <assert.h>
+#include "os/os.h"
+#include "stats/stats.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "ble_ll_dtm_priv.h"
+
+STATS_SECT_START(ble_ll_dtm_stats)
+    STATS_SECT_ENTRY(rx_count)
+    STATS_SECT_ENTRY(tx_failed)
+    STATS_SECT_ENTRY(rx_failed)
+STATS_SECT_END
+STATS_SECT_DECL(ble_ll_dtm_stats) ble_ll_dtm_stats;
+
+STATS_NAME_START(ble_ll_dtm_stats)
+    STATS_NAME(ble_ll_dtm_stats, rx_count)
+    STATS_NAME(ble_ll_dtm_stats, tx_failed)
+    STATS_NAME(ble_ll_dtm_stats, rx_failed)
+STATS_NAME_END(ble_phy_stats)
+
+struct dtm_ctx {
+    uint8_t payload_packet;
+    uint8_t itvl_rem_usec;
+    uint16_t num_of_packets;
+    uint32_t itvl_ticks;
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    uint16_t num_of_packets_max;
+#endif
+    int active;
+    uint8_t rf_channel;
+    uint8_t phy_mode;
+    struct os_mbuf *om;
+    struct ble_npl_event evt;
+    struct ble_ll_sched_item sch;
+    uint32_t pdu_start_ticks;
+    uint8_t pdu_start_usecs;
+};
+
+static struct dtm_ctx g_ble_ll_dtm_ctx;
+
+static const uint8_t g_ble_ll_dtm_prbs9_data[] =
+{
+    0xff, 0xc1, 0xfb, 0xe8, 0x4c, 0x90, 0x72, 0x8b,
+    0xe7, 0xb3, 0x51, 0x89, 0x63, 0xab, 0x23, 0x23,
+    0x02, 0x84, 0x18, 0x72, 0xaa, 0x61, 0x2f, 0x3b,
+    0x51, 0xa8, 0xe5, 0x37, 0x49, 0xfb, 0xc9, 0xca,
+    0x0c, 0x18, 0x53, 0x2c, 0xfd, 0x45, 0xe3, 0x9a,
+    0xe6, 0xf1, 0x5d, 0xb0, 0xb6, 0x1b, 0xb4, 0xbe,
+    0x2a, 0x50, 0xea, 0xe9, 0x0e, 0x9c, 0x4b, 0x5e,
+    0x57, 0x24, 0xcc, 0xa1, 0xb7, 0x59, 0xb8, 0x87,
+    0xff, 0xe0, 0x7d, 0x74, 0x26, 0x48, 0xb9, 0xc5,
+    0xf3, 0xd9, 0xa8, 0xc4, 0xb1, 0xd5, 0x91, 0x11,
+    0x01, 0x42, 0x0c, 0x39, 0xd5, 0xb0, 0x97, 0x9d,
+    0x28, 0xd4, 0xf2, 0x9b, 0xa4, 0xfd, 0x64, 0x65,
+    0x06, 0x8c, 0x29, 0x96, 0xfe, 0xa2, 0x71, 0x4d,
+    0xf3, 0xf8, 0x2e, 0x58, 0xdb, 0x0d, 0x5a, 0x5f,
+    0x15, 0x28, 0xf5, 0x74, 0x07, 0xce, 0x25, 0xaf,
+    0x2b, 0x12, 0xe6, 0xd0, 0xdb, 0x2c, 0xdc, 0xc3,
+    0x7f, 0xf0, 0x3e, 0x3a, 0x13, 0xa4, 0xdc, 0xe2,
+    0xf9, 0x6c, 0x54, 0xe2, 0xd8, 0xea, 0xc8, 0x88,
+    0x00, 0x21, 0x86, 0x9c, 0x6a, 0xd8, 0xcb, 0x4e,
+    0x14, 0x6a, 0xf9, 0x4d, 0xd2, 0x7e, 0xb2, 0x32,
+    0x03, 0xc6, 0x14, 0x4b, 0x7f, 0xd1, 0xb8, 0xa6,
+    0x79, 0x7c, 0x17, 0xac, 0xed, 0x06, 0xad, 0xaf,
+    0x0a, 0x94, 0x7a, 0xba, 0x03, 0xe7, 0x92, 0xd7,
+    0x15, 0x09, 0x73, 0xe8, 0x6d, 0x16, 0xee, 0xe1,
+    0x3f, 0x78, 0x1f, 0x9d, 0x09, 0x52, 0x6e, 0xf1,
+    0x7c, 0x36, 0x2a, 0x71, 0x6c, 0x75, 0x64, 0x44,
+    0x80, 0x10, 0x43, 0x4e, 0x35, 0xec, 0x65, 0x27,
+    0x0a, 0xb5, 0xfc, 0x26, 0x69, 0x3f, 0x59, 0x99,
+    0x01, 0x63, 0x8a, 0xa5, 0xbf, 0x68, 0x5c, 0xd3,
+    0x3c, 0xbe, 0x0b, 0xd6, 0x76, 0x83, 0xd6, 0x57,
+    0x05, 0x4a, 0x3d, 0xdd, 0x81, 0x73, 0xc9, 0xeb,
+    0x8a, 0x84, 0x39, 0xf4, 0x36, 0x0b, 0xf7
+};
+
+static const uint8_t g_ble_ll_dtm_prbs15_data[] =
+{
+    0xff, 0x7f, 0xf0, 0x3e, 0x3a, 0x13, 0xa4, 0xdc,
+    0xe2, 0xf9, 0x6c, 0x54, 0xe2, 0xd8, 0xea, 0xc8,
+    0x88, 0x00, 0x21, 0x86, 0x9c, 0x6a, 0xd8, 0xcb,
+    0x4e, 0x14, 0x6a, 0xf9, 0x4d, 0xd2, 0x7e, 0xb2,
+    0x32, 0x03, 0xc6, 0x14, 0x4b, 0x7f, 0xd1, 0xb8,
+    0xa6, 0x79, 0x7c, 0x17, 0xac, 0xed, 0x06, 0xad,
+    0xaf, 0x0a, 0x94, 0x7a, 0xba, 0x03, 0xe7, 0x92,
+    0xd7, 0x15, 0x09, 0x73, 0xe8, 0x6d, 0x16, 0xee,
+    0xe1, 0x3f, 0x78, 0x1f, 0x9d, 0x09, 0x52, 0x6e,
+    0xf1, 0x7c, 0x36, 0x2a, 0x71, 0x6c, 0x75, 0x64,
+    0x44, 0x80, 0x10, 0x43, 0x4e, 0x35, 0xec, 0x65,
+    0x27, 0x0a, 0xb5, 0xfc, 0x26, 0x69, 0x3f, 0x59,
+    0x99, 0x01, 0x63, 0x8a, 0xa5, 0xbf, 0x68, 0x5c,
+    0xd3, 0x3c, 0xbe, 0x0b, 0xd6, 0x76, 0x83, 0xd6,
+    0x57, 0x05, 0x4a, 0x3d, 0xdd, 0x81, 0x73, 0xc9,
+    0xeb, 0x8a, 0x84, 0x39, 0xf4, 0x36, 0x0b, 0xf7,
+    0xf0, 0x1f, 0xbc, 0x8f, 0xce, 0x04, 0x29, 0xb7,
+    0x78, 0x3e, 0x1b, 0x95, 0x38, 0xb6, 0x3a, 0x32,
+    0x22, 0x40, 0x88, 0x21, 0xa7, 0x1a, 0xf6, 0xb2,
+    0x13, 0x85, 0x5a, 0x7e, 0x93, 0xb4, 0x9f, 0xac,
+    0xcc, 0x80, 0x31, 0xc5, 0xd2, 0x5f, 0x34, 0xae,
+    0x69, 0x1e, 0xdf, 0x05, 0x6b, 0xbb, 0x41, 0xeb,
+    0xab, 0x02, 0xa5, 0x9e, 0xee, 0xc0, 0xb9, 0xe4,
+    0x75, 0x45, 0xc2, 0x1c, 0x7a, 0x9b, 0x85, 0x7b,
+    0xf8, 0x0f, 0xde, 0x47, 0x67, 0x82, 0x94, 0x5b,
+    0x3c, 0x9f, 0x8d, 0x4a, 0x1c, 0x5b, 0x1d, 0x19,
+    0x11, 0x20, 0xc4, 0x90, 0x53, 0x0d, 0x7b, 0xd9,
+    0x89, 0x42, 0x2d, 0xbf, 0x49, 0xda, 0x4f, 0x56,
+    0x66, 0xc0, 0x98, 0x62, 0xe9, 0x2f, 0x1a, 0xd7,
+    0x34, 0x8f, 0xef, 0x82, 0xb5, 0xdd, 0xa0, 0xf5,
+    0x55, 0x81, 0x52, 0x4f, 0x77, 0xe0, 0x5c, 0xf2,
+    0xba, 0x22, 0x61, 0x0e, 0xbd, 0xcd, 0xc2
+};
+
+static const uint8_t channel_rf_to_index[] = {
+        37,  0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 38, 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, 39
+};
+
+#define BLE_DTM_SYNC_WORD          (0x71764129)
+#define BLE_DTM_CRC                (0x555555)
+
+static void ble_ll_dtm_ctx_free(struct dtm_ctx * ctx);
+
+static void
+ble_ll_dtm_set_next(struct dtm_ctx *ctx)
+{
+    struct ble_ll_sched_item *sch = &ctx->sch;
+
+    ctx->pdu_start_ticks += ctx->itvl_ticks;
+    ctx->pdu_start_usecs += ctx->itvl_rem_usec;
+    if (ctx->pdu_start_usecs >= 31) {
+       ctx->pdu_start_ticks++;
+       ctx->pdu_start_usecs -= 31;
+    }
+
+    sch->start_time = ctx->pdu_start_ticks;
+    sch->remainder = ctx->pdu_start_usecs;
+
+    sch->start_time -= g_ble_ll_sched_offset_ticks;
+}
+
+static void
+ble_ll_dtm_ev_tx_resched_cb(struct ble_npl_event *evt) {
+    /* It is called in LL context */
+    struct dtm_ctx *ctx = ble_npl_event_get_arg(evt);
+    int rc;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    if (!ctx->active || !ctx->om) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+    OS_EXIT_CRITICAL(sr);
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (g_ble_ll_dtm_ctx.num_of_packets_max &&
+        (g_ble_ll_dtm_ctx.num_of_packets == g_ble_ll_dtm_ctx.num_of_packets_max)) {
+        /*
+         * XXX do not send more packets, but also do not stop DTM - it shall be
+         * stopped as usual by HCI command since there is no standard way to
+         * signal end of test to host.
+         */
+        return;
+    }
+#endif
+
+    ble_ll_dtm_set_next(ctx);
+    rc = ble_ll_sched_dtm(&ctx->sch);
+    BLE_LL_ASSERT(rc == 0);
+}
+
+static int ble_ll_dtm_rx_start(void);
+
+static void
+ble_ll_dtm_ev_rx_restart_cb(struct ble_npl_event *evt) {
+    if (ble_ll_dtm_rx_start() != 0) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt);
+        STATS_INC(ble_ll_dtm_stats, rx_failed);
+    }
+}
+
+static void
+ble_ll_dtm_tx_done(void *arg)
+{
+    struct dtm_ctx *ctx;
+
+    ctx = arg;
+    if (!ctx->active) {
+        return;
+    }
+
+    g_ble_ll_dtm_ctx.num_of_packets++;
+
+    /* Reschedule event in LL context */
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &ctx->evt);
+
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+}
+
+static int
+ble_ll_dtm_tx_sched_cb(struct ble_ll_sched_item *sch)
+{
+    struct dtm_ctx *ctx = sch->cb_arg;
+    int rc;
+
+    if (!ctx->active) {
+        return BLE_LL_SCHED_STATE_DONE;
+    }
+
+    rc = ble_phy_setchan(channel_rf_to_index[ctx->rf_channel],
+                                            BLE_DTM_SYNC_WORD, BLE_DTM_CRC);
+    if (rc != 0) {
+        BLE_LL_ASSERT(0);
+        return BLE_LL_SCHED_STATE_DONE;
+    }
+
+#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY))
+    ble_phy_mode_set(ctx->phy_mode, ctx->phy_mode);
+#endif
+    ble_phy_set_txend_cb(ble_ll_dtm_tx_done, ctx);
+    ble_phy_txpwr_set(0);
+
+    sch->start_time += g_ble_ll_sched_offset_ticks;
+
+    rc = ble_phy_tx_set_start_time(sch->start_time, sch->remainder);
+    if (rc) {
+        goto resched;
+    }
+
+    rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, ctx->om, BLE_PHY_TRANSITION_NONE);
+    if (rc) {
+        goto resched;
+    }
+
+    ble_ll_state_set(BLE_LL_STATE_DTM);
+
+    return BLE_LL_SCHED_STATE_DONE;
+
+resched:
+    /* Reschedule from LL task if late for this PDU */
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &ctx->evt);
+
+    STATS_INC(ble_ll_dtm_stats, tx_failed);
+
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static void
+ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len,
+                          uint16_t cmd_interval, int phy_mode)
+{
+    uint32_t l;
+    uint32_t itvl_usec;
+    uint32_t itvl_ticks;
+
+    /* Calculate interval as per spec Bluetooth 5.0 Vol 6. Part F, 4.1.6 */
+    l = ble_ll_pdu_tx_time_get(len + BLE_LL_PDU_HDR_LEN, phy_mode);
+    itvl_usec = ((l + 249 + 624) / 625) * 625;
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (cmd_interval > itvl_usec) {
+        itvl_usec = cmd_interval;
+    }
+#endif
+
+    itvl_ticks = os_cputime_usecs_to_ticks(itvl_usec);
+    ctx->itvl_rem_usec = (itvl_usec - os_cputime_ticks_to_usecs(itvl_ticks));
+    if (ctx->itvl_rem_usec == 31) {
+        ctx->itvl_rem_usec = 0;
+        ++itvl_ticks;
+    }
+    ctx->itvl_ticks = itvl_ticks;
+}
+
+static int
+ble_ll_dtm_tx_create_ctx(uint8_t packet_payload, uint8_t len,
+                         uint8_t rf_channel, uint8_t phy_mode,
+                         uint16_t cmd_interval, uint16_t cmd_pkt_count)
+{
+    int rc = 0;
+    uint8_t byte_pattern;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf *m;
+    struct dtm_ctx *ctx = &g_ble_ll_dtm_ctx;
+    struct ble_ll_sched_item *sch = &ctx->sch;
+
+    /* MSYS is big enough to get continues memory */
+    m = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
+    ctx->om = m;
+    BLE_LL_ASSERT(g_ble_ll_dtm_ctx.om);
+
+    ctx->phy_mode = phy_mode;
+    ctx->rf_channel = rf_channel;
+    ctx->num_of_packets = 0;
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    ctx->num_of_packets_max = cmd_pkt_count;
+#endif
+
+    /* Set BLE transmit header */
+    ble_hdr = BLE_MBUF_HDR_PTR(m);
+    ble_hdr->txinfo.flags = 0;
+    ble_hdr->txinfo.offset = 0;
+    ble_hdr->txinfo.pyld_len = len;
+    ble_hdr->txinfo.hdr_byte = packet_payload;
+
+    switch(packet_payload) {
+    case 0x00:
+        if (os_mbuf_copyinto(m, 0, &g_ble_ll_dtm_prbs9_data, len)) {
+            return 1;
+        }
+        goto schedule;
+    case 0x01:
+        byte_pattern = 0x0F;
+        break;
+    case 0x02:
+        byte_pattern = 0x55;
+        break;
+    case 0x03:
+        if (os_mbuf_copyinto(m, 0, &g_ble_ll_dtm_prbs15_data, len)) {
+            return 1;
+        }
+        goto schedule;
+    case 0x04:
+        byte_pattern = 0xFF;
+        break;
+    case 0x05:
+        byte_pattern = 0x00;
+        break;
+    case 0x06:
+        byte_pattern = 0xF0;
+        break;
+    case 0x07:
+        byte_pattern = 0xAA;
+        break;
+    default:
+        return 1;
+    }
+
+    for (rc = 0; rc < len; rc++) {
+        if (os_mbuf_copyinto(m, rc, &byte_pattern, 1)) {
+            return 1;
+        }
+    }
+
+schedule:
+    ble_phy_enable_dtm();
+
+    sch->sched_cb = ble_ll_dtm_tx_sched_cb;
+    sch->cb_arg = ctx;
+    sch->sched_type = BLE_LL_SCHED_TYPE_DTM;
+
+    /* Prepare os_event */
+    ble_npl_event_init(&ctx->evt, ble_ll_dtm_ev_tx_resched_cb, ctx);
+
+    ble_ll_dtm_calculate_itvl(ctx, len, cmd_interval, phy_mode);
+
+    ctx->pdu_start_ticks = ble_ll_rfmgmt_enable_now();
+    ctx->pdu_start_usecs = 0;
+    ble_ll_dtm_set_next(ctx);
+
+    /* Set some start point for TX packets */
+    rc = ble_ll_sched_dtm(sch);
+    BLE_LL_ASSERT(rc == 0);
+
+    g_ble_ll_dtm_ctx.active = 1;
+    return 0;
+}
+
+static int
+ble_ll_dtm_rx_start(void)
+{
+    os_sr_t sr;
+    int rc;
+
+    rc = ble_phy_setchan(channel_rf_to_index[g_ble_ll_dtm_ctx.rf_channel],
+                         BLE_DTM_SYNC_WORD, BLE_DTM_CRC);
+    if (rc) {
+        return rc;
+    }
+
+#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY))
+    ble_phy_mode_set(g_ble_ll_dtm_ctx.phy_mode, g_ble_ll_dtm_ctx.phy_mode);
+#endif
+
+    OS_ENTER_CRITICAL(sr);
+    rc = ble_phy_rx_set_start_time(os_cputime_get32(), 0);
+    OS_EXIT_CRITICAL(sr);
+    if (rc && rc != BLE_PHY_ERR_RX_LATE) {
+        return rc;
+    }
+
+    ble_ll_state_set(BLE_LL_STATE_DTM);
+
+    return 0;
+}
+
+static int
+ble_ll_dtm_rx_sched_cb(struct ble_ll_sched_item *sch)
+{
+    if (ble_ll_dtm_rx_start() != 0) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt);
+        STATS_INC(ble_ll_dtm_stats, rx_failed);
+    }
+
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static int
+ble_ll_dtm_rx_create_ctx(uint8_t rf_channel, uint8_t phy_mode)
+{
+    struct ble_ll_sched_item *sch = &g_ble_ll_dtm_ctx.sch;
+    int rc;
+
+    g_ble_ll_dtm_ctx.phy_mode = phy_mode;
+    g_ble_ll_dtm_ctx.rf_channel = rf_channel;
+
+    STATS_CLEAR(ble_ll_dtm_stats, rx_count);
+
+    ble_npl_event_init(&g_ble_ll_dtm_ctx.evt, ble_ll_dtm_ev_rx_restart_cb,
+                       NULL);
+
+    sch->sched_cb = ble_ll_dtm_rx_sched_cb;
+    sch->cb_arg = &g_ble_ll_dtm_ctx;
+    sch->sched_type = BLE_LL_SCHED_TYPE_DTM;
+    sch->start_time =  ble_ll_rfmgmt_enable_now();
+
+    rc = ble_ll_sched_dtm(sch);
+    BLE_LL_ASSERT(rc == 0);
+
+    ble_phy_enable_dtm();
+
+    g_ble_ll_dtm_ctx.active = 1;
+    return 0;
+}
+
+static void
+ble_ll_dtm_ctx_free(struct dtm_ctx * ctx)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    if (!ctx->active) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    ble_ll_sched_rmv_elem(&ctx->sch);
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt);
+
+    ble_phy_disable();
+    ble_phy_disable_dtm();
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    ble_ll_rfmgmt_release();
+
+    os_mbuf_free_chain(ctx->om);
+    memset(ctx, 0, sizeof(*ctx));
+    OS_EXIT_CRITICAL(sr);
+}
+
+static int
+ble_ll_dtm_tx_test(uint8_t tx_chan, uint8_t len, uint8_t packet_payload,
+                   uint8_t hci_phy, uint16_t interval, uint16_t pkt_count)
+{
+    uint8_t phy_mode;
+
+    if (g_ble_ll_dtm_ctx.active) {
+        return BLE_ERR_CTLR_BUSY;
+    }
+
+    switch (hci_phy) {
+    case BLE_HCI_LE_PHY_1M:
+        phy_mode = BLE_PHY_MODE_1M;
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_HCI_LE_PHY_2M:
+        phy_mode = BLE_PHY_MODE_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_HCI_LE_PHY_CODED_S8:
+        phy_mode = BLE_PHY_MODE_CODED_125KBPS;
+        break;
+    case BLE_HCI_LE_PHY_CODED_S2:
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+        break;
+#endif
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (tx_chan > 0x27 || packet_payload > 0x07) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_dtm_tx_create_ctx(packet_payload, len, tx_chan, phy_mode,
+                                 interval, pkt_count)) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+static int
+ble_ll_hci_dtm_tx_test_ext(const uint8_t *cmdbuf)
+{
+    const struct ble_hci_le_tx_test_ext_cp *cmd = (const void *) cmdbuf;
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              BLE_HCI_LE_PHY_1M, le16toh(cmd->interval),
+                              le16toh(cmd->pkt_count));
+}
+
+static int
+ble_ll_hci_dtm_tx_test_v2_ext(const uint8_t *cmdbuf)
+{
+    const struct ble_hci_le_tx_test_v2_ext_cp *cmd = (const void *) cmdbuf;
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              cmd->phy, le16toh(cmd->interval),
+                              le16toh(cmd->pkt_count));
+}
+#endif
+
+int
+ble_ll_hci_dtm_tx_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_tx_test_cp *cmd = (const void *) cmdbuf;
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (len == sizeof(struct ble_hci_le_tx_test_ext_cp)) {
+        return ble_ll_hci_dtm_tx_test_ext(cmdbuf);
+    }
+#endif
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              BLE_HCI_LE_PHY_1M, 0, 0);
+}
+
+int
+ble_ll_hci_dtm_tx_test_v2(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_tx_test_v2_cp *cmd = (const void *) cmdbuf;
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (len == sizeof(struct ble_hci_le_tx_test_v2_ext_cp)) {
+        return ble_ll_hci_dtm_tx_test_v2_ext(cmdbuf);
+    }
+#endif
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              cmd->phy, 0, 0);
+}
+
+static int
+ble_ll_dtm_rx_test(uint8_t rx_chan, uint8_t hci_phy)
+{
+    uint8_t phy_mode;
+
+    if (g_ble_ll_dtm_ctx.active) {
+        return BLE_ERR_CTLR_BUSY;
+    }
+
+    switch (hci_phy) {
+    case BLE_HCI_LE_PHY_1M:
+        phy_mode = BLE_PHY_MODE_1M;
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_HCI_LE_PHY_2M:
+        phy_mode = BLE_PHY_MODE_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_HCI_LE_PHY_CODED:
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+        break;
+#endif
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (rx_chan > 0x27) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_dtm_rx_create_ctx(rx_chan, phy_mode)) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int ble_ll_hci_dtm_rx_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rx_test_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_dtm_rx_test(cmd->rx_chan, BLE_HCI_LE_PHY_1M);
+}
+
+int ble_ll_hci_dtm_rx_test_v2(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rx_test_v2_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* TODO ignoring modulation index */
+
+    return ble_ll_dtm_rx_test(cmd->rx_chan, cmd->phy);
+}
+
+int ble_ll_dtm_end_test(uint8_t *rsp, uint8_t *rsplen)
+{
+    put_le16(rsp, g_ble_ll_dtm_ctx. num_of_packets);
+    *rsplen = 2;
+
+    ble_ll_dtm_ctx_free(&g_ble_ll_dtm_ctx);
+    return BLE_ERR_SUCCESS;
+}
+
+int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa)
+{
+    return 0;
+}
+
+void
+ble_ll_dtm_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
+{
+    if (BLE_MBUF_HDR_CRC_OK(hdr)) {
+        /* XXX Compare data. */
+        g_ble_ll_dtm_ctx.num_of_packets++;
+        STATS_INC(ble_ll_dtm_stats, rx_count);
+    }
+
+    if (ble_ll_dtm_rx_start() != 0) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt);
+        STATS_INC(ble_ll_dtm_stats, rx_failed);
+    }
+}
+
+int
+ble_ll_dtm_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    struct os_mbuf *rxpdu;
+
+    if (!g_ble_ll_dtm_ctx.active) {
+        return -1;
+    }
+
+    rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN);
+
+    /* Copy the received pdu and hand it up */
+    if (rxpdu) {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
+    return 0;
+}
+
+void
+ble_ll_dtm_wfr_timer_exp(void)
+{
+    /* Should not be needed */
+    BLE_LL_ASSERT(0);
+}
+
+
+void
+ble_ll_dtm_reset(void)
+{
+    ble_ll_dtm_ctx_free(&g_ble_ll_dtm_ctx);
+}
+
+void
+ble_ll_dtm_init(void)
+{
+    int rc;
+
+    rc = stats_init_and_reg(STATS_HDR(ble_ll_dtm_stats),
+                            STATS_SIZE_INIT_PARMS(ble_ll_dtm_stats, STATS_SIZE_32),
+                            STATS_NAME_INIT_PARMS(ble_ll_dtm_stats),
+                            "ble_ll_dtm");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm_priv.h b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm_priv.h
new file mode 100644
index 00000000..e04af07b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm_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_LL_TEST_PRIV_
+#define H_BLE_LL_TEST_PRIV_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "nimble/ble.h"
+
+int ble_ll_hci_dtm_tx_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_hci_dtm_tx_test_v2(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_hci_dtm_rx_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_hci_dtm_rx_test_v2(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_dtm_end_test(uint8_t *rsp, uint8_t *rsplen);
+
+int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa);
+int ble_ll_dtm_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
+void ble_ll_dtm_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
+void ble_ll_dtm_wfr_timer_exp(void);
+void ble_ll_dtm_reset(void);
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci.c
new file mode 100644
index 00000000..b82adc2e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci.c
@@ -0,0 +1,1515 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_sync.h"
+#include "ble_ll_priv.h"
+#include "ble_ll_conn_priv.h"
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+#include "ble_ll_dtm_priv.h"
+#endif
+
+static void ble_ll_hci_cmd_proc(struct ble_npl_event *ev);
+
+/* OS event to enqueue command */
+static struct ble_npl_event g_ble_ll_hci_cmd_ev;
+
+/* LE event mask */
+static uint64_t g_ble_ll_hci_le_event_mask;
+static uint64_t g_ble_ll_hci_event_mask;
+static uint64_t g_ble_ll_hci_event_mask2;
+
+static int16_t rx_path_pwr_compensation;
+static int16_t tx_path_pwr_compensation;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static enum {
+    ADV_MODE_ANY,
+    ADV_MODE_LEGACY,
+    ADV_MODE_EXT,
+} hci_adv_mode;
+
+bool ble_ll_hci_adv_mode_ext(void)
+{
+    return hci_adv_mode == ADV_MODE_EXT;
+}
+#else
+bool
+ble_ll_hci_adv_mode_ext(void)
+{
+    return false;
+}
+#endif
+
+/**
+ * ll hci get num cmd pkts
+ *
+ * Returns the number of command packets that the host is allowed to send
+ * to the controller.
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_hci_get_num_cmd_pkts(void)
+{
+    return BLE_LL_CFG_NUM_HCI_CMD_PKTS;
+}
+
+/**
+ * Send an event to the host.
+ *
+ * @param evbuf Pointer to event buffer to send
+ *
+ * @return int 0: success; -1 otherwise.
+ */
+int
+ble_ll_hci_event_send(struct ble_hci_ev *hci_ev)
+{
+    int rc;
+
+    BLE_LL_DEBUG_GPIO(HCI_EV, 1);
+
+    BLE_LL_ASSERT(sizeof(*hci_ev) + hci_ev->length <= BLE_LL_MAX_EVT_LEN);
+
+    /* Count number of events sent */
+    STATS_INC(ble_ll_stats, hci_events_sent);
+
+    /* Send the event to the host */
+    rc = ble_hci_trans_ll_evt_tx((uint8_t *)hci_ev);
+
+    BLE_LL_DEBUG_GPIO(HCI_EV, 0);
+
+    return rc;
+}
+
+/**
+ * Created and sends a command complete event with the no-op opcode to the
+ * host.
+ */
+void
+ble_ll_hci_send_noop(void)
+{
+    struct ble_hci_ev_command_complete_nop *ev;
+    struct ble_hci_ev *hci_ev;
+
+    hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    if (hci_ev) {
+        /* Create a command complete event with a NO-OP opcode */
+        hci_ev->opcode = BLE_HCI_EVCODE_COMMAND_COMPLETE;
+
+        hci_ev->length = sizeof(*ev);
+        ev = (void *)hci_ev->data;
+
+        ev->num_packets = ble_ll_hci_get_num_cmd_pkts();
+        ev->opcode = BLE_HCI_OPCODE_NOP;
+
+        ble_ll_hci_event_send(hci_ev);
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * LE encrypt command
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_le_encrypt(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf,
+                      uint8_t *rsplen)
+{
+    const struct ble_hci_le_encrypt_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_encrypt_rp *rsp = (void *)rspbuf;
+    struct ble_encryption_block ecb;
+    int rc;
+
+    /* Call the link layer to encrypt the data */
+    swap_buf(ecb.key, cmd->key, BLE_ENC_BLOCK_SIZE);
+    swap_buf(ecb.plain_text, cmd->data, BLE_ENC_BLOCK_SIZE);
+    rc = ble_hw_encrypt_block(&ecb);
+    if (!rc) {
+        swap_buf(rsp->data, ecb.cipher_text, BLE_ENC_BLOCK_SIZE);
+        *rsplen = sizeof(*rsp);
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        rc = BLE_ERR_CTLR_BUSY;
+    }
+
+    return rc;
+}
+#endif
+
+/**
+ * LE rand command
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_le_rand(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rand_rp *rsp = (void *) rspbuf;
+
+    ble_ll_rand_data_get((uint8_t *)&rsp->random_number,
+                         sizeof(rsp->random_number));
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Read local version
+ *
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_rd_local_version(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_ip_rd_local_ver_rp *rsp = (void *) rspbuf;
+
+    rsp->hci_ver = BLE_HCI_VER_BCS;
+    rsp->hci_rev = 0;
+    rsp->lmp_ver = BLE_LMP_VER_BCS;
+    rsp->manufacturer = htole16(MYNEWT_VAL(BLE_LL_MFRG_ID));
+    rsp->lmp_subver = 0;
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Read local supported features
+ *
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_rd_local_supp_feat(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_ip_rd_loc_supp_feat_rp *rsp = (void *) rspbuf;
+
+    /*
+     * The only two bits we set here currently are (5th byte):
+     *      BR/EDR not supported        (bit 5)
+     *      LE supported (controller)   (bit 6)
+     */
+    rsp->features = htole64(0x0000006000000000);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Read local supported commands
+ *
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_rd_local_supp_cmd(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_ip_rd_loc_supp_cmd_rp *rsp = (void *) rspbuf;
+
+    memset(rsp->commands, 0, sizeof(rsp->commands));
+    memcpy(rsp->commands, g_ble_ll_supp_cmds, sizeof(g_ble_ll_supp_cmds));
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Called to read the public device address of the device
+ *
+ *
+ * @param rspbuf
+ * @param rsplen
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_rd_bd_addr(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_ip_rd_bd_addr_rp *rsp = (void *) rspbuf;
+
+    memcpy(rsp->addr, g_dev_addr, BLE_DEV_ADDR_LEN);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * ll hci set le event mask
+ *
+ * Called when the LL controller receives a set LE event mask command.
+ *
+ * Context: Link Layer task (HCI command parser)
+ *
+ * @param cmdbuf Pointer to command buf.
+ *
+ * @return int BLE_ERR_SUCCESS. Does not return any errors.
+ */
+static int
+ble_ll_hci_set_le_event_mask(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_event_mask_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    g_ble_ll_hci_le_event_mask = le64toh(cmd->event_mask);
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * HCI read buffer size command. Returns the ACL data packet length and
+ * num data packets.
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_hci_le_read_bufsize(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_buf_size_rp *rp = (void *) rspbuf;
+
+    rp->data_len = htole16(g_ble_ll_data.ll_acl_pkt_size);
+    rp->data_packets = g_ble_ll_data.ll_num_acl_pkts;
+
+    *rsplen = sizeof(*rp);
+    return BLE_ERR_SUCCESS;
+}
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+/**
+ * Checks the preferred phy masks for validity and places the preferred masks
+ * in the input phy masks
+
+ * @return int BLE_ERR_SUCCESS or BLE_ERR_INV_HCI_CMD_PARMS or BLE_ERR_UNSUPPORTED
+ */
+int
+ble_ll_hci_chk_phy_masks(uint8_t all_phys, uint8_t tx_phys, uint8_t rx_phys,
+                         uint8_t *txphy, uint8_t *rxphy)
+{
+    /* Check for RFU */
+    if ((tx_phys & ~BLE_HCI_LE_PHY_PREF_MASK_ALL) ||
+                    (rx_phys & ~BLE_HCI_LE_PHY_PREF_MASK_ALL)) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+
+    if ((!(all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) && (tx_phys == 0)) ||
+        (!(all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) && (rx_phys == 0))) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If phy not supported, return error */
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    if((tx_phys & BLE_HCI_LE_PHY_2M_PREF_MASK) ||
+                    (rx_phys & BLE_HCI_LE_PHY_2M_PREF_MASK)) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+#endif
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if ((tx_phys & BLE_HCI_LE_PHY_CODED_PREF_MASK) ||
+                    (rx_phys & BLE_HCI_LE_PHY_CODED_PREF_MASK)) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+#endif
+    /* Set the default PHY preferences */
+    if (all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) {
+        tx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL;
+    }
+    *txphy = tx_phys;
+
+    if (all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) {
+        rx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL;
+    }
+    *rxphy = rx_phys;
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Set PHY preferences for connection
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+static int
+ble_ll_hci_le_set_def_phy(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_default_phy_cp *cmd = (const void *) cmdbuf;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rc = ble_ll_hci_chk_phy_masks(cmd->all_phys, cmd->tx_phys, cmd->rx_phys,
+                                  &g_ble_ll_data.ll_pref_tx_phys,
+                                  &g_ble_ll_data.ll_pref_rx_phys);
+    return rc;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+/**
+ * HCI write suggested default data length command.
+ *
+ * This command is used by the host to change the initial max tx octets/time
+ * for all connections. Note that if the controller does not support the
+ * requested times no error is returned; the controller simply ignores the
+ * request (but remembers what the host requested for the read suggested
+ * default data length command). The spec allows for the controller to
+ * disregard the host.
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_hci_le_wr_sugg_data_len(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_wr_sugg_def_data_len_cp *cmd = (const void*) cmdbuf;
+    uint16_t tx_oct;
+    uint16_t tx_time;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Get suggested octets and time */
+    tx_oct = le16toh(cmd->max_tx_octets);
+    tx_time = le16toh(cmd->max_tx_time);
+
+    /* If valid, write into suggested and change connection initial times */
+    if (ble_ll_chk_txrx_octets(tx_oct) && ble_ll_chk_txrx_time(tx_time)) {
+        g_ble_ll_conn_params.sugg_tx_octets = (uint8_t)tx_oct;
+        g_ble_ll_conn_params.sugg_tx_time = tx_time;
+
+        /*
+         * We can disregard host suggestion, but we are a nice controller so
+         * let's use host suggestion, unless they exceed max supported values
+         * in which case we just use our max.
+         */
+        g_ble_ll_conn_params.conn_init_max_tx_octets =
+                        min(tx_oct, g_ble_ll_conn_params.supp_max_tx_octets);
+        g_ble_ll_conn_params.conn_init_max_tx_time =
+                        min(tx_time, g_ble_ll_conn_params.supp_max_tx_time);
+
+        /*
+         * Use the same for coded and uncoded defaults. These are used when PHY
+         * parameters are initialized and we want to use values overridden by
+         * host. Make sure we do not exceed max supported time on uncoded.
+         */
+        g_ble_ll_conn_params.conn_init_max_tx_time_uncoded =
+                                min(BLE_LL_CONN_SUPP_TIME_MAX_UNCODED,
+                                    g_ble_ll_conn_params.conn_init_max_tx_time);
+        g_ble_ll_conn_params.conn_init_max_tx_time_coded =
+                                g_ble_ll_conn_params.conn_init_max_tx_time;
+
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return rc;
+}
+
+/**
+ * HCI read suggested default data length command. Returns the controllers
+ * initial max tx octet/time.
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_hci_le_rd_sugg_data_len(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_sugg_def_data_len_rp *rsp = (void *) rspbuf;
+
+    /* Place the data packet length and number of packets in the buffer */
+    rsp->max_tx_octets = htole16(g_ble_ll_conn_params.sugg_tx_octets);
+    rsp->max_tx_time = htole16(g_ble_ll_conn_params.sugg_tx_time);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * HCI read maximum data length command. Returns the controllers max supported
+ * rx/tx octets/times.
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_hci_le_rd_max_data_len(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_max_data_len_rp *rsp = (void *)rspbuf;
+
+    /* Place the data packet length and number of packets in the buffer */
+    rsp->max_tx_octests = htole16(g_ble_ll_conn_params.supp_max_tx_octets);
+    rsp->max_tx_time = htole16(g_ble_ll_conn_params.supp_max_tx_time);
+    rsp->max_rx_octests = htole16(g_ble_ll_conn_params.supp_max_rx_octets);
+    rsp->max_rx_time = htole16(g_ble_ll_conn_params.supp_max_rx_time);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+#endif
+
+/**
+ * HCI read local supported features command. Returns the features
+ * supported by the controller.
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_hci_le_read_local_features(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_loc_supp_feat_rp *rsp = (void *) rspbuf;
+
+    rsp->features = htole64(ble_ll_read_supp_features());
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * HCI read local supported states command. Returns the states
+ * supported by the controller.
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_hci_le_read_supp_states(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_supp_states_rp *rsp = (void *) rspbuf;
+
+    /* Add list of supported states. */
+    rsp->states = htole64(ble_ll_read_supp_states());
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+
+/**
+ * Checks to see if a LE event has been disabled by the host.
+ *
+ * @param subev Sub-event code of the LE Meta event. Note that this can
+ * be a value from 1 to 64, inclusive.
+ *
+ * @return uint8_t 0: event is not enabled; otherwise event is enabled.
+ */
+bool
+ble_ll_hci_is_le_event_enabled(unsigned int subev)
+{
+    /* The LE meta event must be enabled for any LE event to be enabled */
+    if (g_ble_ll_hci_event_mask & (1ull << (BLE_HCI_EVCODE_LE_META - 1))) {
+        return g_ble_ll_hci_le_event_mask & (1ull << (subev - 1));
+    }
+
+    return false;
+}
+
+/**
+ * Checks to see if an event has been disabled by the host.
+ *
+ * NOTE: there are two "pages" of event masks; the first page is for event
+ * codes between 0 and 63 and the second page is for event codes 64 and
+ * greater.
+ *
+ * @param evcode This is the event code for the event.
+ *
+ * @return uint8_t 0: event is not enabled; otherwise event is enabled.
+ */
+bool
+ble_ll_hci_is_event_enabled(unsigned int evcode)
+{
+    if (evcode >= 64) {
+        return g_ble_ll_hci_event_mask2 & (1ull << (evcode - 64));
+    }
+
+    return g_ble_ll_hci_event_mask & (1ull << (evcode - 1));
+}
+
+/**
+ * Called to determine if the reply to the command should be a command complete
+ * event or a command status event.
+ *
+ * @param ocf
+ *
+ * @return int 0: return command complete; 1: return command status event
+ */
+static int
+ble_ll_hci_le_cmd_send_cmd_status(uint16_t ocf)
+{
+    int rc;
+
+    switch (ocf) {
+    case BLE_HCI_OCF_LE_RD_REM_FEAT:
+    case BLE_HCI_OCF_LE_CREATE_CONN:
+    case BLE_HCI_OCF_LE_EXT_CREATE_CONN:
+    case BLE_HCI_OCF_LE_CONN_UPDATE:
+    case BLE_HCI_OCF_LE_START_ENCRYPT:
+    case BLE_HCI_OCF_LE_RD_P256_PUBKEY:
+    case BLE_HCI_OCF_LE_GEN_DHKEY:
+    case BLE_HCI_OCF_LE_SET_PHY:
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC:
+        rc = 1;
+        break;
+    default:
+        rc = 0;
+        break;
+    }
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/** HCI LE read maximum advertising data length command. Returns the controllers
+* max supported advertising data length;
+*
+* @param rspbuf Pointer to response buffer
+* @param rsplen Length of response buffer
+*
+* @return int BLE error code
+*/
+static int
+ble_ll_adv_rd_max_adv_data_len(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_max_adv_data_len_rp *rsp = (void *) rspbuf;
+
+    rsp->max_adv_data_len = htole16(BLE_ADV_DATA_MAX_LEN);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * HCI LE read number of supported advertising sets
+ *
+ * @param rspbuf Pointer to response buffer
+ * @param rsplen Length of response buffer
+ *
+ * @return int BLE error code
+ */
+static int
+ble_ll_adv_rd_sup_adv_sets(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_num_of_adv_sets_rp *rsp = (void *)rspbuf;
+
+    rsp->num_sets = BLE_ADV_INSTANCES;
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+static bool
+ble_ll_is_valid_adv_mode(uint8_t ocf)
+{
+    /*
+     * If, since the last power-on or reset, the Host has ever issued a legacy
+     * advertising command and then issues an extended advertising command, or
+     * has ever issued an extended advertising command and then issues a legacy
+     * advertising command, the Controller shall return the error code Command
+     * Disallowed (0x0C).
+    */
+
+    switch(ocf) {
+    case BLE_HCI_OCF_LE_CREATE_CONN:
+    case BLE_HCI_OCF_LE_SET_ADV_PARAMS:
+    case BLE_HCI_OCF_LE_SET_ADV_ENABLE:
+    case BLE_HCI_OCF_LE_SET_ADV_DATA:
+    case BLE_HCI_OCF_LE_SET_SCAN_PARAMS:
+    case BLE_HCI_OCF_LE_SET_SCAN_ENABLE:
+    case BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA:
+    case BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR:
+        if (hci_adv_mode == ADV_MODE_EXT) {
+            return false;
+        }
+
+        hci_adv_mode = ADV_MODE_LEGACY;
+        break;
+    case BLE_HCI_OCF_LE_EXT_CREATE_CONN:
+    case BLE_HCI_OCF_LE_SET_EXT_ADV_DATA:
+    case BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE:
+    case BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM:
+    case BLE_HCI_OCF_LE_SET_EXT_SCAN_ENABLE:
+    case BLE_HCI_OCF_LE_SET_EXT_SCAN_PARAM:
+    case BLE_HCI_OCF_LE_SET_EXT_SCAN_RSP_DATA:
+    case BLE_HCI_OCF_LE_RD_MAX_ADV_DATA_LEN:
+    case BLE_HCI_OCF_LE_RD_NUM_OF_ADV_SETS:
+    case BLE_HCI_OCF_LE_REMOVE_ADV_SET:
+    case BLE_HCI_OCF_LE_CLEAR_ADV_SETS:
+    case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_PARAMS:
+    case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA:
+    case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE:
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC:
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC_CANCEL:
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC:
+    case BLE_HCI_OCF_LE_ADD_DEV_TO_PERIODIC_ADV_LIST:
+    case BLE_HCI_OCF_LE_REM_DEV_FROM_PERIODIC_ADV_LIST:
+    case BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST:
+    case BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE:
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_RECEIVE_ENABLE:
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER:
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER:
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS:
+    case BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS:
+#endif
+        if (hci_adv_mode == ADV_MODE_LEGACY) {
+            return false;
+        }
+
+        hci_adv_mode = ADV_MODE_EXT;
+        break;
+    default:
+        break;
+    }
+
+    return true;
+}
+#endif
+
+static int
+ble_ll_read_tx_power(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_transmit_power_rp *rsp = (void *) rspbuf;
+
+    rsp->min_tx_power = ble_phy_txpower_round(-127);
+    rsp->max_tx_power = ble_phy_txpower_round(126);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+static int
+ble_ll_read_rf_path_compensation(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_rf_path_compensation_rp *rsp = (void *) rspbuf;
+
+    rsp->rx_path_compensation = htole16(rx_path_pwr_compensation);
+    rsp->tx_path_compensation = htole16(tx_path_pwr_compensation);
+
+    *rsplen = sizeof(*rsp);;
+    return BLE_ERR_SUCCESS;
+}
+
+static int
+ble_ll_write_rf_path_compensation(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_wr_rf_path_compensation_cp *cmd = (const void *)cmdbuf;
+    int16_t rx;
+    int16_t tx;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    tx = le16toh(cmd->tx_path_compensation);
+    rx = le16toh(cmd->rx_path_compensation);
+
+    if ((tx < -1280) || (tx > 1280) || (rx < -1280) || (rx > 1280)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    tx_path_pwr_compensation = tx;
+    rx_path_pwr_compensation = rx;
+
+    ble_phy_set_rx_pwr_compensation(rx_path_pwr_compensation / 10);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int8_t
+ble_ll_get_tx_pwr_compensation(void)
+{
+    return tx_path_pwr_compensation / 10;
+}
+
+/**
+ * Process a LE command sent from the host to the controller. The HCI command
+ * has a 3 byte command header followed by data. The header is:
+ *  -> opcode (2 bytes)
+ *  -> Length of parameters (1 byte; does include command header bytes).
+ *
+ * @param cmdbuf Pointer to command buffer. Points to start of command header.
+ * @param ocf    Opcode command field.
+ * @param *rsplen Pointer to length of response
+ *
+ * @return int  This function returns a BLE error code. If a command status
+ *              event should be returned as opposed to command complete,
+ *              256 gets added to the return value.
+ */
+static int
+ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf,
+                       uint8_t *rspbuf, uint8_t *rsplen,
+                       ble_ll_hci_post_cmd_complete_cb *cb)
+{
+    int rc;
+
+    /* Assume error; if all pass rc gets set to 0 */
+    rc = BLE_ERR_INV_HCI_CMD_PARMS;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (!ble_ll_is_valid_adv_mode(ocf)) {
+        rc = BLE_ERR_CMD_DISALLOWED;
+
+        /*
+         * This code is here because we add 256 to the return code to denote
+         * that the reply to this command should be command status (as opposed to
+         * command complete).
+         *
+         * For unknown HCI command let us return always command status as per
+         * specification Bluetooth 5, Vol. 2, Chapter 4.4
+         */
+        if (ble_ll_hci_le_cmd_send_cmd_status(ocf)) {
+            rc += (BLE_ERR_MAX + 1);
+        }
+
+        return rc;
+    }
+#endif
+
+    switch (ocf) {
+    case BLE_HCI_OCF_LE_SET_EVENT_MASK:
+        rc = ble_ll_hci_set_le_event_mask(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RD_BUF_SIZE:
+        if (len == 0) {
+            rc = ble_ll_hci_le_read_bufsize(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT:
+        if (len == 0) {
+            rc = ble_ll_hci_le_read_local_features(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_SET_RAND_ADDR:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        rc = ble_ll_set_random_addr(cmdbuf, len, hci_adv_mode == ADV_MODE_EXT);
+#else
+        rc = ble_ll_set_random_addr(cmdbuf, len, false);
+#endif
+        break;
+    case BLE_HCI_OCF_LE_SET_ADV_PARAMS:
+        rc = ble_ll_adv_set_adv_params(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR:
+        if (len == 0) {
+            rc = ble_ll_adv_read_txpwr(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_SET_ADV_DATA:
+        rc = ble_ll_hci_set_adv_data(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA:
+        rc = ble_ll_hci_set_scan_rsp_data(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_ADV_ENABLE:
+        rc = ble_ll_hci_adv_set_enable(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_SCAN_PARAMS:
+        rc = ble_ll_scan_set_scan_params(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_SCAN_ENABLE:
+        rc = ble_ll_hci_scan_set_enable(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_CREATE_CONN:
+        rc = ble_ll_conn_create(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_CREATE_CONN_CANCEL:
+        if (len == 0) {
+            rc = ble_ll_conn_create_cancel(cb);
+        }
+        break;
+    case BLE_HCI_OCF_LE_RD_WHITE_LIST_SIZE:
+        if (len == 0) {
+            rc = ble_ll_whitelist_read_size(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_CLEAR_WHITE_LIST:
+        if (len == 0) {
+            rc = ble_ll_whitelist_clear();
+        }
+        break;
+    case BLE_HCI_OCF_LE_ADD_WHITE_LIST:
+        rc = ble_ll_whitelist_add(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RMV_WHITE_LIST:
+        rc = ble_ll_whitelist_rmv(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_CONN_UPDATE:
+        rc = ble_ll_conn_hci_update(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_HOST_CHAN_CLASS:
+        rc = ble_ll_conn_hci_set_chan_class(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RD_CHAN_MAP:
+        rc = ble_ll_conn_hci_rd_chan_map(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_RD_REM_FEAT:
+        rc = ble_ll_conn_hci_read_rem_features(cmdbuf, len);
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    case BLE_HCI_OCF_LE_ENCRYPT:
+        rc = ble_ll_hci_le_encrypt(cmdbuf, len, rspbuf, rsplen);
+        break;
+#endif
+    case BLE_HCI_OCF_LE_RAND:
+        if (len == 0) {
+            rc = ble_ll_hci_le_rand(rspbuf, rsplen);
+        }
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    case BLE_HCI_OCF_LE_START_ENCRYPT:
+        rc = ble_ll_conn_hci_le_start_encrypt(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY:
+        rc = ble_ll_conn_hci_le_ltk_reply(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY:
+        rc = ble_ll_conn_hci_le_ltk_neg_reply(cmdbuf, len, rspbuf, rsplen);
+        break;
+#endif
+    case BLE_HCI_OCF_LE_RD_SUPP_STATES :
+        if (len == 0) {
+            rc = ble_ll_hci_le_read_supp_states(rspbuf, rsplen);
+        }
+        break;
+#if MYNEWT_VAL(BLE_LL_DTM)
+    case BLE_HCI_OCF_LE_TX_TEST:
+        rc = ble_ll_hci_dtm_tx_test(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RX_TEST:
+        rc = ble_ll_hci_dtm_rx_test(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_TEST_END:
+        if (len == 0) {
+            rc = ble_ll_dtm_end_test(rspbuf, rsplen);
+        }
+        break;
+#endif
+    case BLE_HCI_OCF_LE_REM_CONN_PARAM_RR:
+        rc = ble_ll_conn_hci_param_rr(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR:
+        rc = ble_ll_conn_hci_param_nrr(cmdbuf, len, rspbuf, rsplen);
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+    case BLE_HCI_OCF_LE_SET_DATA_LEN:
+        rc = ble_ll_conn_hci_set_data_len(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_RD_SUGG_DEF_DATA_LEN:
+        if (len == 0) {
+            rc = ble_ll_hci_le_rd_sugg_data_len(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_WR_SUGG_DEF_DATA_LEN:
+        rc = ble_ll_hci_le_wr_sugg_data_len(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_OCF_LE_ADD_RESOLV_LIST:
+        rc = ble_ll_resolv_list_add(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RMV_RESOLV_LIST:
+        rc = ble_ll_resolv_list_rmv(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_CLR_RESOLV_LIST:
+        if (len == 0) {
+            rc = ble_ll_resolv_list_clr();
+        }
+        break;
+    case BLE_HCI_OCF_LE_RD_RESOLV_LIST_SIZE:
+        if (len == 0) {
+            rc = ble_ll_resolv_list_read_size(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_RD_PEER_RESOLV_ADDR:
+        rc = ble_ll_resolv_peer_addr_rd(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR:
+        rc = ble_ll_resolv_local_addr_rd(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_SET_ADDR_RES_EN:
+        rc = ble_ll_resolv_enable_cmd(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_RPA_TMO:
+        rc = ble_ll_resolv_set_rpa_tmo(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+    case BLE_HCI_OCF_LE_RD_MAX_DATA_LEN:
+        if (len == 0) {
+            rc = ble_ll_hci_le_rd_max_data_len(rspbuf, rsplen);
+        }
+        break;
+#endif
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    case BLE_HCI_OCF_LE_RD_PHY:
+        rc = ble_ll_conn_hci_le_rd_phy(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_SET_DEFAULT_PHY:
+        rc = ble_ll_hci_le_set_def_phy(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_PHY:
+        rc = ble_ll_conn_hci_le_set_phy(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_DTM)
+    case BLE_HCI_OCF_LE_RX_TEST_V2:
+        rc = ble_ll_hci_dtm_rx_test_v2(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_TX_TEST_V2:
+        rc = ble_ll_hci_dtm_tx_test_v2(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_HCI_OCF_LE_SET_ADV_SET_RND_ADDR:
+        rc = ble_ll_adv_hci_set_random_addr(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM:
+        rc = ble_ll_adv_ext_set_param(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_SET_EXT_ADV_DATA:
+        rc = ble_ll_adv_ext_set_adv_data(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_EXT_SCAN_RSP_DATA:
+        rc = ble_ll_adv_ext_set_scan_rsp(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE:
+        rc =  ble_ll_adv_ext_set_enable(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_RD_MAX_ADV_DATA_LEN:
+        if (len == 0) {
+            rc = ble_ll_adv_rd_max_adv_data_len(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_RD_NUM_OF_ADV_SETS:
+        if (len == 0) {
+            rc = ble_ll_adv_rd_sup_adv_sets(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_LE_REMOVE_ADV_SET:
+        rc =  ble_ll_adv_remove(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_CLEAR_ADV_SETS:
+        if (len == 0) {
+            rc =  ble_ll_adv_clear_all();
+        }
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_PARAMS:
+        rc = ble_ll_adv_periodic_set_param(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA:
+        rc = ble_ll_adv_periodic_set_data(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE:
+        rc = ble_ll_adv_periodic_enable(cmdbuf, len);
+        break;
+#endif
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_HCI_OCF_LE_SET_EXT_SCAN_PARAM:
+        rc = ble_ll_set_ext_scan_params(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_SET_EXT_SCAN_ENABLE:
+        rc = ble_ll_hci_ext_scan_set_enable(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_EXT_CREATE_CONN:
+        rc = ble_ll_ext_conn_create(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC:
+        rc = ble_ll_sync_create(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC_CANCEL:
+        if (len == 0) {
+            rc = ble_ll_sync_cancel(cb);
+        }
+        break;
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC:
+        rc = ble_ll_sync_terminate(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_ADD_DEV_TO_PERIODIC_ADV_LIST:
+        rc = ble_ll_sync_list_add(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_REM_DEV_FROM_PERIODIC_ADV_LIST:
+        rc = ble_ll_sync_list_remove(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST:
+        if (len == 0) {
+            rc = ble_ll_sync_list_clear();
+        }
+        break;
+    case BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE:
+        if (len == 0) {
+            rc = ble_ll_sync_list_size(rspbuf, rsplen);
+        }
+        break;
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_RECEIVE_ENABLE:
+        rc = ble_ll_sync_receive_enable(cmdbuf, len);
+        break;
+#endif
+#endif
+    case BLE_HCI_OCF_LE_RD_TRANSMIT_POWER:
+        rc = ble_ll_read_tx_power(rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_RD_RF_PATH_COMPENSATION:
+        rc = ble_ll_read_rf_path_compensation(rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_WR_RF_PATH_COMPENSATION:
+        rc = ble_ll_write_rf_path_compensation(cmdbuf, len);
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_OCF_LE_SET_PRIVACY_MODE:
+        rc = ble_ll_resolve_set_priv_mode(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER:
+        rc = ble_ll_sync_transfer(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER:
+        rc = ble_ll_adv_periodic_set_info_transfer(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS:
+        rc = ble_ll_set_sync_transfer_params(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS:
+        rc = ble_ll_set_default_sync_transfer_params(cmdbuf, len);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_VERSION) >= 52
+    case BLE_HCI_OCF_LE_SET_HOST_FEAT:
+        rc = ble_ll_set_host_feat(cmdbuf, len);
+        break;
+#endif
+    default:
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        break;
+    }
+
+    /*
+     * This code is here because we add 256 to the return code to denote
+     * that the reply to this command should be command status (as opposed to
+     * command complete).
+     *
+     * For unknown HCI command let us return always command status as per
+     * specification Bluetooth 5, Vol. 2, Chapter 4.4
+     */
+    if (ble_ll_hci_le_cmd_send_cmd_status(ocf) || rc == BLE_ERR_UNKNOWN_HCI_CMD) {
+        rc += (BLE_ERR_MAX + 1);
+    }
+
+    return rc;
+}
+
+/**
+ * Process a link control command sent from the host to the controller. The HCI
+ * command has a 3 byte command header followed by data. The header is:
+ *  -> opcode (2 bytes)
+ *  -> Length of parameters (1 byte; does include command header bytes).
+ *
+ * @param cmdbuf Pointer to command buffer. Points to start of command header.
+ * @param ocf    Opcode command field.
+ *
+ * @return int  This function returns a BLE error code. If a command status
+ *              event should be returned as opposed to command complete,
+ *              256 gets added to the return value.
+ */
+static int
+ble_ll_hci_link_ctrl_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf)
+{
+    int rc;
+
+    switch (ocf) {
+    case BLE_HCI_OCF_DISCONNECT_CMD:
+        rc = ble_ll_conn_hci_disconnect_cmd(cmdbuf, len);
+        /* Send command status instead of command complete */
+        rc += (BLE_ERR_MAX + 1);
+        break;
+
+    case BLE_HCI_OCF_RD_REM_VER_INFO:
+        rc = ble_ll_conn_hci_rd_rem_ver_cmd(cmdbuf, len);
+        /* Send command status instead of command complete */
+        rc += (BLE_ERR_MAX + 1);
+        break;
+
+    default:
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        break;
+    }
+
+    return rc;
+}
+
+static int
+ble_ll_hci_cb_set_event_mask(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_cb_set_event_mask_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof (*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    g_ble_ll_hci_event_mask = le64toh(cmd->event_mask);
+
+    return BLE_ERR_SUCCESS;
+}
+
+static int
+ble_ll_hci_cb_set_event_mask2(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_cb_set_event_mask2_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof (*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    g_ble_ll_hci_event_mask2 = le64toh(cmd->event_mask2);
+
+    return BLE_ERR_SUCCESS;
+}
+
+static int
+ble_ll_hci_ctlr_bb_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf,
+                            uint8_t *rspbuf, uint8_t *rsplen)
+{
+    int rc;
+
+    /* Assume error; if all pass rc gets set to 0 */
+    rc = BLE_ERR_INV_HCI_CMD_PARMS;
+
+    switch (ocf) {
+    case BLE_HCI_OCF_CB_SET_EVENT_MASK:
+        rc = ble_ll_hci_cb_set_event_mask(cmdbuf, len);
+        break;
+    case BLE_HCI_OCF_CB_RESET:
+        if (len == 0) {
+            rc = ble_ll_reset();
+        }
+        break;
+    case BLE_HCI_OCF_CB_SET_EVENT_MASK2:
+        rc = ble_ll_hci_cb_set_event_mask2(cmdbuf, len);
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
+    case BLE_HCI_OCF_CB_RD_AUTH_PYLD_TMO:
+        rc = ble_ll_conn_hci_rd_auth_pyld_tmo(cmdbuf, len, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_CB_WR_AUTH_PYLD_TMO:
+        rc = ble_ll_conn_hci_wr_auth_pyld_tmo(cmdbuf, len, rspbuf, rsplen);
+        break;
+#endif
+    default:
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        break;
+    }
+
+    return rc;
+}
+
+static int
+ble_ll_hci_info_params_cmd_proc(const uint8_t *cmdbuf, uint8_t len,
+                                uint16_t ocf, uint8_t *rspbuf, uint8_t *rsplen)
+{
+    int rc;
+
+    /* Assume error; if all pass rc gets set to 0 */
+    rc = BLE_ERR_INV_HCI_CMD_PARMS;
+
+    switch (ocf) {
+    case BLE_HCI_OCF_IP_RD_LOCAL_VER:
+        if (len == 0) {
+            rc = ble_ll_hci_rd_local_version(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD:
+        if (len == 0) {
+            rc = ble_ll_hci_rd_local_supp_cmd(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT:
+        if (len == 0) {
+            rc = ble_ll_hci_rd_local_supp_feat(rspbuf, rsplen);
+        }
+        break;
+    case BLE_HCI_OCF_IP_RD_BD_ADDR:
+        if (len == 0) {
+            rc = ble_ll_hci_rd_bd_addr(rspbuf, rsplen);
+        }
+        break;
+    default:
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        break;
+    }
+
+    return rc;
+}
+
+static int
+ble_ll_hci_status_params_cmd_proc(const uint8_t *cmdbuf, uint8_t len,
+                                  uint16_t ocf, uint8_t *rspbuf,
+                                  uint8_t *rsplen)
+{
+    int rc;
+
+    switch (ocf) {
+    case BLE_HCI_OCF_RD_RSSI:
+        rc = ble_ll_conn_hci_rd_rssi(cmdbuf, len, rspbuf, rsplen);
+        break;
+    default:
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        break;
+    }
+
+    return rc;
+}
+
+/**
+ * Called to process an HCI command from the host.
+ *
+ * @param ev Pointer to os event containing a pointer to command buffer
+ */
+static void
+ble_ll_hci_cmd_proc(struct ble_npl_event *ev)
+{
+    int rc;
+    uint8_t ogf;
+    uint8_t rsplen;
+    struct ble_hci_cmd *cmd;
+    uint16_t opcode;
+    uint16_t ocf;
+    ble_ll_hci_post_cmd_complete_cb post_cb = NULL;
+    struct ble_hci_ev *hci_ev;
+    struct ble_hci_ev_command_status *cmd_status;
+    struct ble_hci_ev_command_complete *cmd_complete;
+    uint8_t *rspbuf;
+
+    BLE_LL_DEBUG_GPIO(HCI_CMD, 1);
+
+    /* The command buffer is the event argument */
+    cmd = ble_npl_event_get_arg(ev);
+    BLE_LL_ASSERT(cmd != NULL);
+
+    /* Get the opcode from the command buffer */
+    opcode = le16toh(cmd->opcode);
+    ocf = BLE_HCI_OCF(opcode);
+    ogf = BLE_HCI_OGF(opcode);
+
+    /*
+     * The command response pointer points into the same buffer as the
+     * command data itself. That is fine, as each command reads all the data
+     * before crafting a response.
+     * Also reuse cmd buffer for complete event
+     */
+    hci_ev = (struct ble_hci_ev *) cmd;
+    rspbuf = hci_ev->data + sizeof(*cmd_complete);
+
+    /* Assume response length is zero */
+    rsplen = 0;
+
+    switch (ogf) {
+    case BLE_HCI_OGF_LINK_CTRL:
+        rc = ble_ll_hci_link_ctrl_cmd_proc(cmd->data, cmd->length, ocf);
+        break;
+    case BLE_HCI_OGF_CTLR_BASEBAND:
+        rc = ble_ll_hci_ctlr_bb_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen);
+        break;
+    case BLE_HCI_OGF_INFO_PARAMS:
+        rc = ble_ll_hci_info_params_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen);
+        break;
+    case BLE_HCI_OGF_STATUS_PARAMS:
+        rc = ble_ll_hci_status_params_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen);
+        break;
+    case BLE_HCI_OGF_LE:
+        rc = ble_ll_hci_le_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen, &post_cb);
+        break;
+    default:
+        /* XXX: Need to support other OGF. For now, return unsupported */
+        rc = BLE_ERR_UNKNOWN_HCI_CMD;
+        break;
+    }
+
+    /* If no response is generated, we free the buffers */
+    BLE_LL_ASSERT(rc >= 0);
+    if (rc <= BLE_ERR_MAX) {
+        /* Create a command complete event with status from command */
+        hci_ev->opcode = BLE_HCI_EVCODE_COMMAND_COMPLETE;
+        hci_ev->length = sizeof(*cmd_complete) + rsplen;
+
+        cmd_complete = (void *) hci_ev->data;
+        cmd_complete->num_packets = ble_ll_hci_get_num_cmd_pkts();
+        cmd_complete->opcode = htole16(opcode);
+        cmd_complete->status = (uint8_t) rc;
+    } else {
+        /* Create a command status event */
+        rc -= (BLE_ERR_MAX + 1);
+
+        hci_ev->opcode = BLE_HCI_EVCODE_COMMAND_STATUS;
+        hci_ev->length = sizeof(*cmd_status);
+
+        cmd_status = (void *) hci_ev->data;
+        cmd_status->status = (uint8_t)rc;
+        cmd_status->num_packets = ble_ll_hci_get_num_cmd_pkts();
+        cmd_status->opcode = htole16(opcode);
+    }
+
+    /* Count commands and those in error */
+    if (rc) {
+        STATS_INC(ble_ll_stats, hci_cmd_errs);
+    } else {
+        STATS_INC(ble_ll_stats, hci_cmds);
+    }
+
+    /* Send the event (events cannot be masked) */
+    ble_ll_hci_event_send(hci_ev);
+
+    /* Call post callback if set by command handler */
+    if (post_cb) {
+        post_cb();
+    }
+
+    BLE_LL_DEBUG_GPIO(HCI_CMD, 0);
+}
+
+/**
+ * Sends an HCI command to the controller.  On success, the supplied buffer is
+ * relinquished to the controller task.  On failure, the caller must free the
+ * buffer.
+ *
+ * @param cmd                   A flat buffer containing the HCI command to
+ *                                  send.
+ *
+ * @return                      0 on success;
+ *                              BLE_ERR_MEM_CAPACITY on HCI buffer exhaustion.
+ */
+int
+ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg)
+{
+    struct ble_npl_event *ev;
+
+    /* Get an event structure off the queue */
+    ev = &g_ble_ll_hci_cmd_ev;
+    if (ble_npl_event_is_queued(ev)) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* Fill out the event and post to Link Layer */
+    ble_npl_event_set_arg(ev, cmd);
+    ble_npl_eventq_put(&g_ble_ll_data.ll_evq, ev);
+
+    return 0;
+}
+
+/* Send ACL data from host to contoller */
+int
+ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg)
+{
+    ble_ll_acl_data_in(om);
+    return 0;
+}
+
+/**
+ * Initalize the LL HCI.
+ *
+ * NOTE: This function is called by the HCI RESET command so if any code
+ * is added here it must be OK to be executed when the reset command is used.
+ */
+void
+ble_ll_hci_init(void)
+{
+    BLE_LL_DEBUG_GPIO_INIT(HCI_CMD);
+    BLE_LL_DEBUG_GPIO_INIT(HCI_EV);
+
+    /* Set event callback for command processing */
+    ble_npl_event_init(&g_ble_ll_hci_cmd_ev, ble_ll_hci_cmd_proc, NULL);
+
+    /* Set defaults for LE events: Vol 2 Part E 7.8.1 */
+    g_ble_ll_hci_le_event_mask = 0x1f;
+
+    /* Set defaults for controller/baseband events: Vol 2 Part E 7.3.1 */
+    g_ble_ll_hci_event_mask = 0x1fffffffffff;
+
+
+    /* Set page 2 to 0 */
+    g_ble_ll_hci_event_mask2 = 0;
+
+    /* reset RF path compensation values */
+    rx_path_pwr_compensation = 0;
+    tx_path_pwr_compensation = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* after reset both legacy and extended advertising commands are allowed */
+    hci_adv_mode = ADV_MODE_ANY;
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci_ev.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci_ev.c
new file mode 100644
index 00000000..dbc50db9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci_ev.c
@@ -0,0 +1,522 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_ctrl.h"
+#include "ble_ll_conn_priv.h"
+
+#if (BLETEST_CONCURRENT_CONN_TEST == 1)
+extern void bletest_ltk_req_reply(uint16_t handle);
+#endif
+
+/**
+ * Send a data length change event for a connection to the host.
+ *
+ * @param connsm Pointer to connection state machine
+ */
+void
+ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm)
+{
+    struct ble_hci_ev_le_subev_data_len_chg *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_DATA_LEN_CHG)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_DATA_LEN_CHG;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            ev->max_tx_octets = htole16(connsm->eff_max_tx_octets);
+            ev->max_tx_time = htole16(connsm->eff_max_tx_time);
+            ev->max_rx_octets = htole16(connsm->eff_max_rx_octets);
+            ev->max_rx_time = htole16(connsm->eff_max_rx_time);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+/**
+ * Send a connection parameter request event for a connection to the host.
+ *
+ * @param connsm Pointer to connection state machine
+ */
+void
+ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm,
+                                struct ble_ll_conn_params *cp)
+{
+    struct ble_hci_ev_le_subev_rem_conn_param_req *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->min_interval = htole16(cp->interval_min);
+            ev->max_interval = htole16(cp->interval_max);
+            ev->latency = htole16(cp->latency);
+            ev->timeout = htole16(cp->timeout);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+/**
+ * Send a connection update event.
+ *
+ * @param connsm Pointer to connection state machine
+ * @param status The error code.
+ */
+void
+ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_conn_upd_complete *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->conn_itvl = htole16(connsm->conn_itvl);
+            ev->conn_latency = htole16(connsm->slave_latency);
+            ev->supervision_timeout = htole16(connsm->supervision_tmo);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+void
+ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_enc_key_refresh *ev_key_refresh;
+    struct ble_hci_ev_enrypt_chg *ev_enc_chf;
+    struct ble_hci_ev *hci_ev;
+
+    if (CONN_F_ENC_CHANGE_SENT(connsm) == 0) {
+        if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENCRYPT_CHG)) {
+            hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (hci_ev) {
+                hci_ev->opcode = BLE_HCI_EVCODE_ENCRYPT_CHG;
+                hci_ev->length = sizeof(*ev_enc_chf);
+                ev_enc_chf = (void *) hci_ev->data;
+
+                ev_enc_chf->status = status;
+                ev_enc_chf->connection_handle = htole16(connsm->conn_handle);
+                ev_enc_chf->enabled = (status == BLE_ERR_SUCCESS) ? 0x01 : 0x00;
+
+                ble_ll_hci_event_send(hci_ev);
+            }
+        }
+
+        CONN_F_ENC_CHANGE_SENT(connsm) = 1;
+        return;
+    }
+
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENC_KEY_REFRESH)) {
+        hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_ENC_KEY_REFRESH;
+            hci_ev->length = sizeof(*ev_key_refresh);
+            ev_key_refresh = (void *) hci_ev->data;
+
+            ev_key_refresh->status = status;
+            ev_key_refresh->conn_handle = htole16(connsm->conn_handle);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+/**
+ * Send a long term key request event for a connection to the host.
+ *
+ * @param connsm Pointer to connection state machine
+ */
+int
+ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm)
+{
+    struct ble_hci_ev_le_subev_lt_key_req *ev;
+    struct ble_hci_ev *hci_ev;
+    int rc;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_LT_KEY_REQ)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_LT_KEY_REQ;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->rand = htole64(connsm->enc_data.host_rand_num);
+            ev->div = htole16(connsm->enc_data.enc_div);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+        rc = 0;
+    } else {
+        rc = -1;
+    }
+
+#if (BLETEST_CONCURRENT_CONN_TEST == 1)
+    if (rc == 0) {
+        bletest_ltk_req_reply(connsm->conn_handle);
+    }
+#endif
+    return rc;
+}
+#endif
+
+void
+ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_rd_rem_used_feat *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->features[0] = connsm->conn_features;
+            memcpy(ev->features + 1, connsm->remote_features, 7);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+void
+ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_rd_rem_ver_info_cmp *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->version = connsm->vers_nr;
+            ev->manufacturer = htole16(connsm->comp_id);
+            ev->subversion = htole16(connsm->sub_vers_nr);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+/**
+ * Send a HW error to the host.
+ *
+ * @param hw_err
+ *
+ * @return int 0: event masked or event sent, -1 otherwise
+ */
+int
+ble_ll_hci_ev_hw_err(uint8_t hw_err)
+{
+    struct ble_hci_ev_hw_error *ev;
+    struct ble_hci_ev *hci_ev;
+    int rc;
+
+    rc = 0;
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_HW_ERROR)) {
+        hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_HW_ERROR;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->hw_code = hw_err;
+
+            ble_ll_hci_event_send(hci_ev);
+        } else {
+            rc = -1;
+        }
+    }
+    return rc;
+}
+
+void
+ble_ll_hci_ev_databuf_overflow(void)
+{
+    struct ble_hci_ev_data_buf_overflow *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_DATA_BUF_OVERFLOW)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_DATA_BUF_OVERFLOW;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->link_type = BLE_HCI_EVENT_ACL_BUF_OVERFLOW;
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+/**
+ * Send a LE Channel Selection Algorithm event.
+ *
+ * @param connsm Pointer to connection state machine
+ */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+void
+ble_ll_hci_ev_le_csa(struct ble_ll_conn_sm *connsm)
+{
+    struct ble_hci_ev_le_subev_chan_sel_alg *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CHAN_SEL_ALG)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_CHAN_SEL_ALG;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->csa = connsm->csmflags.cfbit.csa2_supp ? 0x01 : 0x00;
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+#endif
+
+/**
+ * Sends the LE Scan Request Received event
+ *
+ */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+void
+ble_ll_hci_ev_send_scan_req_recv(uint8_t adv_handle, const uint8_t *peer,
+                                 uint8_t peer_addr_type)
+{
+    struct ble_hci_ev_le_subev_scan_req_rcvd *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD;
+            ev->adv_handle = adv_handle;
+            ev->peer_addr_type = peer_addr_type;
+            memcpy(ev->peer_addr, peer, BLE_DEV_ADDR_LEN);
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+#endif
+
+/**
+ * Sends the  LE Scan Timeout Event
+ *
+ */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+void
+ble_ll_hci_ev_send_scan_timeout(void)
+{
+    struct ble_hci_ev_le_subev_scan_timeout *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_SCAN_TIMEOUT)) {
+        hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_SCAN_TIMEOUT;
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+#endif
+
+/**
+ * Sends the LE Advertising Set Terminated event
+ *
+ */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+void
+ble_ll_hci_ev_send_adv_set_terminated(uint8_t status, uint8_t adv_handle,
+                                      uint16_t conn_handle, uint8_t events)
+{
+    struct ble_hci_ev_le_subev_adv_set_terminated *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED;
+            ev->status = status;
+            ev->adv_handle = adv_handle;
+            ev->conn_handle = htole16(conn_handle);
+            ev->num_events = events;
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+#endif
+
+/**
+ * Send a PHY update complete event
+ *
+ * @param connsm Pointer to connection state machine
+ * @param status error status of event
+ */
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+int
+ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_phy_update_complete *ev;
+    struct ble_hci_ev *hci_ev;
+    int rc;
+
+    rc = 0;
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->tx_phy = connsm->phy_data.cur_tx_phy;
+            ev->rx_phy = connsm->phy_data.cur_rx_phy;
+
+            ble_ll_hci_event_send(hci_ev);
+        } else {
+            rc = BLE_ERR_MEM_CAPACITY;
+        }
+    }
+    return rc;
+}
+#endif
+
+void
+ble_ll_hci_ev_send_vendor_err(const char *file, uint32_t line)
+{
+    struct ble_hci_ev_vendor_debug *ev;
+    struct ble_hci_ev *hci_ev;
+    unsigned int str_len;
+    bool skip = true;
+    uint8_t digit;
+    int max_len;
+    int i;
+
+    /* 6 is for line number ":00000" , we assume files have no more than 64k of
+     * lines
+     */
+    max_len = BLE_HCI_MAX_DATA_LEN - sizeof(*ev) - 6;
+
+    hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    if (hci_ev) {
+        hci_ev->opcode = BLE_HCI_EVCODE_VENDOR_DEBUG;
+        hci_ev->length = sizeof(*ev);
+        ev = (void *) hci_ev->data;
+
+        /* Debug id for future use */
+        ev->id = 0x00;
+
+        /* snprintf would be nicer but this is heavy on flash
+         * len = snprintf((char *) ev->data, max_len, "%s:%u", file, line);
+         * if (len < 0) {
+         *     len = 0;
+         * } else if (len > max_len) {
+         *     len = max_len;
+         * }
+         *
+         *  hci_ev->length += len;
+         */
+        str_len = strlen(file);
+        if (str_len > max_len) {
+            str_len = max_len;
+        }
+
+        memcpy(ev->data, file, str_len);
+        ev->data[str_len++] = ':';
+
+        for (i = 100000; i >= 10; i /= 10) {
+            digit = (line % i) / (i/10);
+
+            if (!digit && skip) {
+                continue;
+            }
+
+            skip = false;
+            ev->data[str_len++] = '0' + digit;
+        }
+
+        hci_ev->length += str_len;
+
+        ble_ll_hci_event_send(hci_ev);
+    }
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_priv.h b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_priv.h
new file mode 100644
index 00000000..900950ef
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_priv.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_LL_PRIV_
+#define H_BLE_LL_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef MYNEWT
+
+#include "syscfg/syscfg.h"
+#include "hal/hal_gpio.h"
+
+#define BLE_LL_DEBUG_GPIO_INIT(_name)                                       \
+    if (MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name) >= 0) {                     \
+        hal_gpio_init_out(MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name), 0);      \
+    }
+
+#define BLE_LL_DEBUG_GPIO(_name, _val)                                      \
+    if (MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name) >= 0) {                     \
+        hal_gpio_write(MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name), !!(_val));  \
+    }
+
+#else
+#define BLE_LL_DEBUG_GPIO_INIT(_name)       (void)(0)
+#define BLE_LL_DEBUG_GPIO(_name, _val)      (void)(0)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_PRIV_ */
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rand.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rand.c
new file mode 100644
index 00000000..7b384e9d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rand.c
@@ -0,0 +1,185 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll.h"
+#if MYNEWT_VAL(TRNG)
+#include "trng/trng.h"
+#endif
+
+#if MYNEWT_VAL(TRNG)
+static struct trng_dev *g_trng;
+#else
+/* This is a simple circular buffer for holding N samples of random data */
+struct ble_ll_rnum_data
+{
+    uint8_t *rnd_in;
+    uint8_t *rnd_out;
+    volatile uint8_t rnd_size;
+};
+
+struct ble_ll_rnum_data g_ble_ll_rnum_data;
+uint8_t g_ble_ll_rnum_buf[MYNEWT_VAL(BLE_LL_RNG_BUFSIZE)];
+
+#define IS_RNUM_BUF_END(x)  \
+    (x == &g_ble_ll_rnum_buf[MYNEWT_VAL(BLE_LL_RNG_BUFSIZE) - 1])
+
+void
+ble_ll_rand_sample(uint8_t rnum)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    if (g_ble_ll_rnum_data.rnd_size < MYNEWT_VAL(BLE_LL_RNG_BUFSIZE)) {
+        ++g_ble_ll_rnum_data.rnd_size;
+        g_ble_ll_rnum_data.rnd_in[0] = rnum;
+        if (IS_RNUM_BUF_END(g_ble_ll_rnum_data.rnd_in)) {
+            g_ble_ll_rnum_data.rnd_in = g_ble_ll_rnum_buf;
+        } else {
+            ++g_ble_ll_rnum_data.rnd_in;
+        }
+    } else {
+        /* Stop generating random numbers as we are full */
+        ble_hw_rng_stop();
+    }
+    OS_EXIT_CRITICAL(sr);
+}
+#endif
+
+/* Get 'len' bytes of random data */
+int
+ble_ll_rand_data_get(uint8_t *buf, uint8_t len)
+{
+#if MYNEWT_VAL(TRNG)
+    size_t num;
+
+    while (len) {
+        num = trng_read(g_trng, buf, len);
+        buf += num;
+        len -= num;
+    }
+#else
+    uint8_t rnums;
+    os_sr_t sr;
+
+    while (len != 0) {
+        OS_ENTER_CRITICAL(sr);
+        rnums = g_ble_ll_rnum_data.rnd_size;
+        if (rnums > len) {
+            rnums = len;
+        }
+        len -= rnums;
+        g_ble_ll_rnum_data.rnd_size -= rnums;
+        while (rnums) {
+            buf[0] = g_ble_ll_rnum_data.rnd_out[0];
+            if (IS_RNUM_BUF_END(g_ble_ll_rnum_data.rnd_out)) {
+                g_ble_ll_rnum_data.rnd_out = g_ble_ll_rnum_buf;
+            } else {
+                ++g_ble_ll_rnum_data.rnd_out;
+            }
+            ++buf;
+            --rnums;
+        }
+        OS_EXIT_CRITICAL(sr);
+
+        /* Make sure rng is started! */
+        ble_hw_rng_start();
+
+        /* Wait till bytes are in buffer. */
+        if (len) {
+            while ((g_ble_ll_rnum_data.rnd_size < len) &&
+                   (g_ble_ll_rnum_data.rnd_size < MYNEWT_VAL(BLE_LL_RNG_BUFSIZE))) {
+                /* Spin here */
+            }
+        }
+    }
+#endif
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Called to obtain a "prand" as defined in core V4.2 Vol 6 Part B 1.3.2.2
+ *
+ * @param prand
+ */
+void
+ble_ll_rand_prand_get(uint8_t *prand)
+{
+    uint16_t sum;
+
+    while (1) {
+        /* Get 24 bits of random data */
+        ble_ll_rand_data_get(prand, 3);
+
+        /* Prand cannot be all zeros or 1's. */
+        sum = prand[0] + prand[1] + prand[2];
+        if ((sum != 0) && (sum != (3 * 0xff))) {
+            break;
+        }
+    }
+
+    /* Upper two bits must be 01 */
+    prand[2] &= ~0xc0;
+    prand[2] |= 0x40;
+}
+
+/**
+ * Start the generation of random numbers
+ *
+ * @return int
+ */
+int
+ble_ll_rand_start(void)
+{
+#if MYNEWT_VAL(TRNG)
+    /* Nothing to do - this is handled by driver */
+#else
+    /* Start the generation of numbers if we are not full */
+    if (g_ble_ll_rnum_data.rnd_size < MYNEWT_VAL(BLE_LL_RNG_BUFSIZE)) {
+        ble_hw_rng_start();
+    }
+#endif
+    return 0;
+}
+
+/**
+ * Initialize LL random number generation. Should be called only once on
+ * initialization.
+ *
+ * @return int
+ */
+int
+ble_ll_rand_init(void)
+{
+#if MYNEWT_VAL(TRNG)
+    g_trng = (struct trng_dev *) os_dev_open("trng", OS_TIMEOUT_NEVER, NULL);
+#else
+    g_ble_ll_rnum_data.rnd_in = g_ble_ll_rnum_buf;
+    g_ble_ll_rnum_data.rnd_out = g_ble_ll_rnum_buf;
+    ble_hw_rng_init(ble_ll_rand_sample, 1);
+#endif
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_resolv.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_resolv.c
new file mode 100644
index 00000000..02af9304
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_resolv.c
@@ -0,0 +1,753 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_sync.h"
+#include "controller/ble_hw.h"
+#include "ble_ll_conn_priv.h"
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+struct ble_ll_resolv_data
+{
+    uint8_t addr_res_enabled;
+    uint8_t rl_size;
+    uint8_t rl_cnt_hw;
+    uint8_t rl_cnt;
+    ble_npl_time_t rpa_tmo;
+    struct ble_npl_callout rpa_timer;
+};
+struct ble_ll_resolv_data g_ble_ll_resolv_data;
+
+__attribute__((aligned(4)))
+struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)];
+
+static int
+ble_ll_is_controller_busy(void)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (ble_ll_sync_enabled()) {
+        return 1;
+    }
+#endif
+
+    return ble_ll_adv_enabled() || ble_ll_scan_enabled() ||
+           g_ble_ll_conn_create_sm;
+}
+/**
+ * Called to determine if a change is allowed to the resolving list at this
+ * time. We are not allowed to modify the resolving list if address translation
+ * is enabled and we are either scanning, advertising, or attempting to create
+ * a connection.
+ *
+ * @return int 0: not allowed. 1: allowed.
+ */
+static int
+ble_ll_resolv_list_chg_allowed(void)
+{
+    int rc;
+
+    if (g_ble_ll_resolv_data.addr_res_enabled &&
+       ble_ll_is_controller_busy()) {
+        rc = 0;
+    } else {
+        rc = 1;
+    }
+    return rc;
+}
+
+
+/**
+ * Called to generate a resolvable private address in rl structure
+ *
+ * @param rl
+ * @param local
+ */
+static void
+ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local)
+{
+    uint8_t *irk;
+    uint8_t *prand;
+    struct ble_encryption_block ecb;
+    uint8_t *addr;
+
+    BLE_LL_ASSERT(rl != NULL);
+
+    if (local) {
+        addr = rl->rl_local_rpa;
+        irk = rl->rl_local_irk;
+    } else {
+        addr = rl->rl_peer_rpa;
+        irk = rl->rl_peer_irk;
+    }
+
+    /* Get prand */
+    prand = addr + 3;
+    ble_ll_rand_prand_get(prand);
+
+    /* Calculate hash, hash = ah(local IRK, prand) */
+    memcpy(ecb.key, irk, 16);
+    memset(ecb.plain_text, 0, 13);
+    ecb.plain_text[13] = prand[2];
+    ecb.plain_text[14] = prand[1];
+    ecb.plain_text[15] = prand[0];
+
+    /* Calculate hash */
+    ble_hw_encrypt_block(&ecb);
+
+    addr[0] = ecb.cipher_text[15];
+    addr[1] = ecb.cipher_text[14];
+    addr[2] = ecb.cipher_text[13];
+}
+
+/**
+ * Called when the Resolvable private address timer expires. This timer
+ * is used to regenerate local and peers RPA's in the resolving list.
+ */
+static void
+ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev)
+{
+    int i;
+    os_sr_t sr;
+    struct ble_ll_resolv_entry *rl;
+
+    rl = &g_ble_ll_resolv_list[0];
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
+        if (rl->rl_has_local) {
+            OS_ENTER_CRITICAL(sr);
+            ble_ll_resolv_gen_priv_addr(rl, 1);
+            OS_EXIT_CRITICAL(sr);
+        }
+
+        if (rl->rl_has_peer) {
+            OS_ENTER_CRITICAL(sr);
+            ble_ll_resolv_gen_priv_addr(rl, 0);
+            OS_EXIT_CRITICAL(sr);
+        }
+        ++rl;
+    }
+
+    ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
+                          g_ble_ll_resolv_data.rpa_tmo);
+
+    ble_ll_adv_rpa_timeout();
+}
+
+/**
+ * Called to determine if the IRK is all zero.
+ *
+ * @param irk
+ *
+ * @return int 0: IRK is zero . 1: IRK has non-zero value.
+ */
+static int
+ble_ll_resolv_irk_nonzero(const uint8_t *irk)
+{
+    int i;
+    int rc;
+
+    rc = 0;
+    for (i = 0; i < 16; ++i) {
+        if (*irk != 0) {
+            rc = 1;
+            break;
+        }
+        ++irk;
+    }
+
+    return rc;
+}
+
+/**
+ * Clear the resolving list
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_ll_resolv_list_clr(void)
+{
+    /* Check proper state */
+    if (!ble_ll_resolv_list_chg_allowed()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Sets total on list to 0. Clears HW resolve list */
+    g_ble_ll_resolv_data.rl_cnt_hw = 0;
+    g_ble_ll_resolv_data.rl_cnt = 0;
+    ble_hw_resolv_list_clear();
+
+    /* stop RPA timer when clearing RL */
+    ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Read the size of the resolving list. This is the total number of resolving
+ * list entries allowed by the controller.
+ *
+ * @param rspbuf Pointer to response buffer
+ *
+ * @return int 0: success.
+ */
+int
+ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_resolv_list_size_rp *rsp = (void *) rspbuf;
+
+    rsp->size = g_ble_ll_resolv_data.rl_size;
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Used to determine if the device is on the resolving list.
+ *
+ * @param addr
+ * @param addr_type Public address (0) or random address (1)
+ *
+ * @return int 0: device is not on resolving list; otherwise the return value
+ * is the 'position' of the device in the resolving list (the index of the
+ * element plus 1).
+ */
+static int
+ble_ll_is_on_resolv_list(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    struct ble_ll_resolv_entry *rl;
+
+    rl = &g_ble_ll_resolv_list[0];
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
+        if ((rl->rl_addr_type == addr_type) &&
+            (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) {
+            return i + 1;
+        }
+        ++rl;
+    }
+
+    return 0;
+}
+
+/**
+ * Used to determine if the device is on the resolving list.
+ *
+ * @param addr
+ * @param addr_type Public address (0) or random address (1)
+ *
+ * @return Pointer to resolving list entry or NULL if no entry found.
+ */
+struct ble_ll_resolv_entry *
+ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    struct ble_ll_resolv_entry *rl;
+
+    rl = &g_ble_ll_resolv_list[0];
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
+        if ((rl->rl_addr_type == addr_type) &&
+            (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) {
+            return rl;
+        }
+        ++rl;
+    }
+
+    return NULL;
+}
+
+/**
+ * Add a device to the resolving list
+ *
+ * @return int
+ */
+int
+ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_add_resolv_list_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_resolv_entry *rl;
+    int rc = BLE_ERR_SUCCESS;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Must be in proper state */
+    if (!ble_ll_resolv_list_chg_allowed()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Check if we have any open entries */
+    if (g_ble_ll_resolv_data.rl_cnt >= g_ble_ll_resolv_data.rl_size) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* spec is not clear on how to handle this but make sure host is aware
+     * that new keys are not used in that case
+     */
+    if (ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* we keep this sorted in a way that entries with peer_irk are first */
+    if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) {
+        memmove(&g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw + 1],
+                &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw],
+                (g_ble_ll_resolv_data.rl_cnt - g_ble_ll_resolv_data.rl_cnt_hw) *
+                sizeof(g_ble_ll_resolv_list[0]));
+        rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw];
+    } else {
+        rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt];
+    }
+
+    memset (rl, 0, sizeof(*rl));
+    rl->rl_addr_type = cmd->peer_addr_type;
+    memcpy(rl->rl_identity_addr, cmd->peer_id_addr, BLE_DEV_ADDR_LEN);
+
+    if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) {
+        swap_buf(rl->rl_peer_irk, cmd->peer_irk, 16);
+        rl->rl_has_peer = 1;
+
+        /* generate peer RPA now, those will be updated by timer when
+         * resolution is enabled
+         */
+        ble_ll_resolv_gen_priv_addr(rl, 0);
+    }
+
+    if (ble_ll_resolv_irk_nonzero(cmd->local_irk)) {
+        swap_buf(rl->rl_local_irk, cmd->local_irk, 16);
+        rl->rl_has_local = 1;
+
+        /* generate local RPA now, those will be updated by timer when
+         * resolution is enabled
+         */
+        ble_ll_resolv_gen_priv_addr(rl, 1);
+    }
+
+    /* By default use privacy network mode */
+    rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK;
+
+    /* Add peers IRKs to HW resolving list. Should always succeed since we
+     * already checked if there is room for it.
+     */
+    if (rl->rl_has_peer) {
+        rc = ble_hw_resolv_list_add(rl->rl_peer_irk);
+        BLE_LL_ASSERT(rc == BLE_ERR_SUCCESS);
+        g_ble_ll_resolv_data.rl_cnt_hw++;
+    }
+
+    g_ble_ll_resolv_data.rl_cnt++;
+
+    /* start RPA timer if this was first element added to RL */
+    if (g_ble_ll_resolv_data.rl_cnt == 1) {
+        ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
+                              g_ble_ll_resolv_data.rpa_tmo);
+    }
+
+    return rc;
+}
+
+/**
+ * Remove a device from the resolving list
+ *
+ * @param cmdbuf
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rmv_resolve_list_cp *cmd = (const void *) cmdbuf;
+    int position;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Must be in proper state */
+    if (!ble_ll_resolv_list_chg_allowed()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Remove from IRK records */
+    position = ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type);
+    if (position) {
+        BLE_LL_ASSERT(position <= g_ble_ll_resolv_data.rl_cnt);
+
+        memmove(&g_ble_ll_resolv_list[position - 1],
+                &g_ble_ll_resolv_list[position],
+                (g_ble_ll_resolv_data.rl_cnt - position) *
+                sizeof(g_ble_ll_resolv_list[0]));
+        g_ble_ll_resolv_data.rl_cnt--;
+
+        /* Remove from HW list */
+        if (position <= g_ble_ll_resolv_data.rl_cnt_hw) {
+            ble_hw_resolv_list_rmv(position - 1);
+            g_ble_ll_resolv_data.rl_cnt_hw--;
+        }
+
+        /* stop RPA timer if list is empty */
+        if (g_ble_ll_resolv_data.rl_cnt == 0) {
+            ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
+        }
+
+        return BLE_ERR_SUCCESS;
+    }
+
+    return BLE_ERR_UNK_CONN_ID;
+}
+
+/**
+ * Called to enable or disable address resolution in the controller
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_addr_res_en_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_is_controller_busy()) {
+        return  BLE_ERR_CMD_DISALLOWED;
+
+    }
+
+    if (cmd->enable > 1) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    g_ble_ll_resolv_data.addr_res_enabled = cmd->enable;
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                           uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_rd_peer_recolv_addr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rd_peer_recolv_addr_rp *rsp = (void *) rspbuf;
+    struct ble_ll_resolv_entry *rl;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type);
+    if (rl) {
+        memcpy(rsp->rpa, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN);
+        rc = BLE_ERR_UNK_CONN_ID;
+    }
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+int
+ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                            uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_rd_local_recolv_addr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rd_local_recolv_addr_rp *rsp = (void *) rspbuf;
+    struct ble_ll_resolv_entry *rl;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type);
+    if (rl) {
+        memcpy(rsp->rpa, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN);
+        rc = BLE_ERR_UNK_CONN_ID;
+    }
+
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+
+/**
+ * Set the resolvable private address timeout.
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_rpa_tmo_cp *cmd = (const void *)cmdbuf;
+    uint16_t tmo_secs;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    tmo_secs = le16toh(cmd->rpa_timeout);
+    if (!((tmo_secs > 0) && (tmo_secs <= 0xA1B8))) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(tmo_secs * 1000);
+
+    /* restart timer if there is something on RL */
+    if (g_ble_ll_resolv_data.rl_cnt) {
+        ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
+                              g_ble_ll_resolv_data.rpa_tmo);
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_privacy_mode_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_resolv_entry *rl;
+
+    if (ble_ll_is_controller_busy()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_id_addr_type);
+    if (!rl) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    if (cmd->mode > BLE_HCI_PRIVACY_DEVICE) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rl->rl_priv_mode = cmd->mode;
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Returns the Resolvable Private address timeout, in os ticks
+ *
+ *
+ * @return uint32_t
+ */
+uint32_t
+ble_ll_resolv_get_rpa_tmo(void)
+{
+    return g_ble_ll_resolv_data.rpa_tmo;
+}
+
+void
+ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local,
+                            uint8_t *addr)
+{
+    os_sr_t sr;
+
+    BLE_LL_ASSERT(rl != NULL);
+    BLE_LL_ASSERT(addr != NULL);
+
+    OS_ENTER_CRITICAL(sr);
+    if (local) {
+        BLE_LL_ASSERT(rl->rl_has_local);
+        memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
+    } else {
+        BLE_LL_ASSERT(rl->rl_has_peer);
+        memcpy(addr, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+void
+ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa)
+{
+    os_sr_t sr;
+    struct ble_ll_resolv_entry *rl;
+
+    OS_ENTER_CRITICAL(sr);
+    rl = &g_ble_ll_resolv_list[index];
+    memcpy(rl->rl_peer_rpa, rpa, BLE_DEV_ADDR_LEN);
+    OS_EXIT_CRITICAL(sr);
+}
+
+void
+ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa)
+{
+    os_sr_t sr;
+    struct ble_ll_resolv_entry *rl;
+
+    OS_ENTER_CRITICAL(sr);
+    rl = &g_ble_ll_resolv_list[index];
+    memcpy(rl->rl_local_rpa, rpa, BLE_DEV_ADDR_LEN);
+    OS_EXIT_CRITICAL(sr);
+}
+
+/**
+ * Generate a resolvable private address.
+ *
+ * @param addr
+ * @param addr_type
+ * @param rpa
+ *
+ * @return int
+ */
+int
+ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local)
+{
+    struct ble_ll_resolv_entry *rl;
+
+    rl = ble_ll_resolv_list_find(addr, addr_type);
+    if (rl) {
+        if ((local && rl->rl_has_local) || (!local && rl->rl_has_peer)) {
+            ble_ll_resolv_get_priv_addr(rl, local, rpa);
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Resolve a Resolvable Private Address
+ *
+ * @param rpa
+ * @param index
+ *
+ * @return int
+ */
+int
+ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk)
+{
+    int rc;
+    const uint32_t *irk32;
+    uint32_t *key32;
+    uint32_t *pt32;
+    struct ble_encryption_block ecb;
+
+    irk32 = (const uint32_t *)irk;
+    key32 = (uint32_t *)&ecb.key[0];
+
+    key32[0] = irk32[0];
+    key32[1] = irk32[1];
+    key32[2] = irk32[2];
+    key32[3] = irk32[3];
+
+    pt32 = (uint32_t *)&ecb.plain_text[0];
+    pt32[0] = 0;
+    pt32[1] = 0;
+    pt32[2] = 0;
+    pt32[3] = 0;
+
+    ecb.plain_text[15] = rpa[3];
+    ecb.plain_text[14] = rpa[4];
+    ecb.plain_text[13] = rpa[5];
+
+    ble_hw_encrypt_block(&ecb);
+    if ((ecb.cipher_text[15] == rpa[0]) && (ecb.cipher_text[14] == rpa[1]) &&
+        (ecb.cipher_text[13] == rpa[2])) {
+        rc = 1;
+    } else {
+        rc = 0;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_resolv_peer_rpa_any(const uint8_t *rpa)
+{
+    int i;
+
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt_hw; i++) {
+        if (ble_ll_resolv_rpa(rpa, g_ble_ll_resolv_list[i].rl_peer_irk)) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+/**
+ * Returns whether or not address resolution is enabled.
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_ll_resolv_enabled(void)
+{
+    return g_ble_ll_resolv_data.addr_res_enabled;
+}
+
+/**
+ * Called to reset private address resolution module.
+ */
+void
+ble_ll_resolv_list_reset(void)
+{
+    g_ble_ll_resolv_data.addr_res_enabled = 0;
+    ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
+    ble_ll_resolv_list_clr();
+    ble_ll_resolv_init();
+}
+
+void
+ble_ll_resolv_init(void)
+{
+    uint8_t hw_size;
+
+    /* Default is 15 minutes */
+    g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(15 * 60 * 1000);
+
+    hw_size = ble_hw_resolv_list_size();
+    if (hw_size > MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)) {
+        hw_size = MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE);
+    }
+    g_ble_ll_resolv_data.rl_size = hw_size;
+
+    ble_npl_callout_init(&g_ble_ll_resolv_data.rpa_timer,
+                         &g_ble_ll_data.ll_evq,
+                         ble_ll_resolv_rpa_timer_cb,
+                         NULL);
+}
+
+#endif  /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) */
+
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rfmgmt.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rfmgmt.c
new file mode 100644
index 00000000..3bf5d5fa
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rfmgmt.c
@@ -0,0 +1,346 @@
+/*
+ * 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 <stddef.h>
+#include <assert.h>
+#include <stddef.h>
+#include "syscfg/syscfg.h"
+#include "os/os_cputime.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_rfmgmt.h"
+
+#if MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME) > 0
+
+enum ble_ll_rfmgmt_state {
+    RFMGMT_STATE_OFF = 0,
+    RFMGMT_STATE_ENABLING = 1,
+    RFMGMT_STATE_ENABLED = 2,
+};
+
+struct ble_ll_rfmgmt_data {
+    enum ble_ll_rfmgmt_state state;
+    uint16_t ticks_to_enabled;
+
+    struct hal_timer timer;
+    bool timer_scheduled;
+    uint32_t timer_scheduled_at;
+
+    bool enable_scan;
+    bool enable_sched;
+    uint32_t enable_scan_at;
+    uint32_t enable_sched_at;
+
+    uint32_t enabled_at;
+
+    struct ble_npl_event release_ev;
+};
+
+static struct ble_ll_rfmgmt_data g_ble_ll_rfmgmt_data;
+
+static void
+ble_ll_rfmgmt_enable(void)
+{
+    OS_ASSERT_CRITICAL();
+
+    if (g_ble_ll_rfmgmt_data.state == RFMGMT_STATE_OFF) {
+        g_ble_ll_rfmgmt_data.state = RFMGMT_STATE_ENABLING;
+        g_ble_ll_rfmgmt_data.enabled_at = os_cputime_get32();
+        ble_phy_rfclk_enable();
+    }
+}
+
+static void
+ble_ll_rfmgmt_disable(void)
+{
+    OS_ASSERT_CRITICAL();
+
+    if (g_ble_ll_rfmgmt_data.state != RFMGMT_STATE_OFF) {
+        ble_phy_rfclk_disable();
+        g_ble_ll_rfmgmt_data.state = RFMGMT_STATE_OFF;
+    }
+}
+
+static void
+ble_ll_rfmgmt_timer_reschedule(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t enable_at;
+
+    /* Figure out when we need to enable RF */
+    if (rfmgmt->enable_scan && rfmgmt->enable_sched) {
+        if (CPUTIME_LT(rfmgmt->enable_scan_at, rfmgmt->enable_sched_at)) {
+            enable_at = rfmgmt->enable_scan_at;
+        } else {
+            enable_at = rfmgmt->enable_sched_at;
+        }
+    } else if (rfmgmt->enable_scan) {
+        enable_at = rfmgmt->enable_scan_at;
+    } else if (rfmgmt->enable_sched) {
+        enable_at = rfmgmt->enable_sched_at;
+    } else {
+        rfmgmt->timer_scheduled = false;
+        os_cputime_timer_stop(&rfmgmt->timer);
+        return;
+    }
+
+    if (rfmgmt->timer_scheduled) {
+        /*
+         * If there is timer already scheduled at the same time we do not need
+         * to do anything. Otherwise we need to stop timer and schedule it again
+         * regardless if it's earlier or later to make sure it fires at the time
+         * something expects it.
+         */
+
+        if (rfmgmt->timer_scheduled_at == enable_at) {
+            return;
+        }
+
+        rfmgmt->timer_scheduled = false;
+        os_cputime_timer_stop(&rfmgmt->timer);
+    }
+
+    /*
+     * In case timer was requested to be enabled before current time, just make
+     * sure it's enabled and assume caller can deal with this. This will happen
+     * if something is scheduled "now" since "enable_at" is in the past, but in
+     * such case it's absolutely harmless since we already have clock enabled
+     * and this will do nothing.
+     */
+    if (CPUTIME_LEQ(enable_at, os_cputime_get32())) {
+        ble_ll_rfmgmt_enable();
+        return;
+    }
+
+    rfmgmt->timer_scheduled = true;
+    rfmgmt->timer_scheduled_at = enable_at;
+    os_cputime_timer_start(&rfmgmt->timer, enable_at);
+}
+
+static void
+ble_ll_rfmgmt_timer_exp(void *arg)
+{
+    g_ble_ll_rfmgmt_data.timer_scheduled = false;
+    ble_ll_rfmgmt_enable();
+}
+
+static void
+ble_ll_rfmgmt_release_ev(struct ble_npl_event *ev)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t now;
+    bool can_disable;
+    uint8_t lls;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    now = os_cputime_get32();
+
+    can_disable = true;
+    lls = ble_ll_state_get();
+
+    if (rfmgmt->enable_scan && CPUTIME_GEQ(now, rfmgmt->enable_scan_at)) {
+        /* Blocked by scan */
+        can_disable = false;
+    } else if (rfmgmt->enable_sched && CPUTIME_GEQ(now, rfmgmt->enable_sched_at)) {
+        /* Blocked by scheduler item */
+        can_disable = false;
+    } else if (lls != BLE_LL_STATE_STANDBY) {
+        /* Blocked by LL state */
+        can_disable = false;
+    }
+
+    if (can_disable) {
+        ble_ll_rfmgmt_disable();
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+static uint32_t
+ble_ll_rfmgmt_ticks_to_enabled(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t rem_ticks;
+    uint32_t now;
+
+    switch (rfmgmt->state) {
+    case RFMGMT_STATE_OFF:
+        rem_ticks = rfmgmt->ticks_to_enabled;
+        break;
+    case RFMGMT_STATE_ENABLING:
+        now = os_cputime_get32();
+        if (CPUTIME_LT(now, rfmgmt->enabled_at + rfmgmt->ticks_to_enabled)) {
+            rem_ticks = rfmgmt->enabled_at + rfmgmt->ticks_to_enabled - now;
+            break;
+        }
+        rfmgmt->state = RFMGMT_STATE_ENABLED;
+        /* no break */
+    case RFMGMT_STATE_ENABLED:
+        rem_ticks = 0;
+        break;
+    default:
+        BLE_LL_ASSERT(0);
+        rem_ticks = 0;
+        break;
+    }
+
+    return rem_ticks;
+}
+
+void
+ble_ll_rfmgmt_init(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+
+    rfmgmt->state = RFMGMT_STATE_OFF;
+
+    rfmgmt->ticks_to_enabled =
+            ble_ll_usecs_to_ticks_round_up(MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME));
+
+    rfmgmt->timer_scheduled = false;
+    os_cputime_timer_init(&rfmgmt->timer, ble_ll_rfmgmt_timer_exp, NULL);
+
+    ble_npl_event_init(&rfmgmt->release_ev, ble_ll_rfmgmt_release_ev, NULL);
+}
+
+void
+ble_ll_rfmgmt_reset(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+
+    rfmgmt->timer_scheduled = false;
+    rfmgmt->timer_scheduled_at = 0;
+    os_cputime_timer_stop(&rfmgmt->timer);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev);
+
+    ble_ll_rfmgmt_disable();
+
+    rfmgmt->enable_scan = false;
+    rfmgmt->enable_scan_at = 0;
+    rfmgmt->enable_sched = false;
+    rfmgmt->enable_sched_at = 0;
+
+    rfmgmt->enabled_at = 0;
+}
+
+void
+ble_ll_rfmgmt_scan_changed(bool enabled, uint32_t next_window)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    rfmgmt->enable_scan = enabled;
+    rfmgmt->enable_scan_at = next_window - rfmgmt->ticks_to_enabled;
+
+    ble_ll_rfmgmt_timer_reschedule();
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+void
+ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *first)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    rfmgmt->enable_sched = (first != NULL);
+    if (first) {
+        rfmgmt->enable_sched_at = first->start_time - rfmgmt->ticks_to_enabled;
+    }
+
+    ble_ll_rfmgmt_timer_reschedule();
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+void
+ble_ll_rfmgmt_release(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev);
+
+    if (g_ble_ll_rfmgmt_data.state != RFMGMT_STATE_OFF) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+uint32_t
+ble_ll_rfmgmt_enable_now(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t enabled_at;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    ble_ll_rfmgmt_enable();
+
+    if (rfmgmt->state == RFMGMT_STATE_ENABLED) {
+        enabled_at = os_cputime_get32();
+    } else {
+        enabled_at = rfmgmt->enabled_at + rfmgmt->ticks_to_enabled + 1;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    return enabled_at;
+}
+
+bool
+ble_ll_rfmgmt_is_enabled(void)
+{
+    bool ret;
+
+    OS_ASSERT_CRITICAL();
+
+    ret = ble_ll_rfmgmt_ticks_to_enabled() == 0;
+
+    return ret;
+}
+
+#else
+
+void
+ble_ll_rfmgmt_init(void)
+{
+    static bool enabled = false;
+
+    if (!enabled) {
+        ble_phy_rfclk_enable();
+    }
+
+    enabled = true;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_scan.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_scan.c
new file mode 100644
index 00000000..2e93ba23
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_scan.c
@@ -0,0 +1,3979 @@
+/*
+ * 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 <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "os/os_cputime.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "controller/ble_ll_trace.h"
+#include "controller/ble_ll_sync.h"
+#include "ble_ll_conn_priv.h"
+
+/*
+ * XXX:
+ * 1) I think I can guarantee that we dont process things out of order if
+ * I send an event when a scan request is sent. The scan_rsp_pending flag
+ * code might be made simpler.
+ *
+ * 2) Interleave sending scan requests to different advertisers? I guess I need
+ * a list of advertisers to which I sent a scan request and have yet to
+ * receive a scan response from? Implement this.
+ */
+
+/* Dont allow more than 255 of these entries */
+#if MYNEWT_VAL(BLE_LL_NUM_SCAN_RSP_ADVS) > 255
+    #error "Cannot have more than 255 scan response entries!"
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#define SCAN_VALID_PHY_MASK     (BLE_HCI_LE_PHY_1M_PREF_MASK | BLE_HCI_LE_PHY_CODED_PREF_MASK)
+#else
+#define SCAN_VALID_PHY_MASK     (BLE_HCI_LE_PHY_1M_PREF_MASK)
+#endif
+
+/* The scanning parameters set by host */
+static struct ble_ll_scan_params g_ble_ll_scan_params[BLE_LL_SCAN_PHY_NUMBER];
+
+/* The scanning state machine global object */
+static struct ble_ll_scan_sm g_ble_ll_scan_sm;
+
+struct ble_ll_ext_adv_hdr
+{
+    uint8_t mode;
+    uint8_t hdr_len;
+    uint8_t hdr[0];
+};
+
+struct ble_ll_scan_addr_data {
+    bool adva_present;
+    uint8_t adva_type;
+    uint8_t *adva;
+    uint8_t targeta_type;
+    uint8_t *targeta;
+    uint8_t adv_addr_type;
+    uint8_t *adv_addr;
+    struct ble_ll_resolv_entry *rl;
+};
+
+/*
+ * Structure used to store advertisers. This is used to limit sending scan
+ * requests to the same advertiser and also to filter duplicate events sent
+ * to the host.
+ */
+struct ble_ll_scan_advertisers
+{
+    uint16_t            sc_adv_flags;
+    uint16_t            adi;
+    struct ble_dev_addr adv_addr;
+};
+
+#define BLE_LL_SC_ADV_F_RANDOM_ADDR     (0x01)
+#define BLE_LL_SC_ADV_F_SCAN_RSP_RXD    (0x02)
+#define BLE_LL_SC_ADV_F_DIRECT_RPT_SENT (0x04)
+#define BLE_LL_SC_ADV_F_ADV_RPT_SENT    (0x08)
+#define BLE_LL_SC_ADV_F_SCAN_RSP_SENT   (0x10)
+
+/* Contains list of advertisers that we have heard scan responses from */
+static uint8_t g_ble_ll_scan_num_rsp_advs;
+struct ble_ll_scan_advertisers
+g_ble_ll_scan_rsp_advs[MYNEWT_VAL(BLE_LL_NUM_SCAN_RSP_ADVS)];
+
+/* Duplicates filtering data */
+#define BLE_LL_SCAN_ENTRY_TYPE_LEGACY(addr_type) \
+    ((addr_type) & 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_LL_SCAN_ENTRY_TYPE_EXT(addr_type, has_aux, is_anon, adi) \
+    (((adi >> 8) & 0xF0) | (1 << 3) | (is_anon << 2) | (has_aux << 1) | ((addr_type) & 1))
+#endif
+
+#define BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT       (0x01)
+#define BLE_LL_SCAN_DUP_F_DIR_ADV_REPORT_SENT   (0x02)
+#define BLE_LL_SCAN_DUP_F_SCAN_RSP_SENT         (0x04)
+
+struct ble_ll_scan_dup_entry {
+    uint8_t type;       /* entry type, see BLE_LL_SCAN_ENTRY_TYPE_* */
+    uint8_t addr[6];
+    uint8_t flags;      /* use BLE_LL_SCAN_DUP_F_xxx */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    uint16_t adi;
+#endif
+    TAILQ_ENTRY(ble_ll_scan_dup_entry) link;
+};
+
+static os_membuf_t g_scan_dup_mem[ OS_MEMPOOL_SIZE(
+                                   MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS),
+                                   sizeof(struct ble_ll_scan_dup_entry)) ];
+static struct os_mempool g_scan_dup_pool;
+static TAILQ_HEAD(ble_ll_scan_dup_list, ble_ll_scan_dup_entry) g_scan_dup_list;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#if MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT) != 0
+static os_membuf_t ext_scan_aux_mem[ OS_MEMPOOL_SIZE(
+                    MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT),
+                    sizeof (struct ble_ll_aux_data))
+];
+#else
+#define ext_scan_aux_mem NULL
+#endif
+
+static struct os_mempool ext_scan_aux_pool;
+
+static int ble_ll_scan_start(struct ble_ll_scan_sm *scansm,
+                             struct ble_ll_sched_item *sch);
+
+static void
+ble_ll_aux_scan_drop_event_cb(struct ble_npl_event *ev)
+{
+    struct ble_ll_aux_data *aux_data = ble_npl_event_get_arg(ev);
+
+    ble_ll_scan_end_adv_evt(aux_data);
+    ble_ll_scan_aux_data_unref(aux_data);
+}
+
+static void
+ble_ll_aux_scan_drop(struct ble_ll_aux_data *aux_data)
+{
+    BLE_LL_ASSERT(aux_data);
+
+    STATS_INC(ble_ll_stats, aux_scan_drop);
+
+    ble_npl_event_init(&aux_data->ev, ble_ll_aux_scan_drop_event_cb, aux_data);
+    ble_ll_event_send(&aux_data->ev);
+}
+
+static int
+ble_ll_aux_scan_cb(struct ble_ll_sched_item *sch)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    uint8_t lls = ble_ll_state_get();
+    uint32_t wfr_usec;
+
+    STATS_INC(ble_ll_stats, aux_sched_cb);
+
+    /* Drop the scheduled item if scan was disable or there is aux or scan
+     * response pending
+     */
+    if (!scansm->scan_enabled || scansm->cur_aux_data ||
+            scansm->scan_rsp_pending) {
+        ble_ll_aux_scan_drop(sch->cb_arg);
+        sch->cb_arg = NULL;
+        goto done;
+    }
+
+    /* Check if there is no aux connect sent. If so drop the sched item */
+    if (lls == BLE_LL_STATE_INITIATING && ble_ll_conn_init_pending_aux_conn_rsp()) {
+        ble_ll_aux_scan_drop(sch->cb_arg);
+        sch->cb_arg = NULL;
+        goto done;
+    }
+
+    /* This function is called only when scanner is running. This can happen
+     *  in 3 states:
+     * BLE_LL_STATE_SCANNING
+     * BLE_LL_STATE_INITIATING
+     * BLE_LL_STATE_STANDBY
+     */
+    if (lls != BLE_LL_STATE_STANDBY) {
+        ble_phy_disable();
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    }
+
+    /* When doing RX for AUX pkt, cur_aux_data keeps valid aux data */
+    scansm->cur_aux_data = sch->cb_arg;
+    sch->cb_arg = NULL;
+    BLE_LL_ASSERT(scansm->cur_aux_data != NULL);
+    scansm->cur_aux_data->scanning = 1;
+
+    if (ble_ll_scan_start(scansm, sch)) {
+        ble_ll_scan_interrupted(scansm);
+        goto done;
+    }
+
+    STATS_INC(ble_ll_stats, aux_fired_for_read);
+
+    wfr_usec = scansm->cur_aux_data->offset_units ? 300 : 30;
+    ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usec);
+
+done:
+
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static int
+ble_ll_scan_ext_adv_init(struct ble_ll_aux_data **aux_data)
+{
+    struct ble_ll_aux_data *e;
+
+    e = os_memblock_get(&ext_scan_aux_pool);
+    if (!e) {
+        return -1;
+    }
+
+    memset(e, 0, sizeof(*e));
+    e->sch.sched_cb = ble_ll_aux_scan_cb;
+    e->sch.sched_type = BLE_LL_SCHED_TYPE_AUX_SCAN;
+    e->ref_cnt = 1;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    e->rpa_index = -1;
+#endif
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_REF, (uint32_t)e, e->ref_cnt);
+
+    *aux_data = e;
+    STATS_INC(ble_ll_stats, aux_allocated);
+
+    return 0;
+}
+#endif
+
+static inline uint32_t
+ble_ll_scan_time_hci_to_ticks(uint16_t value)
+{
+    return os_cputime_usecs_to_ticks(value * BLE_HCI_SCAN_ITVL);
+}
+
+/* See Vol 6 Part B Section 4.4.3.2. Active scanning backoff */
+static void
+ble_ll_scan_req_backoff(struct ble_ll_scan_sm *scansm, int success)
+{
+    BLE_LL_ASSERT(scansm->backoff_count == 0);
+    BLE_LL_ASSERT(scansm->scan_rsp_pending == 0);
+
+    if (success) {
+        scansm->scan_rsp_cons_fails = 0;
+        ++scansm->scan_rsp_cons_ok;
+        if (scansm->scan_rsp_cons_ok == 2) {
+            scansm->scan_rsp_cons_ok = 0;
+            if (scansm->upper_limit > 1) {
+                scansm->upper_limit >>= 1;
+            }
+        }
+        STATS_INC(ble_ll_stats, scan_req_txg);
+    } else {
+        scansm->scan_rsp_cons_ok = 0;
+        ++scansm->scan_rsp_cons_fails;
+        if (scansm->scan_rsp_cons_fails == 2) {
+            scansm->scan_rsp_cons_fails = 0;
+            if (scansm->upper_limit < 256) {
+                scansm->upper_limit <<= 1;
+            }
+        }
+        STATS_INC(ble_ll_stats, scan_req_txf);
+    }
+
+    scansm->backoff_count = rand() & (scansm->upper_limit - 1);
+    ++scansm->backoff_count;
+    BLE_LL_ASSERT(scansm->backoff_count <= 256);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+static void
+ble_ll_scan_refresh_nrpa(struct ble_ll_scan_sm *scansm)
+{
+    ble_npl_time_t now;
+
+    now = ble_npl_time_get();
+    if ((ble_npl_stime_t)(now - scansm->scan_nrpa_timer) >= 0) {
+        /* Generate new NRPA */
+        ble_ll_rand_data_get(scansm->scan_nrpa, BLE_DEV_ADDR_LEN);
+        scansm->scan_nrpa[5] &= ~0xc0;
+
+        /* We'll use the same timeout as for RPA rotation */
+        scansm->scan_nrpa_timer = now + ble_ll_resolv_get_rpa_tmo();
+    }
+}
+#endif
+
+static void
+ble_ll_scan_req_pdu_prepare(struct ble_ll_scan_sm *scansm,
+                            const uint8_t *adv_addr, uint8_t adv_addr_type,
+                            struct ble_ll_resolv_entry *rl)
+{
+    uint8_t hdr_byte;
+    struct ble_ll_scan_pdu_data *pdu_data;
+    uint8_t *scana;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    uint8_t rpa[BLE_DEV_ADDR_LEN];
+#endif
+
+    pdu_data = &scansm->pdu_data;
+
+    /* Construct first PDU header byte */
+    hdr_byte = BLE_ADV_PDU_TYPE_SCAN_REQ;
+    if (adv_addr_type) {
+        hdr_byte |= BLE_ADV_PDU_HDR_RXADD_RAND;
+    }
+
+    /* Determine ScanA */
+    if (scansm->own_addr_type & 0x01) {
+        hdr_byte |= BLE_ADV_PDU_HDR_TXADD_RAND;
+        scana = g_random_addr;
+    } else {
+        scana = g_dev_addr;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (scansm->own_addr_type & 0x02) {
+        /*
+         * If device is on RL and we have local IRK, we use RPA generated using
+         * that IRK as ScanA. Otherwise we use NRPA as ScanA to prevent our
+         * device from being tracked when doing an active scan (Core 5.1, Vol 6,
+         * Part B, section 6.3)
+         */
+        if (rl && rl->rl_has_local) {
+            ble_ll_resolv_get_priv_addr(rl, 1, rpa);
+            scana = rpa;
+        } else {
+            ble_ll_scan_refresh_nrpa(scansm);
+            scana = scansm->scan_nrpa;
+        }
+
+        hdr_byte |= BLE_ADV_PDU_HDR_TXADD_RAND;
+    }
+#endif
+
+    /* Save scan request data */
+    pdu_data->hdr_byte = hdr_byte;
+    memcpy(pdu_data->scana, scana, BLE_DEV_ADDR_LEN);
+    memcpy(pdu_data->adva, adv_addr, BLE_DEV_ADDR_LEN);
+}
+
+static uint8_t
+ble_ll_scan_req_tx_pdu_cb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_scan_sm *scansm = pducb_arg;
+    struct ble_ll_scan_pdu_data *pdu_data = &scansm->pdu_data;
+
+    memcpy(dptr, pdu_data->scana, BLE_DEV_ADDR_LEN);
+    memcpy(dptr + BLE_DEV_ADDR_LEN, pdu_data->adva, BLE_DEV_ADDR_LEN);
+
+    *hdr_byte = pdu_data->hdr_byte;
+
+    return BLE_DEV_ADDR_LEN * 2;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/* if copy_from is provided new report is initialized with that instead of
+ * defaults
+ */
+static struct ble_hci_ev *
+ble_ll_scan_get_ext_adv_report(struct ext_adv_report *copy_from)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *ev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+
+    hci_ev = ( void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (!hci_ev) {
+        return NULL;
+    }
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(*ev) + sizeof(*report);
+    ev = (void *) hci_ev->data;
+
+    memset(ev, 0, sizeof(*ev));
+    ev->subev_code = BLE_HCI_LE_SUBEV_EXT_ADV_RPT;
+    /* We support only one report per event now */
+    ev->num_reports = 1;
+
+    report = ev->reports;
+
+    if (copy_from) {
+        memcpy(report, copy_from, sizeof(*report));
+        report->data_len = 0;
+    } else {
+        memset(report, 0, sizeof(*report));
+
+        report->pri_phy = BLE_PHY_1M;
+        /* Init SID with "Not available" which is 0xFF */
+        report->sid = 0xFF;
+        /* Init TX Power with "Not available" which is 127 */
+        report->tx_power = 127;
+        /* Init RSSI with "Not available" which is 127 */
+        report->rssi = 127;
+        /* Init address type with "anonymous" which is 0xFF */
+        report->addr_type = 0xFF;
+    }
+
+    return hci_ev;
+}
+
+static void
+ble_ll_scan_send_truncated(struct ble_ll_aux_data *aux_data)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *ev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+        return;
+    }
+
+    BLE_LL_ASSERT(aux_data);
+
+    /* No need to send if we did not send any report or sent truncated already */
+    if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) ||
+        (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)) {
+        return;
+    }
+
+    BLE_LL_ASSERT(aux_data->evt);
+    hci_ev = aux_data->evt;
+    aux_data->evt = NULL;
+
+    hci_ev->length = sizeof(*ev) + sizeof(*report);
+
+    ev = (void *) hci_ev->data;
+    report = ev->reports;
+
+    report->data_len = 0;
+
+    report->evt_type = aux_data->evt_type;
+    report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED;
+
+    if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) {
+        memcpy(report->addr, aux_data->adva, 6);
+        report->addr_type = aux_data->adva_type;
+    }
+
+    if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) {
+        memcpy(report->dir_addr, aux_data->targeta, 6);
+        report->dir_addr_type = aux_data->targeta_type;
+    }
+
+    report->sid = aux_data->adi >> 12;
+    ble_ll_hci_event_send(hci_ev);
+
+    aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+    aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_ANY;
+    aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED;
+}
+
+static int
+ble_ll_scan_get_adi(struct ble_ll_aux_data *aux_data, uint16_t *adi)
+{
+    if (!aux_data || !(aux_data->flags & BLE_LL_AUX_HAS_ADI)) {
+        return -1;
+    }
+
+    *adi = aux_data->adi;
+
+    return 0;
+}
+
+void
+ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data)
+{
+    /* Make sure we send report with 'truncated' data state if needed */
+    ble_ll_scan_send_truncated(aux_data);
+}
+#endif
+
+static void
+ble_ll_scan_clean_cur_aux_data(void)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+
+    /* If scanner was reading aux ptr, we need to clean it up */
+    if (scansm->cur_aux_data) {
+        ble_ll_scan_end_adv_evt(scansm->cur_aux_data);
+        ble_ll_scan_aux_data_unref(scansm->cur_aux_data);
+        scansm->cur_aux_data = NULL;
+    }
+#endif
+}
+
+void
+ble_ll_scan_halt(void)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+
+    ble_ll_scan_clean_cur_aux_data();
+
+    /* Update backoff if we failed to receive scan response */
+    if (scansm->scan_rsp_pending) {
+        scansm->scan_rsp_pending = 0;
+        ble_ll_scan_req_backoff(scansm, 0);
+    }
+}
+
+/**
+ * Checks to see if we have received a scan response from this advertiser.
+ *
+ * @param adv_addr Address of advertiser
+ * @param txadd TxAdd bit (0: public; random otherwise)
+ *
+ * @return int 0: have not received a scan response; 1 otherwise.
+ */
+static int
+ble_ll_scan_have_rxd_scan_rsp(uint8_t *addr, uint8_t txadd,
+                              uint8_t ext_adv, uint16_t adi)
+{
+    uint8_t num_advs;
+    struct ble_ll_scan_advertisers *adv;
+
+    /* Do we have an address match? Must match address type */
+    adv = &g_ble_ll_scan_rsp_advs[0];
+    num_advs = g_ble_ll_scan_num_rsp_advs;
+    while (num_advs) {
+        if (!memcmp(&adv->adv_addr, addr, BLE_DEV_ADDR_LEN)) {
+            /* Address type must match */
+            if (txadd) {
+                if (adv->sc_adv_flags & BLE_LL_SC_ADV_F_RANDOM_ADDR) {
+                    if (ext_adv) {
+                        if (adi == adv->adi) {
+                            return 1;
+                        }
+                        goto next;
+                    }
+                    return 1;
+                }
+            } else {
+                if ((adv->sc_adv_flags & BLE_LL_SC_ADV_F_RANDOM_ADDR) == 0) {
+                    if (ext_adv) {
+                        if (adi == adv->adi) {
+                            return 1;
+                        }
+                        goto next;
+                    }
+                    return 1;
+                }
+            }
+        }
+next:
+        ++adv;
+        --num_advs;
+    }
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_scan_add_scan_rsp_adv(uint8_t *addr, uint8_t txadd,
+                             uint8_t ext_adv, uint16_t adi)
+{
+    uint8_t num_advs;
+    struct ble_ll_scan_advertisers *adv;
+
+    /* XXX: for now, if we dont have room, just leave */
+    num_advs = g_ble_ll_scan_num_rsp_advs;
+    if (num_advs == MYNEWT_VAL(BLE_LL_NUM_SCAN_RSP_ADVS)) {
+        return;
+    }
+
+    /* Check if address is already on the list */
+    if (ble_ll_scan_have_rxd_scan_rsp(addr, txadd, ext_adv, adi)) {
+        return;
+    }
+
+    /* Add the advertiser to the array */
+    adv = &g_ble_ll_scan_rsp_advs[num_advs];
+    memcpy(&adv->adv_addr, addr, BLE_DEV_ADDR_LEN);
+    adv->sc_adv_flags = BLE_LL_SC_ADV_F_SCAN_RSP_RXD;
+    if (txadd) {
+        adv->sc_adv_flags |= BLE_LL_SC_ADV_F_RANDOM_ADDR;
+    }
+    adv->adi = adi;
+    ++g_ble_ll_scan_num_rsp_advs;
+
+    return;
+}
+
+static int
+ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype,
+                                      const uint8_t *addr, uint8_t addr_type,
+                                      int8_t rssi,
+                                      uint8_t adv_data_len,
+                                      struct os_mbuf *adv_data,
+                                      const uint8_t *inita, uint8_t inita_type)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *ev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+        return -1;
+    }
+
+    /* Drop packet if adv data doesn't fit  */
+    if ((sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len) > BLE_HCI_MAX_DATA_LEN) {
+        STATS_INC(ble_ll_stats, adv_evt_dropped);
+        return -1;
+    }
+
+    hci_ev = ble_ll_scan_get_ext_adv_report(NULL);
+    if (!hci_ev) {
+        return -1;
+    }
+
+    ev = (void *) hci_ev->data;
+    report = ev->reports;
+
+    switch (evtype) {
+    case BLE_HCI_ADV_RPT_EVTYPE_ADV_IND:
+        report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_IND;
+        break;
+    case BLE_HCI_ADV_RPT_EVTYPE_DIR_IND:
+        report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_DIRECT_IND;
+        break;
+    case BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND:
+        report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_NONCON_IND;
+        break;
+    case BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP:
+        report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_SCAN_RSP_ADV_IND;
+         break;
+    case BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND:
+        report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_SCAN_IND;
+        break;
+    default:
+        BLE_LL_ASSERT(0);
+        ble_hci_trans_buf_free((uint8_t *) hci_ev);
+        return -1;
+    }
+
+    report->addr_type = addr_type;
+    memcpy(report->addr, addr, BLE_DEV_ADDR_LEN);
+    report->pri_phy = BLE_PHY_1M;
+    report->sid = 0xFF;
+    report->tx_power = 127;
+    report->rssi = rssi;
+
+    if (inita) {
+        report->dir_addr_type = inita_type;
+        memcpy(report->dir_addr, inita, BLE_DEV_ADDR_LEN);
+    }
+
+    if (adv_data_len) {
+        hci_ev->length += adv_data_len;
+        report->data_len = adv_data_len;
+        os_mbuf_copydata(adv_data, 0, adv_data_len, report->data);
+    }
+
+    return ble_ll_hci_event_send(hci_ev);
+}
+#endif
+
+static int
+ble_ll_hci_send_adv_report(uint8_t evtype,
+                           const uint8_t *addr, uint8_t addr_type, int8_t rssi,
+                           uint8_t adv_data_len, struct os_mbuf *adv_data)
+{
+    struct ble_hci_ev_le_subev_adv_rpt *ev;
+    struct ble_hci_ev *hci_ev;
+    int8_t *ev_rssi;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ADV_RPT)) {
+        return -1;
+    }
+
+    /* Drop packet if adv data doesn't fit, note extra 1 is for RSSI   */
+    if ((sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len + 1) > BLE_HCI_MAX_DATA_LEN) {
+        STATS_INC(ble_ll_stats, adv_evt_dropped);
+        return -1;
+    }
+
+    hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (!hci_ev) {
+        return -1;
+    }
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len + 1;
+    ev = (void *) hci_ev->data;
+
+    ev->subev_code = BLE_HCI_LE_SUBEV_ADV_RPT;
+    ev->num_reports = 1;
+
+    ev->reports[0].type = evtype;
+    ev->reports[0].addr_type = addr_type;
+    memcpy(ev->reports[0].addr, addr, BLE_DEV_ADDR_LEN);
+    ev->reports[0].data_len = adv_data_len;
+    os_mbuf_copydata(adv_data, 0, adv_data_len, ev->reports[0].data);
+
+    /* RSSI is after adv data... */
+    ev_rssi = (int8_t *) (hci_ev->data + sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len);
+    *ev_rssi = rssi;
+
+    return ble_ll_hci_event_send(hci_ev);
+}
+
+static int
+ble_ll_hci_send_dir_adv_report(const uint8_t *addr, uint8_t addr_type,
+                               const uint8_t *inita, uint8_t inita_type,
+                               int8_t rssi)
+{
+    struct ble_hci_ev_le_subev_direct_adv_rpt *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT)) {
+        return -1;
+    }
+
+    hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (!hci_ev) {
+        return -1;
+    }
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(*ev) + sizeof(*(ev->reports));
+    ev = (void *) hci_ev->data;
+
+    ev->subev_code = BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT;
+    ev->num_reports = 1;
+
+    ev->reports[0].type = BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
+    ev->reports[0].addr_type = addr_type;
+    memcpy(ev->reports[0].addr, addr, BLE_DEV_ADDR_LEN);
+    ev->reports[0].dir_addr_type = inita_type;
+    memcpy(ev->reports[0].dir_addr, inita, BLE_DEV_ADDR_LEN);
+    ev->reports[0].rssi = rssi;
+
+    return ble_ll_hci_event_send(hci_ev);
+}
+
+static int
+ble_ll_scan_dup_update_legacy(uint8_t addr_type, const uint8_t *addr,
+                              uint8_t subev, uint8_t evtype)
+{
+    struct ble_ll_scan_dup_entry *e;
+    uint8_t type;
+
+    type = BLE_LL_SCAN_ENTRY_TYPE_LEGACY(addr_type);
+
+    /*
+     * We assume ble_ll_scan_dup_check() was called before which either matched
+     * some entry or allocated new one and placed in on the top of queue.
+     */
+
+    e = TAILQ_FIRST(&g_scan_dup_list);
+    BLE_LL_ASSERT(e && e->type == type && !memcmp(e->addr, addr, 6));
+
+    if (subev == BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT) {
+        e->flags |= BLE_LL_SCAN_DUP_F_DIR_ADV_REPORT_SENT;
+    } else {
+        if (evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+            e->flags |= BLE_LL_SCAN_DUP_F_SCAN_RSP_SENT;
+        } else {
+            e->flags |= BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT;
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Send an advertising report to the host.
+ *
+ * NOTE: while we are allowed to send multiple devices in one report, we
+ * will just send for one for now.
+ *
+ * @param pdu_type
+ * @param txadd
+ * @param rxbuf
+ * @param hdr
+ * @param scansm
+ */
+static void
+ble_ll_scan_send_adv_report(uint8_t pdu_type,
+                            const uint8_t *adva, uint8_t adva_type,
+                            const uint8_t *inita, uint8_t inita_type,
+                            struct os_mbuf *om,
+                            struct ble_mbuf_hdr *hdr,
+                            struct ble_ll_scan_sm *scansm)
+{
+    uint8_t subev = BLE_HCI_LE_SUBEV_ADV_RPT;
+    uint8_t adv_data_len;
+    uint8_t evtype;
+    int rc;
+
+    if (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) {
+        if (ble_ll_is_rpa(inita, inita_type)) {
+            /* For resolvable we send separate subevent */
+            subev = BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT;
+        }
+
+        evtype = BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
+        adv_data_len = 0;
+    } else {
+        if (pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) {
+            evtype = BLE_HCI_ADV_RPT_EVTYPE_ADV_IND;
+        } else if (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND) {
+            evtype = BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND;
+        } else if (pdu_type == BLE_ADV_PDU_TYPE_ADV_NONCONN_IND) {
+            evtype = BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND;
+        } else {
+            evtype = BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP;
+        }
+        adv_data_len = om->om_data[1] - BLE_DEV_ADDR_LEN;
+        os_mbuf_adj(om, BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN);
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* If RPA has been used, make sure we use correct address types
+     * in the advertising report.
+     */
+    if (BLE_MBUF_HDR_RESOLVED(hdr)) {
+        adva_type += 2;
+    }
+    if (BLE_MBUF_HDR_TARGETA_RESOLVED(hdr)) {
+        inita_type += 2;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (scansm->ext_scanning) {
+        rc = ble_ll_hci_send_legacy_ext_adv_report(evtype,
+                                                   adva, adva_type,
+                                                   hdr->rxinfo.rssi,
+                                                   adv_data_len, om,
+                                                   inita, inita_type);
+        goto done;
+    }
+#endif
+
+    if (subev == BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT) {
+        rc = ble_ll_hci_send_dir_adv_report(adva, adva_type, inita, inita_type,
+                                            hdr->rxinfo.rssi);
+        goto done;
+    }
+
+    rc = ble_ll_hci_send_adv_report(evtype, adva, adva_type, hdr->rxinfo.rssi,
+                                    adv_data_len, om);
+done:
+    if (!rc && scansm->scan_filt_dups) {
+        ble_ll_scan_dup_update_legacy(adva_type, adva, subev, evtype);
+    }
+}
+
+static void
+ble_ll_get_chan_to_scan(struct ble_ll_scan_sm *scansm, uint8_t *chan,
+                        int *phy)
+{
+    struct ble_ll_scan_params *scanp = scansm->scanp;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_aux_data *aux_data = scansm->cur_aux_data;
+
+    if (!scansm->ext_scanning || !aux_data || !aux_data->scanning) {
+        *chan = scanp->scan_chan;
+        *phy = scanp->phy;
+        return;
+    }
+
+    *chan = aux_data->chan;
+    *phy = aux_data->aux_phy;
+#else
+    *chan = scanp->scan_chan;
+    *phy = scanp->phy;
+#endif
+}
+/**
+ * Called to enable the receiver for scanning.
+ *
+ * Context: Link Layer task
+ *
+ * @param sch
+ *
+ * @return int
+ */
+static int
+ble_ll_scan_start(struct ble_ll_scan_sm *scansm, struct ble_ll_sched_item *sch)
+{
+    int rc;
+    struct ble_ll_scan_params *scanp = scansm->scanp;
+    uint8_t scan_chan;
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    uint8_t phy_mode;
+#endif
+    int phy;
+
+    BLE_LL_ASSERT(scansm->scan_rsp_pending == 0);
+
+    ble_ll_get_chan_to_scan(scansm, &scan_chan, &phy);
+
+    /* XXX: right now scheduled item is only present if we schedule for aux
+     *      scan just make sanity check that we have proper combination of
+     *      sch and resulting scan_chan
+     */
+    BLE_LL_ASSERT(!sch || scan_chan < BLE_PHY_ADV_CHAN_START);
+    BLE_LL_ASSERT(sch || scan_chan >= BLE_PHY_ADV_CHAN_START);
+
+    /* Set channel */
+    rc = ble_phy_setchan(scan_chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV);
+    BLE_LL_ASSERT(rc == 0);
+
+    /*
+     * Set transmit end callback to NULL in case we transmit a scan request.
+     * There is a callback for the connect request.
+     */
+    ble_phy_set_txend_cb(NULL, NULL);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (ble_ll_resolv_enabled()) {
+        ble_phy_resolv_list_enable();
+    } else {
+        ble_phy_resolv_list_disable();
+    }
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    phy_mode = ble_ll_phy_to_phy_mode(phy, BLE_HCI_LE_PHY_CODED_ANY);
+    ble_phy_mode_set(phy_mode, phy_mode);
+#endif
+
+    /* XXX: probably need to make sure hfxo is running too */
+    /* XXX: can make this better; want to just start asap. */
+    if (sch) {
+        rc = ble_phy_rx_set_start_time(sch->start_time +
+                                       g_ble_ll_sched_offset_ticks,
+                                       sch->remainder);
+    } else {
+        rc = ble_phy_rx_set_start_time(os_cputime_get32() +
+                                       g_ble_ll_sched_offset_ticks, 0);
+    }
+    if (!rc || rc == BLE_PHY_ERR_RX_LATE) {
+        /* If we are late here, it is still OK because we keep scanning.
+         * Clear error
+         */
+        rc = 0;
+
+        /* Enable/disable whitelisting */
+        if (scanp->scan_filt_policy & 1) {
+            ble_ll_whitelist_enable();
+        } else {
+            ble_ll_whitelist_disable();
+        }
+
+        /* Set link layer state to scanning */
+        if (scanp->scan_type == BLE_SCAN_TYPE_INITIATE) {
+            ble_ll_state_set(BLE_LL_STATE_INITIATING);
+        } else {
+            ble_ll_state_set(BLE_LL_STATE_SCANNING);
+        }
+    }
+
+    return rc;
+}
+
+static uint8_t
+ble_ll_scan_get_next_adv_prim_chan(uint8_t chan)
+{
+    ++chan;
+    if (chan == BLE_PHY_NUM_CHANS) {
+        chan = BLE_PHY_ADV_CHAN_START;
+    }
+
+    return chan;
+}
+
+static uint32_t
+ble_ll_scan_move_window_to(struct ble_ll_scan_params *scanp, uint32_t time)
+{
+    uint32_t end_time;
+
+    /*
+     * Move window until given tick is before or inside window and move to next
+     * channel for each skipped interval.
+     */
+
+    end_time = scanp->timing.start_time + scanp->timing.window;
+    while (CPUTIME_GEQ(time, end_time)) {
+        scanp->timing.start_time += scanp->timing.interval;
+        scanp->scan_chan = ble_ll_scan_get_next_adv_prim_chan(scanp->scan_chan);
+        end_time = scanp->timing.start_time + scanp->timing.window;
+    }
+
+    return scanp->timing.start_time;
+}
+
+static bool
+ble_ll_scan_is_inside_window(struct ble_ll_scan_params *scanp, uint32_t time)
+{
+    uint32_t start_time;
+
+    /* Make sure we are checking against closest window */
+    start_time = ble_ll_scan_move_window_to(scanp, time);
+
+    if (scanp->timing.window == scanp->timing.interval) {
+        /* always inside window in continuous scan */
+        return true;
+    }
+
+    return CPUTIME_GEQ(time, start_time) &&
+           CPUTIME_LT(time, start_time + scanp->timing.window);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_data)
+{
+    if (aux_data) {
+        if (aux_data->evt) {
+            ble_hci_trans_buf_free((uint8_t *)aux_data->evt);
+            aux_data->evt = NULL;
+        }
+        os_memblock_put(&ext_scan_aux_pool, aux_data);
+        STATS_INC(ble_ll_stats, aux_freed);
+    }
+}
+
+struct ble_ll_aux_data *
+ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_data)
+{
+    os_sr_t sr;
+
+    BLE_LL_ASSERT(aux_data);
+
+    OS_ENTER_CRITICAL(sr);
+    aux_data->ref_cnt++;
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_REF, (uint32_t) aux_data, aux_data->ref_cnt);
+
+    OS_EXIT_CRITICAL(sr);
+
+    return aux_data;
+}
+
+void
+ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_data)
+{
+    os_sr_t sr;
+
+    BLE_LL_ASSERT(aux_data);
+
+    OS_ENTER_CRITICAL(sr);
+    aux_data->ref_cnt--;
+    ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_UNREF, (uint32_t) aux_data, aux_data->ref_cnt);
+
+    if (aux_data->ref_cnt == 0) {
+        /*
+         * Some validation to make sure that we completed scan properly:
+         * - we either did not send any report or sent completed/truncated
+         * - we only sent one of completed/truncated
+         * - in case of error, we wither did not send anything or sent truncated
+         */
+        BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) ||
+                      ((aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) &&
+                       (aux_data->flags_ll & (BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED | BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED))));
+        BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED) || !(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED));
+        BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR) ||
+                      !(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) ||
+                      (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED));
+
+        ble_ll_scan_aux_data_free(aux_data);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+static void
+ble_ll_scan_sched_remove(struct ble_ll_sched_item *sch)
+{
+    ble_ll_scan_end_adv_evt(sch->cb_arg);
+    ble_ll_scan_aux_data_unref(sch->cb_arg);
+    sch->cb_arg = NULL;
+}
+#endif
+/**
+ * Stop the scanning state machine
+ */
+void
+ble_ll_scan_sm_stop(int chk_disable)
+{
+    os_sr_t sr;
+    uint8_t lls;
+    struct ble_ll_scan_sm *scansm;
+
+    /* Stop the scanning timer  */
+    scansm = &g_ble_ll_scan_sm;
+    os_cputime_timer_stop(&scansm->scan_timer);
+
+    OS_ENTER_CRITICAL(sr);
+
+    /* Disable scanning state machine */
+    scansm->scan_enabled = 0;
+    scansm->restart_timer_needed = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (scansm->ext_scanning) {
+        ble_ll_scan_clean_cur_aux_data();
+        ble_ll_sched_rmv_elem_type(BLE_LL_SCHED_TYPE_AUX_SCAN, ble_ll_scan_sched_remove);
+        scansm->ext_scanning = 0;
+    }
+#endif
+
+    /* Update backoff if we failed to receive scan response */
+    if (scansm->scan_rsp_pending) {
+        scansm->scan_rsp_pending = 0;
+        ble_ll_scan_req_backoff(scansm, 0);
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    /* Count # of times stopped */
+    STATS_INC(ble_ll_stats, scan_stops);
+
+    /* Only set state if we are currently in a scan window */
+    if (chk_disable) {
+        OS_ENTER_CRITICAL(sr);
+        lls = ble_ll_state_get();
+
+        if ((lls == BLE_LL_STATE_SCANNING) ||
+                        (lls == BLE_LL_STATE_INITIATING && chk_disable == 1)) {
+            /* Disable phy */
+            ble_phy_disable();
+
+            /* Set LL state to standby */
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        }
+        OS_EXIT_CRITICAL(sr);
+    }
+
+    /* No need for RF anymore */
+    OS_ENTER_CRITICAL(sr);
+    ble_ll_rfmgmt_scan_changed(false, 0);
+    ble_ll_rfmgmt_release();
+    OS_EXIT_CRITICAL(sr);
+}
+
+static int
+ble_ll_scan_sm_start(struct ble_ll_scan_sm *scansm)
+{
+    struct ble_ll_scan_params *scanp;
+    struct ble_ll_scan_params *scanp_next;
+
+    if (!ble_ll_is_valid_own_addr_type(scansm->own_addr_type, g_random_addr)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    BLE_LL_ASSERT(scansm->scanp);
+    scanp = scansm->scanp;
+    scanp_next = scansm->scanp_next;
+
+    /* Count # of times started */
+    STATS_INC(ble_ll_stats, scan_starts);
+
+    /* Set flag telling us that scanning is enabled */
+    scansm->scan_enabled = 1;
+
+    /* Set first advertising channel */
+    scanp->scan_chan = BLE_PHY_ADV_CHAN_START;
+    if (scanp_next) {
+        scanp_next->scan_chan = BLE_PHY_ADV_CHAN_START;
+    }
+
+    /* Reset scan request backoff parameters to default */
+    scansm->upper_limit = 1;
+    scansm->backoff_count = 1;
+    scansm->scan_rsp_pending = 0;
+
+    /* Forget filtered advertisers from previous scan. */
+    g_ble_ll_scan_num_rsp_advs = 0;
+
+    os_mempool_clear(&g_scan_dup_pool);
+    TAILQ_INIT(&g_scan_dup_list);
+
+    /*
+     * First scan window can start when RF is enabled. Add 1 tick since we are
+     * most likely not aligned with ticks so RF may be effectively enabled 1
+     * tick later.
+     */
+    scanp->timing.start_time = ble_ll_rfmgmt_enable_now();
+    ble_ll_rfmgmt_scan_changed(true, scanp->timing.start_time);
+
+    if (scanp_next) {
+        /* Schedule start time right after first phy */
+        scanp_next->timing.start_time = scanp->timing.start_time +
+                                        scanp->timing.window;
+    }
+
+    /* Start scan at 1st window */
+    os_cputime_timer_start(&scansm->scan_timer, scanp->timing.start_time);
+
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_aux_scan_rsp_failed(struct ble_ll_scan_sm *scansm)
+{
+    if (!scansm->cur_aux_data) {
+        return;
+    }
+
+    STATS_INC(ble_ll_stats, aux_scan_rsp_err);
+    ble_ll_scan_interrupted(scansm);
+}
+#endif
+
+static void
+ble_ll_scan_interrupted_event_cb(struct ble_npl_event *ev)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_aux_data *aux_data;
+#endif
+
+    if (!scansm->scan_enabled) {
+        return;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    aux_data = ble_npl_event_get_arg(ev);
+
+    if (aux_data) {
+        if (scansm->scan_rsp_pending) {
+            STATS_INC(ble_ll_stats, aux_scan_rsp_err);
+        }
+        ble_ll_scan_end_adv_evt(aux_data);
+        ble_ll_scan_aux_data_unref(aux_data);
+        ble_npl_event_set_arg(ev, NULL);
+        STATS_INC(ble_ll_stats, aux_missed_adv);
+    }
+#endif
+
+    /*
+    * If we timed out waiting for a response, the scan response pending
+    * flag should be set. Deal with scan backoff. Put device back into rx.
+    */
+
+    if (scansm->scan_rsp_pending) {
+        scansm->scan_rsp_pending = 0;
+        ble_ll_scan_req_backoff(scansm, 0);
+    }
+
+    ble_ll_scan_chk_resume();
+}
+
+/**
+ * Called to process the scanning OS event which was posted to the LL task
+ *
+ * Context: Link Layer task.
+ *
+ * @param arg
+ */
+static void
+ble_ll_scan_event_proc(struct ble_npl_event *ev)
+{
+    struct ble_ll_scan_sm *scansm;
+    os_sr_t sr;
+    bool start_scan;
+    bool inside_window;
+    struct ble_ll_scan_params *scanp;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    bool inside_window_next;
+    struct ble_ll_scan_params *scanp_next;
+#endif
+    uint32_t next_proc_time;
+    uint32_t now;
+    /*
+     * Get the scanning state machine. If not enabled (this is possible), just
+     * leave and do nothing (just make sure timer is stopped).
+     */
+    scansm = (struct ble_ll_scan_sm *)ble_npl_event_get_arg(ev);
+    scanp = scansm->scanp;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    scanp_next = scansm->scanp_next;
+#endif
+
+    OS_ENTER_CRITICAL(sr);
+    if (!scansm->scan_enabled) {
+        os_cputime_timer_stop(&scansm->scan_timer);
+        ble_ll_rfmgmt_scan_changed(false, 0);
+        ble_ll_rfmgmt_release();
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    if (scansm->cur_aux_data || scansm->scan_rsp_pending) {
+        /* Aux scan in progress. Wait */
+        STATS_INC(ble_ll_stats, scan_timer_stopped);
+        scansm->restart_timer_needed = 1;
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    now = os_cputime_get32();
+
+    inside_window = ble_ll_scan_is_inside_window(scanp, now);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* Update also next PHY if configured */
+    if (scanp_next) {
+        inside_window_next = ble_ll_scan_is_inside_window(scanp_next, now);
+
+        /*
+         * Switch PHY if current PHY is outside window and next PHY is either
+         * inside window or has next window earlier than current PHY.
+         */
+        if (!inside_window &&
+            ((inside_window_next || CPUTIME_LEQ(scanp_next->timing.start_time,
+                                                scanp->timing.start_time)))) {
+            scansm->scanp = scanp_next;
+            scansm->scanp_next = scanp;
+            scanp = scansm->scanp;
+            scanp_next = scansm->scanp_next;
+            inside_window = inside_window_next;
+        }
+    }
+#endif
+
+    /*
+     * At this point scanp and scanp_next point to current or closest scan
+     * window on both PHYs (scanp is the closer one). Make sure RF is enabled
+     * on time.
+     */
+    ble_ll_rfmgmt_scan_changed(true, scanp->timing.start_time);
+
+    /*
+     * If we are inside window, next scan proc should happen at the end of
+     * current window to either disable scan or switch to next PHY.
+     * If we are outside window, next scan proc should happen at the time of
+     * closest scan window.
+     */
+    if (inside_window) {
+        next_proc_time = scanp->timing.start_time + scanp->timing.window;
+    } else {
+        next_proc_time = scanp->timing.start_time;
+    }
+
+    /*
+     * If we are not in the standby state it means that the scheduled
+     * scanning event was overlapped in the schedule. In this case all we do
+     * is post the scan schedule end event.
+     */
+    start_scan = inside_window;
+    switch (ble_ll_state_get()) {
+    case BLE_LL_STATE_ADV:
+    case BLE_LL_STATE_CONNECTION:
+    case BLE_LL_STATE_SYNC:
+         start_scan = false;
+         break;
+    case BLE_LL_STATE_INITIATING:
+        /* Must disable PHY since we will move to a new channel */
+        ble_phy_disable();
+        if (!inside_window) {
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        }
+        /* PHY is disabled - make sure we do not wait for AUX_CONNECT_RSP */
+        ble_ll_conn_reset_pending_aux_conn_rsp();
+        break;
+    case BLE_LL_STATE_SCANNING:
+        /* Must disable PHY since we will move to a new channel */
+        ble_phy_disable();
+        if (!inside_window) {
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        }
+        break;
+    case BLE_LL_STATE_STANDBY:
+        break;
+    default:
+        BLE_LL_ASSERT(0);
+        break;
+    }
+
+    if (start_scan) {
+        ble_ll_scan_start(scansm, NULL);
+    } else {
+        ble_ll_rfmgmt_release();
+    }
+
+    OS_EXIT_CRITICAL(sr);
+    os_cputime_timer_start(&scansm->scan_timer, next_proc_time);
+}
+
+/**
+ * ble ll scan rx pdu start
+ *
+ * Called when a PDU reception has started and the Link Layer is in the
+ * scanning state.
+ *
+ * Context: Interrupt
+ *
+ * @param pdu_type
+ * @param rxflags
+ *
+ * @return int
+ *  0: we will not attempt to reply to this frame
+ *  1: we may send a response to this frame.
+ */
+int
+ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags)
+{
+    int rc;
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp;
+
+    rc = 0;
+    scansm = &g_ble_ll_scan_sm;
+    scanp = scansm->scanp;
+
+    switch (scanp->scan_type) {
+    case BLE_SCAN_TYPE_ACTIVE:
+        /* If adv ind or scan ind, we may send scan request */
+        if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) ||
+            (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND)) {
+            rc = 1;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && scansm->ext_scanning)) {
+            *rxflags |= BLE_MBUF_HDR_F_EXT_ADV;
+            rc = 1;
+        }
+#endif
+
+        if (scansm->cur_aux_data && !scansm->scan_rsp_pending ) {
+            STATS_INC(ble_ll_stats, aux_received);
+        }
+
+        /*
+         * If this is the first PDU after we sent the scan response (as
+         * denoted by the scan rsp pending flag), we set a bit in the ble
+         * header so the link layer can check to see if the scan request
+         * was successful. We do it this way to let the Link Layer do the
+         * work for successful scan requests. If failed, we do the work here.
+         */
+        if (scansm->scan_rsp_pending) {
+            scansm->scan_rsp_pending = 0;
+
+            if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) {
+                *rxflags |= BLE_MBUF_HDR_F_SCAN_RSP_RXD;
+            } else if (pdu_type == BLE_ADV_PDU_TYPE_AUX_SCAN_RSP) {
+                *rxflags |= BLE_MBUF_HDR_F_SCAN_RSP_RXD;
+            } else {
+                ble_ll_scan_req_backoff(scansm, 0);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+                ble_ll_aux_scan_rsp_failed(scansm);
+#endif
+            }
+        }
+        break;
+    case BLE_SCAN_TYPE_PASSIVE:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && scansm->ext_scanning)) {
+            *rxflags |= BLE_MBUF_HDR_F_EXT_ADV;
+        }
+        break;
+#endif
+    default:
+        break;
+    }
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static uint8_t
+ble_ll_ext_adv_phy_mode_to_local_phy(uint8_t adv_phy_mode)
+{
+    switch (adv_phy_mode) {
+    case 0x00:
+        return BLE_PHY_1M;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+     case 0x01:
+        return BLE_PHY_2M;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case 0x02:
+        return BLE_PHY_CODED;
+#endif
+    }
+
+    return 0;
+}
+
+static int
+ble_ll_ext_scan_parse_aux_ptr(struct ble_ll_aux_data *aux_data, uint8_t *buf)
+{
+    uint32_t aux_ptr_field = get_le32(buf) & 0x00FFFFFF;
+
+    aux_data->chan = (aux_ptr_field) & 0x3F;
+    if (aux_data->chan >= BLE_PHY_NUM_DATA_CHANS) {
+        return -1;
+    }
+
+    /* TODO use CA aux_ptr_field >> 6 */
+
+    aux_data->offset = 30 * ((aux_ptr_field >> 8) & 0x1FFF);
+
+    if ((aux_ptr_field >> 7) & 0x01) {
+            aux_data->offset *= 10;
+            aux_data->offset_units = 1;
+    }
+
+    if (aux_data->offset < BLE_LL_MAFS) {
+        return -1;
+    }
+
+    aux_data->aux_phy =
+            ble_ll_ext_adv_phy_mode_to_local_phy((aux_ptr_field >> 21) & 0x07);
+    if (aux_data->aux_phy == 0) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static void
+ble_ll_ext_scan_parse_adv_info(struct ext_adv_report *report, const uint8_t *buf)
+{
+    uint16_t adv_info = get_le16(buf);
+
+    /* TODO Use DID */
+
+    report->sid = (adv_info >> 12);
+}
+
+/**
+ * ble_ll_scan_update_aux_data
+ *
+ * Update aux_data stored in ble_hdr.rxinfo.user_data. If no aux_data is present
+ * (i.e. processing ADV_EXT_IND) this will try to allocate new aux_data.
+ *
+ * Context: Interrupt
+ *
+ * @param ble_hdr
+ * @param rxbuf
+ *
+ * @return int
+ *  1: do not scan for next AUX (no AuxPtr or malformed data)
+ *  0: scan for next AUX (valid AuxPtr found)
+ * -1: error
+ */
+int
+ble_ll_scan_update_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf,
+                            bool *adva_present)
+{
+    uint8_t pdu_hdr;
+    uint8_t pdu_len;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh;
+    struct ble_ll_aux_data *aux_data;
+    bool is_aux;
+
+    aux_data = ble_hdr->rxinfo.user_data;
+    /* aux_data is initially not set only for ADV_EXT_IND */
+    is_aux = aux_data;
+
+    pdu_hdr = rxbuf[0];
+    pdu_len = rxbuf[1];
+
+    /* PDU without at least Extended Header Length is invalid */
+    if (pdu_len == 0) {
+        return -1;
+    }
+
+    adv_mode = rxbuf[2] >> 6;
+    eh_len = rxbuf[2] & 0x3f;
+    eh_flags = rxbuf[3];
+    eh = &rxbuf[4];
+
+    /*
+     * PDU without Extended Header is valid in case of last AUX_CHAIN_IND in
+     * chain so aux_data has to be set and advertising mode has to be 00b,
+     * otherwise it's an invalid PDU.
+     */
+    if (eh_len == 0) {
+        if (!aux_data || adv_mode) {
+            return -1;
+        }
+        aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_COMPLETE;
+        return 1;
+    }
+
+    /*
+     * If aux_data is not set, this is ADV_EXT_IND which starts new extended
+     * advertising event.
+     */
+    if (!aux_data) {
+        if (ble_ll_scan_ext_adv_init(&aux_data)) {
+            return -1;
+        }
+
+        aux_data->aux_primary_phy = ble_hdr->rxinfo.phy;
+    } else {
+        if (aux_data->flags_isr & BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED) {
+            aux_data->flags_isr |= BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED;
+        } else {
+            aux_data->flags_isr |= BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED;
+        }
+    }
+
+    /* Now parse extended header... */
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+        aux_data->flags |= BLE_LL_AUX_HAS_ADVA;
+        memcpy(aux_data->adva, eh, 6);
+        aux_data->adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_MASK);
+        eh += BLE_LL_EXT_ADV_ADVA_SIZE;
+
+        if (adva_present) {
+            *adva_present = true;
+        }
+    } else if (adva_present) {
+        *adva_present = false;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        aux_data->flags |= BLE_LL_AUX_HAS_TARGETA;
+        memcpy(aux_data->targeta, eh, 6);
+        aux_data->targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_MASK);
+        eh += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+        eh += 1;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        aux_data->flags |= BLE_LL_AUX_HAS_ADI;
+        if (is_aux) {
+            if (get_le16(eh) != aux_data->adi) {
+                aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+                STATS_INC(ble_ll_stats, aux_chain_err);
+            }
+        } else {
+            aux_data->adi = get_le16(eh);
+        }
+        eh += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        if (ble_ll_ext_scan_parse_aux_ptr(aux_data, eh)) {
+            aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+        }
+    } else if (!(adv_mode & BLE_LL_EXT_ADV_MODE_SCAN)) {
+        /* No AuxPtr for scannable PDU is ignored since we can still scan it */
+        aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_COMPLETE;
+    }
+
+    ble_hdr->rxinfo.user_data = aux_data;
+
+    /* Do not scan for next AUX if either no AuxPtr or malformed data found */
+    return !(eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) ||
+           (aux_data->flags_isr & BLE_LL_AUX_FLAG_SCAN_ERROR);
+}
+
+/**
+ * Called when a receive ADV_EXT PDU has ended.
+ *
+ * Context: Interrupt
+ *
+ * @return int
+ *       < 0  Error
+ *      >= 0: Success (number of bytes left in PDU)
+ *
+ */
+static int
+ble_ll_scan_parse_ext_hdr(struct os_mbuf *om,
+                          const uint8_t *adva, uint8_t adva_type,
+                          const uint8_t *inita, uint8_t inita_type,
+                          struct ble_mbuf_hdr *ble_hdr,
+                          struct ext_adv_report *report)
+{
+    uint8_t pdu_len;
+    uint8_t ext_hdr_len;
+    uint8_t ext_hdr_flags;
+    uint8_t *ext_hdr;
+    uint8_t *rxbuf = om->om_data;
+    int i = 1;
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_aux_data *aux_data = ble_hdr->rxinfo.user_data;
+
+    BLE_LL_ASSERT(report);
+
+    scansm = &g_ble_ll_scan_sm;
+
+    if (!scansm->ext_scanning) {
+       /* Ignore ext adv if host does not want it*/
+       return -1;
+    }
+
+    pdu_len = rxbuf[1];
+    if (pdu_len == 0) {
+        return -1;
+    }
+
+    report->evt_type = rxbuf[2] >> 6;
+    if ( report->evt_type > BLE_LL_EXT_ADV_MODE_SCAN) {
+        return -1;
+    }
+
+    if (BLE_MBUF_HDR_SCAN_RSP_RXD(ble_hdr)) {
+        report->evt_type |= BLE_HCI_ADV_SCAN_RSP_MASK;
+    }
+
+    ext_hdr_len = rxbuf[2] & 0x3F;
+    os_mbuf_adj(om, 3);
+
+    ext_hdr_flags = rxbuf[3];
+    ext_hdr = &rxbuf[4];
+
+    if (ext_hdr_len) {
+        i = 0;
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+            i += BLE_LL_EXT_ADV_ADVA_SIZE;
+        }
+
+        if (adva) {
+            memcpy(report->addr, adva, 6);
+            report->addr_type = adva_type;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+            i += BLE_LL_EXT_ADV_TARGETA_SIZE;
+        }
+
+        if (inita) {
+           memcpy(report->dir_addr, inita, 6);
+           report->dir_addr_type = inita_type;
+           report->evt_type |= BLE_HCI_ADV_DIRECT_MASK;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+            /* Just skip it for now*/
+            i += 1;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+            ble_ll_ext_scan_parse_adv_info(report, (ext_hdr + i));
+            i += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+        } else if (report->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK) {
+            report->sid = (aux_data->adi >> 12);
+        }
+
+        /* In this point of time we don't want to care about aux ptr */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+            i += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+            report->periodic_itvl = get_le16(ext_hdr + i + 2);
+            i += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+            report->tx_power = *(ext_hdr + i);
+            i += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+        }
+
+        /* TODO Handle ACAD if needed */
+    }
+
+    /* In the event we need information on primary and secondary PHY used during
+     * advertising.
+     */
+    if (!aux_data) {
+        report->pri_phy = ble_hdr->rxinfo.phy;
+        goto done;
+    }
+
+    report->sec_phy = aux_data->aux_phy;
+    report->pri_phy = aux_data->aux_primary_phy;
+
+    if (ext_hdr_len) {
+        /* Adjust mbuf to contain advertising data only */
+        os_mbuf_adj(om, ext_hdr_len);
+    }
+
+    /* Let us first keep update event type in aux data.
+     * Note that in aux chain and aux scan response packets
+     * we do miss original event type, which we need for advertising report.
+     */
+    aux_data->evt_type |= report->evt_type;
+    report->evt_type = aux_data->evt_type;
+
+done:
+    return pdu_len - ext_hdr_len - 1;
+}
+
+static int
+ble_ll_scan_get_addr_from_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr,
+                                  uint8_t **addr, uint8_t *addr_type,
+                                  uint8_t **inita, uint8_t *inita_type,
+                                  int *ext_mode)
+{
+    uint8_t pdu_len;
+    uint8_t ext_hdr_len;
+    uint8_t ext_hdr_flags;
+    uint8_t *ext_hdr;
+    bool has_adva = false;
+    bool has_inita = false;
+    int i;
+    struct ble_ll_aux_data *aux_data = ble_hdr->rxinfo.user_data;
+
+    *addr = NULL;
+    *inita = NULL;
+
+    pdu_len = rxbuf[1];
+    if (pdu_len == 0) {
+        return -1;
+    }
+
+    *ext_mode = rxbuf[2] >> 6;
+    if (*ext_mode > BLE_LL_EXT_ADV_MODE_SCAN) {
+        return -1;
+    }
+
+    ext_hdr_len = rxbuf[2] & 0x3F;
+    if (ext_hdr_len == 0) {
+        goto done;
+    }
+
+    ext_hdr_flags = rxbuf[3];
+    ext_hdr = &rxbuf[4];
+
+    i = 0;
+    if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+        if (ext_hdr_len < BLE_LL_EXT_ADV_ADVA_SIZE) {
+            return -1;
+        }
+
+        *addr = ext_hdr + i;
+        *addr_type =
+                ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK);
+        i += BLE_LL_EXT_ADV_ADVA_SIZE;
+
+        has_adva = true;
+    }
+
+    if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        *inita = ext_hdr + i;
+        *inita_type =
+                ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK);
+        i += BLE_LL_EXT_ADV_TARGETA_SIZE;
+
+        has_inita = true;
+    }
+
+done:
+    /* Check if we had address already. If yes, replace it with new one */
+
+    if (aux_data) {
+        /* If address has been provided, we do have it already in aux_data.*/
+        if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) {
+            if (!has_adva) {
+                *addr = aux_data->adva;
+                *addr_type = aux_data->adva_type;
+            } else {
+                memcpy(aux_data->adva, *addr, 6);
+                aux_data->adva_type = *addr_type;
+            }
+        }
+
+        if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) {
+            if (!has_inita) {
+                *inita = aux_data->targeta;
+                *inita_type = aux_data->targeta_type;
+            } else {
+                memcpy(aux_data->targeta, *inita, 6);
+                aux_data->targeta_type = *inita_type;
+            }
+        }
+    }
+
+    return 0;
+}
+#endif
+
+int
+ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf,
+                            struct ble_mbuf_hdr *ble_hdr,
+                            uint8_t **addr, uint8_t *addr_type,
+                            uint8_t **inita, uint8_t *inita_type,
+                            int *ext_mode)
+{
+    /*
+     * XXX this should be only used for legacy advertising, but need to refactor
+     *     code in ble_ll_init first so it does not call this for ext
+     */
+
+    if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND &&
+        pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) {
+        /* Legacy advertising */
+        *addr_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK);
+        *addr = rxbuf + BLE_LL_PDU_HDR_LEN;
+
+        if (pdu_type != BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) {
+            *inita = NULL;
+            *inita_type = 0;
+            return 0;
+        }
+
+        *inita = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN;
+        *inita_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK);
+
+        return 0;
+    }
+
+    /* Extended advertising */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    return ble_ll_scan_get_addr_from_ext_adv(rxbuf, ble_hdr, addr, addr_type,
+                                          inita, inita_type, ext_mode);
+#else
+    return -1;
+#endif
+
+    return 0;
+}
+
+static void
+ble_ll_scan_get_addr_data_from_legacy(uint8_t pdu_type, uint8_t *rxbuf,
+                                     struct ble_ll_scan_addr_data *addrd)
+{
+    BLE_LL_ASSERT(pdu_type < BLE_ADV_PDU_TYPE_ADV_EXT_IND);
+
+    addrd->adva_present = true;
+
+    addrd->adva = rxbuf + BLE_LL_PDU_HDR_LEN;
+    addrd->adva_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK);
+
+    if (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) {
+        addrd->targeta = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN;
+        addrd->targeta_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK);
+    } else {
+        addrd->targeta = NULL;
+        addrd->targeta_type = 0;
+    }
+}
+
+/*
+ * Matches incoming PDU using scan filter policy and whitelist, if applicable.
+ * This will also resolve addresses and update flags/fields in header and
+ * addr_data as needed.
+ *
+ * @return  0 = no match
+ *          1 = match
+ *          2 = match, but do not scan
+ */
+static int
+ble_ll_scan_rx_filter(struct ble_mbuf_hdr *hdr, struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    struct ble_ll_scan_params *scanp = scansm->scanp;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_aux_data *aux_data = hdr->rxinfo.user_data;
+#endif
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    struct ble_ll_resolv_entry *rl = NULL;
+#endif
+    bool scan_req_allowed = true;
+    int resolved = 0;
+
+    /* Use AdvA as initial advertiser address, we may try to resolve it later */
+    addrd->adv_addr = addrd->adva;
+    addrd->adv_addr_type = addrd->adva_type;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* By default, assume AdvA is not resolved */
+    rxinfo->rpa_index = -1;
+
+    switch (ble_ll_addr_subtype(addrd->adva, addrd->adva_type)) {
+    case BLE_LL_ADDR_SUBTYPE_RPA:
+        /*
+         * Only resolve if packet actually contained AdvA.
+         * In extended advertising PDUs we may use RL index from a PDU that
+         * already had AdvA (e.g. ADV_EXT_IND in case of AUX_ADV_IND without
+         * AdvA). In legacy advertising PDUs we always need to resolve AdvA.
+         */
+        if (addrd->adva_present) {
+            rxinfo->rpa_index = ble_hw_resolv_list_match();
+        } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+            BLE_LL_ASSERT(aux_data);
+            rxinfo->rpa_index = aux_data->rpa_index;
+#else
+            BLE_LL_ASSERT(false);
+            rxinfo->rpa_index = -1;
+#endif
+        }
+
+        if (rxinfo->rpa_index < 0) {
+            break;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        if (aux_data) {
+            aux_data->rpa_index = rxinfo->rpa_index;
+        }
+#endif
+
+        /* Use resolved identity address as advertiser address */
+        rl = &g_ble_ll_resolv_list[rxinfo->rpa_index];
+        addrd->adv_addr = rl->rl_identity_addr;
+        addrd->adv_addr_type = rl->rl_addr_type;
+        addrd->rl = rl;
+
+        rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED;
+        resolved = 1;
+        break;
+    case BLE_LL_ADDR_SUBTYPE_IDENTITY:
+        /*
+         * If AdvA is an identity address, we need to check if that device was
+         * added to RL in order to use proper privacy mode.
+         */
+        rl = ble_ll_resolv_list_find(addrd->adva, addrd->adva_type);
+        if (!rl) {
+            break;
+        }
+
+        addrd->rl = rl;
+
+        /* Ignore device if using network privacy mode and it has IRK */
+        if ((rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && rl->rl_has_peer) {
+            return 0;
+        }
+        break;
+    default:
+        /* NRPA goes through filtering policy directly */
+        break;
+    }
+
+    if (addrd->targeta) {
+        switch (ble_ll_addr_subtype(addrd->targeta, addrd->targeta_type)) {
+        case BLE_LL_ADDR_SUBTYPE_RPA:
+            /* Check if TargetA can be resolved using the same RL entry as AdvA */
+            if (rl && ble_ll_resolv_rpa(addrd->targeta, rl->rl_local_irk)) {
+                rxinfo->flags |= BLE_MBUF_HDR_F_TARGETA_RESOLVED;
+                break;
+            }
+
+            /* Check if scan filter policy allows unresolved RPAs to be processed */
+            if (!(scanp->scan_filt_policy & 0x02)) {
+                return 0;
+            }
+
+            /*
+             * We will notify host as requited by scan policy, but make sure we
+             * do not send scan request since we do not know if this is directed
+             * to us.
+             */
+            scan_req_allowed = false;
+            break;
+        case BLE_LL_ADDR_SUBTYPE_IDENTITY:
+            /* We shall ignore identity in TargetA if we are using RPA */
+            if ((scanp->own_addr_type & 0x02) && rl && rl->rl_has_local) {
+                return 0;
+            }
+            /* Ignore if not directed to us */
+            if (!ble_ll_is_our_devaddr(addrd->targeta, addrd->targeta_type)) {
+                return 0;
+            }
+            break;
+        default:
+            /* NRPA goes through filtering policy directly */
+            break;
+        }
+    }
+#else
+    /* Ignore if not directed to us */
+    if (addrd->targeta &&
+        !ble_ll_is_our_devaddr(addrd->targeta, addrd->targeta_type)) {
+        return 0;
+    }
+#endif
+
+    /* Check on WL if required by scan filter policy */
+    if (scanp->scan_filt_policy & 0x01) {
+        if (!ble_ll_whitelist_match(addrd->adv_addr, addrd->adv_addr_type, resolved)) {
+            return 0;
+        }
+    }
+
+    return scan_req_allowed ? 1 : 2;
+}
+
+static int
+ble_ll_scan_rx_isr_on_legacy(uint8_t pdu_type, uint8_t *rxbuf,
+                             struct ble_mbuf_hdr *hdr,
+                             struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    struct ble_ll_scan_params *scanp = scansm->scanp;
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    uint8_t sreq_adva_type;
+    uint8_t *sreq_adva;
+    int rc;
+
+    ble_ll_scan_get_addr_data_from_legacy(pdu_type, rxbuf, addrd);
+
+    if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) {
+        if (!BLE_MBUF_HDR_SCAN_RSP_RXD(hdr)) {
+            /*
+             * We were not expecting scan response so just ignore and do not
+             * update backoff.
+             */
+            return -1;
+        }
+
+        sreq_adva_type = !!(scansm->pdu_data.hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK);
+        sreq_adva = scansm->pdu_data.adva;
+
+        /*
+         * Ignore scan response if AdvA does not match AdvA in request and also
+         * update backoff as if there was no scan response.
+         */
+        if ((addrd->adva_type != sreq_adva_type) ||
+            memcmp(addrd->adva, sreq_adva, BLE_DEV_ADDR_LEN)) {
+            ble_ll_scan_req_backoff(scansm, 0);
+            return -1;
+        }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        /*
+         * We are not pushing this one through filters so need to update
+         * rpa_index here as otherwise pkt_in won't be able to determine
+         * advertiser address properly.
+         */
+        rxinfo->rpa_index = ble_hw_resolv_list_match();
+        if (rxinfo->rpa_index >= 0) {
+            rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED;
+        }
+#endif
+
+        rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH;
+
+        return 0;
+    }
+
+    rc = ble_ll_scan_rx_filter(hdr, addrd);
+    if (!rc) {
+        return 0;
+    }
+
+    rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH;
+
+    if (rc == 2) {
+        /* Scan request forbidden by filter policy */
+        return 0;
+    }
+
+    return (scanp->scan_type == BLE_SCAN_TYPE_ACTIVE) &&
+           ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) ||
+            (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND));
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static int
+ble_ll_scan_rx_isr_on_aux(uint8_t pdu_type, uint8_t *rxbuf,
+                          struct ble_mbuf_hdr *hdr,
+                          struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    struct ble_ll_scan_params *scanp = scansm->scanp;
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    struct ble_ll_aux_data *aux_data;
+    int rc;
+
+    if (!scansm->ext_scanning) {
+        return -1;
+    }
+
+    rc = ble_ll_scan_update_aux_data(hdr, rxbuf, &addrd->adva_present);
+    if (rc < 0) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_AUX_INVALID;
+        return -1;
+    } else if (rc == 0) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_AUX_PTR_WAIT;
+    }
+
+    /* Now we can update aux_data from header since it may have just been created */
+    aux_data = rxinfo->user_data;
+
+    /*
+     * Restore proper header flags if filtering was already done successfully on
+     * some previous PDU in an event.
+     */
+    if (aux_data->flags & BLE_LL_AUX_IS_MATCHED) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        rxinfo->rpa_index = aux_data->rpa_index;
+        if (rxinfo->rpa_index >= 0) {
+            rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED;
+        }
+        if (aux_data->flags & BLE_LL_AUX_IS_TARGETA_RESOLVED) {
+            rxinfo->flags |= BLE_MBUF_HDR_F_TARGETA_RESOLVED;
+        }
+#endif
+        goto done;
+    }
+
+    if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) {
+        addrd->adva = aux_data->adva;
+        addrd->adva_type = aux_data->adva_type;
+    } else {
+        /* Accept this PDU and wait for AdvA in aux */
+        rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH;
+        return 0;
+    }
+    if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) {
+        addrd->targeta = aux_data->targeta;
+        addrd->targeta_type = aux_data->targeta_type;
+    } else {
+        addrd->targeta = NULL;
+    }
+
+    rc = ble_ll_scan_rx_filter(hdr, addrd);
+    if (!rc) {
+        return 0;
+    }
+
+    rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH;
+
+    /*
+     * Once we matched device, there's no need to go through filtering on every
+     * other PDU in an event so just store info required to restore state for
+     * subsequent PDUs in aux_data.
+     */
+    aux_data->flags |= BLE_LL_AUX_IS_MATCHED;
+    if (rxinfo->flags & BLE_MBUF_HDR_F_TARGETA_RESOLVED) {
+        aux_data->flags |= BLE_LL_AUX_IS_TARGETA_RESOLVED;
+        /* AdvA state is already stored in rpa_index */
+    }
+
+    if (rc == 2) {
+        /* Scan request forbidden by filter policy */
+        return 0;
+    }
+
+done:
+    return (scanp->scan_type == BLE_SCAN_TYPE_ACTIVE) &&
+           ((rxbuf[2] >> 6) == BLE_LL_EXT_ADV_MODE_SCAN);
+}
+#endif
+
+static bool
+ble_ll_scan_send_scan_req(uint8_t pdu_type, uint8_t *rxbuf,
+                          struct ble_mbuf_hdr *hdr,
+                          struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_aux_data *aux_data = rxinfo->user_data;
+    uint8_t phy_mode;
+#endif
+    bool is_ext_adv = false;
+    uint16_t adi = 0;
+    int rc;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) {
+        if (ble_ll_scan_get_adi(aux_data, &adi) < 0) {
+            return false;
+        }
+        is_ext_adv = true;
+    }
+#endif
+
+    /* Check if we already scanned this device successfully */
+    if (ble_ll_scan_have_rxd_scan_rsp(addrd->adv_addr, addrd->adv_addr_type,
+                                      is_ext_adv, adi)) {
+        return false;
+    }
+
+    /* Better not be a scan response pending */
+    BLE_LL_ASSERT(scansm->scan_rsp_pending == 0);
+
+    /* We want to send a request. See if backoff allows us */
+    if (scansm->backoff_count > 0) {
+        if (--scansm->backoff_count != 0) {
+            return false;
+        }
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    phy_mode = ble_ll_phy_to_phy_mode(rxinfo->phy, BLE_HCI_LE_PHY_CODED_ANY);
+    if (ble_ll_sched_scan_req_over_aux_ptr(rxinfo->channel, phy_mode)) {
+        return false;
+    }
+#endif
+
+    /* Use original AdvA in scan request (Core 5.1, Vol 6, Part B, section 6.3) */
+    ble_ll_scan_req_pdu_prepare(scansm, addrd->adva, addrd->adva_type, addrd->rl);
+
+    rc = ble_phy_tx(ble_ll_scan_req_tx_pdu_cb, scansm, BLE_PHY_TRANSITION_TX_RX);
+    if (rc) {
+        return false;
+    }
+
+    scansm->scan_rsp_pending = 1;
+    rxinfo->flags |= BLE_MBUF_HDR_F_SCAN_REQ_TXD;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (rxinfo->channel <  BLE_PHY_NUM_DATA_CHANS) {
+        /* Keep aux_data for expected scan response */
+        scansm->cur_aux_data = ble_ll_scan_aux_data_ref(aux_data);
+        STATS_INC(ble_ll_stats, aux_scan_req_tx);
+    }
+#endif
+
+    return true;
+}
+
+/**
+ * Called when a receive PDU has ended.
+ *
+ * Context: Interrupt
+ *
+ * @param rxpdu
+ *
+ * @return int
+ *       < 0: Disable the phy after reception.
+ *      == 0: Success. Do not disable the PHY.
+ *       > 0: Do not disable PHY as that has already been done.
+ */
+int
+ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    struct ble_mbuf_hdr *hdr = BLE_MBUF_HDR_PTR(rxpdu);
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    uint8_t *rxbuf;
+    uint8_t pdu_type;
+    struct ble_ll_scan_addr_data addrd;
+    int rc;
+
+    /*
+     * If buffer for incoming PDU was not allocated we need to force scan to be
+     * restarted since LL will not be notified. Keep PHY enabled.
+     */
+    if (rxpdu == NULL) {
+        ble_ll_scan_interrupted(scansm);
+        return 0;
+    }
+
+    rxbuf = rxpdu->om_data;
+    pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /*
+     * In case aux was expected, copy aux_data for LL to use. Make sure this was
+     * indeed an aux as otherwise there's no need to process it and just pass to
+     * LL immediately.
+     */
+    if (scansm->cur_aux_data) {
+        rxinfo->user_data = scansm->cur_aux_data;
+        scansm->cur_aux_data = NULL;
+        if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND) {
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+            return -1;
+        }
+    }
+#endif
+
+    if (!crcok) {
+        goto scan_rx_isr_ignore;
+    }
+
+    /*
+     * Addresses will be always set in handlers, no need to initialize them. We
+     * only need to initialize rl which may not be always set, depending on how
+     * filtering goes.
+     */
+    addrd.rl = NULL;
+
+    switch (pdu_type) {
+    case BLE_ADV_PDU_TYPE_ADV_IND:
+    case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND:
+    case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND:
+    case BLE_ADV_PDU_TYPE_SCAN_RSP:
+    case BLE_ADV_PDU_TYPE_ADV_SCAN_IND:
+        rc = ble_ll_scan_rx_isr_on_legacy(pdu_type, rxbuf, hdr, &addrd);
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_ADV_PDU_TYPE_ADV_EXT_IND:
+        rc = ble_ll_scan_rx_isr_on_aux(pdu_type, rxbuf, hdr, &addrd);
+        break;
+#endif
+    default:
+        /* This is not something we would like to process here */
+        rc = -1;
+        break;
+    }
+
+    if (rc == -1) {
+        goto scan_rx_isr_ignore;
+    } else if (rc == 1) {
+        if (ble_ll_scan_send_scan_req(pdu_type, rxbuf, hdr, &addrd)) {
+            /* Keep PHY active and LL in scanning state */
+            return 0;
+        }
+    }
+
+    /* We are done with this PDU so go to standby and let LL resume if needed */
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    return -1;
+
+scan_rx_isr_ignore:
+    rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED;
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    return -1;
+}
+
+/**
+ * Called to resume scanning. This is called after an advertising event or
+ * connection event has ended. It is also called if we receive a packet while
+ * in the initiating or scanning state.
+ *
+ * If periodic advertising is enabled this is also called on sync event end
+ * or sync packet received if chaining
+ *
+ * Context: Link Layer task
+ */
+void
+ble_ll_scan_chk_resume(void)
+{
+    os_sr_t sr;
+    struct ble_ll_scan_sm *scansm;
+    uint32_t now;
+
+    scansm = &g_ble_ll_scan_sm;
+    if (scansm->scan_enabled) {
+        OS_ENTER_CRITICAL(sr);
+        if (scansm->restart_timer_needed) {
+            scansm->restart_timer_needed = 0;
+            ble_ll_event_send(&scansm->scan_sched_ev);
+            STATS_INC(ble_ll_stats, scan_timer_restarted);
+            OS_EXIT_CRITICAL(sr);
+            return;
+        }
+
+        now = os_cputime_get32();
+        if (ble_ll_state_get() == BLE_LL_STATE_STANDBY &&
+            ble_ll_scan_is_inside_window(scansm->scanp, now)) {
+            /* Turn on the receiver and set state */
+            ble_ll_scan_start(scansm, NULL);
+        }
+        OS_EXIT_CRITICAL(sr);
+    }
+}
+
+/**
+ * Scan timer callback; means that the scan window timeout has been reached
+ * and we should perform the appropriate actions.
+ *
+ * Context: Interrupt (cputimer)
+ *
+ * @param arg Pointer to scan state machine.
+ */
+void
+ble_ll_scan_timer_cb(void *arg)
+{
+    struct ble_ll_scan_sm *scansm;
+
+    scansm = (struct ble_ll_scan_sm *)arg;
+    ble_ll_event_send(&scansm->scan_sched_ev);
+}
+
+void
+ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_npl_event_set_arg(&scansm->scan_interrupted_ev, scansm->cur_aux_data);
+    scansm->cur_aux_data = NULL;
+#endif
+
+    ble_ll_event_send(&scansm->scan_interrupted_ev);
+}
+
+/**
+ * Called when the wait for response timer expires while in the scanning
+ * state.
+ *
+ * Context: Interrupt.
+ */
+void
+ble_ll_scan_wfr_timer_exp(void)
+{
+    struct ble_ll_scan_sm *scansm;
+    uint8_t chan;
+    int phy;
+    int rc;
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    uint8_t phy_mode;
+#endif
+    uint32_t now;
+
+    scansm = &g_ble_ll_scan_sm;
+
+    /* Update backoff if we failed to receive scan response */
+    if (scansm->scan_rsp_pending) {
+        scansm->scan_rsp_pending = 0;
+        ble_ll_scan_req_backoff(scansm, 0);
+    }
+
+    if (scansm->cur_aux_data) {
+        /* We actually care about interrupted scan only for EXT ADV because only
+         * then we might consider to send truncated event to the host.
+         */
+        ble_ll_scan_interrupted(scansm);
+
+        /* Need to disable phy since we are going to move to BLE_LL_STATE_STANDBY
+         * or we will need to change channel to primary one
+         */
+        ble_phy_disable();
+
+        now = os_cputime_get32();
+        if (!ble_ll_scan_is_inside_window(scansm->scanp, now)) {
+            /* Outside the window scan */
+            ble_ll_state_set(BLE_LL_STATE_STANDBY);
+            return;
+        }
+
+        ble_ll_get_chan_to_scan(scansm, &chan, &phy);
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        phy_mode = ble_ll_phy_to_phy_mode(phy, BLE_HCI_LE_PHY_CODED_ANY);
+        ble_phy_mode_set(phy_mode, phy_mode);
+#endif
+        rc = ble_phy_setchan(chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV);
+        BLE_LL_ASSERT(rc == 0);
+    }
+
+
+    ble_phy_restart_rx();
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/*
+ * Send extended advertising report
+ *
+ * @return -1 on error (data truncated or other error)
+ *          0 on success (data status is "completed")
+ *          1 on success (data status is not "completed")
+ */
+static int
+ble_ll_hci_send_ext_adv_report(uint8_t ptype, uint8_t *adva, uint8_t adva_type,
+                               uint8_t *inita, uint8_t inita_type,
+                               struct os_mbuf *om,
+                               struct ble_mbuf_hdr *hdr)
+{
+    struct ble_ll_aux_data *aux_data = hdr->rxinfo.user_data;
+    struct ble_hci_ev_le_subev_ext_adv_rpt *ev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+    struct ble_hci_ev *hci_ev_next;
+    int offset;
+    int datalen;
+    int rc;
+    bool need_event;
+    bool is_scannable_aux;
+    uint8_t max_data_len;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+        rc = -1;
+        goto done;
+    }
+
+    /*
+     * We keep one allocated event in aux_data to be able to truncate chain
+     * properly in case of error. If there is no event in aux_data it means this
+     * is the first event for this chain.
+     */
+    if (aux_data && aux_data->evt) {
+        hci_ev = aux_data->evt;
+        aux_data->evt = NULL;
+
+        hci_ev->length = sizeof(*ev) + sizeof(*report);
+    } else {
+        hci_ev = ble_ll_scan_get_ext_adv_report(NULL);
+        if (!hci_ev) {
+            rc = -1;
+            goto done;
+        }
+    }
+
+    ev = (void *) hci_ev->data;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* If RPA has been used, make sure we use correct address types
+     * in the advertising report.
+     */
+    if (BLE_MBUF_HDR_RESOLVED(hdr)) {
+        adva_type += 2;
+    }
+    if (BLE_MBUF_HDR_TARGETA_RESOLVED(hdr)) {
+        inita_type += 2;
+    }
+#endif
+
+    datalen = ble_ll_scan_parse_ext_hdr(om, adva, adva_type, inita, inita_type,
+                                        hdr, ev->reports);
+    if (datalen < 0) {
+        rc = -1;
+
+        /* Need to send truncated event if we already sent some reports */
+        if (aux_data && (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY)) {
+            BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED));
+            BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED));
+
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED;
+
+            report = ev->reports;
+            report->data_len = 0;
+            report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED;
+
+            ble_ll_hci_event_send(hci_ev);
+            goto done;
+        }
+
+        ble_hci_trans_buf_free((uint8_t *)hci_ev);
+        goto done;
+    }
+
+    is_scannable_aux = aux_data &&
+                       (aux_data->evt_type & BLE_HCI_ADV_SCAN_MASK) &&
+                       !(aux_data->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK);
+
+    max_data_len = BLE_LL_MAX_EVT_LEN - sizeof(*hci_ev) - sizeof(*ev) - sizeof(*report);
+    offset = 0;
+
+    do {
+        hci_ev_next = NULL;
+
+        ev = (void *) hci_ev->data;
+        report = ev->reports;
+
+        report->data_len = min(max_data_len, datalen - offset);
+
+        /* adjust event length */
+        hci_ev->length += report->data_len;
+        report->rssi = hdr->rxinfo.rssi;
+
+        os_mbuf_copydata(om, offset, report->data_len, report->data);
+        offset += report->data_len;
+
+        /*
+         * We need another event if either there are still some data left to
+         * send in current PDU or scan is not completed. The only exception is
+         * when this is a scannable event which is not a scan response.
+         */
+        need_event = ((offset < datalen) || (aux_data && !(aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_COMPLETE))) && !is_scannable_aux;
+
+        if (need_event) {
+            /*
+             * We will need another event so let's try to allocate one now. If
+             * we cannot do this, need to mark event as truncated.
+             */
+            hci_ev_next = ble_ll_scan_get_ext_adv_report(report);
+
+            if (hci_ev_next) {
+                report->evt_type |= BLE_HCI_ADV_DATA_STATUS_INCOMPLETE;
+                rc = 1;
+            } else {
+                report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED;
+                rc = -1;
+            }
+        } else if (aux_data && (aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR)) {
+            report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED;
+            rc = -1;
+        } else {
+            rc = 0;
+        }
+
+        if ((rc == -1) && aux_data) {
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+
+            if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY)) {
+                ble_hci_trans_buf_free((uint8_t *)hci_ev);
+                goto  done;
+            }
+
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED;
+        } else if (!is_scannable_aux) {
+            /*
+             * We do not set 'sent' flags for scannable AUX since we only care
+             * about scan response that will come next.
+             */
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_ANY;
+            if (rc == 0) {
+                aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED;
+            }
+        }
+
+        ble_ll_hci_event_send(hci_ev);
+
+        hci_ev = hci_ev_next;
+    } while ((offset < datalen) && hci_ev);
+
+    BLE_LL_ASSERT(offset <= datalen);
+
+    if (aux_data) {
+        /* Store any event left for later use */
+        aux_data->evt = hci_ev;
+    } else {
+        /* If it is empty beacon, evt shall be NULL */
+        BLE_LL_ASSERT(!hci_ev);
+    }
+
+done:
+    if (!aux_data) {
+        return rc;
+    }
+
+    if (rc == 0) {
+        if (aux_data->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK) {
+            /* Complete scan response can be added to duplicates list */
+            ble_ll_scan_add_scan_rsp_adv(aux_data->adva, aux_data->adva_type,
+                                         1, aux_data->adi);
+        } else if (is_scannable_aux) {
+            /*
+             * Scannable AUX is marked as incomplete because we do not want to
+             * add this to duplicates list now, this should happen only after
+             * we receive complete scan response. The drawback here is that we
+             * will keep receiving reports for scannable PDUs until complete
+             * scan response is received.
+             *
+             * XXX ^^ extend duplicates list to fix
+             */
+            rc = 1;
+        }
+    } else if (rc < 0) {
+        aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+    }
+
+    return rc;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+static void
+ble_ll_scan_check_periodic_sync(const struct os_mbuf *om, struct ble_mbuf_hdr *rxhdr,
+                          uint8_t *adva, uint8_t adva_type, int rpa_index)
+{
+    uint8_t pdu_len;
+    uint8_t ext_hdr_len;
+    uint8_t ext_hdr_flags;
+    uint8_t *ext_hdr;
+    uint8_t *rxbuf = om->om_data;
+    uint8_t sid;
+    int i;
+
+    pdu_len = rxbuf[1];
+    if (pdu_len == 0) {
+        return;
+    }
+
+    ext_hdr_len = rxbuf[2] & 0x3F;
+
+    if (ext_hdr_len) {
+        ext_hdr_flags = rxbuf[3];
+        ext_hdr = &rxbuf[4];
+        i = 0;
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+            i += BLE_LL_EXT_ADV_ADVA_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+            i += BLE_LL_EXT_ADV_TARGETA_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+            i += 1;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+            sid = (get_le16(ext_hdr + i) >> 12);
+            i += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+        } else {
+            /* ADI is mandatory */
+            return;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+            i += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+            ble_ll_sync_info_event(adva, adva_type, rpa_index, sid, rxhdr,
+                                   ext_hdr + i);
+        }
+    }
+}
+#endif
+
+static inline void
+ble_ll_scan_dup_move_to_head(struct ble_ll_scan_dup_entry *e)
+{
+    if (e != TAILQ_FIRST(&g_scan_dup_list)) {
+        TAILQ_REMOVE(&g_scan_dup_list, e, link);
+        TAILQ_INSERT_HEAD(&g_scan_dup_list, e, link);
+    }
+}
+
+static inline struct ble_ll_scan_dup_entry *
+ble_ll_scan_dup_new(void)
+{
+    struct ble_ll_scan_dup_entry *e;
+
+    e = os_memblock_get(&g_scan_dup_pool);
+    if (!e) {
+        e = TAILQ_LAST(&g_scan_dup_list, ble_ll_scan_dup_list);
+        TAILQ_REMOVE(&g_scan_dup_list, e, link);
+    }
+
+    memset(e, 0, sizeof(*e));
+
+    return e;
+}
+
+static int
+ble_ll_scan_dup_check_legacy(uint8_t addr_type, uint8_t *addr, uint8_t pdu_type)
+{
+    struct ble_ll_scan_dup_entry *e;
+    uint8_t type;
+    int rc;
+
+    type = BLE_LL_SCAN_ENTRY_TYPE_LEGACY(addr_type);
+
+    TAILQ_FOREACH(e, &g_scan_dup_list, link) {
+        if ((e->type == type) && !memcmp(e->addr, addr, 6)) {
+            break;
+        }
+    }
+
+    if (e) {
+        if (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) {
+            rc = e->flags & BLE_LL_SCAN_DUP_F_DIR_ADV_REPORT_SENT;
+        } else if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) {
+            rc = e->flags & BLE_LL_SCAN_DUP_F_SCAN_RSP_SENT;
+        } else {
+            rc = e->flags & BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT;
+        }
+
+        ble_ll_scan_dup_move_to_head(e);
+    } else {
+        rc = 0;
+
+        e = ble_ll_scan_dup_new();
+        e->flags = 0;
+        e->type = type;
+        memcpy(e->addr, addr, 6);
+
+        TAILQ_INSERT_HEAD(&g_scan_dup_list, e, link);
+    }
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static int
+ble_ll_scan_dup_check_ext(uint8_t addr_type, uint8_t *addr,
+                          struct ble_ll_aux_data *aux_data)
+{
+    struct ble_ll_scan_dup_entry *e;
+    bool has_aux;
+    bool is_anon;
+    uint16_t adi;
+    uint8_t type;
+    int rc;
+
+    has_aux = aux_data != NULL;
+    is_anon = addr == NULL;
+    adi = has_aux ? aux_data->adi : 0;
+
+    type = BLE_LL_SCAN_ENTRY_TYPE_EXT(addr_type, has_aux, is_anon, adi);
+
+    TAILQ_FOREACH(e, &g_scan_dup_list, link) {
+        if ((e->type == type) &&
+            (is_anon || !memcmp(e->addr, addr, BLE_DEV_ADDR_LEN))) {
+            break;
+        }
+    }
+
+    if (e) {
+        if (e->adi != adi) {
+            rc = 0;
+
+            e->flags = 0;
+            e->adi = adi;
+        } else {
+            rc = e->flags & BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT;
+        }
+
+        ble_ll_scan_dup_move_to_head(e);
+    } else {
+        rc = 0;
+
+        e = ble_ll_scan_dup_new();
+        e->flags = 0;
+        e->type = type;
+        e->adi = adi;
+        if (!is_anon) {
+            memcpy(e->addr, addr, 6);
+        }
+
+        TAILQ_INSERT_HEAD(&g_scan_dup_list, e, link);
+    }
+
+    return rc;
+}
+
+static int
+ble_ll_scan_dup_update_ext(uint8_t addr_type, uint8_t *addr,
+                           struct ble_ll_aux_data *aux_data)
+{
+    struct ble_ll_scan_dup_entry *e;
+    bool has_aux;
+    bool is_anon;
+    uint16_t adi;
+    uint8_t type;
+
+    has_aux = aux_data != NULL;
+    is_anon = addr == NULL;
+    adi = has_aux ? aux_data->adi : 0;
+
+    type = BLE_LL_SCAN_ENTRY_TYPE_EXT(addr_type, has_aux, is_anon, adi);
+
+    /*
+     * We assume ble_ll_scan_dup_check() was called before which either matched
+     * some entry or allocated new one and placed in on the top of queue.
+     */
+
+    e = TAILQ_FIRST(&g_scan_dup_list);
+    BLE_LL_ASSERT(e && e->type == type && (is_anon || !memcmp(e->addr, addr, 6)));
+
+    e->flags |= BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT;
+
+    return 0;
+}
+#endif
+
+static void
+ble_ll_scan_rx_pkt_in_restore_addr_data(struct ble_mbuf_hdr *hdr,
+                                        struct ble_ll_scan_addr_data *addrd)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    struct ble_ll_resolv_entry *rl;
+#endif
+
+    addrd->adv_addr = addrd->adva;
+    addrd->adv_addr_type = addrd->adva_type;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (rxinfo->rpa_index >= 0) {
+        rl = &g_ble_ll_resolv_list[rxinfo->rpa_index];
+        addrd->adv_addr = rl->rl_identity_addr;
+        addrd->adv_addr_type = rl->rl_addr_type;
+        addrd->rl = rl;
+    }
+    if (hdr->rxinfo.flags & BLE_MBUF_HDR_F_TARGETA_RESOLVED) {
+        addrd->targeta = ble_ll_get_our_devaddr(scansm->own_addr_type & 1);
+        addrd->targeta_type = scansm->own_addr_type & 1;
+    }
+#endif
+}
+
+static void
+ble_ll_scan_rx_pkt_in_on_legacy(uint8_t pdu_type, struct os_mbuf *om,
+                                struct ble_mbuf_hdr *hdr,
+                                struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+    uint8_t *rxbuf = om->om_data;
+    bool send_hci_report;
+
+
+    if (!BLE_MBUF_HDR_DEVMATCH(hdr) ||
+        !BLE_MBUF_HDR_CRC_OK(hdr) ||
+        BLE_MBUF_HDR_IGNORED(hdr)) {
+        return;
+    }
+
+    ble_ll_scan_get_addr_data_from_legacy(pdu_type, rxbuf, addrd);
+    ble_ll_scan_rx_pkt_in_restore_addr_data(hdr, addrd);
+
+    send_hci_report = !scansm->scan_filt_dups ||
+                      !ble_ll_scan_dup_check_legacy(addrd->adv_addr_type,
+                                                    addrd->adv_addr,
+                                                    pdu_type);
+    if (send_hci_report) {
+        /* Sending advertising report will also update scan_dup list */
+        ble_ll_scan_send_adv_report(pdu_type,
+                                    addrd->adv_addr, addrd->adv_addr_type,
+                                    addrd->targeta, addrd->targeta_type,
+                                    om, hdr, scansm);
+    }
+
+    if (BLE_MBUF_HDR_SCAN_RSP_RXD(hdr)) {
+        ble_ll_scan_req_backoff(scansm, 1);
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_scan_rx_pkt_in_on_aux(uint8_t pdu_type, struct os_mbuf *om,
+                             struct ble_mbuf_hdr *hdr,
+                             struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    uint8_t *rxbuf = om->om_data;
+#endif
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    struct ble_ll_aux_data *aux_data = rxinfo->user_data;
+    bool send_hci_report;
+    int rc;
+
+    if (!scansm->ext_scanning) {
+        goto scan_continue;
+    }
+
+    if (aux_data) {
+        aux_data->flags_ll |= aux_data->flags_isr;
+    }
+
+    /*
+     * For every new extended advertising event scanned, rx_isr_end will either
+     * allocate new aux_data or set 'invalid' flag. This means if no 'invalid'
+     * flag is set, aux_data is always valid.
+     */
+
+    /* Drop on scan error or if we received not what we expected to receive */
+    if (!BLE_MBUF_HDR_CRC_OK(hdr) ||
+        BLE_MBUF_HDR_IGNORED(hdr) ||
+        BLE_MBUF_HDR_AUX_INVALID(hdr) ||
+        (aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR) ||
+        (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND)) {
+        if (aux_data) {
+            ble_ll_scan_end_adv_evt(aux_data);
+            ble_ll_scan_aux_data_unref(aux_data);
+            rxinfo->user_data = NULL;
+        }
+        return;
+    }
+
+    BLE_LL_ASSERT(aux_data);
+
+    if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) {
+        addrd->adva = aux_data->adva;
+        addrd->adva_type = aux_data->adva_type;
+    } else {
+        addrd->adva = NULL;
+        addrd->adva_type = 0;
+    }
+    if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) {
+        addrd->targeta = aux_data->targeta;
+        addrd->targeta_type = aux_data->targeta_type;
+    } else {
+        addrd->targeta = NULL;
+        addrd->targeta_type = 0;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    /*
+     * Periodic scan uses own filter list so we need to let it do own filtering
+     * regardless of scanner filtering. Just make sure we already have AdvA.
+     */
+    if (ble_ll_sync_enabled() &&
+        ((rxbuf[2] >> 6) == BLE_LL_EXT_ADV_MODE_NON_CONN) && addrd->adva &&
+        !(aux_data->flags_ll & BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED)) {
+        ble_ll_scan_check_periodic_sync(om, hdr, addrd->adva, addrd->adva_type,
+                                        rxinfo->rpa_index);
+    }
+#endif
+
+    /* Ignore if device was not matched by either whitelist or scan policy */
+    if (!BLE_MBUF_HDR_DEVMATCH(hdr)) {
+        goto scan_continue;
+    }
+
+    ble_ll_scan_rx_pkt_in_restore_addr_data(hdr, addrd);
+
+    /*
+     * If there is AuxPtr in this PDU, we should first try to schedule scan for
+     * subsequent aux.
+     */
+    if (BLE_MBUF_HDR_WAIT_AUX(hdr)) {
+        if (ble_ll_sched_aux_scan(hdr, scansm, aux_data)) {
+            rxinfo->flags &= ~BLE_MBUF_HDR_F_AUX_PTR_WAIT;
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+
+            /* Silently ignore if no HCI event was sent to host */
+            if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY)) {
+                goto scan_continue;
+            }
+        }
+
+        /* Ignore if this was just ADV_EXT_IND with AuxPtr, will process aux */
+        if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED)) {
+            goto scan_continue;
+        }
+
+        STATS_INC(ble_ll_stats, aux_chain_cnt);
+    }
+
+    send_hci_report = !scansm->scan_filt_dups ||
+                      !ble_ll_scan_dup_check_ext(addrd->adv_addr_type,
+                                                 addrd->adv_addr, aux_data);
+    if (send_hci_report) {
+        rc = ble_ll_hci_send_ext_adv_report(pdu_type,
+                                            addrd->adv_addr, addrd->adv_addr_type,
+                                            addrd->targeta, addrd->targeta_type,
+                                            om, hdr);
+        if ((rc < 0) && BLE_MBUF_HDR_WAIT_AUX(hdr)) {
+            /* Data were truncated so stop scanning for subsequent auxes */
+            aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR;
+
+            if (ble_ll_sched_rmv_elem(&aux_data->sch) == 0) {
+                ble_ll_scan_aux_data_unref(aux_data->sch.cb_arg);
+                aux_data->sch.cb_arg = NULL;
+            }
+        } else if ((rc == 0) && scansm->scan_filt_dups) {
+            /* Complete data were send so we can update scan_dup list */
+            ble_ll_scan_dup_update_ext(addrd->adv_addr_type, addrd->adv_addr,
+                                       aux_data);
+        }
+    }
+
+    if (BLE_MBUF_HDR_SCAN_RSP_RXD(hdr)) {
+        /*
+         * For now assume success if we just received direct scan response,
+         * don't care about complete aux chain.
+         */
+        ble_ll_scan_req_backoff(scansm, 1);
+    }
+
+scan_continue:
+    ble_ll_scan_aux_data_unref(rxinfo->user_data);
+    rxinfo->user_data = NULL;
+}
+#endif
+
+/**
+ * Process a received PDU while in the scanning state.
+ *
+ * Context: Link Layer task.
+ *
+ * @param pdu_type
+ * @param rxbuf
+ */
+void
+ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hdr)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo;
+    struct ble_ll_aux_data *aux_data = rxinfo->user_data;
+#endif
+    struct ble_ll_scan_addr_data addrd;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (aux_data || (ptype == BLE_ADV_PDU_TYPE_ADV_EXT_IND)) {
+        ble_ll_scan_rx_pkt_in_on_aux(ptype, om, hdr, &addrd);
+        ble_ll_scan_chk_resume();
+        return;
+    }
+#endif
+
+    ble_ll_scan_rx_pkt_in_on_legacy(ptype, om, hdr, &addrd);
+    ble_ll_scan_chk_resume();
+}
+
+int
+ble_ll_scan_set_scan_params(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_scan_params_cp *cmd = (const void *)cmdbuf;
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* If already enabled, we return an error */
+    scansm = &g_ble_ll_scan_sm;
+    if (scansm->scan_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Get the scan interval and window */
+    scan_itvl = le16toh(cmd->scan_itvl);
+    scan_window = le16toh(cmd->scan_window);
+
+    /* Check scan type */
+    if ((cmd->scan_type != BLE_HCI_SCAN_TYPE_PASSIVE) &&
+        (cmd->scan_type != BLE_HCI_SCAN_TYPE_ACTIVE)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check interval and window */
+    if ((scan_itvl < BLE_HCI_SCAN_ITVL_MIN) ||
+        (scan_itvl > BLE_HCI_SCAN_ITVL_MAX) ||
+        (scan_window < BLE_HCI_SCAN_WINDOW_MIN) ||
+        (scan_window > BLE_HCI_SCAN_WINDOW_MAX) ||
+        (scan_itvl < scan_window)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check own addr type */
+    if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check scanner filter policy */
+    if (cmd->filter_policy > BLE_HCI_SCAN_FILT_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Store scan parameters */
+    scanp = &g_ble_ll_scan_params[PHY_UNCODED];
+    scanp->configured = 1;
+    scanp->scan_type = cmd->scan_type;
+    scanp->timing.interval = ble_ll_scan_time_hci_to_ticks(scan_itvl);
+    scanp->timing.window = ble_ll_scan_time_hci_to_ticks(scan_window);
+    scanp->scan_filt_policy = cmd->filter_policy;
+    scanp->own_addr_type = cmd->own_addr_type;
+
+#if (BLE_LL_SCAN_PHY_NUMBER == 2)
+    g_ble_ll_scan_params[PHY_CODED].configured = 0;
+#endif
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static int
+ble_ll_check_scan_params(uint8_t type, uint16_t itvl, uint16_t window)
+{
+    /* Check scan type */
+    if ((type != BLE_HCI_SCAN_TYPE_PASSIVE) &&
+        (type != BLE_HCI_SCAN_TYPE_ACTIVE)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Check interval and window */
+    if ((itvl < BLE_HCI_SCAN_ITVL_MIN) ||
+        (itvl > BLE_HCI_SCAN_ITVL_MAX) ||
+        (window < BLE_HCI_SCAN_WINDOW_MIN) ||
+        (window > BLE_HCI_SCAN_WINDOW_MAX) ||
+        (itvl < window)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return 0;
+}
+
+int
+ble_ll_set_ext_scan_params(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_ext_scan_params_cp *cmd = (const void *) cmdbuf;
+    const struct scan_params *params = cmd->scans;
+
+    struct ble_ll_scan_params new_params[BLE_LL_SCAN_PHY_NUMBER] = { };
+    struct ble_ll_scan_params *uncoded = &new_params[PHY_UNCODED];
+    struct ble_ll_scan_params *coded = &new_params[PHY_CODED];
+    uint16_t interval;
+    uint16_t window;
+    int rc;
+
+    if (len <= sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    len -= sizeof(*cmd);
+
+    /* If already enabled, we return an error */
+    if (g_ble_ll_scan_sm.scan_enabled) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Check own addr type */
+    if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    coded->own_addr_type = cmd->own_addr_type;
+    uncoded->own_addr_type = cmd->own_addr_type;
+
+    /* Check scanner filter policy */
+    if (cmd->filter_policy > BLE_HCI_SCAN_FILT_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    coded->scan_filt_policy = cmd->filter_policy;
+    uncoded->scan_filt_policy = cmd->filter_policy;
+
+    /* Check if no reserved bits in PHYS are set and that at least one valid PHY
+     * is set.
+     */
+    if (!(cmd->phys & SCAN_VALID_PHY_MASK) ||
+        (cmd->phys & ~SCAN_VALID_PHY_MASK)) {
+         return BLE_ERR_INV_HCI_CMD_PARMS;
+     }
+
+    if (cmd->phys & BLE_HCI_LE_PHY_1M_PREF_MASK) {
+        if (len < sizeof(*params)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        interval = le16toh(params->itvl);
+        window = le16toh(params->window);
+
+        rc = ble_ll_check_scan_params(params->type, interval, window);
+        if (rc) {
+            return rc;
+        }
+
+        uncoded->scan_type = params->type;
+        uncoded->timing.interval = ble_ll_scan_time_hci_to_ticks(interval);
+        uncoded->timing.window = ble_ll_scan_time_hci_to_ticks(window);
+
+        /* That means user wants to use this PHY for scanning */
+        uncoded->configured = 1;
+        params++;
+        len -= sizeof(*params);
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    if (cmd->phys & BLE_HCI_LE_PHY_CODED_PREF_MASK) {
+        if (len < sizeof(*params)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        interval = le16toh(params->itvl);
+        window = le16toh(params->window);
+
+        rc = ble_ll_check_scan_params(params->type, interval, window);
+        if (rc) {
+            return rc;
+        }
+
+        coded->scan_type = params->type;
+        coded->timing.interval = ble_ll_scan_time_hci_to_ticks(interval);
+        coded->timing.window = ble_ll_scan_time_hci_to_ticks(window);
+
+        /* That means user wants to use this PHY for scanning */
+        coded->configured = 1;
+    }
+#endif
+
+    /* if any of PHYs is configured for continuous scan we alter interval to
+     * fit other PHY
+     */
+    if (coded->configured && uncoded->configured) {
+        if (coded->timing.interval == coded->timing.window) {
+            coded->timing.interval += uncoded->timing.window;
+        }
+
+        if (uncoded->timing.interval == uncoded->timing.window) {
+            uncoded->timing.window += coded->timing.window;
+        }
+    }
+
+    memcpy(g_ble_ll_scan_params, new_params, sizeof(new_params));
+
+    return 0;
+}
+
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+static void
+ble_ll_scan_duration_period_timers_restart(struct ble_ll_scan_sm *scansm)
+{
+    uint32_t now;
+
+    now = os_cputime_get32();
+
+    os_cputime_timer_stop(&scansm->duration_timer);
+    os_cputime_timer_stop(&scansm->period_timer);
+
+    if (scansm->duration_ticks) {
+        os_cputime_timer_start(&scansm->duration_timer,
+                                                now + scansm->duration_ticks);
+
+        if (scansm->period_ticks) {
+            os_cputime_timer_start(&scansm->period_timer,
+                                                    now + scansm->period_ticks);
+        }
+    }
+}
+
+static void
+ble_ll_scan_duration_timer_cb(void *arg)
+{
+    struct ble_ll_scan_sm *scansm;
+
+    scansm = (struct ble_ll_scan_sm *)arg;
+
+    ble_ll_scan_sm_stop(2);
+
+    /* if period is set both timers get started from period cb */
+    if (!scansm->period_ticks) {
+        ble_ll_hci_ev_send_scan_timeout();
+    }
+}
+
+static void
+ble_ll_scan_period_timer_cb(void *arg)
+{
+    struct ble_ll_scan_sm *scansm = arg;
+
+    ble_ll_scan_sm_start(scansm);
+
+    /* always start timer regardless of ble_ll_scan_sm_start result
+     * if it failed will restart in next period
+     */
+    ble_ll_scan_duration_period_timers_restart(scansm);
+}
+#endif
+
+/**
+ * ble ll scan set enable
+ *
+ * HCI scan set enable command processing function
+ *
+ *  Context: Link Layer task (HCI Command parser).
+ *
+ * @return int BLE error code.
+ */
+static int
+ble_ll_scan_set_enable(uint8_t enable, uint8_t filter_dups, uint16_t period,
+                       uint16_t dur, bool ext)
+{
+    int rc;
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp;
+    struct ble_ll_scan_params *scanp_phy;
+    int i;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    uint32_t period_ticks = 0;
+    uint32_t dur_ticks = 0;
+#endif
+
+    /* Check for valid parameters */
+    if ((filter_dups > 1) || (enable > 1)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    scansm = &g_ble_ll_scan_sm;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* we can do that here since value will never change until reset */
+    scansm->ext_scanning = ext;
+
+    if (ext) {
+        /* Period parameter is ignored when the Duration parameter is zero */
+        if (!dur) {
+            period = 0;
+        }
+
+        /* period is in 1.28 sec units
+         * TODO support full range, would require os_cputime milliseconds API
+         */
+        if (period > 3355) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+        period_ticks = os_cputime_usecs_to_ticks(period * 1280000);
+
+        /* duration is in 10ms units */
+        dur_ticks = os_cputime_usecs_to_ticks(dur * 10000);
+
+        if (dur_ticks && period_ticks && (dur_ticks >= period_ticks)) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+#endif
+
+    /* disable*/
+    if (!enable) {
+        if (scansm->scan_enabled) {
+            ble_ll_scan_sm_stop(1);
+        }
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        os_cputime_timer_stop(&scansm->duration_timer);
+        os_cputime_timer_stop(&scansm->period_timer);
+#endif
+
+        return BLE_ERR_SUCCESS;
+    }
+
+    /* if already enable we just need to update parameters */
+    if (scansm->scan_enabled) {
+        /* Controller does not allow initiating and scanning.*/
+        for (i = 0; i < BLE_LL_SCAN_PHY_NUMBER; i++) {
+            scanp_phy = &scansm->scanp_phys[i];
+            if (scanp_phy->configured &&
+                                scanp_phy->scan_type == BLE_SCAN_TYPE_INITIATE) {
+                return BLE_ERR_CMD_DISALLOWED;
+            }
+        }
+
+#if MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS)
+        /* update filter policy */
+        scansm->scan_filt_dups = filter_dups;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        /* restart timers according to new settings */
+        scansm->duration_ticks = dur_ticks;
+        scansm->period_ticks = period_ticks;
+        ble_ll_scan_duration_period_timers_restart(scansm);
+#endif
+
+        return BLE_ERR_SUCCESS;
+    }
+
+    /* we can store those upfront regardless of start scan result since scan is
+     * disabled now
+     */
+
+#if MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS)
+    scansm->scan_filt_dups = filter_dups;
+#endif
+    scansm->scanp = NULL;
+    scansm->scanp_next = NULL;
+
+    for (i = 0; i < BLE_LL_SCAN_PHY_NUMBER; i++) {
+        scanp_phy = &scansm->scanp_phys[i];
+        scanp = &g_ble_ll_scan_params[i];
+
+        if (!scanp->configured) {
+            continue;
+        }
+
+        scanp_phy->configured = scanp->configured;
+        scanp_phy->scan_type = scanp->scan_type;
+        scanp_phy->timing = scanp->timing;
+        scanp_phy->scan_filt_policy = scanp->scan_filt_policy;
+        scanp_phy->own_addr_type = scanp->own_addr_type;
+
+        if (!scansm->scanp) {
+            scansm->scanp = scanp_phy;
+            /* Take own_addr_type from the first configured PHY.
+             * Note: All configured PHYs shall have the same own_addr_type
+             */
+            scansm->own_addr_type = scanp_phy->own_addr_type;
+        } else {
+            scansm->scanp_next = scanp_phy;
+        }
+    }
+
+    /* spec is not really clear if we should use defaults in this case
+     * or just disallow starting scan without explicit configuration
+     * For now be nice to host and just use values based on LE Set Scan
+     * Parameters defaults.
+     */
+    if (!scansm->scanp) {
+        scansm->scanp = &scansm->scanp_phys[PHY_UNCODED];
+        scansm->own_addr_type = BLE_ADDR_PUBLIC;
+
+        scanp_phy = scansm->scanp;
+        scanp_phy->configured = 1;
+        scanp_phy->scan_type = BLE_SCAN_TYPE_PASSIVE;
+        scanp_phy->timing.interval =
+                        ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_ITVL_DEF);
+        scanp_phy->timing.window =
+                        ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_WINDOW_DEF);
+        scanp_phy->scan_filt_policy = BLE_HCI_SCAN_FILT_NO_WL;
+        scanp_phy->own_addr_type = BLE_ADDR_PUBLIC;
+    }
+
+    rc = ble_ll_scan_sm_start(scansm);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    if (rc == BLE_ERR_SUCCESS) {
+        scansm->duration_ticks = dur_ticks;
+        scansm->period_ticks = period_ticks;
+        ble_ll_scan_duration_period_timers_restart(scansm);
+    }
+#endif
+
+    return rc;
+}
+
+int ble_ll_hci_scan_set_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_scan_enable_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_scan_set_enable(cmd->enable, cmd->filter_duplicates, 0, 0,
+                                  false);
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+int ble_ll_hci_ext_scan_set_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_ext_scan_enable_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_scan_set_enable(cmd->enable, cmd->filter_dup,
+                                  le16toh(cmd->period), le16toh(cmd->duration),
+                                  true);
+}
+#endif
+
+/**
+ * Checks if controller can change the whitelist. If scanning is enabled and
+ * using the whitelist the controller is not allowed to change the whitelist.
+ *
+ * @return int 0: not allowed to change whitelist; 1: change allowed.
+ */
+int
+ble_ll_scan_can_chg_whitelist(void)
+{
+    int rc;
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp;
+
+    scansm = &g_ble_ll_scan_sm;
+    scanp = scansm->scanp;
+    if (scansm->scan_enabled && (scanp->scan_filt_policy & 1)) {
+        rc = 0;
+    } else {
+        rc = 1;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_scan_initiator_start(struct hci_create_conn *hcc,
+                            struct ble_ll_scan_sm **sm)
+{
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp;
+    int rc;
+
+    scansm = &g_ble_ll_scan_sm;
+    scansm->own_addr_type = hcc->own_addr_type;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    scansm->ext_scanning = 0;
+#endif
+    scansm->scanp = &scansm->scanp_phys[PHY_UNCODED];
+    scansm->scanp_next = NULL;
+
+    scanp = scansm->scanp;
+    scanp->scan_filt_policy = hcc->filter_policy;
+    scanp->timing.interval = ble_ll_scan_time_hci_to_ticks(hcc->scan_itvl);
+    scanp->timing.window = ble_ll_scan_time_hci_to_ticks(hcc->scan_window);
+    scanp->scan_type = BLE_SCAN_TYPE_INITIATE;
+
+    rc = ble_ll_scan_sm_start(scansm);
+    if (sm == NULL) {
+        return rc;
+    }
+
+    if (rc == BLE_ERR_SUCCESS) {
+        *sm = scansm;
+    } else {
+        *sm = NULL;
+    }
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+int
+ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc,
+                                struct ble_ll_scan_sm **sm)
+{
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp_uncoded;
+    struct ble_ll_scan_params *scanp_coded;
+    struct hci_ext_conn_params *params;
+    int rc;
+
+    scansm = &g_ble_ll_scan_sm;
+    scansm->own_addr_type = hcc->own_addr_type;
+    scansm->scanp = NULL;
+    scansm->scanp_next = NULL;
+    scansm->ext_scanning = 1;
+
+    if (hcc->init_phy_mask & BLE_PHY_MASK_1M) {
+        params = &hcc->params[0];
+        scanp_uncoded = &scansm->scanp_phys[PHY_UNCODED];
+
+        scanp_uncoded->timing.interval = ble_ll_scan_time_hci_to_ticks(params->scan_itvl);
+        scanp_uncoded->timing.window = ble_ll_scan_time_hci_to_ticks(params->scan_window);
+        scanp_uncoded->scan_type = BLE_SCAN_TYPE_INITIATE;
+        scanp_uncoded->scan_filt_policy = hcc->filter_policy;
+        scansm->scanp = scanp_uncoded;
+    }
+
+    if (hcc->init_phy_mask & BLE_PHY_MASK_CODED) {
+        params = &hcc->params[2];
+        scanp_coded = &scansm->scanp_phys[PHY_CODED];
+
+        scanp_coded->timing.interval = ble_ll_scan_time_hci_to_ticks(params->scan_itvl);
+        scanp_coded->timing.window = ble_ll_scan_time_hci_to_ticks(params->scan_window);
+        scanp_coded->scan_type = BLE_SCAN_TYPE_INITIATE;
+        scanp_coded->scan_filt_policy = hcc->filter_policy;
+        if (scansm->scanp) {
+            scansm->scanp_next = scanp_coded;
+        } else {
+            scansm->scanp = scanp_coded;
+        }
+    }
+
+    /* if any of PHYs is configured for continuous scan we alter interval to
+     * fit other PHY
+     */
+    if (scansm->scanp && scansm->scanp_next && scanp_coded->configured &&
+        scanp_uncoded->configured) {
+        if (scanp_coded->timing.interval == scanp_coded->timing.window) {
+            scanp_coded->timing.interval += scanp_uncoded->timing.window;
+        }
+
+        if (scanp_uncoded->timing.interval == scanp_uncoded->timing.window) {
+            scanp_uncoded->timing.interval += scanp_coded->timing.window;
+        }
+    }
+
+    rc = ble_ll_scan_sm_start(scansm);
+    if (sm == NULL) {
+        return rc;
+    }
+
+    if (rc == BLE_ERR_SUCCESS) {
+        *sm = scansm;
+    } else {
+        *sm = NULL;
+    }
+
+    return rc;
+}
+#endif
+
+/**
+ * Checks to see if the scanner is enabled.
+ *
+ * @return int 0: not enabled; enabled otherwise
+ */
+int
+ble_ll_scan_enabled(void)
+{
+    return (int)g_ble_ll_scan_sm.scan_enabled;
+}
+
+/**
+ * Returns the peer resolvable private address of last device connecting to us
+ *
+ * @return uint8_t*
+ */
+uint8_t *
+ble_ll_scan_get_peer_rpa(void)
+{
+    struct ble_ll_scan_sm *scansm;
+
+    /* XXX: should this go into IRK list or connection? */
+    scansm = &g_ble_ll_scan_sm;
+    return scansm->scan_peer_rpa;
+}
+
+/**
+ * Returns the local resolvable private address currently being using by
+ * the scanner/initiator
+ *
+ * @return uint8_t*
+ */
+uint8_t *
+ble_ll_scan_get_local_rpa(void)
+{
+    return g_ble_ll_scan_sm.pdu_data.scana;
+}
+
+/**
+ * Set the Resolvable Private Address in the scanning (or initiating) state
+ * machine.
+ *
+ * XXX: should this go into IRK list or connection?
+ *
+ * @param rpa
+ */
+void
+ble_ll_scan_set_peer_rpa(uint8_t *rpa)
+{
+    struct ble_ll_scan_sm *scansm;
+
+    scansm = &g_ble_ll_scan_sm;
+    memcpy(scansm->scan_peer_rpa, rpa, BLE_DEV_ADDR_LEN);
+}
+
+struct ble_ll_scan_pdu_data *
+ble_ll_scan_get_pdu_data(void)
+{
+    return &g_ble_ll_scan_sm.pdu_data;
+}
+
+/* Returns true if whitelist is enabled for scanning */
+int
+ble_ll_scan_whitelist_enabled(void)
+{
+    return g_ble_ll_scan_sm.scanp->scan_filt_policy & 1;
+}
+
+static void
+ble_ll_scan_common_init(void)
+{
+    struct ble_ll_scan_sm *scansm;
+    struct ble_ll_scan_params *scanp;
+    int i;
+
+    /* Clear state machine in case re-initialized */
+    scansm = &g_ble_ll_scan_sm;
+    memset(scansm, 0, sizeof(struct ble_ll_scan_sm));
+
+    /* Clear scan parameters in case re-initialized */
+    memset(g_ble_ll_scan_params, 0, sizeof(g_ble_ll_scan_params));
+
+    /* Initialize scanning window end event */
+    ble_npl_event_init(&scansm->scan_sched_ev, ble_ll_scan_event_proc, scansm);
+
+    for (i = 0; i < BLE_LL_SCAN_PHY_NUMBER; i++) {
+        /* Set all non-zero default parameters */
+        scanp = &g_ble_ll_scan_params[i];
+        scanp->timing.interval =
+                        ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_ITVL_DEF);
+        scanp->timing.window =
+                        ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_WINDOW_DEF);
+    }
+
+    scansm->scanp_phys[PHY_UNCODED].phy = BLE_PHY_1M;
+#if (BLE_LL_SCAN_PHY_NUMBER == 2)
+    scansm->scanp_phys[PHY_CODED].phy = BLE_PHY_CODED;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* Make sure we'll generate new NRPA if necessary */
+    scansm->scan_nrpa_timer = ble_npl_time_get();
+#endif
+
+    /* Initialize scanning timer */
+    os_cputime_timer_init(&scansm->scan_timer, ble_ll_scan_timer_cb, scansm);
+
+    /* Initialize extended scan timers */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    os_cputime_timer_init(&scansm->duration_timer,
+                                        ble_ll_scan_duration_timer_cb, scansm);
+    os_cputime_timer_init(&scansm->period_timer, ble_ll_scan_period_timer_cb,
+                                                                        scansm);
+#endif
+
+    ble_npl_event_init(&scansm->scan_interrupted_ev, ble_ll_scan_interrupted_event_cb, NULL);
+}
+
+/**
+ * Called when the controller receives the reset command. Resets the
+ * scanning state machine to its initial state.
+ *
+ * @return int
+ */
+void
+ble_ll_scan_reset(void)
+{
+    struct ble_ll_scan_sm *scansm;
+
+    scansm = &g_ble_ll_scan_sm;
+
+    /* If enabled, stop it. */
+    if (scansm->scan_enabled) {
+        ble_ll_scan_sm_stop(0);
+    }
+
+    /* stop extended scan timers */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    os_cputime_timer_stop(&scansm->duration_timer);
+    os_cputime_timer_stop(&scansm->period_timer);
+#endif
+
+    /* Reset duplicate advertisers and those from which we rxd a response */
+    g_ble_ll_scan_num_rsp_advs = 0;
+    memset(&g_ble_ll_scan_rsp_advs[0], 0, sizeof(g_ble_ll_scan_rsp_advs));
+
+    os_mempool_clear(&g_scan_dup_pool);
+    TAILQ_INIT(&g_scan_dup_list);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* clear memory pool for AUX scan results */
+    os_mempool_clear(&ext_scan_aux_pool);
+#endif
+
+    /* Call the common init function again */
+    ble_ll_scan_common_init();
+}
+
+/**
+ * ble ll scan init
+ *
+ * Initialize a scanner. Must be called before scanning can be started.
+ * Expected to be called with a un-initialized scanning state machine.
+ */
+void
+ble_ll_scan_init(void)
+{
+    os_error_t err;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    err = os_mempool_init(&ext_scan_aux_pool,
+                          MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT),
+                          sizeof (struct ble_ll_aux_data),
+                          ext_scan_aux_mem,
+                          "ble_ll_aux_scan_pool");
+    BLE_LL_ASSERT(err == 0);
+#endif
+
+    err = os_mempool_init(&g_scan_dup_pool,
+                          MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS),
+                          sizeof(struct ble_ll_scan_dup_entry),
+                          g_scan_dup_mem,
+                          "ble_ll_scan_dup_pool");
+    BLE_LL_ASSERT(err == 0);
+
+    TAILQ_INIT(&g_scan_dup_list);
+
+    ble_ll_scan_common_init();
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sched.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sched.c
new file mode 100644
index 00000000..370faddf
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sched.c
@@ -0,0 +1,1828 @@
+/*
+ * 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 <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include "os/os.h"
+#include "os/os_cputime.h"
+#include "ble/xcvr.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "controller/ble_ll_trace.h"
+#include "controller/ble_ll_sync.h"
+#include "ble_ll_priv.h"
+#include "ble_ll_conn_priv.h"
+
+/* XXX: this is temporary. Not sure what I want to do here */
+struct hal_timer g_ble_ll_sched_timer;
+
+uint8_t g_ble_ll_sched_offset_ticks;
+
+#define BLE_LL_SCHED_ADV_WORST_CASE_USECS       \
+    (BLE_LL_SCHED_MAX_ADV_PDU_USECS + BLE_LL_IFS + BLE_LL_SCHED_ADV_MAX_USECS \
+     + XCVR_TX_SCHED_DELAY_USECS)
+
+#if (BLE_LL_SCHED_DEBUG == 1)
+int32_t g_ble_ll_sched_max_late;
+int32_t g_ble_ll_sched_max_early;
+#endif
+
+/* XXX: TODO:
+ *  1) Add some accounting to the schedule code to see how late we are
+ *  (min/max?)
+ *
+ *  2) Need to determine how we really want to handle the case when we execute
+ *  a schedule item but there is a current event. We could:
+ *      -> Reschedule the schedule item and let current event finish
+ *      -> Kill the current event and run the scheduled item.
+ *      -> Disable schedule timer while in an event; could cause us to be late.
+ *      -> Wait for current event to finish hoping it does before schedule item.
+ */
+
+/* Queue for timers */
+TAILQ_HEAD(ll_sched_qhead, ble_ll_sched_item) g_ble_ll_sched_q;
+
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+struct ble_ll_sched_obj g_ble_ll_sched_data;
+#endif
+
+/**
+ * Checks if two events in the schedule will overlap in time. NOTE: consecutive
+ * schedule items can end and start at the same time.
+ *
+ * @param s1
+ * @param s2
+ *
+ * @return int 0: dont overlap 1:overlap
+ */
+static int
+ble_ll_sched_is_overlap(struct ble_ll_sched_item *s1,
+                        struct ble_ll_sched_item *s2)
+{
+    int rc;
+
+    rc = 1;
+    if ((int32_t)(s1->start_time - s2->start_time) < 0) {
+        /* Make sure this event does not overlap current event */
+        if ((int32_t)(s1->end_time - s2->start_time) <= 0) {
+            rc = 0;
+        }
+    } else {
+        /* Check for overlap */
+        if ((int32_t)(s1->start_time - s2->end_time) >= 0) {
+            rc = 0;
+        }
+    }
+
+    return rc;
+}
+
+/*
+ * Determines if the schedule item overlaps the currently running schedule
+ * item. We only care about connection schedule items
+ */
+static int
+ble_ll_sched_overlaps_current(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    uint32_t ce_end_time;
+
+    rc = 0;
+    if (ble_ll_state_get() == BLE_LL_STATE_CONNECTION) {
+        ce_end_time = ble_ll_conn_get_ce_end_time();
+        if ((int32_t)(ce_end_time - sch->start_time) > 0) {
+            rc = 1;
+        }
+    }
+    return rc;
+}
+
+static int
+ble_ll_sched_conn_overlap(struct ble_ll_sched_item *entry)
+{
+    int rc;
+    struct ble_ll_conn_sm *connsm;
+
+    /* Should only be advertising or a connection here */
+    if (entry->sched_type == BLE_LL_SCHED_TYPE_CONN) {
+        connsm = (struct ble_ll_conn_sm *)entry->cb_arg;
+        entry->enqueued = 0;
+        TAILQ_REMOVE(&g_ble_ll_sched_q, entry, link);
+        ble_ll_event_send(&connsm->conn_ev_end);
+        rc = 0;
+    } else {
+        rc = -1;
+    }
+
+    return rc;
+}
+
+static struct ble_ll_sched_item *
+ble_ll_sched_insert_if_empty(struct ble_ll_sched_item *sch)
+{
+    struct ble_ll_sched_item *entry;
+
+    entry = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (!entry) {
+        TAILQ_INSERT_HEAD(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 1;
+    }
+    return entry;
+}
+
+int
+ble_ll_sched_conn_reschedule(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+    os_sr_t sr;
+    uint32_t usecs;
+    struct ble_ll_sched_item *sch;
+    struct ble_ll_sched_item *start_overlap;
+    struct ble_ll_sched_item *end_overlap;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_conn_sm *tmp;
+
+    /* Get schedule element from connection */
+    sch = &connsm->conn_sch;
+
+    /* Set schedule start and end times */
+    sch->start_time = connsm->anchor_point - g_ble_ll_sched_offset_ticks;
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+        usecs = connsm->slave_cur_window_widening;
+        sch->start_time -= (os_cputime_usecs_to_ticks(usecs) + 1);
+        sch->remainder = 0;
+    } else {
+        sch->remainder = connsm->anchor_point_usecs;
+    }
+    sch->end_time = connsm->ce_end_time;
+
+    /* Better be past current time or we just leave */
+    if ((int32_t)(sch->start_time - os_cputime_get32()) < 0) {
+        return -1;
+    }
+
+    /* We have to find a place for this schedule */
+    OS_ENTER_CRITICAL(sr);
+
+    if (ble_ll_sched_overlaps_current(sch)) {
+        OS_EXIT_CRITICAL(sr);
+        return -1;
+    }
+
+    /* Stop timer since we will add an element */
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+
+    start_overlap = NULL;
+    end_overlap = NULL;
+    rc = 0;
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+           if (entry->sched_type == BLE_LL_SCHED_TYPE_CONN &&
+                            !ble_ll_conn_is_lru((struct ble_ll_conn_sm *)sch->cb_arg,
+                                    (struct ble_ll_conn_sm *)entry->cb_arg)) {
+                /* Only insert if this element is older than all that we
+                 * overlap
+                 */
+                start_overlap = NULL;
+                rc = -1;
+                break;
+            }
+
+            if (start_overlap == NULL) {
+                start_overlap = entry;
+                end_overlap = entry;
+            } else {
+                end_overlap = entry;
+            }
+        } else {
+            if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                rc = 0;
+                TAILQ_INSERT_BEFORE(entry, sch, link);
+                break;
+            }
+        }
+    }
+
+    if (!rc) {
+        if (!entry) {
+            TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        }
+        sch->enqueued = 1;
+    }
+
+    /* Remove first to last scheduled elements */
+    entry = start_overlap;
+    while (entry) {
+        start_overlap = TAILQ_NEXT(entry,link);
+        switch (entry->sched_type) {
+        case BLE_LL_SCHED_TYPE_CONN:
+            tmp = (struct ble_ll_conn_sm *)entry->cb_arg;
+            ble_ll_event_send(&tmp->conn_ev_end);
+            break;
+        case BLE_LL_SCHED_TYPE_ADV:
+            ble_ll_adv_event_rmvd_from_sched((struct ble_ll_adv_sm *)entry->cb_arg);
+            break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        case BLE_LL_SCHED_TYPE_AUX_SCAN:
+            ble_ll_scan_end_adv_evt((struct ble_ll_aux_data *)entry->cb_arg);
+            break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        case BLE_LL_SCHED_TYPE_PERIODIC:
+            ble_ll_adv_periodic_rmvd_from_sched((struct ble_ll_adv_sm *)entry->cb_arg);
+            break;
+        case BLE_LL_SCHED_TYPE_SYNC:
+            ble_ll_sync_rmvd_from_sched((struct ble_ll_sync_sm *)entry->cb_arg);
+            break;
+#endif
+#endif
+        default:
+            BLE_LL_ASSERT(0);
+            break;
+        }
+
+        TAILQ_REMOVE(&g_ble_ll_sched_q, entry, link);
+        entry->enqueued = 0;
+
+        if (entry == end_overlap) {
+            break;
+        }
+        entry = start_overlap;
+    }
+
+    entry = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (entry == sch) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    } else {
+        sch = entry;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+
+/**
+ * Called to schedule a connection when the current role is master.
+ *
+ * Context: Interrupt
+ *
+ * @param connsm
+ * @param ble_hdr
+ * @param pyld_len
+ *
+ * @return int
+ */
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+int
+ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm,
+                        struct ble_mbuf_hdr *ble_hdr, uint8_t pyld_len)
+{
+    int rc;
+    os_sr_t sr;
+    uint32_t initial_start;
+    uint32_t earliest_start;
+    uint32_t earliest_end;
+    uint32_t dur;
+    uint32_t itvl_t;
+    uint32_t adv_rxend;
+    int i;
+    uint32_t tpp;
+    uint32_t tse;
+    uint32_t np;
+    uint32_t cp;
+    uint32_t tick_in_period;
+
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *sch;
+
+    /* Better have a connsm */
+    BLE_LL_ASSERT(connsm != NULL);
+
+    /* Get schedule element from connection */
+    rc = -1;
+    sch = &connsm->conn_sch;
+
+    /* XXX:
+     * The calculations for the 32kHz crystal bear alot of explanation. The
+     * earliest possible time that the master can start the connection with a
+     * slave is 1.25 msecs from the end of the connection request. The
+     * connection request is sent an IFS time from the end of the advertising
+     * packet that was received plus the time it takes to send the connection
+     * request. At 1 Mbps, this is 1752 usecs, or 57.41 ticks. Using 57 ticks
+     * makes us off ~13 usecs. Since we dont want to actually calculate the
+     * receive end time tick (this would take too long), we assume the end of
+     * the advertising PDU is 'now' (we call os_cputime_get32). We dont know
+     * how much time it will take to service the ISR but if we are more than the
+     * rx to tx time of the chip we will not be successful transmitting the
+     * connect request. All this means is that we presume that the slave will
+     * receive the connect request later than we expect but no earlier than
+     * 13 usecs before (this is important).
+     *
+     * The code then attempts to schedule the connection at the
+     * earliest time although this may not be possible. When the actual
+     * schedule start time is determined, the master has to determine if this
+     * time is more than a transmit window offset interval (1.25 msecs). The
+     * master has to tell the slave how many transmit window offsets there are
+     * from the earliest possible time to when the actual transmit start will
+     * occur. Later in this function you will see the calculation. The actual
+     * transmission start has to occur within the transmit window. The transmit
+     * window interval is in units of 1.25 msecs and has to be at least 1. To
+     * make things a bit easier (but less power efficient for the slave), we
+     * use a transmit window of 2. We do this because we dont quite know the
+     * exact start of the transmission and if we are too early or too late we
+     * could miss the transmit window. A final note: the actual transmission
+     * start (the anchor point) is sched offset ticks from the schedule start
+     * time. We dont add this to the calculation when calculating the window
+     * offset. The reason we dont do this is we want to insure we transmit
+     * after the window offset we tell the slave. For example, say we think
+     * we are transmitting 1253 usecs from the earliest start. This would cause
+     * us to send a transmit window offset of 1. Since we are actually
+     * transmitting earlier than the slave thinks we could end up transmitting
+     * before the window offset. Transmitting later is fine since we have the
+     * transmit window to do so. Transmitting before is bad, since the slave
+     * wont be listening. We could do better calculation if we wanted to use
+     * a transmit window of 1 as opposed to 2, but for now we dont care.
+     */
+    dur = os_cputime_usecs_to_ticks(g_ble_ll_sched_data.sch_ticks_per_period);
+    adv_rxend = os_cputime_get32();
+    if (ble_hdr->rxinfo.channel >= BLE_PHY_NUM_DATA_CHANS) {
+        /*
+         * We received packet on advertising channel which means this is a legacy
+         * PDU on 1 Mbps - we do as described above.
+         */
+        earliest_start = adv_rxend + 57;
+    } else {
+        /*
+         * The calculations are similar as above.
+         *
+         * We received packet on data channel which means this is AUX_ADV_IND
+         * received on secondary adv channel. We can schedule first packet at
+         * the earliest after "T_IFS + AUX_CONNECT_REQ + transmitWindowDelay".
+         * AUX_CONNECT_REQ and transmitWindowDelay times vary depending on which
+         * PHY we received on.
+         *
+         */
+        if (ble_hdr->rxinfo.phy == BLE_PHY_1M) {
+            // 150 + 352 + 2500 = 3002us = 98.37 ticks
+            earliest_start = adv_rxend + 98;
+        } else if (ble_hdr->rxinfo.phy == BLE_PHY_2M) {
+            // 150 + 180 + 2500 = 2830us = 92.73 ticks
+            earliest_start = adv_rxend + 93;
+        } else if (ble_hdr->rxinfo.phy == BLE_PHY_CODED) {
+            // 150 + 2896 + 3750 = 6796us = 222.69 ticks
+            earliest_start = adv_rxend + 223;
+        } else {
+            BLE_LL_ASSERT(0);
+        }
+    }
+    earliest_start += MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET) *
+                      BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT;
+    itvl_t = connsm->conn_itvl_ticks;
+
+    /* We have to find a place for this schedule */
+    OS_ENTER_CRITICAL(sr);
+
+    /*
+     * Are there any allocated periods? If not, set epoch start to earliest
+     * time
+     */
+    if (g_ble_ll_sched_data.sch_num_occ_periods == 0) {
+        g_ble_ll_sched_data.sch_epoch_start = earliest_start;
+        cp = 0;
+    } else {
+        /*
+         * Earliest start must occur on period boundary.
+         * (tse = ticks since epoch)
+         */
+        tpp = g_ble_ll_sched_data.sch_ticks_per_period;
+        tse = earliest_start - g_ble_ll_sched_data.sch_epoch_start;
+        np = tse / tpp;
+        cp = np % BLE_LL_SCHED_PERIODS;
+        tick_in_period = tse - (np * tpp);
+        if (tick_in_period != 0) {
+            ++cp;
+            if (cp == BLE_LL_SCHED_PERIODS) {
+                cp = 0;
+            }
+            earliest_start += (tpp - tick_in_period);
+        }
+
+        /* Now find first un-occupied period starting from cp */
+        for (i = 0; i < BLE_LL_SCHED_PERIODS; ++i) {
+            if (g_ble_ll_sched_data.sch_occ_period_mask & (1 << cp)) {
+                ++cp;
+                if (cp == BLE_LL_SCHED_PERIODS) {
+                    cp = 0;
+                }
+                earliest_start += tpp;
+            } else {
+                /* not occupied */
+                break;
+            }
+        }
+        /* Should never happen but if it does... */
+        if (i == BLE_LL_SCHED_PERIODS) {
+            OS_EXIT_CRITICAL(sr);
+            return rc;
+        }
+    }
+
+    sch->start_time = earliest_start;
+    initial_start = earliest_start;
+    earliest_end = earliest_start + dur;
+
+    if (!ble_ll_sched_insert_if_empty(sch)) {
+        /* Nothing in schedule. Schedule as soon as possible */
+        rc = 0;
+        connsm->tx_win_off = MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET);
+    } else {
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+            /* Set these because overlap function needs them to be set */
+            sch->start_time = earliest_start;
+            sch->end_time = earliest_end;
+
+            /* We can insert if before entry in list */
+            if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                if ((earliest_start - initial_start) <= itvl_t) {
+                    rc = 0;
+                    TAILQ_INSERT_BEFORE(entry, sch, link);
+                }
+                break;
+            }
+
+            /* Check for overlapping events */
+            if (ble_ll_sched_is_overlap(sch, entry)) {
+                /* Earliest start is end of this event since we overlap */
+                earliest_start = entry->end_time;
+                earliest_end = earliest_start + dur;
+            }
+        }
+
+        /* Must be able to schedule within one connection interval */
+        if (!entry) {
+            if ((earliest_start - initial_start) <= itvl_t) {
+                rc = 0;
+                TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+            }
+        }
+
+        if (!rc) {
+            /* calculate number of window offsets. Each offset is 1.25 ms */
+            sch->enqueued = 1;
+            /*
+             * NOTE: we dont add sched offset ticks as we want to under-estimate
+             * the transmit window slightly since the window size is currently
+             * 2 when using a 32768 crystal.
+             */
+            dur = os_cputime_ticks_to_usecs(earliest_start - initial_start);
+            connsm->tx_win_off = dur / BLE_LL_CONN_TX_OFF_USECS;
+        }
+    }
+
+    if (!rc) {
+        sch->start_time = earliest_start;
+        sch->end_time = earliest_end;
+        /*
+         * Since we have the transmit window to transmit in, we dont need
+         * to set the anchor point usecs; just transmit to the nearest tick.
+         */
+        connsm->anchor_point = earliest_start + g_ble_ll_sched_offset_ticks;
+        connsm->anchor_point_usecs = 0;
+        connsm->ce_end_time = earliest_end;
+        connsm->period_occ_mask = (1 << cp);
+        g_ble_ll_sched_data.sch_occ_period_mask |= connsm->period_occ_mask;
+        ++g_ble_ll_sched_data.sch_num_occ_periods;
+    }
+
+
+    /* Get head of list to restart timer */
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    ble_ll_rfmgmt_sched_changed(sch);
+
+    OS_EXIT_CRITICAL(sr);
+
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+#else
+int
+ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm,
+                        struct ble_mbuf_hdr *ble_hdr, uint8_t pyld_len)
+{
+    int rc;
+    os_sr_t sr;
+    uint8_t req_slots;
+    uint32_t initial_start;
+    uint32_t earliest_start;
+    uint32_t earliest_end;
+    uint32_t dur;
+    uint32_t itvl_t;
+    uint32_t adv_rxend;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *sch;
+
+    /*
+     * XXX: TODO this code assumes the advertisement and connect request were
+     * sent at 1Mbps.
+     */
+
+    /* Get schedule element from connection */
+    rc = -1;
+    sch = &connsm->conn_sch;
+    req_slots = MYNEWT_VAL(BLE_LL_CONN_INIT_SLOTS);
+
+    /* XXX:
+     * The calculations for the 32kHz crystal bear alot of explanation. The
+     * earliest possible time that the master can start the connection with a
+     * slave is 1.25 msecs from the end of the connection request. The
+     * connection request is sent an IFS time from the end of the advertising
+     * packet that was received plus the time it takes to send the connection
+     * request. At 1 Mbps, this is 1752 usecs, or 57.41 ticks. Using 57 ticks
+     * makes us off ~13 usecs. Since we dont want to actually calculate the
+     * receive end time tick (this would take too long), we assume the end of
+     * the advertising PDU is 'now' (we call os_cputime_get32). We dont know
+     * how much time it will take to service the ISR but if we are more than the
+     * rx to tx time of the chip we will not be successful transmitting the
+     * connect request. All this means is that we presume that the slave will
+     * receive the connect request later than we expect but no earlier than
+     * 13 usecs before (this is important).
+     *
+     * The code then attempts to schedule the connection at the
+     * earliest time although this may not be possible. When the actual
+     * schedule start time is determined, the master has to determine if this
+     * time is more than a transmit window offset interval (1.25 msecs). The
+     * master has to tell the slave how many transmit window offsets there are
+     * from the earliest possible time to when the actual transmit start will
+     * occur. Later in this function you will see the calculation. The actual
+     * transmission start has to occur within the transmit window. The transmit
+     * window interval is in units of 1.25 msecs and has to be at least 1. To
+     * make things a bit easier (but less power efficient for the slave), we
+     * use a transmit window of 2. We do this because we dont quite know the
+     * exact start of the transmission and if we are too early or too late we
+     * could miss the transmit window. A final note: the actual transmission
+     * start (the anchor point) is sched offset ticks from the schedule start
+     * time. We dont add this to the calculation when calculating the window
+     * offset. The reason we dont do this is we want to insure we transmit
+     * after the window offset we tell the slave. For example, say we think
+     * we are transmitting 1253 usecs from the earliest start. This would cause
+     * us to send a transmit window offset of 1. Since we are actually
+     * transmitting earlier than the slave thinks we could end up transmitting
+     * before the window offset. Transmitting later is fine since we have the
+     * transmit window to do so. Transmitting before is bad, since the slave
+     * wont be listening. We could do better calculation if we wanted to use
+     * a transmit window of 1 as opposed to 2, but for now we dont care.
+     */
+    dur = req_slots * BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT;
+    adv_rxend = os_cputime_get32();
+    if (ble_hdr->rxinfo.channel >= BLE_PHY_NUM_DATA_CHANS) {
+        /*
+         * We received packet on advertising channel which means this is a legacy
+         * PDU on 1 Mbps - we do as described above.
+         */
+        earliest_start = adv_rxend + 57;
+    } else {
+        /*
+         * The calculations are similar as above.
+         *
+         * We received packet on data channel which means this is AUX_ADV_IND
+         * received on secondary adv channel. We can schedule first packet at
+         * the earliest after "T_IFS + AUX_CONNECT_REQ + transmitWindowDelay".
+         * AUX_CONNECT_REQ and transmitWindowDelay times vary depending on which
+         * PHY we received on.
+         *
+         */
+        if (ble_hdr->rxinfo.phy == BLE_PHY_1M) {
+            // 150 + 352 + 2500 = 3002us = 98.37 ticks
+            earliest_start = adv_rxend + 98;
+        } else if (ble_hdr->rxinfo.phy == BLE_PHY_2M) {
+            // 150 + 180 + 2500 = 2830us = 92.73 ticks
+            earliest_start = adv_rxend + 93;
+        } else if (ble_hdr->rxinfo.phy == BLE_PHY_CODED) {
+            // 150 + 2896 + 3750 = 6796us = 222.69 ticks
+            earliest_start = adv_rxend + 223;
+        } else {
+            BLE_LL_ASSERT(0);
+        }
+    }
+    earliest_start += MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET) *
+                      BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT;
+    earliest_end = earliest_start + dur;
+    itvl_t = connsm->conn_itvl_ticks;
+
+    /* We have to find a place for this schedule */
+    OS_ENTER_CRITICAL(sr);
+
+    /* The schedule item must occur after current running item (if any) */
+    sch->start_time = earliest_start;
+    initial_start = earliest_start;
+
+    if (!ble_ll_sched_insert_if_empty(sch)) {
+        /* Nothing in schedule. Schedule as soon as possible */
+        rc = 0;
+        connsm->tx_win_off = MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET);
+    } else {
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+            /* Set these because overlap function needs them to be set */
+            sch->start_time = earliest_start;
+            sch->end_time = earliest_end;
+
+            /* We can insert if before entry in list */
+            if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                if ((earliest_start - initial_start) <= itvl_t) {
+                    rc = 0;
+                    TAILQ_INSERT_BEFORE(entry, sch, link);
+                }
+                break;
+            }
+
+            /* Check for overlapping events */
+            if (ble_ll_sched_is_overlap(sch, entry)) {
+                /* Earliest start is end of this event since we overlap */
+                earliest_start = entry->end_time;
+                earliest_end = earliest_start + dur;
+            }
+        }
+
+        /* Must be able to schedule within one connection interval */
+        if (!entry) {
+            if ((earliest_start - initial_start) <= itvl_t) {
+                rc = 0;
+                TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+            }
+        }
+
+        if (!rc) {
+            /* calculate number of window offsets. Each offset is 1.25 ms */
+            sch->enqueued = 1;
+            /*
+             * NOTE: we dont add sched offset ticks as we want to under-estimate
+             * the transmit window slightly since the window size is currently
+             * 2 when using a 32768 crystal.
+             */
+            dur = os_cputime_ticks_to_usecs(earliest_start - initial_start);
+            connsm->tx_win_off = dur / BLE_LL_CONN_TX_OFF_USECS;
+        }
+    }
+
+    if (!rc) {
+        sch->start_time = earliest_start;
+        sch->end_time = earliest_end;
+        /*
+         * Since we have the transmit window to transmit in, we dont need
+         * to set the anchor point usecs; just transmit to the nearest tick.
+         */
+        connsm->anchor_point = earliest_start + g_ble_ll_sched_offset_ticks;
+        connsm->anchor_point_usecs = 0;
+        connsm->ce_end_time = earliest_end;
+    }
+
+    /* Get head of list to restart timer */
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    ble_ll_rfmgmt_sched_changed(sch);
+
+    OS_EXIT_CRITICAL(sr);
+
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+#endif
+
+/**
+ * Schedules a slave connection for the first time.
+ *
+ * Context: Link Layer
+ *
+ * @param connsm
+ *
+ * @return int
+ */
+int
+ble_ll_sched_slave_new(struct ble_ll_conn_sm *connsm)
+{
+    int rc;
+    os_sr_t sr;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *next_sch;
+    struct ble_ll_sched_item *sch;
+    int first = 0;
+
+    /* Get schedule element from connection */
+    rc = -1;
+    sch = &connsm->conn_sch;
+
+    /* Set schedule start and end times */
+    /*
+     * XXX: for now, we dont care about anchor point usecs for the slave. It
+     * does not matter if we turn on the receiver up to one tick before w
+     * need to. We also subtract one extra tick since the conversion from
+     * usecs to ticks could be off by up to 1 tick.
+     */
+    sch->start_time = connsm->anchor_point - g_ble_ll_sched_offset_ticks -
+        os_cputime_usecs_to_ticks(connsm->slave_cur_window_widening) - 1;
+    sch->end_time = connsm->ce_end_time;
+    sch->remainder = 0;
+
+    /* We have to find a place for this schedule */
+    OS_ENTER_CRITICAL(sr);
+
+    /* The schedule item must occur after current running item (if any) */
+    if (ble_ll_sched_overlaps_current(sch)) {
+        OS_EXIT_CRITICAL(sr);
+        return rc;
+    }
+
+    entry = ble_ll_sched_insert_if_empty(sch);
+    if (!entry) {
+        /* Nothing in schedule. Schedule as soon as possible */
+        rc = 0;
+        first = 1;
+    } else {
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        while (1) {
+            next_sch = entry->link.tqe_next;
+            /* Insert if event ends before next starts */
+            if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                rc = 0;
+                TAILQ_INSERT_BEFORE(entry, sch, link);
+                break;
+            }
+
+            if (ble_ll_sched_is_overlap(sch, entry)) {
+                /* If we overlap with a connection, we re-schedule */
+                if (ble_ll_sched_conn_overlap(entry)) {
+                    break;
+                }
+            }
+
+            /* Move to next entry */
+            entry = next_sch;
+
+            /* Insert at tail if none left to check */
+            if (!entry) {
+                rc = 0;
+                TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+                break;
+            }
+        }
+
+        if (!rc) {
+            sch->enqueued = 1;
+        }
+
+        next_sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+        if (next_sch == sch) {
+            first = 1;
+        } else {
+            sch = next_sch;
+        }
+    }
+
+    if (first) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+/*
+ * Determines if the schedule item overlaps the currently running schedule
+ * item. This function cares about connection and sync.
+ */
+static int
+ble_ll_sched_sync_overlaps_current(struct ble_ll_sched_item *sch)
+{
+    uint32_t end_time;
+    uint8_t state;
+
+    state = ble_ll_state_get();
+    switch (state) {
+    case BLE_LL_STATE_CONNECTION:
+        end_time = ble_ll_conn_get_ce_end_time();
+        break;
+    case BLE_LL_STATE_SYNC:
+        end_time = ble_ll_sync_get_event_end_time();
+        break;
+    default:
+        return 0;
+    }
+
+    return CPUTIME_GT(end_time, sch->start_time);
+}
+
+int
+ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch,
+                             uint32_t anchor_point, uint8_t anchor_point_usecs,
+                             uint32_t window_widening,
+                             int8_t phy_mode)
+{
+    struct ble_ll_sched_item *entry;
+    uint8_t start_time_rem_usecs;
+    uint8_t window_rem_usecs;
+    uint32_t window_ticks;
+    uint32_t start_time;
+    uint32_t end_time;
+    uint32_t dur;
+    int rc = 0;
+    os_sr_t sr;
+
+    window_ticks = os_cputime_usecs_to_ticks(window_widening);
+    window_rem_usecs = window_widening - os_cputime_ticks_to_usecs(window_ticks);
+
+    /* adjust for subtraction */
+    anchor_point_usecs += 31;
+    anchor_point--;
+
+    start_time = anchor_point - window_ticks;
+    start_time_rem_usecs = anchor_point_usecs - window_rem_usecs;
+    if (start_time_rem_usecs >= 31) {
+        start_time++;
+        start_time_rem_usecs -= 31;
+    }
+
+    dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_SYNC_PDU_LEN),
+                                 phy_mode);
+    end_time = start_time + os_cputime_usecs_to_ticks(dur);
+
+    start_time -= g_ble_ll_sched_offset_ticks;
+
+    /* Set schedule start and end times */
+    sch->start_time = start_time;
+    sch->remainder = start_time_rem_usecs;
+    sch->end_time = end_time;
+
+    /* Better be past current time or we just leave */
+    if (CPUTIME_LEQ(sch->start_time, os_cputime_get32())) {
+        return -1;
+    }
+
+    /* We have to find a place for this schedule */
+    OS_ENTER_CRITICAL(sr);
+
+    if (ble_ll_sched_sync_overlaps_current(sch)) {
+        OS_EXIT_CRITICAL(sr);
+        return -1;
+    }
+
+    /* Try to find slot for sync scan. */
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        /* We can insert if before entry in list */
+        if (CPUTIME_LEQ(sch->end_time, entry->start_time)) {
+            TAILQ_INSERT_BEFORE(entry, sch, link);
+            sch->enqueued = 1;
+            break;
+        }
+
+        /* Check for overlapping events. For now drop if it overlaps with
+         * anything. We can make it smarter later on
+         */
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+            rc = -1;
+            break;
+        }
+    }
+
+    if (!entry) {
+        TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 1;
+    }
+
+    entry = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (entry == sch) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    } else {
+        sch = entry;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+
+int
+ble_ll_sched_sync(struct ble_ll_sched_item *sch,
+                  uint32_t beg_cputime, uint32_t rem_usecs,
+                  uint32_t offset, int8_t phy_mode)
+{
+    struct ble_ll_sched_item *entry;
+    uint32_t start_time_rem_usecs;
+    uint32_t off_rem_usecs;
+    uint32_t start_time;
+    uint32_t off_ticks;
+    uint32_t end_time;
+    uint32_t dur;
+    os_sr_t sr;
+    int rc = 0;
+
+    off_ticks = os_cputime_usecs_to_ticks(offset);
+    off_rem_usecs = offset - os_cputime_ticks_to_usecs(off_ticks);
+
+    start_time = beg_cputime + off_ticks;
+    start_time_rem_usecs = rem_usecs + off_rem_usecs;
+    if (start_time_rem_usecs >= 31) {
+        start_time++;
+        start_time_rem_usecs -= 31;
+    }
+
+    dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_SYNC_PDU_LEN),
+                                  phy_mode);
+    end_time = start_time + os_cputime_usecs_to_ticks(dur);
+
+    start_time -= g_ble_ll_sched_offset_ticks;
+
+    sch->start_time = start_time;
+    sch->remainder = start_time_rem_usecs;
+    sch->end_time = end_time;
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!ble_ll_sched_insert_if_empty(sch)) {
+        /* Nothing in schedule. Schedule as soon as possible
+         * If we are here it means sch has been added to the scheduler */
+        goto done;
+    }
+
+    /* Try to find slot for scan. */
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        /* We can insert if before entry in list */
+        if (CPUTIME_LEQ(sch->end_time, entry->start_time)) {
+            TAILQ_INSERT_BEFORE(entry, sch, link);
+            sch->enqueued = 1;
+            break;
+        }
+
+        /* Check for overlapping events. For now drop if it overlaps with
+         * anything. We can make it smarter later on
+         */
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+            rc = -1;
+            break;
+        }
+    }
+
+    if (!entry) {
+        TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 1;
+    }
+
+done:
+    entry = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (entry == sch) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    } else {
+        sch = entry;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    STATS_INC(ble_ll_stats, sync_scheduled);
+    return rc;
+}
+#endif
+
+int
+ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, ble_ll_sched_adv_new_cb cb,
+                     void *arg)
+{
+    os_sr_t sr;
+    uint32_t adv_start;
+    uint32_t duration;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *orig;
+
+    /* Get length of schedule item */
+    duration = sch->end_time - sch->start_time;
+    orig = sch;
+
+    OS_ENTER_CRITICAL(sr);
+    entry = ble_ll_sched_insert_if_empty(sch);
+    if (!entry) {
+        adv_start = sch->start_time;
+    } else {
+        /* XXX: no need to stop timer if not first on list. Modify code? */
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+            /* We can insert if before entry in list */
+            if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                TAILQ_INSERT_BEFORE(entry, sch, link);
+                break;
+            }
+
+            /* Check for overlapping events */
+            if (ble_ll_sched_is_overlap(sch, entry)) {
+                /* Earliest start is end of this event since we overlap */
+                sch->start_time = entry->end_time;
+                sch->end_time = sch->start_time + duration;
+            }
+        }
+
+        if (!entry) {
+            TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        }
+        adv_start = sch->start_time;
+
+        sch->enqueued = 1;
+
+        /* Restart with head of list */
+        sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    }
+
+    if (cb) {
+        cb((struct ble_ll_adv_sm *)orig->cb_arg, adv_start, arg);
+    }
+
+    if (orig == sch) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return 0;
+}
+
+int
+ble_ll_sched_periodic_adv(struct ble_ll_sched_item *sch, uint32_t *start,
+                          bool after_overlap)
+{
+    int rc = 0;
+    os_sr_t sr;
+    uint32_t adv_start;
+    uint32_t duration;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *orig = sch;
+
+    /* Get length of schedule item */
+    duration = sch->end_time - sch->start_time;
+
+    OS_ENTER_CRITICAL(sr);
+    entry = ble_ll_sched_insert_if_empty(sch);
+    if (!entry) {
+        adv_start = sch->start_time;
+    } else {
+        /* XXX: no need to stop timer if not first on list. Modify code? */
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+            /* We can insert if before entry in list */
+            if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                TAILQ_INSERT_BEFORE(entry, sch, link);
+                break;
+            }
+
+            /* Check for overlapping events */
+            if (ble_ll_sched_is_overlap(sch, entry)) {
+                if (after_overlap) {
+                    /* Earliest start is end of this event since we overlap */
+                    sch->start_time = entry->end_time;
+                    sch->end_time = sch->start_time + duration;
+                } else {
+                    rc = -1;
+                    break;
+                }
+            }
+        }
+
+        if (!entry) {
+            TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        }
+        adv_start = sch->start_time;
+
+        if (!rc) {
+            sch->enqueued = 1;
+        }
+
+        /* Restart with head of list */
+        sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    }
+
+    if (!rc) {
+        *start = adv_start;
+    }
+
+    if (orig == sch) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+
+int
+ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch, uint32_t *start,
+                            uint32_t max_delay_ticks)
+{
+    int rc;
+    os_sr_t sr;
+    uint32_t orig_start;
+    uint32_t duration;
+    uint32_t rand_ticks;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *next_sch;
+    struct ble_ll_sched_item *before;
+    struct ble_ll_sched_item *start_overlap;
+    struct ble_ll_sched_item *end_overlap;
+
+    /* Get length of schedule item */
+    duration = sch->end_time - sch->start_time;
+
+    /* Add maximum randomization delay to end */
+    rand_ticks = max_delay_ticks;
+    sch->end_time += max_delay_ticks;
+
+    start_overlap = NULL;
+    end_overlap = NULL;
+    before = NULL;
+    rc = 0;
+    OS_ENTER_CRITICAL(sr);
+
+    entry = ble_ll_sched_insert_if_empty(sch);
+    if (entry) {
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        while (1) {
+            next_sch = entry->link.tqe_next;
+            if (ble_ll_sched_is_overlap(sch, entry)) {
+                if (start_overlap == NULL) {
+                    start_overlap = entry;
+                    end_overlap = entry;
+                } else {
+                    end_overlap = entry;
+                }
+            } else {
+                if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                    before = entry;
+                    break;
+                }
+            }
+
+            entry = next_sch;
+            if (entry == NULL) {
+                break;
+            }
+        }
+
+        /*
+         * If there is no overlap, we either insert before the 'before' entry
+         * or we insert at the end if there is no before entry.
+         */
+        if (start_overlap == NULL) {
+            if (before) {
+                TAILQ_INSERT_BEFORE(before, sch, link);
+            } else {
+                TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+            }
+        } else {
+            /*
+             * This item will overlap with others. See if we can fit it in
+             * with original duration.
+             */
+            before = NULL;
+            orig_start = sch->start_time;
+            entry = start_overlap;
+            sch->end_time = sch->start_time + duration;
+            while (1) {
+                next_sch = entry->link.tqe_next;
+                if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+                    rand_ticks = entry->start_time - sch->end_time;
+                    before = entry;
+                    TAILQ_INSERT_BEFORE(before, sch, link);
+                    break;
+                } else {
+                    sch->start_time = entry->end_time;
+                    sch->end_time = sch->start_time + duration;
+                }
+
+                if (entry == end_overlap) {
+                    rand_ticks = (orig_start + max_delay_ticks) - sch->start_time;
+                    if (rand_ticks > max_delay_ticks) {
+                        /* No place for advertisement. */
+                        rc = -1;
+                    } else {
+                        if (next_sch == NULL) {
+                            TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+                        } else {
+                            TAILQ_INSERT_BEFORE(next_sch, sch, link);
+                        }
+                    }
+                    break;
+                }
+                entry = next_sch;
+                BLE_LL_ASSERT(entry != NULL);
+            }
+        }
+    }
+
+    if (!rc) {
+        sch->enqueued = 1;
+        if (rand_ticks) {
+            sch->start_time += rand() % rand_ticks;
+        }
+        sch->end_time = sch->start_time + duration;
+        *start = sch->start_time;
+
+        if (sch == TAILQ_FIRST(&g_ble_ll_sched_q)) {
+            ble_ll_rfmgmt_sched_changed(sch);
+        }
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+
+int
+ble_ll_sched_adv_resched_pdu(struct ble_ll_sched_item *sch)
+{
+    uint8_t lls;
+    os_sr_t sr;
+    struct ble_ll_sched_item *entry;
+
+    OS_ENTER_CRITICAL(sr);
+
+    lls = ble_ll_state_get();
+    if ((lls == BLE_LL_STATE_ADV) || (lls == BLE_LL_STATE_CONNECTION) ||
+            (lls == BLE_LL_STATE_SYNC)) {
+        goto adv_resched_pdu_fail;
+    }
+
+    entry = ble_ll_sched_insert_if_empty(sch);
+    if (entry) {
+        /* If we overlap with the first item, simply re-schedule */
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+            goto adv_resched_pdu_fail;
+        }
+        os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        TAILQ_INSERT_BEFORE(entry, sch, link);
+        sch->enqueued = 1;
+    }
+
+    ble_ll_rfmgmt_sched_changed(TAILQ_FIRST(&g_ble_ll_sched_q));
+
+    OS_EXIT_CRITICAL(sr);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+    return 0;
+
+adv_resched_pdu_fail:
+    OS_EXIT_CRITICAL(sr);
+    return -1;
+}
+
+/**
+ * Remove a schedule element
+ *
+ * @param sched_type
+ *
+ * @return int 0 - removed, 1 - not in the list
+ */
+int
+ble_ll_sched_rmv_elem(struct ble_ll_sched_item *sch)
+{
+    os_sr_t sr;
+    struct ble_ll_sched_item *first;
+    int rc = 1;
+
+    if (!sch) {
+        return rc;
+    }
+
+    OS_ENTER_CRITICAL(sr);
+    if (sch->enqueued) {
+        first = TAILQ_FIRST(&g_ble_ll_sched_q);
+        if (first == sch) {
+            os_cputime_timer_stop(&g_ble_ll_sched_timer);
+        }
+
+        TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 0;
+        rc = 0;
+
+        if (first == sch) {
+            first = TAILQ_FIRST(&g_ble_ll_sched_q);
+            if (first) {
+                os_cputime_timer_start(&g_ble_ll_sched_timer, first->start_time);
+            }
+            ble_ll_rfmgmt_sched_changed(first);
+        }
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    return rc;
+}
+
+void
+ble_ll_sched_rmv_elem_type(uint8_t type, sched_remove_cb_func remove_cb)
+{
+    os_sr_t sr;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *first;
+
+    OS_ENTER_CRITICAL(sr);
+    first = TAILQ_FIRST(&g_ble_ll_sched_q);
+
+    if (!first) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        if (entry->sched_type == type) {
+            if (first == entry) {
+                os_cputime_timer_stop(&g_ble_ll_sched_timer);
+                first = NULL;
+            }
+
+            TAILQ_REMOVE(&g_ble_ll_sched_q, entry, link);
+            remove_cb(entry);
+            entry->enqueued = 0;
+        }
+    }
+
+    if (!first) {
+        first = TAILQ_FIRST(&g_ble_ll_sched_q);
+        if (first) {
+            os_cputime_timer_start(&g_ble_ll_sched_timer, first->start_time);
+        }
+        ble_ll_rfmgmt_sched_changed(first);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+/**
+ * Executes a schedule item by calling the schedule callback function.
+ *
+ * Context: Interrupt
+ *
+ * @param sch Pointer to schedule item
+ *
+ * @return int 0: schedule item is not over; otherwise schedule item is done.
+ */
+static int
+ble_ll_sched_execute_item(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    uint8_t lls;
+
+    lls = ble_ll_state_get();
+
+    ble_ll_trace_u32x3(BLE_LL_TRACE_ID_SCHED, lls, os_cputime_get32(),
+                       sch->start_time);
+
+    if (lls == BLE_LL_STATE_STANDBY) {
+        goto sched;
+    }
+
+    /* If aux scan scheduled and LL is in state when scanner is running
+     * in 3 states:
+     * BLE_LL_STATE_SCANNING
+     * BLE_LL_STATE_INITIATING
+     * BLE_LL_STATE_STANDBY
+     *
+     * Let scanner to decide to disable phy or not.
+     */
+    if (sch->sched_type == BLE_LL_SCHED_TYPE_AUX_SCAN) {
+        if (lls == BLE_LL_STATE_INITIATING || lls == BLE_LL_STATE_SCANNING) {
+            goto sched;
+        }
+    }
+
+    /*
+     * This is either an advertising event or connection event start. If
+     * we are scanning or initiating just stop it.
+     */
+
+    /* We have to disable the PHY no matter what */
+    ble_phy_disable();
+
+    if (lls == BLE_LL_STATE_SCANNING) {
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        ble_ll_scan_halt();
+    } else if (lls == BLE_LL_STATE_INITIATING) {
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        ble_ll_scan_halt();
+        /* PHY is disabled - make sure we do not wait for AUX_CONNECT_RSP */
+        ble_ll_conn_reset_pending_aux_conn_rsp();
+    } else if (lls == BLE_LL_STATE_ADV) {
+        STATS_INC(ble_ll_stats, sched_state_adv_errs);
+        ble_ll_adv_halt();
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    } else if (lls == BLE_LL_STATE_SYNC) {
+        STATS_INC(ble_ll_stats, sched_state_sync_errs);
+        ble_ll_sync_halt();
+#endif
+    } else {
+        STATS_INC(ble_ll_stats, sched_state_conn_errs);
+        ble_ll_conn_event_halt();
+    }
+
+sched:
+    BLE_LL_DEBUG_GPIO(SCHED_ITEM_CB, 1);
+    BLE_LL_ASSERT(sch->sched_cb);
+    rc = sch->sched_cb(sch);
+    BLE_LL_DEBUG_GPIO(SCHED_ITEM_CB, 0);
+    return rc;
+}
+
+/**
+ * Run the BLE scheduler. Iterate through all items on the schedule queue.
+ *
+ * Context: interrupt (scheduler)
+ *
+ * @return int
+ */
+static void
+ble_ll_sched_run(void *arg)
+{
+    struct ble_ll_sched_item *sch;
+
+    BLE_LL_DEBUG_GPIO(SCHED_RUN, 1);
+
+    /* Look through schedule queue */
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (sch) {
+#if (BLE_LL_SCHED_DEBUG == 1)
+        int32_t dt;
+
+        /* Make sure we have passed the start time of the first event */
+        dt = (int32_t)(os_cputime_get32() - sch->start_time);
+        if (dt > g_ble_ll_sched_max_late) {
+            g_ble_ll_sched_max_late = dt;
+        }
+        if (dt < g_ble_ll_sched_max_early) {
+            g_ble_ll_sched_max_early = dt;
+        }
+#endif
+
+        /* Remove schedule item and execute the callback */
+        TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 0;
+        ble_ll_sched_execute_item(sch);
+
+        /* Restart if there is an item on the schedule */
+        sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+        if (sch) {
+            os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+        }
+        ble_ll_rfmgmt_sched_changed(sch);
+    }
+
+    BLE_LL_DEBUG_GPIO(SCHED_RUN, 0);
+}
+
+/**
+ * Called to determine when the next scheduled event will occur.
+ *
+ * If there are not scheduled events this function returns 0; otherwise it
+ * returns 1 and *next_event_time is set to the start time of the next event.
+ *
+ * @param next_event_time
+ *
+ * @return int 0: No events are scheduled 1: there is an upcoming event
+ */
+int
+ble_ll_sched_next_time(uint32_t *next_event_time)
+{
+    int rc;
+    os_sr_t sr;
+    struct ble_ll_sched_item *first;
+
+    rc = 0;
+    OS_ENTER_CRITICAL(sr);
+    first = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (first) {
+        *next_event_time = first->start_time;
+        rc = 1;
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/**
+ * Called to check if there is place for a planned scan req.
+ *
+ * @param chan
+ * @param phy_mode
+ *
+ * @return int 0: Clear for scan req 1: there is an upcoming event
+ */
+int
+ble_ll_sched_scan_req_over_aux_ptr(uint32_t chan, uint8_t phy_mode)
+{
+    struct ble_ll_sched_item *sch;
+    uint32_t usec_dur;
+    uint32_t now = os_cputime_get32();
+
+    /* Lets calculate roughly how much time we need for scan req and scan rsp */
+    usec_dur = ble_ll_pdu_tx_time_get(BLE_SCAN_REQ_LEN, phy_mode);
+    if (chan >=  BLE_PHY_NUM_DATA_CHANS) {
+        usec_dur += ble_ll_pdu_tx_time_get(BLE_SCAN_RSP_MAX_LEN, phy_mode);
+    } else {
+        usec_dur += ble_ll_pdu_tx_time_get(BLE_SCAN_RSP_MAX_EXT_LEN, phy_mode);
+    }
+
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    while (sch) {
+        /* Let's check if there is no scheduled item which want to start within
+         * given usecs.*/
+        if ((int32_t)(sch->start_time - now + os_cputime_usecs_to_ticks(usec_dur)) > 0) {
+            /* We are fine. Have time for scan req */
+            return 0;
+        }
+
+        /* There is something in the scheduler. If it is not aux ptr we assume
+         * it is more important that scan req
+         */
+        if (sch->sched_type != BLE_LL_SCHED_TYPE_AUX_SCAN) {
+            return 1;
+        }
+
+        ble_ll_scan_end_adv_evt((struct ble_ll_aux_data *)sch->cb_arg);
+        TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 0;
+        sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+    }
+    return 0;
+}
+
+/**
+ * Called to schedule a aux scan.
+ *
+ * Context: Interrupt
+ *
+ * @param ble_hdr
+ * @param scansm
+ * @param aux_scan
+ *
+ * @return 0 on success, 1 otherwise
+ */
+int
+ble_ll_sched_aux_scan(struct ble_mbuf_hdr *ble_hdr,
+                      struct ble_ll_scan_sm *scansm,
+                      struct ble_ll_aux_data *aux_scan)
+{
+    int rc = 1;
+    os_sr_t sr;
+    uint32_t off_ticks;
+    uint32_t off_rem_usecs;
+    uint32_t start_time;
+    uint32_t start_time_rem_usecs;
+    uint32_t end_time;
+    uint32_t dur;
+    struct ble_ll_sched_item *entry;
+    struct ble_ll_sched_item *sch;
+    int phy_mode;
+
+    sch = &aux_scan->sch;
+    BLE_LL_ASSERT(sch->cb_arg == NULL);
+
+    off_ticks = os_cputime_usecs_to_ticks(aux_scan->offset);
+    off_rem_usecs = aux_scan->offset - os_cputime_ticks_to_usecs(off_ticks);
+
+    start_time = ble_hdr->beg_cputime + off_ticks;
+    start_time_rem_usecs = ble_hdr->rem_usecs + off_rem_usecs;
+    if (start_time_rem_usecs >= 31) {
+        start_time++;
+        start_time_rem_usecs -= 31;
+    }
+    start_time -= g_ble_ll_sched_offset_ticks;
+
+    /* Let's calculate time we reserve for aux packet. For now we assume to wait
+     * for fixed number of bytes and handle possible interrupting it in
+     * ble_ll_sched_execute_item(). This is because aux packet can be up to
+     * 256bytes and we don't want to block sched that long
+     */
+    phy_mode = ble_ll_phy_to_phy_mode(aux_scan->aux_phy,
+                                      BLE_HCI_LE_PHY_CODED_ANY);
+    dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_AUX_PDU_LEN),
+                                 phy_mode);
+    end_time = start_time + os_cputime_usecs_to_ticks(dur);
+
+    sch->start_time = start_time;
+    sch->remainder = start_time_rem_usecs;
+    sch->end_time = end_time;
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!ble_ll_sched_insert_if_empty(sch)) {
+        /* Nothing in schedule. Schedule as soon as possible
+         * If we are here it means sch has been added to the scheduler */
+        rc = 0;
+        goto done;
+    }
+
+    /* Try to find slot for aux scan. */
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        /* We can insert if before entry in list */
+        if ((int32_t)(sch->end_time - entry->start_time) <= 0) {
+            rc = 0;
+            TAILQ_INSERT_BEFORE(entry, sch, link);
+            sch->enqueued = 1;
+            break;
+        }
+
+        /* Check for overlapping events. For now drop if it overlaps with
+         * anything. We can make it smarter later on
+         */
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+            break;
+        }
+    }
+
+    if (!entry) {
+        rc = 0;
+        TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 1;
+    }
+
+done:
+
+    if (rc == 0) {
+        sch->cb_arg = ble_ll_scan_aux_data_ref(aux_scan);
+        STATS_INC(ble_ll_stats, aux_scheduled);
+    }
+
+    /* Get head of list to restart timer */
+    entry = TAILQ_FIRST(&g_ble_ll_sched_q);
+    if (entry == sch) {
+        ble_ll_rfmgmt_sched_changed(sch);
+    } else {
+        sch = entry;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+int ble_ll_sched_dtm(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    os_sr_t sr;
+    struct ble_ll_sched_item *entry;
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!ble_ll_sched_insert_if_empty(sch)) {
+        /* Nothing in schedule. Schedule as soon as possible
+         * If we are here it means sch has been added to the scheduler */
+        rc = 0;
+        goto done;
+    }
+
+    /* Try to find slot for test. */
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        /* We can insert if before entry in list */
+        if (sch->end_time <= entry->start_time) {
+            rc = 0;
+            TAILQ_INSERT_BEFORE(entry, sch, link);
+            sch->enqueued = 1;
+            break;
+        }
+
+        /* Check for overlapping events. For now drop if it overlaps with
+         * anything. We can make it smarter later on
+         */
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+            OS_EXIT_CRITICAL(sr);
+            return -1;
+        }
+    }
+
+    if (!entry) {
+        rc = 0;
+        TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 1;
+    }
+
+done:
+
+    /* Get head of list to restart timer */
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+
+    ble_ll_rfmgmt_sched_changed(sch);
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    BLE_LL_ASSERT(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+#endif
+/**
+ * Stop the scheduler
+ *
+ * Context: Link Layer task
+ */
+void
+ble_ll_sched_stop(void)
+{
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+}
+
+/**
+ * Initialize the scheduler. Should only be called once and should be called
+ * before any of the scheduler API are called.
+ *
+ * @return int
+ */
+int
+ble_ll_sched_init(void)
+{
+    BLE_LL_DEBUG_GPIO_INIT(SCHED_ITEM_CB);
+    BLE_LL_DEBUG_GPIO_INIT(SCHED_RUN);
+
+    /*
+     * Initialize max early to large negative number. This is used
+     * to determine the worst-case "early" time the schedule was called. Dont
+     * expect this to be less than -3 or -4.
+     */
+#if (BLE_LL_SCHED_DEBUG == 1)
+    g_ble_ll_sched_max_early = -50000;
+#endif
+
+    /*
+     * This is the offset from the start of the scheduled item until the actual
+     * tx/rx should occur, in ticks. We also "round up" to the nearest tick.
+     */
+    g_ble_ll_sched_offset_ticks =
+        (uint8_t) os_cputime_usecs_to_ticks(XCVR_TX_SCHED_DELAY_USECS + 30);
+
+    /* Initialize cputimer for the scheduler */
+    os_cputime_timer_init(&g_ble_ll_sched_timer, ble_ll_sched_run, NULL);
+
+#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING)
+    memset(&g_ble_ll_sched_data, 0, sizeof(struct ble_ll_sched_obj));
+    g_ble_ll_sched_data.sch_ticks_per_period =
+        os_cputime_usecs_to_ticks(MYNEWT_VAL(BLE_LL_USECS_PER_PERIOD));
+    g_ble_ll_sched_data.sch_ticks_per_epoch = BLE_LL_SCHED_PERIODS *
+        g_ble_ll_sched_data.sch_ticks_per_period;
+#endif
+
+    return 0;
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_supp_cmd.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_supp_cmd.c
new file mode 100644
index 00000000..834e0095
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_supp_cmd.c
@@ -0,0 +1,458 @@
+/*
+ * 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 "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+
+/* Octet 0 */
+#define BLE_SUPP_CMD_DISCONNECT             (1 << 5)
+#define BLE_LL_SUPP_CMD_OCTET_0             (BLE_SUPP_CMD_DISCONNECT)
+
+/* Octet 5 */
+#define BLE_SUPP_CMD_SET_EVENT_MASK         (1 << 6)
+#define BLE_LL_SUPP_CMD_OCTET_5             (BLE_SUPP_CMD_SET_EVENT_MASK)
+
+/* Octet 10 */
+#define BLE_SUPP_CMD_RD_TX_PWR              (0 << 2)
+#define BLE_LL_SUPP_CMD_OCTET_10            (BLE_SUPP_CMD_RD_TX_PWR)
+
+/* Octet 14 */
+#define BLE_SUPP_CMD_RD_LOC_VER             (1 << 3)
+#define BLE_SUPP_CMD_RD_LOC_SUPP_FEAT       (1 << 5)
+#define BLE_LL_SUPP_CMD_OCTET_14            \
+(                                           \
+    BLE_SUPP_CMD_RD_LOC_VER         |       \
+    BLE_SUPP_CMD_RD_LOC_SUPP_FEAT           \
+)
+
+/* Octet 15 */
+#define BLE_SUPP_CMD_RD_BD_ADDR             (1 << 1)
+#define BLE_SUPP_CMD_RD_RSSI                (1 << 5)
+
+#define BLE_LL_SUPP_CMD_OCTET_15            \
+(                                           \
+    BLE_SUPP_CMD_RD_BD_ADDR         |       \
+    BLE_SUPP_CMD_RD_RSSI                    \
+)
+
+/* Octet 25 */
+#define BLE_SUPP_CMD_LE_SET_EV_MASK         (1 << 0)
+#define BLE_SUPP_CMD_LE_RD_BUF_SIZE         (1 << 1)
+#define BLE_SUPP_CMD_LE_RD_LOC_FEAT         (1 << 2)
+#define BLE_SUPP_CMD_LE_SET_RAND_ADDR       (1 << 4)
+#define BLE_SUPP_CMD_LE_SET_ADV_PARAMS      (1 << 5)
+#define BLE_SUPP_CMD_LE_SET_ADV_TX_PWR      (1 << 6)
+#define BLE_SUPP_CMD_LE_SET_ADV_DATA        (1 << 7)
+
+#define BLE_LL_SUPP_CMD_OCTET_25            \
+(                                           \
+    BLE_SUPP_CMD_LE_SET_EV_MASK     |       \
+    BLE_SUPP_CMD_LE_RD_BUF_SIZE     |       \
+    BLE_SUPP_CMD_LE_RD_LOC_FEAT     |       \
+    BLE_SUPP_CMD_LE_SET_RAND_ADDR   |       \
+    BLE_SUPP_CMD_LE_SET_ADV_PARAMS  |       \
+    BLE_SUPP_CMD_LE_SET_ADV_TX_PWR  |       \
+    BLE_SUPP_CMD_LE_SET_ADV_DATA            \
+)
+
+/* Octet 26 */
+#define BLE_SUPP_CMD_LE_SET_SCAN_RSP_DATA   (1 << 0)
+#define BLE_SUPP_CMD_LE_SET_ADV_ENABLE      (1 << 1)
+#define BLE_SUPP_CMD_LE_SET_SCAN_PARAMS     (1 << 2)
+#define BLE_SUPP_CMD_LE_SET_SCAN_ENABLE     (1 << 3)
+#define BLE_SUPP_CMD_LE_CREATE_CONN         (1 << 4)
+#define BLE_SUPP_CMD_LE_CREATE_CONN_CANCEL  (1 << 5)
+#define BLE_SUPP_CMD_LE_RD_WHITELIST_SIZE   (1 << 6)
+#define BLE_SUPP_CMD_LE_CLR_WHITELIST       (1 << 7)
+
+#define BLE_LL_SUPP_CMD_OCTET_26            \
+(                                           \
+    BLE_SUPP_CMD_LE_SET_SCAN_RSP_DATA   |   \
+    BLE_SUPP_CMD_LE_SET_ADV_ENABLE      |   \
+    BLE_SUPP_CMD_LE_SET_SCAN_PARAMS     |   \
+    BLE_SUPP_CMD_LE_SET_SCAN_ENABLE     |   \
+    BLE_SUPP_CMD_LE_CREATE_CONN         |   \
+    BLE_SUPP_CMD_LE_CREATE_CONN_CANCEL  |   \
+    BLE_SUPP_CMD_LE_RD_WHITELIST_SIZE   |   \
+    BLE_SUPP_CMD_LE_CLR_WHITELIST           \
+)
+
+/* Octet 27 */
+#define BLE_SUPP_CMD_LE_ADD_DEV_WHITELIST   (1 << 0)
+#define BLE_SUPP_CMD_LE_RMV_DEV_WHITELIST   (1 << 1)
+#define BLE_SUPP_CMD_LE_CONN_UPDATE         (1 << 2)
+#define BLE_SUPP_CMD_LE_SET_HOST_CHAN_CLASS (1 << 3)
+#define BLE_SUPP_CMD_LE_RD_CHAN_MAP         (1 << 4)
+#define BLE_SUPP_CMD_LE_RD_REM_USED_FEAT    (1 << 5)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+#define BLE_SUPP_CMD_LE_ENCRYPT             (1 << 6)
+#else
+#define BLE_SUPP_CMD_LE_ENCRYPT             (0 << 6)
+#endif
+#define BLE_SUPP_CMD_LE_RAND                (1 << 7)
+
+#define BLE_LL_SUPP_CMD_OCTET_27            \
+(                                           \
+    BLE_SUPP_CMD_LE_ENCRYPT             |   \
+    BLE_SUPP_CMD_LE_RAND                |   \
+    BLE_SUPP_CMD_LE_ADD_DEV_WHITELIST   |   \
+    BLE_SUPP_CMD_LE_RMV_DEV_WHITELIST   |   \
+    BLE_SUPP_CMD_LE_CONN_UPDATE         |   \
+    BLE_SUPP_CMD_LE_SET_HOST_CHAN_CLASS |   \
+    BLE_SUPP_CMD_LE_RD_CHAN_MAP         |   \
+    BLE_SUPP_CMD_LE_RD_REM_USED_FEAT        \
+)
+
+/* Octet 28 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+#define BLE_SUPP_CMD_LE_START_ENCRYPT       (1 << 0)
+#define BLE_SUPP_CMD_LE_LTK_REQ_REPLY       (1 << 1)
+#define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY   (1 << 2)
+#else
+#define BLE_SUPP_CMD_LE_START_ENCRYPT       (0 << 0)
+#define BLE_SUPP_CMD_LE_LTK_REQ_REPLY       (0 << 1)
+#define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY   (0 << 2)
+#endif
+#define BLE_SUPP_CMD_LE_READ_SUPP_STATES    (1 << 3)
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+#define BLE_SUPP_CMD_LE_RX_TEST             (1 << 4)
+#define BLE_SUPP_CMD_LE_TX_TEST             (1 << 5)
+#define BLE_SUPP_CMD_LE_TEST_END            (1 << 6)
+
+#else
+#define BLE_SUPP_CMD_LE_RX_TEST             (0 << 4)
+#define BLE_SUPP_CMD_LE_TX_TEST             (0 << 5)
+#define BLE_SUPP_CMD_LE_TEST_END            (0 << 6)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_28            \
+(                                           \
+    BLE_SUPP_CMD_LE_START_ENCRYPT       |   \
+    BLE_SUPP_CMD_LE_LTK_REQ_REPLY       |   \
+    BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY   |   \
+    BLE_SUPP_CMD_LE_READ_SUPP_STATES    |   \
+    BLE_SUPP_CMD_LE_RX_TEST             |   \
+    BLE_SUPP_CMD_LE_TX_TEST             |   \
+    BLE_SUPP_CMD_LE_TEST_END                \
+)
+
+/* Octet 33 */
+#define BLE_SUPP_CMD_LE_REM_CONN_PRR        (1 << 4)
+#define BLE_SUPP_CMD_LE_REM_CONN_PRNR       (1 << 5)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+#define BLE_SUPP_CMD_LE_SET_DATALEN         (1 << 6)
+#define BLE_SUPP_CMD_LE_RD_SUGG_DATALEN     (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_SET_DATALEN         (0 << 6)
+#define BLE_SUPP_CMD_LE_RD_SUGG_DATALEN     (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_33            \
+(                                           \
+    BLE_SUPP_CMD_LE_REM_CONN_PRR        |   \
+    BLE_SUPP_CMD_LE_REM_CONN_PRNR       |   \
+    BLE_SUPP_CMD_LE_SET_DATALEN         |   \
+    BLE_SUPP_CMD_LE_RD_SUGG_DATALEN         \
+)
+
+/* Octet 34 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
+#define BLE_SUPP_CMD_LE_WR_SUGG_DATALEN     (1 << 0)
+#else
+#define BLE_SUPP_CMD_LE_WR_SUGG_DATALEN     (0 << 0)
+#endif
+#define BLE_SUPP_CMD_LE_READ_LOCAL_P256_PK  (0 << 1)
+#define BLE_SUPP_CMD_LE_GENERATE_DH_KEY     (0 << 2)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+#define BLE_SUPP_CMD_LE_ADD_RESOLV_LIST     (1 << 3)
+#define BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST  (1 << 4)
+#define BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST   (1 << 5)
+#define BLE_SUPP_CMD_LE_RD_RESOLV_SIZE      (1 << 6)
+#define BLE_SUPP_CMD_LE_RD_PEER_RESV_ADDR   (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_ADD_RESOLV_LIST     (0 << 3)
+#define BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST  (0 << 4)
+#define BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST   (0 << 5)
+#define BLE_SUPP_CMD_LE_RD_RESOLV_SIZE      (0 << 6)
+#define BLE_SUPP_CMD_LE_RD_PEER_RESV_ADDR   (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_34            \
+(                                           \
+    BLE_SUPP_CMD_LE_WR_SUGG_DATALEN     |   \
+    BLE_SUPP_CMD_LE_READ_LOCAL_P256_PK  |   \
+    BLE_SUPP_CMD_LE_GENERATE_DH_KEY     |   \
+    BLE_SUPP_CMD_LE_ADD_RESOLV_LIST     |   \
+    BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST  |   \
+    BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST   |   \
+    BLE_SUPP_CMD_LE_RD_RESOLV_SIZE      |   \
+    BLE_SUPP_CMD_LE_RD_PEER_RESV_ADDR       \
+)
+
+/* Octet 35 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+#define BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR  (1 << 0)
+#define BLE_SUPP_CMD_LE_SET_ADDR_RES_EN     (1 << 1)
+#define BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO   (1 << 2)
+#else
+#define BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR  (0 << 0)
+#define BLE_SUPP_CMD_LE_SET_ADDR_RES_EN     (0 << 1)
+#define BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO   (0 << 2)
+#endif
+#define BLE_SUPP_CMD_LE_RD_MAX_DATALEN      (1 << 3)
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+#define BLE_SUPP_CMD_LE_READ_PHY            (1 << 4)
+#define BLE_SUPP_CMD_LE_SET_DEFAULT_PHY     (1 << 5)
+#define BLE_SUPP_CMD_LE_SET_PHY             (1 << 6)
+#else
+#define BLE_SUPP_CMD_LE_READ_PHY            (0 << 4)
+#define BLE_SUPP_CMD_LE_SET_DEFAULT_PHY     (0 << 5)
+#define BLE_SUPP_CMD_LE_SET_PHY             (0 << 6)
+#endif
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_35            \
+(                                           \
+    BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR  |   \
+    BLE_SUPP_CMD_LE_SET_ADDR_RES_EN     |   \
+    BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO   |   \
+    BLE_SUPP_CMD_LE_RD_MAX_DATALEN      |   \
+    BLE_SUPP_CMD_LE_READ_PHY            |   \
+    BLE_SUPP_CMD_LE_SET_DEFAULT_PHY     |   \
+    BLE_SUPP_CMD_LE_SET_PHY             |   \
+    BLE_SUPP_CMD_LE_ENHANCED_RX_TEST        \
+)
+
+/* Octet 36 */
+#if MYNEWT_VAL(BLE_LL_DTM)
+#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (1 << 0)
+#else
+#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (0 << 0)
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR  (1 << 1)
+#define BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM   (1 << 2)
+#define BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA    (1 << 3)
+#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_RSP    (1 << 4)
+#define BLE_SUPP_CMD_LE_SET_EXT_ADV_ENABLE  (1 << 5)
+#define BLE_SUPP_CMD_LE_RD_MAX_ADV_DATA_LEN (1 << 6)
+#define BLE_SUPP_CMD_LE_RD_NUM_SUPP_ADVS    (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR  (0 << 1)
+#define BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM   (0 << 2)
+#define BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA    (0 << 3)
+#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_RSP    (0 << 4)
+#define BLE_SUPP_CMD_LE_SET_EXT_ADV_ENABLE  (0 << 5)
+#define BLE_SUPP_CMD_LE_RD_MAX_ADV_DATA_LEN (0 << 6)
+#define BLE_SUPP_CMD_LE_RD_NUM_SUPP_ADVS    (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_36            \
+(                                           \
+    BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    |   \
+    BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR  |   \
+    BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM   |   \
+    BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA    |   \
+    BLE_SUPP_CMD_LE_SET_EXT_SCAN_RSP    |   \
+    BLE_SUPP_CMD_LE_SET_EXT_ADV_ENABLE  |   \
+    BLE_SUPP_CMD_LE_RD_MAX_ADV_DATA_LEN |   \
+    BLE_SUPP_CMD_LE_RD_NUM_SUPP_ADVS        \
+)
+
+/* Octet 37 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_SUPP_CMD_LE_REMOVE_ADVS         (1 << 0)
+#define BLE_SUPP_CMD_LE_CLEAR_ADVS          (1 << 1)
+#else
+#define BLE_SUPP_CMD_LE_REMOVE_ADVS         (0 << 0)
+#define BLE_SUPP_CMD_LE_CLEAR_ADVS          (0 << 1)
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+#define BLE_SUPP_CMD_LE_SET_PADV_PARAM      (1 << 2)
+#define BLE_SUPP_CMD_LE_SET_PADV_DATA       (1 << 3)
+#define BLE_SUPP_CMD_LE_SET_PADV_ENABLE     (1 << 4)
+#else
+#define BLE_SUPP_CMD_LE_SET_PADV_PARAM      (0 << 2)
+#define BLE_SUPP_CMD_LE_SET_PADV_DATA       (0 << 3)
+#define BLE_SUPP_CMD_LE_SET_PADV_ENABLE     (0 << 4)
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM  (1 << 5)
+#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE (1 << 6)
+#define BLE_SUPP_CMD_LE_EXT_CREATE_CONN     (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM  (0 << 5)
+#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE (0 << 6)
+#define BLE_SUPP_CMD_LE_EXT_CREATE_CONN     (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_37            \
+(                                           \
+    BLE_SUPP_CMD_LE_REMOVE_ADVS         |   \
+    BLE_SUPP_CMD_LE_CLEAR_ADVS          |   \
+    BLE_SUPP_CMD_LE_SET_PADV_PARAM      |   \
+    BLE_SUPP_CMD_LE_SET_PADV_DATA       |   \
+    BLE_SUPP_CMD_LE_SET_PADV_ENABLE     |   \
+    BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM  |   \
+    BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE |   \
+    BLE_SUPP_CMD_LE_EXT_CREATE_CONN         \
+)
+
+/* Octet 38 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC    (1 << 0)
+#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C  (1 << 1)
+#define BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC (1 << 2)
+#define BLE_SUPP_CMD_LE_ADD_PADV_LIST       (1 << 3)
+#define BLE_SUPP_CMD_LE_REMOVE_PADV_LIST    (1 << 4)
+#define BLE_SUPP_CMD_LE_CLEAR_PADV_LIST     (1 << 5)
+#define BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE   (1 << 6)
+#else
+#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC    (0 << 0)
+#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C  (0 << 1)
+#define BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC (0 << 2)
+#define BLE_SUPP_CMD_LE_ADD_PADV_LIST       (0 << 3)
+#define BLE_SUPP_CMD_LE_REMOVE_PADV_LIST    (0 << 4)
+#define BLE_SUPP_CMD_LE_CLEAR_PADV_LIST     (0 << 5)
+#define BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE   (0 << 6)
+#endif
+#define BLE_SUPP_CMD_LE_RD_TX_POWER         (1 << 7)
+
+#define BLE_LL_SUPP_CMD_OCTET_38            \
+(                                           \
+    BLE_SUPP_CMD_LE_PADV_CREATE_SYNC    |   \
+    BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C  |   \
+    BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC |   \
+    BLE_SUPP_CMD_LE_ADD_PADV_LIST       |   \
+    BLE_SUPP_CMD_LE_REMOVE_PADV_LIST    |   \
+    BLE_SUPP_CMD_LE_CLEAR_PADV_LIST     |   \
+    BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE   |   \
+    BLE_SUPP_CMD_LE_RD_TX_POWER             \
+)
+
+/* Octet 39 */
+#define BLE_SUPP_CMD_LE_RD_RF_PATH_COMP     (1 << 0)
+#define BLE_SUPP_CMD_LE_WR_RF_PATH_COMP     (1 << 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+#define BLE_SUPP_CMD_LE_SET_PRIVACY_MODE    (1 << 2)
+#else
+#define BLE_SUPP_CMD_LE_SET_PRIVACY_MODE    (0 << 2)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_39            \
+(                                           \
+    BLE_SUPP_CMD_LE_RD_RF_PATH_COMP     |   \
+    BLE_SUPP_CMD_LE_WR_RF_PATH_COMP     |   \
+    BLE_SUPP_CMD_LE_SET_PRIVACY_MODE        \
+)
+
+/* Octet 40 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) && MYNEWT_VAL(BLE_VERSION) >= 51
+#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (1 << 5)
+#else
+#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (0 << 5)
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER (1 << 6)
+#define BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER (0 << 6)
+#define BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_40             \
+(                                            \
+    BLE_SUPP_CMD_LE_PADV_RECV_ENABLE       | \
+    BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER     | \
+    BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER   \
+)
+
+/* Octet 41 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS (1 << 0)
+#define BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS (1 << 1)
+#else
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS (0 << 0)
+#define BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS (0 << 1)
+#endif
+#define BLE_LL_SUPP_CMD_OCTET_41                        \
+(                                                       \
+    BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS |         \
+    BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS   \
+)
+
+/* Defines the array of supported commands */
+const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN] =
+{
+    BLE_LL_SUPP_CMD_OCTET_0,            /* Octet 0 */
+    0,
+    0,
+    0,
+    0,
+    BLE_LL_SUPP_CMD_OCTET_5,
+    0,
+    0,
+    0,                                  /* Octet 8 */
+    0,
+    BLE_LL_SUPP_CMD_OCTET_10,
+    0,
+    0,
+    0,
+    BLE_LL_SUPP_CMD_OCTET_14,
+    BLE_LL_SUPP_CMD_OCTET_15,
+    0,                                  /* Octet 16 */
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,                                 /* Octet 24 */
+    BLE_LL_SUPP_CMD_OCTET_25,
+    BLE_LL_SUPP_CMD_OCTET_26,
+    BLE_LL_SUPP_CMD_OCTET_27,
+    BLE_LL_SUPP_CMD_OCTET_28,
+    0,
+    0,
+    0,
+    0,                                  /* Octet 32 */
+    BLE_LL_SUPP_CMD_OCTET_33,
+    BLE_LL_SUPP_CMD_OCTET_34,
+    BLE_LL_SUPP_CMD_OCTET_35,
+    BLE_LL_SUPP_CMD_OCTET_36,
+    BLE_LL_SUPP_CMD_OCTET_37,
+    BLE_LL_SUPP_CMD_OCTET_38,
+    BLE_LL_SUPP_CMD_OCTET_39,
+    BLE_LL_SUPP_CMD_OCTET_40,           /* Octet 40 */
+    BLE_LL_SUPP_CMD_OCTET_41,
+};
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sync.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sync.c
new file mode 100644
index 00000000..75f18bf2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sync.c
@@ -0,0 +1,2246 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <assert.h>
+
+#include "syscfg/syscfg.h"
+
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_sync.h"
+#include "controller/ble_ll_utils.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_rfmgmt.h"
+
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+
+#include "ble_ll_conn_priv.h"
+
+#include "stats/stats.h"
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+
+/* defines number of events that can be lost during sync establishment
+ * before failed to be established error is reported
+ */
+#define BLE_LL_SYNC_ESTABLISH_CNT 6
+
+#define BLE_LL_SYNC_CNT MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_CNT)
+#define BLE_LL_SYNC_LIST_CNT MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_LIST_CNT)
+
+#define BLE_LL_SYNC_SM_FLAG_RESERVED        0x0001
+#define BLE_LL_SYNC_SM_FLAG_ESTABLISHING    0x0002
+#define BLE_LL_SYNC_SM_FLAG_ESTABLISHED     0x0004
+#define BLE_LL_SYNC_SM_FLAG_SET_ANCHOR      0x0008
+#define BLE_LL_SYNC_SM_FLAG_OFFSET_300      0x0010
+#define BLE_LL_SYNC_SM_FLAG_SYNC_INFO       0x0020
+#define BLE_LL_SYNC_SM_FLAG_DISABLED        0x0040
+#define BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED   0x0080
+#define BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED   0x0100
+
+#define BLE_LL_SYNC_CHMAP_LEN               5
+#define BLE_LL_SYNC_ITVL_USECS              1250
+
+struct ble_ll_sync_sm {
+    uint16_t flags;
+
+    uint8_t adv_sid;
+    uint8_t adv_addr[BLE_DEV_ADDR_LEN];
+    uint8_t adv_addr_type;
+
+    uint8_t sca;
+    uint8_t chanmap[BLE_LL_SYNC_CHMAP_LEN];
+    uint8_t num_used_chans;
+
+    uint8_t chan_index;
+    uint8_t chan_chain;
+
+    uint8_t phy_mode;
+
+    uint8_t sync_pending_cnt;
+
+    uint32_t timeout;
+    uint16_t skip;
+
+    uint16_t itvl;
+    uint8_t  itvl_usecs;
+    uint32_t itvl_ticks;
+
+    uint32_t crcinit; /* only 3 bytes are used */
+    uint32_t access_addr;
+    uint16_t event_cntr;
+    uint16_t channel_id;
+
+    uint32_t window_widening;
+    uint32_t last_anchor_point;
+    uint32_t anchor_point;
+    uint8_t anchor_point_usecs;
+
+    struct ble_ll_sched_item sch;
+
+    struct ble_npl_event sync_ev_end;
+
+    uint8_t *next_report;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    struct ble_ll_conn_sm *transfer_conn;
+    uint8_t *transfer_received_ev;
+    uint16_t transfer_id;
+    uint16_t event_cntr_last_received;
+    uint8_t adv_addr_rpa[6];
+#endif
+};
+
+static struct ble_ll_sync_sm g_ble_ll_sync_sm[BLE_LL_SYNC_CNT];
+
+static struct {
+    uint8_t adv_sid;
+    uint8_t adv_addr[BLE_DEV_ADDR_LEN];
+    uint8_t adv_addr_type;
+} g_ble_ll_sync_adv_list[BLE_LL_SYNC_LIST_CNT];
+
+static struct {
+    uint32_t timeout;
+    uint16_t max_skip;
+    uint16_t options;
+} g_ble_ll_sync_create_params;
+
+/* if this is set HCI LE Sync Create is pending */
+static uint8_t *g_ble_ll_sync_create_comp_ev;
+
+static struct ble_ll_sync_sm *g_ble_ll_sync_sm_current;
+
+static int
+ble_ll_sync_on_list(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        if ((g_ble_ll_sync_adv_list[i].adv_sid == sid) &&
+                (g_ble_ll_sync_adv_list[i].adv_addr_type == addr_type) &&
+                !memcmp(g_ble_ll_sync_adv_list[i].adv_addr, addr, BLE_DEV_ADDR_LEN)) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+static int
+ble_ll_sync_list_get_free(void)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        if (g_ble_ll_sync_adv_list[i].adv_sid == 0xff) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+static bool
+ble_ll_sync_list_empty(void) {
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        if (g_ble_ll_sync_adv_list[i].adv_sid != 0xff) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+static uint8_t
+ble_ll_sync_get_handle(struct ble_ll_sync_sm *sm)
+{
+    /* handle number is offset in global array */
+    return sm - g_ble_ll_sync_sm;
+}
+
+static void
+ble_ll_sync_sm_clear(struct ble_ll_sync_sm *sm)
+{
+    if (sm->flags & (BLE_LL_SYNC_SM_FLAG_ESTABLISHING |
+                     BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        ble_ll_sched_rmv_elem(&sm->sch);
+        ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &sm->sync_ev_end);
+    }
+
+    if (sm->next_report) {
+        ble_hci_trans_buf_free(sm->next_report);
+    }
+
+    if (g_ble_ll_sync_sm_current == sm) {
+        ble_phy_disable();
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        g_ble_ll_sync_sm_current = NULL;
+        ble_ll_scan_chk_resume();
+    }
+
+    ble_ll_rfmgmt_release();
+
+    BLE_LL_ASSERT(sm->sync_ev_end.ev.ev_queued == 0);
+    BLE_LL_ASSERT(sm->sch.enqueued == 0);
+    memset(sm, 0, sizeof(*sm));
+}
+
+static uint8_t
+ble_ll_sync_phy_mode_to_hci(int8_t phy_mode)
+{
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return BLE_HCI_LE_PHY_1M;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_PHY_MODE_2M:
+        return BLE_HCI_LE_PHY_2M;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_PHY_MODE_CODED_125KBPS:
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return BLE_HCI_LE_PHY_CODED;
+#endif
+    default:
+        BLE_LL_ASSERT(false);
+        return BLE_PHY_MODE_1M;
+    }
+#else
+    return BLE_PHY_MODE_1M;
+#endif
+}
+
+static struct ble_ll_sync_sm *
+ble_ll_sync_find(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
+{
+    struct ble_ll_sync_sm *sm;
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        if (!sm->flags) {
+            continue;
+        }
+        if ((sm->adv_sid == sid) && (sm->adv_addr_type == addr_type) &&
+                !memcmp(&sm->adv_addr, addr, BLE_DEV_ADDR_LEN)) {
+            return sm;
+        }
+    }
+
+    return NULL;
+}
+
+static uint16_t
+get_max_skip(uint32_t interval_us, uint32_t timeout_us)
+{
+    uint16_t max_skip;
+
+    BLE_LL_ASSERT(interval_us);
+    BLE_LL_ASSERT(timeout_us);
+
+    if (timeout_us <= interval_us) {
+        return 0;
+    }
+
+    /*
+     * Calculate max allowed skip to receive something before timeout. We adjust
+     * current skip value to be no more than max_skip-6 so we have at least few
+     * attempts to receive an event (so we don't timeout immediately after just
+     * one missed event).
+     */
+
+    max_skip = (timeout_us / interval_us) - 1;
+
+    if (max_skip < 6) {
+        return 0;
+    }
+
+    return max_skip - 6;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+static void
+ble_ll_sync_transfer_received(struct ble_ll_sync_sm *sm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev;
+    struct ble_hci_ev *hci_ev;
+
+    BLE_LL_ASSERT(sm->transfer_received_ev);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER)) {
+        hci_ev = (void *) sm->transfer_received_ev;
+
+        hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+        hci_ev->length = sizeof(*ev);
+
+        ev = (void *) hci_ev->data;
+        ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER;
+
+        ev->status = status;
+        ev->conn_handle = htole16(sm->transfer_conn->conn_handle);
+        ev->service_data = htole16(sm->transfer_id);
+
+        /* this is ignored by host on error */
+        ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+        ev->sid = sm->adv_sid;
+        ev->peer_addr_type = sm->adv_addr_type;
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+            ev->peer_addr_type += 2;
+        }
+        memcpy(ev->peer_addr, sm->adv_addr, BLE_DEV_ADDR_LEN);
+        ev->phy = ble_ll_sync_phy_mode_to_hci(sm->phy_mode);
+        ev->interval = htole16(sm->itvl);
+        ev->aca = sm->sca;
+
+        ble_ll_hci_event_send(hci_ev);
+    } else {
+        ble_hci_trans_buf_free(sm->transfer_received_ev);
+    }
+
+    sm->transfer_received_ev = NULL;
+    sm->transfer_conn = NULL;
+}
+#endif
+
+static void
+ble_ll_sync_est_event_success(struct ble_ll_sync_sm *sm)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev;
+    struct ble_hci_ev *hci_ev;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_create_comp_ev);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB)) {
+        hci_ev = (void *) g_ble_ll_sync_create_comp_ev;
+
+        hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+        hci_ev->length = sizeof(*ev);
+        ev = (void *) hci_ev->data;
+
+        ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB;
+        ev->status = BLE_ERR_SUCCESS;
+        ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+        ev->sid = sm->adv_sid;
+        ev->peer_addr_type = sm->adv_addr_type;
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+            ev->peer_addr_type += 2;
+        }
+        memcpy(ev->peer_addr, sm->adv_addr, BLE_DEV_ADDR_LEN);
+        ev->phy = ble_ll_sync_phy_mode_to_hci(sm->phy_mode);
+        ev->interval = htole16(sm->itvl);
+        ev->aca = sm->sca;
+
+        ble_ll_hci_event_send(hci_ev);
+    } else {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+    }
+
+    g_ble_ll_sync_create_comp_ev = NULL;
+}
+
+static void
+ble_ll_sync_est_event_failed(uint8_t status)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev;
+    struct ble_hci_ev *hci_ev;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_create_comp_ev);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB)) {
+        hci_ev = (void *) g_ble_ll_sync_create_comp_ev;
+
+        hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+        hci_ev->length = sizeof(*ev);
+        ev = (void *) hci_ev->data;
+
+        memset(ev, 0, sizeof(*ev));
+
+        ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB;
+        ev->status = status;
+
+        ble_ll_hci_event_send(hci_ev);
+    } else {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+    }
+
+    g_ble_ll_sync_create_comp_ev = NULL;
+}
+
+static void
+ble_ll_sync_lost_event(struct ble_ll_sync_sm *sm)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST;
+            ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+static void
+ble_ll_sync_current_sm_over(void)
+{
+    /* Disable the PHY */
+    ble_phy_disable();
+
+    /* Link-layer is in standby state now */
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+
+    /* Set current LL sync to NULL */
+    g_ble_ll_sync_sm_current = NULL;
+}
+
+static int
+ble_ll_sync_event_start_cb(struct ble_ll_sched_item *sch)
+{
+    struct ble_ll_sync_sm *sm;
+    uint32_t wfr_usecs;
+    uint32_t start;
+    int rc;
+
+    /* Set current connection state machine */
+    sm = sch->cb_arg;
+    BLE_LL_ASSERT(sm);
+
+    g_ble_ll_sync_sm_current = sm;
+
+    /* Disable whitelisting */
+    ble_ll_whitelist_disable();
+
+    /* Set LL state */
+    ble_ll_state_set(BLE_LL_STATE_SYNC);
+
+    /* Set channel */
+    ble_phy_setchan(sm->chan_index, sm->access_addr, sm->crcinit);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_phy_resolv_list_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_set(sm->phy_mode, sm->phy_mode);
+#endif
+
+    start = sch->start_time + g_ble_ll_sched_offset_ticks;
+    rc = ble_phy_rx_set_start_time(start, sch->remainder);
+    if (rc && rc != BLE_PHY_ERR_RX_LATE) {
+        STATS_INC(ble_ll_stats, sync_event_failed);
+        rc = BLE_LL_SCHED_STATE_DONE;
+        ble_ll_event_send(&sm->sync_ev_end);
+        ble_ll_sync_current_sm_over();
+    } else {
+        /*
+         * Set flag that tells to set last anchor point if a packet
+         * has been received.
+         */
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_SET_ANCHOR;
+
+        /* Set WFR timer.
+         * If establishing we always adjust with offset unit.
+         * If this is first packet of sync (one that was pointed by from
+         * SyncInfo we don't adjust WFT with window widening.
+         */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+            wfr_usecs = (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) ? 300 : 30;
+            if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_SYNC_INFO)) {
+                wfr_usecs += 2 * sm->window_widening;
+            }
+        } else {
+            wfr_usecs = 2 * sm->window_widening;
+        }
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs);
+
+        rc = BLE_LL_SCHED_STATE_RUNNING;
+    }
+
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SYNC_INFO;
+
+    return rc;
+}
+
+/**
+ * Called when a receive PDU has started.
+ *
+ * Context: interrupt
+ *
+ * @return int
+ *   < 0: A frame we dont want to receive.
+ *   = 0: Continue to receive frame. Dont go from rx to tx
+ */
+int
+ble_ll_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr)
+{
+    BLE_LL_ASSERT(g_ble_ll_sync_sm_current);
+
+    /* this also handles chains as those have same PDU type */
+    if (pdu_type != BLE_ADV_PDU_TYPE_AUX_SYNC_IND) {
+        ble_ll_event_send(&g_ble_ll_sync_sm_current->sync_ev_end);
+        ble_ll_sync_current_sm_over();
+        STATS_INC(ble_ll_stats, sched_invalid_pdu);
+        return -1;
+    }
+
+    STATS_INC(ble_ll_stats, sync_received);
+    return 0;
+}
+
+static int
+ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power)
+{
+    uint8_t *rxbuf = om->om_data;
+    uint8_t ext_hdr_flags;
+    uint8_t ext_hdr_len;
+    uint8_t *ext_hdr;
+    uint8_t pdu_len;
+    int i;
+
+    pdu_len = rxbuf[1];
+    if (pdu_len == 0) {
+        return -1;
+    }
+    ext_hdr_len = rxbuf[2] & 0x3F;
+    if (ext_hdr_len > (pdu_len - 1)) {
+        return -1;
+    }
+
+    if (ext_hdr_len) {
+        ext_hdr_flags = rxbuf[3];
+        ext_hdr = &rxbuf[4];
+
+        i = 0;
+
+        /* there should be no AdvA in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+            i += BLE_LL_EXT_ADV_ADVA_SIZE;
+        }
+
+        /* there should be no TargetA in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+            i += BLE_LL_EXT_ADV_TARGETA_SIZE;
+        }
+
+        /* Ignore CTE for now */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+            i += 1;
+        }
+
+        /* there should be no ADI in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+            i += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+        }
+
+        /* get AuXPTR if present */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+            *aux = ext_hdr + i;
+            i += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+        }
+
+        /* there should be no SyncInfo in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+            i += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+            *tx_power = *(ext_hdr + i);
+            i += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+        }
+
+        /* TODO Handle ACAD if needed */
+
+        /* sanity check */
+        if (i > ext_hdr_len) {
+            return -1;
+        }
+    }
+
+    return pdu_len - ext_hdr_len - 1;
+}
+
+static void
+ble_ll_sync_adjust_ext_hdr(struct os_mbuf *om)
+{
+    uint8_t *rxbuf = om->om_data;
+    uint8_t ext_hdr_len;
+
+    /* this was already verified in ble_ll_sync_parse_ext_hdr() */
+    ext_hdr_len = rxbuf[2] & 0x3F;
+
+    os_mbuf_adj(om, 3 + ext_hdr_len);
+}
+
+static void
+ble_ll_sync_send_truncated_per_adv_rpt(struct ble_ll_sync_sm *sm, uint8_t *evbuf)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_rpt *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) ||
+        (sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED)) {
+        ble_hci_trans_buf_free(evbuf);
+        return;
+    }
+
+    hci_ev = (void *) evbuf;
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(*ev);
+    ev = (void *) hci_ev->data;
+
+    ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT;
+    ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+    ev->tx_power = 127; /* not available */
+    ev->rssi = 127; /* not available */
+    ev->cte_type = 0xff;
+    ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+    ev->data_len = 0;
+
+    ble_ll_hci_event_send(hci_ev);
+}
+
+static void
+ble_ll_sync_send_per_adv_rpt(struct ble_ll_sync_sm *sm, struct os_mbuf *rxpdu,
+                             int8_t rssi, int8_t tx_power, int datalen,
+                             uint8_t *aux, bool aux_scheduled)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_rpt *ev;
+    struct ble_hci_ev *hci_ev;
+    struct ble_hci_ev *hci_ev_next = NULL;
+    uint8_t max_data_len;
+    int offset;
+
+    /* use next report buffer if present, this means we are chaining */
+    if (sm->next_report) {
+        hci_ev = (void *) sm->next_report;
+        sm->next_report = NULL;
+    } else {
+        hci_ev = (void * )ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+        if (!hci_ev) {
+            goto done;
+        }
+    }
+
+    max_data_len = BLE_LL_MAX_EVT_LEN - sizeof(*hci_ev) - sizeof(*ev);
+    offset = 0;
+
+    do {
+        if (hci_ev_next) {
+            hci_ev = hci_ev_next;
+            hci_ev_next = NULL;
+        }
+
+        hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+        hci_ev->length = sizeof(*ev);
+
+        ev = (void *) hci_ev->data;
+
+        ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT;
+        ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+        ev->tx_power = tx_power;
+        ev->rssi = rssi;
+        ev->cte_type = 0xff;
+
+        ev->data_len = min(max_data_len, datalen - offset);
+        /* adjust event length */
+        hci_ev->length += ev->data_len;
+
+        os_mbuf_copydata(rxpdu, offset, ev->data_len, ev->data);
+        offset += ev->data_len;
+
+        /* Need another event for next fragment of this PDU */
+        if (offset < datalen) {
+            hci_ev_next = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+            if (hci_ev_next) {
+                ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE;
+            } else {
+                ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+            }
+        } else {
+            /* last report of this PDU */
+            if (aux) {
+                if (aux_scheduled) {
+                    /* if we scheduled aux, we need buffer for next report */
+                    hci_ev_next = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+                    if (hci_ev_next) {
+                        ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE;
+                    } else {
+                        ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+                    }
+                } else {
+                    ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+                }
+            } else {
+                ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_COMPLETE;
+            }
+        }
+        ble_ll_hci_event_send(hci_ev);
+    } while ((offset < datalen) && hci_ev_next);
+
+done:
+    /* this means that we already truncated data (or didn't sent first at all)
+     * in HCI report but has scheduled for next PDU in chain. In that case mark
+     * it so that we end event properly when next PDU is received.
+     * */
+    if (aux_scheduled && !hci_ev_next) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED;
+    }
+
+    /* store for chain */
+    sm->next_report = (void *) hci_ev_next;
+}
+
+/**
+ * Called when a receive PDU has ended.
+ *
+ * Context: Interrupt
+ *
+ * @param rxpdu
+ *
+ * @return int
+ *       < 0: Disable the phy after reception.
+ *      == 0: Success. Do not disable the PHY.
+ *       > 0: Do not disable PHY as that has already been done.
+ */
+int
+ble_ll_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf *rxpdu;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_sm_current);
+
+    /* type was verified in isr_start */
+
+    rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN);
+    if (rxpdu) {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+
+        ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+        ble_hdr->rxinfo.user_data = g_ble_ll_sync_sm_current;
+
+        ble_ll_rx_pdu_in(rxpdu);
+    } else {
+        STATS_INC(ble_ll_stats, sync_rx_buf_err);
+        ble_ll_event_send(&g_ble_ll_sync_sm_current->sync_ev_end);
+    }
+
+    /* PHY is disabled here */
+    ble_ll_sync_current_sm_over();
+
+    return 1;
+}
+
+/**
+ * Called when the wait for response timer expires while in the sync state.
+ *
+ * Context: Interrupt.
+ */
+void
+ble_ll_sync_wfr_timer_exp(void)
+{
+    struct ble_ll_sync_sm *sm = g_ble_ll_sync_sm_current;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_sm_current);
+    STATS_INC(ble_ll_stats, sync_missed_err);
+
+    ble_ll_sync_current_sm_over();
+    ble_ll_event_send(&sm->sync_ev_end);
+}
+
+/**
+ * Called when sync event needs to be halted. This normally should not be called
+ * and is only called when a scheduled item executes but scanning for sync/chain
+ * is stil ongoing
+ * Context: Interrupt
+ */
+void
+ble_ll_sync_halt(void)
+{
+    struct ble_ll_sync_sm *sm = g_ble_ll_sync_sm_current;
+
+    ble_ll_sync_current_sm_over();
+
+    if (sm) {
+        ble_ll_event_send(&sm->sync_ev_end);
+    }
+}
+
+uint32_t
+ble_ll_sync_get_event_end_time(void)
+{
+    uint32_t end_time;
+
+    if (g_ble_ll_sync_sm_current) {
+        end_time = g_ble_ll_sync_sm_current->sch.end_time;
+    } else {
+        end_time = os_cputime_get32();
+    }
+    return end_time;
+}
+
+static uint8_t
+ble_ll_sync_phy_mode_to_aux_phy(uint8_t phy_mode)
+{
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return 0x00;
+    case BLE_PHY_MODE_2M:
+        return 0x01;
+    case BLE_PHY_MODE_CODED_125KBPS:
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return 0x02;
+    default:
+        BLE_LL_ASSERT(false);
+        return 0x00;
+    }
+}
+
+static void
+ble_ll_sync_parse_aux_ptr(const uint8_t *buf, uint8_t *chan, uint32_t *offset,
+                          uint8_t *offset_units, uint8_t *phy)
+{
+    uint32_t aux_ptr_field = get_le32(buf) & 0x00FFFFFF;
+
+    *chan = aux_ptr_field & 0x3F;
+
+    /* TODO use CA aux_ptr_field >> 6 */
+
+    if ((aux_ptr_field >> 7) & 0x01) {
+        *offset = 300 * ((aux_ptr_field >> 8) & 0x1FFF);
+        *offset_units = 1;
+    } else {
+        *offset = 30 * ((aux_ptr_field >> 8) & 0x1FFF);
+        *offset_units = 0;
+    }
+
+    *phy = (aux_ptr_field >> 21) & 0x07;
+}
+
+static int
+ble_ll_sync_chain_start_cb(struct ble_ll_sched_item *sch)
+{
+    struct ble_ll_sync_sm *sm;
+    uint32_t wfr_usecs;
+    uint32_t start;
+    int rc;
+
+    /* Set current connection state machine */
+    sm = sch->cb_arg;
+    g_ble_ll_sync_sm_current = sm;
+    BLE_LL_ASSERT(sm);
+
+    /* Disable whitelisting */
+    ble_ll_whitelist_disable();
+
+    /* Set LL state */
+    ble_ll_state_set(BLE_LL_STATE_SYNC);
+
+    /* Set channel */
+    ble_phy_setchan(sm->chan_chain, sm->access_addr, sm->crcinit);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_phy_resolv_list_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_set(sm->phy_mode, sm->phy_mode);
+#endif
+
+    start = sch->start_time + g_ble_ll_sched_offset_ticks;
+    rc = ble_phy_rx_set_start_time(start, sch->remainder);
+    if (rc && rc != BLE_PHY_ERR_RX_LATE) {
+        STATS_INC(ble_ll_stats, sync_chain_failed);
+        rc = BLE_LL_SCHED_STATE_DONE;
+        ble_ll_event_send(&sm->sync_ev_end);
+        ble_ll_sync_current_sm_over();
+    } else {
+        /*
+         * Clear flag that tells to set last anchor point if a packet
+         * has been received, this is chain and we don't need it.
+         */
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SET_ANCHOR;
+
+        wfr_usecs = (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) ? 300 : 30;
+
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs);
+        rc = BLE_LL_SCHED_STATE_RUNNING;
+    }
+
+    return rc;
+}
+
+static int
+ble_ll_sync_schedule_chain(struct ble_ll_sync_sm *sm, struct ble_mbuf_hdr *hdr,
+                           const uint8_t *aux)
+{
+    uint8_t offset_units;
+    uint32_t offset;
+    uint8_t chan;
+    uint8_t phy;
+
+    ble_ll_sync_parse_aux_ptr(aux, &chan, &offset, &offset_units, &phy);
+
+    if (chan >= BLE_PHY_NUM_DATA_CHANS) {
+        return -1;
+    }
+
+    if (offset < BLE_LL_MAFS) {
+        return -1;
+    }
+
+    /* chain should use same PHY as master PDU */
+    if (phy != ble_ll_sync_phy_mode_to_aux_phy(sm->phy_mode)) {
+        return -1;
+    }
+
+    if (offset_units) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    } else {
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    }
+
+    sm->chan_chain = chan;
+
+    sm->sch.sched_cb = ble_ll_sync_chain_start_cb;
+    sm->sch.cb_arg = sm;
+    sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC;
+
+    return ble_ll_sched_sync(&sm->sch, hdr->beg_cputime, hdr->rem_usecs,
+                             offset, sm->phy_mode);
+}
+
+static void
+ble_ll_sync_established(struct ble_ll_sync_sm *sm)
+{
+    BLE_LL_ASSERT(sm->sync_pending_cnt);
+
+    /* mark as established */
+
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHED;
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+
+    sm->sync_pending_cnt = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    if (sm->transfer_conn) {
+        ble_ll_sync_transfer_received(sm, BLE_ERR_SUCCESS);
+        return;
+    }
+#endif
+
+    ble_ll_sync_est_event_success(sm);
+}
+
+static void
+ble_ll_sync_check_failed(struct ble_ll_sync_sm *sm)
+{
+    BLE_LL_ASSERT(sm->sync_pending_cnt);
+
+    /* if we can retry on next event */
+    if (--sm->sync_pending_cnt) {
+        return;
+    }
+
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    if (sm->transfer_conn) {
+        ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT);
+        return;
+    }
+#endif
+
+    ble_ll_sync_est_event_failed(BLE_ERR_CONN_ESTABLISHMENT);
+}
+
+void
+ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
+{
+    struct ble_ll_sync_sm *sm = hdr->rxinfo.user_data;
+    bool aux_scheduled = false;
+    int8_t tx_power = 127; /* defaults to not available */
+    uint8_t *aux = NULL;
+    int datalen;
+
+    BLE_LL_ASSERT(sm);
+
+    /* this could happen if sync was cancelled or terminated while pkt_in was
+     * already in LL queue, just drop in that case
+     */
+    if (!sm->flags) {
+        ble_ll_scan_chk_resume();
+        ble_ll_rfmgmt_release();
+        return;
+    }
+
+    /* Set anchor point (and last) if 1st rxd frame in sync event.
+     * According to spec this should be done even if CRC is not valid so we
+     * can store it here
+     */
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_SET_ANCHOR) {
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SET_ANCHOR;
+
+        sm->anchor_point = hdr->beg_cputime;
+        sm->anchor_point_usecs = hdr->rem_usecs;
+        sm->last_anchor_point = sm->anchor_point;
+    }
+
+    /* CRC error, end event */
+    if (!BLE_MBUF_HDR_CRC_OK(hdr)) {
+        STATS_INC(ble_ll_stats, sync_crc_err);
+        goto end_event;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    /* save last pa counter */
+    sm->event_cntr_last_received = sm->event_cntr;
+#endif
+
+    /* this means we are chaining but due to low buffers already sent data
+     * truncated report to host (or didn't sent any at all). If this happens
+     * next_buf should be already set to NULL and we just end event.
+     */
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED) {
+        BLE_LL_ASSERT(!sm->next_report);
+        goto end_event;
+    }
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) &&
+        !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED)) {
+        /* get ext header data */
+        datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power);
+        if (datalen < 0) {
+            /* we got bad packet, end event */
+            goto end_event;
+        }
+
+        /* if aux is present, we need to schedule ASAP */
+        if (aux && (ble_ll_sync_schedule_chain(sm, hdr, aux) == 0)) {
+            aux_scheduled = true;
+        }
+
+        /* in case data reporting is enabled we need to send sync established here */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+            ble_ll_sync_established(sm);
+        }
+
+        /* Adjust rxpdu to contain advertising data only */
+        ble_ll_sync_adjust_ext_hdr(rxpdu);
+
+        /* send reports from this PDU */
+        ble_ll_sync_send_per_adv_rpt(sm, rxpdu, hdr->rxinfo.rssi, tx_power,
+                                     datalen, aux, aux_scheduled);
+    } else {
+        /* we need to establish link even if reporting was disabled */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+            ble_ll_sync_established(sm);
+        }
+    }
+
+    /* if chain was scheduled we don't end event yet */
+    /* TODO should we check resume only if offset is high? */
+    if (aux_scheduled) {
+        ble_ll_scan_chk_resume();
+        ble_ll_rfmgmt_release();
+        return;
+    }
+
+end_event:
+    ble_ll_event_send(&sm->sync_ev_end);
+    ble_ll_rfmgmt_release();
+}
+
+static int
+ble_ll_sync_next_event(struct ble_ll_sync_sm *sm, uint32_t cur_ww_adjust)
+{
+    uint32_t cur_ww;
+    uint32_t max_ww;
+    uint32_t ticks;
+    uint32_t itvl;
+    uint8_t usecs;
+    uint16_t skip = sm->skip;
+
+    /* don't skip if are establishing sync or we missed last event */
+    if (skip && ((sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) ||
+                  CPUTIME_LT(sm->last_anchor_point, sm->anchor_point))) {
+        skip = 0;
+    }
+
+    /* Set next event start time, we can use pre-calculated values for one
+     * interval if not skipping
+     */
+    if (skip == 0) {
+        ticks = sm->itvl_ticks;
+        usecs = sm->itvl_usecs;
+    } else {
+        itvl = sm->itvl * BLE_LL_SYNC_ITVL_USECS * (1 + skip);
+        ticks = os_cputime_usecs_to_ticks(itvl);
+        usecs = itvl - os_cputime_ticks_to_usecs(ticks);
+    }
+
+    sm->anchor_point += ticks;
+    sm->anchor_point_usecs += usecs;
+    if (sm->anchor_point_usecs >= 31) {
+        sm->anchor_point++;
+        sm->anchor_point_usecs -= 31;
+    }
+
+    /* Set event counter to the next event */
+    sm->event_cntr += 1 + skip;
+
+    /* Calculate channel index of next event */
+    sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id,
+                                                sm->num_used_chans, sm->chanmap);
+
+    cur_ww = ble_ll_utils_calc_window_widening(sm->anchor_point,
+                                               sm->last_anchor_point,
+                                               sm->sca);
+
+    cur_ww += cur_ww_adjust;
+
+    max_ww = (sm->itvl * (BLE_LL_SYNC_ITVL_USECS / 2)) - BLE_LL_IFS;
+    if (cur_ww >= max_ww) {
+        return -1;
+    }
+
+    cur_ww += BLE_LL_JITTER_USECS;
+
+    /* if updated anchor is pass last anchor + timeout it means we will not be
+     * able to get it in time and hit sync timeout
+     *
+     * note that this may result in sync timeout being sent before real
+     * timeout but we won't be able to fit in time anyway..
+     *
+     * We don't do that when establishing since we try up to
+     * BLE_LL_SYNC_ESTABLISH_CNT events before failing regardless of timeout
+     */
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING)) {
+        if (CPUTIME_GT(sm->anchor_point - os_cputime_usecs_to_ticks(cur_ww),
+                       sm->last_anchor_point + sm->timeout )) {
+            return -1;
+        }
+    }
+
+    sm->window_widening = cur_ww;
+
+    return 0;
+}
+
+static void
+ble_ll_sync_event_end(struct ble_npl_event *ev)
+{
+    struct ble_ll_sync_sm *sm;
+
+    /* Better be a connection state machine! */
+    sm = ble_npl_event_get_arg(ev);
+    BLE_LL_ASSERT(sm);
+
+    ble_ll_rfmgmt_release();
+
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+        ble_ll_sync_check_failed(sm);
+    }
+
+    /* Check if we need to resume scanning */
+    ble_ll_scan_chk_resume();
+
+    /* Remove any end events that might be enqueued */
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &sm->sync_ev_end);
+
+    /* don't schedule next event if sync is not established nor establishing
+     * at this point SM is no longer valid
+     */
+    if (!(sm->flags & (BLE_LL_SYNC_SM_FLAG_ESTABLISHED |
+                       BLE_LL_SYNC_SM_FLAG_ESTABLISHING))) {
+        ble_ll_sync_sm_clear(sm);
+        return;
+    }
+
+    /* if we had prepared buffer for next even it means we were chaining and
+     * must send truncated report to host
+     */
+    if (sm->next_report) {
+        BLE_LL_ASSERT(!(sm->flags & BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED));
+        ble_ll_sync_send_truncated_per_adv_rpt(sm, sm->next_report);
+        sm->next_report = NULL;
+    }
+
+    /* Event ended so we are no longer chaining */
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED;
+
+    sm->sch.sched_cb = ble_ll_sync_event_start_cb;
+    sm->sch.cb_arg = sm;
+    sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC;
+
+    do {
+        if (ble_ll_sync_next_event(sm, 0) < 0) {
+            if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+                /* don't allow any retry if this failed */
+                sm->sync_pending_cnt = 1;
+                ble_ll_sync_check_failed(sm);
+            } else {
+                ble_ll_sync_lost_event(sm);
+            }
+
+            /* at this point SM is no longer valid */
+            ble_ll_sync_sm_clear(sm);
+            return;
+        }
+    } while (ble_ll_sched_sync_reschedule(&sm->sch, sm->anchor_point,
+                                          sm->anchor_point_usecs,
+                                          sm->window_widening, sm->phy_mode));
+}
+
+void
+ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, int rpa_index,
+                       uint8_t sid, struct ble_mbuf_hdr *rxhdr,
+                       const uint8_t *syncinfo)
+{
+    struct ble_ll_sync_sm *sm = NULL;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    const uint8_t *rpa = NULL;
+#endif
+    uint16_t max_skip;
+    uint32_t offset;
+    uint32_t usecs;
+    uint16_t itvl;
+    int i;
+
+    /* ignore if not synchronizing */
+    if (!g_ble_ll_sync_create_comp_ev) {
+        return;
+    }
+
+    /* get reserved SM */
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        if (g_ble_ll_sync_sm[i].flags & BLE_LL_SYNC_SM_FLAG_RESERVED) {
+            sm = &g_ble_ll_sync_sm[i];
+            break;
+        }
+    }
+
+    /* this means we already got sync info event and pending sync */
+    if (!sm) {
+        return;
+    }
+
+    /* check if resolved */
+    if (rpa_index >= 0) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+        rpa = addr;
+#endif
+        addr = g_ble_ll_resolv_list[rpa_index].rl_identity_addr;
+        addr_type = g_ble_ll_resolv_list[rpa_index].rl_addr_type;
+    }
+
+    /* check peer */
+    if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) {
+        if (ble_ll_sync_on_list(addr, addr_type, sid) < 0) {
+            return;
+        }
+
+        /* set addr and sid in sm */
+        sm->adv_sid = sid;
+        sm->adv_addr_type = addr_type;
+        memcpy(sm->adv_addr, addr, BLE_DEV_ADDR_LEN);
+    } else {
+        if ((sm->adv_sid != sid) || (sm->adv_addr_type != addr_type) ||
+                memcmp(sm->adv_addr, addr, BLE_DEV_ADDR_LEN)) {
+            return;
+        }
+    }
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), RFU (2 bits) */
+    offset = syncinfo[0];
+    offset |= (uint16_t)(syncinfo[1] & 0x1f) << 8;
+
+    /* ignore if offset is not valid */
+    if (!offset) {
+        return;
+    }
+
+    /* Interval (2 bytes), ignore if invalid */
+    itvl = get_le16(&syncinfo[2]);
+    if (itvl < 6) {
+        return;
+    }
+
+    if (rpa_index >= 0) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+        memcpy(sm->adv_addr_rpa, rpa, BLE_DEV_ADDR_LEN);
+#endif
+    }
+
+    /* set params from HCI LE Create Periodic Sync */
+    sm->timeout = g_ble_ll_sync_create_params.timeout;
+    sm->skip = g_ble_ll_sync_create_params.max_skip;
+    sm->sync_pending_cnt = BLE_LL_SYNC_ESTABLISH_CNT;
+
+    if (syncinfo[1] & 0x20) {
+        offset *= 300;
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    } else {
+        offset *= 30;
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    }
+
+    /* sync end event */
+    ble_npl_event_init(&sm->sync_ev_end, ble_ll_sync_event_end, sm);
+
+    sm->itvl = itvl;
+
+    /* precalculate interval ticks and usecs */
+    usecs = sm->itvl * BLE_LL_SYNC_ITVL_USECS;
+    sm->itvl_ticks = os_cputime_usecs_to_ticks(usecs);
+    sm->itvl_usecs = (uint8_t)(usecs -
+                               os_cputime_ticks_to_usecs(sm->itvl_ticks));
+    if (sm->itvl_usecs == 31) {
+        sm->itvl_usecs = 0;
+        sm->itvl_ticks++;
+    }
+
+    /* Channels Mask (37 bits) */
+    sm->chanmap[0] = syncinfo[4];
+    sm->chanmap[1] = syncinfo[5];
+    sm->chanmap[2] = syncinfo[6];
+    sm->chanmap[3] = syncinfo[7];
+    sm->chanmap[4] = syncinfo[8] & 0x1f;
+    sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap);
+
+    /* SCA (3 bits) */
+    sm->sca = syncinfo[8] >> 5;
+
+    /* AA (4 bytes) */
+    sm->access_addr = get_le32(&syncinfo[9]);
+    sm->channel_id = ((sm->access_addr & 0xffff0000) >> 16) ^
+                      (sm->access_addr & 0x0000ffff);
+
+    /* CRCInit (3 bytes) */
+    sm->crcinit = syncinfo[15];
+    sm->crcinit = (sm->crcinit << 8) | syncinfo[14];
+    sm->crcinit = (sm->crcinit << 8) | syncinfo[13];
+
+    /* Event Counter (2 bytes) */
+    sm->event_cntr = get_le16(&syncinfo[16]);
+
+    /* adjust skip if pass timeout */
+    max_skip = get_max_skip(sm->itvl * BLE_LL_SYNC_ITVL_USECS, sm->timeout);
+    if (sm->skip > max_skip) {
+        sm->skip = max_skip;
+    }
+
+    /* from now on we only need timeout in ticks */
+    sm->timeout = os_cputime_usecs_to_ticks(sm->timeout);
+
+    sm->phy_mode = rxhdr->rxinfo.phy_mode;
+    sm->window_widening = BLE_LL_JITTER_USECS;
+
+    /* Calculate channel index of first event */
+    sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id,
+                                                sm->num_used_chans, sm->chanmap);
+
+    sm->sch.sched_cb = ble_ll_sync_event_start_cb;
+    sm->sch.cb_arg = sm;
+    sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC;
+
+    if (ble_ll_sched_sync(&sm->sch, rxhdr->beg_cputime, rxhdr->rem_usecs,
+                          offset, sm->phy_mode)) {
+        return;
+    }
+
+    sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks;
+    sm->anchor_point_usecs = sm->sch.remainder;
+    sm->last_anchor_point = sm->anchor_point;
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+    }
+#endif
+
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_RESERVED;
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_SYNC_INFO;
+}
+
+static struct ble_ll_sync_sm *
+ble_ll_sync_reserve(void)
+{
+    struct ble_ll_sync_sm *sm;
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        if (!sm->flags) {
+            sm->flags |= BLE_LL_SYNC_SM_FLAG_RESERVED;
+            return sm;
+        }
+    }
+
+    return NULL;
+}
+
+int
+ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_periodic_adv_create_sync_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_sync_sm *sm;
+    uint16_t timeout;
+    os_sr_t sr;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    if (cmd->options > BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) {
+#else
+    if (cmd->options > BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) {
+#endif
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->skip > 0x01f3) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    timeout = le16toh(cmd->sync_timeout);
+    if (timeout < 0x000a || timeout > 0x4000) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    /* we don't support any CTE yet */
+    if (cmd->sync_cte_type) {
+        if (cmd->sync_cte_type > 4) {
+           return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        return BLE_ERR_UNSUPPORTED;
+    }
+#endif
+
+    if (cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) {
+        if (ble_ll_sync_list_empty()) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+    } else {
+        if (cmd->sid > 0x0f) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        OS_ENTER_CRITICAL(sr);
+        sm = ble_ll_sync_find(cmd->peer_addr, cmd->peer_addr_type, cmd->sid);
+        OS_EXIT_CRITICAL(sr);
+
+        if (sm) {
+            return BLE_ERR_ACL_CONN_EXISTS;
+        }
+    }
+
+    /* reserve buffer for sync complete event */
+    g_ble_ll_sync_create_comp_ev = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    if (!g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    OS_ENTER_CRITICAL(sr);
+
+    /* reserve 1 SM for created sync */
+    sm = ble_ll_sync_reserve();
+    if (!sm) {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+        g_ble_ll_sync_create_comp_ev = NULL;
+        OS_EXIT_CRITICAL(sr);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* if we don't use list, store expected address in reserved SM */
+    if (!(cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER)) {
+        sm->adv_sid = cmd->sid;
+        sm->adv_addr_type = cmd->peer_addr_type;
+        memcpy(&sm->adv_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    g_ble_ll_sync_create_params.timeout = timeout * 10000; /* 10ms units, store in us */;
+    g_ble_ll_sync_create_params.max_skip = cmd->skip;
+    g_ble_ll_sync_create_params.options = cmd->options;
+
+    OS_EXIT_CRITICAL(sr);
+    return BLE_ERR_SUCCESS;
+}
+
+static void
+ble_ll_sync_cancel_complete_event(void)
+{
+    ble_ll_sync_est_event_failed(BLE_ERR_OPERATION_CANCELLED);
+}
+
+int
+ble_ll_sync_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb)
+{
+    struct ble_ll_sync_sm *sm;
+    os_sr_t sr;
+    int i;
+
+    if (!g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    OS_ENTER_CRITICAL(sr);
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        /* cancelled before fist sync info packet */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_RESERVED) {
+            memset(sm, 0, sizeof(*sm));
+            break;
+        }
+
+        /* cancelled while pending sync */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+            ble_ll_sync_sm_clear(sm);
+            break;
+        }
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* g_ble_ll_sync_create_comp_ev will be cleared by this callback */
+    *post_cmd_cb = ble_ll_sync_cancel_complete_event;
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_terminate(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_periodic_adv_term_sync_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_sync_sm *sm;
+    uint16_t handle;
+    os_sr_t sr;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->sync_handle);
+    if (handle > 0xeff) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (handle >= BLE_LL_SYNC_CNT) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    sm = &g_ble_ll_sync_sm[handle];
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        OS_EXIT_CRITICAL(sr);
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    ble_ll_sync_sm_clear(sm);
+
+    OS_EXIT_CRITICAL(sr);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_add(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_add_dev_to_periodic_adv_list_cp *cmd = (const void *)cmdbuf;
+    int i;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+    if (cmd->sid > 0x0f) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    i = ble_ll_sync_on_list(cmd->peer_addr, cmd->peer_addr_type, cmd->sid);
+    if (i >= 0) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    i = ble_ll_sync_list_get_free();
+    if (i < 0) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    g_ble_ll_sync_adv_list[i].adv_sid = cmd->sid;
+    g_ble_ll_sync_adv_list[i].adv_addr_type = cmd->peer_addr_type;
+    memcpy(&g_ble_ll_sync_adv_list[i].adv_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_remove(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rem_dev_from_periodic_adv_list_cp *cmd = (const void *)cmdbuf;
+    int i;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->sid > 0x0f) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    i = ble_ll_sync_on_list(cmd->peer_addr, cmd->peer_addr_type, cmd->sid);
+    if (i < 0) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i]));
+    g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_clear(void)
+{
+    int i;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i]));
+        g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_size(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_periodic_adv_list_size_rp *rsp = (void *) rspbuf;
+
+    rsp->list_size = ARRAY_SIZE(g_ble_ll_sync_adv_list);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+int
+ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_periodic_adv_receive_enable_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_sync_sm *sm;
+    uint16_t handle;
+    os_sr_t sr;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->enable > 0x01) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->sync_handle);
+    if (handle > 0xeff) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (handle >= BLE_LL_SYNC_CNT) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    sm = &g_ble_ll_sync_sm[handle];
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        OS_EXIT_CRITICAL(sr);
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    if (cmd->enable) {
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DISABLED;
+    } else {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+    return BLE_ERR_SUCCESS;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+static struct ble_ll_sync_sm *
+ble_ll_sync_transfer_get(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
+{
+    struct ble_ll_sync_sm *sm;
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        if (!sm->flags) {
+            /* allocate event for transfer received event */
+            sm->transfer_received_ev = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (!sm->transfer_received_ev) {
+                break;
+            }
+
+            sm->adv_sid = sid;
+            sm->adv_addr_type = addr_type;
+            memcpy(&sm->adv_addr, addr, BLE_DEV_ADDR_LEN);
+
+            sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+            return sm;
+        }
+    }
+
+    return NULL;
+}
+
+void
+ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
+                         const uint8_t *sync_ind, bool reports_disabled,
+                         uint16_t max_skip, uint32_t sync_timeout)
+{
+    const uint8_t *syncinfo = sync_ind + 2;
+    uint16_t sync_conn_event_count;
+    uint16_t last_pa_event_count;
+    struct ble_ll_sync_sm *sm;
+    uint16_t conn_event_count;
+    uint8_t sync_anchor_usecs;
+    const uint8_t *rpa = NULL;
+    int last_pa_diff;
+    uint32_t sync_anchor;
+    const uint8_t *addr;
+    uint16_t event_cntr;
+    uint32_t itvl_usecs;
+    uint32_t ww_adjust;
+    uint8_t addr_type;
+    uint8_t phy_mode;
+    uint32_t offset;
+    uint32_t future;
+    uint16_t itvl;
+    int rpa_index;
+    uint8_t sid;
+    uint8_t sca;
+    os_sr_t sr;
+
+    phy_mode = ble_ll_ctrl_phy_from_phy_mask(sync_ind[25]);
+    itvl = get_le16(syncinfo + 2);
+    /* ignore if sync params are not valid */
+    if ((phy_mode == 0) || (itvl < 6)) {
+        return;
+    }
+
+    last_pa_event_count = get_le16(sync_ind + 22);
+    event_cntr = get_le16(syncinfo + 16);
+    itvl_usecs = itvl * BLE_LL_SYNC_ITVL_USECS;
+
+    last_pa_diff = abs((int16_t)(event_cntr - last_pa_event_count));
+    /* check if not 5 seconds apart, if so ignore sync transfer */
+    if ((last_pa_diff * itvl_usecs) > 5000000) {
+        return;
+    }
+
+    sid = (sync_ind[24] & 0x0f);
+    addr_type = (sync_ind[24] & 0x10) ? BLE_ADDR_RANDOM : BLE_ADDR_PUBLIC;
+    addr = sync_ind + 26;
+
+    rpa_index = -1;
+
+    /* check if need to resolve */
+    if (ble_ll_is_rpa(addr, addr_type)) {
+        rpa_index = ble_ll_resolv_peer_rpa_any(addr);
+        if (rpa_index >= 0) {
+            rpa = addr;
+            addr = g_ble_ll_resolv_list[rpa_index].rl_identity_addr;
+            addr_type = g_ble_ll_resolv_list[rpa_index].rl_addr_type;
+        }
+    }
+
+    OS_ENTER_CRITICAL(sr);
+    /* check if already synchronized with this peer */
+    sm = ble_ll_sync_find(addr, addr_type, sid);
+    if (sm) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    /* ignore if no memory for new sync */
+    sm = ble_ll_sync_transfer_get(addr, addr_type, sid);
+    if (!sm) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    if (rpa_index >= 0) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED;
+        memcpy(sm->adv_addr_rpa, rpa, BLE_DEV_ADDR_LEN);
+    }
+
+    /* set params from transfer */
+    sm->timeout = os_cputime_usecs_to_ticks(sync_timeout);
+    sm->skip = max_skip;
+    sm->sync_pending_cnt = BLE_LL_SYNC_ESTABLISH_CNT;
+    sm->transfer_id = get_le16(sync_ind); /* first two bytes */
+    sm->transfer_conn = connsm;
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit),
+     * RFU (1 bit)
+     */
+    offset = syncinfo[0];
+    offset |= (uint16_t)(syncinfo[1] & 0x1f) << 8;
+
+    if (syncinfo[1] & 0x20) {
+        if (syncinfo[1] & 0x40) {
+            offset += 0x2000;
+        }
+
+        offset *= 300;
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    } else {
+        offset *= 30;
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    }
+
+    /* sync end event */
+    ble_npl_event_init(&sm->sync_ev_end, ble_ll_sync_event_end, sm);
+
+    sm->itvl = itvl;
+
+    /* precalculate interval ticks and usecs */
+    sm->itvl_ticks = os_cputime_usecs_to_ticks(itvl_usecs);
+    sm->itvl_usecs = (uint8_t)(itvl_usecs -
+                               os_cputime_ticks_to_usecs(sm->itvl_ticks));
+    if (sm->itvl_usecs == 31) {
+        sm->itvl_usecs = 0;
+        sm->itvl_ticks++;
+    }
+
+    /* Channels Mask (37 bits) */
+    sm->chanmap[0] = syncinfo[4];
+    sm->chanmap[1] = syncinfo[5];
+    sm->chanmap[2] = syncinfo[6];
+    sm->chanmap[3] = syncinfo[7];
+    sm->chanmap[4] = syncinfo[8] & 0x1f;
+    sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap);
+
+    /* SCA (3 bits) */
+    sm->sca = syncinfo[8] >> 5;
+
+    /* AA (4 bytes) */
+    sm->access_addr = get_le32(syncinfo + 9);
+    sm->channel_id = ((sm->access_addr & 0xffff0000) >> 16) ^
+                      (sm->access_addr & 0x0000ffff);
+
+    /* CRCInit (3 bytes) */
+    sm->crcinit = syncinfo[13];
+    sm->crcinit |= syncinfo[14] << 8;
+    sm->crcinit |= syncinfo[15] << 16;
+
+    /* Event Counter (2 bytes) */
+    sm->event_cntr = event_cntr;
+
+    /* adjust skip if pass timeout */
+    max_skip = get_max_skip(sm->itvl * BLE_LL_SYNC_ITVL_USECS, sync_timeout);
+    if (sm->skip > max_skip) {
+        sm->skip = max_skip;
+    }
+
+    sm->phy_mode = phy_mode;
+
+    /* Calculate channel index of first event */
+    sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id,
+                                                sm->num_used_chans, sm->chanmap);
+
+    sm->sch.sched_cb = ble_ll_sync_event_start_cb;
+    sm->sch.cb_arg = sm;
+    sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC;
+
+    /* get anchor for specified conn event */
+    conn_event_count = get_le16(sync_ind + 20);
+    ble_ll_conn_get_anchor(connsm, conn_event_count, &sm->anchor_point,
+                           &sm->anchor_point_usecs);
+
+    /* Set last anchor point */
+    sm->last_anchor_point = sm->anchor_point - (last_pa_diff * sm->itvl_ticks);
+
+    /* calculate extra window widening */
+    sync_conn_event_count = get_le16(sync_ind + 32);
+    sca = sync_ind[24] >> 5;
+    ble_ll_conn_get_anchor(connsm, sync_conn_event_count, &sync_anchor,
+                           &sync_anchor_usecs);
+    ww_adjust = ble_ll_utils_calc_window_widening(connsm->anchor_point,
+                                                  sync_anchor, sca);
+
+    /* spin until we get anchor in future */
+    future = os_cputime_get32() + g_ble_ll_sched_offset_ticks;
+    while (CPUTIME_LT(sm->anchor_point, future)) {
+        if (ble_ll_sync_next_event(sm, ww_adjust) < 0) {
+            /* release SM if this failed */
+            ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT);
+            memset(sm, 0, sizeof(*sm));
+            return;
+        }
+    }
+
+    if (ble_ll_sched_sync(&sm->sch, sm->anchor_point, sm->anchor_point_usecs,
+                          offset, sm->phy_mode)) {
+        /* release SM if this failed */
+        ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT);
+        memset(sm, 0, sizeof(*sm));
+        return;
+    }
+
+    /* Set new anchor point */
+    sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks;
+    sm->anchor_point_usecs = sm->sch.remainder;
+
+    if (reports_disabled) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+    }
+}
+
+static void
+ble_ll_sync_put_syncinfo(struct ble_ll_sync_sm *syncsm,
+                         struct ble_ll_conn_sm *connsm, uint8_t *conn_event_cnt,
+                         uint8_t *dptr)
+{
+    uint8_t anchor_usecs;
+    uint16_t conn_cnt;
+    uint32_t offset;
+    uint32_t anchor;
+    uint8_t units;
+
+    anchor = connsm->anchor_point;
+    anchor_usecs = connsm->anchor_point_usecs;
+    conn_cnt = connsm->event_cntr;
+
+    /* get anchor for conn event that is before periodic_adv_event_start_time */
+    while (CPUTIME_GT(anchor, syncsm->anchor_point)) {
+        ble_ll_conn_get_anchor(connsm, --conn_cnt, &anchor, &anchor_usecs);
+    }
+
+    offset = os_cputime_ticks_to_usecs(syncsm->anchor_point - anchor);
+    offset -= anchor_usecs;
+    offset += syncsm->anchor_point_usecs;
+
+    /* connEventCount */
+    put_le16(conn_event_cnt, conn_cnt);
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit),
+     * RFU (1 bit)
+     */
+    if (offset > 245700) {
+        units = 0x20;
+
+        if (offset >= 0x2000) {
+            offset -= 0x2000;
+            units |= 0x40;
+        }
+
+        offset = offset / 300;
+    } else {
+        units = 0x00;
+        offset = offset / 30;
+    }
+
+    dptr[0] = (offset & 0x000000ff);
+    dptr[1] = ((offset >> 8) & 0x0000001f) | units;
+
+    /* Interval (2 bytes) */
+    put_le16(&dptr[2], syncsm->itvl);
+
+    /* Channels Mask (37 bits) */
+    dptr[4] = syncsm->chanmap[0];
+    dptr[5] = syncsm->chanmap[1];
+    dptr[6] = syncsm->chanmap[2];
+    dptr[7] = syncsm->chanmap[3];
+    dptr[8] = syncsm->chanmap[4] & 0x1f;
+
+    /* SCA (3 bits) */
+    dptr[8] |= syncsm->sca << 5;
+
+    /* AA (4 bytes) */
+    put_le32(&dptr[9], syncsm->access_addr);
+
+    /* CRCInit (3 bytes) */
+    dptr[13] = (uint8_t)syncsm->crcinit;
+    dptr[14] = (uint8_t)(syncsm->crcinit >> 8);
+    dptr[15] = (uint8_t)(syncsm->crcinit >> 16);
+
+    /* Event Counter (2 bytes) */
+    put_le16(&dptr[16], syncsm->event_cntr);
+}
+
+static int
+ble_ll_sync_send_sync_ind(struct ble_ll_sync_sm *syncsm,
+                          struct ble_ll_conn_sm *connsm, uint16_t service_data)
+{
+    struct os_mbuf *om;
+    uint8_t *sync_ind;
+
+    om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+                            sizeof(struct ble_mbuf_hdr));
+    if (!om) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    om->om_data[0] = BLE_LL_CTRL_PERIODIC_SYNC_IND;
+
+    sync_ind = om->om_data + 1;
+
+    /* ID (service_data), already in LE order */
+    memcpy(sync_ind, &service_data, sizeof(service_data));
+
+    /* fill in syncinfo */
+    ble_ll_sync_put_syncinfo(syncsm, connsm, sync_ind + 20, sync_ind + 2);
+
+    /* lastPaEventCounter */
+    put_le16(sync_ind + 22, syncsm->event_cntr_last_received);
+
+    /* SID, AType, SCA */
+    sync_ind[24] = syncsm->adv_sid;
+
+    if (syncsm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+        sync_ind[24] |= 1 << 4;
+    } else {
+        sync_ind[24] |= (syncsm->adv_addr_type == BLE_ADDR_RANDOM) << 4 ;
+    }
+
+    sync_ind[24] |= MYNEWT_VAL(BLE_LL_MASTER_SCA) << 5;
+
+    /* PHY */
+    sync_ind[25] = (0x01 << (ble_ll_sync_phy_mode_to_hci(syncsm->phy_mode) - 1));
+
+    /* AdvA */
+    if (syncsm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+        memcpy(sync_ind + 26, syncsm->adv_addr_rpa, BLE_DEV_ADDR_LEN);
+    } else {
+        memcpy(sync_ind + 26, syncsm->adv_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    /* syncConnEventCount */
+    put_le16(sync_ind + 32, connsm->event_cntr);
+
+    ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL,
+                            BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN + 1);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len,
+                     uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_periodic_adv_sync_transfer_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_periodic_adv_sync_transfer_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_sync_sm *sm;
+    uint16_t handle;
+    os_sr_t sr;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    handle = le16toh(cmd->sync_handle);
+    if (handle > 0xeff) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (handle >= BLE_LL_SYNC_CNT) {
+        rc = BLE_ERR_UNK_ADV_INDENT;
+        goto done;
+    }
+
+    sm = &g_ble_ll_sync_sm[handle];
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        rc = BLE_ERR_UNK_ADV_INDENT;
+        OS_EXIT_CRITICAL(sr);
+        goto done;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+    if (handle > 0xeff) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        OS_EXIT_CRITICAL(sr);
+        goto done;
+    }
+
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        OS_EXIT_CRITICAL(sr);
+        goto done;
+    }
+
+    /* TODO should not need to shift
+     * byte 3 (0 byte is conn_feature) , bit 1
+     *
+     * Allow initiate LL procedure only if remote supports it.
+     */
+    if (!(connsm->remote_features[2] & (BLE_LL_FEAT_SYNC_TRANS_RECV >> (8 * 3)))) {
+        rc = BLE_ERR_UNSUPP_REM_FEATURE;
+        goto done;
+    }
+
+    rc = ble_ll_sync_send_sync_ind(sm, connsm, cmd->service_data);
+
+    OS_EXIT_CRITICAL(sr);
+done:
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+#endif
+
+/*
+ * Called when a sync scan event has been removed from the scheduler
+ * without being run.
+ */
+void
+ble_ll_sync_rmvd_from_sched(struct ble_ll_sync_sm *sm)
+{
+    ble_ll_event_send(&sm->sync_ev_end);
+}
+
+bool
+ble_ll_sync_enabled(void)
+{
+    return g_ble_ll_sync_create_comp_ev != NULL;
+}
+
+/**
+ * Called to reset the sync module. When this function is called the
+ * scheduler has been stopped and the phy has been disabled. The LL should
+ * be in the standby state.
+ */
+void
+ble_ll_sync_reset(void)
+{
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        ble_ll_sync_sm_clear(&g_ble_ll_sync_sm[i]);
+    }
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i]));
+        g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+    }
+
+    g_ble_ll_sync_create_params.timeout = 0;
+    g_ble_ll_sync_create_params.max_skip = 0;
+    g_ble_ll_sync_create_params.options = 0;
+
+    g_ble_ll_sync_sm_current = NULL;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+        g_ble_ll_sync_create_comp_ev = NULL;
+    }
+}
+
+void
+ble_ll_sync_init(void)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+    }
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_trace.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_trace.c
new file mode 100644
index 00000000..330b3d4c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_trace.c
@@ -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.
+ */
+
+#include <stdint.h>
+#include "syscfg/syscfg.h"
+#include "os/os_trace_api.h"
+
+#if MYNEWT_VAL(BLE_LL_SYSVIEW)
+
+static os_trace_module_t g_ble_ll_trace_mod;
+uint32_t ble_ll_trace_off;
+
+static void
+ble_ll_trace_module_send_desc(void)
+{
+    os_trace_module_desc(&g_ble_ll_trace_mod, "0 ll_sched lls=%u cputime=%u start_time=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "1 ll_rx_start lls=%u pdu_type=%x");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "2 ll_rx_end pdu_type=%x len=%u flags=%x");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "3 ll_wfr_timer_exp lls=%u xcvr=%u rx_start=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "4 ll_ctrl_rx opcode=%u len=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "5 ll_conn_ev_start conn_handle=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "6 ll_conn_ev_end conn_handle=%u event_cntr=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "7 ll_conn_end conn_handle=%u event_cntr=%u err=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "8 ll_conn_tx len=%u offset=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "9 ll_conn_rx conn_sn=%u pdu_nesn=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "10 ll_adv_txdone inst=%u chanset=%x");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "11 ll_adv_halt inst=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "12 ll_aux_ref aux=%p ref=%u");
+    os_trace_module_desc(&g_ble_ll_trace_mod, "13 ll_aux_unref aux=%p ref=%u");
+}
+
+void
+ble_ll_trace_init(void)
+{
+    ble_ll_trace_off =
+            os_trace_module_register(&g_ble_ll_trace_mod, "ble_ll", 12,
+                                     ble_ll_trace_module_send_desc);
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_utils.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_utils.c
new file mode 100644
index 00000000..7fbb18f1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_utils.c
@@ -0,0 +1,301 @@
+/*
+ * 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 "nimble/ble.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_utils.h"
+
+/* 37 bits require 5 bytes */
+#define BLE_LL_CHMAP_LEN (5)
+
+/* Sleep clock accuracy table (in ppm) */
+static const uint16_t g_ble_sca_ppm_tbl[8] = {
+    500, 250, 150, 100, 75, 50, 30, 20
+};
+
+uint32_t
+ble_ll_utils_calc_access_addr(void)
+{
+    uint32_t aa;
+    uint16_t aa_low;
+    uint16_t aa_high;
+    uint32_t temp;
+    uint32_t mask;
+    uint32_t prev_bit;
+    uint8_t bits_diff;
+    uint8_t consecutive;
+    uint8_t transitions;
+    uint8_t ones;
+    int tmp;
+
+    /* Calculate a random access address */
+    aa = 0;
+    while (1) {
+        /* Get two, 16-bit random numbers */
+        aa_low = rand() & 0xFFFF;
+        aa_high = rand() & 0xFFFF;
+
+        /* All four bytes cannot be equal */
+        if (aa_low == aa_high) {
+            continue;
+        }
+
+        /* Upper 6 bits must have 2 transitions */
+        tmp = (int16_t)aa_high >> 10;
+        if (__builtin_popcount(tmp ^ (tmp >> 1)) < 2) {
+            continue;
+        }
+
+        /* Cannot be access address or be 1 bit different */
+        aa = aa_high;
+        aa = (aa << 16) | aa_low;
+        bits_diff = 0;
+        temp = aa ^ BLE_ACCESS_ADDR_ADV;
+        for (mask = 0x00000001; mask != 0; mask <<= 1) {
+            if (mask & temp) {
+                ++bits_diff;
+                if (bits_diff > 1) {
+                    break;
+                }
+            }
+        }
+        if (bits_diff <= 1) {
+            continue;
+        }
+
+        /* Cannot have more than 24 transitions */
+        transitions = 0;
+        consecutive = 1;
+        ones = 0;
+        mask = 0x00000001;
+        while (mask < 0x80000000) {
+            prev_bit = aa & mask;
+            mask <<= 1;
+            if (mask & aa) {
+                if (prev_bit == 0) {
+                    ++transitions;
+                    consecutive = 1;
+                } else {
+                    ++consecutive;
+                }
+            } else {
+                if (prev_bit == 0) {
+                    ++consecutive;
+                } else {
+                    ++transitions;
+                    consecutive = 1;
+                }
+            }
+
+            if (prev_bit) {
+                ones++;
+            }
+
+            /* 8 lsb should have at least three 1 */
+            if (mask == 0x00000100 && ones < 3) {
+                break;
+            }
+
+            /* 16 lsb should have no more than 11 transitions */
+            if (mask == 0x00010000 && transitions > 11) {
+                break;
+            }
+
+            /* This is invalid! */
+            if (consecutive > 6) {
+                /* Make sure we always detect invalid sequence below */
+                mask = 0;
+                break;
+            }
+        }
+
+        /* Invalid sequence found */
+        if (mask != 0x80000000) {
+            continue;
+        }
+
+        /* Cannot be more than 24 transitions */
+        if (transitions > 24) {
+            continue;
+        }
+
+        /* We have a valid access address */
+        break;
+    }
+    return aa;
+}
+
+uint8_t
+ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap)
+{
+    uint8_t cntr;
+    uint8_t mask;
+    uint8_t usable_chans;
+    uint8_t chan;
+    int i, j;
+
+    /* NOTE: possible to build a map but this would use memory. For now,
+     * we just calculate
+     * Iterate through channel map to find this channel
+     */
+    chan = 0;
+    cntr = 0;
+    for (i = 0; i < BLE_LL_CHMAP_LEN; i++) {
+        usable_chans = chanmap[i];
+        if (usable_chans != 0) {
+            mask = 0x01;
+            for (j = 0; j < 8; j++) {
+                if (usable_chans & mask) {
+                    if (cntr == remap_index) {
+                        return (chan + j);
+                    }
+                    ++cntr;
+                }
+                mask <<= 1;
+            }
+        }
+        chan += 8;
+    }
+
+    /* we should never reach here */
+    BLE_LL_ASSERT(0);
+    return 0;
+}
+
+uint8_t
+ble_ll_utils_calc_num_used_chans(const uint8_t *chmap)
+{
+    int i;
+    int j;
+    uint8_t mask;
+    uint8_t chanbyte;
+    uint8_t used_channels;
+
+    used_channels = 0;
+    for (i = 0; i < BLE_LL_CHMAP_LEN; ++i) {
+        chanbyte = chmap[i];
+        if (chanbyte) {
+            if (chanbyte == 0xff) {
+                used_channels += 8;
+            } else {
+                mask = 0x01;
+                for (j = 0; j < 8; ++j) {
+                    if (chanbyte & mask) {
+                        ++used_channels;
+                    }
+                    mask <<= 1;
+                }
+            }
+        }
+    }
+    return used_channels;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+static uint16_t
+ble_ll_utils_csa2_perm(uint16_t in)
+{
+    uint16_t out = 0;
+    int i;
+
+    for (i = 0; i < 8; i++) {
+        out |= ((in >> i) & 0x00000001) << (7 - i);
+    }
+
+    for (i = 8; i < 16; i++) {
+        out |= ((in >> i) & 0x00000001) << (15 + 8 - i);
+    }
+
+    return out;
+}
+
+static uint16_t
+ble_ll_utils_csa2_prng(uint16_t counter, uint16_t ch_id)
+{
+    uint16_t prn_e;
+
+    prn_e = counter ^ ch_id;
+
+    prn_e = ble_ll_utils_csa2_perm(prn_e);
+    prn_e = (prn_e * 17) + ch_id;
+
+    prn_e = ble_ll_utils_csa2_perm(prn_e);
+    prn_e = (prn_e * 17) + ch_id;
+
+    prn_e = ble_ll_utils_csa2_perm(prn_e);
+    prn_e = (prn_e * 17) + ch_id;
+
+    prn_e = prn_e ^ ch_id;
+
+    return prn_e;
+}
+
+uint8_t
+ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id,
+                           uint8_t num_used_chans, const uint8_t *chanmap)
+{
+    uint16_t channel_unmapped;
+    uint8_t remap_index;
+
+    uint16_t prn_e;
+    uint8_t bitpos;
+
+    prn_e = ble_ll_utils_csa2_prng(event_cntr, channel_id);
+
+    channel_unmapped = prn_e % 37;
+
+    /*
+     * If unmapped channel is the channel index of a used channel it is used
+     * as channel index.
+     */
+    bitpos = 1 << (channel_unmapped & 0x07);
+    if (chanmap[channel_unmapped >> 3] & bitpos) {
+        return channel_unmapped;
+    }
+
+    remap_index = (num_used_chans * prn_e) / 0x10000;
+
+    return ble_ll_utils_remapped_channel(remap_index, chanmap);
+}
+#endif
+
+uint32_t
+ble_ll_utils_calc_window_widening(uint32_t anchor_point,
+                                  uint32_t last_anchor_point,
+                                  uint8_t master_sca)
+{
+    uint32_t total_sca_ppm;
+    uint32_t window_widening;
+    int32_t time_since_last_anchor;
+    uint32_t delta_msec;
+
+    window_widening = 0;
+
+    time_since_last_anchor = (int32_t)(anchor_point - last_anchor_point);
+    if (time_since_last_anchor > 0) {
+        delta_msec = os_cputime_ticks_to_usecs(time_since_last_anchor) / 1000;
+        total_sca_ppm = g_ble_sca_ppm_tbl[master_sca] +
+                                          MYNEWT_VAL(BLE_LL_OUR_SCA);
+        window_widening = (total_sca_ppm * delta_msec) / 1000;
+    }
+
+    return window_widening;
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_whitelist.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_whitelist.c
new file mode 100644
index 00000000..04ec6428
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_whitelist.c
@@ -0,0 +1,287 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "ble/xcvr.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_hw.h"
+
+#if (MYNEWT_VAL(BLE_LL_WHITELIST_SIZE) < BLE_HW_WHITE_LIST_SIZE)
+#define BLE_LL_WHITELIST_SIZE       MYNEWT_VAL(BLE_LL_WHITELIST_SIZE)
+#else
+#define BLE_LL_WHITELIST_SIZE       BLE_HW_WHITE_LIST_SIZE
+#endif
+
+struct ble_ll_whitelist_entry
+{
+    uint8_t wl_valid;
+    uint8_t wl_addr_type;
+    uint8_t wl_dev_addr[BLE_DEV_ADDR_LEN];
+};
+
+struct ble_ll_whitelist_entry g_ble_ll_whitelist[BLE_LL_WHITELIST_SIZE];
+
+static int
+ble_ll_whitelist_chg_allowed(void)
+{
+    int rc;
+
+    /*
+     * This command is not allowed if:
+     *  -> advertising uses the whitelist and we are currently advertising.
+     *  -> scanning uses the whitelist and is enabled.
+     *  -> initiating uses whitelist and a LE create connection command is in
+     *     progress
+     */
+    rc = 1;
+    if (!ble_ll_adv_can_chg_whitelist() || !ble_ll_scan_can_chg_whitelist()) {
+        rc = 0;
+    }
+    return rc;
+}
+
+/**
+ * Clear the whitelist.
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_ll_whitelist_clear(void)
+{
+    int i;
+    struct ble_ll_whitelist_entry *wl;
+
+    /* Check proper state */
+    if (!ble_ll_whitelist_chg_allowed()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Set the number of entries to 0 */
+    wl = &g_ble_ll_whitelist[0];
+    for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) {
+        wl->wl_valid = 0;
+        ++wl;
+    }
+
+#if (BLE_USES_HW_WHITELIST == 1)
+    ble_hw_whitelist_clear();
+#endif
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Read the size of the whitelist. This is the total number of whitelist
+ * entries allowed by the controller.
+ *
+ * @param rspbuf Pointer to response buffer
+ *
+ * @return int 0: success.
+ */
+int
+ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_white_list_rp *rsp = (void *) rspbuf;
+
+    rsp->size = BLE_LL_WHITELIST_SIZE;
+
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Searches the whitelist to determine if the address is present in the
+ * whitelist. This is an internal API that only searches the link layer
+ * whitelist and does not care about the hardware whitelist
+ *
+ * @param addr      Device or identity address to check.
+ * @param addr_type Public address (0) or random address (1)
+ *
+ * @return int 0: device is not on whitelist; otherwise the return value
+ * is the 'position' of the device in the whitelist (the index of the element
+ * plus 1).
+ */
+static int
+ble_ll_whitelist_search(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    struct ble_ll_whitelist_entry *wl;
+
+    wl = &g_ble_ll_whitelist[0];
+    for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) {
+        if ((wl->wl_valid) && (wl->wl_addr_type == addr_type) &&
+            (!memcmp(&wl->wl_dev_addr[0], addr, BLE_DEV_ADDR_LEN))) {
+            return i + 1;
+        }
+        ++wl;
+    }
+
+    return 0;
+}
+
+/**
+ * Is there a match between the device and a device on the whitelist.
+ *
+ * NOTE: This API uses the HW, if present, to determine if there was a match
+ * between a received address and an address in the whitelist. If the HW does
+ * not support whitelisting this API is the same as the whitelist search API
+ *
+ * @param addr
+ * @param addr_type Public address (0) or random address (1)
+ * @param is_ident  True if addr is an identity address; false otherwise
+ *
+ * @return int
+ */
+int
+ble_ll_whitelist_match(uint8_t *addr, uint8_t addr_type, int is_ident)
+{
+    int rc;
+#if (BLE_USES_HW_WHITELIST == 1)
+    /*
+     * XXX: This should be changed. This is HW specific: some HW may be able
+     * to both resolve a private address and perform a whitelist check. The
+     * current BLE hw cannot support this.
+     */
+    if (is_ident) {
+        rc = ble_ll_whitelist_search(addr, addr_type);
+    } else {
+        rc = ble_hw_whitelist_match();
+    }
+#else
+    rc = ble_ll_whitelist_search(addr, addr_type);
+#endif
+    return rc;
+}
+
+/**
+ * Add a device to the whitelist
+ *
+ * @return int
+ */
+int
+ble_ll_whitelist_add(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_add_whte_list_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_whitelist_entry *wl;
+    int rc;
+    int i;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Must be in proper state */
+    if (!ble_ll_whitelist_chg_allowed()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    /* Check if we have any open entries */
+    rc = BLE_ERR_SUCCESS;
+    if (!ble_ll_whitelist_search(cmd->addr, cmd->addr_type)) {
+        wl = &g_ble_ll_whitelist[0];
+        for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) {
+            if (wl->wl_valid == 0) {
+                memcpy(&wl->wl_dev_addr[0], cmd->addr, BLE_DEV_ADDR_LEN);
+                wl->wl_addr_type = cmd->addr_type;
+                wl->wl_valid = 1;
+                break;
+            }
+            ++wl;
+        }
+
+        if (i == BLE_LL_WHITELIST_SIZE) {
+            rc = BLE_ERR_MEM_CAPACITY;
+        } else {
+#if (BLE_USES_HW_WHITELIST == 1)
+            rc = ble_hw_whitelist_add(cmd->addr, cmd->addr_type);
+#endif
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Remove a device from the whitelist
+ *
+ * @param cmdbuf
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_ll_whitelist_rmv(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rmv_white_list_cp *cmd = (const void *) cmdbuf;
+    int position;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* Must be in proper state */
+    if (!ble_ll_whitelist_chg_allowed()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    position = ble_ll_whitelist_search(cmd->addr, cmd->addr_type);
+    if (position) {
+        g_ble_ll_whitelist[position - 1].wl_valid = 0;
+    }
+
+#if (BLE_USES_HW_WHITELIST == 1)
+    ble_hw_whitelist_rmv(cmd->addr, cmd->addr_type);
+#endif
+
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Enable whitelisting.
+ *
+ * Note: This function has no effect if we are not using HW whitelisting
+ */
+void
+ble_ll_whitelist_enable(void)
+{
+#if (BLE_USES_HW_WHITELIST == 1)
+    ble_hw_whitelist_enable();
+#endif
+}
+
+/**
+ * Disable whitelisting.
+ *
+ * Note: This function has no effect if we are not using HW whitelisting
+ */
+void
+ble_ll_whitelist_disable(void)
+{
+#if (BLE_USES_HW_WHITELIST == 1)
+    ble_hw_whitelist_disable();
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/syscfg.yml b/src/libs/mynewt-nimble/nimble/controller/syscfg.yml
new file mode 100644
index 00000000..85049cb0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/syscfg.yml
@@ -0,0 +1,434 @@
+# 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_CONTROLLER:
+        description: >
+            Indicates that NimBLE controller is present. The default value for
+            this setting shall not be overriden.
+        value: 1
+
+    BLE_HW_WHITELIST_ENABLE:
+        description: >
+            Used to enable hardware white list
+        value: 1
+
+    BLE_LL_SYSVIEW:
+        description: >
+            Enable SystemView tracing module for controller.
+        value: 0
+
+    BLE_LL_PRIO:
+        description: 'The priority of the LL task'
+        type: 'task_priority'
+        value: 0
+
+    # Sleep clock accuracy (sca). This is the amount of drift in the system
+    # during when the device is sleeping (in parts per million).
+    #
+    # NOTE: 'the' master sca is an enumerated value based on the sca. Rather
+    # than have a piece of code calculate this value, the developer must set
+    # this value based on the value of the SCA using the following table:
+    #
+    #  SCA between 251 and 500 ppm (inclusive); master sca = 0
+    #  SCA between 151 and 250 ppm (inclusive); master sca = 1
+    #  SCA between 101 and 150 ppm (inclusive); master sca = 2
+    #  SCA between 76 and 100 ppm (inclusive); master sca = 3
+    #  SCA between 51 and 75 ppm (inclusive); master sca = 4
+    #  SCA between 31 and 50 ppm (inclusive); master sca = 5
+    #  SCA between 21 and 30 ppm (inclusive); master sca = 6
+    #  SCA between 0 and 20 ppm (inclusive); master sca = 7
+    #
+    #  For example:
+    #      if your clock drift is 101 ppm, your master should be set to 2.
+    #      if your clock drift is 20, your master sca should be set to 7.
+    #
+    #  The values provided below are merely meant to be an example and should
+    #  be replaced by values appropriate for your platform.
+    BLE_LL_OUR_SCA:
+        description: 'The system clock accuracy of the device.'
+        value: '60'    # in ppm
+
+    BLE_LL_MASTER_SCA:
+        description: 'Enumerated value based on our sca'
+        value: '4'
+
+    BLE_LL_TX_PWR_DBM:
+        description: 'Transmit power level.'
+        value: '0'
+
+    BLE_LL_NUM_COMP_PKT_ITVL_MS:
+        description: >
+            Determines the interval at which the controller will send the
+            number of completed packets event to the host. Rate is in milliseconds.
+        value: 2000
+
+    BLE_LL_MFRG_ID:
+        description: >
+            Manufacturer ID. Should be set to unique ID per manufacturer.
+        value: '0xFFFF'
+
+    # Configuration items for the number of duplicate advertisers and the
+    # number of advertisers from which we have heard a scan response.
+    BLE_LL_NUM_SCAN_DUP_ADVS:
+        description: 'The number of duplicate advertisers stored.'
+        value: '8'
+    BLE_LL_NUM_SCAN_RSP_ADVS:
+        description: >
+            The number of advertisers from which we have heard a scan
+            response. Prevents sending duplicate events to host.
+        value: '8'
+
+    BLE_LL_WHITELIST_SIZE:
+        description: 'Size of the LL whitelist.'
+        value: '8'
+
+    BLE_LL_RESOLV_LIST_SIZE:
+        description: 'Size of the resolving list.'
+        value: '4'
+
+    # Data length management definitions for connections. These define the
+    # maximum size of the PDU's that will be sent and/or received in a
+    # connection.
+    BLE_LL_MAX_PKT_SIZE:
+        description: 'The maximum PDU size that can be sent/received'
+        value: '251'
+    BLE_LL_SUPP_MAX_RX_BYTES:
+        description: 'The maximum supported received PDU size'
+        value: MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE)
+    BLE_LL_SUPP_MAX_TX_BYTES:
+        description: 'The maximum supported transmit PDU size'
+        value: MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE)
+    BLE_LL_CONN_INIT_MAX_TX_BYTES:
+        description: >
+            Used to set the initial maximum transmit PDU size in a
+            connection. If this is set to a value greater than 27,
+            the controller will automatically attempt to do the
+            data length update procedure. The host can always tell
+            the controller to update this value.
+        value: '27'
+
+    # The number of slots that will be allocated to each connection
+    BLE_LL_CONN_INIT_SLOTS:
+        description: >
+            This is the number of "slots" allocated to a connection when scheduling
+            connections. Each slot is 1.25 msecs long. Note that a connection event may
+            last longer than the number of slots allocated here and may also end earlier
+            (depending on when the next scheduled event occurs and how much data needs
+            to be transferred in the connection). However, you will be guaranteed that
+            a connection event will be given this much time, if needed. Consecutively
+            scheduled items will be at least this far apart
+        value: '4'
+
+    BLE_LL_CONN_INIT_MIN_WIN_OFFSET:
+        description: >
+            This is the minimum number of "slots" for WindowOffset value used for
+            CONNECT_IND when creating new connection as a master. Each slot is 1.25
+            msecs long. Increasing this value will delay first connection event after
+            connection is created. However, older TI CC254x controllers cannot change
+            connection parameters later if WindowOffset was set to 0 in CONNECT_IND. To
+            ensure interoperability with such devices set this value to 2 (or more).
+        value: '0'
+
+    # Strict scheduling
+    BLE_LL_STRICT_CONN_SCHEDULING:
+        description: >
+            Forces the scheduler on a central to schedule connections in fixed
+            time intervals called periods. If set to 0, the scheduler is not forced
+            to do this. If set to 1, the scheduler will only schedule connections at
+            period boundaries. See comments in ble_ll_sched.h for more details.
+        value: '0'
+
+    BLE_LL_ADD_STRICT_SCHED_PERIODS:
+        description: >
+            The number of additional periods that will be allocated for strict
+            scheduling. The total # of periods allocated for strict scheduling
+            will be equal to the number of connections plus this number.
+        value: '0'
+
+    BLE_LL_USECS_PER_PERIOD:
+        description: >
+            The number of usecs per period.
+        value: '3250'
+
+    # The number of random bytes to store
+    BLE_LL_RNG_BUFSIZE:
+        description: >
+            The number of random bytes that the link layer will try to
+            always have available for the host to use. Decreasing this
+            value may cause host delays if the host needs lots of random
+            material often.
+        value: '32'
+
+    BLE_LL_RFMGMT_ENABLE_TIME:
+        description: >
+            Time required for radio and/or related components to be fully
+            enabled before any request from LL is sent. This value is used
+            by rfmgmt to enable PHY in advance, before request from LL is
+            made. It depends on radio driver selected and may also depend
+            on hardware used:
+            - nrf51 - time required for XTAL to settle
+            - nrf52 - time required for XTAL to settle
+            Value is specified in microseconds. If set to 0, rfmgmt keeps
+            PHY enabled all the time.
+        value: MYNEWT_VAL(BLE_XTAL_SETTLE_TIME)
+
+    # Configuration for LL supported features.
+    #
+    # There are a total 8 features that the LL can support. These can be found
+    # in v4.2, Vol 6 Part B Section 4.6.
+    #
+    # These feature definitions are used to inform a host or other controller
+    # about the LL features supported by the controller.
+    #
+    # NOTE: 'the' controller always supports extended reject indicate and thus
+    # is not listed here.
+
+
+    BLE_LL_CFG_FEAT_LE_ENCRYPTION:
+        description: >
+            This option enables/disables encryption support in the controller.
+            This option saves both both code and RAM.
+        value: '1'
+
+    BLE_LL_CFG_FEAT_CONN_PARAM_REQ:
+        description: >
+            This option enables/disables the connection parameter request
+            procedure.  This is implemented in the controller but is disabled
+            by default.
+        value: '1'
+
+    BLE_LL_CFG_FEAT_SLAVE_INIT_FEAT_XCHG:
+        description: >
+            This option allows a slave to initiate the feature exchange
+            procedure.  This feature is implemented but currently has no impact
+            on code or ram size
+        value: '1'
+
+    BLE_LL_CFG_FEAT_LE_PING:
+        description: >
+            This option allows a controller to send/receive LE pings.
+            Currently, this feature is not implemented by the controller so
+            turning it on or off has no effect.
+        value: 'MYNEWT_VAL_BLE_LL_CFG_FEAT_LE_ENCRYPTION'
+
+    BLE_LL_CFG_FEAT_DATA_LEN_EXT:
+        description: >
+            This option enables/disables the data length update procedure in
+            the controller. If enabled, the controller is allowed to change the
+            size of tx/rx pdu's used in a connection. This option has only
+            minor impact on code size and non on RAM.
+        value: '1'
+
+    BLE_LL_CFG_FEAT_LL_PRIVACY:
+        description: >
+            This option is used to enable/disable LL privacy.
+        value: '1'
+
+    BLE_LL_CFG_FEAT_LE_CSA2:
+        description: >
+            This option is used to enable/disable support for LE Channel
+            Selection Algorithm #2.
+        value: '0'
+
+    BLE_LL_CFG_FEAT_LE_2M_PHY:
+        description: >
+            This option is used to enable/disable support for the 2Mbps PHY.
+        value: '0'
+
+    BLE_LL_CFG_FEAT_LE_CODED_PHY:
+        description: >
+            This option is used to enable/disable support for the coded PHY.
+        value: '0'
+
+    BLE_LL_CFG_FEAT_LL_EXT_ADV:
+        description: >
+            This option is used to enable/disable support for Extended
+            Advertising Feature. That means extended scanner, advertiser
+            and connect.
+        value: MYNEWT_VAL(BLE_EXT_ADV)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV:
+        description: >
+            This option is used to enable/disable support for Periodic
+            Advertising Feature.
+        value: MYNEWT_VAL(BLE_PERIODIC_ADV)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_CNT:
+        description: >
+            This option is used to configure number of supported periodic syncs.
+        value: MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_LIST_CNT:
+        description: >
+            Size of Periodic Advertiser sync list.
+        value: MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER:
+        description: >
+            This option is use to enable/disable support for Periodic
+            Advertising Sync Transfer Feature.
+        value: MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+
+    BLE_LL_EXT_ADV_AUX_PTR_CNT:
+         description: >
+            This option configure a max number of scheduled outstanding auxiliary
+            packets for receive on secondary advertising channel.
+         value: 0
+
+    BLE_PUBLIC_DEV_ADDR:
+        description: >
+            Allows the target or app to override the public device address
+            used by the controller. If all zero, the controller will
+            attempt to retrieve the public device address from its
+            chip specific location. If non-zero, this address will
+            be used.
+        value: "(uint8_t[6]){0x00, 0x00, 0x00, 0x00, 0x00, 0x00}"
+
+    BLE_LL_DTM:
+        description: >
+             Enables HCI Test commands needed for Bluetooth SIG certification
+        value: MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
+    BLE_LL_DTM_EXTENSIONS:
+        description: >
+             Enables non-standard extensions to HCI test commands. Once enabled,
+             HCI_LE_Transmitter_Test accepts extra parameters in addition to
+             those defined in Core specification
+               interval (2 octets)   interval between packets (usecs), overrides
+                                     standard interval
+               pkt_count (2 octets)  number of packets to transmit, controller
+                                     will automatically stop sending packets
+                                     after given number of packets was sent
+             Setting either of these parameters to 0 will configure for default
+             behavior, as per Core specification.
+             If specified interval is shorter then allowed by specification it
+             will be ignored.
+             Extended parameters shall immediately follow standard parameters.
+             Controller can accept both standard and extended version of command
+             depending on specified HCI command length.
+        value: 0
+
+    BLE_LL_VND_EVENT_ON_ASSERT:
+        description: >
+            This options enables controller to send a vendor-specific event on
+            an assertion in controller code. The event contains file name and
+            line number where assertion occured.
+        value: 0
+
+    BLE_LL_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the NimBLE controller.
+        value: 250
+
+    BLE_LL_DEBUG_GPIO_HCI_CMD:
+        description: >
+            GPIO pin number to debug HCI commands flow. Pin is set to high state
+            when HCI command is being processed.
+        value: -1
+    BLE_LL_DEBUG_GPIO_HCI_EV:
+        description: >
+            GPIO pin number to debug HCI events flow. Pin is set to high state
+            when HCI event is being sent.
+        value: -1
+    BLE_LL_DEBUG_GPIO_SCHED_RUN:
+        description: >
+            GPIO pin number to debug scheduler running (on timer). Pin is set
+            to high state while scheduler is running.
+        value: -1
+    BLE_LL_DEBUG_GPIO_SCHED_ITEM_CB:
+        description: >
+            GPIO pin number to debug scheduler item execution times. Pin is set
+            to high state while item is executed.
+        value: -1
+
+# Below settings allow to change scheduler timings. These should be left at
+# default values unless you know what you are doing!
+    BLE_LL_SCHED_AUX_MAFS_DELAY:
+        description: >
+            Additional delay [us] between last ADV_EXT_IND and AUX_ADV_IND PDUs
+            when scheduling extended advertising event. This extends T_MAFS.
+        value: 0
+    BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY:
+        description: >
+            Additional delay [us] between consecutive AUX_CHAIN_IND PDUs
+            when scheduling extended or periodic advertising event. This extends
+            T_MAFS.
+        value: 0
+    BLE_LL_SCHED_SCAN_AUX_PDU_LEN:
+        description: >
+            This is expected PDU len for AUX_ADV_IND and subsequent
+            AUX_CHAIN_IND. When scheduling scan scheduler will reserve time for
+            receiving this amount of time. Setting this to high value improves
+            reception of large PDUs but results in wasting scheduler space when
+            receiving small PDUs only. On the other hand too low value can
+            result in not being able to scan whole PDU due to being preempted
+            by next scheduled item. By default size matching legacy ADV_IND PDU
+            payload is used: ExtHeader (Flags, AdvA, ADI) + 31 bytes of data.
+        range: 1..257
+        value: 41
+
+    BLE_LL_SCHED_SCAN_SYNC_PDU_LEN:
+        description: >
+            This is expected PDU len for AUX_SYNC_IND and subsequent
+            AUX_CHAIN_IND. When scheduling scan scheduler will reserve time for
+            receiving this amount of time. Setting this to high value improves
+            reception of large PDUs but results in wasting scheduler space when
+            receiving small PDUs only. On the other hand too low value can
+            result in not being able to scan whole PDU due to being preempted
+            by next scheduled item. By default size matching PDU with legacy
+            data size is used: ExtHeader + 31 bytes of data.
+        range: 1..257
+        value: 32
+
+# deprecated settings (to be defunct/removed eventually)
+    BLE_LL_DIRECT_TEST_MODE:
+        description: use BLE_LL_DTM instead
+        value: 0
+        deprecated: 1
+    BLE_XTAL_SETTLE_TIME:
+        description: use BLE_LL_RFMGMT_ENABLE_TIME instead
+        value: 0
+        deprecated: 1
+
+# defunct settings (to be removed eventually)
+    BLE_DEVICE:
+        description: Superseded by BLE_CONTROLLER
+        value: 1
+        defunct: 1
+    BLE_LP_CLOCK:
+        description: Superseded by BLE_CONTROLLER
+        value: 1
+        defunct: 1
+    BLE_NUM_COMP_PKT_RATE:
+        description: Superseded by BLE_LL_NUM_COMP_PKT_ITVL_MS
+        value: '(2 * OS_TICKS_PER_SEC)'
+        defunct: 1
+
+
+syscfg.vals.BLE_LL_CFG_FEAT_LL_EXT_ADV:
+    BLE_LL_CFG_FEAT_LE_CSA2: 1
+    BLE_HW_WHITELIST_ENABLE: 0
+    BLE_LL_EXT_ADV_AUX_PTR_CNT: 5
+
+# Enable vendor event on assert in standalone build to make failed assertions in
+# controller code visible when connected to external host
+syscfg.vals.!BLE_HOST:
+    BLE_LL_VND_EVENT_ON_ASSERT: 1
+
+syscfg.restrictions:
+    - OS_CPUTIME_FREQ == 32768
diff --git a/src/libs/mynewt-nimble/nimble/controller/test/pkg.yml b/src/libs/mynewt-nimble/nimble/controller/test/pkg.yml
new file mode 100644
index 00000000..ea728811
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/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/controller/test
+pkg.type: unittest
+pkg.description: "NimBLE controller 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/controller
+
+pkg.deps.SELFTEST:
+    - "@apache-mynewt-core/sys/console/stub"
+    - "@apache-mynewt-core/sys/log/full"
+    - "@apache-mynewt-core/sys/stats/stub"
+    - nimble/drivers/native
+    - nimble/transport/ram
diff --git a/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.c b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.c
new file mode 100644
index 00000000..5261eb5b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.c
@@ -0,0 +1,115 @@
+/*
+ * 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 "controller/ble_ll_test.h"
+#include "controller/ble_ll_conn.h"
+#include "ble_ll_csa2_test.h"
+
+TEST_CASE_SELF(ble_ll_csa2_test_1)
+{
+    struct ble_ll_conn_sm conn;
+    uint8_t rc;
+
+    /*
+     * Note: This test only verified mapped channel. Sample data also specifies
+     * prn_e and unmapped channel values but those would require extra access
+     * to internal state of algorithm which is not exposed.
+     */
+
+    memset(&conn, 0, sizeof(conn));
+
+    CONN_F_CSA2_SUPP(&conn) = 1;
+
+    /*
+     * based on sample data from CoreSpec 5.0 Vol 6 Part C 3.1
+     * (all channels used)
+     */
+    conn.channel_id = ((0x8e89bed6 & 0xffff0000) >> 16) ^
+                       (0x8e89bed6 & 0x0000ffff);
+
+    conn.num_used_chans = 37;
+    conn.chanmap[0] = 0xff;
+    conn.chanmap[1] = 0xff;
+    conn.chanmap[2] = 0xff;
+    conn.chanmap[3] = 0xff;
+    conn.chanmap[4] = 0x1f;
+
+    conn.event_cntr = 1;
+    rc = ble_ll_conn_calc_dci(&conn, 0);
+    TEST_ASSERT(rc == 20);
+
+    conn.event_cntr = 2;
+    rc = ble_ll_conn_calc_dci(&conn, 0);
+    TEST_ASSERT(rc == 6);
+
+    conn.event_cntr = 3;
+    rc = ble_ll_conn_calc_dci(&conn, 0);
+    TEST_ASSERT(rc == 21);
+}
+
+TEST_CASE_SELF(ble_ll_csa2_test_2)
+{
+    struct ble_ll_conn_sm conn;
+    uint8_t rc;
+
+    /*
+     * Note: This test only verified mapped channel. Sample data also specifies
+     * prn_e and unmapped channel values but those would require extra access
+     * to internal state of algorithm which is not exposed.
+     */
+
+    memset(&conn, 0, sizeof(conn));
+
+    CONN_F_CSA2_SUPP(&conn) = 1;
+
+    /*
+     * based on sample data from CoreSpec 5.0 Vol 6 Part C 3.2
+     * (9 channels used)
+     */
+    conn.channel_id = ((0x8e89bed6 & 0xffff0000) >> 16) ^
+                       (0x8e89bed6 & 0x0000ffff);
+
+    conn.num_used_chans = 9;
+    conn.chanmap[0] = 0x00;
+    conn.chanmap[1] = 0x06;
+    conn.chanmap[2] = 0xe0;
+    conn.chanmap[3] = 0x00;
+    conn.chanmap[4] = 0x1e;
+
+    conn.event_cntr = 6;
+    rc = ble_ll_conn_calc_dci(&conn, 0);
+    TEST_ASSERT(rc == 23);
+
+    conn.event_cntr = 7;
+    rc = ble_ll_conn_calc_dci(&conn, 0);
+    TEST_ASSERT(rc == 9);
+
+    conn.event_cntr = 8;
+    rc = ble_ll_conn_calc_dci(&conn, 0);
+    TEST_ASSERT(rc == 34);
+}
+
+TEST_SUITE(ble_ll_csa2_test_suite)
+{
+    ble_ll_csa2_test_1();
+    ble_ll_csa2_test_2();
+}
diff --git a/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.h b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.h
new file mode 100644
index 00000000..5bcc142b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.h
@@ -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.
+ */
+
+#ifndef H_BLE_LL_CSA2_TEST_
+#define H_BLE_LL_CSA2_TEST_
+
+#include "testutil/testutil.h"
+
+TEST_SUITE_DECL(ble_ll_csa2_test_suite);
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_test.c b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_test.c
new file mode 100644
index 00000000..ee089afe
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_test.c
@@ -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.
+ */
+
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "controller/ble_ll_test.h"
+#include "os/os.h"
+#include "testutil/testutil.h"
+#include "ble_ll_csa2_test.h"
+
+#if MYNEWT_VAL(SELFTEST)
+
+int
+main(int argc, char **argv)
+{
+    ble_ll_csa2_test_suite();
+    return tu_any_failed;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/controller/test/syscfg.yml b/src/libs/mynewt-nimble/nimble/controller/test/syscfg.yml
new file mode 100644
index 00000000..6edad438
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/controller/test/syscfg.yml
@@ -0,0 +1,25 @@
+# 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_LL_CFG_FEAT_LE_CSA2: 1
+
+    # Prevent priority conflict with controller task.
+    MCU_TIMER_POLLER_PRIO: 1
+    MCU_UART_POLLER_PRIO: 2
+    NATIVE_SOCKETS_PRIO: 3
diff --git a/src/libs/mynewt-nimble/nimble/drivers/native/include/ble/xcvr.h b/src/libs/mynewt-nimble/nimble/drivers/native/include/ble/xcvr.h
new file mode 100644
index 00000000..04741dd3
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/native/include/ble/xcvr.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_BLE_XCVR_
+#define H_BLE_XCVR_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Transceiver specific defintions */
+#define XCVR_RX_START_DELAY_USECS     (140)
+#define XCVR_TX_START_DELAY_USECS     (140)
+#define XCVR_PROC_DELAY_USECS         (100)
+#define XCVR_TX_SCHED_DELAY_USECS     \
+    (XCVR_TX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+#define XCVR_RX_SCHED_DELAY_USECS     \
+    (XCVR_RX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+
+/*
+ * Define HW whitelist size. This is the total possible whitelist size;
+ * not necessarily the size that will be used (may be smaller)
+ */
+#define BLE_HW_WHITE_LIST_SIZE        (0)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_XCVR_ */
diff --git a/src/libs/mynewt-nimble/nimble/drivers/native/pkg.yml b/src/libs/mynewt-nimble/nimble/drivers/native/pkg.yml
new file mode 100644
index 00000000..d0af185b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/native/pkg.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.
+#
+
+pkg.name: nimble/drivers/native
+pkg.description: BLE driver for simulations.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.apis: ble_driver
+pkg.deps:
+    - nimble/controller
diff --git a/src/libs/mynewt-nimble/nimble/drivers/native/src/ble_hw.c b/src/libs/mynewt-nimble/nimble/drivers/native/src/ble_hw.c
new file mode 100644
index 00000000..5eb1eb95
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/native/src/ble_hw.c
@@ -0,0 +1,239 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "controller/ble_hw.h"
+
+/* Total number of white list elements supported by nrf52 */
+#define BLE_HW_WHITE_LIST_SIZE      (0)
+
+/* We use this to keep track of which entries are set to valid addresses */
+static uint8_t g_ble_hw_whitelist_mask;
+
+/* Returns public device address or -1 if not present */
+int
+ble_hw_get_public_addr(ble_addr_t *addr)
+{
+    return -1;
+}
+
+/* Returns random static address or -1 if not present */
+int
+ble_hw_get_static_addr(ble_addr_t *addr)
+{
+    return -1;
+}
+
+/**
+ * Clear the whitelist
+ *
+ * @return int
+ */
+void
+ble_hw_whitelist_clear(void)
+{
+    g_ble_hw_whitelist_mask = 0;
+}
+
+/**
+ * Add a device to the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
+{
+    return BLE_ERR_MEM_CAPACITY;
+}
+
+/**
+ * Remove a device from the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ */
+void
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
+{
+    return;
+}
+
+/**
+ * Returns the size of the whitelist in HW
+ *
+ * @return int Number of devices allowed in whitelist
+ */
+uint8_t
+ble_hw_whitelist_size(void)
+{
+    return BLE_HW_WHITE_LIST_SIZE;
+}
+
+/**
+ * Enable the whitelisted devices
+ */
+void
+ble_hw_whitelist_enable(void)
+{
+    return;
+}
+
+/**
+ * Disables the whitelisted devices
+ */
+void
+ble_hw_whitelist_disable(void)
+{
+    return;
+}
+
+/**
+ * Boolean function which returns true ('1') if there is a match on the
+ * whitelist.
+ *
+ * @return int
+ */
+int
+ble_hw_whitelist_match(void)
+{
+    return 0;
+}
+
+/* Encrypt data */
+int
+ble_hw_encrypt_block(struct ble_encryption_block *ecb)
+{
+    return -1;
+}
+
+/**
+ * Initialize the random number generator
+ *
+ * @param cb
+ * @param bias
+ *
+ * @return int
+ */
+int
+ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
+{
+    return -1;
+}
+
+/**
+ * Start the random number generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_start(void)
+{
+    return -1;
+}
+
+/**
+ * Stop the random generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_stop(void)
+{
+    return -1;
+}
+
+/**
+ * Read the random number generator.
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_hw_rng_read(void)
+{
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+/**
+ * Clear the resolving list
+ *
+ * @return int
+ */
+void
+ble_hw_resolv_list_clear(void)
+{
+}
+
+/**
+ * Add a device to the hw resolving list
+ *
+ * @param irk   Pointer to IRK to add
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_resolv_list_add(uint8_t *irk)
+{
+    return BLE_ERR_MEM_CAPACITY;
+}
+
+/**
+ * Remove a device from the hw resolving list
+ *
+ * @param index Index of IRK to remove
+ */
+void
+ble_hw_resolv_list_rmv(int index)
+{
+}
+
+/**
+ * Returns the size of the resolving list. NOTE: this returns the maximum
+ * allowable entries in the HW. Configuration options may limit this.
+ *
+ * @return int Number of devices allowed in resolving list
+ */
+uint8_t
+ble_hw_resolv_list_size(void)
+{
+    return 0;
+}
+
+/**
+ * Called to determine if the address received was resolved.
+ *
+ * @return int  Negative values indicate unresolved address; positive values
+ *              indicate index in resolving list of resolved address.
+ */
+int
+ble_hw_resolv_list_match(void)
+{
+    return -1;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/drivers/native/src/ble_phy.c b/src/libs/mynewt-nimble/nimble/drivers/native/src/ble_phy.c
new file mode 100644
index 00000000..f9ab0fcb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/native/src/ble_phy.c
@@ -0,0 +1,652 @@
+/*
+ * 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 <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll.h"
+
+/* BLE PHY data structure */
+struct ble_phy_obj
+{
+    uint8_t phy_stats_initialized;
+    int8_t  phy_txpwr_dbm;
+    int16_t rx_pwr_compensation;
+    uint8_t phy_chan;
+    uint8_t phy_state;
+    uint8_t phy_transition;
+    uint8_t phy_rx_started;
+    uint8_t phy_encrypted;
+    uint8_t phy_privacy;
+    uint8_t phy_tx_pyld_len;
+    uint32_t phy_aar_scratch;
+    uint32_t phy_access_address;
+    struct ble_mbuf_hdr rxhdr;
+    void *txend_arg;
+    uint8_t *rxdptr;
+    ble_phy_tx_end_func txend_cb;
+};
+struct ble_phy_obj g_ble_phy_data;
+
+/* Statistics */
+struct ble_phy_statistics
+{
+    uint32_t tx_good;
+    uint32_t tx_fail;
+    uint32_t tx_late;
+    uint32_t tx_bytes;
+    uint32_t rx_starts;
+    uint32_t rx_aborts;
+    uint32_t rx_valid;
+    uint32_t rx_crc_err;
+    uint32_t phy_isrs;
+    uint32_t radio_state_errs;
+    uint32_t no_bufs;
+};
+
+struct ble_phy_statistics g_ble_phy_stats;
+
+static uint8_t g_ble_phy_tx_buf[BLE_PHY_MAX_PDU_LEN];
+
+/* XCVR object to emulate transceiver */
+struct xcvr_data
+{
+    uint32_t irq_status;
+};
+static struct xcvr_data g_xcvr_data;
+
+#define BLE_XCVR_IRQ_F_RX_START     (0x00000001)
+#define BLE_XCVR_IRQ_F_RX_END       (0x00000002)
+#define BLE_XCVR_IRQ_F_TX_START     (0x00000004)
+#define BLE_XCVR_IRQ_F_TX_END       (0x00000008)
+#define BLE_XCVR_IRQ_F_BYTE_CNTR    (0x00000010)
+
+/* "Rail" power level if outside supported range */
+#define BLE_XCVR_TX_PWR_MAX_DBM     (30)
+#define BLE_XCVR_TX_PWR_MIN_DBM     (-20)
+
+/* Statistics */
+STATS_SECT_START(ble_phy_stats)
+    STATS_SECT_ENTRY(phy_isrs)
+    STATS_SECT_ENTRY(tx_good)
+    STATS_SECT_ENTRY(tx_fail)
+    STATS_SECT_ENTRY(tx_late)
+    STATS_SECT_ENTRY(tx_bytes)
+    STATS_SECT_ENTRY(rx_starts)
+    STATS_SECT_ENTRY(rx_aborts)
+    STATS_SECT_ENTRY(rx_valid)
+    STATS_SECT_ENTRY(rx_crc_err)
+    STATS_SECT_ENTRY(rx_late)
+    STATS_SECT_ENTRY(no_bufs)
+    STATS_SECT_ENTRY(radio_state_errs)
+    STATS_SECT_ENTRY(rx_hw_err)
+    STATS_SECT_ENTRY(tx_hw_err)
+STATS_SECT_END
+STATS_SECT_DECL(ble_phy_stats) ble_phy_stats;
+
+STATS_NAME_START(ble_phy_stats)
+    STATS_NAME(ble_phy_stats, phy_isrs)
+    STATS_NAME(ble_phy_stats, tx_good)
+    STATS_NAME(ble_phy_stats, tx_fail)
+    STATS_NAME(ble_phy_stats, tx_late)
+    STATS_NAME(ble_phy_stats, tx_bytes)
+    STATS_NAME(ble_phy_stats, rx_starts)
+    STATS_NAME(ble_phy_stats, rx_aborts)
+    STATS_NAME(ble_phy_stats, rx_valid)
+    STATS_NAME(ble_phy_stats, rx_crc_err)
+    STATS_NAME(ble_phy_stats, rx_late)
+    STATS_NAME(ble_phy_stats, no_bufs)
+    STATS_NAME(ble_phy_stats, radio_state_errs)
+    STATS_NAME(ble_phy_stats, rx_hw_err)
+    STATS_NAME(ble_phy_stats, tx_hw_err)
+STATS_NAME_END(ble_phy_stats)
+
+/* XXX: TODO:
+
+ * 1) Test the following to make sure it works: suppose an event is already
+ * set to 1 and the interrupt is not enabled. What happens if you enable the
+ * interrupt with the event bit already set to 1
+ * 2) how to deal with interrupts?
+ */
+static uint32_t
+ble_xcvr_get_irq_status(void)
+{
+    return g_xcvr_data.irq_status;
+}
+
+static void
+ble_xcvr_clear_irq(uint32_t mask)
+{
+    g_xcvr_data.irq_status &= ~mask;
+}
+
+/**
+ * Copies the data from the phy receive buffer into a mbuf chain.
+ *
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
+ *
+ */
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
+{
+    uint16_t rem_bytes;
+    uint16_t mb_bytes;
+    uint16_t copylen;
+    uint32_t *dst;
+    uint32_t *src;
+    struct os_mbuf *m;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf_pkthdr *pkthdr;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    pkthdr = OS_MBUF_PKTHDR(rxpdu);
+    rem_bytes = pkthdr->omp_len;
+
+    /* Fill in the mbuf pkthdr first. */
+    dst = (uint32_t *)(rxpdu->om_data);
+    src = (uint32_t *)dptr;
+
+    mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
+    copylen = min(mb_bytes, rem_bytes);
+    copylen &= 0xFFFC;
+    rem_bytes -= copylen;
+    mb_bytes -= copylen;
+    rxpdu->om_len = copylen;
+    while (copylen > 0) {
+        *dst = *src;
+        ++dst;
+        ++src;
+        copylen -= 4;
+    }
+
+    /* Copy remaining bytes */
+    m = rxpdu;
+    while (rem_bytes > 0) {
+        /* If there are enough bytes in the mbuf, copy them and leave */
+        if (rem_bytes <= mb_bytes) {
+            memcpy(m->om_data + m->om_len, src, rem_bytes);
+            m->om_len += rem_bytes;
+            break;
+        }
+
+        m = SLIST_NEXT(m, om_next);
+        assert(m != NULL);
+
+        mb_bytes = m->om_omp->omp_databuf_len;
+        copylen = min(mb_bytes, rem_bytes);
+        copylen &= 0xFFFC;
+        rem_bytes -= copylen;
+        mb_bytes -= copylen;
+        m->om_len = copylen;
+        dst = (uint32_t *)m->om_data;
+        while (copylen > 0) {
+            *dst = *src;
+            ++dst;
+            ++src;
+            copylen -= 4;
+        }
+    }
+
+    /* Copy ble header */
+    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+    memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
+}
+
+void
+ble_phy_isr(void)
+{
+    int rc;
+    uint8_t transition;
+    uint32_t irq_en;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* Check for disabled event. This only happens for transmits now */
+    irq_en = ble_xcvr_get_irq_status();
+    if (irq_en & BLE_XCVR_IRQ_F_TX_END) {
+
+        /* Better be in TX state! */
+        assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
+        ble_xcvr_clear_irq(BLE_XCVR_IRQ_F_TX_END);
+
+        transition = g_ble_phy_data.phy_transition;
+        if (transition == BLE_PHY_TRANSITION_TX_RX) {
+            /* Disable the phy */
+            /* XXX: count no bufs? */
+            ble_phy_disable();
+        } else {
+            /* Better not be going from rx to tx! */
+            assert(transition == BLE_PHY_TRANSITION_NONE);
+        }
+    }
+
+    /* We get this if we have started to receive a frame */
+    if (irq_en & BLE_XCVR_IRQ_F_RX_START) {
+
+        ble_xcvr_clear_irq(BLE_XCVR_IRQ_F_RX_START);
+
+        /* Call Link Layer receive start function */
+        rc = ble_ll_rx_start(g_ble_phy_data.rxdptr, g_ble_phy_data.phy_chan,
+                             &g_ble_phy_data.rxhdr);
+        if (rc >= 0) {
+            /* XXX: set rx end enable isr */
+        } else {
+            /* Disable PHY */
+            ble_phy_disable();
+            irq_en = 0;
+            ++g_ble_phy_stats.rx_aborts;
+        }
+
+        /* Count rx starts */
+        ++g_ble_phy_stats.rx_starts;
+    }
+
+    /* Receive packet end (we dont enable this for transmit) */
+    if (irq_en & BLE_XCVR_IRQ_F_RX_END) {
+
+        ble_xcvr_clear_irq(BLE_XCVR_IRQ_F_RX_END);
+
+        /* Construct BLE header before handing up */
+        ble_hdr = &g_ble_phy_data.rxhdr;
+        ble_hdr->rxinfo.flags = 0;
+        /* XXX: dummy rssi */
+        ble_hdr->rxinfo.rssi = -77 + g_ble_phy_data.rx_pwr_compensation;
+        ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
+        ble_hdr->rxinfo.phy = BLE_PHY_1M;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        ble_hdr->rxinfo.aux_data = NULL;
+#endif
+
+        /* Count PHY valid packets */
+        ++g_ble_phy_stats.rx_valid;
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
+
+        /* Call Link Layer receive payload function */
+        rc = ble_ll_rx_end(g_ble_phy_data.rxdptr, ble_hdr);
+        if (rc < 0) {
+            /* Disable the PHY. */
+            ble_phy_disable();
+        }
+    }
+
+    /* Count # of interrupts */
+    ++g_ble_phy_stats.phy_isrs;
+}
+
+/**
+ * ble phy init
+ *
+ * Initialize the PHY. This is expected to be called once.
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+int
+ble_phy_init(void)
+{
+    /* Set phy channel to an invalid channel so first set channel works */
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+    g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
+
+    g_ble_phy_data.rx_pwr_compensation = 0;
+
+    /* XXX: emulate ISR? */
+
+    return 0;
+}
+
+int
+ble_phy_rx(void)
+{
+    /* Check radio state */
+    if (ble_phy_state_get() != BLE_PHY_STATE_IDLE) {
+        ble_phy_disable();
+        ++g_ble_phy_stats.radio_state_errs;
+        return BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+
+    return 0;
+}
+
+void
+ble_phy_restart_rx(void)
+{
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * Called to enable encryption at the PHY. Note that this state will persist
+ * in the PHY; in other words, if you call this function you have to call
+ * disable so that future PHY transmits/receives will not be encrypted.
+ *
+ * @param pkt_counter
+ * @param iv
+ * @param key
+ * @param is_master
+ */
+void
+ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+                       uint8_t is_master)
+{
+}
+
+void
+ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir)
+{
+}
+
+void
+ble_phy_encrypt_disable(void)
+{
+}
+#endif
+
+void
+ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg)
+{
+    /* Set transmit end callback and arg */
+    g_ble_phy_data.txend_cb = txend_cb;
+    g_ble_phy_data.txend_arg = arg;
+}
+
+/**
+ * Called to set the start time of a transmission.
+ *
+ * This function is called to set the start time when we are not going from
+ * rx to tx automatically.
+ *
+ * NOTE: care must be taken when calling this function. The channel should
+ * already be set.
+ *
+ * @param cputime
+ * @param rem_usecs
+ *
+ * @return int
+ */
+int
+ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    return 0;
+}
+
+/**
+ * Called to set the start time of a reception
+ *
+ * This function acts a bit differently than transmit. If we are late getting
+ * here we will still attempt to receive.
+ *
+ * NOTE: care must be taken when calling this function. The channel should
+ * already be set.
+ *
+ * @param cputime
+ * @param rem_usecs
+ *
+ * @return int
+ */
+int
+ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    return 0;
+}
+
+
+int
+ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
+{
+    uint8_t hdr_byte;
+    int rc;
+
+    if (ble_phy_state_get() != BLE_PHY_STATE_IDLE) {
+        ble_phy_disable();
+        ++g_ble_phy_stats.radio_state_errs;
+        return BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    /* Select tx address */
+    if (g_ble_phy_data.phy_chan < BLE_PHY_NUM_DATA_CHANS) {
+        /* XXX: fix this */
+        assert(0);
+    } else {
+    }
+
+    /* Set the PHY transition */
+    g_ble_phy_data.phy_transition = end_trans;
+
+    /* Set phy state to transmitting and count packet statistics */
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
+    ++g_ble_phy_stats.tx_good;
+    g_ble_phy_stats.tx_bytes += pducb(g_ble_phy_tx_buf, pducb_arg, &hdr_byte) +
+                                BLE_LL_PDU_HDR_LEN;
+    rc = BLE_ERR_SUCCESS;
+
+    return rc;
+}
+
+/**
+ * ble phy txpwr set
+ *
+ * Set the transmit output power (in dBm).
+ *
+ * NOTE: If the output power specified is within the BLE limits but outside
+ * the chip limits, we "rail" the power level so we dont exceed the min/max
+ * chip values.
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int 0: success; anything else is an error
+ */
+int
+ble_phy_txpwr_set(int dbm)
+{
+    /* Check valid range */
+    assert(dbm <= BLE_PHY_MAX_PWR_DBM);
+
+    /* "Rail" power level if outside supported range */
+    if (dbm > BLE_XCVR_TX_PWR_MAX_DBM) {
+        dbm = BLE_XCVR_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < BLE_XCVR_TX_PWR_MIN_DBM) {
+            dbm = BLE_XCVR_TX_PWR_MIN_DBM;
+        }
+    }
+
+    g_ble_phy_data.phy_txpwr_dbm = dbm;
+
+    return 0;
+}
+
+/**
+ * ble phy txpwr round
+ *
+ * Get the rounded transmit output power (in dBm).
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int Rounded power in dBm
+ */
+int ble_phy_txpower_round(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    if (dbm > BLE_XCVR_TX_PWR_MAX_DBM) {
+        dbm = BLE_XCVR_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < BLE_XCVR_TX_PWR_MIN_DBM) {
+            dbm = BLE_XCVR_TX_PWR_MIN_DBM;
+        }
+    }
+
+    return dbm;
+}
+
+/**
+ * ble phy txpwr get
+ *
+ * Get the transmit power.
+ *
+ * @return int  The current PHY transmit power, in dBm
+ */
+int
+ble_phy_txpwr_get(void)
+{
+    return g_ble_phy_data.phy_txpwr_dbm;
+}
+
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
+/**
+ * ble phy setchan
+ *
+ * Sets the logical frequency of the transceiver. The input parameter is the
+ * BLE channel index (0 to 39, inclusive). The NRF52 frequency register
+ * works like this: logical frequency = 2400 + FREQ (MHz).
+ *
+ * Thus, to get a logical frequency of 2402 MHz, you would program the
+ * FREQUENCY register to 2.
+ *
+ * @param chan This is the Data Channel Index or Advertising Channel index
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+int
+ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
+{
+    assert(chan < BLE_PHY_NUM_CHANS);
+
+    /* Check for valid channel range */
+    if (chan >= BLE_PHY_NUM_CHANS) {
+        return BLE_PHY_ERR_INV_PARAM;
+    }
+
+    g_ble_phy_data.phy_access_address = access_addr;
+
+    g_ble_phy_data.phy_chan = chan;
+
+    return 0;
+}
+
+/**
+ * Disable the PHY. This will do the following:
+ *  -> Turn off all phy interrupts.
+ *  -> Disable internal shortcuts.
+ *  -> Disable the radio.
+ *  -> Sets phy state to idle.
+ */
+void
+ble_phy_disable(void)
+{
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+}
+
+/* Gets the current access address */
+uint32_t ble_phy_access_addr_get(void)
+{
+    return g_ble_phy_data.phy_access_address;
+}
+
+/**
+ * Return the phy state
+ *
+ * @return int The current PHY state.
+ */
+int
+ble_phy_state_get(void)
+{
+    return g_ble_phy_data.phy_state;
+}
+
+/**
+ * Called to see if a reception has started
+ *
+ * @return int
+ */
+int
+ble_phy_rx_started(void)
+{
+    return g_ble_phy_data.phy_rx_started;
+}
+
+/**
+ * Called to return the maximum data pdu payload length supported by the
+ * phy. For this chip, if encryption is enabled, the maximum payload is 27
+ * bytes.
+ *
+ * @return uint8_t Maximum data channel PDU payload size supported
+ */
+uint8_t
+ble_phy_max_data_pdu_pyld(void)
+{
+    return BLE_LL_DATA_PDU_MAX_PYLD;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+void
+ble_phy_resolv_list_enable(void)
+{
+    g_ble_phy_data.phy_privacy = 1;
+}
+
+void
+ble_phy_resolv_list_disable(void)
+{
+    g_ble_phy_data.phy_privacy = 0;
+}
+
+/**
+ * Return the transceiver state
+ *
+ * @return int transceiver state.
+ */
+uint8_t
+ble_phy_xcvr_state_get(void)
+{
+   return g_ble_phy_data.phy_state;
+}
+
+#endif
+
+void
+ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs)
+{
+}
+
+void
+ble_phy_rfclk_enable(void)
+{
+}
+
+void
+ble_phy_rfclk_disable(void)
+{
+}
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf51/include/ble/xcvr.h b/src/libs/mynewt-nimble/nimble/drivers/nrf51/include/ble/xcvr.h
new file mode 100644
index 00000000..848c8cc8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf51/include/ble/xcvr.h
@@ -0,0 +1,48 @@
+/*
+ * 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_XCVR_
+#define H_BLE_XCVR_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Transceiver specific defintions */
+/* NOTE: we have to account for the RTC output compare issue */
+#define XCVR_PROC_DELAY_USECS         (230)
+
+#define XCVR_RX_START_DELAY_USECS     (140)
+#define XCVR_TX_START_DELAY_USECS     (140)
+#define XCVR_TX_SCHED_DELAY_USECS     \
+    (XCVR_TX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+#define XCVR_RX_SCHED_DELAY_USECS     \
+    (XCVR_RX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+
+/*
+ * Define HW whitelist size. This is the total possible whitelist size;
+ * not necessarily the size that will be used (may be smaller)
+ */
+#define BLE_HW_WHITE_LIST_SIZE        (8)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_XCVR_ */
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf51/pkg.yml b/src/libs/mynewt-nimble/nimble/drivers/nrf51/pkg.yml
new file mode 100644
index 00000000..816a5635
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf51/pkg.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.
+#
+
+pkg.name: nimble/drivers/nrf51
+pkg.description: BLE driver for nRF51 systems.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.apis: ble_driver
+pkg.deps:
+    - nimble
+    - nimble/controller
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf51/src/ble_hw.c b/src/libs/mynewt-nimble/nimble/drivers/nrf51/src/ble_hw.c
new file mode 100644
index 00000000..3c7296b8
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf51/src/ble_hw.c
@@ -0,0 +1,488 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nrfx.h"
+#include "controller/ble_hw.h"
+#if MYNEWT
+#include "mcu/cmsis_nvic.h"
+#else
+#include "core_cm0.h"
+#include <nimble/nimble_npl_os.h>
+#endif
+#include "os/os_trace_api.h"
+
+/* Total number of resolving list elements */
+#define BLE_HW_RESOLV_LIST_SIZE     (16)
+
+/* We use this to keep track of which entries are set to valid addresses */
+static uint8_t g_ble_hw_whitelist_mask;
+
+/* Random number generator isr callback */
+ble_rng_isr_cb_t g_ble_rng_isr_cb;
+
+/* If LL privacy is enabled, allocate memory for AAR */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+
+/* The NRF51 supports up to 16 IRK entries */
+#if (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE) < 16)
+#define NRF_IRK_LIST_ENTRIES    (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE))
+#else
+#define NRF_IRK_LIST_ENTRIES    (16)
+#endif
+
+/* NOTE: each entry is 16 bytes long. */
+uint32_t g_nrf_irk_list[NRF_IRK_LIST_ENTRIES * 4];
+
+/* Current number of IRK entries */
+uint8_t g_nrf_num_irks;
+
+#endif
+
+/* Returns public device address or -1 if not present */
+int
+ble_hw_get_public_addr(ble_addr_t *addr)
+{
+    uint32_t addr_high;
+    uint32_t addr_low;
+
+    /* Does FICR have a public address */
+    if ((NRF_FICR->DEVICEADDRTYPE & 1) != 0) {
+        return -1;
+    }
+
+    /* Copy into device address. We can do this because we know platform */
+    addr_low = NRF_FICR->DEVICEADDR[0];
+    addr_high = NRF_FICR->DEVICEADDR[1];
+    memcpy(addr->val, &addr_low, 4);
+    memcpy(&addr->val[4], &addr_high, 2);
+    addr->type = BLE_ADDR_PUBLIC;
+
+    return 0;
+}
+
+/* Returns random static address or -1 if not present */
+int
+ble_hw_get_static_addr(ble_addr_t *addr)
+{
+    int rc;
+
+    if ((NRF_FICR->DEVICEADDRTYPE & 1) == 1) {
+        memcpy(addr->val, (void *)&NRF_FICR->DEVICEADDR[0], 4);
+        memcpy(&addr->val[4], (void *)&NRF_FICR->DEVICEADDR[1], 2);
+        addr->val[5] |= 0xc0;
+        addr->type = BLE_ADDR_RANDOM;
+        rc = 0;
+    } else {
+        rc = -1;
+    }
+
+    return rc;
+}
+
+/**
+ * Clear the whitelist
+ *
+ * @return int
+ */
+void
+ble_hw_whitelist_clear(void)
+{
+    NRF_RADIO->DACNF = 0;
+    g_ble_hw_whitelist_mask = 0;
+}
+
+/**
+ * Add a device to the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    uint32_t mask;
+
+    /* Find first ununsed device address match element */
+    mask = 0x01;
+    for (i = 0; i < BLE_HW_WHITE_LIST_SIZE; ++i) {
+        if ((mask & g_ble_hw_whitelist_mask) == 0) {
+            NRF_RADIO->DAB[i] = get_le32(addr);
+            NRF_RADIO->DAP[i] = get_le16(addr + 4);
+            if (addr_type == BLE_ADDR_RANDOM) {
+                NRF_RADIO->DACNF |= (mask << 8);
+            }
+            g_ble_hw_whitelist_mask |= mask;
+            return BLE_ERR_SUCCESS;
+        }
+        mask <<= 1;
+    }
+
+    return BLE_ERR_MEM_CAPACITY;
+}
+
+/**
+ * Remove a device from the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ */
+void
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    uint8_t cfg_addr;
+    uint16_t dap;
+    uint16_t txadd;
+    uint32_t dab;
+    uint32_t mask;
+
+    /* Find first ununsed device address match element */
+    dab = get_le32(addr);
+    dap = get_le16(addr + 4);
+    txadd = NRF_RADIO->DACNF >> 8;
+    mask = 0x01;
+    for (i = 0; i < BLE_HW_WHITE_LIST_SIZE; ++i) {
+        if (mask & g_ble_hw_whitelist_mask) {
+            if ((dab == NRF_RADIO->DAB[i]) && (dap == NRF_RADIO->DAP[i])) {
+                cfg_addr = txadd & mask;
+                if (addr_type == BLE_ADDR_RANDOM) {
+                    if (cfg_addr != 0) {
+                        break;
+                    }
+                } else {
+                    if (cfg_addr == 0) {
+                        break;
+                    }
+                }
+            }
+        }
+        mask <<= 1;
+    }
+
+    if (i < BLE_HW_WHITE_LIST_SIZE) {
+        g_ble_hw_whitelist_mask &= ~mask;
+        NRF_RADIO->DACNF &= ~mask;
+    }
+}
+
+/**
+ * Returns the size of the whitelist in HW
+ *
+ * @return int Number of devices allowed in whitelist
+ */
+uint8_t
+ble_hw_whitelist_size(void)
+{
+    return BLE_HW_WHITE_LIST_SIZE;
+}
+
+/**
+ * Enable the whitelisted devices
+ */
+void
+ble_hw_whitelist_enable(void)
+{
+    /* Enable the configured device addresses */
+    NRF_RADIO->DACNF |= g_ble_hw_whitelist_mask;
+}
+
+/**
+ * Disables the whitelisted devices
+ */
+void
+ble_hw_whitelist_disable(void)
+{
+    /* Disable all whitelist devices */
+    NRF_RADIO->DACNF &= 0x0000ff00;
+}
+
+/**
+ * Boolean function which returns true ('1') if there is a match on the
+ * whitelist.
+ *
+ * @return int
+ */
+int
+ble_hw_whitelist_match(void)
+{
+    return (int)NRF_RADIO->EVENTS_DEVMATCH;
+}
+
+/* Encrypt data */
+int
+ble_hw_encrypt_block(struct ble_encryption_block *ecb)
+{
+    int rc;
+    uint32_t end;
+    uint32_t err;
+
+    /* Stop ECB */
+    NRF_ECB->TASKS_STOPECB = 1;
+    /* XXX: does task stop clear these counters? Anyway to do this quicker? */
+    NRF_ECB->EVENTS_ENDECB = 0;
+    NRF_ECB->EVENTS_ERRORECB = 0;
+    NRF_ECB->ECBDATAPTR = (uint32_t)ecb;
+
+    /* Start ECB */
+    NRF_ECB->TASKS_STARTECB = 1;
+
+    /* Wait till error or done */
+    rc = 0;
+    while (1) {
+        end = NRF_ECB->EVENTS_ENDECB;
+        err = NRF_ECB->EVENTS_ERRORECB;
+        if (end || err) {
+            if (err) {
+                rc = -1;
+            }
+            break;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Random number generator ISR.
+ */
+static void
+ble_rng_isr(void)
+{
+    uint8_t rnum;
+
+    os_trace_isr_enter();
+
+    /* No callback? Clear and disable interrupts */
+    if (g_ble_rng_isr_cb == NULL) {
+        NRF_RNG->INTENCLR = 1;
+        NRF_RNG->EVENTS_VALRDY = 0;
+        (void)NRF_RNG->SHORTS;
+        os_trace_isr_exit();
+        return;
+    }
+
+    /* If there is a value ready grab it */
+    if (NRF_RNG->EVENTS_VALRDY) {
+        NRF_RNG->EVENTS_VALRDY = 0;
+        rnum = (uint8_t)NRF_RNG->VALUE;
+        (*g_ble_rng_isr_cb)(rnum);
+    }
+
+    os_trace_isr_exit();
+}
+
+/**
+ * Initialize the random number generator
+ *
+ * @param cb
+ * @param bias
+ *
+ * @return int
+ */
+int
+ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
+{
+    /* Set bias */
+    if (bias) {
+        NRF_RNG->CONFIG = 1;
+    } else {
+        NRF_RNG->CONFIG = 0;
+    }
+
+    /* If we were passed a function pointer we need to enable the interrupt */
+    if (cb != NULL) {
+#ifndef RIOT_VERSION
+        NVIC_SetPriority(RNG_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+#endif
+#if MYNEWT
+        NVIC_SetVector(RNG_IRQn, (uint32_t)ble_rng_isr);
+#else
+        ble_npl_hw_set_isr(RNG_IRQn, ble_rng_isr);
+#endif
+        NVIC_EnableIRQ(RNG_IRQn);
+        g_ble_rng_isr_cb = cb;
+    }
+
+    return 0;
+}
+
+/**
+ * Start the random number generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_start(void)
+{
+    os_sr_t sr;
+
+    /* No need for interrupt if there is no callback */
+    OS_ENTER_CRITICAL(sr);
+    NRF_RNG->EVENTS_VALRDY = 0;
+    if (g_ble_rng_isr_cb) {
+        NRF_RNG->INTENSET = 1;
+    }
+    NRF_RNG->TASKS_START = 1;
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Stop the random generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_stop(void)
+{
+    os_sr_t sr;
+
+    /* No need for interrupt if there is no callback */
+    OS_ENTER_CRITICAL(sr);
+    NRF_RNG->INTENCLR = 1;
+    NRF_RNG->TASKS_STOP = 1;
+    NRF_RNG->EVENTS_VALRDY = 0;
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Read the random number generator.
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_hw_rng_read(void)
+{
+    uint8_t rnum;
+
+    /* Wait for a sample */
+    while (NRF_RNG->EVENTS_VALRDY == 0) {
+    }
+
+    NRF_RNG->EVENTS_VALRDY = 0;
+    rnum = (uint8_t)NRF_RNG->VALUE;
+
+    return rnum;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+/**
+ * Clear the resolving list
+ *
+ * @return int
+ */
+void
+ble_hw_resolv_list_clear(void)
+{
+    g_nrf_num_irks = 0;
+}
+
+/**
+ * Add a device to the hw resolving list
+ *
+ * @param irk   Pointer to IRK to add
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_resolv_list_add(uint8_t *irk)
+{
+    uint32_t *nrf_entry;
+
+    /* Find first ununsed device address match element */
+    if (g_nrf_num_irks == NRF_IRK_LIST_ENTRIES) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* Copy into irk list */
+    nrf_entry = &g_nrf_irk_list[4 * g_nrf_num_irks];
+    memcpy(nrf_entry, irk, 16);
+
+    /* Add to total */
+    ++g_nrf_num_irks;
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Remove a device from the hw resolving list
+ *
+ * @param index Index of IRK to remove
+ */
+void
+ble_hw_resolv_list_rmv(int index)
+{
+    uint32_t *irk_entry;
+
+    if (index < g_nrf_num_irks) {
+        --g_nrf_num_irks;
+        irk_entry = &g_nrf_irk_list[index];
+        if (g_nrf_num_irks > index) {
+            memmove(irk_entry, irk_entry + 4, 16 * (g_nrf_num_irks - index));
+        }
+    }
+}
+
+/**
+ * Returns the size of the resolving list. NOTE: this returns the maximum
+ * allowable entries in the HW. Configuration options may limit this.
+ *
+ * @return int Number of devices allowed in resolving list
+ */
+uint8_t
+ble_hw_resolv_list_size(void)
+{
+    return BLE_HW_RESOLV_LIST_SIZE;
+}
+
+/**
+ * Called to determine if the address received was resolved.
+ *
+ * @return int  Negative values indicate unresolved address; positive values
+ *              indicate index in resolving list of resolved address.
+ */
+int
+ble_hw_resolv_list_match(void)
+{
+    uint32_t index;
+
+    if (NRF_AAR->EVENTS_END) {
+        if (NRF_AAR->EVENTS_RESOLVED) {
+            index = NRF_AAR->STATUS;
+            return (int)index;
+        }
+    }
+
+    return -1;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf51/src/ble_phy.c b/src/libs/mynewt-nimble/nimble/drivers/nrf51/src/ble_phy.c
new file mode 100644
index 00000000..b7e63297
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf51/src/ble_phy.c
@@ -0,0 +1,1524 @@
+/*
+ * 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 <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_phy_trace.h"
+#include "controller/ble_ll.h"
+#include "nrfx.h"
+
+#if MYNEWT
+#include "mcu/nrf51_clock.h"
+#include "mcu/cmsis_nvic.h"
+#else
+#include "core_cm0.h"
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+#error LE 2M PHY cannot be enabled on nRF51
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#error LE Coded PHY cannot be enabled on nRF51
+#endif
+
+/* XXX: 4) Make sure RF is higher priority interrupt than schedule */
+
+/*
+ * XXX: Maximum possible transmit time is 1 msec for a 60ppm crystal
+ * and 16ms for a 30ppm crystal! We need to limit PDU size based on
+ * crystal accuracy. Look at this in the spec.
+ */
+
+/* XXX: private header file? */
+extern uint8_t g_nrf_num_irks;
+extern uint32_t g_nrf_irk_list[];
+
+/* To disable all radio interrupts */
+#define NRF_RADIO_IRQ_MASK_ALL  (0x34FF)
+
+/*
+ * We configure the nrf with a 1 byte S0 field, 8 bit length field, and
+ * zero bit S1 field. The preamble is 8 bits long.
+ */
+#define NRF_LFLEN_BITS          (8)
+#define NRF_S0_LEN              (1)
+
+/* Maximum length of frames */
+#define NRF_MAXLEN              (255)
+#define NRF_BALEN               (3)     /* For base address of 3 bytes */
+
+/* Maximum tx power */
+#define NRF_TX_PWR_MAX_DBM      (4)
+#define NRF_TX_PWR_MIN_DBM      (-40)
+
+/* Max. encrypted payload length */
+#define NRF_MAX_ENCRYPTED_PYLD_LEN  (27)
+#define NRF_ENC_HDR_SIZE            (3)
+#define NRF_ENC_BUF_SIZE            \
+    (NRF_MAX_ENCRYPTED_PYLD_LEN + NRF_ENC_HDR_SIZE + BLE_LL_DATA_MIC_LEN)
+
+/* BLE PHY data structure */
+struct ble_phy_obj
+{
+    uint8_t phy_stats_initialized;
+    int8_t  phy_txpwr_dbm;
+    uint8_t phy_chan;
+    uint8_t phy_state;
+    uint8_t phy_transition;
+    uint8_t phy_rx_started;
+    uint8_t phy_encrypted;
+    uint8_t phy_privacy;
+    uint8_t phy_tx_pyld_len;
+    uint8_t *rxdptr;
+    int8_t  rx_pwr_compensation;
+    uint32_t phy_aar_scratch;
+    uint32_t phy_access_address;
+    struct ble_mbuf_hdr rxhdr;
+    void *txend_arg;
+    ble_phy_tx_end_func txend_cb;
+    uint32_t phy_start_cputime;
+};
+struct ble_phy_obj g_ble_phy_data;
+
+/* XXX: if 27 byte packets desired we can make this smaller */
+/* Global transmit/receive buffer */
+static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/* Make sure word-aligned for faster copies */
+static uint32_t g_ble_phy_enc_buf[(NRF_ENC_BUF_SIZE + 3) / 4];
+#endif
+
+/* RF center frequency for each channel index (offset from 2400 MHz) */
+static const uint8_t g_ble_phy_chan_freq[BLE_PHY_NUM_CHANS] = {
+     4,  6,  8, 10, 12, 14, 16, 18, 20, 22, /* 0-9 */
+    24, 28, 30, 32, 34, 36, 38, 40, 42, 44, /* 10-19 */
+    46, 48, 50, 52, 54, 56, 58, 60, 62, 64, /* 20-29 */
+    66, 68, 70, 72, 74, 76, 78,  2, 26, 80, /* 30-39 */
+};
+
+/* Statistics */
+STATS_SECT_START(ble_phy_stats)
+    STATS_SECT_ENTRY(phy_isrs)
+    STATS_SECT_ENTRY(tx_good)
+    STATS_SECT_ENTRY(tx_fail)
+    STATS_SECT_ENTRY(tx_late)
+    STATS_SECT_ENTRY(tx_bytes)
+    STATS_SECT_ENTRY(rx_starts)
+    STATS_SECT_ENTRY(rx_aborts)
+    STATS_SECT_ENTRY(rx_valid)
+    STATS_SECT_ENTRY(rx_crc_err)
+    STATS_SECT_ENTRY(rx_late)
+    STATS_SECT_ENTRY(radio_state_errs)
+    STATS_SECT_ENTRY(rx_hw_err)
+    STATS_SECT_ENTRY(tx_hw_err)
+STATS_SECT_END
+STATS_SECT_DECL(ble_phy_stats) ble_phy_stats;
+
+STATS_NAME_START(ble_phy_stats)
+    STATS_NAME(ble_phy_stats, phy_isrs)
+    STATS_NAME(ble_phy_stats, tx_good)
+    STATS_NAME(ble_phy_stats, tx_fail)
+    STATS_NAME(ble_phy_stats, tx_late)
+    STATS_NAME(ble_phy_stats, tx_bytes)
+    STATS_NAME(ble_phy_stats, rx_starts)
+    STATS_NAME(ble_phy_stats, rx_aborts)
+    STATS_NAME(ble_phy_stats, rx_valid)
+    STATS_NAME(ble_phy_stats, rx_crc_err)
+    STATS_NAME(ble_phy_stats, rx_late)
+    STATS_NAME(ble_phy_stats, radio_state_errs)
+    STATS_NAME(ble_phy_stats, rx_hw_err)
+    STATS_NAME(ble_phy_stats, tx_hw_err)
+STATS_NAME_END(ble_phy_stats)
+
+/*
+ * NOTE:
+ * Tested the following to see what would happen:
+ *  -> NVIC has radio irq enabled (interrupt # 1, mask 0x2).
+ *  -> Set up nrf to receive. Clear ADDRESS event register.
+ *  -> Enable ADDRESS interrupt on nrf5 by writing to INTENSET.
+ *  -> Enable RX.
+ *  -> Disable interrupts globally using OS_ENTER_CRITICAL().
+ *  -> Wait until a packet is received and the ADDRESS event occurs.
+ *  -> Call ble_phy_disable().
+ *
+ *  At this point I wanted to see the state of the cortex NVIC. The IRQ
+ *  pending bit was TRUE for the radio interrupt (as expected) as we never
+ *  serviced the radio interrupt (interrupts were disabled).
+ *
+ *  What was unexpected was this: without clearing the pending IRQ in the NVIC,
+ *  when radio interrupts were re-enabled (address event bit in INTENSET set to
+ *  1) and the radio ADDRESS event register read 1 (it was never cleared after
+ *  the first address event), the radio did not enter the ISR! I would have
+ *  expected that if the following were true, an interrupt would occur:
+ *      -> NVIC ISER bit set to TRUE
+ *      -> NVIC ISPR bit reads TRUE, meaning interrupt is pending.
+ *      -> Radio peripheral interrupts are enabled for some event (or events).
+ *      -> Corresponding event register(s) in radio peripheral read 1.
+ *
+ *  Not sure what the end result of all this is. We will clear the pending
+ *  bit in the NVIC just to be sure when we disable the PHY.
+ */
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/* Per nordic, the number of bytes needed for scratch is 16 + MAX_PKT_SIZE. */
+#define NRF_ENC_SCRATCH_WORDS   (((NRF_MAX_ENCRYPTED_PYLD_LEN + 16) + 3) / 4)
+
+uint32_t g_nrf_encrypt_scratchpad[NRF_ENC_SCRATCH_WORDS];
+
+struct nrf_ccm_data
+{
+    uint8_t key[16];
+    uint64_t pkt_counter;
+    uint8_t dir_bit;
+    uint8_t iv[8];
+} __attribute__((packed));
+
+struct nrf_ccm_data g_nrf_ccm_data;
+#endif
+
+/**
+ * Copies the data from the phy receive buffer into a mbuf chain.
+ *
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
+ *
+ */
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
+{
+    uint32_t rem_len;
+    uint32_t copy_len;
+    uint32_t block_len;
+    uint32_t block_rem_len;
+    void *dst;
+    void *src;
+    struct os_mbuf * om;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    block_len = rxpdu->om_omp->omp_databuf_len;
+    rem_len = OS_MBUF_PKTHDR(rxpdu)->omp_len;
+    src = dptr;
+
+    /*
+     * Setup for copying from first mbuf which is shorter due to packet header
+     * and extra leading space
+     */
+    copy_len = block_len - rxpdu->om_pkthdr_len - 4;
+    om = rxpdu;
+    dst = om->om_data;
+
+    while (true) {
+        /*
+         * Always copy blocks of length aligned to word size, only last mbuf
+         * will have remaining non-word size bytes appended.
+         */
+        block_rem_len = copy_len;
+        copy_len = min(copy_len, rem_len);
+        copy_len &= ~3;
+
+        dst = om->om_data;
+        om->om_len = copy_len;
+        rem_len -= copy_len;
+        block_rem_len -= copy_len;
+
+        __asm__ volatile (".syntax unified              \n"
+                          "   mov  r4, %[len]           \n"
+                          "   b    2f                   \n"
+                          "1: ldr  r3, [%[src], %[len]] \n"
+                          "   str  r3, [%[dst], %[len]] \n"
+                          "2: subs %[len], #4           \n"
+                          "   bpl  1b                   \n"
+                          "   adds %[src], %[src], r4   \n"
+                          "   adds %[dst], %[dst], r4   \n"
+                          : [dst] "+l" (dst), [src] "+l" (src),
+                            [len] "+l" (copy_len)
+                          :
+                          : "r3", "r4", "memory"
+                         );
+
+        if ((rem_len < 4) && (block_rem_len >= rem_len)) {
+            break;
+        }
+
+        /* Move to next mbuf */
+        om = SLIST_NEXT(om, om_next);
+        copy_len = block_len;
+    }
+
+    /* Copy remaining bytes, if any, to last mbuf */
+    om->om_len += rem_len;
+    __asm__ volatile (".syntax unified              \n"
+                      "   b    2f                   \n"
+                      "1: ldrb r3, [%[src], %[len]] \n"
+                      "   strb r3, [%[dst], %[len]] \n"
+                      "2: subs %[len], #1           \n"
+                      "   bpl  1b                   \n"
+                      : [len] "+l" (rem_len)
+                      : [dst] "l" (dst), [src] "l" (src)
+                      : "r3", "memory"
+                     );
+
+    /* Copy header */
+    memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
+           sizeof(struct ble_mbuf_hdr));
+}
+
+/**
+ * Called when we want to wait if the radio is in either the rx or tx
+ * disable states. We want to wait until that state is over before doing
+ * anything to the radio
+ */
+static void
+nrf_wait_disabled(void)
+{
+    uint32_t state;
+
+    /*
+     * RX and TX states have the same values except for 3rd bit (0=RX, 1=TX) so
+     * we use RX symbols only.
+     */
+    state = NRF_RADIO->STATE & 0x07;
+
+    if (state != RADIO_STATE_STATE_Disabled) {
+        /* If PHY is in idle state for whatever reason, disable it now */
+        if (state == RADIO_STATE_STATE_RxIdle) {
+            NRF_RADIO->TASKS_DISABLE = 1;
+            STATS_INC(ble_phy_stats, radio_state_errs);
+        }
+
+        if (state == RADIO_STATE_STATE_RxDisable) {
+            /* This will end within a short time (6 usecs). Just poll */
+            while (NRF_RADIO->STATE == state) {
+                /* If this fails, something is really wrong. Should last
+                 * no more than 6 usecs */
+            }
+        }
+    }
+}
+
+/**
+ *
+ *
+ */
+int
+ble_phy_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    uint32_t next_cc;
+    uint32_t cur_cc;
+    uint32_t cntr;
+    uint32_t delta;
+
+    /*
+     * XXX: The TXEN time is 140 usecs but there may be additional delays
+     * Need to look at this.
+     */
+
+    /*
+     * With the 32.768 kHz crystal, we may need to adjust the RTC compare
+     * value by 1 tick due to the time it takes for TXEN. The code uses a 5 RTC
+     * tick offset, which is 152.5 usecs. The TXEN time is 140 usecs. This
+     * means that with a remainder of 0, TIMER0 should be set to 12 or 13 (as
+     * TIMER0 counts at 1MHz). A remainder of 19 or more we will need to add
+     * 1 tick. We dont need to add 1 tick per se, but it does give us slightly
+     * more time and thus less of a chance to miss a tick. Another note: we
+     * cant set TIMER0 CC to 0 as the compare wont occur; it must be 1 or more.
+     * This is why we subtract 18 (as opposed to 19) as rem_uses will be >= 1.
+     */
+    if (rem_usecs <= 18) {
+        cputime -= 5;
+        rem_usecs += 12;
+    } else {
+        cputime -= 4;
+        rem_usecs -= 18;
+    }
+
+    /*
+     * Can we set the RTC compare to start TIMER0? We can do it if:
+     *      a) Current compare value is not N+1 or N+2 ticks from current
+     *      counter.
+     *      b) The value we want to set is not at least N+2 from current
+     *      counter.
+     *
+     * NOTE: since the counter can tick 1 while we do these calculations we
+     * need to account for it.
+     */
+    next_cc = cputime & 0xffffff;
+    cur_cc = NRF_RTC0->CC[0];
+    cntr = NRF_RTC0->COUNTER;
+
+    delta = (cur_cc - cntr) & 0xffffff;
+    if ((delta <= 3) && (delta != 0)) {
+        return -1;
+    }
+    delta = (next_cc - cntr) & 0xffffff;
+    if ((delta & 0x800000) || (delta < 3)) {
+        return -1;
+    }
+
+    /* Clear and set TIMER0 to fire off at proper time */
+    NRF_TIMER0->TASKS_CLEAR = 1;
+    NRF_TIMER0->CC[0] = rem_usecs;
+    NRF_TIMER0->EVENTS_COMPARE[0] = 0;
+
+    /* Set RTC compare to start TIMER0 */
+    NRF_RTC0->EVENTS_COMPARE[0] = 0;
+    NRF_RTC0->CC[0] = next_cc;
+    NRF_RTC0->EVTENSET = RTC_EVTENSET_COMPARE0_Msk;
+
+    /* Enable PPI */
+    NRF_PPI->CHENSET = PPI_CHEN_CH31_Msk;
+
+    /* Store the cputime at which we set the RTC */
+    g_ble_phy_data.phy_start_cputime = cputime;
+
+    return 0;
+}
+
+/**
+ * Function is used to set PPI so that we can time out waiting for a reception
+ * to occur. This happens for two reasons: we have sent a packet and we are
+ * waiting for a respons (txrx should be set to ENABLE_TXRX) or we are
+ * starting a connection event and we are a slave and we are waiting for the
+ * master to send us a packet (txrx should be set to ENABLE_RX).
+ *
+ * NOTE: when waiting for a txrx turn-around, wfr_usecs is not used as there
+ * is no additional time to wait; we know when we should receive the address of
+ * the received frame.
+ *
+ * @param txrx Flag denoting if this wfr is a txrx turn-around or not.
+ * @param tx_phy_mode phy mode for last TX (not used on nRF51)
+ * @param wfr_usecs Amount of usecs to wait.
+ */
+void
+ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs)
+{
+    uint32_t end_time;
+
+    if (txrx == BLE_PHY_WFR_ENABLE_TXRX) {
+        /*
+         * Timeout occurs an IFS time plus time it takes to receive address
+         * from the transmit end. We add additional time to make sure the
+         * address event comes before the compare. Note that transmit end
+         * is captured in CC[2]. I just made up the 16 usecs I add here.
+         */
+        end_time = NRF_TIMER0->CC[2] + BLE_LL_IFS +
+            ble_phy_mode_pdu_start_off(BLE_PHY_MODE_1M) + 16;
+    } else {
+        /* CC[0] is set to when RXEN occurs. */
+        end_time = NRF_TIMER0->CC[0] + XCVR_RX_START_DELAY_USECS + wfr_usecs +
+            ble_phy_mode_pdu_start_off(BLE_PHY_MODE_1M) + BLE_LL_JITTER_USECS;
+    }
+
+    /* wfr_secs is the time from rxen until timeout */
+    NRF_TIMER0->CC[3] = end_time;
+    NRF_TIMER0->EVENTS_COMPARE[3] = 0;
+
+    /* Enable wait for response PPI */
+    NRF_PPI->CHENSET = (PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk);
+
+    /* Enable the disabled interrupt so we time out on events compare */
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+}
+
+/**
+ * Setup transceiver for receive.
+ */
+static void
+ble_phy_rx_xcvr_setup(void)
+{
+    uint8_t *dptr;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        dptr += 3;
+        NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM->OUTPTR = (uint32_t)dptr;
+        NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+        NRF_CCM->MODE = CCM_MODE_MODE_Decryption;
+        NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+        NRF_CCM->SHORTS = 0;
+        NRF_CCM->EVENTS_ERROR = 0;
+        NRF_CCM->EVENTS_ENDCRYPT = 0;
+        NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk;
+    } else {
+        NRF_RADIO->PACKETPTR = (uint32_t)dptr;
+    }
+#else
+    NRF_RADIO->PACKETPTR = (uint32_t)dptr;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (g_ble_phy_data.phy_privacy) {
+        dptr += 3;
+        NRF_RADIO->PACKETPTR = (uint32_t)dptr;
+        NRF_RADIO->PCNF0 = (6 << RADIO_PCNF0_LFLEN_Pos) |
+                           (2 << RADIO_PCNF0_S1LEN_Pos) |
+                           (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+        NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Enabled;
+        NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+        NRF_AAR->SCRATCHPTR = (uint32_t)&g_ble_phy_data.phy_aar_scratch;
+        NRF_AAR->EVENTS_END = 0;
+        NRF_AAR->EVENTS_RESOLVED = 0;
+        NRF_AAR->EVENTS_NOTRESOLVED = 0;
+    } else {
+        if (g_ble_phy_data.phy_encrypted == 0) {
+            NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
+                               (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+            /* XXX: do I only need to do this once? Figure out what I can do
+               once. */
+            NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Disabled;
+        }
+    }
+#endif
+
+    /* Turn off trigger TXEN on output compare match and AAR on bcmatch */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk | PPI_CHEN_CH23_Msk;
+
+    /* Reset the rx started flag. Used for the wait for response */
+    g_ble_phy_data.phy_rx_started = 0;
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+    g_ble_phy_data.rxdptr = dptr;
+
+    /* I want to know when 1st byte received (after address) */
+    NRF_RADIO->BCC = 8; /* in bits */
+    NRF_RADIO->EVENTS_ADDRESS = 0;
+    NRF_RADIO->EVENTS_DEVMATCH = 0;
+    NRF_RADIO->EVENTS_BCMATCH = 0;
+    NRF_RADIO->EVENTS_RSSIEND = 0;
+    NRF_RADIO->SHORTS = RADIO_SHORTS_END_DISABLE_Msk |
+                        RADIO_SHORTS_READY_START_Msk |
+                        RADIO_SHORTS_DISABLED_TXEN_Msk |
+                        RADIO_SHORTS_ADDRESS_BCSTART_Msk |
+                        RADIO_SHORTS_ADDRESS_RSSISTART_Msk |
+                        RADIO_SHORTS_DISABLED_RSSISTOP_Msk;
+
+    NRF_RADIO->INTENSET = RADIO_INTENSET_ADDRESS_Msk;
+}
+
+/**
+ * Called from interrupt context when the transmit ends
+ *
+ */
+static void
+ble_phy_tx_end_isr(void)
+{
+    uint8_t was_encrypted;
+    uint8_t transition;
+    uint8_t txlen;
+    uint32_t wfr_time;
+
+    /* If this transmission was encrypted we need to remember it */
+    was_encrypted = g_ble_phy_data.phy_encrypted;
+    (void)was_encrypted;
+
+    /* Better be in TX state! */
+    assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
+
+    /* Clear events and clear interrupt on disabled event */
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_DISABLED_Msk;
+    NRF_RADIO->EVENTS_END = 0;
+    wfr_time = NRF_RADIO->SHORTS;
+    (void)wfr_time;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    /*
+     * XXX: not sure what to do. We had a HW error during transmission.
+     * For now I just count a stat but continue on like all is good.
+     */
+    if (was_encrypted) {
+        if (NRF_CCM->EVENTS_ERROR) {
+            STATS_INC(ble_phy_stats, tx_hw_err);
+            NRF_CCM->EVENTS_ERROR = 0;
+        }
+    }
+#endif
+
+    /* Call transmit end callback */
+    if (g_ble_phy_data.txend_cb) {
+        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
+    }
+
+    transition = g_ble_phy_data.phy_transition;
+    if (transition == BLE_PHY_TRANSITION_TX_RX) {
+        /* Packet pointer needs to be reset. */
+        ble_phy_rx_xcvr_setup();
+
+        /*
+         * Enable the wait for response timer. Note that cc #1 on
+         * timer 0 contains the transmit start time
+         */
+        txlen = g_ble_phy_data.phy_tx_pyld_len;
+        if (txlen && was_encrypted) {
+            txlen += BLE_LL_DATA_MIC_LEN;
+        }
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_TXRX, 0, 0);
+    } else {
+        /*
+         * XXX: not sure we need to stop the timer here all the time. Or that
+         * it should be stopped here.
+         */
+        NRF_TIMER0->TASKS_STOP = 1;
+        NRF_TIMER0->TASKS_SHUTDOWN = 1;
+        NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk |
+                           PPI_CHEN_CH20_Msk | PPI_CHEN_CH31_Msk;
+        assert(transition == BLE_PHY_TRANSITION_NONE);
+    }
+}
+
+static void
+ble_phy_rx_end_isr(void)
+{
+    int rc;
+    uint8_t *dptr;
+    uint8_t crcok;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* Clear events and clear interrupt */
+    NRF_RADIO->EVENTS_END = 0;
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_END_Msk;
+
+    /* Disable automatic RXEN */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
+
+    /* Set RSSI and CRC status flag in header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    assert(NRF_RADIO->EVENTS_RSSIEND != 0);
+    ble_hdr->rxinfo.rssi = (-1 * NRF_RADIO->RSSISAMPLE) +
+                            g_ble_phy_data.rx_pwr_compensation;
+
+    dptr = g_ble_phy_data.rxdptr;
+
+    /* Count PHY crc errors and valid packets */
+    crcok = (uint8_t)NRF_RADIO->CRCSTATUS;
+    if (!crcok) {
+        STATS_INC(ble_phy_stats, rx_crc_err);
+    } else {
+        STATS_INC(ble_phy_stats, rx_valid);
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (g_ble_phy_data.phy_encrypted) {
+            /* Only set MIC failure flag if frame is not zero length */
+            if ((dptr[1] != 0) && (NRF_CCM->MICSTATUS == 0)) {
+                ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_MIC_FAILURE;
+            }
+
+            /*
+             * XXX: not sure how to deal with this. This should not
+             * be a MIC failure but we should not hand it up. I guess
+             * this is just some form of rx error and that is how we
+             * handle it? For now, just set CRC error flags
+             */
+            if (NRF_CCM->EVENTS_ERROR) {
+                STATS_INC(ble_phy_stats, rx_hw_err);
+                ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+            }
+
+            /*
+             * XXX: This is a total hack work-around for now but I dont
+             * know what else to do. If ENDCRYPT is not set and we are
+             * encrypted we need to not trust this frame and drop it.
+             */
+            if (NRF_CCM->EVENTS_ENDCRYPT == 0) {
+                STATS_INC(ble_phy_stats, rx_hw_err);
+                ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+            }
+        }
+#endif
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (g_ble_phy_data.phy_encrypted || g_ble_phy_data.phy_privacy) {
+        /*
+         * XXX: This is a horrible ugly hack to deal with the RAM S1 byte.
+         * This should get fixed as we should not be handing up the header
+         * and length as part of the pdu.
+         */
+        dptr[2] = dptr[1];
+        dptr[1] = dptr[0];
+        ++dptr;
+    }
+#endif
+    rc = ble_ll_rx_end(dptr, ble_hdr);
+    if (rc < 0) {
+        ble_phy_disable();
+    }
+}
+
+static void
+ble_phy_rx_start_isr(void)
+{
+    int rc;
+    uint32_t state;
+    uint32_t usecs;
+    uint32_t ticks;
+    struct ble_mbuf_hdr *ble_hdr;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    uint8_t *dptr;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+#endif
+
+    /* Clear events and clear interrupt */
+    NRF_RADIO->EVENTS_ADDRESS = 0;
+
+    /* Clear wfr timer channels and DISABLED interrupt */
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_DISABLED_Msk | RADIO_INTENCLR_ADDRESS_Msk;
+    NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk;
+
+    /* Initialize flags, channel and state in ble header at rx start */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    ble_hdr->rxinfo.flags = ble_ll_state_get();
+    ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
+    ble_hdr->rxinfo.handle = 0;
+    ble_hdr->rxinfo.phy = BLE_PHY_1M;
+    ble_hdr->rxinfo.phy_mode = BLE_PHY_MODE_1M;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_hdr->rxinfo.user_data = NULL;
+#endif
+
+    /*
+     * Calculate receive start time.
+     *
+     * XXX: possibly use other routine with remainder!
+     */
+    usecs = NRF_TIMER0->CC[1] - ble_phy_mode_pdu_start_off(BLE_PHY_MODE_1M);
+    ticks = os_cputime_usecs_to_ticks(usecs);
+    ble_hdr->rem_usecs = usecs - os_cputime_ticks_to_usecs(ticks);
+    if (ble_hdr->rem_usecs == 31) {
+        ble_hdr->rem_usecs = 0;
+        ++ticks;
+    }
+    ble_hdr->beg_cputime = g_ble_phy_data.phy_start_cputime + ticks;
+
+    /* Wait to get 1st byte of frame */
+    while (1) {
+        state = NRF_RADIO->STATE;
+        if (NRF_RADIO->EVENTS_BCMATCH != 0) {
+            break;
+        }
+
+        /*
+         * If state is disabled, we should have the BCMATCH. If not,
+         * something is wrong!
+         */
+        if (state == RADIO_STATE_STATE_Disabled) {
+            NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+            NRF_RADIO->SHORTS = 0;
+            return;
+        }
+    }
+
+    /* Call Link Layer receive start function */
+    rc = ble_ll_rx_start(g_ble_phy_data.rxdptr, g_ble_phy_data.phy_chan,
+                         &g_ble_phy_data.rxhdr);
+    if (rc >= 0) {
+        /* Set rx started flag and enable rx end ISR */
+        g_ble_phy_data.phy_rx_started = 1;
+        NRF_RADIO->INTENSET = RADIO_INTENSET_END_Msk;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        /* Must start aar if we need to  */
+        if (g_ble_phy_data.phy_privacy) {
+            NRF_RADIO->EVENTS_BCMATCH = 0;
+            NRF_PPI->CHENSET = PPI_CHEN_CH23_Msk;
+            /*
+             * Setup AAR to resolve AdvA and trigger it after complete address
+             * is received, i.e. after PDU header and AdvA is received.
+             *
+             * AdvA starts at 4th octet in receive buffer, after S0, len and S1
+             * fields.
+             *
+             * In case of extended advertising AdvA is located after extended
+             * header (+2 octets).
+             */
+            if (BLE_MBUF_HDR_EXT_ADV(&g_ble_phy_data.rxhdr)) {
+                NRF_AAR->ADDRPTR = (uint32_t)(dptr + 5);
+                NRF_RADIO->BCC = (BLE_DEV_ADDR_LEN + BLE_LL_PDU_HDR_LEN + 2) * 8;
+
+            } else {
+                NRF_AAR->ADDRPTR = (uint32_t)(dptr + 3);
+                NRF_RADIO->BCC = (BLE_DEV_ADDR_LEN + BLE_LL_PDU_HDR_LEN) * 8;
+            }
+        }
+#endif
+    } else {
+        /* Disable PHY */
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, rx_aborts);
+    }
+
+    /* Count rx starts */
+    STATS_INC(ble_phy_stats, rx_starts);
+}
+
+static void
+ble_phy_isr(void)
+{
+    uint32_t irq_en;
+
+    os_trace_isr_enter();
+
+    /* Read irq register to determine which interrupts are enabled */
+    irq_en = NRF_RADIO->INTENCLR;
+
+    /*
+     * NOTE: order of checking is important! Possible, if things get delayed,
+     * we have both an ADDRESS and DISABLED interrupt in rx state. If we get
+     * an address, we disable the DISABLED interrupt.
+     */
+
+    /* We get this if we have started to receive a frame */
+    if ((irq_en & RADIO_INTENCLR_ADDRESS_Msk) && NRF_RADIO->EVENTS_ADDRESS) {
+        irq_en &= ~RADIO_INTENCLR_DISABLED_Msk;
+        ble_phy_rx_start_isr();
+    }
+
+    /* Check for disabled event. This only happens for transmits now */
+    if ((irq_en & RADIO_INTENCLR_DISABLED_Msk) && NRF_RADIO->EVENTS_DISABLED) {
+        if (g_ble_phy_data.phy_state == BLE_PHY_STATE_RX) {
+            NRF_RADIO->EVENTS_DISABLED = 0;
+            ble_ll_wfr_timer_exp(NULL);
+        } else {
+            ble_phy_tx_end_isr();
+        }
+    }
+
+    /* Receive packet end (we dont enable this for transmit) */
+    if ((irq_en & RADIO_INTENCLR_END_Msk) && NRF_RADIO->EVENTS_END) {
+        ble_phy_rx_end_isr();
+    }
+
+    /* Ensures IRQ is cleared */
+    irq_en = NRF_RADIO->SHORTS;
+
+    /* Count # of interrupts */
+    STATS_INC(ble_phy_stats, phy_isrs);
+
+    os_trace_isr_exit();
+}
+
+/**
+ * ble phy init
+ *
+ * Initialize the PHY.
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+int
+ble_phy_init(void)
+{
+    int rc;
+
+    /* Set phy channel to an invalid channel so first set channel works */
+    g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
+
+    g_ble_phy_data.rx_pwr_compensation = 0;
+
+    /* Toggle peripheral power to reset (just in case) */
+    NRF_RADIO->POWER = 0;
+    NRF_RADIO->POWER = 1;
+
+    /* Disable all interrupts */
+    NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+
+    /* Set configuration registers */
+    NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_1Mbit;
+    NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
+                       (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+    /* XXX: should maxlen be 251 for encryption? */
+    NRF_RADIO->PCNF1 = NRF_MAXLEN |
+                       (RADIO_PCNF1_ENDIAN_Little <<  RADIO_PCNF1_ENDIAN_Pos) |
+                       (NRF_BALEN << RADIO_PCNF1_BALEN_Pos) |
+                       RADIO_PCNF1_WHITEEN_Msk;
+
+    /* Set base0 with the advertising access address */
+    NRF_RADIO->BASE0 = (BLE_ACCESS_ADDR_ADV << 8) & 0xFFFFFF00;
+    NRF_RADIO->PREFIX0 = (BLE_ACCESS_ADDR_ADV >> 24) & 0xFF;
+
+    /* Configure the CRC registers */
+    NRF_RADIO->CRCCNF = RADIO_CRCCNF_SKIPADDR_Msk | RADIO_CRCCNF_LEN_Three;
+
+    /* Configure BLE poly */
+    NRF_RADIO->CRCPOLY = 0x0100065B;
+
+    /* Configure IFS */
+    NRF_RADIO->TIFS = BLE_LL_IFS;
+
+    /* Captures tx/rx start in timer0 cc 1 and tx/rx end in timer0 cc 2 */
+    NRF_PPI->CHENSET = PPI_CHEN_CH26_Msk | PPI_CHEN_CH27_Msk;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    NRF_CCM->INTENCLR = 0xffffffff;
+    NRF_CCM->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
+    NRF_CCM->EVENTS_ERROR = 0;
+    memset(g_nrf_encrypt_scratchpad, 0, sizeof(g_nrf_encrypt_scratchpad));
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    g_ble_phy_data.phy_aar_scratch = 0;
+    NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+    NRF_AAR->INTENCLR = 0xffffffff;
+    NRF_AAR->EVENTS_END = 0;
+    NRF_AAR->EVENTS_RESOLVED = 0;
+    NRF_AAR->EVENTS_NOTRESOLVED = 0;
+    NRF_AAR->NIRK = 0;
+#endif
+
+    /* TIMER0 setup for PHY when using RTC */
+    NRF_TIMER0->TASKS_STOP = 1;
+    NRF_TIMER0->TASKS_SHUTDOWN = 1;
+    NRF_TIMER0->BITMODE = 3;    /* 32-bit timer */
+    NRF_TIMER0->MODE = 0;       /* Timer mode */
+    NRF_TIMER0->PRESCALER = 4;  /* gives us 1 MHz */
+
+    /*
+     * PPI setup.
+     * Channel 4: Captures TIMER0 in CC[3] when EVENTS_ADDRESS occurs. Used
+     *            to cancel the wait for response timer.
+     * Channel 5: TIMER0 CC[3] to TASKS_DISABLE on radio. This is the wait
+     *            for response timer.
+     */
+    NRF_PPI->CH[4].EEP = (uint32_t)&(NRF_RADIO->EVENTS_ADDRESS);
+    NRF_PPI->CH[4].TEP = (uint32_t)&(NRF_TIMER0->TASKS_CAPTURE[3]);
+    NRF_PPI->CH[5].EEP = (uint32_t)&(NRF_TIMER0->EVENTS_COMPARE[3]);
+    NRF_PPI->CH[5].TEP = (uint32_t)&(NRF_RADIO->TASKS_DISABLE);
+
+    /* Set isr in vector table and enable interrupt */
+#ifndef RIOT_VERSION
+    NVIC_SetPriority(RADIO_IRQn, 0);
+#endif
+#if MYNEWT
+    NVIC_SetVector(RADIO_IRQn, (uint32_t)ble_phy_isr);
+#else
+    ble_npl_hw_set_isr(RADIO_IRQn, ble_phy_isr);
+#endif
+    NVIC_EnableIRQ(RADIO_IRQn);
+
+    /* Register phy statistics */
+    if (!g_ble_phy_data.phy_stats_initialized) {
+        rc = stats_init_and_reg(STATS_HDR(ble_phy_stats),
+                                STATS_SIZE_INIT_PARMS(ble_phy_stats,
+                                                      STATS_SIZE_32),
+                                STATS_NAME_INIT_PARMS(ble_phy_stats),
+                                "ble_phy");
+        assert(rc == 0);
+
+        g_ble_phy_data.phy_stats_initialized  = 1;
+    }
+
+    return 0;
+}
+
+/**
+ * Puts the phy into receive mode.
+ *
+ * @return int 0: success; BLE Phy error code otherwise
+ */
+int
+ble_phy_rx(void)
+{
+    /* Check radio state */
+    nrf_wait_disabled();
+    if (NRF_RADIO->STATE != RADIO_STATE_STATE_Disabled) {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, radio_state_errs);
+        return BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    /* Make sure all interrupts are disabled */
+    NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+
+    /* Clear events prior to enabling receive */
+    NRF_RADIO->EVENTS_END = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+
+    /* Setup for rx */
+    ble_phy_rx_xcvr_setup();
+
+    /* Start the receive task in the radio if not automatically going to rx */
+    if ((NRF_PPI->CHEN & PPI_CHEN_CH21_Msk) == 0) {
+        NRF_RADIO->TASKS_RXEN = 1;
+    }
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * Called to enable encryption at the PHY. Note that this state will persist
+ * in the PHY; in other words, if you call this function you have to call
+ * disable so that future PHY transmits/receives will not be encrypted.
+ *
+ * @param pkt_counter
+ * @param iv
+ * @param key
+ * @param is_master
+ */
+void
+ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+                       uint8_t is_master)
+{
+    memcpy(g_nrf_ccm_data.key, key, 16);
+    g_nrf_ccm_data.pkt_counter = pkt_counter;
+    memcpy(g_nrf_ccm_data.iv, iv, 8);
+    g_nrf_ccm_data.dir_bit = is_master;
+    g_ble_phy_data.phy_encrypted = 1;
+
+    /* Encryption uses LFLEN=5, S1LEN = 3. */
+    NRF_RADIO->PCNF0 = (5 << RADIO_PCNF0_LFLEN_Pos) |
+                       (3 << RADIO_PCNF0_S1LEN_Pos) |
+                       (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+
+    /* Enable the module (AAR cannot be on while CCM on) */
+    NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Disabled;
+    NRF_CCM->ENABLE = CCM_ENABLE_ENABLE_Enabled;
+}
+
+void
+ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir)
+{
+    g_nrf_ccm_data.pkt_counter = pkt_counter;
+    g_nrf_ccm_data.dir_bit = dir;
+}
+
+void
+ble_phy_encrypt_disable(void)
+{
+    NRF_PPI->CHENCLR = (PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk);
+    NRF_CCM->TASKS_STOP = 1;
+    NRF_CCM->EVENTS_ERROR = 0;
+    NRF_CCM->ENABLE = CCM_ENABLE_ENABLE_Disabled;
+
+    /* Switch back to normal length */
+    NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
+                       (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+
+    g_ble_phy_data.phy_encrypted = 0;
+}
+#endif
+
+void
+ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg)
+{
+    /* Set transmit end callback and arg */
+    g_ble_phy_data.txend_cb = txend_cb;
+    g_ble_phy_data.txend_arg = arg;
+}
+
+/**
+ * Called to set the start time of a transmission.
+ *
+ * This function is called to set the start time when we are not going from
+ * rx to tx automatically.
+ *
+ * NOTE: care must be taken when calling this function. The channel should
+ * already be set.
+ *
+ * @param cputime   This is the tick at which the 1st bit of the preamble
+ *                  should be transmitted
+ * @param rem_usecs This is used only when the underlying timing uses a 32.768
+ *                  kHz crystal. It is the # of usecs from the cputime tick
+ *                  at which the first bit of the preamble should be
+ *                  transmitted.
+ * @return int
+ */
+int
+ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    int rc;
+
+    ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_TX, cputime, rem_usecs);
+
+    /* XXX: This should not be necessary, but paranoia is good! */
+    /* Clear timer0 compare to RXEN since we are transmitting */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
+
+    /*
+     * XXX: The TXEN time is 140 usecs but there may be additional delays
+     * Need to look at this.
+     */
+    if (ble_phy_set_start_time(cputime, rem_usecs) != 0) {
+        STATS_INC(ble_phy_stats, tx_late);
+        ble_phy_disable();
+        rc = BLE_PHY_ERR_TX_LATE;
+    } else {
+        /* Enable PPI to automatically start TXEN */
+        NRF_PPI->CHENSET = PPI_CHEN_CH20_Msk;
+        rc = 0;
+    }
+    return rc;
+}
+/**
+ * Called to set the start time of a reception
+ *
+ * This function acts a bit differently than transmit. If we are late getting
+ * here we will still attempt to receive.
+ *
+ * NOTE: care must be taken when calling this function. The channel should
+ * already be set.
+ *
+ * @param cputime
+ *
+ * @return int
+ */
+int
+ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    int rc;
+
+    ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_RX, cputime, rem_usecs);
+
+    /* XXX: This should not be necessary, but paranoia is good! */
+    /* Clear timer0 compare to TXEN since we are transmitting */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk;
+
+    /*
+     * XXX: The RXEN time is 138 usecs but there may be additional delays
+     * Need to look at this.
+     */
+    if (ble_phy_set_start_time(cputime, rem_usecs) != 0) {
+        STATS_INC(ble_phy_stats, rx_late);
+        NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
+        NRF_RADIO->TASKS_RXEN = 1;
+        rc = BLE_PHY_ERR_RX_LATE;
+    } else {
+        /* Enable PPI to automatically start RXEN */
+        NRF_PPI->CHENSET = PPI_CHEN_CH21_Msk;
+
+        /* Start rx */
+        rc = ble_phy_rx();
+    }
+    return rc;
+}
+
+int
+ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
+{
+    int rc;
+    uint8_t *dptr;
+    uint8_t payload_len;
+    uint8_t payload_off;
+    uint8_t hdr_byte;
+    uint32_t state;
+    uint32_t shortcuts;
+
+    /*
+     * This check is to make sure that the radio is not in a state where
+     * it is moving to disabled state. If so, let it get there.
+     */
+    nrf_wait_disabled();
+
+    /*
+     * XXX: Although we may not have to do this here, I clear all the PPI
+     * that should not be used when transmitting. Some of them are only enabled
+     * if encryption and/or privacy is on, but I dont care. Better to be
+     * paranoid, and if you are going to clear one, might as well clear them
+     * all.
+     */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH23_Msk |
+                       PPI_CHEN_CH25_Msk;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
+        dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
+        payload_off = 3;
+
+        NRF_CCM->SHORTS = 1;
+        NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM->OUTPTR = (uint32_t)&g_ble_phy_tx_buf[0];
+        NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+        NRF_CCM->EVENTS_ERROR = 0;
+        NRF_CCM->MODE = CCM_MODE_MODE_Encryption;
+        NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+        NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk;
+    } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        /* Reconfigure PCNF0 */
+        NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
+                           (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+        NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+#endif
+        /* RAM representation has S0 and LENGTH fields (2 bytes) */
+        dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+        payload_off = 2;
+    }
+#else
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* Reconfigure PCNF0 */
+    NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
+                       (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
+#endif
+
+    /* RAM representation has S0 and LENGTH fields (2 bytes) */
+    dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+    payload_off = 2;
+#endif
+
+    NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_tx_buf[0];
+
+    /* Clear the ready, end and disabled events */
+    NRF_RADIO->EVENTS_READY = 0;
+    NRF_RADIO->EVENTS_END = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+
+    /* Enable shortcuts for transmit start/end. */
+    shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
+    if (end_trans == BLE_PHY_TRANSITION_TX_RX) {
+        shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
+    }
+    NRF_RADIO->SHORTS = shortcuts;
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    /* Set PDU payload */
+    payload_len = pducb(&dptr[payload_off], pducb_arg, &hdr_byte);
+
+    /* Set PDU header */
+    dptr[0] = hdr_byte;
+    dptr[1] = payload_len;
+    if (payload_off > 2) {
+        dptr[2] = 0;
+    }
+
+    /* Set the PHY transition */
+    g_ble_phy_data.phy_transition = end_trans;
+
+    /* Set transmitted payload length */
+    g_ble_phy_data.phy_tx_pyld_len = payload_len;
+
+    /* If we already started transmitting, abort it! */
+    state = NRF_RADIO->STATE;
+    if (state != RADIO_STATE_STATE_Tx) {
+
+        /* Set phy state to transmitting and count packet statistics */
+        g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
+        STATS_INC(ble_phy_stats, tx_good);
+        STATS_INCN(ble_phy_stats, tx_bytes, payload_len + BLE_LL_PDU_HDR_LEN);
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, tx_late);
+        rc = BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    return rc;
+}
+
+/**
+ * ble phy txpwr set
+ *
+ * Set the transmit output power (in dBm).
+ *
+ * NOTE: If the output power specified is within the BLE limits but outside
+ * the chip limits, we "rail" the power level so we dont exceed the min/max
+ * chip values.
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int 0: success; anything else is an error
+ */
+int
+ble_phy_txpwr_set(int dbm)
+{
+    /* Check valid range */
+    assert(dbm <= BLE_PHY_MAX_PWR_DBM);
+
+    /* "Rail" power level if outside supported range */
+    if (dbm > NRF_TX_PWR_MAX_DBM) {
+        dbm = NRF_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < NRF_TX_PWR_MIN_DBM) {
+            dbm = NRF_TX_PWR_MIN_DBM;
+        }
+    }
+
+    NRF_RADIO->TXPOWER = dbm;
+    g_ble_phy_data.phy_txpwr_dbm = dbm;
+
+    return 0;
+}
+
+/**
+ * ble phy txpwr round
+ *
+ * Get the rounded transmit output power (in dBm).
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int Rounded power in dBm
+ */
+int ble_phy_txpower_round(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    if (dbm > NRF_TX_PWR_MAX_DBM) {
+        dbm = NRF_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < NRF_TX_PWR_MIN_DBM) {
+            dbm = NRF_TX_PWR_MIN_DBM;
+        }
+    }
+
+    return dbm;
+}
+
+/**
+ * ble phy txpwr get
+ *
+ * Get the transmit power.
+ *
+ * @return int  The current PHY transmit power, in dBm
+ */
+int
+ble_phy_txpwr_get(void)
+{
+    return g_ble_phy_data.phy_txpwr_dbm;
+}
+
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
+/**
+ * ble phy setchan
+ *
+ * Sets the logical frequency of the transceiver. The input parameter is the
+ * BLE channel index (0 to 39, inclusive). The NRF frequency register works like
+ * this: logical frequency = 2400 + FREQ (MHz).
+ *
+ * Thus, to get a logical frequency of 2402 MHz, you would program the
+ * FREQUENCY register to 2.
+ *
+ * @param chan This is the Data Channel Index or Advertising Channel index
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+int
+ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
+{
+    uint32_t prefix;
+
+    assert(chan < BLE_PHY_NUM_CHANS);
+
+    /* Check for valid channel range */
+    if (chan >= BLE_PHY_NUM_CHANS) {
+        return BLE_PHY_ERR_INV_PARAM;
+    }
+
+    /* Get correct frequency */
+    if (chan < BLE_PHY_NUM_DATA_CHANS) {
+        /* Set current access address */
+        g_ble_phy_data.phy_access_address = access_addr;
+
+        /* Configure logical address 1 and crcinit */
+        prefix = NRF_RADIO->PREFIX0;
+        prefix &= 0xffff00ff;
+        prefix |= ((access_addr >> 24) & 0xFF) << 8;
+        NRF_RADIO->BASE1 = (access_addr << 8) & 0xFFFFFF00;
+        NRF_RADIO->PREFIX0 = prefix;
+        NRF_RADIO->TXADDRESS = 1;
+        NRF_RADIO->RXADDRESSES = (1 << 1);
+        NRF_RADIO->CRCINIT = crcinit;
+    } else {
+        /* Logical adddress 0 preconfigured */
+        NRF_RADIO->TXADDRESS = 0;
+        NRF_RADIO->RXADDRESSES = (1 << 0);
+        NRF_RADIO->CRCINIT = BLE_LL_CRCINIT_ADV;
+
+        /* Set current access address */
+        g_ble_phy_data.phy_access_address = BLE_ACCESS_ADDR_ADV;
+    }
+
+    /* Set the frequency and the data whitening initial value */
+    g_ble_phy_data.phy_chan = chan;
+    NRF_RADIO->FREQUENCY = g_ble_phy_chan_freq[chan];
+    NRF_RADIO->DATAWHITEIV = chan;
+
+    return 0;
+}
+
+/**
+ * Stop the timer used to count microseconds when using RTC for cputime
+ */
+static void
+ble_phy_stop_usec_timer(void)
+{
+    NRF_TIMER0->TASKS_STOP = 1;
+    NRF_TIMER0->TASKS_SHUTDOWN = 1;
+    NRF_RTC0->EVTENCLR = RTC_EVTENSET_COMPARE0_Msk;
+}
+
+/**
+ * ble phy disable irq and ppi
+ *
+ * This routine is to be called when reception was stopped due to either a
+ * wait for response timeout or a packet being received and the phy is to be
+ * restarted in receive mode. Generally, the disable routine is called to stop
+ * the phy.
+ */
+static void
+ble_phy_disable_irq_and_ppi(void)
+{
+    NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+    NRF_RADIO->SHORTS = 0;
+    NRF_RADIO->TASKS_DISABLE = 1;
+    NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH20_Msk |
+          PPI_CHEN_CH21_Msk | PPI_CHEN_CH23_Msk | PPI_CHEN_CH24_Msk |
+          PPI_CHEN_CH25_Msk | PPI_CHEN_CH31_Msk;
+    NVIC_ClearPendingIRQ(RADIO_IRQn);
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+}
+
+void
+ble_phy_restart_rx(void)
+{
+    ble_phy_stop_usec_timer();
+    ble_phy_disable_irq_and_ppi();
+    ble_phy_rx();
+}
+
+/**
+ * ble phy disable
+ *
+ * Disables the PHY. This should be called when an event is over. It stops
+ * the usec timer (if used), disables interrupts, disables the RADIO, disables
+ * PPI and sets state to idle.
+ */
+void
+ble_phy_disable(void)
+{
+    ble_phy_trace_void(BLE_PHY_TRACE_ID_DISABLE);
+
+    ble_phy_stop_usec_timer();
+    ble_phy_disable_irq_and_ppi();
+}
+
+/* Gets the current access address */
+uint32_t ble_phy_access_addr_get(void)
+{
+    return g_ble_phy_data.phy_access_address;
+}
+
+/**
+ * Return the phy state
+ *
+ * @return int The current PHY state.
+ */
+int
+ble_phy_state_get(void)
+{
+    return g_ble_phy_data.phy_state;
+}
+
+/**
+ * Called to see if a reception has started
+ *
+ * @return int
+ */
+int
+ble_phy_rx_started(void)
+{
+    return g_ble_phy_data.phy_rx_started;
+}
+
+/**
+ * Return the transceiver state
+ *
+ * @return int transceiver state.
+ */
+uint8_t
+ble_phy_xcvr_state_get(void)
+{
+    uint32_t state;
+    state = NRF_RADIO->STATE;
+    return (uint8_t)state;
+}
+
+/**
+ * Called to return the maximum data pdu payload length supported by the
+ * phy. For this chip, if encryption is enabled, the maximum payload is 27
+ * bytes.
+ *
+ * @return uint8_t Maximum data channel PDU payload size supported
+ */
+uint8_t
+ble_phy_max_data_pdu_pyld(void)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    return NRF_MAX_ENCRYPTED_PYLD_LEN;
+#else
+    return BLE_LL_DATA_PDU_MAX_PYLD;
+#endif
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+void
+ble_phy_resolv_list_enable(void)
+{
+    NRF_AAR->NIRK = (uint32_t)g_nrf_num_irks;
+    g_ble_phy_data.phy_privacy = 1;
+}
+
+void
+ble_phy_resolv_list_disable(void)
+{
+    g_ble_phy_data.phy_privacy = 0;
+}
+#endif
+
+void
+ble_phy_rfclk_enable(void)
+{
+#if MYNEWT
+    nrf51_clock_hfxo_request();
+#else
+    NRF_CLOCK->TASKS_HFCLKSTART = 1;
+#endif
+}
+
+void
+ble_phy_rfclk_disable(void)
+{
+#if MYNEWT
+    nrf51_clock_hfxo_release();
+#else
+    NRF_CLOCK->TASKS_HFCLKSTOP = 1;
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf52/include/ble/xcvr.h b/src/libs/mynewt-nimble/nimble/drivers/nrf52/include/ble/xcvr.h
new file mode 100644
index 00000000..757bb80f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf52/include/ble/xcvr.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_XCVR_
+#define H_BLE_XCVR_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define XCVR_RX_RADIO_RAMPUP_USECS  (40)
+#define XCVR_TX_RADIO_RAMPUP_USECS  (40)
+
+/*
+ * NOTE: we have to account for the RTC output compare issue. We want it to be
+ * 5 ticks.
+ */
+#define XCVR_PROC_DELAY_USECS         (153)
+#define XCVR_RX_START_DELAY_USECS     (XCVR_RX_RADIO_RAMPUP_USECS)
+#define XCVR_TX_START_DELAY_USECS     (XCVR_TX_RADIO_RAMPUP_USECS)
+#define XCVR_TX_SCHED_DELAY_USECS     \
+    (XCVR_TX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+#define XCVR_RX_SCHED_DELAY_USECS     \
+    (XCVR_RX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+
+/*
+ * Define HW whitelist size. This is the total possible whitelist size;
+ * not necessarily the size that will be used (may be smaller)
+ */
+#define BLE_HW_WHITE_LIST_SIZE        (8)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_XCVR_ */
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf52/pkg.yml b/src/libs/mynewt-nimble/nimble/drivers/nrf52/pkg.yml
new file mode 100644
index 00000000..a1ff457e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf52/pkg.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.
+#
+
+pkg.name: nimble/drivers/nrf52
+pkg.description: BLE driver for nRF52 systems.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.apis: ble_driver
+pkg.deps:
+    - nimble
+    - nimble/controller
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_hw.c b/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_hw.c
new file mode 100644
index 00000000..82e6d17e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_hw.c
@@ -0,0 +1,488 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os.h"
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nrfx.h"
+#include "controller/ble_hw.h"
+#if MYNEWT
+#include "mcu/cmsis_nvic.h"
+#else
+#include "core_cm4.h"
+#include <nimble/nimble_npl_os.h>
+#endif
+#include "os/os_trace_api.h"
+
+/* Total number of resolving list elements */
+#define BLE_HW_RESOLV_LIST_SIZE     (16)
+
+/* We use this to keep track of which entries are set to valid addresses */
+static uint8_t g_ble_hw_whitelist_mask;
+
+/* Random number generator isr callback */
+ble_rng_isr_cb_t g_ble_rng_isr_cb;
+
+/* If LL privacy is enabled, allocate memory for AAR */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+
+/* The NRF51 supports up to 16 IRK entries */
+#if (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE) < 16)
+#define NRF_IRK_LIST_ENTRIES    (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE))
+#else
+#define NRF_IRK_LIST_ENTRIES    (16)
+#endif
+
+/* NOTE: each entry is 16 bytes long. */
+uint32_t g_nrf_irk_list[NRF_IRK_LIST_ENTRIES * 4];
+
+/* Current number of IRK entries */
+uint8_t g_nrf_num_irks;
+
+#endif
+
+/* Returns public device address or -1 if not present */
+int
+ble_hw_get_public_addr(ble_addr_t *addr)
+{
+    uint32_t addr_high;
+    uint32_t addr_low;
+
+    /* Does FICR have a public address */
+    if ((NRF_FICR->DEVICEADDRTYPE & 1) != 0) {
+        return -1;
+    }
+
+    /* Copy into device address. We can do this because we know platform */
+    addr_low = NRF_FICR->DEVICEADDR[0];
+    addr_high = NRF_FICR->DEVICEADDR[1];
+    memcpy(addr->val, &addr_low, 4);
+    memcpy(&addr->val[4], &addr_high, 2);
+    addr->type = BLE_ADDR_PUBLIC;
+
+    return 0;
+}
+
+/* Returns random static address or -1 if not present */
+int
+ble_hw_get_static_addr(ble_addr_t *addr)
+{
+    int rc;
+
+    if ((NRF_FICR->DEVICEADDRTYPE & 1) == 1) {
+        memcpy(addr->val, (void *)&NRF_FICR->DEVICEADDR[0], 4);
+        memcpy(&addr->val[4], (void *)&NRF_FICR->DEVICEADDR[1], 2);
+        addr->val[5] |= 0xc0;
+        addr->type = BLE_ADDR_RANDOM;
+        rc = 0;
+    } else {
+        rc = -1;
+    }
+
+    return rc;
+}
+
+/**
+ * Clear the whitelist
+ *
+ * @return int
+ */
+void
+ble_hw_whitelist_clear(void)
+{
+    NRF_RADIO->DACNF = 0;
+    g_ble_hw_whitelist_mask = 0;
+}
+
+/**
+ * Add a device to the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    uint32_t mask;
+
+    /* Find first ununsed device address match element */
+    mask = 0x01;
+    for (i = 0; i < BLE_HW_WHITE_LIST_SIZE; ++i) {
+        if ((mask & g_ble_hw_whitelist_mask) == 0) {
+            NRF_RADIO->DAB[i] = get_le32(addr);
+            NRF_RADIO->DAP[i] = get_le16(addr + 4);
+            if (addr_type == BLE_ADDR_RANDOM) {
+                NRF_RADIO->DACNF |= (mask << 8);
+            }
+            g_ble_hw_whitelist_mask |= mask;
+            return BLE_ERR_SUCCESS;
+        }
+        mask <<= 1;
+    }
+
+    return BLE_ERR_MEM_CAPACITY;
+}
+
+/**
+ * Remove a device from the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ */
+void
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    uint8_t cfg_addr;
+    uint16_t dap;
+    uint16_t txadd;
+    uint32_t dab;
+    uint32_t mask;
+
+    /* Find first ununsed device address match element */
+    dab = get_le32(addr);
+    dap = get_le16(addr + 4);
+    txadd = NRF_RADIO->DACNF >> 8;
+    mask = 0x01;
+    for (i = 0; i < BLE_HW_WHITE_LIST_SIZE; ++i) {
+        if (mask & g_ble_hw_whitelist_mask) {
+            if ((dab == NRF_RADIO->DAB[i]) && (dap == NRF_RADIO->DAP[i])) {
+                cfg_addr = txadd & mask;
+                if (addr_type == BLE_ADDR_RANDOM) {
+                    if (cfg_addr != 0) {
+                        break;
+                    }
+                } else {
+                    if (cfg_addr == 0) {
+                        break;
+                    }
+                }
+            }
+        }
+        mask <<= 1;
+    }
+
+    if (i < BLE_HW_WHITE_LIST_SIZE) {
+        g_ble_hw_whitelist_mask &= ~mask;
+        NRF_RADIO->DACNF &= ~mask;
+    }
+}
+
+/**
+ * Returns the size of the whitelist in HW
+ *
+ * @return int Number of devices allowed in whitelist
+ */
+uint8_t
+ble_hw_whitelist_size(void)
+{
+    return BLE_HW_WHITE_LIST_SIZE;
+}
+
+/**
+ * Enable the whitelisted devices
+ */
+void
+ble_hw_whitelist_enable(void)
+{
+    /* Enable the configured device addresses */
+    NRF_RADIO->DACNF |= g_ble_hw_whitelist_mask;
+}
+
+/**
+ * Disables the whitelisted devices
+ */
+void
+ble_hw_whitelist_disable(void)
+{
+    /* Disable all whitelist devices */
+    NRF_RADIO->DACNF &= 0x0000ff00;
+}
+
+/**
+ * Boolean function which returns true ('1') if there is a match on the
+ * whitelist.
+ *
+ * @return int
+ */
+int
+ble_hw_whitelist_match(void)
+{
+    return (int)NRF_RADIO->EVENTS_DEVMATCH;
+}
+
+/* Encrypt data */
+int
+ble_hw_encrypt_block(struct ble_encryption_block *ecb)
+{
+    int rc;
+    uint32_t end;
+    uint32_t err;
+
+    /* Stop ECB */
+    NRF_ECB->TASKS_STOPECB = 1;
+    /* XXX: does task stop clear these counters? Anyway to do this quicker? */
+    NRF_ECB->EVENTS_ENDECB = 0;
+    NRF_ECB->EVENTS_ERRORECB = 0;
+    NRF_ECB->ECBDATAPTR = (uint32_t)ecb;
+
+    /* Start ECB */
+    NRF_ECB->TASKS_STARTECB = 1;
+
+    /* Wait till error or done */
+    rc = 0;
+    while (1) {
+        end = NRF_ECB->EVENTS_ENDECB;
+        err = NRF_ECB->EVENTS_ERRORECB;
+        if (end || err) {
+            if (err) {
+                rc = -1;
+            }
+            break;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Random number generator ISR.
+ */
+static void
+ble_rng_isr(void)
+{
+    uint8_t rnum;
+
+    os_trace_isr_enter();
+
+    /* No callback? Clear and disable interrupts */
+    if (g_ble_rng_isr_cb == NULL) {
+        NRF_RNG->INTENCLR = 1;
+        NRF_RNG->EVENTS_VALRDY = 0;
+        (void)NRF_RNG->SHORTS;
+        os_trace_isr_exit();
+        return;
+    }
+
+    /* If there is a value ready grab it */
+    if (NRF_RNG->EVENTS_VALRDY) {
+        NRF_RNG->EVENTS_VALRDY = 0;
+        rnum = (uint8_t)NRF_RNG->VALUE;
+        (*g_ble_rng_isr_cb)(rnum);
+    }
+
+    os_trace_isr_exit();
+}
+
+/**
+ * Initialize the random number generator
+ *
+ * @param cb
+ * @param bias
+ *
+ * @return int
+ */
+int
+ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
+{
+    /* Set bias */
+    if (bias) {
+        NRF_RNG->CONFIG = 1;
+    } else {
+        NRF_RNG->CONFIG = 0;
+    }
+
+    /* If we were passed a function pointer we need to enable the interrupt */
+    if (cb != NULL) {
+#ifndef RIOT_VERSION
+        NVIC_SetPriority(RNG_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+#endif
+#if MYNEWT
+        NVIC_SetVector(RNG_IRQn, (uint32_t)ble_rng_isr);
+#else
+        ble_npl_hw_set_isr(RNG_IRQn, ble_rng_isr);
+#endif
+        NVIC_EnableIRQ(RNG_IRQn);
+        g_ble_rng_isr_cb = cb;
+    }
+
+    return 0;
+}
+
+/**
+ * Start the random number generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_start(void)
+{
+    os_sr_t sr;
+
+    /* No need for interrupt if there is no callback */
+    OS_ENTER_CRITICAL(sr);
+    NRF_RNG->EVENTS_VALRDY = 0;
+    if (g_ble_rng_isr_cb) {
+        NRF_RNG->INTENSET = 1;
+    }
+    NRF_RNG->TASKS_START = 1;
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Stop the random generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_stop(void)
+{
+    os_sr_t sr;
+
+    /* No need for interrupt if there is no callback */
+    OS_ENTER_CRITICAL(sr);
+    NRF_RNG->INTENCLR = 1;
+    NRF_RNG->TASKS_STOP = 1;
+    NRF_RNG->EVENTS_VALRDY = 0;
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Read the random number generator.
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_hw_rng_read(void)
+{
+    uint8_t rnum;
+
+    /* Wait for a sample */
+    while (NRF_RNG->EVENTS_VALRDY == 0) {
+    }
+
+    NRF_RNG->EVENTS_VALRDY = 0;
+    rnum = (uint8_t)NRF_RNG->VALUE;
+
+    return rnum;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+/**
+ * Clear the resolving list
+ *
+ * @return int
+ */
+void
+ble_hw_resolv_list_clear(void)
+{
+    g_nrf_num_irks = 0;
+}
+
+/**
+ * Add a device to the hw resolving list
+ *
+ * @param irk   Pointer to IRK to add
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_resolv_list_add(uint8_t *irk)
+{
+    uint32_t *nrf_entry;
+
+    /* Find first ununsed device address match element */
+    if (g_nrf_num_irks == NRF_IRK_LIST_ENTRIES) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* Copy into irk list */
+    nrf_entry = &g_nrf_irk_list[4 * g_nrf_num_irks];
+    memcpy(nrf_entry, irk, 16);
+
+    /* Add to total */
+    ++g_nrf_num_irks;
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Remove a device from the hw resolving list
+ *
+ * @param index Index of IRK to remove
+ */
+void
+ble_hw_resolv_list_rmv(int index)
+{
+    uint32_t *irk_entry;
+
+    if (index < g_nrf_num_irks) {
+        --g_nrf_num_irks;
+        irk_entry = &g_nrf_irk_list[index];
+        if (g_nrf_num_irks > index) {
+            memmove(irk_entry, irk_entry + 4, 16 * (g_nrf_num_irks - index));
+        }
+    }
+}
+
+/**
+ * Returns the size of the resolving list. NOTE: this returns the maximum
+ * allowable entries in the HW. Configuration options may limit this.
+ *
+ * @return int Number of devices allowed in resolving list
+ */
+uint8_t
+ble_hw_resolv_list_size(void)
+{
+    return BLE_HW_RESOLV_LIST_SIZE;
+}
+
+/**
+ * Called to determine if the address received was resolved.
+ *
+ * @return int  Negative values indicate unresolved address; positive values
+ *              indicate index in resolving list of resolved address.
+ */
+int
+ble_hw_resolv_list_match(void)
+{
+    uint32_t index;
+
+    if (NRF_AAR->EVENTS_END) {
+        if (NRF_AAR->EVENTS_RESOLVED) {
+            index = NRF_AAR->STATUS;
+            return (int)index;
+        }
+    }
+
+    return -1;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_phy.c b/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_phy.c
new file mode 100644
index 00000000..2f6ce08e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_phy.c
@@ -0,0 +1,2116 @@
+/*
+ * 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 <assert.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/nimble_npl.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_phy_trace.h"
+#include "controller/ble_ll.h"
+#include "nrfx.h"
+#if MYNEWT
+#include "mcu/nrf52_clock.h"
+#include "mcu/cmsis_nvic.h"
+#include "hal/hal_gpio.h"
+#else
+#include "core_cm4.h"
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#if !MYNEWT_VAL_CHOICE(MCU_TARGET, nRF52840) && !MYNEWT_VAL_CHOICE(MCU_TARGET, nRF52811)
+#error LE Coded PHY can only be enabled on nRF52811 or nRF52840
+#endif
+#endif
+
+/*
+ * NOTE: This code uses a couple of PPI channels so care should be taken when
+ *       using PPI somewhere else.
+ *
+ * Pre-programmed channels: CH20, CH21, CH23, CH25, CH31
+ * Regular channels: CH4, CH5 and optionally CH17, CH18, CH19
+ *  - CH4 = cancel wfr timer on address match
+ *  - CH5 = disable radio on wfr timer expiry
+ *  - CH17 = (optional) gpio debug for radio ramp-up
+ *  - CH18 = (optional) gpio debug for wfr timer RX enabled
+ *  - CH19 = (optional) gpio debug for wfr timer radio disabled
+ *
+ */
+
+/* XXX: 4) Make sure RF is higher priority interrupt than schedule */
+
+/*
+ * XXX: Maximum possible transmit time is 1 msec for a 60ppm crystal
+ * and 16ms for a 30ppm crystal! We need to limit PDU size based on
+ * crystal accuracy. Look at this in the spec.
+ */
+
+/* XXX: private header file? */
+extern uint8_t g_nrf_num_irks;
+extern uint32_t g_nrf_irk_list[];
+
+/* To disable all radio interrupts */
+#define NRF_RADIO_IRQ_MASK_ALL  (0x34FF)
+
+/*
+ * We configure the nrf with a 1 byte S0 field, 8 bit length field, and
+ * zero bit S1 field. The preamble is 8 bits long.
+ */
+#define NRF_LFLEN_BITS          (8)
+#define NRF_S0LEN               (1)
+#define NRF_S1LEN_BITS          (0)
+#define NRF_CILEN_BITS          (2)
+#define NRF_TERMLEN_BITS        (3)
+
+/* Maximum length of frames */
+#define NRF_MAXLEN              (255)
+#define NRF_BALEN               (3)     /* For base address of 3 bytes */
+
+/* NRF_RADIO->PCNF0 configuration values */
+#define NRF_PCNF0               (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) | \
+                                (RADIO_PCNF0_S1INCL_Msk) | \
+                                (NRF_S0LEN << RADIO_PCNF0_S0LEN_Pos) | \
+                                (NRF_S1LEN_BITS << RADIO_PCNF0_S1LEN_Pos)
+#define NRF_PCNF0_1M            (NRF_PCNF0) | \
+                                (RADIO_PCNF0_PLEN_8bit << RADIO_PCNF0_PLEN_Pos)
+#define NRF_PCNF0_2M            (NRF_PCNF0) | \
+                                (RADIO_PCNF0_PLEN_16bit << RADIO_PCNF0_PLEN_Pos)
+#define NRF_PCNF0_CODED         (NRF_PCNF0) | \
+                                (RADIO_PCNF0_PLEN_LongRange << RADIO_PCNF0_PLEN_Pos) | \
+                                (NRF_CILEN_BITS << RADIO_PCNF0_CILEN_Pos) | \
+                                (NRF_TERMLEN_BITS << RADIO_PCNF0_TERMLEN_Pos)
+
+/* BLE PHY data structure */
+struct ble_phy_obj
+{
+    uint8_t phy_stats_initialized;
+    int8_t  phy_txpwr_dbm;
+    uint8_t phy_chan;
+    uint8_t phy_state;
+    uint8_t phy_transition;
+    uint8_t phy_transition_late;
+    uint8_t phy_rx_started;
+    uint8_t phy_encrypted;
+    uint8_t phy_privacy;
+    uint8_t phy_tx_pyld_len;
+    uint8_t phy_cur_phy_mode;
+    uint8_t phy_tx_phy_mode;
+    uint8_t phy_rx_phy_mode;
+    uint8_t phy_bcc_offset;
+    int8_t  rx_pwr_compensation;
+    uint32_t phy_aar_scratch;
+    uint32_t phy_access_address;
+    struct ble_mbuf_hdr rxhdr;
+    void *txend_arg;
+    ble_phy_tx_end_func txend_cb;
+    uint32_t phy_start_cputime;
+};
+struct ble_phy_obj g_ble_phy_data;
+
+/* XXX: if 27 byte packets desired we can make this smaller */
+/* Global transmit/receive buffer */
+static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/* Make sure word-aligned for faster copies */
+static uint32_t g_ble_phy_enc_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+#endif
+
+/* RF center frequency for each channel index (offset from 2400 MHz) */
+static const uint8_t g_ble_phy_chan_freq[BLE_PHY_NUM_CHANS] = {
+     4,  6,  8, 10, 12, 14, 16, 18, 20, 22, /* 0-9 */
+    24, 28, 30, 32, 34, 36, 38, 40, 42, 44, /* 10-19 */
+    46, 48, 50, 52, 54, 56, 58, 60, 62, 64, /* 20-29 */
+    66, 68, 70, 72, 74, 76, 78,  2, 26, 80, /* 30-39 */
+};
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+/* packet start offsets (in usecs) */
+static const uint16_t g_ble_phy_mode_pkt_start_off[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 40,
+    [BLE_PHY_MODE_2M] = 24,
+    [BLE_PHY_MODE_CODED_125KBPS] = 376,
+    [BLE_PHY_MODE_CODED_500KBPS] = 376
+};
+#endif
+
+/* Various radio timings */
+/* Radio ramp-up times in usecs (fast mode) */
+#define BLE_PHY_T_TXENFAST      (XCVR_TX_RADIO_RAMPUP_USECS)
+#define BLE_PHY_T_RXENFAST      (XCVR_RX_RADIO_RAMPUP_USECS)
+/* delay between EVENTS_READY and start of tx */
+static const uint8_t g_ble_phy_t_txdelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 4,
+    [BLE_PHY_MODE_2M] = 3,
+    [BLE_PHY_MODE_CODED_125KBPS] = 5,
+    [BLE_PHY_MODE_CODED_500KBPS] = 5
+};
+/* delay between EVENTS_END and end of txd packet */
+static const uint8_t g_ble_phy_t_txenddelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 4,
+    [BLE_PHY_MODE_2M] = 3,
+    [BLE_PHY_MODE_CODED_125KBPS] = 9,
+    [BLE_PHY_MODE_CODED_500KBPS] = 3
+};
+/* delay between rxd access address (w/ TERM1 for coded) and EVENTS_ADDRESS */
+static const uint8_t g_ble_phy_t_rxaddrdelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 6,
+    [BLE_PHY_MODE_2M] = 2,
+    [BLE_PHY_MODE_CODED_125KBPS] = 17,
+    [BLE_PHY_MODE_CODED_500KBPS] = 17
+};
+/* delay between end of rxd packet and EVENTS_END */
+static const uint8_t g_ble_phy_t_rxenddelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 6,
+    [BLE_PHY_MODE_2M] = 2,
+    [BLE_PHY_MODE_CODED_125KBPS] = 27,
+    [BLE_PHY_MODE_CODED_500KBPS] = 22
+};
+
+/* Statistics */
+STATS_SECT_START(ble_phy_stats)
+    STATS_SECT_ENTRY(phy_isrs)
+    STATS_SECT_ENTRY(tx_good)
+    STATS_SECT_ENTRY(tx_fail)
+    STATS_SECT_ENTRY(tx_late)
+    STATS_SECT_ENTRY(tx_bytes)
+    STATS_SECT_ENTRY(rx_starts)
+    STATS_SECT_ENTRY(rx_aborts)
+    STATS_SECT_ENTRY(rx_valid)
+    STATS_SECT_ENTRY(rx_crc_err)
+    STATS_SECT_ENTRY(rx_late)
+    STATS_SECT_ENTRY(radio_state_errs)
+    STATS_SECT_ENTRY(rx_hw_err)
+    STATS_SECT_ENTRY(tx_hw_err)
+STATS_SECT_END
+STATS_SECT_DECL(ble_phy_stats) ble_phy_stats;
+
+STATS_NAME_START(ble_phy_stats)
+    STATS_NAME(ble_phy_stats, phy_isrs)
+    STATS_NAME(ble_phy_stats, tx_good)
+    STATS_NAME(ble_phy_stats, tx_fail)
+    STATS_NAME(ble_phy_stats, tx_late)
+    STATS_NAME(ble_phy_stats, tx_bytes)
+    STATS_NAME(ble_phy_stats, rx_starts)
+    STATS_NAME(ble_phy_stats, rx_aborts)
+    STATS_NAME(ble_phy_stats, rx_valid)
+    STATS_NAME(ble_phy_stats, rx_crc_err)
+    STATS_NAME(ble_phy_stats, rx_late)
+    STATS_NAME(ble_phy_stats, radio_state_errs)
+    STATS_NAME(ble_phy_stats, rx_hw_err)
+    STATS_NAME(ble_phy_stats, tx_hw_err)
+STATS_NAME_END(ble_phy_stats)
+
+/*
+ * NOTE:
+ * Tested the following to see what would happen:
+ *  -> NVIC has radio irq enabled (interrupt # 1, mask 0x2).
+ *  -> Set up nrf to receive. Clear ADDRESS event register.
+ *  -> Enable ADDRESS interrupt on nrf5 by writing to INTENSET.
+ *  -> Enable RX.
+ *  -> Disable interrupts globally using OS_ENTER_CRITICAL().
+ *  -> Wait until a packet is received and the ADDRESS event occurs.
+ *  -> Call ble_phy_disable().
+ *
+ *  At this point I wanted to see the state of the cortex NVIC. The IRQ
+ *  pending bit was TRUE for the radio interrupt (as expected) as we never
+ *  serviced the radio interrupt (interrupts were disabled).
+ *
+ *  What was unexpected was this: without clearing the pending IRQ in the NVIC,
+ *  when radio interrupts were re-enabled (address event bit in INTENSET set to
+ *  1) and the radio ADDRESS event register read 1 (it was never cleared after
+ *  the first address event), the radio did not enter the ISR! I would have
+ *  expected that if the following were true, an interrupt would occur:
+ *      -> NVIC ISER bit set to TRUE
+ *      -> NVIC ISPR bit reads TRUE, meaning interrupt is pending.
+ *      -> Radio peripheral interrupts are enabled for some event (or events).
+ *      -> Corresponding event register(s) in radio peripheral read 1.
+ *
+ *  Not sure what the end result of all this is. We will clear the pending
+ *  bit in the NVIC just to be sure when we disable the PHY.
+ */
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+
+/*
+ * Per nordic, the number of bytes needed for scratch is 16 + MAX_PKT_SIZE.
+ * However, when I used a smaller size it still overwrote the scratchpad. Until
+ * I figure this out I am just going to allocate 67 words so we have enough
+ * space for 267 bytes of scratch. I used 268 bytes since not sure if this
+ * needs to be aligned and burning a byte is no big deal.
+ */
+//#define NRF_ENC_SCRATCH_WORDS (((MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE) + 16) + 3) / 4)
+#define NRF_ENC_SCRATCH_WORDS   (67)
+
+uint32_t g_nrf_encrypt_scratchpad[NRF_ENC_SCRATCH_WORDS];
+
+struct nrf_ccm_data
+{
+    uint8_t key[16];
+    uint64_t pkt_counter;
+    uint8_t dir_bit;
+    uint8_t iv[8];
+} __attribute__((packed));
+
+struct nrf_ccm_data g_nrf_ccm_data;
+#endif
+
+static void
+ble_phy_apply_errata_102_106_107(void)
+{
+    /* [102] RADIO: PAYLOAD/END events delayed or not triggered after ADDRESS
+     * [106] RADIO: Higher CRC error rates for some access addresses
+     * [107] RADIO: Immediate address match for access addresses containing MSBs 0x00
+     */
+    *(volatile uint32_t *)0x40001774 = ((*(volatile uint32_t *)0x40001774) &
+                         0xfffffffe) | 0x01000000;
+}
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+
+/* Packet start offset (in usecs). This is the preamble plus access address.
+ * For LE Coded PHY this also includes CI and TERM1. */
+uint32_t
+ble_phy_mode_pdu_start_off(int phy_mode)
+{
+    return g_ble_phy_mode_pkt_start_off[phy_mode];
+}
+
+#if NRF52840_XXAA
+static inline bool
+ble_phy_mode_is_coded(uint8_t phy_mode)
+{
+    return (phy_mode == BLE_PHY_MODE_CODED_125KBPS) ||
+           (phy_mode == BLE_PHY_MODE_CODED_500KBPS);
+}
+
+static void
+ble_phy_apply_nrf52840_errata(uint8_t new_phy_mode)
+{
+    bool new_coded = ble_phy_mode_is_coded(new_phy_mode);
+    bool cur_coded = ble_phy_mode_is_coded(g_ble_phy_data.phy_cur_phy_mode);
+
+    /*
+     * Workarounds should be applied only when switching to/from LE Coded PHY
+     * so no need to apply them every time.
+     *
+     * nRF52840 Engineering A Errata v1.2
+     * [164] RADIO: Low sensitivity in long range mode
+     *
+     * nRF52840 Rev 1 Errata
+     * [191] RADIO: High packet error rate in BLE Long Range mode
+     */
+    if (new_coded == cur_coded) {
+        return;
+    }
+
+    if (new_coded) {
+#if MYNEWT_VAL(BLE_PHY_NRF52840_ERRATA_164)
+        /* [164] */
+        *(volatile uint32_t *)0x4000173C |= 0x80000000;
+        *(volatile uint32_t *)0x4000173C =
+                        ((*(volatile uint32_t *)0x4000173C & 0xFFFFFF00) | 0x5C);
+#endif
+#if MYNEWT_VAL(BLE_PHY_NRF52840_ERRATA_191)
+        /* [191] */
+        *(volatile uint32_t *) 0x40001740 =
+                        ((*((volatile uint32_t *) 0x40001740)) & 0x7FFF00FF) |
+                        0x80000000 | (((uint32_t)(196)) << 8);
+#endif
+    } else {
+#if MYNEWT_VAL(BLE_PHY_NRF52840_ERRATA_164)
+        /* [164] */
+        *(volatile uint32_t *)0x4000173C &= ~0x80000000;
+#endif
+#if MYNEWT_VAL(BLE_PHY_NRF52840_ERRATA_191)
+        /* [191] */
+        *(volatile uint32_t *) 0x40001740 =
+                        ((*((volatile uint32_t *) 0x40001740)) & 0x7FFFFFFF);
+#endif
+    }
+}
+#endif
+
+static void
+ble_phy_mode_apply(uint8_t phy_mode)
+{
+    if (phy_mode == g_ble_phy_data.phy_cur_phy_mode) {
+        return;
+    }
+
+#if NRF52840_XXAA
+    ble_phy_apply_nrf52840_errata(phy_mode);
+#endif
+
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+        NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_1Mbit;
+        NRF_RADIO->PCNF0 = NRF_PCNF0_1M;
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_PHY_MODE_2M:
+        NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_2Mbit;
+        NRF_RADIO->PCNF0 = NRF_PCNF0_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_PHY_MODE_CODED_125KBPS:
+        NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_LR125Kbit;
+        NRF_RADIO->PCNF0 = NRF_PCNF0_CODED;
+        break;
+    case BLE_PHY_MODE_CODED_500KBPS:
+        NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_LR500Kbit;
+        NRF_RADIO->PCNF0 = NRF_PCNF0_CODED;
+        break;
+#endif
+    default:
+        assert(0);
+    }
+
+    g_ble_phy_data.phy_cur_phy_mode = phy_mode;
+}
+
+void
+ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode)
+{
+    g_ble_phy_data.phy_tx_phy_mode = tx_phy_mode;
+    g_ble_phy_data.phy_rx_phy_mode = rx_phy_mode;
+}
+#endif
+
+int
+ble_phy_get_cur_phy(void)
+{
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    switch (g_ble_phy_data.phy_cur_phy_mode) {
+        case BLE_PHY_MODE_1M:
+            return BLE_PHY_1M;
+        case BLE_PHY_MODE_2M:
+            return BLE_PHY_2M;
+        case BLE_PHY_MODE_CODED_125KBPS:
+        case BLE_PHY_MODE_CODED_500KBPS:
+            return BLE_PHY_CODED;
+        default:
+            assert(0);
+            return -1;
+    }
+#else
+    return BLE_PHY_1M;
+#endif
+}
+
+/**
+ * Copies the data from the phy receive buffer into a mbuf chain.
+ *
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
+ *
+ */
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
+{
+    uint32_t rem_len;
+    uint32_t copy_len;
+    uint32_t block_len;
+    uint32_t block_rem_len;
+    void *dst;
+    void *src;
+    struct os_mbuf * om;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    block_len = rxpdu->om_omp->omp_databuf_len;
+    rem_len = OS_MBUF_PKTHDR(rxpdu)->omp_len;
+    src = dptr;
+
+    /*
+     * Setup for copying from first mbuf which is shorter due to packet header
+     * and extra leading space
+     */
+    copy_len = block_len - rxpdu->om_pkthdr_len - 4;
+    om = rxpdu;
+    dst = om->om_data;
+
+    while (true) {
+        /*
+         * Always copy blocks of length aligned to word size, only last mbuf
+         * will have remaining non-word size bytes appended.
+         */
+        block_rem_len = copy_len;
+        copy_len = min(copy_len, rem_len);
+        copy_len &= ~3;
+
+        dst = om->om_data;
+        om->om_len = copy_len;
+        rem_len -= copy_len;
+        block_rem_len -= copy_len;
+
+        __asm__ volatile (".syntax unified              \n"
+                          "   mov  r4, %[len]           \n"
+                          "   b    2f                   \n"
+                          "1: ldr  r3, [%[src], %[len]] \n"
+                          "   str  r3, [%[dst], %[len]] \n"
+                          "2: subs %[len], #4           \n"
+                          "   bpl  1b                   \n"
+                          "   adds %[src], %[src], r4   \n"
+                          "   adds %[dst], %[dst], r4   \n"
+                          : [dst] "+r" (dst), [src] "+r" (src),
+                            [len] "+r" (copy_len)
+                          :
+                          : "r3", "r4", "memory"
+                         );
+
+        if ((rem_len < 4) && (block_rem_len >= rem_len)) {
+            break;
+        }
+
+        /* Move to next mbuf */
+        om = SLIST_NEXT(om, om_next);
+        copy_len = block_len;
+    }
+
+    /* Copy remaining bytes, if any, to last mbuf */
+    om->om_len += rem_len;
+    __asm__ volatile (".syntax unified              \n"
+                      "   b    2f                   \n"
+                      "1: ldrb r3, [%[src], %[len]] \n"
+                      "   strb r3, [%[dst], %[len]] \n"
+                      "2: subs %[len], #1           \n"
+                      "   bpl  1b                   \n"
+                      : [len] "+r" (rem_len)
+                      : [dst] "r" (dst), [src] "r" (src)
+                      : "r3", "memory"
+                     );
+
+    /* Copy header */
+    memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
+           sizeof(struct ble_mbuf_hdr));
+}
+
+/**
+ * Called when we want to wait if the radio is in either the rx or tx
+ * disable states. We want to wait until that state is over before doing
+ * anything to the radio
+ */
+static void
+nrf_wait_disabled(void)
+{
+    uint32_t state;
+
+    state = NRF_RADIO->STATE;
+    if (state != RADIO_STATE_STATE_Disabled) {
+        if ((state == RADIO_STATE_STATE_RxDisable) ||
+            (state == RADIO_STATE_STATE_TxDisable)) {
+            /* This will end within a short time (6 usecs). Just poll */
+            while (NRF_RADIO->STATE == state) {
+                /* If this fails, something is really wrong. Should last
+                 * no more than 6 usecs */
+            }
+        }
+    }
+}
+
+/**
+ *
+ *
+ */
+static int
+ble_phy_set_start_time(uint32_t cputime, uint8_t rem_usecs, bool tx)
+{
+    uint32_t next_cc;
+    uint32_t cur_cc;
+    uint32_t cntr;
+    uint32_t delta;
+
+    /*
+     * We need to adjust start time to include radio ramp-up and TX pipeline
+     * delay (the latter only if applicable, so only for TX).
+     *
+     * Radio ramp-up time is 40 usecs and TX delay is 3 or 5 usecs depending on
+     * phy, thus we'll offset RTC by 2 full ticks (61 usecs) and then compensate
+     * using TIMER0 with 1 usec precision.
+     */
+
+    cputime -= 2;
+    rem_usecs += 61;
+    if (tx) {
+        rem_usecs -= BLE_PHY_T_TXENFAST;
+        rem_usecs -= g_ble_phy_t_txdelay[g_ble_phy_data.phy_cur_phy_mode];
+    } else {
+        rem_usecs -= BLE_PHY_T_RXENFAST;
+    }
+
+    /*
+     * rem_usecs will be no more than 2 ticks, but if it is more than single
+     * tick then we should better count one more low-power tick rather than
+     * 30 high-power usecs. Also make sure we don't set TIMER0 CC to 0 as the
+     * compare won't occur.
+     */
+
+    if (rem_usecs > 30) {
+        cputime++;
+        rem_usecs -= 30;
+    }
+
+    /*
+     * Can we set the RTC compare to start TIMER0? We can do it if:
+     *      a) Current compare value is not N+1 or N+2 ticks from current
+     *      counter.
+     *      b) The value we want to set is not at least N+2 from current
+     *      counter.
+     *
+     * NOTE: since the counter can tick 1 while we do these calculations we
+     * need to account for it.
+     */
+    next_cc = cputime & 0xffffff;
+    cur_cc = NRF_RTC0->CC[0];
+    cntr = NRF_RTC0->COUNTER;
+
+    delta = (cur_cc - cntr) & 0xffffff;
+    if ((delta <= 3) && (delta != 0)) {
+        return -1;
+    }
+    delta = (next_cc - cntr) & 0xffffff;
+    if ((delta & 0x800000) || (delta < 3)) {
+        return -1;
+    }
+
+    /* Clear and set TIMER0 to fire off at proper time */
+    NRF_TIMER0->TASKS_CLEAR = 1;
+    NRF_TIMER0->CC[0] = rem_usecs;
+    NRF_TIMER0->EVENTS_COMPARE[0] = 0;
+
+    /* Set RTC compare to start TIMER0 */
+    NRF_RTC0->EVENTS_COMPARE[0] = 0;
+    NRF_RTC0->CC[0] = next_cc;
+    NRF_RTC0->EVTENSET = RTC_EVTENSET_COMPARE0_Msk;
+
+    /* Enable PPI */
+    NRF_PPI->CHENSET = PPI_CHEN_CH31_Msk;
+
+    /* Store the cputime at which we set the RTC */
+    g_ble_phy_data.phy_start_cputime = cputime;
+
+    return 0;
+}
+
+static int
+ble_phy_set_start_now(void)
+{
+    os_sr_t sr;
+    uint32_t now;
+
+    OS_ENTER_CRITICAL(sr);
+
+    /*
+     * Set TIMER0 to fire immediately. We can't set CC to 0 as compare will not
+     * occur in such case.
+     */
+    NRF_TIMER0->TASKS_CLEAR = 1;
+    NRF_TIMER0->CC[0] = 1;
+    NRF_TIMER0->EVENTS_COMPARE[0] = 0;
+
+    /*
+     * Set RTC compare to start TIMER0. We need to set it to at least N+2 ticks
+     * from current value to guarantee triggering compare event, but let's set
+     * it to N+3 to account for possible extra tick on RTC0 during these
+     * operations.
+     */
+    now = os_cputime_get32();
+    NRF_RTC0->EVENTS_COMPARE[0] = 0;
+    NRF_RTC0->CC[0] = now + 3;
+    NRF_RTC0->EVTENSET = RTC_EVTENSET_COMPARE0_Msk;
+
+    /* Enable PPI */
+    NRF_PPI->CHENSET = PPI_CHEN_CH31_Msk;
+
+    /*
+     * Store the cputime at which we set the RTC
+     *
+     * XXX Compare event may be triggered on previous CC value (if it was set to
+     * less than N+2) so in rare cases actual start time may be 2 ticks earlier
+     * than what we expect. Since this is only used on RX, it may cause AUX scan
+     * to be scheduled 1 or 2 ticks too late so we'll miss it - it's acceptable
+     * for now.
+     */
+    g_ble_phy_data.phy_start_cputime = now + 3;
+
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Function is used to set PPI so that we can time out waiting for a reception
+ * to occur. This happens for two reasons: we have sent a packet and we are
+ * waiting for a respons (txrx should be set to ENABLE_TXRX) or we are
+ * starting a connection event and we are a slave and we are waiting for the
+ * master to send us a packet (txrx should be set to ENABLE_RX).
+ *
+ * NOTE: when waiting for a txrx turn-around, wfr_usecs is not used as there
+ * is no additional time to wait; we know when we should receive the address of
+ * the received frame.
+ *
+ * @param txrx Flag denoting if this wfr is a txrx turn-around or not.
+ * @param tx_phy_mode phy mode for last TX (only valid for TX->RX)
+ * @param wfr_usecs Amount of usecs to wait.
+ */
+void
+ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs)
+{
+    uint32_t end_time;
+    uint8_t phy;
+
+    phy = g_ble_phy_data.phy_cur_phy_mode;
+
+    if (txrx == BLE_PHY_WFR_ENABLE_TXRX) {
+        /* RX shall start exactly T_IFS after TX end captured in CC[2] */
+        end_time = NRF_TIMER0->CC[2] + BLE_LL_IFS;
+        /* Adjust for delay between EVENT_END and actual TX end time */
+        end_time += g_ble_phy_t_txenddelay[tx_phy_mode];
+        /* Wait a bit longer due to allowed active clock accuracy */
+        end_time += 2;
+        /*
+         * It's possible that we'll capture PDU start time at the end of timer
+         * cycle and since wfr expires at the beginning of calculated timer
+         * cycle it can be almost 1 usec too early. Let's compensate for this
+         * by waiting 1 usec more.
+         */
+        end_time += 1;
+#if MYNEWT_VAL(BLE_PHY_CODED_RX_IFS_EXTRA_MARGIN) > 0
+        if ((phy == BLE_PHY_MODE_CODED_125KBPS) ||
+                                    (phy == BLE_PHY_MODE_CODED_500KBPS)) {
+            /*
+             * Some controllers exceed T_IFS when transmitting on coded phy
+             * so let's wait a bit longer to be able to talk to them if this
+             * workaround is enabled.
+             */
+            end_time += MYNEWT_VAL(BLE_PHY_CODED_RX_IFS_EXTRA_MARGIN);
+        }
+#endif
+    } else {
+        /*
+         * RX shall start no later than wfr_usecs after RX enabled.
+         * CC[0] is the time of RXEN so adjust for radio ram-up.
+         * Do not add jitter since this is already covered by LL.
+         */
+        end_time = NRF_TIMER0->CC[0] + BLE_PHY_T_RXENFAST + wfr_usecs;
+    }
+
+    /*
+     * Note: on LE Coded EVENT_ADDRESS is fired after TERM1 is received, so
+     *       we are actually calculating relative to start of packet payload
+     *       which is fine.
+     */
+
+    /* Adjust for receiving access address since this triggers EVENT_ADDRESS */
+    end_time += ble_phy_mode_pdu_start_off(phy);
+    /* Adjust for delay between actual access address RX and EVENT_ADDRESS */
+    end_time += g_ble_phy_t_rxaddrdelay[phy];
+
+    /* wfr_secs is the time from rxen until timeout */
+    NRF_TIMER0->CC[3] = end_time;
+    NRF_TIMER0->EVENTS_COMPARE[3] = 0;
+
+    /* Enable wait for response PPI */
+    NRF_PPI->CHENSET = (PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk);
+
+    /* Enable the disabled interrupt so we time out on events compare */
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    /*
+     * It may happen that if CPU is halted for a brief moment (e.g. during flash
+     * erase or write), TIMER0 already counted past CC[3] and thus wfr will not
+     * fire as expected. In case this happened, let's just disable PPIs for wfr
+     * and trigger wfr manually (i.e. disable radio).
+     *
+     * Note that the same applies to RX start time set in CC[0] but since it
+     * should fire earlier than wfr, fixing wfr is enough.
+     *
+     * CC[1] is only used as a reference on RX start, we do not need it here so
+     * it can be used to read TIMER0 counter.
+     */
+    NRF_TIMER0->TASKS_CAPTURE[1] = 1;
+    if (NRF_TIMER0->CC[1] > NRF_TIMER0->CC[3]) {
+        NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk;
+        NRF_RADIO->TASKS_DISABLE = 1;
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+static uint32_t
+ble_phy_get_ccm_datarate(void)
+{
+#if BLE_LL_BT5_PHY_SUPPORTED
+    switch (g_ble_phy_data.phy_cur_phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return CCM_MODE_DATARATE_1Mbit << CCM_MODE_DATARATE_Pos;
+    case BLE_PHY_MODE_2M:
+        return CCM_MODE_DATARATE_2Mbit << CCM_MODE_DATARATE_Pos;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_PHY_MODE_CODED_125KBPS:
+        return CCM_MODE_DATARATE_125Kbps << CCM_MODE_DATARATE_Pos;
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return CCM_MODE_DATARATE_500Kbps << CCM_MODE_DATARATE_Pos;
+#endif
+    }
+
+    assert(0);
+    return 0;
+#else
+    return CCM_MODE_DATARATE_1Mbit << CCM_MODE_DATARATE_Pos;
+#endif
+}
+#endif
+
+/**
+ * Setup transceiver for receive.
+ */
+static void
+ble_phy_rx_xcvr_setup(void)
+{
+    uint8_t *dptr;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+    dptr += 3;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM->OUTPTR = (uint32_t)dptr;
+        NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+        NRF_CCM->MODE = CCM_MODE_LENGTH_Msk | CCM_MODE_MODE_Decryption |
+                                                    ble_phy_get_ccm_datarate();
+        NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+        NRF_CCM->SHORTS = 0;
+        NRF_CCM->EVENTS_ERROR = 0;
+        NRF_CCM->EVENTS_ENDCRYPT = 0;
+        NRF_CCM->TASKS_KSGEN = 1;
+        NRF_PPI->CHENSET = PPI_CHEN_CH25_Msk;
+    } else {
+        NRF_RADIO->PACKETPTR = (uint32_t)dptr;
+    }
+#else
+    NRF_RADIO->PACKETPTR = (uint32_t)dptr;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (g_ble_phy_data.phy_privacy) {
+        NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Enabled;
+        NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+        NRF_AAR->SCRATCHPTR = (uint32_t)&g_ble_phy_data.phy_aar_scratch;
+        NRF_AAR->EVENTS_END = 0;
+        NRF_AAR->EVENTS_RESOLVED = 0;
+        NRF_AAR->EVENTS_NOTRESOLVED = 0;
+    } else {
+        if (g_ble_phy_data.phy_encrypted == 0) {
+            NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Disabled;
+        }
+    }
+#endif
+
+    /* Turn off trigger TXEN on output compare match and AAR on bcmatch */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk | PPI_CHEN_CH23_Msk;
+
+    /* Reset the rx started flag. Used for the wait for response */
+    g_ble_phy_data.phy_rx_started = 0;
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+
+#if BLE_LL_BT5_PHY_SUPPORTED
+    /*
+     * On Coded PHY there are CI and TERM1 fields before PDU starts so we need
+     * to take this into account when setting up BCC.
+     */
+    if (g_ble_phy_data.phy_cur_phy_mode == BLE_PHY_MODE_CODED_125KBPS ||
+            g_ble_phy_data.phy_cur_phy_mode == BLE_PHY_MODE_CODED_500KBPS) {
+        g_ble_phy_data.phy_bcc_offset = 5;
+    } else {
+        g_ble_phy_data.phy_bcc_offset = 0;
+    }
+#else
+    g_ble_phy_data.phy_bcc_offset = 0;
+#endif
+
+    /* I want to know when 1st byte received (after address) */
+    NRF_RADIO->BCC = 8 + g_ble_phy_data.phy_bcc_offset; /* in bits */
+    NRF_RADIO->EVENTS_ADDRESS = 0;
+    NRF_RADIO->EVENTS_DEVMATCH = 0;
+    NRF_RADIO->EVENTS_BCMATCH = 0;
+    NRF_RADIO->EVENTS_RSSIEND = 0;
+    NRF_RADIO->EVENTS_CRCOK = 0;
+    NRF_RADIO->SHORTS = RADIO_SHORTS_END_DISABLE_Msk |
+                        RADIO_SHORTS_READY_START_Msk |
+                        RADIO_SHORTS_ADDRESS_BCSTART_Msk |
+                        RADIO_SHORTS_ADDRESS_RSSISTART_Msk |
+                        RADIO_SHORTS_DISABLED_RSSISTOP_Msk;
+
+    NRF_RADIO->INTENSET = RADIO_INTENSET_ADDRESS_Msk;
+}
+
+/**
+ * Called from interrupt context when the transmit ends
+ *
+ */
+static void
+ble_phy_tx_end_isr(void)
+{
+    uint8_t tx_phy_mode;
+    uint8_t was_encrypted;
+    uint8_t transition;
+    uint32_t rx_time;
+    uint32_t wfr_time;
+
+    /* Store PHY on which we've just transmitted smth */
+    tx_phy_mode = g_ble_phy_data.phy_cur_phy_mode;
+
+    /* If this transmission was encrypted we need to remember it */
+    was_encrypted = g_ble_phy_data.phy_encrypted;
+    (void)was_encrypted;
+
+    /* Better be in TX state! */
+    assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
+
+    /* Clear events and clear interrupt on disabled event */
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_DISABLED_Msk;
+    NRF_RADIO->EVENTS_END = 0;
+    wfr_time = NRF_RADIO->SHORTS;
+    (void)wfr_time;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    /*
+     * XXX: not sure what to do. We had a HW error during transmission.
+     * For now I just count a stat but continue on like all is good.
+     */
+    if (was_encrypted) {
+        if (NRF_CCM->EVENTS_ERROR) {
+            STATS_INC(ble_phy_stats, tx_hw_err);
+            NRF_CCM->EVENTS_ERROR = 0;
+        }
+    }
+#endif
+
+    /* Call transmit end callback */
+    if (g_ble_phy_data.txend_cb) {
+        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
+    }
+
+    transition = g_ble_phy_data.phy_transition;
+    if (transition == BLE_PHY_TRANSITION_TX_RX) {
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        ble_phy_mode_apply(g_ble_phy_data.phy_rx_phy_mode);
+#endif
+
+        /* Packet pointer needs to be reset. */
+        ble_phy_rx_xcvr_setup();
+
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_TXRX, tx_phy_mode, 0);
+
+        /* Schedule RX exactly T_IFS after TX end captured in CC[2] */
+        rx_time = NRF_TIMER0->CC[2] + BLE_LL_IFS;
+        /* Adjust for delay between EVENT_END and actual TX end time */
+        rx_time += g_ble_phy_t_txenddelay[tx_phy_mode];
+        /* Adjust for radio ramp-up */
+        rx_time -= BLE_PHY_T_RXENFAST;
+        /* Start listening a bit earlier due to allowed active clock accuracy */
+        rx_time -= 2;
+
+        NRF_TIMER0->CC[0] = rx_time;
+        NRF_TIMER0->EVENTS_COMPARE[0] = 0;
+        NRF_PPI->CHENSET = PPI_CHEN_CH21_Msk;
+    } else {
+        /*
+         * XXX: not sure we need to stop the timer here all the time. Or that
+         * it should be stopped here.
+         */
+        NRF_TIMER0->TASKS_STOP = 1;
+        NRF_TIMER0->TASKS_SHUTDOWN = 1;
+        NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk |
+                           PPI_CHEN_CH20_Msk | PPI_CHEN_CH31_Msk;
+        assert(transition == BLE_PHY_TRANSITION_NONE);
+    }
+}
+
+static inline uint8_t
+ble_phy_get_cur_rx_phy_mode(void)
+{
+    uint8_t phy;
+
+    phy = g_ble_phy_data.phy_cur_phy_mode;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    /*
+     * For Coded PHY mode can be set to either codings since actual coding is
+     * set in packet header. However, here we need actual coding of received
+     * packet as this determines pipeline delays so need to figure this out
+     * using CI field.
+     */
+    if ((phy == BLE_PHY_MODE_CODED_125KBPS) ||
+                                    (phy == BLE_PHY_MODE_CODED_500KBPS)) {
+        phy = NRF_RADIO->PDUSTAT & RADIO_PDUSTAT_CISTAT_Msk ?
+                                   BLE_PHY_MODE_CODED_500KBPS :
+                                   BLE_PHY_MODE_CODED_125KBPS;
+    }
+#endif
+
+    return phy;
+}
+
+static void
+ble_phy_rx_end_isr(void)
+{
+    int rc;
+    uint8_t *dptr;
+    uint8_t crcok;
+    uint32_t tx_time;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* Clear events and clear interrupt */
+    NRF_RADIO->EVENTS_END = 0;
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_END_Msk;
+
+    /* Disable automatic RXEN */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
+
+    /* Set RSSI and CRC status flag in header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    assert(NRF_RADIO->EVENTS_RSSIEND != 0);
+    ble_hdr->rxinfo.rssi = (-1 * NRF_RADIO->RSSISAMPLE) +
+                           g_ble_phy_data.rx_pwr_compensation;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+    dptr += 3;
+
+    /* Count PHY crc errors and valid packets */
+    crcok = NRF_RADIO->EVENTS_CRCOK;
+    if (!crcok) {
+        STATS_INC(ble_phy_stats, rx_crc_err);
+    } else {
+        STATS_INC(ble_phy_stats, rx_valid);
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (g_ble_phy_data.phy_encrypted) {
+            /* Only set MIC failure flag if frame is not zero length */
+            if ((dptr[1] != 0) && (NRF_CCM->MICSTATUS == 0)) {
+                ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_MIC_FAILURE;
+            }
+
+            /*
+             * XXX: not sure how to deal with this. This should not
+             * be a MIC failure but we should not hand it up. I guess
+             * this is just some form of rx error and that is how we
+             * handle it? For now, just set CRC error flags
+             */
+            if (NRF_CCM->EVENTS_ERROR) {
+                STATS_INC(ble_phy_stats, rx_hw_err);
+                ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+            }
+
+            /*
+             * XXX: This is a total hack work-around for now but I dont
+             * know what else to do. If ENDCRYPT is not set and we are
+             * encrypted we need to not trust this frame and drop it.
+             */
+            if (NRF_CCM->EVENTS_ENDCRYPT == 0) {
+                STATS_INC(ble_phy_stats, rx_hw_err);
+                ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+            }
+        }
+#endif
+    }
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode);
+#endif
+
+    /*
+     * Let's schedule TX now and we will just cancel it after processing RXed
+     * packet if we don't need TX.
+     *
+     * We need this to initiate connection in case AUX_CONNECT_REQ was sent on
+     * LE Coded S8. In this case the time we process RXed packet is roughly the
+     * same as the limit when we need to have TX scheduled (i.e. TIMER0 and PPI
+     * armed) so we may simply miss the slot and set the timer in the past.
+     *
+     * When TX is scheduled in advance, we may event process packet a bit longer
+     * during radio ramp-up - this gives us extra 40 usecs which is more than
+     * enough.
+     */
+
+    /* Schedule TX exactly T_IFS after RX end captured in CC[2] */
+    tx_time = NRF_TIMER0->CC[2] + BLE_LL_IFS;
+    /* Adjust for delay between actual RX end time and EVENT_END */
+    tx_time -= g_ble_phy_t_rxenddelay[ble_hdr->rxinfo.phy_mode];
+    /* Adjust for radio ramp-up */
+    tx_time -= BLE_PHY_T_TXENFAST;
+    /* Adjust for delay between EVENT_READY and actual TX start time */
+    tx_time -= g_ble_phy_t_txdelay[g_ble_phy_data.phy_cur_phy_mode];
+
+    NRF_TIMER0->CC[0] = tx_time;
+    NRF_TIMER0->EVENTS_COMPARE[0] = 0;
+    NRF_PPI->CHENSET = PPI_CHEN_CH20_Msk;
+
+    /*
+     * XXX: Hack warning!
+     *
+     * It may happen (during flash erase) that CPU is stopped for a moment and
+     * TIMER0 already counted past CC[0]. In such case we will be stuck waiting
+     * for TX to start since EVENTS_COMPARE[0] will not happen any time soon.
+     * For now let's set a flag denoting that we are late in RX-TX transition so
+     * ble_phy_tx() will fail - this allows everything to cleanup nicely without
+     * the need for extra handling in many places.
+     *
+     * Note: CC[3] is used only for wfr which we do not need here.
+     */
+    NRF_TIMER0->TASKS_CAPTURE[3] = 1;
+    if (NRF_TIMER0->CC[3] > NRF_TIMER0->CC[0]) {
+        NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk;
+        g_ble_phy_data.phy_transition_late = 1;
+    }
+
+    /*
+     * XXX: This is a horrible ugly hack to deal with the RAM S1 byte
+     * that is not sent over the air but is present here. Simply move the
+     * data pointer to deal with it. Fix this later.
+     */
+    dptr[2] = dptr[1];
+    dptr[1] = dptr[0];
+    rc = ble_ll_rx_end(dptr + 1, ble_hdr);
+    if (rc < 0) {
+        ble_phy_disable();
+    }
+}
+
+static bool
+ble_phy_rx_start_isr(void)
+{
+    int rc;
+    uint32_t state;
+    uint32_t usecs;
+    uint32_t pdu_usecs;
+    uint32_t ticks;
+    struct ble_mbuf_hdr *ble_hdr;
+    uint8_t *dptr;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int adva_offset;
+#endif
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+
+    /* Clear events and clear interrupt */
+    NRF_RADIO->EVENTS_ADDRESS = 0;
+
+    /* Clear wfr timer channels and DISABLED interrupt */
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_DISABLED_Msk | RADIO_INTENCLR_ADDRESS_Msk;
+    NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk;
+
+    /* Initialize the ble mbuf header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    ble_hdr->rxinfo.flags = ble_ll_state_get();
+    ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
+    ble_hdr->rxinfo.handle = 0;
+    ble_hdr->rxinfo.phy = ble_phy_get_cur_phy();
+    ble_hdr->rxinfo.phy_mode = ble_phy_get_cur_rx_phy_mode();
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_hdr->rxinfo.user_data = NULL;
+#endif
+
+    /*
+     * Calculate accurate packets start time (with remainder)
+     *
+     * We may start receiving packet somewhere during preamble in which case
+     * it is possible that actual transmission started before TIMER0 was
+     * running - need to take this into account.
+     */
+    ble_hdr->beg_cputime = g_ble_phy_data.phy_start_cputime;
+
+    usecs = NRF_TIMER0->CC[1];
+    pdu_usecs = ble_phy_mode_pdu_start_off(ble_hdr->rxinfo.phy_mode) +
+                g_ble_phy_t_rxaddrdelay[ble_hdr->rxinfo.phy_mode];
+    if (usecs < pdu_usecs) {
+        g_ble_phy_data.phy_start_cputime--;
+        usecs += 30;
+    }
+    usecs -= pdu_usecs;
+
+    ticks = os_cputime_usecs_to_ticks(usecs);
+    usecs -= os_cputime_ticks_to_usecs(ticks);
+    if (usecs == 31) {
+        usecs = 0;
+        ++ticks;
+    }
+
+    ble_hdr->beg_cputime += ticks;
+    ble_hdr->rem_usecs = usecs;
+
+    /* XXX: I wonder if we always have the 1st byte. If we need to wait for
+     * rx chain delay, it could be 18 usecs from address interrupt. The
+       nrf52 may be able to get here early. */
+    /* Wait to get 1st byte of frame */
+    while (1) {
+        state = NRF_RADIO->STATE;
+        if (NRF_RADIO->EVENTS_BCMATCH != 0) {
+            break;
+        }
+
+        /*
+         * If state is disabled, we should have the BCMATCH. If not,
+         * something is wrong!
+         */
+        if (state == RADIO_STATE_STATE_Disabled) {
+            NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+            NRF_RADIO->SHORTS = 0;
+            return false;
+        }
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /*
+     * If privacy is enabled and received PDU has TxAdd bit set (i.e. random
+     * address) we try to resolve address using AAR.
+     */
+    if (g_ble_phy_data.phy_privacy && (dptr[3] & 0x40)) {
+        /*
+         * AdvA is located at 4th octet in RX buffer (after S0, length an S1
+         * fields). In case of extended advertising PDU we need to add 2 more
+         * octets for extended header.
+         */
+        adva_offset = (dptr[3] & 0x0f) == 0x07 ? 2 : 0;
+        NRF_AAR->ADDRPTR = (uint32_t)(dptr + 3 + adva_offset);
+
+        /* Trigger AAR after last bit of AdvA is received */
+        NRF_RADIO->EVENTS_BCMATCH = 0;
+        NRF_PPI->CHENSET = PPI_CHEN_CH23_Msk;
+        NRF_RADIO->BCC = (BLE_LL_PDU_HDR_LEN + adva_offset + BLE_DEV_ADDR_LEN) * 8 +
+                         g_ble_phy_data.phy_bcc_offset;
+    }
+#endif
+
+    /* Call Link Layer receive start function */
+    rc = ble_ll_rx_start(dptr + 3,
+                         g_ble_phy_data.phy_chan,
+                         &g_ble_phy_data.rxhdr);
+    if (rc >= 0) {
+        /* Set rx started flag and enable rx end ISR */
+        g_ble_phy_data.phy_rx_started = 1;
+        NRF_RADIO->INTENSET = RADIO_INTENSET_END_Msk;
+    } else {
+        /* Disable PHY */
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, rx_aborts);
+    }
+
+    /* Count rx starts */
+    STATS_INC(ble_phy_stats, rx_starts);
+
+    return true;
+}
+
+static void
+ble_phy_isr(void)
+{
+    uint32_t irq_en;
+
+    os_trace_isr_enter();
+
+    /* Read irq register to determine which interrupts are enabled */
+    irq_en = NRF_RADIO->INTENCLR;
+
+    /*
+     * NOTE: order of checking is important! Possible, if things get delayed,
+     * we have both an ADDRESS and DISABLED interrupt in rx state. If we get
+     * an address, we disable the DISABLED interrupt.
+     */
+
+    /* We get this if we have started to receive a frame */
+    if ((irq_en & RADIO_INTENCLR_ADDRESS_Msk) && NRF_RADIO->EVENTS_ADDRESS) {
+        /*
+         * wfr timer is calculated to expire at the exact time we should start
+         * receiving a packet (with 1 usec precision) so it is possible  it will
+         * fire at the same time as EVENT_ADDRESS. If this happens, radio will
+         * be disabled while we are waiting for EVENT_BCCMATCH after 1st byte
+         * of payload is received and ble_phy_rx_start_isr() will fail. In this
+         * case we should not clear DISABLED irq mask so it will be handled as
+         * regular radio disabled event below. In other case radio was disabled
+         * on purpose and there's nothing more to handle so we can clear mask.
+         */
+        if (ble_phy_rx_start_isr()) {
+            irq_en &= ~RADIO_INTENCLR_DISABLED_Msk;
+        }
+    }
+
+    /* Check for disabled event. This only happens for transmits now */
+    if ((irq_en & RADIO_INTENCLR_DISABLED_Msk) && NRF_RADIO->EVENTS_DISABLED) {
+        if (g_ble_phy_data.phy_state == BLE_PHY_STATE_RX) {
+            NRF_RADIO->EVENTS_DISABLED = 0;
+            ble_ll_wfr_timer_exp(NULL);
+        } else if (g_ble_phy_data.phy_state == BLE_PHY_STATE_IDLE) {
+            assert(0);
+        } else {
+            ble_phy_tx_end_isr();
+        }
+    }
+
+    /* Receive packet end (we dont enable this for transmit) */
+    if ((irq_en & RADIO_INTENCLR_END_Msk) && NRF_RADIO->EVENTS_END) {
+        ble_phy_rx_end_isr();
+    }
+
+    g_ble_phy_data.phy_transition_late = 0;
+
+    /* Ensures IRQ is cleared */
+    irq_en = NRF_RADIO->SHORTS;
+
+    /* Count # of interrupts */
+    STATS_INC(ble_phy_stats, phy_isrs);
+
+    os_trace_isr_exit();
+}
+
+#if MYNEWT_VAL(BLE_PHY_DBG_TIME_TXRXEN_READY_PIN) >= 0 || \
+        MYNEWT_VAL(BLE_PHY_DBG_TIME_ADDRESS_END_PIN) >= 0 || \
+        MYNEWT_VAL(BLE_PHY_DBG_TIME_WFR_PIN) >= 0
+static inline void
+ble_phy_dbg_time_setup_gpiote(int index, int pin)
+{
+    NRF_GPIO_Type *port;
+
+#if NRF52840_XXAA
+    port = pin > 31 ? NRF_P1 : NRF_P0;
+    pin &= 0x1f;
+#else
+    port = NRF_P0;
+#endif
+
+    /* Configure GPIO directly to avoid dependency to hal_gpio (for porting) */
+    port->DIRSET = (1 << pin);
+    port->OUTCLR = (1 << pin);
+
+    NRF_GPIOTE->CONFIG[index] =
+                        (GPIOTE_CONFIG_MODE_Task << GPIOTE_CONFIG_MODE_Pos) |
+                        ((pin & 0x1F) << GPIOTE_CONFIG_PSEL_Pos) |
+#if NRF52840_XXAA
+                        ((port == NRF_P1) << GPIOTE_CONFIG_PORT_Pos);
+#else
+                        0;
+#endif
+}
+#endif
+
+static void
+ble_phy_dbg_time_setup(void)
+{
+    int gpiote_idx __attribute__((unused)) = 8;
+
+    /*
+     * We setup GPIOTE starting from last configuration index to minimize risk
+     * of conflict with GPIO setup via hal. It's not great solution, but since
+     * this is just debugging code we can live with this.
+     */
+
+#if MYNEWT_VAL(BLE_PHY_DBG_TIME_TXRXEN_READY_PIN) >= 0
+    ble_phy_dbg_time_setup_gpiote(--gpiote_idx,
+                              MYNEWT_VAL(BLE_PHY_DBG_TIME_TXRXEN_READY_PIN));
+
+    NRF_PPI->CH[17].EEP = (uint32_t)&(NRF_RADIO->EVENTS_READY);
+    NRF_PPI->CH[17].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_CLR[gpiote_idx]);
+    NRF_PPI->CHENSET = PPI_CHEN_CH17_Msk;
+
+    /* CH[20] and PPI CH[21] are on to trigger TASKS_TXEN or TASKS_RXEN */
+    NRF_PPI->FORK[20].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_SET[gpiote_idx]);
+    NRF_PPI->FORK[21].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_SET[gpiote_idx]);
+#endif
+
+#if MYNEWT_VAL(BLE_PHY_DBG_TIME_ADDRESS_END_PIN) >= 0
+    ble_phy_dbg_time_setup_gpiote(--gpiote_idx,
+                              MYNEWT_VAL(BLE_PHY_DBG_TIME_ADDRESS_END_PIN));
+
+    /* CH[26] and CH[27] are always on for EVENT_ADDRESS and EVENT_END */
+    NRF_PPI->FORK[26].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_SET[gpiote_idx]);
+    NRF_PPI->FORK[27].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_CLR[gpiote_idx]);
+#endif
+
+#if MYNEWT_VAL(BLE_PHY_DBG_TIME_WFR_PIN) >= 0
+    ble_phy_dbg_time_setup_gpiote(--gpiote_idx,
+                              MYNEWT_VAL(BLE_PHY_DBG_TIME_WFR_PIN));
+
+#if NRF52840_XXAA
+    NRF_PPI->CH[18].EEP = (uint32_t)&(NRF_RADIO->EVENTS_RXREADY);
+#else
+    NRF_PPI->CH[18].EEP = (uint32_t)&(NRF_RADIO->EVENTS_READY);
+#endif
+    NRF_PPI->CH[18].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_SET[gpiote_idx]);
+    NRF_PPI->CH[19].EEP = (uint32_t)&(NRF_RADIO->EVENTS_DISABLED);
+    NRF_PPI->CH[19].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_CLR[gpiote_idx]);
+    NRF_PPI->CHENSET = PPI_CHEN_CH18_Msk | PPI_CHEN_CH19_Msk;
+
+    /* CH[4] and CH[5] are always on for wfr */
+    NRF_PPI->FORK[4].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_CLR[gpiote_idx]);
+    NRF_PPI->FORK[5].TEP = (uint32_t)&(NRF_GPIOTE->TASKS_CLR[gpiote_idx]);
+#endif
+}
+
+/**
+ * ble phy init
+ *
+ * Initialize the PHY.
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+int
+ble_phy_init(void)
+{
+    int rc;
+
+    /* Default phy to use is 1M */
+    g_ble_phy_data.phy_cur_phy_mode = BLE_PHY_MODE_1M;
+    g_ble_phy_data.phy_tx_phy_mode = BLE_PHY_MODE_1M;
+    g_ble_phy_data.phy_rx_phy_mode = BLE_PHY_MODE_1M;
+
+    g_ble_phy_data.rx_pwr_compensation = 0;
+
+    /* Set phy channel to an invalid channel so first set channel works */
+    g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
+
+    /* Toggle peripheral power to reset (just in case) */
+    NRF_RADIO->POWER = 0;
+    NRF_RADIO->POWER = 1;
+
+    /* Disable all interrupts */
+    NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+
+    /* Set configuration registers */
+    NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_1Mbit;
+    NRF_RADIO->PCNF0 = NRF_PCNF0;
+
+    /* XXX: should maxlen be 251 for encryption? */
+    NRF_RADIO->PCNF1 = NRF_MAXLEN |
+                       (RADIO_PCNF1_ENDIAN_Little <<  RADIO_PCNF1_ENDIAN_Pos) |
+                       (NRF_BALEN << RADIO_PCNF1_BALEN_Pos) |
+                       RADIO_PCNF1_WHITEEN_Msk;
+
+    /* Enable radio fast ramp-up */
+    NRF_RADIO->MODECNF0 |= (RADIO_MODECNF0_RU_Fast << RADIO_MODECNF0_RU_Pos) &
+                            RADIO_MODECNF0_RU_Msk;
+
+    /* Set logical address 1 for TX and RX */
+    NRF_RADIO->TXADDRESS  = 0;
+    NRF_RADIO->RXADDRESSES  = (1 << 0);
+
+    /* Configure the CRC registers */
+    NRF_RADIO->CRCCNF = (RADIO_CRCCNF_SKIPADDR_Skip << RADIO_CRCCNF_SKIPADDR_Pos) | RADIO_CRCCNF_LEN_Three;
+
+    /* Configure BLE poly */
+    NRF_RADIO->CRCPOLY = 0x0000065B;
+
+    /* Configure IFS */
+    NRF_RADIO->TIFS = BLE_LL_IFS;
+
+    /* Captures tx/rx start in timer0 cc 1 and tx/rx end in timer0 cc 2 */
+    NRF_PPI->CHENSET = PPI_CHEN_CH26_Msk | PPI_CHEN_CH27_Msk;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    NRF_CCM->INTENCLR = 0xffffffff;
+    NRF_CCM->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
+    NRF_CCM->EVENTS_ERROR = 0;
+    memset(g_nrf_encrypt_scratchpad, 0, sizeof(g_nrf_encrypt_scratchpad));
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    g_ble_phy_data.phy_aar_scratch = 0;
+    NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+    NRF_AAR->INTENCLR = 0xffffffff;
+    NRF_AAR->EVENTS_END = 0;
+    NRF_AAR->EVENTS_RESOLVED = 0;
+    NRF_AAR->EVENTS_NOTRESOLVED = 0;
+    NRF_AAR->NIRK = 0;
+#endif
+
+    /* TIMER0 setup for PHY when using RTC */
+    NRF_TIMER0->TASKS_STOP = 1;
+    NRF_TIMER0->TASKS_SHUTDOWN = 1;
+    NRF_TIMER0->BITMODE = 3;    /* 32-bit timer */
+    NRF_TIMER0->MODE = 0;       /* Timer mode */
+    NRF_TIMER0->PRESCALER = 4;  /* gives us 1 MHz */
+
+    /*
+     * PPI setup.
+     * Channel 4: Captures TIMER0 in CC[3] when EVENTS_ADDRESS occurs. Used
+     *            to cancel the wait for response timer.
+     * Channel 5: TIMER0 CC[3] to TASKS_DISABLE on radio. This is the wait
+     *            for response timer.
+     */
+    NRF_PPI->CH[4].EEP = (uint32_t)&(NRF_RADIO->EVENTS_ADDRESS);
+    NRF_PPI->CH[4].TEP = (uint32_t)&(NRF_TIMER0->TASKS_CAPTURE[3]);
+    NRF_PPI->CH[5].EEP = (uint32_t)&(NRF_TIMER0->EVENTS_COMPARE[3]);
+    NRF_PPI->CH[5].TEP = (uint32_t)&(NRF_RADIO->TASKS_DISABLE);
+
+    /* Set isr in vector table and enable interrupt */
+#ifndef RIOT_VERSION
+    NVIC_SetPriority(RADIO_IRQn, 0);
+#endif
+#if MYNEWT
+    NVIC_SetVector(RADIO_IRQn, (uint32_t)ble_phy_isr);
+#else
+    ble_npl_hw_set_isr(RADIO_IRQn, ble_phy_isr);
+#endif
+    NVIC_EnableIRQ(RADIO_IRQn);
+
+    /* Register phy statistics */
+    if (!g_ble_phy_data.phy_stats_initialized) {
+        rc = stats_init_and_reg(STATS_HDR(ble_phy_stats),
+                                STATS_SIZE_INIT_PARMS(ble_phy_stats,
+                                                      STATS_SIZE_32),
+                                STATS_NAME_INIT_PARMS(ble_phy_stats),
+                                "ble_phy");
+        assert(rc == 0);
+
+        g_ble_phy_data.phy_stats_initialized  = 1;
+    }
+
+    ble_phy_dbg_time_setup();
+
+    return 0;
+}
+
+/**
+ * Puts the phy into receive mode.
+ *
+ * @return int 0: success; BLE Phy error code otherwise
+ */
+int
+ble_phy_rx(void)
+{
+    /*
+     * Check radio state.
+     *
+     * In case radio is now disabling we'll wait for it to finish, but if for
+     * any reason it's just in idle state we proceed with RX as usual since
+     * nRF52 radio can ramp-up from idle state as well.
+     *
+     * Note that TX and RX states values are the same except for 3rd bit so we
+     * can make a shortcut here when checking for idle state.
+     */
+    nrf_wait_disabled();
+    if ((NRF_RADIO->STATE != RADIO_STATE_STATE_Disabled) &&
+            ((NRF_RADIO->STATE & 0x07) != RADIO_STATE_STATE_RxIdle)) {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, radio_state_errs);
+        return BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    /* Make sure all interrupts are disabled */
+    NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+
+    /* Clear events prior to enabling receive */
+    NRF_RADIO->EVENTS_END = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+
+    /* Setup for rx */
+    ble_phy_rx_xcvr_setup();
+
+    /* PPI to start radio automatically shall be set here */
+    assert(NRF_PPI->CHEN & PPI_CHEN_CH21_Msk);
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * Called to enable encryption at the PHY. Note that this state will persist
+ * in the PHY; in other words, if you call this function you have to call
+ * disable so that future PHY transmits/receives will not be encrypted.
+ *
+ * @param pkt_counter
+ * @param iv
+ * @param key
+ * @param is_master
+ */
+void
+ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+                       uint8_t is_master)
+{
+    memcpy(g_nrf_ccm_data.key, key, 16);
+    g_nrf_ccm_data.pkt_counter = pkt_counter;
+    memcpy(g_nrf_ccm_data.iv, iv, 8);
+    g_nrf_ccm_data.dir_bit = is_master;
+    g_ble_phy_data.phy_encrypted = 1;
+    /* Enable the module (AAR cannot be on while CCM on) */
+    NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Disabled;
+    NRF_CCM->ENABLE = CCM_ENABLE_ENABLE_Enabled;
+}
+
+void
+ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir)
+{
+    g_nrf_ccm_data.pkt_counter = pkt_counter;
+    g_nrf_ccm_data.dir_bit = dir;
+}
+
+void
+ble_phy_encrypt_disable(void)
+{
+    NRF_PPI->CHENCLR = PPI_CHEN_CH25_Msk;
+    NRF_CCM->TASKS_STOP = 1;
+    NRF_CCM->EVENTS_ERROR = 0;
+    NRF_CCM->ENABLE = CCM_ENABLE_ENABLE_Disabled;
+
+    g_ble_phy_data.phy_encrypted = 0;
+}
+#endif
+
+void
+ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg)
+{
+    /* Set transmit end callback and arg */
+    g_ble_phy_data.txend_cb = txend_cb;
+    g_ble_phy_data.txend_arg = arg;
+}
+
+/**
+ * Called to set the start time of a transmission.
+ *
+ * This function is called to set the start time when we are not going from
+ * rx to tx automatically.
+ *
+ * NOTE: care must be taken when calling this function. The channel should
+ * already be set.
+ *
+ * @param cputime   This is the tick at which the 1st bit of the preamble
+ *                  should be transmitted
+ * @param rem_usecs This is used only when the underlying timing uses a 32.768
+ *                  kHz crystal. It is the # of usecs from the cputime tick
+ *                  at which the first bit of the preamble should be
+ *                  transmitted.
+ * @return int
+ */
+int
+ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    int rc;
+
+    ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_TX, cputime, rem_usecs);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode);
+#endif
+
+    /* XXX: This should not be necessary, but paranoia is good! */
+    /* Clear timer0 compare to RXEN since we are transmitting */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
+
+    if (ble_phy_set_start_time(cputime, rem_usecs, true) != 0) {
+        STATS_INC(ble_phy_stats, tx_late);
+        ble_phy_disable();
+        rc = BLE_PHY_ERR_TX_LATE;
+    } else {
+        /* Enable PPI to automatically start TXEN */
+        NRF_PPI->CHENSET = PPI_CHEN_CH20_Msk;
+        rc = 0;
+    }
+    return rc;
+}
+
+/**
+ * Called to set the start time of a reception
+ *
+ * This function acts a bit differently than transmit. If we are late getting
+ * here we will still attempt to receive.
+ *
+ * NOTE: care must be taken when calling this function. The channel should
+ * already be set.
+ *
+ * @param cputime
+ *
+ * @return int
+ */
+int
+ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    bool late = false;
+    int rc = 0;
+
+    ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_RX, cputime, rem_usecs);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_rx_phy_mode);
+#endif
+
+    /* XXX: This should not be necessary, but paranoia is good! */
+    /* Clear timer0 compare to TXEN since we are transmitting */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk;
+
+    if (ble_phy_set_start_time(cputime, rem_usecs, false) != 0) {
+        STATS_INC(ble_phy_stats, rx_late);
+
+        /* We're late so let's just try to start RX as soon as possible */
+        ble_phy_set_start_now();
+
+        late = true;
+    }
+
+    /* Enable PPI to automatically start RXEN */
+    NRF_PPI->CHENSET = PPI_CHEN_CH21_Msk;
+
+    /* Start rx */
+    rc = ble_phy_rx();
+
+    /*
+     * If we enabled receiver but were late, let's return proper error code so
+     * caller can handle this.
+     */
+    if (!rc && late) {
+        rc = BLE_PHY_ERR_RX_LATE;
+    }
+
+    return rc;
+}
+
+int
+ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
+{
+    int rc;
+    uint8_t *dptr;
+    uint8_t *pktptr;
+    uint8_t payload_len;
+    uint8_t hdr_byte;
+    uint32_t state;
+    uint32_t shortcuts;
+
+    if (g_ble_phy_data.phy_transition_late) {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, tx_late);
+        return BLE_PHY_ERR_TX_LATE;
+    }
+
+    /*
+     * This check is to make sure that the radio is not in a state where
+     * it is moving to disabled state. If so, let it get there.
+     */
+    nrf_wait_disabled();
+
+    /*
+     * XXX: Although we may not have to do this here, I clear all the PPI
+     * that should not be used when transmitting. Some of them are only enabled
+     * if encryption and/or privacy is on, but I dont care. Better to be
+     * paranoid, and if you are going to clear one, might as well clear them
+     * all.
+     */
+    NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH23_Msk |
+                       PPI_CHEN_CH25_Msk;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
+        pktptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+        NRF_CCM->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
+        NRF_CCM->INPTR = (uint32_t)dptr;
+        NRF_CCM->OUTPTR = (uint32_t)pktptr;
+        NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+        NRF_CCM->EVENTS_ERROR = 0;
+        NRF_CCM->MODE = CCM_MODE_LENGTH_Msk | ble_phy_get_ccm_datarate();
+        NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+    } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+#endif
+        dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+        pktptr = dptr;
+    }
+#else
+    dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+    pktptr = dptr;
+#endif
+
+    /* Set PDU payload */
+    payload_len = pducb(&dptr[3], pducb_arg, &hdr_byte);
+
+    /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
+    dptr[0] = hdr_byte;
+    dptr[1] = payload_len;
+    dptr[2] = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    /* Start key-stream generation and encryption (via short) */
+    if (g_ble_phy_data.phy_encrypted) {
+        NRF_CCM->TASKS_KSGEN = 1;
+    }
+#endif
+
+    NRF_RADIO->PACKETPTR = (uint32_t)pktptr;
+
+    /* Clear the ready, end and disabled events */
+    NRF_RADIO->EVENTS_READY = 0;
+    NRF_RADIO->EVENTS_END = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+
+    /* Enable shortcuts for transmit start/end. */
+    shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
+    NRF_RADIO->SHORTS = shortcuts;
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    /* Set the PHY transition */
+    g_ble_phy_data.phy_transition = end_trans;
+
+    /* Set transmitted payload length */
+    g_ble_phy_data.phy_tx_pyld_len = payload_len;
+
+    /* If we already started transmitting, abort it! */
+    state = NRF_RADIO->STATE;
+    if (state != RADIO_STATE_STATE_Tx) {
+        /* Set phy state to transmitting and count packet statistics */
+        g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
+        STATS_INC(ble_phy_stats, tx_good);
+        STATS_INCN(ble_phy_stats, tx_bytes, payload_len + BLE_LL_PDU_HDR_LEN);
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, tx_late);
+        rc = BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    return rc;
+}
+
+/**
+ * ble phy txpwr set
+ *
+ * Set the transmit output power (in dBm).
+ *
+ * NOTE: If the output power specified is within the BLE limits but outside
+ * the chip limits, we "rail" the power level so we dont exceed the min/max
+ * chip values.
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int 0: success; anything else is an error
+ */
+int
+ble_phy_txpwr_set(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    dbm = ble_phy_txpower_round(dbm);
+
+    NRF_RADIO->TXPOWER = dbm;
+    g_ble_phy_data.phy_txpwr_dbm = dbm;
+
+    return 0;
+}
+
+/**
+ * ble phy txpwr round
+ *
+ * Get the rounded transmit output power (in dBm).
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int Rounded power in dBm
+ */
+int ble_phy_txpower_round(int dbm)
+{
+    /* TODO this should be per nRF52XXX */
+
+    /* "Rail" power level if outside supported range */
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_0dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_0dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm;
+    }
+
+    return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm;
+}
+
+/**
+ * ble phy set access addr
+ *
+ * Set access address.
+ *
+ * @param access_addr Access address
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+static int
+ble_phy_set_access_addr(uint32_t access_addr)
+{
+    NRF_RADIO->BASE0 = (access_addr << 8);
+    NRF_RADIO->PREFIX0 = (NRF_RADIO->PREFIX0 & 0xFFFFFF00) | (access_addr >> 24);
+
+    g_ble_phy_data.phy_access_address = access_addr;
+
+    ble_phy_apply_errata_102_106_107();
+
+    return 0;
+}
+
+/**
+ * ble phy txpwr get
+ *
+ * Get the transmit power.
+ *
+ * @return int  The current PHY transmit power, in dBm
+ */
+int
+ble_phy_txpwr_get(void)
+{
+    return g_ble_phy_data.phy_txpwr_dbm;
+}
+
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
+/**
+ * ble phy setchan
+ *
+ * Sets the logical frequency of the transceiver. The input parameter is the
+ * BLE channel index (0 to 39, inclusive). The NRF frequency register works like
+ * this: logical frequency = 2400 + FREQ (MHz).
+ *
+ * Thus, to get a logical frequency of 2402 MHz, you would program the
+ * FREQUENCY register to 2.
+ *
+ * @param chan This is the Data Channel Index or Advertising Channel index
+ *
+ * @return int 0: success; PHY error code otherwise
+ */
+int
+ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
+{
+    assert(chan < BLE_PHY_NUM_CHANS);
+
+    /* Check for valid channel range */
+    if (chan >= BLE_PHY_NUM_CHANS) {
+        return BLE_PHY_ERR_INV_PARAM;
+    }
+
+    /* Set current access address */
+    ble_phy_set_access_addr(access_addr);
+
+    /* Configure crcinit */
+    NRF_RADIO->CRCINIT = crcinit;
+
+    /* Set the frequency and the data whitening initial value */
+    g_ble_phy_data.phy_chan = chan;
+    NRF_RADIO->FREQUENCY = g_ble_phy_chan_freq[chan];
+    NRF_RADIO->DATAWHITEIV = chan;
+
+    return 0;
+}
+
+/**
+ * Stop the timer used to count microseconds when using RTC for cputime
+ */
+static void
+ble_phy_stop_usec_timer(void)
+{
+    NRF_TIMER0->TASKS_STOP = 1;
+    NRF_TIMER0->TASKS_SHUTDOWN = 1;
+    NRF_RTC0->EVTENCLR = RTC_EVTENSET_COMPARE0_Msk;
+}
+
+/**
+ * ble phy disable irq and ppi
+ *
+ * This routine is to be called when reception was stopped due to either a
+ * wait for response timeout or a packet being received and the phy is to be
+ * restarted in receive mode. Generally, the disable routine is called to stop
+ * the phy.
+ */
+static void
+ble_phy_disable_irq_and_ppi(void)
+{
+    NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+    NRF_RADIO->SHORTS = 0;
+    NRF_RADIO->TASKS_DISABLE = 1;
+    NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH20_Msk |
+          PPI_CHEN_CH21_Msk | PPI_CHEN_CH23_Msk |
+          PPI_CHEN_CH25_Msk | PPI_CHEN_CH31_Msk;
+    NVIC_ClearPendingIRQ(RADIO_IRQn);
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+}
+
+void
+ble_phy_restart_rx(void)
+{
+    ble_phy_stop_usec_timer();
+    ble_phy_disable_irq_and_ppi();
+
+    ble_phy_set_start_now();
+    /* Enable PPI to automatically start RXEN */
+    NRF_PPI->CHENSET = PPI_CHEN_CH21_Msk;
+
+    ble_phy_rx();
+}
+
+/**
+ * ble phy disable
+ *
+ * Disables the PHY. This should be called when an event is over. It stops
+ * the usec timer (if used), disables interrupts, disables the RADIO, disables
+ * PPI and sets state to idle.
+ */
+void
+ble_phy_disable(void)
+{
+    ble_phy_trace_void(BLE_PHY_TRACE_ID_DISABLE);
+
+    ble_phy_stop_usec_timer();
+    ble_phy_disable_irq_and_ppi();
+}
+
+/* Gets the current access address */
+uint32_t ble_phy_access_addr_get(void)
+{
+    return g_ble_phy_data.phy_access_address;
+}
+
+/**
+ * Return the phy state
+ *
+ * @return int The current PHY state.
+ */
+int
+ble_phy_state_get(void)
+{
+    return g_ble_phy_data.phy_state;
+}
+
+/**
+ * Called to see if a reception has started
+ *
+ * @return int
+ */
+int
+ble_phy_rx_started(void)
+{
+    return g_ble_phy_data.phy_rx_started;
+}
+
+/**
+ * Return the transceiver state
+ *
+ * @return int transceiver state.
+ */
+uint8_t
+ble_phy_xcvr_state_get(void)
+{
+    uint32_t state;
+    state = NRF_RADIO->STATE;
+    return (uint8_t)state;
+}
+
+/**
+ * Called to return the maximum data pdu payload length supported by the
+ * phy. For this chip, if encryption is enabled, the maximum payload is 27
+ * bytes.
+ *
+ * @return uint8_t Maximum data channel PDU payload size supported
+ */
+uint8_t
+ble_phy_max_data_pdu_pyld(void)
+{
+    return BLE_LL_DATA_PDU_MAX_PYLD;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+void
+ble_phy_resolv_list_enable(void)
+{
+    NRF_AAR->NIRK = (uint32_t)g_nrf_num_irks;
+    g_ble_phy_data.phy_privacy = 1;
+}
+
+void
+ble_phy_resolv_list_disable(void)
+{
+    g_ble_phy_data.phy_privacy = 0;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+void ble_phy_enable_dtm(void)
+{
+    /* When DTM is enabled we need to disable whitening as per
+     * Bluetooth v5.0 Vol 6. Part F. 4.1.1
+     */
+    NRF_RADIO->PCNF1 &= ~RADIO_PCNF1_WHITEEN_Msk;
+}
+
+void ble_phy_disable_dtm(void)
+{
+    /* Enable whitening */
+    NRF_RADIO->PCNF1 |= RADIO_PCNF1_WHITEEN_Msk;
+}
+#endif
+
+void
+ble_phy_rfclk_enable(void)
+{
+#if MYNEWT
+    nrf52_clock_hfxo_request();
+#else
+    NRF_CLOCK->TASKS_HFCLKSTART = 1;
+#endif
+}
+
+void
+ble_phy_rfclk_disable(void)
+{
+#if MYNEWT
+    nrf52_clock_hfxo_release();
+#else
+    NRF_CLOCK->TASKS_HFCLKSTOP = 1;
+#endif
+}
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_phy_trace.c b/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_phy_trace.c
new file mode 100644
index 00000000..93b2eb32
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf52/src/ble_phy_trace.c
@@ -0,0 +1,44 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#include "os/os_trace_api.h"
+
+#if MYNEWT_VAL(BLE_PHY_SYSVIEW)
+
+static os_trace_module_t g_ble_phy_trace_mod;
+uint32_t ble_phy_trace_off;
+
+static void
+ble_phy_trace_module_send_desc(void)
+{
+    os_trace_module_desc(&g_ble_phy_trace_mod, "0 phy_set_tx cputime=%u usecs=%u");
+    os_trace_module_desc(&g_ble_phy_trace_mod, "1 phy_set_rx cputime=%u usecs=%u");
+    os_trace_module_desc(&g_ble_phy_trace_mod, "2 phy_disable");
+}
+
+void
+ble_phy_trace_init(void)
+{
+    ble_phy_trace_off =
+            os_trace_module_register(&g_ble_phy_trace_mod, "ble_phy", 3,
+                                     ble_phy_trace_module_send_desc);
+}
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/drivers/nrf52/syscfg.yml b/src/libs/mynewt-nimble/nimble/drivers/nrf52/syscfg.yml
new file mode 100644
index 00000000..ce512372
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/drivers/nrf52/syscfg.yml
@@ -0,0 +1,75 @@
+# 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_PHY_SYSVIEW:
+        description: >
+            Enable SystemView tracing module for radio driver.
+        value: 0
+
+    BLE_PHY_CODED_RX_IFS_EXTRA_MARGIN:
+        description: >
+            This defines additional margin for T_IFS tolerance while in
+            RX on coded phy to allow maintaining connections with some
+            controllers that exceed proper T_IFS (150 usecs) by more
+            than allowed 2 usecs.
+            This value shall be only used for debugging purposes. It is
+            strongly recommended to keep this settings at default value
+            to ensure compliance with specification.
+        value: 0
+    BLE_PHY_DBG_TIME_TXRXEN_READY_PIN:
+        description: >
+            When set to proper GPIO pin number, this pin will be set
+            to high state when radio is enabled using PPI channels
+            20 or 21 and back to low state on radio EVENTS_READY.
+            This can be used to measure radio ram-up time.
+        value: -1
+
+    BLE_PHY_DBG_TIME_ADDRESS_END_PIN:
+        description: >
+            When set to proper GPIO pin number, this pin will be set
+            to high state on radio EVENTS_ADDRESS and back to low state
+            on radio EVENTS_END.
+            This can be used to measure radio pipeline delays.
+        value: -1
+
+    BLE_PHY_DBG_TIME_WFR_PIN:
+        description: >
+            When set to proper GPIO pin number, this pin will be set
+            to high state on radio EVENTS_RXREADY and back to low
+            state when wfr timer expires.
+            This can be used to check if wfr is calculated properly.
+        value: -1
+
+    BLE_PHY_NRF52840_ERRATA_164:
+        description: >
+            Enable workaround for anomaly 164 found in nRF52840.
+            "[164] RADIO: Low selectivity in long range mode"
+            This shall be only enabled for:
+            - nRF52840 Engineering A
+        value: 0
+
+    BLE_PHY_NRF52840_ERRATA_191:
+        description: >
+            Enable workaround for anomaly 191 found in nRF52840.
+            "[191] RADIO: High packet error rate in BLE Long Range mode"
+            This shall be only enabled for:
+            - nRF52840 Engineering B
+            - nRF52840 Engineering C
+            - nRF52840 Rev 1 (final silicon)
+        value: 1
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:
diff --git a/src/libs/mynewt-nimble/nimble/include/nimble/ble.h b/src/libs/mynewt-nimble/nimble/include/nimble/ble.h
new file mode 100644
index 00000000..3fc2902e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/include/nimble/ble.h
@@ -0,0 +1,304 @@
+/*
+ * 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_
+#define H_BLE_
+
+#include <inttypes.h>
+#include <string.h>
+#include "syscfg/syscfg.h"
+#include "os/os.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* The number of advertising instances */
+#define BLE_ADV_INSTANCES    (MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES) + 1)
+
+/* BLE encryption block definitions */
+#define BLE_ENC_BLOCK_SIZE       (16)
+
+/* 4 byte header + 251 byte payload. */
+#define BLE_ACL_MAX_PKT_SIZE     255
+
+struct ble_encryption_block
+{
+    uint8_t     key[BLE_ENC_BLOCK_SIZE];
+    uint8_t     plain_text[BLE_ENC_BLOCK_SIZE];
+    uint8_t     cipher_text[BLE_ENC_BLOCK_SIZE];
+};
+
+/*
+ * BLE MBUF structure:
+ *
+ * The BLE mbuf structure is as follows. Note that this structure applies to
+ * the packet header mbuf (not mbufs that are part of a "packet chain"):
+ *      struct os_mbuf          (16)
+ *      struct os_mbuf_pkthdr   (8)
+ *      struct ble_mbuf_hdr     (8)
+ *      Data buffer             (payload size, in bytes)
+ *
+ * The BLE mbuf header contains the following:
+ *  flags: bitfield with the following values
+ *      0x01:   Set if there was a match on the whitelist
+ *      0x02:   Set if a connect request was transmitted upon receiving pdu
+ *      0x04:   Set the first time we transmit the PDU (used to detect retry).
+ *  channel: The logical BLE channel PHY channel # (0 - 39)
+ *  crcok: flag denoting CRC check passed (1) or failed (0).
+ *  rssi: RSSI, in dBm.
+ */
+struct ble_mbuf_hdr_rxinfo
+{
+    uint16_t flags;
+    uint8_t channel;
+    uint8_t handle;
+    int8_t  rssi;
+    /* XXX: we could just use single phy_mode field */
+    int8_t  phy;
+    uint8_t phy_mode;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int8_t  rpa_index;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    void *user_data;
+#endif
+};
+
+/* Flag definitions for rxinfo  */
+#define BLE_MBUF_HDR_F_IGNORED          (0x8000)
+#define BLE_MBUF_HDR_F_SCAN_REQ_TXD     (0x4000)
+#define BLE_MBUF_HDR_F_INITA_RESOLVED   (0x2000)
+#define BLE_MBUF_HDR_F_TARGETA_RESOLVED (0x2000)
+#define BLE_MBUF_HDR_F_EXT_ADV_SEC      (0x1000)
+#define BLE_MBUF_HDR_F_EXT_ADV          (0x0800)
+#define BLE_MBUF_HDR_F_RESOLVED         (0x0400)
+#define BLE_MBUF_HDR_F_AUX_PTR_WAIT     (0x0200)
+#define BLE_MBUF_HDR_F_AUX_INVALID      (0x0100)
+#define BLE_MBUF_HDR_F_CRC_OK           (0x0080)
+#define BLE_MBUF_HDR_F_DEVMATCH         (0x0040)
+#define BLE_MBUF_HDR_F_MIC_FAILURE      (0x0020)
+#define BLE_MBUF_HDR_F_SCAN_RSP_TXD     (0x0010)
+#define BLE_MBUF_HDR_F_SCAN_RSP_RXD     (0x0008)
+#define BLE_MBUF_HDR_F_RXSTATE_MASK     (0x0007)
+
+/* Transmit info. NOTE: no flags defined */
+struct ble_mbuf_hdr_txinfo
+{
+    uint8_t flags;
+    uint8_t offset;
+    uint8_t pyld_len;
+    uint8_t hdr_byte;
+};
+
+struct ble_mbuf_hdr
+{
+    union {
+        struct ble_mbuf_hdr_rxinfo rxinfo;
+        struct ble_mbuf_hdr_txinfo txinfo;
+    };
+    uint32_t beg_cputime;
+    uint32_t rem_usecs;
+};
+
+#define BLE_MBUF_HDR_IGNORED(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_IGNORED))
+
+#define BLE_MBUF_HDR_SCAN_REQ_TXD(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_SCAN_REQ_TXD))
+
+#define BLE_MBUF_HDR_EXT_ADV_SEC(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_EXT_ADV_SEC))
+
+#define BLE_MBUF_HDR_EXT_ADV(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_EXT_ADV))
+
+#define BLE_MBUF_HDR_DEVMATCH(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_DEVMATCH))
+
+#define BLE_MBUF_HDR_SCAN_RSP_RXD(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_SCAN_RSP_RXD))
+
+#define BLE_MBUF_HDR_AUX_INVALID(hdr) \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_AUX_INVALID))
+
+#define BLE_MBUF_HDR_WAIT_AUX(hdr)      \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_AUX_PTR_WAIT))
+
+#define BLE_MBUF_HDR_CRC_OK(hdr)        \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_CRC_OK))
+
+#define BLE_MBUF_HDR_MIC_FAILURE(hdr)   \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_MIC_FAILURE))
+
+#define BLE_MBUF_HDR_RESOLVED(hdr)      \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_RESOLVED))
+
+#define BLE_MBUF_HDR_INITA_RESOLVED(hdr)      \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_INITA_RESOLVED))
+
+#define BLE_MBUF_HDR_TARGETA_RESOLVED(hdr)      \
+    (!!((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_TARGETA_RESOLVED))
+
+#define BLE_MBUF_HDR_RX_STATE(hdr)      \
+    ((uint8_t)((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_RXSTATE_MASK))
+
+#define BLE_MBUF_HDR_PTR(om)            \
+    (struct ble_mbuf_hdr *)((uint8_t *)om + sizeof(struct os_mbuf) + \
+                            sizeof(struct os_mbuf_pkthdr))
+
+/* BLE mbuf overhead per packet header mbuf */
+#define BLE_MBUF_PKTHDR_OVERHEAD        \
+    (sizeof(struct os_mbuf_pkthdr) + sizeof(struct ble_mbuf_hdr))
+
+#define BLE_MBUF_MEMBLOCK_OVERHEAD      \
+    (sizeof(struct os_mbuf) + BLE_MBUF_PKTHDR_OVERHEAD)
+
+/* Length of host user header.  Only contains the peer's connection handle. */
+#define BLE_MBUF_HS_HDR_LEN     (2)
+
+#define BLE_DEV_ADDR_LEN        (6)
+extern uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
+extern uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
+
+/* BLE Error Codes (Core v4.2 Vol 2 part D) */
+enum ble_error_codes
+{
+    /* An "error" code of 0x0 means success */
+    BLE_ERR_SUCCESS             = 0x00,
+    BLE_ERR_UNKNOWN_HCI_CMD     = 0x01,
+    BLE_ERR_UNK_CONN_ID         = 0x02,
+    BLE_ERR_HW_FAIL             = 0x03,
+    BLE_ERR_PAGE_TMO            = 0x04,
+    BLE_ERR_AUTH_FAIL           = 0x05,
+    BLE_ERR_PINKEY_MISSING      = 0x06,
+    BLE_ERR_MEM_CAPACITY        = 0x07,
+    BLE_ERR_CONN_SPVN_TMO       = 0x08,
+    BLE_ERR_CONN_LIMIT          = 0x09,
+    BLE_ERR_SYNCH_CONN_LIMIT    = 0x0a,
+    BLE_ERR_ACL_CONN_EXISTS     = 0x0b,
+    BLE_ERR_CMD_DISALLOWED      = 0x0c,
+    BLE_ERR_CONN_REJ_RESOURCES  = 0x0d,
+    BLE_ERR_CONN_REJ_SECURITY   = 0x0e,
+    BLE_ERR_CONN_REJ_BD_ADDR    = 0x0f,
+    BLE_ERR_CONN_ACCEPT_TMO     = 0x10,
+    BLE_ERR_UNSUPPORTED         = 0x11,
+    BLE_ERR_INV_HCI_CMD_PARMS   = 0x12,
+    BLE_ERR_REM_USER_CONN_TERM  = 0x13,
+    BLE_ERR_RD_CONN_TERM_RESRCS = 0x14,
+    BLE_ERR_RD_CONN_TERM_PWROFF = 0x15,
+    BLE_ERR_CONN_TERM_LOCAL     = 0x16,
+    BLE_ERR_REPEATED_ATTEMPTS   = 0x17,
+    BLE_ERR_NO_PAIRING          = 0x18,
+    BLE_ERR_UNK_LMP             = 0x19,
+    BLE_ERR_UNSUPP_REM_FEATURE  = 0x1a,
+    BLE_ERR_SCO_OFFSET          = 0x1b,
+    BLE_ERR_SCO_ITVL            = 0x1c,
+    BLE_ERR_SCO_AIR_MODE        = 0x1d,
+    BLE_ERR_INV_LMP_LL_PARM     = 0x1e,
+    BLE_ERR_UNSPECIFIED         = 0x1f,
+    BLE_ERR_UNSUPP_LMP_LL_PARM  = 0x20,
+    BLE_ERR_NO_ROLE_CHANGE      = 0x21,
+    BLE_ERR_LMP_LL_RSP_TMO      = 0x22,
+    BLE_ERR_LMP_COLLISION       = 0x23,
+    BLE_ERR_LMP_PDU             = 0x24,
+    BLE_ERR_ENCRYPTION_MODE     = 0x25,
+    BLE_ERR_LINK_KEY_CHANGE     = 0x26,
+    BLE_ERR_UNSUPP_QOS          = 0x27,
+    BLE_ERR_INSTANT_PASSED      = 0x28,
+    BLE_ERR_UNIT_KEY_PAIRING    = 0x29,
+    BLE_ERR_DIFF_TRANS_COLL     = 0x2a,
+    /* BLE_ERR_RESERVED         = 0x2b */
+    BLE_ERR_QOS_PARM            = 0x2c,
+    BLE_ERR_QOS_REJECTED        = 0x2d,
+    BLE_ERR_CHAN_CLASS          = 0x2e,
+    BLE_ERR_INSUFFICIENT_SEC    = 0x2f,
+    BLE_ERR_PARM_OUT_OF_RANGE   = 0x30,
+    /* BLE_ERR_RESERVED         = 0x31 */
+    BLE_ERR_PENDING_ROLE_SW     = 0x32,
+    /* BLE_ERR_RESERVED         = 0x33 */
+    BLE_ERR_RESERVED_SLOT       = 0x34,
+    BLE_ERR_ROLE_SW_FAIL        = 0x35,
+    BLE_ERR_INQ_RSP_TOO_BIG     = 0x36,
+    BLE_ERR_SEC_SIMPLE_PAIR     = 0x37,
+    BLE_ERR_HOST_BUSY_PAIR      = 0x38,
+    BLE_ERR_CONN_REJ_CHANNEL    = 0x39,
+    BLE_ERR_CTLR_BUSY           = 0x3a,
+    BLE_ERR_CONN_PARMS          = 0x3b,
+    BLE_ERR_DIR_ADV_TMO         = 0x3c,
+    BLE_ERR_CONN_TERM_MIC       = 0x3d,
+    BLE_ERR_CONN_ESTABLISHMENT  = 0x3e,
+    BLE_ERR_MAC_CONN_FAIL       = 0x3f,
+    BLE_ERR_COARSE_CLK_ADJ      = 0x40,
+    BLE_ERR_TYPE0_SUBMAP_NDEF   = 0x41,
+    BLE_ERR_UNK_ADV_INDENT      = 0x42,
+    BLE_ERR_LIMIT_REACHED       = 0x43,
+    BLE_ERR_OPERATION_CANCELLED = 0x44,
+    BLE_ERR_PACKET_TOO_LONG     = 0x45,
+    BLE_ERR_MAX                 = 0xff
+};
+
+/* HW error codes */
+#define BLE_HW_ERR_DO_NOT_USE           (0) /* XXX: reserve this one for now */
+#define BLE_HW_ERR_HCI_SYNC_LOSS        (1)
+
+/* Own Bluetooth Device address type */
+#define BLE_OWN_ADDR_PUBLIC                  (0x00)
+#define BLE_OWN_ADDR_RANDOM                  (0x01)
+#define BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT      (0x02)
+#define BLE_OWN_ADDR_RPA_RANDOM_DEFAULT      (0x03)
+
+/* Bluetooth Device address type */
+#define BLE_ADDR_PUBLIC      (0x00)
+#define BLE_ADDR_RANDOM      (0x01)
+#define BLE_ADDR_PUBLIC_ID   (0x02)
+#define BLE_ADDR_RANDOM_ID   (0x03)
+
+#define BLE_ADDR_ANY (&(ble_addr_t) { 0, {0, 0, 0, 0, 0, 0} })
+
+#define BLE_ADDR_IS_RPA(addr)     (((addr)->type == BLE_ADDR_RANDOM) && \
+                                   ((addr)->val[5] & 0xc0) == 0x40)
+#define BLE_ADDR_IS_NRPA(addr)    (((addr)->type == BLE_ADDR_RANDOM) && \
+                                   ((addr)->val[5] & 0xc0) == 0x00)
+#define BLE_ADDR_IS_STATIC(addr)  (((addr)->type == BLE_ADDR_RANDOM) && \
+                                   ((addr)->val[5] & 0xc0) == 0xc0)
+
+typedef struct {
+    uint8_t type;
+    uint8_t val[6];
+} ble_addr_t;
+
+
+static inline int ble_addr_cmp(const ble_addr_t *a, const ble_addr_t *b)
+{
+    int type_diff;
+
+    type_diff = a->type - b->type;
+    if (type_diff != 0) {
+        return type_diff;
+    }
+
+    return memcmp(a->val, b->val, sizeof(a->val));
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_ */
diff --git a/src/libs/mynewt-nimble/nimble/include/nimble/ble_hci_trans.h b/src/libs/mynewt-nimble/nimble/include/nimble/ble_hci_trans.h
new file mode 100644
index 00000000..e8d3ec03
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/include/nimble/ble_hci_trans.h
@@ -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.
+ */
+
+#ifndef H_HCI_TRANSPORT_
+#define H_HCI_TRANSPORT_
+
+#include <inttypes.h>
+#include "os/os_mempool.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_mbuf;
+
+#define BLE_HCI_TRANS_CMD_SZ        260
+
+/*** Type of buffers for holding commands and events. */
+/**
+ * Controller-to-host event buffers.  Events have one of two priorities:
+ * o Low-priority   (BLE_HCI_TRANS_BUF_EVT_LO)
+ * o High-priority  (BLE_HCI_TRANS_BUF_EVT_HI)
+ *
+ * Low-priority event buffers are only used for advertising reports.  If there
+ * are no free low-priority event buffers, then an incoming advertising report
+ * will get dropped.
+ *
+ * High-priority event buffers are for everything except advertising reports.
+ * If there are no free high-priority event buffers, a request to allocate one
+ * will try to allocate a low-priority buffer instead.
+ *
+ * If you want all events to be given equal treatment, then you should allocate
+ * low-priority events only.
+ *
+ * Event priorities solve the problem of critical events getting dropped due to
+ * a flood of advertising reports.  This solution is likely temporary: when
+ * HCI flow control is added, event priorities may become obsolete.
+ *
+ * Not all transports distinguish between low and high priority events.  If the
+ * transport does not have separate settings for low and high buffer counts,
+ * then it treats all events with equal priority.
+ */
+#define BLE_HCI_TRANS_BUF_EVT_LO    1
+#define BLE_HCI_TRANS_BUF_EVT_HI    2
+
+/* Host-to-controller command. */
+#define BLE_HCI_TRANS_BUF_CMD       3
+
+/** Callback function types; executed when HCI packets are received. */
+typedef int ble_hci_trans_rx_cmd_fn(uint8_t *cmd, void *arg);
+typedef int ble_hci_trans_rx_acl_fn(struct os_mbuf *om, void *arg);
+
+/**
+ * Sends an HCI event from the controller to the host.
+ *
+ * @param cmd                   The HCI event to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_ll_evt_tx(uint8_t *hci_ev);
+
+/**
+ * Sends ACL data from controller to host.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_ll_acl_tx(struct os_mbuf *om);
+
+/**
+ * Sends an HCI command from the host to the controller.
+ *
+ * @param cmd                   The HCI command to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_hs_cmd_tx(uint8_t *cmd);
+
+/**
+ * Sends ACL data from host to controller.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_hs_acl_tx(struct os_mbuf *om);
+
+/**
+ * Allocates a flat buffer of the specified type.
+ *
+ * @param type                  The type of buffer to allocate; one of the
+ *                                  BLE_HCI_TRANS_BUF_[...] constants.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+uint8_t *ble_hci_trans_buf_alloc(int type);
+
+/**
+ * Frees the specified flat buffer.  The buffer must have been allocated via
+ * ble_hci_trans_buf_alloc().
+ *
+ * @param buf                   The buffer to free.
+ */
+void ble_hci_trans_buf_free(uint8_t *buf);
+
+/**
+ * Configures a callback to get executed whenever an ACL data packet is freed.
+ * The function is called immediately before the free occurs.
+ *
+ * @param cb                    The callback to configure.
+ * @param arg                   An optional argument to pass to the callback.
+ *
+ * @return                      0 on success;
+ *                              BLE_ERR_UNSUPPORTED if the transport does not
+ *                                  support this operation.
+ */
+int ble_hci_trans_set_acl_free_cb(os_mempool_put_fn *cb, void *arg);
+
+/**
+ * Configures the HCI transport to operate with a controller.  The transport
+ * will execute specified callbacks upon receiving HCI packets from the host.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  command.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                          void *cmd_arg,
+                          ble_hci_trans_rx_acl_fn *acl_cb,
+                          void *acl_arg);
+
+/**
+ * Configures the HCI transport to operate with a host.  The transport will
+ * execute specified callbacks upon receiving HCI packets from the controller.
+ *
+ * @param evt_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param evt_arg               Optional argument to pass to the event
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *evt_cb,
+                          void *evt_arg,
+                          ble_hci_trans_rx_acl_fn *acl_cb,
+                          void *acl_arg);
+
+/**
+ * Resets the HCI module to a clean state.  Frees all buffers and reinitializes
+ * the underlying transport.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_reset(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_HCI_TRANSPORT_ */
diff --git a/src/libs/mynewt-nimble/nimble/include/nimble/hci_common.h b/src/libs/mynewt-nimble/nimble/include/nimble/hci_common.h
new file mode 100644
index 00000000..c79abb31
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/include/nimble/hci_common.h
@@ -0,0 +1,1536 @@
+/*
+ * 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_HCI_COMMON_
+#define H_BLE_HCI_COMMON_
+
+#include "ble.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_HCI_MAX_DATA_LEN (MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE) - \
+                              sizeof(struct ble_hci_ev))
+
+/* Generic command header */
+struct ble_hci_cmd {
+    uint16_t opcode;
+    uint8_t  length;
+    uint8_t  data[0];
+} __attribute__((packed));
+
+/* Generic event header */
+struct ble_hci_ev {
+    uint8_t opcode;
+    uint8_t length;
+    uint8_t  data[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OPCODE_NOP                  (0)
+
+/* Set opcode based on OCF and OGF */
+#define BLE_HCI_OP(ogf, ocf)            ((ocf) | ((ogf) << 10))
+
+/* Get the OGF and OCF from the opcode in the command */
+#define BLE_HCI_OGF(opcode)                 (((opcode) >> 10) & 0x003F)
+#define BLE_HCI_OCF(opcode)                 ((opcode) & 0x03FF)
+
+/* Opcode Group definitions (note: 0x07 not defined in spec) */
+#define BLE_HCI_OGF_LINK_CTRL               (0x01)
+#define BLE_HCI_OGF_LINK_POLICY             (0x02)
+#define BLE_HCI_OGF_CTLR_BASEBAND           (0x03)
+#define BLE_HCI_OGF_INFO_PARAMS             (0x04)
+#define BLE_HCI_OGF_STATUS_PARAMS           (0x05)
+#define BLE_HCI_OGF_TESTING                 (0x06)
+#define BLE_HCI_OGF_LE                      (0x08)
+#define BLE_HCI_OGF_VENDOR                  (0x3F)
+
+/*
+ * Number of LE commands. NOTE: this is really just used to size the array
+ * containing the lengths of the LE commands.
+ */
+#define BLE_HCI_NUM_LE_CMDS                 (79)
+
+/* List of OCF for Link Control commands (OGF=0x01) */
+#define BLE_HCI_OCF_DISCONNECT_CMD          (0x0006)
+struct ble_hci_lc_disconnect_cp {
+    uint16_t conn_handle;
+    uint8_t  reason;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_RD_REM_VER_INFO         (0x001D)
+struct ble_hci_rd_rem_ver_info_cp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+/* List of OCF for Controller and Baseband commands (OGF=0x03) */
+#define BLE_HCI_OCF_CB_SET_EVENT_MASK       (0x0001)
+struct ble_hci_cb_set_event_mask_cp {
+    uint64_t event_mask;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_CB_RESET                (0x0003)
+
+#define BLE_HCI_OCF_CB_READ_TX_PWR          (0x002D)
+struct ble_hci_cb_read_tx_pwr_cp {
+    uint16_t conn_handle;
+    uint8_t  type;
+} __attribute__((packed));
+
+struct ble_hci_cb_read_tx_pwr_rp {
+    uint16_t conn_handle;
+    int8_t   tx_level;
+} __attribute__((packed));
+
+
+#define BLE_HCI_OCF_CB_SET_CTLR_TO_HOST_FC  (0x0031)
+struct ble_hci_cb_ctlr_to_host_fc_cp {
+    uint8_t enable;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_CB_HOST_BUF_SIZE        (0x0033)
+struct ble_hci_cb_host_buf_size_cp {
+    uint16_t acl_data_len;
+    uint16_t sco_data_len;
+    uint16_t acl_num;
+    uint16_t sco_num;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_CB_HOST_NUM_COMP_PKTS   (0x0035)
+struct  ble_hci_cb_host_num_comp_pkts_entry {
+    uint16_t handle;
+    uint16_t count;
+} __attribute__((packed));
+struct ble_hci_cb_host_num_comp_pkts_cp {
+    uint8_t handles;
+    struct ble_hci_cb_host_num_comp_pkts_entry h[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_CB_SET_EVENT_MASK2      (0x0063)
+struct ble_hci_cb_set_event_mask2_cp {
+    uint64_t event_mask2;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_CB_RD_AUTH_PYLD_TMO     (0x007B)
+struct ble_hci_cb_rd_auth_pyld_tmo_cp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+struct ble_hci_cb_rd_auth_pyld_tmo_rp {
+    uint16_t conn_handle;
+    uint16_t tmo;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_CB_WR_AUTH_PYLD_TMO     (0x007C)
+struct ble_hci_cb_wr_auth_pyld_tmo_cp {
+    uint16_t conn_handle;
+    uint16_t tmo;
+} __attribute__((packed));
+struct ble_hci_cb_wr_auth_pyld_tmo_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+/* List of OCF for Info Param commands (OGF=0x04) */
+#define BLE_HCI_OCF_IP_RD_LOCAL_VER         (0x0001)
+struct ble_hci_ip_rd_local_ver_rp {
+    uint8_t  hci_ver;
+    uint16_t hci_rev;
+    uint8_t  lmp_ver;
+    uint16_t manufacturer;
+    uint16_t lmp_subver;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD      (0x0002)
+struct ble_hci_ip_rd_loc_supp_cmd_rp {
+    uint8_t commands[64];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT     (0x0003)
+struct ble_hci_ip_rd_loc_supp_feat_rp {
+    uint64_t features;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_IP_RD_BUF_SIZE          (0x0005)
+struct ble_hci_ip_rd_buf_size_rp {
+    uint16_t acl_data_len;
+    uint8_t  sco_data_len;
+    uint16_t acl_num;
+    uint16_t sco_num;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_IP_RD_BD_ADDR           (0x0009)
+struct ble_hci_ip_rd_bd_addr_rp {
+    uint8_t addr[6];
+} __attribute__((packed));
+
+/* List of OCF for Status parameters commands (OGF = 0x05) */
+#define BLE_HCI_OCF_RD_RSSI                 (0x0005)
+struct ble_hci_rd_rssi_cp {
+    uint16_t handle;
+} __attribute__((packed));
+struct ble_hci_rd_rssi_rp {
+    uint16_t handle;
+    int8_t   rssi;
+} __attribute__((packed));
+
+/* List of OCF for LE commands (OGF = 0x08) */
+#define BLE_HCI_OCF_LE_SET_EVENT_MASK               (0x0001)
+struct ble_hci_le_set_event_mask_cp {
+    uint64_t event_mask;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_BUF_SIZE                  (0x0002)
+struct ble_hci_le_rd_buf_size_rp {
+    uint16_t data_len;
+    uint8_t  data_packets;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT             (0x0003)
+struct ble_hci_le_rd_loc_supp_feat_rp {
+    uint64_t features;
+} __attribute__((packed));
+
+/* NOTE: 0x0004 is intentionally left undefined */
+#define BLE_HCI_OCF_LE_SET_RAND_ADDR                (0x0005)
+struct ble_hci_le_set_rand_addr_cp {
+    uint8_t addr[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_ADV_PARAMS               (0x0006)
+struct ble_hci_le_set_adv_params_cp {
+    uint16_t min_interval;
+    uint16_t max_interval;
+    uint8_t type;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+    uint8_t chan_map;
+    uint8_t filter_policy;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR            (0x0007)
+struct ble_hci_le_rd_adv_chan_txpwr_rp {
+    int8_t power_level;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_ADV_DATA                 (0x0008)
+#define BLE_HCI_MAX_ADV_DATA_LEN                    (31)
+struct ble_hci_le_set_adv_data_cp {
+    uint8_t adv_data_len;
+    uint8_t adv_data[BLE_HCI_MAX_ADV_DATA_LEN];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA            (0x0009)
+#define BLE_HCI_MAX_SCAN_RSP_DATA_LEN               (31)
+struct ble_hci_le_set_scan_rsp_data_cp {
+    uint8_t scan_rsp_len;
+    uint8_t scan_rsp[BLE_HCI_MAX_SCAN_RSP_DATA_LEN];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_ADV_ENABLE               (0x000A)
+struct ble_hci_le_set_adv_enable_cp {
+    uint8_t enable;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_SCAN_PARAMS              (0x000B)
+struct ble_hci_le_set_scan_params_cp {
+    uint8_t  scan_type;
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    uint8_t  own_addr_type;
+    uint8_t  filter_policy;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_SCAN_ENABLE              (0x000C)
+struct ble_hci_le_set_scan_enable_cp {
+    uint8_t enable;
+    uint8_t filter_duplicates;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CREATE_CONN                  (0x000D)
+struct ble_hci_le_create_conn_cp {
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    uint8_t  filter_policy;
+    uint8_t  peer_addr_type;
+    uint8_t  peer_addr[6];
+    uint8_t  own_addr_type;
+    uint16_t min_conn_itvl;
+    uint16_t max_conn_itvl;
+    uint16_t conn_latency;
+    uint16_t tmo;
+    uint16_t min_ce;
+    uint16_t max_ce;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CREATE_CONN_CANCEL           (0x000E)
+
+#define BLE_HCI_OCF_LE_RD_WHITE_LIST_SIZE           (0x000F)
+struct ble_hci_le_rd_white_list_rp {
+    uint8_t size;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CLEAR_WHITE_LIST             (0x0010)
+
+#define BLE_HCI_OCF_LE_ADD_WHITE_LIST               (0x0011)
+struct ble_hci_le_add_whte_list_cp {
+    uint8_t addr_type;
+    uint8_t addr[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RMV_WHITE_LIST               (0x0012)
+struct ble_hci_le_rmv_white_list_cp {
+    uint8_t addr_type;
+    uint8_t addr[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CONN_UPDATE                  (0x0013)
+struct ble_hci_le_conn_update_cp {
+    uint16_t conn_handle;
+    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;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_HOST_CHAN_CLASS          (0x0014)
+struct ble_hci_le_set_host_chan_class_cp {
+    uint8_t chan_map[5];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_CHAN_MAP                  (0x0015)
+struct ble_hci_le_rd_chan_map_cp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+struct ble_hci_le_rd_chan_map_rp {
+    uint16_t conn_handle;
+    uint8_t chan_map[5];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_REM_FEAT                  (0x0016)
+struct ble_hci_le_rd_rem_feat_cp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_ENCRYPT                      (0x0017)
+struct ble_hci_le_encrypt_cp {
+    uint8_t key[16];
+    uint8_t data[16];
+} __attribute__((packed));
+struct ble_hci_le_encrypt_rp {
+    uint8_t data[16];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RAND                         (0x0018)
+struct ble_hci_le_rand_rp {
+    uint64_t random_number;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_START_ENCRYPT                (0x0019)
+struct ble_hci_le_start_encrypt_cp {
+    uint16_t conn_handle;
+    uint64_t rand;
+    uint16_t div;
+    uint8_t  ltk[16];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY             (0x001A)
+struct ble_hci_le_lt_key_req_reply_cp {
+    uint16_t conn_handle;
+    uint8_t  ltk[16];
+} __attribute__((packed));
+struct ble_hci_le_lt_key_req_reply_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY         (0x001B)
+struct ble_hci_le_lt_key_req_neg_reply_cp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+struct ble_hci_le_lt_key_req_neg_reply_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_SUPP_STATES               (0x001C)
+struct ble_hci_le_rd_supp_states_rp {
+    uint64_t states;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RX_TEST                      (0x001D)
+struct ble_hci_le_rx_test_cp {
+    uint8_t rx_chan;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_TX_TEST                      (0x001E)
+struct ble_hci_le_tx_test_cp {
+    uint8_t tx_chan;
+    uint8_t test_data_len;
+    uint8_t payload;
+} __attribute__((packed));
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+struct ble_hci_le_tx_test_ext_cp {
+    uint8_t tx_chan;
+    uint8_t test_data_len;
+    uint8_t payload;
+    uint16_t interval;
+    uint16_t pkt_count;
+} __attribute__((packed));
+#endif
+
+#define BLE_HCI_OCF_LE_TEST_END                     (0x001F)
+struct ble_hci_le_test_end_rp {
+    uint16_t num_packets;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_REM_CONN_PARAM_RR            (0x0020)
+struct ble_hci_le_rem_conn_param_rr_cp {
+    uint16_t conn_handle;
+    uint16_t conn_itvl_min;
+    uint16_t conn_itvl_max;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint16_t min_ce;
+    uint16_t max_ce;
+} __attribute__((packed));
+struct ble_hci_le_rem_conn_param_rr_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR           (0x0021)
+struct ble_hci_le_rem_conn_params_nrr_cp {
+    uint16_t conn_handle;
+    uint8_t  reason;
+} __attribute__((packed));
+struct ble_hci_le_rem_conn_params_nrr_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_DATA_LEN                 (0x0022)
+struct ble_hci_le_set_data_len_cp {
+    uint16_t conn_handle;
+    uint16_t tx_octets;
+    uint16_t tx_time;
+} __attribute__((packed));
+struct ble_hci_le_set_data_len_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_SUGG_DEF_DATA_LEN         (0x0023)
+struct ble_hci_le_rd_sugg_def_data_len_rp {
+    uint16_t max_tx_octets;
+    uint16_t max_tx_time;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_WR_SUGG_DEF_DATA_LEN         (0x0024)
+struct ble_hci_le_wr_sugg_def_data_len_cp {
+    uint16_t max_tx_octets;
+    uint16_t max_tx_time;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_P256_PUBKEY               (0x0025)
+
+#define BLE_HCI_OCF_LE_GEN_DHKEY                    (0x0026)
+struct ble_hci_le_gen_dhkey_cp {
+    uint8_t pkey[64];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_ADD_RESOLV_LIST              (0x0027)
+struct ble_hci_le_add_resolv_list_cp {
+    uint8_t peer_addr_type;
+    uint8_t peer_id_addr[6];
+    uint8_t peer_irk[16];
+    uint8_t local_irk[16];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RMV_RESOLV_LIST              (0x0028)
+struct ble_hci_le_rmv_resolve_list_cp {
+    uint8_t peer_addr_type;
+    uint8_t peer_id_addr[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CLR_RESOLV_LIST              (0x0029)
+
+#define BLE_HCI_OCF_LE_RD_RESOLV_LIST_SIZE          (0x002A)
+struct ble_hci_le_rd_resolv_list_size_rp {
+    uint8_t size;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_PEER_RESOLV_ADDR          (0x002B)
+struct ble_hci_le_rd_peer_recolv_addr_cp {
+    uint8_t peer_addr_type;
+    uint8_t peer_id_addr[6];
+} __attribute__((packed));
+struct ble_hci_le_rd_peer_recolv_addr_rp {
+    uint8_t rpa[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR         (0x002C)
+struct ble_hci_le_rd_local_recolv_addr_cp {
+    uint8_t peer_addr_type;
+    uint8_t peer_id_addr[6];
+} __attribute__((packed));
+struct ble_hci_le_rd_local_recolv_addr_rp {
+    uint8_t rpa[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_ADDR_RES_EN              (0x002D)
+struct ble_hci_le_set_addr_res_en_cp {
+    uint8_t enable;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_RPA_TMO                  (0x002E)
+struct ble_hci_le_set_rpa_tmo_cp {
+    uint16_t rpa_timeout;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_MAX_DATA_LEN              (0x002F)
+struct ble_hci_le_rd_max_data_len_rp {
+    uint16_t max_tx_octests;
+    uint16_t max_tx_time;
+    uint16_t max_rx_octests;
+    uint16_t max_rx_time;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_PHY                       (0x0030)
+struct ble_hci_le_rd_phy_cp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+struct ble_hci_le_rd_phy_rp {
+    uint16_t conn_handle;
+    uint8_t tx_phy;
+    uint8_t rx_phy;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_DEFAULT_PHY              (0x0031)
+struct ble_hci_le_set_default_phy_cp {
+    uint8_t all_phys;
+    uint8_t tx_phys;
+    uint8_t rx_phys;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_PHY                      (0x0032)
+struct ble_hci_le_set_phy_cp {
+    uint16_t conn_handle;
+    uint8_t all_phys;
+    uint8_t tx_phys;
+    uint8_t rx_phys;
+    uint16_t phy_options;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RX_TEST_V2                  (0x0033)
+struct ble_hci_le_rx_test_v2_cp {
+    uint8_t rx_chan;
+    uint8_t phy;
+    uint8_t index;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_TX_TEST_V2                  (0x0034)
+struct ble_hci_le_tx_test_v2_cp {
+    uint8_t tx_chan;
+    uint8_t test_data_len;
+    uint8_t payload;
+    uint8_t phy;
+} __attribute__((packed));
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+struct ble_hci_le_tx_test_v2_ext_cp {
+    uint8_t tx_chan;
+    uint8_t test_data_len;
+    uint8_t payload;
+    uint8_t phy;
+    uint16_t interval;
+    uint16_t pkt_count;
+} __attribute__((packed));
+#endif
+
+#define BLE_HCI_OCF_LE_SET_ADV_SET_RND_ADDR         (0x0035)
+struct ble_hci_le_set_adv_set_rnd_addr_cp {
+    uint8_t adv_handle;
+    uint8_t addr[6];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM            (0x0036)
+struct ble_hci_le_set_ext_adv_params_cp {
+    uint8_t  adv_handle;
+    uint16_t props;
+    uint8_t  pri_itvl_min[3];
+    uint8_t  pri_itvl_max[3];
+    uint8_t pri_chan_map;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+    uint8_t filter_policy;
+    int8_t tx_power;
+    uint8_t pri_phy;
+    uint8_t sec_max_skip;
+    uint8_t sec_phy;
+    uint8_t sid;
+    uint8_t scan_req_notif;
+} __attribute__((packed));
+struct ble_hci_le_set_ext_adv_params_rp {
+    int8_t  tx_power;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_EXT_ADV_DATA             (0x0037)
+struct ble_hci_le_set_ext_adv_data_cp {
+    uint8_t adv_handle;
+    uint8_t operation;
+    uint8_t fragment_pref;
+    uint8_t adv_data_len;
+    uint8_t adv_data[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_EXT_SCAN_RSP_DATA        (0x0038)
+struct ble_hci_le_set_ext_scan_rsp_data_cp {
+    uint8_t adv_handle;
+    uint8_t operation;
+    uint8_t fragment_pref;
+    uint8_t scan_rsp_len;
+    uint8_t scan_rsp[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE           (0x0039)
+struct adv_set {
+    uint8_t adv_handle;
+    uint16_t duration;
+    uint8_t max_events;
+} __attribute__((packed));
+struct ble_hci_le_set_ext_adv_enable_cp {
+    uint8_t enable;
+    uint8_t num_sets;
+    struct adv_set sets[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_MAX_ADV_DATA_LEN          (0x003A)
+struct ble_hci_le_rd_max_adv_data_len_rp {
+    uint16_t max_adv_data_len;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_NUM_OF_ADV_SETS           (0x003B)
+struct ble_hci_le_rd_num_of_adv_sets_rp {
+    uint8_t num_sets;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_REMOVE_ADV_SET               (0x003C)
+struct ble_hci_le_remove_adv_set_cp {
+    uint8_t adv_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CLEAR_ADV_SETS               (0x003D)
+
+#define BLE_HCI_OCF_LE_SET_PERIODIC_ADV_PARAMS      (0x003E)
+struct ble_hci_le_set_periodic_adv_params_cp {
+    uint8_t adv_handle;
+    uint16_t min_itvl;
+    uint16_t max_itvl;
+    uint16_t props;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA        (0x003F)
+struct ble_hci_le_set_periodic_adv_data_cp {
+    uint8_t adv_handle;
+    uint8_t operation;
+    uint8_t adv_data_len;
+    uint8_t adv_data[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE      (0x0040)
+struct ble_hci_le_set_periodic_adv_enable_cp {
+    uint8_t enable;
+    uint8_t adv_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_EXT_SCAN_PARAM           (0x0041)
+struct scan_params {
+    uint8_t  type;
+    uint16_t itvl;
+    uint16_t window;
+} __attribute__((packed));
+struct ble_hci_le_set_ext_scan_params_cp {
+    uint8_t own_addr_type;
+    uint8_t filter_policy;
+    uint8_t phys;
+    struct scan_params scans[0];
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_EXT_SCAN_ENABLE          (0x0042)
+struct ble_hci_le_set_ext_scan_enable_cp {
+    uint8_t  enable;
+    uint8_t  filter_dup;
+    uint16_t duration;
+    uint16_t period;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_EXT_CREATE_CONN              (0x0043)
+struct conn_params {
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    uint16_t conn_min_itvl;
+    uint16_t conn_max_itvl;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint16_t min_ce;
+    uint16_t max_ce;
+} __attribute__((packed));
+struct ble_hci_le_ext_create_conn_cp {
+    uint8_t filter_policy;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+    uint8_t init_phy_mask;
+    struct conn_params conn_params[0];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER      0x01
+#define BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED    0x02
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC          (0x0044)
+struct ble_hci_le_periodic_adv_create_sync_cp {
+    uint8_t  options;
+    uint8_t  sid;
+    uint8_t  peer_addr_type;
+    uint8_t  peer_addr[6];
+    uint16_t skip;
+    uint16_t sync_timeout;
+    uint8_t  sync_cte_type;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC_CANCEL   (0x0045)
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC            (0x0046)
+struct ble_hci_le_periodic_adv_term_sync_cp {
+    uint16_t sync_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_ADD_DEV_TO_PERIODIC_ADV_LIST      (0x0047)
+struct ble_hci_le_add_dev_to_periodic_adv_list_cp {
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+    uint8_t sid;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_REM_DEV_FROM_PERIODIC_ADV_LIST    (0x0048)
+struct ble_hci_le_rem_dev_from_periodic_adv_list_cp {
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+    uint8_t sid;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST           (0x0049)
+
+#define BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE         (0x004A)
+struct ble_hci_le_rd_periodic_adv_list_size_rp {
+    uint8_t list_size;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_TRANSMIT_POWER            (0x004B)
+struct ble_hci_le_rd_transmit_power_rp {
+    int8_t min_tx_power;
+    int8_t max_tx_power;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RD_RF_PATH_COMPENSATION      (0x004C)
+struct ble_hci_le_rd_rf_path_compensation_rp {
+    int16_t tx_path_compensation;
+    int16_t rx_path_compensation;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_WR_RF_PATH_COMPENSATION      (0x004D)
+struct ble_hci_le_wr_rf_path_compensation_cp {
+    int16_t tx_path_compensation;
+    int16_t rx_path_compensation;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_PRIVACY_MODE             (0x004E)
+struct ble_hci_le_set_privacy_mode_cp {
+    uint8_t peer_id_addr_type;
+    uint8_t peer_id_addr[6];
+    uint8_t mode;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_RX_TEST_V3                        (0x004F)
+#define BLE_HCI_OCF_LE_TX_TEST_V3                        (0x0050)
+#define BLE_HCI_OCF_LE_SET_CONNLESS_CTE_TX_PARAMS        (0x0051)
+#define BLE_HCI_OCF_LE_SET_CONNLESS_CTE_TX_ENABLE        (0x0052)
+#define BLE_HCI_OCF_LE_SET_CONNLESS_IQ_SAMPLING_ENABLE   (0x0053)
+#define BLE_HCI_OCF_LE_SET_CONN_CTE_RX_PARAMS            (0x0054)
+#define BLE_HCI_OCF_LE_SET_CONN_CTE_TX_PARAMS            (0x0055)
+#define BLE_HCI_OCF_LE_SET_CONN_CTE_REQ_ENABLE           (0x0056)
+#define BLE_HCI_OCF_LE_SET_CONN_CTE_RESP_ENABLE          (0x0057)
+#define BLE_HCI_OCF_LE_RD_ANTENNA_INFO                   (0x0058)
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_RECEIVE_ENABLE       (0x0059)
+struct ble_hci_le_periodic_adv_receive_enable_cp {
+    uint16_t sync_handle;
+    uint8_t enable;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER        (0x005A)
+struct ble_hci_le_periodic_adv_sync_transfer_cp {
+    uint16_t conn_handle;
+    uint16_t service_data;
+    uint16_t sync_handle;
+} __attribute__((packed));
+struct ble_hci_le_periodic_adv_sync_transfer_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER    (0x005B)
+struct ble_hci_le_periodic_adv_set_info_transfer_cp {
+    uint16_t conn_handle;
+    uint16_t service_data;
+    uint8_t adv_handle;
+} __attribute__((packed));
+struct ble_hci_le_periodic_adv_set_info_transfer_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS (0x005C)
+struct ble_hci_le_periodic_adv_sync_transfer_params_cp {
+    uint16_t conn_handle;
+    uint8_t  mode;
+    uint16_t skip;
+    uint16_t sync_timeout;
+    uint8_t  sync_cte_type;
+} __attribute__((packed));
+struct ble_hci_le_periodic_adv_sync_transfer_params_rp {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS  (0x005D)
+struct ble_hci_le_set_default_periodic_sync_transfer_params_cp {
+    uint8_t  mode;
+    uint16_t skip;
+    uint16_t sync_timeout;
+    uint8_t  sync_cte_type;
+} __attribute__((packed));
+
+#define BLE_HCI_OCF_LE_GENERATE_DHKEY_V2                 (0x005E)
+#define BLE_HCI_OCF_LE_MODIFY_SCA                        (0x005F)
+
+#define BLE_HCI_OCF_LE_SET_HOST_FEAT                     (0x0074)
+struct ble_hci_le_set_host_feat_cp {
+    uint8_t bit_num;
+    uint8_t val;
+} __attribute__((packed));
+
+/* Command Specific Definitions */
+/* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */
+#define BLE_HCI_CTLR_TO_HOST_FC_OFF         (0)
+#define BLE_HCI_CTLR_TO_HOST_FC_ACL         (1)
+#define BLE_HCI_CTLR_TO_HOST_FC_SYNC        (2)
+#define BLE_HCI_CTLR_TO_HOST_FC_BOTH        (3)
+
+/* --- LE set advertising parameters (OCF 0x0006) */
+/* Advertising types */
+#define BLE_HCI_ADV_TYPE_ADV_IND            (0)
+#define BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD  (1)
+#define BLE_HCI_ADV_TYPE_ADV_SCAN_IND       (2)
+#define BLE_HCI_ADV_TYPE_ADV_NONCONN_IND    (3)
+#define BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD  (4)
+#define BLE_HCI_ADV_TYPE_MAX                (4)
+
+#define BLE_HCI_ADV_CONN_MASK               (0x0001)
+#define BLE_HCI_ADV_SCAN_MASK               (0x0002)
+#define BLE_HCI_ADV_DIRECT_MASK             (0x0004)
+#define BLE_HCI_ADV_SCAN_RSP_MASK           (0x0008)
+#define BLE_HCI_ADV_LEGACY_MASK             (0x0010)
+
+#define BLE_HCI_ADV_DATA_STATUS_COMPLETE    (0x0000)
+#define BLE_HCI_ADV_DATA_STATUS_INCOMPLETE  (0x0020)
+#define BLE_HCI_ADV_DATA_STATUS_TRUNCATED   (0x0040)
+#define BLE_HCI_ADV_DATA_STATUS_MASK        (0x0060)
+
+/* Own address types */
+#define BLE_HCI_ADV_OWN_ADDR_PUBLIC         (0)
+#define BLE_HCI_ADV_OWN_ADDR_RANDOM         (1)
+#define BLE_HCI_ADV_OWN_ADDR_PRIV_PUB       (2)
+#define BLE_HCI_ADV_OWN_ADDR_PRIV_RAND      (3)
+#define BLE_HCI_ADV_OWN_ADDR_MAX            (3)
+
+/* Advertisement peer address Type */
+#define BLE_HCI_ADV_PEER_ADDR_PUBLIC        (0)
+#define BLE_HCI_ADV_PEER_ADDR_RANDOM        (1)
+#define BLE_HCI_ADV_PEER_ADDR_MAX           (1)
+
+/* --- LE advertising channel tx power (OCF 0x0007) */
+#define BLE_HCI_ADV_CHAN_TXPWR_MIN             (-20)
+#define BLE_HCI_ADV_CHAN_TXPWR_MAX             (10)
+
+/* --- LE set scan enable (OCF 0x000c) */
+
+/* Connect peer address type */
+#define BLE_HCI_CONN_PEER_ADDR_PUBLIC        (0)
+#define BLE_HCI_CONN_PEER_ADDR_RANDOM        (1)
+#define BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT  (2)
+#define BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT  (3)
+#define BLE_HCI_CONN_PEER_ADDR_MAX           (3)
+
+/*
+ * Advertising filter policy
+ *
+ * Determines how an advertiser filters scan and connection requests.
+ *
+ *  NONE: no filtering (default value). No whitelist used.
+ *  SCAN: process all connection requests but only scans from white list.
+ *  CONN: process all scan request but only connection requests from white list
+ *  BOTH: ignore all scan and connection requests unless in white list.
+ */
+#define BLE_HCI_ADV_FILT_NONE               (0)
+#define BLE_HCI_ADV_FILT_SCAN               (1)
+#define BLE_HCI_ADV_FILT_CONN               (2)
+#define BLE_HCI_ADV_FILT_BOTH               (3)
+#define BLE_HCI_ADV_FILT_MAX                (3)
+
+#define BLE_HCI_ADV_FILT_DEF                (BLE_HCI_ADV_FILT_NONE)
+
+/* Advertising interval */
+#define BLE_HCI_ADV_ITVL                    (625)           /* usecs */
+#define BLE_HCI_ADV_ITVL_MIN                (32)            /* units */
+#define BLE_HCI_ADV_ITVL_MAX                (16384)         /* units */
+#define BLE_HCI_ADV_ITVL_NONCONN_MIN        (160)           /* units */
+
+#define BLE_HCI_ADV_ITVL_DEF                (0x800)         /* 1.28 seconds */
+#define BLE_HCI_ADV_CHANMASK_DEF            (0x7)           /* all channels */
+
+/* Set scan parameters */
+#define BLE_HCI_SCAN_TYPE_PASSIVE           (0)
+#define BLE_HCI_SCAN_TYPE_ACTIVE            (1)
+
+/* Scan interval and scan window timing */
+#define BLE_HCI_SCAN_ITVL                   (625)           /* usecs */
+#define BLE_HCI_SCAN_ITVL_MIN               (4)             /* units */
+#define BLE_HCI_SCAN_ITVL_MAX               (16384)         /* units */
+#define BLE_HCI_SCAN_ITVL_DEF               (16)            /* units */
+#define BLE_HCI_SCAN_WINDOW_MIN             (4)             /* units */
+#define BLE_HCI_SCAN_WINDOW_MAX             (16384)         /* units */
+#define BLE_HCI_SCAN_WINDOW_DEF             (16)            /* units */
+
+/*
+ * Scanning filter policy
+ *  NO_WL:
+ *      Scanner processes all advertising packets (white list not used) except
+ *      directed, connectable advertising packets not sent to the scanner.
+ *  USE_WL:
+ *      Scanner processes advertisements from white list only. A connectable,
+ *      directed advertisment is ignored unless it contains scanners address.
+ *  NO_WL_INITA:
+ *      Scanner process all advertising packets (white list not used). A
+ *      connectable, directed advertisement shall not be ignored if the InitA
+ *      is a resolvable private address.
+ *  USE_WL_INITA:
+ *      Scanner process advertisements from white list only. A connectable,
+ *      directed advertisement shall not be ignored if the InitA is a
+ *      resolvable private address.
+ */
+#define BLE_HCI_SCAN_FILT_NO_WL             (0)
+#define BLE_HCI_SCAN_FILT_USE_WL            (1)
+#define BLE_HCI_SCAN_FILT_NO_WL_INITA       (2)
+#define BLE_HCI_SCAN_FILT_USE_WL_INITA      (3)
+#define BLE_HCI_SCAN_FILT_MAX               (3)
+
+/* Whitelist commands */
+#define BLE_HCI_ADD_WHITE_LIST_LEN          (7)
+#define BLE_HCI_RMV_WHITE_LIST_LEN          (7)
+
+/* Create Connection */
+#define BLE_HCI_CREATE_CONN_LEN             (25)
+#define BLE_HCI_CONN_ITVL                   (1250)  /* usecs */
+#define BLE_HCI_CONN_FILT_NO_WL             (0)
+#define BLE_HCI_CONN_FILT_USE_WL            (1)
+#define BLE_HCI_CONN_FILT_MAX               (1)
+#define BLE_HCI_CONN_ITVL_MIN               (0x0006)
+#define BLE_HCI_CONN_ITVL_MAX               (0x0c80)
+#define BLE_HCI_CONN_LATENCY_MIN            (0x0000)
+#define BLE_HCI_CONN_LATENCY_MAX            (0x01f3)
+#define BLE_HCI_CONN_SPVN_TIMEOUT_MIN       (0x000a)
+#define BLE_HCI_CONN_SPVN_TIMEOUT_MAX       (0x0c80)
+#define BLE_HCI_CONN_SPVN_TMO_UNITS         (10)    /* msecs */
+#define BLE_HCI_INITIATOR_FILT_POLICY_MAX   (1)
+
+/* Peer Address Type */
+#define BLE_HCI_CONN_PEER_ADDR_PUBLIC       (0)
+#define BLE_HCI_CONN_PEER_ADDR_RANDOM       (1)
+#define BLE_HCI_CONN_PEER_ADDR_PUB_ID       (2)
+#define BLE_HCI_CONN_PEER_ADDR_RAND_ID      (3)
+#define BLE_HCI_CONN_PEER_ADDR_MAX          (3)
+
+
+/* --- LE set data length (OCF 0x0022) */
+#define BLE_HCI_SET_DATALEN_TX_OCTETS_MIN   (0x001b)
+#define BLE_HCI_SET_DATALEN_TX_OCTETS_MAX   (0x00fb)
+#define BLE_HCI_SET_DATALEN_TX_TIME_MIN     (0x0148)
+#define BLE_HCI_SET_DATALEN_TX_TIME_MAX     (0x4290)
+
+/* --- LE read maximum default PHY (OCF 0x0030) */
+#define BLE_HCI_LE_PHY_1M                   (1)
+#define BLE_HCI_LE_PHY_2M                   (2)
+#define BLE_HCI_LE_PHY_CODED                (3)
+
+/* --- LE set default PHY (OCF 0x0031) */
+#define BLE_HCI_LE_PHY_NO_TX_PREF_MASK              (0x01)
+#define BLE_HCI_LE_PHY_NO_RX_PREF_MASK              (0x02)
+#define BLE_HCI_LE_PHY_1M_PREF_MASK                 (0x01)
+#define BLE_HCI_LE_PHY_2M_PREF_MASK                 (0x02)
+#define BLE_HCI_LE_PHY_CODED_PREF_MASK              (0x04)
+
+#define BLE_HCI_LE_PHY_PREF_MASK_ALL                \
+    (BLE_HCI_LE_PHY_1M_PREF_MASK | BLE_HCI_LE_PHY_2M_PREF_MASK |  \
+     BLE_HCI_LE_PHY_CODED_PREF_MASK)
+
+/* --- LE set PHY (OCF 0x0032) */
+#define BLE_HCI_LE_PHY_CODED_ANY                    (0x0000)
+#define BLE_HCI_LE_PHY_CODED_S2_PREF                (0x0001)
+#define BLE_HCI_LE_PHY_CODED_S8_PREF                (0x0002)
+
+/* --- LE enhanced receiver test (OCF 0x0033) */
+#define BLE_HCI_LE_PHY_1M                           (1)
+#define BLE_HCI_LE_PHY_2M                           (2)
+#define BLE_HCI_LE_PHY_CODED                        (3)
+
+/* --- LE enhanced transmitter test (OCF 0x0034) */
+#define BLE_HCI_LE_PHY_CODED_S8                     (3)
+#define BLE_HCI_LE_PHY_CODED_S2                     (4)
+
+/* --- LE set extended advertising parameters (OCF 0x0036) */
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE     (0x0001)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE       (0x0002)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED        (0x0004)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED     (0x0008)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY          (0x0010)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV        (0x0020)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR      (0x0040)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_MASK            (0x7F)
+
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_IND      (0x0013)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_LD_DIR   (0x0015)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_HD_DIR   (0x001d)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_SCAN     (0x0012)
+#define BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_NONCONN  (0x0010)
+
+/* --- LE set extended advertising data (OCF 0x0037) */
+#define BLE_HCI_MAX_EXT_ADV_DATA_LEN                (251)
+
+#define BLE_HCI_LE_SET_DATA_OPER_INT        (0)
+#define BLE_HCI_LE_SET_DATA_OPER_FIRST      (1)
+#define BLE_HCI_LE_SET_DATA_OPER_LAST       (2)
+#define BLE_HCI_LE_SET_DATA_OPER_COMPLETE   (3)
+#define BLE_HCI_LE_SET_DATA_OPER_UNCHANGED  (4)
+
+/* --- LE set extended scan response data (OCF 0x0038) */
+#define BLE_HCI_MAX_EXT_SCAN_RSP_DATA_LEN           (251)
+
+/* --- LE set periodic advertising parameters (OCF 0x003E) */
+#define BLE_HCI_LE_SET_PERIODIC_ADV_PROP_INC_TX_PWR (0x0040)
+#define BLE_HCI_LE_SET_PERIODIC_ADV_PROP_MASK       (0x0040)
+
+/* --- LE set periodic advertising data (OCF 0x003F) */
+#define BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN                (252)
+
+/* --- LE remove device from periodic advertising list (OCF 0x0048) */
+#define BLE_HCI_PERIODIC_DATA_STATUS_COMPLETE   0x00
+#define BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE 0x01
+#define BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED  0x02
+
+/* --- LE set privacy mode (OCF 0x004E) */
+#define BLE_HCI_PRIVACY_NETWORK                     (0)
+#define BLE_HCI_PRIVACY_DEVICE                      (1)
+
+/* Event Codes */
+#define BLE_HCI_EVCODE_INQUIRY_CMP          (0x01)
+#define BLE_HCI_EVCODE_INQUIRY_RESULT       (0x02)
+#define BLE_HCI_EVCODE_CONN_DONE            (0x03)
+#define BLE_HCI_EVCODE_CONN_REQUEST         (0x04)
+#define BLE_HCI_EVCODE_DISCONN_CMP          (0x05)
+struct ble_hci_ev_disconn_cmp {
+    uint8_t status;
+    uint16_t conn_handle;
+    uint8_t reason;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_AUTH_CMP             (0x06)
+#define BLE_HCI_EVCODE_REM_NAME_REQ_CMP     (0x07)
+
+#define BLE_HCI_EVCODE_ENCRYPT_CHG          (0x08)
+struct ble_hci_ev_enrypt_chg {
+    uint8_t status;
+    uint16_t connection_handle;
+    uint8_t enabled;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_CHG_LINK_KEY_CMP     (0x09)
+#define BLE_HCI_EVCODE_MASTER_LINK_KEY_CMP  (0x0A)
+#define BLE_HCI_EVCODE_RD_REM_SUPP_FEAT_CMP (0x0B)
+#define BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP  (0x0C)
+struct ble_hci_ev_rd_rem_ver_info_cmp {
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint8_t  version;
+    uint16_t manufacturer;
+    uint16_t subversion;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_QOS_SETUP_CMP        (0x0D)
+
+#define BLE_HCI_EVCODE_COMMAND_COMPLETE     (0x0E)
+struct ble_hci_ev_command_complete {
+    uint8_t  num_packets;
+    uint16_t opcode;
+    uint8_t  status;
+    uint8_t  return_params[0];
+} __attribute__((packed));
+/* NOP is exception and has no return parameters */
+struct ble_hci_ev_command_complete_nop {
+    uint8_t  num_packets;
+    uint16_t opcode;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_COMMAND_STATUS       (0x0F)
+struct ble_hci_ev_command_status {
+    uint8_t status;
+    uint8_t  num_packets;
+    uint16_t opcode;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_HW_ERROR             (0x10)
+struct ble_hci_ev_hw_error {
+    uint8_t hw_code;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_NUM_COMP_PKTS        (0x13)
+struct comp_pkt {
+    uint16_t handle;
+    uint16_t packets;
+} __attribute__((packed));;
+struct ble_hci_ev_num_comp_pkts {
+    uint8_t count;
+    struct comp_pkt completed[0];
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_MODE_CHANGE          (0x14)
+#define BLE_HCI_EVCODE_RETURN_LINK_KEYS     (0x15)
+#define BLE_HCI_EVCODE_PIN_CODE_REQ         (0x16)
+#define BLE_HCI_EVCODE_LINK_KEY_REQ         (0x17)
+#define BLE_HCI_EVCODE_LINK_KEY_NOTIFY      (0x18)
+#define BLE_HCI_EVCODE_LOOPBACK_CMD         (0x19)
+
+#define BLE_HCI_EVCODE_DATA_BUF_OVERFLOW    (0x1A)
+struct ble_hci_ev_data_buf_overflow {
+    uint8_t link_type;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_MAX_SLOTS_CHG        (0x1B)
+#define BLE_HCI_EVCODE_READ_CLK_OFF_COMP    (0x1C)
+#define BLE_HCI_EVCODE_CONN_PKT_TYPE_CHG    (0x1D)
+#define BLE_HCI_EVCODE_QOS_VIOLATION        (0x1E)
+/* NOTE: 0x1F not defined */
+#define BLE_HCI_EVCODE_PSR_MODE_CHG         (0x20)
+#define BLE_HCI_EVCODE_FLOW_SPEC_COMP       (0x21)
+#define BLE_HCI_EVCODE_INQ_RESULT_RSSI      (0x22)
+#define BLE_HCI_EVCODE_READ_REM_EXT_FEAT    (0x23)
+/* NOTE: 0x24 - 0x2B not defined */
+#define BLE_HCI_EVCODE_SYNCH_CONN_COMP      (0x2C)
+#define BLE_HCI_EVCODE_SYNCH_CONN_CHG       (0x2D)
+#define BLE_HCI_EVCODE_SNIFF_SUBRATING      (0x2E)
+#define BLE_HCI_EVCODE_EXT_INQ_RESULT       (0x2F)
+
+#define BLE_HCI_EVCODE_ENC_KEY_REFRESH      (0x30)
+struct ble_hci_ev_enc_key_refresh {
+    uint8_t status;
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_EVOCDE_IO_CAP_REQ           (0x31)
+#define BLE_HCI_EVCODE_IO_CAP_RSP           (0x32)
+#define BLE_HCI_EVCODE_USER_CONFIRM_REQ     (0x33)
+#define BLE_HCI_EVCODE_PASSKEY_REQ          (0x34)
+#define BLE_HCI_EVCODE_REM_OOB_DATA_REQ     (0x35)
+#define BLE_HCI_EVCODE_SIMPLE_PAIR_COMP     (0x36)
+/* NOTE: 0x37 not defined */
+#define BLE_HCI_EVCODE_LNK_SPVN_TMO_CHG     (0x38)
+#define BLE_HCI_EVCODE_ENH_FLUSH_COMP       (0x39)
+#define BLE_HCI_EVCODE_USER_PASSKEY_NOTIFY  (0x3B)
+#define BLE_HCI_EVCODE_KEYPRESS_NOTIFY      (0x3C)
+#define BLE_HCI_EVCODE_REM_HOST_SUPP_FEAT   (0x3D)
+
+#define BLE_HCI_EVCODE_LE_META              (0x3E)
+struct ble_hci_ev_le_meta {
+    uint8_t subevent;
+    uint8_t data[0];
+} __attribute__((packed));
+
+/* NOTE: 0x3F not defined */
+#define BLE_HCI_EVCODE_PHYS_LINK_COMP       (0x40)
+#define BLE_HCI_EVCODE_CHAN_SELECTED        (0x41)
+#define BLE_HCI_EVCODE_DISCONN_PHYS_LINK    (0x42)
+#define BLE_HCI_EVCODE_PHYS_LINK_LOSS_EARLY (0x43)
+#define BLE_HCI_EVCODE_PHYS_LINK_RECOVERY   (0x44)
+#define BLE_HCI_EVCODE_LOGICAL_LINK_COMP    (0x45)
+#define BLE_HCI_EVCODE_DISCONN_LOGICAL_LINK (0x46)
+#define BLE_HCI_EVCODE_FLOW_SPEC_MODE_COMP  (0x47)
+#define BLE_HCI_EVCODE_NUM_COMP_DATA_BLKS   (0x48)
+#define BLE_HCI_EVCODE_AMP_START_TEST       (0x49)
+#define BLE_HCI_EVOCDE_AMP_TEST_END         (0x4A)
+#define BLE_HCI_EVOCDE_AMP_RCVR_REPORT      (0x4B)
+#define BLE_HCI_EVCODE_SHORT_RANGE_MODE_CHG (0x4C)
+#define BLE_HCI_EVCODE_AMP_STATUS_CHG       (0x4D)
+#define BLE_HCI_EVCODE_TRIG_CLK_CAPTURE     (0x4E)
+#define BLE_HCI_EVCODE_SYNCH_TRAIN_COMP     (0x4F)
+#define BLE_HCI_EVCODE_SYNCH_TRAIN_RCVD     (0x50)
+#define BLE_HCI_EVCODE_SLAVE_BCAST_RX       (0x51)
+#define BLE_HCI_EVCODE_SLAVE_BCAST_TMO      (0x52)
+#define BLE_HCI_EVCODE_TRUNC_PAGE_COMP      (0x53)
+#define BLE_HCI_EVCODE_SLAVE_PAGE_RSP_TMO   (0x54)
+#define BLE_HCI_EVCODE_SLAVE_BCAST_CHAN_MAP (0x55)
+#define BLE_HCI_EVCODE_INQ_RSP_NOTIFY       (0x56)
+
+#define BLE_HCI_EVCODE_AUTH_PYLD_TMO        (0x57)
+struct ble_hci_ev_auth_pyld_tmo {
+    uint16_t conn_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_EVCODE_SAM_STATUS_CHG       (0x58)
+
+#define BLE_HCI_EVCODE_VENDOR_DEBUG         (0xFF)
+struct ble_hci_ev_vendor_debug {
+    uint8_t id;
+    uint8_t data[0];
+}__attribute__((packed));
+
+/* LE sub-event codes */
+#define BLE_HCI_LE_SUBEV_CONN_COMPLETE          (0x01)
+struct ble_hci_ev_le_subev_conn_complete {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint8_t  role;
+    uint8_t  peer_addr_type;
+    uint8_t  peer_addr[6];
+    uint16_t conn_itvl;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint8_t  mca;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_ADV_RPT                (0x02)
+struct adv_report {
+    uint8_t type;
+    uint8_t addr_type;
+    uint8_t addr[6];
+    uint8_t data_len;
+    uint8_t data[0];
+} __attribute__((packed));
+struct ble_hci_ev_le_subev_adv_rpt {
+    uint8_t  subev_code;
+    uint8_t  num_reports;
+    struct adv_report reports[0];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE      (0x03)
+struct ble_hci_ev_le_subev_conn_upd_complete {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint16_t conn_itvl;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT       (0x04)
+struct ble_hci_ev_le_subev_rd_rem_used_feat {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint8_t features[8];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_LT_KEY_REQ             (0x05)
+struct ble_hci_ev_le_subev_lt_key_req {
+    uint8_t  subev_code;
+    uint16_t conn_handle;
+    uint64_t rand;
+    uint16_t div;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ      (0x06)
+struct ble_hci_ev_le_subev_rem_conn_param_req {
+    uint8_t  subev_code;
+    uint16_t conn_handle;
+    uint16_t min_interval;
+    uint16_t max_interval;
+    uint16_t latency;
+    uint16_t timeout;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_DATA_LEN_CHG           (0x07)
+struct ble_hci_ev_le_subev_data_len_chg {
+    uint8_t  subev_code;
+    uint16_t conn_handle;
+    uint16_t max_tx_octets;
+    uint16_t max_tx_time;
+    uint16_t max_rx_octets;
+    uint16_t max_rx_time;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_RD_LOC_P256_PUBKEY     (0x08)
+struct ble_hci_ev_le_subev_rd_loc_p256_pubkey {
+    uint8_t subev_code;
+    uint8_t status;
+    uint8_t public_key[64];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_GEN_DHKEY_COMPLETE     (0x09)
+struct ble_hci_ev_le_subev_gen_dhkey_complete {
+    uint8_t subev_code;
+    uint8_t status;
+    uint8_t dh_key[32];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE      (0x0A)
+struct ble_hci_ev_le_subev_enh_conn_complete {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint8_t  role;
+    uint8_t  peer_addr_type;
+    uint8_t  peer_addr[6];
+    uint8_t  local_rpa[6];
+    uint8_t  peer_rpa[6];
+    uint16_t conn_itvl;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint8_t  mca;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT         (0x0B)
+struct dir_adv_report {
+    uint8_t type;
+    uint8_t addr_type;
+    uint8_t addr[6];
+    uint8_t dir_addr_type;
+    uint8_t dir_addr[6];
+    int8_t  rssi;
+} __attribute__((packed));
+struct ble_hci_ev_le_subev_direct_adv_rpt {
+    uint8_t subev_code;
+    uint8_t num_reports;
+    struct dir_adv_report reports[0];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE    (0x0C)
+struct ble_hci_ev_le_subev_phy_update_complete {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint8_t  tx_phy;
+    uint8_t  rx_phy;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_EXT_ADV_RPT            (0x0D)
+struct ext_adv_report {
+    uint16_t  evt_type;
+    uint8_t  addr_type;
+    uint8_t  addr[6];
+    uint8_t  pri_phy;
+    uint8_t  sec_phy;
+    uint8_t  sid;
+    int8_t   tx_power;
+    int8_t   rssi;
+    uint16_t periodic_itvl;
+    uint8_t  dir_addr_type;
+    uint8_t  dir_addr[6];
+    uint8_t  data_len;
+    uint8_t  data[0];
+} __attribute__((packed));
+struct ble_hci_ev_le_subev_ext_adv_rpt {
+    uint8_t subev_code;
+    uint8_t num_reports;
+    struct ext_adv_report reports[0];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB     (0x0E)
+struct ble_hci_ev_le_subev_periodic_adv_sync_estab {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t sync_handle;
+    uint8_t  sid;
+    uint8_t  peer_addr_type;
+    uint8_t  peer_addr[6];
+    uint8_t  phy;
+    uint16_t interval;
+    uint8_t  aca;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT            (0x0F)
+struct ble_hci_ev_le_subev_periodic_adv_rpt {
+    uint8_t  subev_code;
+    uint16_t sync_handle;
+    int8_t   tx_power;
+    int8_t   rssi;
+    uint8_t  cte_type;
+    uint8_t  data_status;
+    uint8_t  data_len;
+    uint8_t  data[0];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST      (0x10)
+struct ble_hci_ev_le_subev_periodic_adv_sync_lost {
+    uint8_t  subev_code;
+    uint16_t sync_handle;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_SCAN_TIMEOUT           (0x11)
+struct ble_hci_ev_le_subev_scan_timeout {
+    uint8_t  subev_code;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED     (0x12)
+struct ble_hci_ev_le_subev_adv_set_terminated {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint8_t  adv_handle;
+    uint16_t conn_handle;
+    uint8_t  num_events;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD          (0x13)
+struct ble_hci_ev_le_subev_scan_req_rcvd {
+    uint8_t subev_code;
+    uint8_t adv_handle;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_CHAN_SEL_ALG           (0x14)
+struct ble_hci_ev_le_subev_chan_sel_alg {
+    uint8_t  subev_code;
+    uint16_t conn_handle;
+    uint8_t  csa;
+} __attribute__((packed));
+
+#define BLE_HCI_LE_SUBEV_CONNLESS_IQ_RPT        (0x15)
+#define BLE_HCI_LE_SUBEV_CONN_IQ_RPT            (0x16)
+#define BLE_HCI_LE_SUBEV_CTE_REQ_FAILED         (0x17)
+
+#define BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER   (0x18)
+struct ble_hci_ev_le_subev_periodic_adv_sync_transfer {
+    uint8_t  subev_code;
+    uint8_t  status;
+    uint16_t conn_handle;
+    uint16_t service_data;
+    uint16_t sync_handle;
+    uint8_t  sid;
+    uint8_t  peer_addr_type;
+    uint8_t  peer_addr[6];
+    uint8_t  phy;
+    uint16_t interval;
+    uint8_t  aca;
+} __attribute__((packed));
+
+/* Data buffer overflow event */
+#define BLE_HCI_EVENT_ACL_BUF_OVERFLOW      (0x01)
+
+/* Advertising report */
+#define BLE_HCI_ADV_RPT_EVTYPE_ADV_IND      (0)
+#define BLE_HCI_ADV_RPT_EVTYPE_DIR_IND      (1)
+#define BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND     (2)
+#define BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND  (3)
+#define BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP     (4)
+
+/* Bluetooth 5, Vol 2, Part E, 7.7.65.13 */
+#define BLE_HCI_LEGACY_ADV_EVTYPE_ADV_IND                 (0x13)
+#define BLE_HCI_LEGACY_ADV_EVTYPE_ADV_DIRECT_IND          (0x15)
+#define BLE_HCI_LEGACY_ADV_EVTYPE_ADV_SCAN_IND            (0x12)
+#define BLE_HCI_LEGACY_ADV_EVTYPE_ADV_NONCON_IND          (0x10)
+#define BLE_HCI_LEGACY_ADV_EVTYPE_SCAN_RSP_ADV_IND        (0x1b)
+#define BLE_HCI_LEGACY_ADV_EVTYPE_SCAN_RSP_ADV_SCAN_IND   (0x1a)
+
+/* LE connection complete event (sub event 0x01) */
+#define BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER    (0x00)
+#define BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE     (0x01)
+
+/* Maximum valid connection handle value */
+#define BLE_HCI_LE_CONN_HANDLE_MAX              (0x0eff)
+
+/* LE advertising report event. (sub event 0x02) */
+#define BLE_HCI_LE_ADV_RPT_NUM_RPTS_MIN     (1)
+#define BLE_HCI_LE_ADV_RPT_NUM_RPTS_MAX     (0x19)
+
+/* Bluetooth Assigned numbers for version information.*/
+#define BLE_HCI_VER_BCS_1_0b                (0)
+#define BLE_HCI_VER_BCS_1_1                 (1)
+#define BLE_HCI_VER_BCS_1_2                 (2)
+#define BLE_HCI_VER_BCS_2_0_EDR             (3)
+#define BLE_HCI_VER_BCS_2_1_EDR             (4)
+#define BLE_HCI_VER_BCS_3_0_HCS             (5)
+#define BLE_HCI_VER_BCS_4_0                 (6)
+#define BLE_HCI_VER_BCS_4_1                 (7)
+#define BLE_HCI_VER_BCS_4_2                 (8)
+#define BLE_HCI_VER_BCS_5_0                 (9)
+#define BLE_HCI_VER_BCS_5_1                 (10)
+#define BLE_HCI_VER_BCS_5_2                 (11)
+
+#define BLE_LMP_VER_BCS_1_0b                (0)
+#define BLE_LMP_VER_BCS_1_1                 (1)
+#define BLE_LMP_VER_BCS_1_2                 (2)
+#define BLE_LMP_VER_BCS_2_0_EDR             (3)
+#define BLE_LMP_VER_BCS_2_1_EDR             (4)
+#define BLE_LMP_VER_BCS_3_0_HCS             (5)
+#define BLE_LMP_VER_BCS_4_0                 (6)
+#define BLE_LMP_VER_BCS_4_1                 (7)
+#define BLE_LMP_VER_BCS_4_2                 (8)
+#define BLE_LMP_VER_BCS_5_0                 (9)
+#define BLE_LMP_VER_BCS_5_1                 (10)
+#define BLE_LMP_VER_BCS_5_2                 (11)
+
+/* selected HCI and LMP version */
+#if MYNEWT_VAL(BLE_VERSION) == 50
+#define BLE_HCI_VER_BCS BLE_HCI_VER_BCS_5_0
+#define BLE_LMP_VER_BCS BLE_LMP_VER_BCS_5_0
+#elif MYNEWT_VAL(BLE_VERSION) == 51
+#define BLE_HCI_VER_BCS BLE_HCI_VER_BCS_5_1
+#define BLE_LMP_VER_BCS BLE_LMP_VER_BCS_5_1
+#elif MYNEWT_VAL(BLE_VERSION) == 52
+#define BLE_HCI_VER_BCS BLE_HCI_VER_BCS_5_2
+#define BLE_LMP_VER_BCS BLE_LMP_VER_BCS_5_2
+
+#endif
+
+#define BLE_HCI_DATA_HDR_SZ                 4
+#define BLE_HCI_DATA_HANDLE(handle_pb_bc)   (((handle_pb_bc) & 0x0fff) >> 0)
+#define BLE_HCI_DATA_PB(handle_pb_bc)       (((handle_pb_bc) & 0x3000) >> 12)
+#define BLE_HCI_DATA_BC(handle_pb_bc)       (((handle_pb_bc) & 0xc000) >> 14)
+
+struct hci_data_hdr
+{
+    uint16_t hdh_handle_pb_bc;
+    uint16_t hdh_len;
+};
+
+#define BLE_HCI_PB_FIRST_NON_FLUSH          0
+#define BLE_HCI_PB_MIDDLE                   1
+#define BLE_HCI_PB_FIRST_FLUSH              2
+#define BLE_HCI_PB_FULL                     3
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_HCI_COMMON_ */
diff --git a/src/libs/mynewt-nimble/nimble/include/nimble/nimble_npl.h b/src/libs/mynewt-nimble/nimble/include/nimble/nimble_npl.h
new file mode 100644
index 00000000..5935b740
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/include/nimble/nimble_npl.h
@@ -0,0 +1,174 @@
+/*
+ * 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 _NIMBLE_NPL_H_
+#define _NIMBLE_NPL_H_
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_npl_event;
+typedef void ble_npl_event_fn(struct ble_npl_event *ev);
+
+enum ble_npl_error {
+    BLE_NPL_OK = 0,
+    BLE_NPL_ENOMEM = 1,
+    BLE_NPL_EINVAL = 2,
+    BLE_NPL_INVALID_PARAM = 3,
+    BLE_NPL_MEM_NOT_ALIGNED = 4,
+    BLE_NPL_BAD_MUTEX = 5,
+    BLE_NPL_TIMEOUT = 6,
+    BLE_NPL_ERR_IN_ISR = 7,
+    BLE_NPL_ERR_PRIV = 8,
+    BLE_NPL_OS_NOT_STARTED = 9,
+    BLE_NPL_ENOENT = 10,
+    BLE_NPL_EBUSY = 11,
+    BLE_NPL_ERROR = 12,
+};
+
+typedef enum ble_npl_error ble_npl_error_t;
+
+/* Include OS-specific definitions */
+#include "nimble/nimble_npl_os.h"
+
+/*
+ * Generic
+ */
+
+bool ble_npl_os_started(void);
+
+void *ble_npl_get_current_task_id(void);
+
+/*
+ * Event queue
+ */
+
+void ble_npl_eventq_init(struct ble_npl_eventq *evq);
+
+struct ble_npl_event *ble_npl_eventq_get(struct ble_npl_eventq *evq,
+                                         ble_npl_time_t tmo);
+
+void ble_npl_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev);
+
+void ble_npl_eventq_remove(struct ble_npl_eventq *evq,
+                           struct ble_npl_event *ev);
+
+void ble_npl_event_init(struct ble_npl_event *ev, ble_npl_event_fn *fn,
+                        void *arg);
+
+bool ble_npl_event_is_queued(struct ble_npl_event *ev);
+
+void *ble_npl_event_get_arg(struct ble_npl_event *ev);
+
+void ble_npl_event_set_arg(struct ble_npl_event *ev, void *arg);
+
+bool ble_npl_eventq_is_empty(struct ble_npl_eventq *evq);
+
+void ble_npl_event_run(struct ble_npl_event *ev);
+
+/*
+ * Mutexes
+ */
+
+ble_npl_error_t ble_npl_mutex_init(struct ble_npl_mutex *mu);
+
+ble_npl_error_t ble_npl_mutex_pend(struct ble_npl_mutex *mu,
+                                   ble_npl_time_t timeout);
+
+ble_npl_error_t ble_npl_mutex_release(struct ble_npl_mutex *mu);
+
+/*
+ * Semaphores
+ */
+
+ble_npl_error_t ble_npl_sem_init(struct ble_npl_sem *sem, uint16_t tokens);
+
+ble_npl_error_t ble_npl_sem_pend(struct ble_npl_sem *sem,
+                                 ble_npl_time_t timeout);
+
+ble_npl_error_t ble_npl_sem_release(struct ble_npl_sem *sem);
+
+uint16_t ble_npl_sem_get_count(struct ble_npl_sem *sem);
+
+/*
+ * Callouts
+ */
+
+void ble_npl_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq,
+                          ble_npl_event_fn *ev_cb, void *ev_arg);
+
+ble_npl_error_t ble_npl_callout_reset(struct ble_npl_callout *co,
+                                      ble_npl_time_t ticks);
+
+void ble_npl_callout_stop(struct ble_npl_callout *co);
+
+bool ble_npl_callout_is_active(struct ble_npl_callout *co);
+
+ble_npl_time_t ble_npl_callout_get_ticks(struct ble_npl_callout *co);
+
+ble_npl_time_t ble_npl_callout_remaining_ticks(struct ble_npl_callout *co,
+                                               ble_npl_time_t time);
+
+void ble_npl_callout_set_arg(struct ble_npl_callout *co,
+                             void *arg);
+/*
+ * Time functions
+ */
+
+ble_npl_time_t ble_npl_time_get(void);
+
+ble_npl_error_t ble_npl_time_ms_to_ticks(uint32_t ms, ble_npl_time_t *out_ticks);
+
+ble_npl_error_t ble_npl_time_ticks_to_ms(ble_npl_time_t ticks, uint32_t *out_ms);
+
+ble_npl_time_t ble_npl_time_ms_to_ticks32(uint32_t ms);
+
+uint32_t ble_npl_time_ticks_to_ms32(ble_npl_time_t ticks);
+
+void ble_npl_time_delay(ble_npl_time_t ticks);
+
+/*
+ * Hardware-specific
+ *
+ * These symbols should be most likely defined by application since they are
+ * specific to hardware, not to OS.
+ */
+
+#if NIMBLE_CFG_CONTROLLER
+
+void ble_npl_hw_set_isr(int irqn, void (*addr)(void));
+
+#endif
+
+uint32_t ble_npl_hw_enter_critical(void);
+
+void ble_npl_hw_exit_critical(uint32_t ctx);
+
+bool ble_npl_hw_is_in_critical(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* _NIMBLE_NPL_H_ */
diff --git a/src/libs/mynewt-nimble/nimble/include/nimble/nimble_opt.h b/src/libs/mynewt-nimble/nimble/include/nimble/nimble_opt.h
new file mode 100644
index 00000000..f0e988b2
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/include/nimble/nimble_opt.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_NIMBLE_OPT_
+#define H_NIMBLE_OPT_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Include automatically-generated settings. */
+#include "nimble/nimble_opt_auto.h"
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/include/nimble/nimble_opt_auto.h b/src/libs/mynewt-nimble/nimble/include/nimble/nimble_opt_auto.h
new file mode 100644
index 00000000..33a1e2aa
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/include/nimble/nimble_opt_auto.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_NIMBLE_OPT_AUTO_
+#define H_NIMBLE_OPT_AUTO_
+
+#include "syscfg/syscfg.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/***
+ * Automatic options.
+ *
+ * These settings are generated automatically from the user-specified syscfg
+ * settings.
+ */
+
+#undef NIMBLE_BLE_ADVERTISE
+#define NIMBLE_BLE_ADVERTISE                    \
+    (MYNEWT_VAL(BLE_ROLE_BROADCASTER) || MYNEWT_VAL(BLE_ROLE_PERIPHERAL))
+
+#undef NIMBLE_BLE_SCAN
+#define NIMBLE_BLE_SCAN                         \
+    (MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_OBSERVER))
+
+#undef NIMBLE_BLE_CONNECT
+#define NIMBLE_BLE_CONNECT                      \
+    (MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_PERIPHERAL))
+
+
+/** Supported client ATT commands. */
+
+#undef NIMBLE_BLE_ATT_CLT_FIND_INFO
+#define NIMBLE_BLE_ATT_CLT_FIND_INFO            \
+    (MYNEWT_VAL(BLE_GATT_DISC_ALL_DSCS))
+
+#undef NIMBLE_BLE_ATT_CLT_FIND_TYPE
+#define NIMBLE_BLE_ATT_CLT_FIND_TYPE            \
+    (MYNEWT_VAL(BLE_GATT_DISC_SVC_UUID))
+
+#undef NIMBLE_BLE_ATT_CLT_READ_TYPE
+#define NIMBLE_BLE_ATT_CLT_READ_TYPE            \
+    (MYNEWT_VAL(BLE_GATT_FIND_INC_SVCS) ||      \
+     MYNEWT_VAL(BLE_GATT_DISC_ALL_CHRS) ||      \
+     MYNEWT_VAL(BLE_GATT_DISC_CHRS_UUID) ||     \
+     MYNEWT_VAL(BLE_GATT_READ_UUID))
+
+#undef NIMBLE_BLE_ATT_CLT_READ
+#define NIMBLE_BLE_ATT_CLT_READ                 \
+    (MYNEWT_VAL(BLE_GATT_READ) ||               \
+     MYNEWT_VAL(BLE_GATT_READ_LONG) ||          \
+     MYNEWT_VAL(BLE_GATT_FIND_INC_SVCS))
+
+#undef NIMBLE_BLE_ATT_CLT_READ_BLOB
+#define NIMBLE_BLE_ATT_CLT_READ_BLOB            \
+    (MYNEWT_VAL(BLE_GATT_READ_LONG))
+
+#undef NIMBLE_BLE_ATT_CLT_READ_MULT
+#define NIMBLE_BLE_ATT_CLT_READ_MULT            \
+    (MYNEWT_VAL(BLE_GATT_READ_MULT))
+
+#undef NIMBLE_BLE_ATT_CLT_READ_GROUP_TYPE
+#define NIMBLE_BLE_ATT_CLT_READ_GROUP_TYPE      \
+    (MYNEWT_VAL(BLE_GATT_DISC_ALL_SVCS))
+
+#undef NIMBLE_BLE_ATT_CLT_WRITE
+#define NIMBLE_BLE_ATT_CLT_WRITE                \
+    (MYNEWT_VAL(BLE_GATT_WRITE))
+
+#undef NIMBLE_BLE_ATT_CLT_WRITE_NO_RSP
+#define NIMBLE_BLE_ATT_CLT_WRITE_NO_RSP         \
+    (MYNEWT_VAL(BLE_GATT_WRITE_NO_RSP))
+
+#undef NIMBLE_BLE_ATT_CLT_PREP_WRITE
+#define NIMBLE_BLE_ATT_CLT_PREP_WRITE           \
+    (MYNEWT_VAL(BLE_GATT_WRITE_LONG))
+
+#undef NIMBLE_BLE_ATT_CLT_EXEC_WRITE
+#define NIMBLE_BLE_ATT_CLT_EXEC_WRITE           \
+    (MYNEWT_VAL(BLE_GATT_WRITE_LONG))
+
+#undef NIMBLE_BLE_ATT_CLT_NOTIFY
+#define NIMBLE_BLE_ATT_CLT_NOTIFY               \
+    (MYNEWT_VAL(BLE_GATT_NOTIFY))
+
+#undef NIMBLE_BLE_ATT_CLT_INDICATE
+#define NIMBLE_BLE_ATT_CLT_INDICATE             \
+    (MYNEWT_VAL(BLE_GATT_INDICATE))
+
+/** Security manager settings. */
+
+#undef NIMBLE_BLE_SM
+#define NIMBLE_BLE_SM   (MYNEWT_VAL(BLE_SM_LEGACY) || MYNEWT_VAL(BLE_SM_SC))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/pkg.yml b/src/libs/mynewt-nimble/nimble/pkg.yml
new file mode 100644
index 00000000..8b6058e1
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/pkg.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.
+#
+
+pkg.name: nimble
+pkg.description: Generic functionality for the nimble Bluetooth Smart stack; used by both the controller and the host.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - porting/npl/mynewt
+    - "@apache-mynewt-core/kernel/os"
diff --git a/src/libs/mynewt-nimble/nimble/syscfg.yml b/src/libs/mynewt-nimble/nimble/syscfg.yml
new file mode 100644
index 00000000..537fd9ba
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/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:
+    # Supported GAP roles.  By default, all four roles are enabled.
+    BLE_ROLE_CENTRAL:
+        description: 'Enables the Central bluetooth role. (0/1)'
+        value: 1
+    BLE_ROLE_PERIPHERAL:
+        description: 'Enables the Peripheral bluetooth role. (0/1)'
+        value: 1
+    BLE_ROLE_BROADCASTER:
+        description: 'Enables the Broadcaster bluetooth role. (0/1)'
+        value: 1
+    BLE_ROLE_OBSERVER:
+        description: 'Enables the Observer bluetooth role. (0/1)'
+        value: 1
+
+    BLE_MAX_CONNECTIONS:
+        description: 'The maximum number of concurrent connections.'
+        value: 1
+    BLE_MAX_PERIODIC_SYNCS:
+        description: >
+            The maximum number of concurrent periodic syncs that can 
+            be created
+        value: 1
+    BLE_WHITELIST:
+        description: >
+            Enables the BLE whitelist for controlling who to connect to or
+            accept a connection from. (0/1)
+        value: 1
+    BLE_MULTI_ADV_INSTANCES:
+        description: >
+            This is the number of multi-advertising instances. This is NOT the
+            total number of advertising instances. The total number of
+            advertising instances is this number plus 1 (assuming the device
+            supports advertising).
+        value: 0
+    BLE_EXT_ADV:
+        description: >
+            This enables extended advertising feature.
+        value: 0
+    BLE_PERIODIC_ADV:
+        description: >
+            This enables periodic advertising feature.
+        value: 0
+    BLE_PERIODIC_ADV_SYNC_TRANSFER:
+        description: >
+            This enables Periodic Advertising Sync Transfer Feature.
+        value: 0
+
+    BLE_EXT_ADV_MAX_SIZE:
+        description: >
+            This allows to configure maximum size of advertising data and
+            scan response data used in LE Advertising Extensions.
+            Valid range 31-1650.
+        value: 31
+    BLE_VERSION:
+        description: >
+            This allows to configure supported Bluetooth Core version. Some
+            features may not be available if version is too low. Version is
+            integer for easy comparison.
+        range: 50, 51, 52
+        value: 50
+
+# Allow periodic sync transfer only if 5.1 or higher
+syscfg.restrictions:
+    - "'BLE_PERIODIC_ADV_SYNC_TRANSFER == 0' || 'BLE_VERSION >= 51'"
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/.gitignore b/src/libs/mynewt-nimble/nimble/transport/da1469x/.gitignore
new file mode 100644
index 00000000..bba9f995
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/.gitignore
@@ -0,0 +1,2 @@
+/src/libble_stack_da1469x.a
+
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/README b/src/libs/mynewt-nimble/nimble/transport/da1469x/README
new file mode 100644
index 00000000..b30a01c4
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/README
@@ -0,0 +1,13 @@
+Integrated BLE Controller (CMAC) requires a binary firmware to be loaded. Such
+firmware is distributed by Dialog Semiconductor in SDK package which has to be
+obtained separately, see:
+https://www.dialog-semiconductor.com/products/da1469x-product-family
+
+Firmware is available as part of following library in SDK package:
+sdk/interfaces/ble/binaries/DA1469x-Release/libble_stack_da1469x.a
+
+By default, CMAC driver will look for this file in its package root directory
+(i.e. nimble/transport/da1469x/cmac_driver) but this can be changed using
+CMAC_IMAGE_FILE_NAME syscfg variable.
+
+Current version of CMAC driver was tested with SDK version 10.0.4.66.
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/include/cmac_driver/cmac_host.h b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/include/cmac_driver/cmac_host.h
new file mode 100644
index 00000000..f623a48a
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/include/cmac_driver/cmac_host.h
@@ -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.
+ */
+
+#ifndef __CMAC_HOST_H_
+#define __CMAC_HOST_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef int (* cmac_mbox_read_cb)(const uint8_t *, uint16_t len);
+
+void cmac_host_init(void);
+void cmac_mbox_write(const uint8_t *buf, size_t len);
+void cmac_mbox_set_read_cb(cmac_mbox_read_cb cb);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CMAC_HOST_H_ */
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/pkg.yml
new file mode 100644
index 00000000..90a89318
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/pkg.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.
+#
+
+pkg.name: nimble/transport/da1469x/cmac_driver
+pkg.description: Driver for Dialog's BLE controller
+pkg.author:
+pkg.homepage:
+pkg.keywords:
+
+pkg.pre_link_cmds.BLE_HOST:
+    scripts/build_libcmac.sh: 100
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/scripts/build_libcmac.sh b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/scripts/build_libcmac.sh
new file mode 100755
index 00000000..659c2b9f
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/scripts/build_libcmac.sh
@@ -0,0 +1,51 @@
+#!/bin/bash
+
+# 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 [ ! -f ${MYNEWT_VAL_CMAC_IMAGE_FILE_NAME} ]; then
+    >&2 echo ERROR: BLE stack library not found. Please check nimble/transport/da1469x/README for details.
+    exit 1
+fi
+
+OBJCOPY=${MYNEWT_OBJCOPY_PATH}
+AR=${MYNEWT_AR_PATH}
+LIBBLE_A=$(readlink -e ${MYNEWT_VAL_CMAC_IMAGE_FILE_NAME})
+LIBCMAC_A=${MYNEWT_USER_SRC_DIR}/libcmac.a
+
+BASENAME_ROM=cmac.rom
+BASENAME_RAM=cmac.ram
+
+cd ${MYNEWT_USER_WORK_DIR}
+
+# Extract firmware binary from .a since we do not need to link all other
+# objects with our image.
+${AR} x ${LIBBLE_A} cmac_fw.c.obj
+${OBJCOPY} -O binary --only-section=.cmi_fw_area cmac_fw.c.obj cmac.bin
+
+# We need separate copies for RAM and ROM image since section names are derived
+# from file names. For now files are the same, but it would be possible to
+# link ROM image without data and shared sections which contains zeroes anyway.
+cp cmac.bin ${BASENAME_ROM}.bin
+cp cmac.bin ${BASENAME_RAM}.bin
+
+# Convert both binaries to objects and create archive to link
+${OBJCOPY} -I binary -O elf32-littlearm -B armv8-m.main \
+    --rename-section .data=.libcmac.rom ${BASENAME_ROM}.bin ${BASENAME_ROM}.o
+${OBJCOPY} -I binary -O elf32-littlearm -B armv8-m.main \
+    --rename-section .data=.libcmac.ram ${BASENAME_RAM}.bin ${BASENAME_RAM}.o
+${AR} -rcs ${LIBCMAC_A} ${BASENAME_ROM}.o ${BASENAME_RAM}.o
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/src/cmac_host.c b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/src/cmac_host.c
new file mode 100644
index 00000000..0bfb715e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/src/cmac_host.c
@@ -0,0 +1,326 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include "os/os.h"
+#include "mcu/cmsis_nvic.h"
+#include "mcu/da1469x_lpclk.h"
+#include "mcu/da1469x_hal.h"
+#include "mcu/da1469x_pdc.h"
+#include "mcu/mcu.h"
+#include "cmac_driver/cmac_host.h"
+
+#define CMAC_SYM_CONFIG     ((void *)(0x00818f20 + MEMCTRL->CMI_CODE_BASE_REG))
+#define CMAC_SYM_CONFIG_DYN ((void *)(0x00821af8 + MEMCTRL->CMI_CODE_BASE_REG))
+#define CMAC_SYM_MBOX_RX    ((void *)(0x008216b0 + MEMCTRL->CMI_CODE_BASE_REG))
+#define CMAC_SYM_MBOX_TX    ((void *)(0x008218b0 + MEMCTRL->CMI_CODE_BASE_REG))
+
+#define CMAC_MBOX_SIZE              504
+#define CMAC_MBOX_F_RESET           0x0008
+#define CMAC_MBOX_F_WRITEPENDING    0x0010
+
+struct cmac_config {
+    uint8_t  bdaddr[6];             /* Device address */
+
+    uint8_t  rf_calibration_delay;
+
+    uint8_t  lp_clock_freq;         /* Sleep clock frequency (0 = 32768Hz, 1 = 32000Hz) */
+    uint16_t lp_clock_sca;          /* Sleep clock accuracy [ppm] */
+
+    uint16_t rx_buf_len;            /* RX buffer size */
+    uint16_t tx_buf_len;            /* TX buffer size */
+    bool     initial_length_req;
+
+    /* Channel assessment algorithm settings */
+    uint16_t chan_assess_itvl;
+    uint8_t  chan_assess_itvl_mult;
+    int8_t   chan_assess_min_rssi;
+    uint16_t chan_assess_pkt_num;
+    uint16_t chan_assess_bad_pkt_num;
+
+    /* Calibration settings */
+    uint8_t  system_tcs_length;
+    uint8_t  synth_tcs_length;
+    uint8_t  rfcu_tcs_length;
+
+    uint8_t  default_tx_power;      /* Default TX power for connection/advertising */
+    bool     filter_dup_ov_discard; /* Discard unknown devices when filter buffer is full */
+    bool     use_hp_1m;
+    bool     use_hp_2m;
+};
+
+struct cmac_config_dynamic {
+    bool     enable_sleep;          /* Enable sleep */
+
+    /* More options here, don't care now */
+};
+
+struct cmac_mbox {
+    volatile uint16_t magic;
+    volatile uint16_t flags;
+    volatile uint16_t wr_off;
+    volatile uint16_t rd_off;
+    uint8_t data[CMAC_MBOX_SIZE];
+};
+
+/* CMAC data */
+extern char _binary_cmac_rom_bin_start[];
+extern char _binary_cmac_rom_bin_end;
+extern char _binary_cmac_ram_bin_start[];
+extern char _binary_cmac_ram_bin_end;
+
+struct cmac_image_info {
+    uint32_t _dummy1;
+    uint32_t size_rom;
+    uint32_t _dummy2;
+    uint32_t magic;
+    uint32_t _dummy3[5];
+};
+
+/* Mailboxes for SYS<->CMAC communication */
+static struct cmac_mbox *cmac_mbox_rx;
+static struct cmac_mbox *cmac_mbox_tx;
+
+/* PDC entry for waking up CMAC */
+static int8_t g_cmac_host_pdc_sys2cmac;
+/* PDC entry for waking up M33 */
+static int8_t g_cmac_host_pdc_cmac2sys;
+/* Callback for data ready from CMAC */
+static cmac_mbox_read_cb g_cmac_mbox_read_cb;
+
+static inline void
+cmac_host_signal2cmac(void)
+{
+    da1469x_pdc_set(g_cmac_host_pdc_sys2cmac);
+}
+
+static void
+cmac2sys_isr(void)
+{
+    uint16_t wr_off;
+    uint16_t rd_off;
+    uint16_t chunk;
+    uint16_t len;
+
+    os_trace_isr_enter();
+
+    /* Clear CMAC2SYS interrupt */
+    *(volatile uint32_t *)0x40002000 = 2;
+
+    if (*(volatile uint32_t *)0x40002000 & 0x1c00) {
+        /* XXX CMAC is in error state, need to recover */
+        assert(0);
+    }
+
+    if (cmac_mbox_rx->flags & CMAC_MBOX_F_RESET) {
+        cmac_mbox_rx->flags &= ~CMAC_MBOX_F_RESET;
+        goto done;
+    }
+
+    if (!g_cmac_mbox_read_cb) {
+        cmac_mbox_rx->rd_off = cmac_mbox_rx->wr_off;
+        goto done;
+    }
+
+    do {
+        rd_off = cmac_mbox_rx->rd_off;
+        wr_off = cmac_mbox_rx->wr_off;
+
+        if (rd_off <= wr_off) {
+            chunk = wr_off - rd_off;
+        } else {
+            chunk = CMAC_MBOX_SIZE - rd_off;
+        }
+
+        while (chunk) {
+            len = g_cmac_mbox_read_cb(&cmac_mbox_rx->data[rd_off], chunk);
+
+            rd_off += len;
+            chunk -= len;
+        };
+
+        cmac_mbox_rx->rd_off = rd_off % CMAC_MBOX_SIZE;
+    } while (cmac_mbox_rx->rd_off != cmac_mbox_rx->wr_off);
+
+done:
+
+    if (cmac_mbox_rx->flags & CMAC_MBOX_F_WRITEPENDING) {
+        cmac_host_signal2cmac();
+    }
+
+    os_trace_isr_exit();
+}
+
+static void
+cmac_host_lpclk_cb(uint32_t freq)
+{
+    struct cmac_config_dynamic *cmac_config_dyn;
+
+    cmac_config_dyn = CMAC_SYM_CONFIG_DYN;
+    cmac_config_dyn->enable_sleep = freq == 32768;
+}
+
+void
+cmac_mbox_write(const uint8_t *buf, size_t len)
+{
+    uint16_t wr_off;
+    uint16_t rd_off;
+    uint16_t chunk;
+    uint32_t primask;
+
+    __HAL_DISABLE_INTERRUPTS(primask);
+
+    while (len) {
+        rd_off = cmac_mbox_tx->rd_off;
+        wr_off = cmac_mbox_tx->wr_off;
+
+        if (rd_off > wr_off) {
+            chunk = min(len, rd_off - wr_off);
+        } else {
+            chunk = min(len, CMAC_MBOX_SIZE - wr_off);
+        }
+
+        if (chunk == 0) {
+            continue;
+        }
+
+        memcpy(&cmac_mbox_tx->data[wr_off], buf, chunk);
+
+        wr_off += chunk;
+        cmac_mbox_tx->wr_off = wr_off % CMAC_MBOX_SIZE;
+
+        cmac_host_signal2cmac();
+
+        len -= chunk;
+        buf += chunk;
+    }
+
+    __HAL_ENABLE_INTERRUPTS(primask);
+}
+
+void
+cmac_mbox_set_read_cb(cmac_mbox_read_cb cb)
+{
+    g_cmac_mbox_read_cb = cb;
+}
+
+void
+cmac_host_init(void)
+{
+    struct cmac_image_info ii;
+    uint32_t cmac_rom_size;
+    uint32_t cmac_ram_size;
+    struct cmac_config *cmac_config;
+    struct cmac_config_dynamic *cmac_config_dyn;
+
+    /* Add PDC entry to wake up CMAC from M33 */
+    g_cmac_host_pdc_sys2cmac = da1469x_pdc_add(MCU_PDC_TRIGGER_MAC_TIMER,
+                                             MCU_PDC_MASTER_CMAC,
+                                             MCU_PDC_EN_XTAL);
+    da1469x_pdc_set(g_cmac_host_pdc_sys2cmac);
+    da1469x_pdc_ack(g_cmac_host_pdc_sys2cmac);
+
+    /* Add PDC entry to wake up M33 from CMAC, if does not exist yet */
+    g_cmac_host_pdc_cmac2sys = da1469x_pdc_find(MCU_PDC_TRIGGER_COMBO,
+                                              MCU_PDC_MASTER_M33, 0);
+    if (g_cmac_host_pdc_cmac2sys < 0) {
+        g_cmac_host_pdc_cmac2sys = da1469x_pdc_add(MCU_PDC_TRIGGER_COMBO,
+                                                 MCU_PDC_MASTER_M33,
+                                                 MCU_PDC_EN_XTAL);
+        da1469x_pdc_set(g_cmac_host_pdc_cmac2sys);
+        da1469x_pdc_ack(g_cmac_host_pdc_cmac2sys);
+    }
+
+    /* Enable Radio LDO */
+    CRG_TOP->POWER_CTRL_REG |= CRG_TOP_POWER_CTRL_REG_LDO_RADIO_ENABLE_Msk;
+
+    /* Enable CMAC, but keep it in reset */
+    CRG_TOP->CLK_RADIO_REG = (1 << CRG_TOP_CLK_RADIO_REG_RFCU_ENABLE_Pos) |
+                             (1 << CRG_TOP_CLK_RADIO_REG_CMAC_SYNCH_RESET_Pos) |
+                             (0 << CRG_TOP_CLK_RADIO_REG_CMAC_CLK_SEL_Pos) |
+                             (1 << CRG_TOP_CLK_RADIO_REG_CMAC_CLK_ENABLE_Pos) |
+                             (0 << CRG_TOP_CLK_RADIO_REG_CMAC_DIV_Pos);
+
+    /* Calculate size of ROM and RAM area (for now they should be the same) */
+    cmac_rom_size = &_binary_cmac_rom_bin_end - &_binary_cmac_rom_bin_start[0];
+    cmac_ram_size = &_binary_cmac_ram_bin_end - &_binary_cmac_ram_bin_start[0];
+    assert(cmac_rom_size == cmac_ram_size);
+
+    /* Load image header and check if image can be loaded */
+    memcpy(&ii, &_binary_cmac_rom_bin_start, sizeof(ii));
+    assert(ii.magic == 0x43414d43); /* "CMAC" */
+
+    /* Copy CMAC image to RAM */
+    memset(&_binary_cmac_ram_bin_start, 0, cmac_ram_size);
+    memcpy(&_binary_cmac_ram_bin_start, &_binary_cmac_rom_bin_start[sizeof(ii)],
+           ii.size_rom);
+
+    /*
+     * Setup memory controller for CMAC
+     * Code and data are set to the same address initially since CMAC will
+     * update data address on init. Also shared address is updated on init.
+     */
+    MEMCTRL->CMI_CODE_BASE_REG = (uint32_t)&_binary_cmac_ram_bin_start;
+    MEMCTRL->CMI_DATA_BASE_REG = MEMCTRL->CMI_CODE_BASE_REG;
+    MEMCTRL->CMI_SHARED_BASE_REG = 0;
+    MEMCTRL->CMI_END_REG = (uint32_t)&_binary_cmac_ram_bin_end;
+
+    /* Symbols below are in shared memory, can update them now */
+    cmac_config = CMAC_SYM_CONFIG;
+    cmac_config_dyn = CMAC_SYM_CONFIG_DYN;
+    cmac_mbox_rx = CMAC_SYM_MBOX_RX;
+    cmac_mbox_tx = CMAC_SYM_MBOX_TX;
+
+    /* Update CMAC configuration */
+    cmac_config->lp_clock_freq = 0;
+    cmac_config->lp_clock_sca = 50;
+    cmac_config->rx_buf_len = 251 + 11;
+    cmac_config->tx_buf_len = 251 + 11;
+    cmac_config->initial_length_req = 0;
+    cmac_config->system_tcs_length = 0;
+    cmac_config->synth_tcs_length = 0;
+    cmac_config->rfcu_tcs_length = 0;
+    cmac_config->default_tx_power = 4;
+    cmac_config_dyn->enable_sleep = false;
+
+    /* Release CMAC from reset */
+    CRG_TOP->CLK_RADIO_REG &= ~CRG_TOP_CLK_RADIO_REG_CMAC_SYNCH_RESET_Msk;
+
+    /* Wait for CMAC to update registers */
+    while (MEMCTRL->CMI_DATA_BASE_REG == MEMCTRL->CMI_CODE_BASE_REG);
+    while (MEMCTRL->CMI_SHARED_BASE_REG != (MEMCTRL->CMI_END_REG & 0xfffffc00));
+
+    /* Initialize mailboxes and sync with CMAC */
+    cmac_mbox_tx->flags = CMAC_MBOX_F_RESET;
+    cmac_mbox_tx->wr_off = 0;
+    cmac_mbox_tx->rd_off = 0;
+    cmac_mbox_tx->magic = 0xa55a;
+    while (cmac_mbox_rx->magic != 0xa55a);
+
+    NVIC_SetVector(CMAC2SYS_IRQn, (uint32_t)cmac2sys_isr);
+    NVIC_SetPriority(CMAC2SYS_IRQn, 0);
+    NVIC_EnableIRQ(CMAC2SYS_IRQn);
+
+    cmac_host_signal2cmac();
+
+    da1469x_lpclk_register_cmac_cb(cmac_host_lpclk_cb);
+}
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/syscfg.yml
new file mode 100644
index 00000000..be2e62d9
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/cmac_driver/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:
+    CMAC_IMAGE_FILE_NAME:
+        description: >
+            Path to library with CMAC firmware. See README for details.
+        value: "libble_stack_da1469x.a"
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/da1469x/pkg.yml
new file mode 100644
index 00000000..5d9b533c
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/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/transport/da1469x
+pkg.description: HCI transport for DA1469x
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - "@apache-mynewt-nimble/nimble"
+    - "@apache-mynewt-nimble/nimble/transport/da1469x/cmac_driver"
+    - "@apache-mynewt-core/kernel/os"
+
+pkg.apis:
+    - ble_transport
+
+pkg.init:
+    da1469x_ble_hci_init: 100
+    da1469x_ble_hci_cmac_init: 201
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/src/da1469x_ble_hci.c b/src/libs/mynewt-nimble/nimble/transport/da1469x/src/da1469x_ble_hci.c
new file mode 100644
index 00000000..971d0cd0
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/src/da1469x_ble_hci.c
@@ -0,0 +1,368 @@
+/*
+ * 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 "os/mynewt.h"
+#include "nimble/ble.h"
+#include "nimble/ble_hci_trans.h"
+#include "nimble/hci_common.h"
+#include "cmac_driver/cmac_host.h"
+
+#define HCI_PKT_NONE    0x00
+#define HCI_PKT_CMD     0x01
+#define HCI_PKT_ACL     0x02
+#define HCI_PKT_EVT     0x04
+
+#define POOL_ACL_BLOCK_SIZE                                             \
+                        OS_ALIGN(MYNEWT_VAL(BLE_ACL_BUF_SIZE) +         \
+                                BLE_MBUF_MEMBLOCK_OVERHEAD +            \
+                                BLE_HCI_DATA_HDR_SZ, OS_ALIGNMENT)
+
+struct da1469x_ble_hci_host_api {
+    ble_hci_trans_rx_cmd_fn *evt_cb;
+    void *evt_arg;
+    ble_hci_trans_rx_acl_fn *acl_cb;
+    void *acl_arg;
+};
+
+struct da1469x_ble_hci_rx_data {
+    uint8_t type;
+    uint8_t hdr[4];
+    uint8_t min_len;
+    uint16_t len;
+    uint16_t expected_len;
+    union {
+        uint8_t *buf;
+        struct os_mbuf *om;
+    };
+};
+
+struct da1469x_ble_hci_pool_cmd {
+    uint8_t cmd[BLE_HCI_TRANS_CMD_SZ];
+    bool allocated;
+};
+
+/* (Pseudo)pool for HCI commands */
+static struct da1469x_ble_hci_pool_cmd da1469x_ble_hci_pool_cmd;
+
+/* Pools for HCI events (high and low priority) */
+static uint8_t da1469x_ble_hci_pool_evt_hi_buf[ OS_MEMPOOL_BYTES(
+                                            MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                                            MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)) ];
+static struct os_mempool da1469x_ble_hci_pool_evt_hi;
+static uint8_t da1469x_ble_hci_pool_evt_lo_buf[ OS_MEMPOOL_BYTES(
+                                            MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                                            MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)) ];
+static struct os_mempool da1469x_ble_hci_pool_evt_lo;
+
+/* Pool for ACL data */
+static uint8_t da1469x_ble_hci_pool_acl_buf[ OS_MEMPOOL_BYTES(
+                                            MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                                            POOL_ACL_BLOCK_SIZE) ];
+static struct os_mempool da1469x_ble_hci_pool_acl;
+static struct os_mbuf_pool da1469x_ble_hci_pool_acl_mbuf;
+
+/* Interface to host */
+static struct da1469x_ble_hci_host_api da1469x_ble_hci_host_api;
+
+/* State of RX currently in progress (needs to reassemble frame) */
+static struct da1469x_ble_hci_rx_data da1469x_ble_hci_rx_data;
+
+int
+ble_hci_trans_reset(void)
+{
+    /* XXX Should we do something with RF and/or BLE core? */
+    return 0;
+}
+
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *evt_cb, void *evt_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb, void *acl_arg)
+{
+    da1469x_ble_hci_host_api.evt_cb = evt_cb;
+    da1469x_ble_hci_host_api.evt_arg = evt_arg;
+    da1469x_ble_hci_host_api.acl_cb = acl_cb;
+    da1469x_ble_hci_host_api.acl_arg = acl_arg;
+}
+
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_CMD:
+        assert(!da1469x_ble_hci_pool_cmd.allocated);
+        da1469x_ble_hci_pool_cmd.allocated = 1;
+        buf = da1469x_ble_hci_pool_cmd.cmd;
+        break;
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&da1469x_ble_hci_pool_evt_hi);
+        if (buf) {
+            break;
+        }
+        /* no break */
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+        buf = os_memblock_get(&da1469x_ble_hci_pool_evt_lo);
+        break;
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    if (buf == da1469x_ble_hci_pool_cmd.cmd) {
+        assert(da1469x_ble_hci_pool_cmd.allocated);
+        da1469x_ble_hci_pool_cmd.allocated = 0;
+    } else if (os_memblock_from(&da1469x_ble_hci_pool_evt_hi, buf)) {
+        rc = os_memblock_put(&da1469x_ble_hci_pool_evt_hi, buf);
+        assert(rc == 0);
+    } else {
+        assert(os_memblock_from(&da1469x_ble_hci_pool_evt_lo, buf));
+        rc = os_memblock_put(&da1469x_ble_hci_pool_evt_lo, buf);
+        assert(rc == 0);
+    }
+}
+
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    uint8_t ind = HCI_PKT_CMD;
+    int len = 3 + cmd[2];
+
+    cmac_mbox_write(&ind, 1);
+    cmac_mbox_write(cmd, len);
+
+    ble_hci_trans_buf_free(cmd);
+
+    return 0;
+}
+
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+    uint8_t ind = HCI_PKT_ACL;
+    struct os_mbuf *x;
+
+    cmac_mbox_write(&ind, 1);
+
+    x = om;
+    while (x) {
+        cmac_mbox_write(x->om_data, x->om_len);
+        x = SLIST_NEXT(x, om_next);
+    }
+
+    os_mbuf_free_chain(om);
+
+    return 0;
+}
+
+static int
+da1469x_ble_hci_trans_ll_rx(const uint8_t *buf, uint16_t len)
+{
+    struct da1469x_ble_hci_rx_data *rxd = &da1469x_ble_hci_rx_data;
+    void *data;
+    int pool = BLE_HCI_TRANS_BUF_EVT_HI;
+    int rc;
+
+    assert(len);
+
+    if (rxd->type == HCI_PKT_NONE) {
+        rxd->type = buf[0];
+        rxd->len = 0;
+        rxd->expected_len = 0;
+
+        switch (rxd->type) {
+        case HCI_PKT_ACL:
+            rxd->min_len = 4;
+            break;
+        case HCI_PKT_EVT:
+            rxd->min_len = 2;
+            break;
+        default:
+            assert(0);
+            break;
+        }
+
+        return 1;
+    }
+
+    /* Ensure we have minimum length of bytes required to process header */
+    if (rxd->len < rxd->min_len) {
+        len = min(len, rxd->min_len - rxd->len);
+        memcpy(&rxd->hdr[rxd->len], buf, len);
+        rxd->len += len;
+        return len;
+    }
+
+    /* Parse header and allocate proper buffer if not done yet */
+    if (rxd->expected_len == 0) {
+        switch (rxd->type) {
+        case HCI_PKT_ACL:
+            data = os_mbuf_get_pkthdr(&da1469x_ble_hci_pool_acl_mbuf,
+                                      sizeof(struct ble_mbuf_hdr));
+            if (!data) {
+                return 0;
+            }
+
+            rxd->om = data;
+            os_mbuf_append(rxd->om, rxd->hdr, rxd->len);
+            rxd->expected_len = get_le16(&rxd->hdr[2]) + 4;
+            break;
+        case HCI_PKT_EVT:
+            if (rxd->hdr[0] == BLE_HCI_EVCODE_LE_META) {
+                /* For LE Meta event we need 3 bytes to parse header */
+                if (rxd->min_len < 3) {
+                    rxd->min_len = 3;
+                    return 0;
+                }
+
+                /* Advertising reports shall be allocated from low-prio pool */
+                if ((rxd->hdr[2] == BLE_HCI_LE_SUBEV_ADV_RPT) ||
+                    (rxd->hdr[2] == BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+                    pool = BLE_HCI_TRANS_BUF_EVT_LO;
+                }
+            }
+
+            data = ble_hci_trans_buf_alloc(pool);
+            if (!data) {
+                /*
+                 * Only care about valid buffer when shall be allocated from
+                 * high-prio pool, otherwise NULL is fine and we'll just skip
+                 * this event.
+                 */
+                if (pool != BLE_HCI_TRANS_BUF_EVT_LO) {
+                    data = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+                    if (!data) {
+                        return 0;
+                    }
+                }
+            }
+
+            rxd->buf = data;
+            memcpy(rxd->buf, rxd->hdr, rxd->len);
+            rxd->expected_len = rxd->hdr[1] + 2;
+            break;
+        default:
+            assert(0);
+            return len;
+        }
+    }
+
+    len = min(len, rxd->expected_len - rxd->len);
+
+    switch (rxd->type) {
+    case HCI_PKT_ACL:
+        os_mbuf_append(rxd->om, buf, len);
+        rxd->len += len;
+
+        if (rxd->len == rxd->expected_len) {
+            rc = da1469x_ble_hci_host_api.acl_cb(rxd->om,
+                                                 da1469x_ble_hci_host_api.acl_arg);
+            if (rc != 0) {
+                os_mbuf_free_chain(rxd->om);
+            }
+            rxd->type = HCI_PKT_NONE;
+        }
+        break;
+    case HCI_PKT_EVT:
+        if (rxd->buf) {
+            memcpy(&rxd->buf[rxd->len], buf, len);
+        }
+        rxd->len += len;
+
+        if (rxd->len == rxd->expected_len) {
+            /*
+             * XXX for unknown reason at startup controller sends command
+             *     complete for a vendor specific command which we never sent
+             *     and this messes up with our ack code - just discard this
+             *     event
+             */
+            if ((rxd->buf[0] == 0x0E) && (get_le16(&rxd->buf[3]) == 0xfc11)) {
+                ble_hci_trans_buf_free(rxd->buf);
+            } else if (rxd->buf) {
+                rc = da1469x_ble_hci_host_api.evt_cb(rxd->buf,
+                                                     da1469x_ble_hci_host_api.evt_arg);
+                if (rc != 0) {
+                    ble_hci_trans_buf_free(rxd->buf);
+                }
+            }
+            rxd->type = HCI_PKT_NONE;
+        }
+        break;
+    default:
+        assert(0);
+        break;
+    }
+
+    return len;
+}
+
+static int
+da1469x_ble_hci_read_cb(const uint8_t *buf, uint16_t len)
+{
+    return da1469x_ble_hci_trans_ll_rx(buf, len);
+}
+
+void
+da1469x_ble_hci_init(void)
+{
+    int rc;
+
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = os_mempool_init(&da1469x_ble_hci_pool_acl, MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                         POOL_ACL_BLOCK_SIZE, da1469x_ble_hci_pool_acl_buf,
+                         "da1469x_ble_hci_pool_acl");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mbuf_pool_init(&da1469x_ble_hci_pool_acl_mbuf,
+                           &da1469x_ble_hci_pool_acl, POOL_ACL_BLOCK_SIZE,
+                           MYNEWT_VAL(BLE_ACL_BUF_COUNT));
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&da1469x_ble_hci_pool_evt_hi,
+                         MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         da1469x_ble_hci_pool_evt_hi_buf,
+                         "da1469x_ble_hci_pool_evt_hi");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&da1469x_ble_hci_pool_evt_lo,
+                         MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         da1469x_ble_hci_pool_evt_lo_buf,
+                         "da1469x_ble_hci_pool_evt_lo");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
+
+void
+da1469x_ble_hci_cmac_init(void)
+{
+    cmac_mbox_set_read_cb(da1469x_ble_hci_read_cb);
+    cmac_host_init();
+}
diff --git a/src/libs/mynewt-nimble/nimble/transport/da1469x/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/da1469x/syscfg.yml
new file mode 100644
index 00000000..4ea9da9b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/da1469x/syscfg.yml
@@ -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.
+#
+
+syscfg.defs:
+    BLE_HCI_EVT_HI_BUF_COUNT:
+        description: 'Number of high-priority event buffers.'
+        value:  2
+
+    BLE_HCI_EVT_LO_BUF_COUNT:
+        description: 'Number of low-priority event buffers.'
+        value:  8
+
+    BLE_HCI_EVT_BUF_SIZE:
+        description: 'Size of each event buffer, in bytes.'
+        value:  70
+
+    BLE_ACL_BUF_COUNT:
+        description: 'The number of ACL data buffers'
+        value: 4
+
+    BLE_ACL_BUF_SIZE:
+        description: >
+            This is the maximum size of the data portion of HCI ACL data
+            packets. It does not include the HCI data header (of 4 bytes).
+        value: 255
+
+syscfg.vals.BLE_EXT_ADV:
+    BLE_HCI_EVT_BUF_SIZE: 274
diff --git a/src/libs/mynewt-nimble/nimble/transport/emspi/include/transport/emspi/ble_hci_emspi.h b/src/libs/mynewt-nimble/nimble/transport/emspi/include/transport/emspi/ble_hci_emspi.h
new file mode 100644
index 00000000..738d9e6d
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/emspi/include/transport/emspi/ble_hci_emspi.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_HCI_EMSPI_
+#define H_BLE_HCI_EMSPI_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_hci_emspi_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/transport/emspi/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/emspi/pkg.yml
new file mode 100644
index 00000000..5df5a5bb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/emspi/pkg.yml
@@ -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.
+#
+
+pkg.name: nimble/transport/emspi
+pkg.description: "HCI transport using EM's HCI SPI protocol."
+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"
+    - nimble
+
+pkg.apis:
+    - ble_transport
+
+pkg.init:
+    ble_hci_emspi_init: 'MYNEWT_VAL(BLE_HCI_EMSPI_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/transport/emspi/src/ble_hci_emspi.c b/src/libs/mynewt-nimble/nimble/transport/emspi/src/ble_hci_emspi.c
new file mode 100644
index 00000000..61c0c9cb
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/emspi/src/ble_hci_emspi.c
@@ -0,0 +1,965 @@
+/*
+ * 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 <stdio.h>
+#include <errno.h>
+#include <stdint.h>
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "os/os_cputime.h"
+#include "bsp/bsp.h"
+#include "os/os.h"
+#include "mem/mem.h"
+#include "hal/hal_gpio.h"
+#include "hal/hal_spi.h"
+
+/* BLE */
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+
+#include "transport/emspi/ble_hci_emspi.h"
+
+#include "am_mcu_apollo.h"
+
+/***
+ * NOTES:
+ * The emspi HCI transport doesn't use event buffer priorities.  All incoming
+ * and outgoing events use buffers from the same pool.
+ *
+ */
+
+#define BLE_HCI_EMSPI_PKT_EVT_COUNT         \
+    (MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT) + \
+     MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT))
+
+#define BLE_HCI_EMSPI_PKT_NONE          0x00
+#define BLE_HCI_EMSPI_PKT_CMD           0x01
+#define BLE_HCI_EMSPI_PKT_ACL           0x02
+#define BLE_HCI_EMSPI_PKT_EVT           0x04
+
+#define BLE_HCI_EMSPI_CTLR_STATUS_OK    0xc0
+#define BLE_HCI_EMSPI_OP_TX             0x42
+#define BLE_HCI_EMSPI_OP_RX             0x81
+
+static os_event_fn ble_hci_emspi_event_txrx;
+
+static struct os_event ble_hci_emspi_ev_txrx = {
+    .ev_cb = ble_hci_emspi_event_txrx,
+};
+
+static struct os_eventq ble_hci_emspi_evq;
+static struct os_task ble_hci_emspi_task;
+static os_stack_t ble_hci_emspi_stack[MYNEWT_VAL(BLE_HCI_EMSPI_STACK_SIZE)];
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_emspi_rx_cmd_cb;
+static void *ble_hci_emspi_rx_cmd_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_emspi_rx_acl_cb;
+static void *ble_hci_emspi_rx_acl_arg;
+
+static struct os_mempool ble_hci_emspi_evt_hi_pool;
+static os_membuf_t ble_hci_emspi_evt_hi_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_emspi_evt_lo_pool;
+static os_membuf_t ble_hci_emspi_evt_lo_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_emspi_cmd_pool;
+static os_membuf_t ble_hci_emspi_cmd_buf[
+        OS_MEMPOOL_SIZE(1, BLE_HCI_TRANS_CMD_SZ)
+];
+
+static struct os_mbuf_pool ble_hci_emspi_acl_mbuf_pool;
+static struct os_mempool_ext ble_hci_emspi_acl_pool;
+
+/*
+ * The MBUF payload size must accommodate the HCI data header size plus the
+ * maximum ACL data packet length. The ACL block size is the size of the
+ * mbufs we will allocate.
+ */
+#define ACL_BLOCK_SIZE  OS_ALIGN(MYNEWT_VAL(BLE_ACL_BUF_SIZE) \
+                                 + BLE_MBUF_MEMBLOCK_OVERHEAD \
+                                 + BLE_HCI_DATA_HDR_SZ, OS_ALIGNMENT)
+
+static os_membuf_t ble_hci_emspi_acl_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                        ACL_BLOCK_SIZE)
+];
+
+/**
+ * A packet to be sent over the EMSPI.  This can be a command, an event, or ACL
+ * data.
+ */
+struct ble_hci_emspi_pkt {
+    STAILQ_ENTRY(ble_hci_emspi_pkt) next;
+    void *data;
+    uint8_t type;
+};
+STAILQ_HEAD(, ble_hci_emspi_pkt) ble_hci_emspi_tx_q;
+
+static struct os_mempool ble_hci_emspi_pkt_pool;
+static os_membuf_t ble_hci_emspi_pkt_buf[
+        OS_MEMPOOL_SIZE(BLE_HCI_EMSPI_PKT_EVT_COUNT + 1 +
+                        MYNEWT_VAL(BLE_HCI_ACL_OUT_COUNT),
+                        sizeof (struct ble_hci_emspi_pkt))
+];
+
+static void
+ble_hci_emspi_rdy_isr(void *arg)
+{
+    os_eventq_put(&ble_hci_emspi_evq, &ble_hci_emspi_ev_txrx);
+}
+
+static void
+ble_hci_emspi_initiate_write(void)
+{
+    hal_gpio_irq_disable(MYNEWT_VAL(BLE_HCI_EMSPI_RDY_PIN));
+
+    /* Assert slave select. */
+    hal_gpio_write(MYNEWT_VAL(BLE_HCI_EMSPI_SS_PIN), 0);
+
+    /* Wait for controller to indicate ready-to-receive. */
+    while (!hal_gpio_read(MYNEWT_VAL(BLE_HCI_EMSPI_RDY_PIN))) { }
+}
+
+static void
+ble_hci_emspi_terminate_write(void)
+{
+    const uint64_t rdy_mask =
+        AM_HAL_GPIO_BIT(MYNEWT_VAL(BLE_HCI_EMSPI_RDY_PIN));
+    os_sr_t sr;
+
+    am_hal_gpio_int_clear(rdy_mask);
+
+    /* Deassert slave select. */
+    hal_gpio_write(MYNEWT_VAL(BLE_HCI_EMSPI_SS_PIN), 1);
+
+    OS_ENTER_CRITICAL(sr);
+    hal_gpio_irq_enable(MYNEWT_VAL(BLE_HCI_EMSPI_RDY_PIN));
+
+    if (hal_gpio_read(MYNEWT_VAL(BLE_HCI_EMSPI_RDY_PIN))) {
+        am_hal_gpio_int_set(rdy_mask);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+static int
+ble_hci_emspi_write_hdr(uint8_t first_byte, uint8_t *out_buf_size)
+{
+    const uint8_t hdr[2] = { first_byte, 0x00 };
+    uint8_t rx[2];
+    int rc;
+
+    /* Send command header. */
+    rc = hal_spi_txrx(MYNEWT_VAL(BLE_HCI_EMSPI_SPI_NUM), (void *)hdr, rx, 2);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* Check for "OK" status. */
+    if (rx[0] != BLE_HCI_EMSPI_CTLR_STATUS_OK) {
+        return BLE_ERR_HW_FAIL;
+    }
+
+    *out_buf_size = rx[1];
+    return 0;
+}
+
+/**
+ * Transmits a chunk of bytes to the controller.
+ */
+static int
+ble_hci_emspi_tx_chunk(const uint8_t *data, int len, int *out_bytes_txed)
+{
+    uint8_t buf_size;
+    int rc;
+
+    /* Silence spurious "may be used uninitialized" warning. */
+    *out_bytes_txed = 0;
+
+    ble_hci_emspi_initiate_write();
+
+    rc = ble_hci_emspi_write_hdr(BLE_HCI_EMSPI_OP_TX, &buf_size);
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (buf_size == 0) {
+        rc = 0;
+        goto done;
+    }
+
+    if (buf_size < len) {
+        len = buf_size;
+    }
+    rc = hal_spi_txrx(MYNEWT_VAL(BLE_HCI_EMSPI_SPI_NUM), (void *)data, NULL,
+                      len);
+    if (rc != 0) {
+        goto done;
+    }
+    *out_bytes_txed = len;
+
+done:
+    ble_hci_emspi_terminate_write();
+    return rc;
+}
+
+/**
+ * Transmits a full command or ACL data packet to the controller.
+ */
+static int
+ble_hci_emspi_tx(const uint8_t *data, int len)
+{
+    int bytes_txed;
+    int rc;
+
+    while (len > 0) {
+        rc = ble_hci_emspi_tx_chunk(data, len, &bytes_txed);
+        if (rc != 0) {
+            goto done;
+        }
+
+        data += bytes_txed;
+        len -= bytes_txed;
+    }
+
+    rc = 0;
+
+done:
+    return rc;
+}
+
+/**
+ * Reads the specified number of bytes from the controller.
+ */
+static int
+ble_hci_emspi_rx(uint8_t *data, int max_len)
+{
+    uint8_t buf_size;
+    int rc;
+
+    ble_hci_emspi_initiate_write();
+
+    rc = ble_hci_emspi_write_hdr(BLE_HCI_EMSPI_OP_RX, &buf_size);
+    if (rc != 0) {
+        goto done;
+    }
+
+    if (buf_size > max_len) {
+        buf_size = max_len;
+    }
+
+    rc = hal_spi_txrx(MYNEWT_VAL(BLE_HCI_EMSPI_SPI_NUM), NULL, data, buf_size);
+    if (rc != 0) {
+        rc = BLE_ERR_HW_FAIL;
+        goto done;
+    }
+
+done:
+    ble_hci_emspi_terminate_write();
+    return rc;
+}
+
+/**
+ * Allocates a buffer (mbuf) for ACL operation.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+static struct os_mbuf *
+ble_hci_trans_acl_buf_alloc(void)
+{
+    uint8_t usrhdr_len;
+
+#if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
+    usrhdr_len = BLE_MBUF_HS_HDR_LEN;
+#else
+    usrhdr_len = 0;
+#endif
+
+    return os_mbuf_get_pkthdr(&ble_hci_emspi_acl_mbuf_pool, usrhdr_len);
+}
+
+/**
+ * Transmits an ACL data packet to the controller.  The caller relinquishes the
+ * specified mbuf, regardless of return status.
+ */
+static int
+ble_hci_emspi_acl_tx(struct os_mbuf *om)
+{
+    struct ble_hci_emspi_pkt *pkt;
+    os_sr_t sr;
+
+    /* If this packet is zero length, just free it */
+    if (OS_MBUF_PKTLEN(om) == 0) {
+        os_mbuf_free_chain(om);
+        return 0;
+    }
+
+    pkt = os_memblock_get(&ble_hci_emspi_pkt_pool);
+    if (pkt == NULL) {
+        os_mbuf_free_chain(om);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    pkt->type = BLE_HCI_EMSPI_PKT_ACL;
+    pkt->data = om;
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&ble_hci_emspi_tx_q, pkt, next);
+    OS_EXIT_CRITICAL(sr);
+
+    os_eventq_put(&ble_hci_emspi_evq, &ble_hci_emspi_ev_txrx);
+
+    return 0;
+}
+
+/**
+ * Transmits a command packet to the controller.  The caller relinquishes the
+ * specified buffer, regardless of return status.
+ */
+static int
+ble_hci_emspi_cmdevt_tx(uint8_t *cmd_buf, uint8_t pkt_type)
+{
+    struct ble_hci_emspi_pkt *pkt;
+    os_sr_t sr;
+
+    pkt = os_memblock_get(&ble_hci_emspi_pkt_pool);
+    if (pkt == NULL) {
+        ble_hci_trans_buf_free(cmd_buf);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    pkt->type = pkt_type;
+    pkt->data = cmd_buf;
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&ble_hci_emspi_tx_q, pkt, next);
+    OS_EXIT_CRITICAL(sr);
+
+    os_eventq_put(&ble_hci_emspi_evq, &ble_hci_emspi_ev_txrx);
+
+    return 0;
+}
+
+static int
+ble_hci_emspi_tx_flat(const uint8_t *data, int len)
+{
+    int rc;
+
+    rc = ble_hci_emspi_tx(data, len);
+    return rc;
+}
+
+static int
+ble_hci_emspi_tx_pkt_type(uint8_t pkt_type)
+{
+    return ble_hci_emspi_tx_flat(&pkt_type, 1);
+}
+
+static int
+ble_hci_emspi_tx_cmd(const uint8_t *data)
+{
+    int len;
+    int rc;
+
+    rc = ble_hci_emspi_tx_pkt_type(BLE_HCI_EMSPI_PKT_CMD);
+    if (rc != 0) {
+        return rc;
+    }
+
+    len = data[2] + sizeof(struct ble_hci_cmd);
+    rc = ble_hci_emspi_tx_flat(data, len);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+static int
+ble_hci_emspi_tx_acl(struct os_mbuf *om)
+{
+    struct os_mbuf *cur;
+    int rc;
+
+    rc = ble_hci_emspi_tx_pkt_type(BLE_HCI_EMSPI_PKT_ACL);
+    if (rc != 0) {
+        return rc;
+    }
+
+    cur = om;
+    while (cur != NULL) {
+        rc = ble_hci_emspi_tx(cur->om_data, cur->om_len);
+        if (rc != 0) {
+            break;
+        }
+
+        cur = SLIST_NEXT(cur, om_next);
+    }
+
+    return rc;
+}
+
+static struct ble_hci_emspi_pkt *
+ble_hci_emspi_pull_next_tx(void)
+{
+    struct ble_hci_emspi_pkt *pkt;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    pkt = STAILQ_FIRST(&ble_hci_emspi_tx_q);
+    if (pkt != NULL) {
+        STAILQ_REMOVE(&ble_hci_emspi_tx_q, pkt, ble_hci_emspi_pkt, next);
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    return pkt;
+}
+
+static int
+ble_hci_emspi_tx_pkt(void)
+{
+    struct ble_hci_emspi_pkt *pkt;
+    int rc;
+
+    pkt = ble_hci_emspi_pull_next_tx();
+    if (pkt == NULL) {
+        return -1;
+    }
+
+    switch (pkt->type) {
+    case BLE_HCI_EMSPI_PKT_CMD:
+        rc = ble_hci_emspi_tx_cmd(pkt->data);
+        ble_hci_trans_buf_free(pkt->data);
+        break;
+
+    case BLE_HCI_EMSPI_PKT_ACL:
+        rc = ble_hci_emspi_tx_acl(pkt->data);
+        os_mbuf_free_chain(pkt->data);
+        break;
+
+    default:
+        rc = -1;
+        break;
+    }
+
+    os_memblock_put(&ble_hci_emspi_pkt_pool, pkt);
+
+    return rc;
+}
+
+static int
+ble_hci_emspi_rx_evt(void)
+{
+    uint8_t *data;
+    uint8_t len;
+    int rc;
+
+    /* XXX: we should not assert if host cannot allocate an event. Need
+     * to determine what to do here.
+     */
+    data = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    assert(data != NULL);
+
+    rc = ble_hci_emspi_rx(data, sizeof(struct ble_hci_ev));
+    if (rc != 0) {
+        goto err;
+    }
+
+    len = data[1];
+    if (len > 0) {
+        rc = ble_hci_emspi_rx(data + sizeof(struct ble_hci_ev), len);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+
+    assert(ble_hci_emspi_rx_cmd_cb != NULL);
+    ble_hci_emspi_rx_cmd_cb(data, ble_hci_emspi_rx_cmd_arg);
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    ble_hci_trans_buf_free(data);
+    return rc;
+}
+
+static int
+ble_hci_emspi_rx_acl(void)
+{
+    struct os_mbuf *om;
+    uint16_t len;
+    int rc;
+
+    /* XXX: we should not assert if host cannot allocate an mbuf. Need to
+     * determine what to do here.
+     */
+    om = ble_hci_trans_acl_buf_alloc();
+    assert(om != NULL);
+
+    rc = ble_hci_emspi_rx(om->om_data, BLE_HCI_DATA_HDR_SZ);
+    if (rc != 0) {
+        goto err;
+    }
+
+    len = get_le16(om->om_data + 2);
+    if (len > MYNEWT_VAL(BLE_ACL_BUF_SIZE)) {
+        /*
+         * Data portion cannot exceed data length of acl buffer. If it does
+         * this is considered to be a loss of sync.
+         */
+        rc = BLE_ERR_UNSPECIFIED;
+        goto err;
+    }
+
+    if (len > 0) {
+        rc = ble_hci_emspi_rx(om->om_data + BLE_HCI_DATA_HDR_SZ, len);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+
+    OS_MBUF_PKTLEN(om) = BLE_HCI_DATA_HDR_SZ + len;
+    om->om_len = BLE_HCI_DATA_HDR_SZ + len;
+
+    assert(ble_hci_emspi_rx_cmd_cb != NULL);
+    rc = ble_hci_emspi_rx_acl_cb(om, ble_hci_emspi_rx_acl_arg);
+    if (rc != 0) {
+        goto err;
+    }
+
+    return 0;
+
+err:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
+/**
+ * @return                      The type of packet to follow success;
+ *                              -1 if there is no valid packet to receive.
+ */
+static int
+ble_hci_emspi_rx_pkt(void)
+{
+    uint8_t pkt_type;
+    int rc;
+
+    /* XXX: This is awkward; should read the full packet in "one go". */
+    rc = ble_hci_emspi_rx(&pkt_type, 1);
+    if (rc != 0) {
+        return rc;
+    }
+
+    switch (pkt_type) {
+    case BLE_HCI_EMSPI_PKT_EVT:
+        return ble_hci_emspi_rx_evt();
+
+    case BLE_HCI_EMSPI_PKT_ACL:
+        return ble_hci_emspi_rx_acl();
+
+    default:
+        /* XXX */
+        return -1;
+    }
+}
+
+static void
+ble_hci_emspi_set_rx_cbs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                        void *cmd_arg,
+                        ble_hci_trans_rx_acl_fn *acl_cb,
+                        void *acl_arg)
+{
+    ble_hci_emspi_rx_cmd_cb = cmd_cb;
+    ble_hci_emspi_rx_cmd_arg = cmd_arg;
+    ble_hci_emspi_rx_acl_cb = acl_cb;
+    ble_hci_emspi_rx_acl_arg = acl_arg;
+}
+
+static void
+ble_hci_emspi_free_pkt(uint8_t type, uint8_t *cmdevt, struct os_mbuf *acl)
+{
+    switch (type) {
+    case BLE_HCI_EMSPI_PKT_NONE:
+        break;
+
+    case BLE_HCI_EMSPI_PKT_CMD:
+    case BLE_HCI_EMSPI_PKT_EVT:
+        ble_hci_trans_buf_free(cmdevt);
+        break;
+
+    case BLE_HCI_EMSPI_PKT_ACL:
+        os_mbuf_free_chain(acl);
+        break;
+
+    default:
+        assert(0);
+        break;
+    }
+}
+
+/**
+ * Unsupported.  This is a host-only transport.
+ */
+int
+ble_hci_trans_ll_evt_tx(uint8_t *cmd)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+/**
+ * Unsupported.  This is a host-only transport.
+ */
+int
+ble_hci_trans_ll_acl_tx(struct os_mbuf *om)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+/**
+ * Sends an HCI command from the host to the controller.
+ *
+ * @param cmd                   The HCI command to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    int rc;
+
+    rc = ble_hci_emspi_cmdevt_tx(cmd, BLE_HCI_EMSPI_PKT_CMD);
+    return rc;
+}
+
+/**
+ * Sends ACL data from host to controller.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    rc = ble_hci_emspi_acl_tx(om);
+    return rc;
+}
+
+/**
+ * Configures the HCI transport to call the specified callback upon receiving
+ * HCI packets from the controller.  This function should only be called by by
+ * host.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_emspi_set_rx_cbs(cmd_cb, cmd_arg, acl_cb, acl_arg);
+}
+
+/**
+ * Configures the HCI transport to operate with a host.  The transport will
+ * execute specified callbacks upon receiving HCI packets from the controller.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    /* Unsupported. */
+    assert(0);
+}
+
+/**
+ * Allocates a flat buffer of the specified type.
+ *
+ * @param type                  The type of buffer to allocate; one of the
+ *                                  BLE_HCI_TRANS_BUF_[...] constants.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_CMD:
+        buf = os_memblock_get(&ble_hci_emspi_cmd_pool);
+        break;
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&ble_hci_emspi_evt_hi_pool);
+        if (buf == NULL) {
+            /* If no high-priority event buffers remain, try to grab a
+             * low-priority one.
+             */
+            buf = os_memblock_get(&ble_hci_emspi_evt_lo_pool);
+        }
+        break;
+
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+        buf = os_memblock_get(&ble_hci_emspi_evt_lo_pool);
+        break;
+
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+/**
+ * Frees the specified flat buffer.  The buffer must have been allocated via
+ * ble_hci_trans_buf_alloc().
+ *
+ * @param buf                   The buffer to free.
+ */
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    if (buf != NULL) {
+        if (os_memblock_from(&ble_hci_emspi_evt_hi_pool, buf)) {
+            rc = os_memblock_put(&ble_hci_emspi_evt_hi_pool, buf);
+            assert(rc == 0);
+        } else if (os_memblock_from(&ble_hci_emspi_evt_lo_pool, buf)) {
+            rc = os_memblock_put(&ble_hci_emspi_evt_lo_pool, buf);
+            assert(rc == 0);
+        } else {
+            assert(os_memblock_from(&ble_hci_emspi_cmd_pool, buf));
+            rc = os_memblock_put(&ble_hci_emspi_cmd_pool, buf);
+            assert(rc == 0);
+        }
+    }
+}
+
+/**
+ * Configures a callback to get executed whenever an ACL data packet is freed.
+ * The function is called in lieu of actually freeing the packet.
+ *
+ * @param cb                    The callback to configure.
+ *
+ * @return                      0 on success.
+ */
+int
+ble_hci_trans_set_acl_free_cb(os_mempool_put_fn *cb, void *arg)
+{
+    ble_hci_emspi_acl_pool.mpe_put_cb = cb;
+    ble_hci_emspi_acl_pool.mpe_put_arg = arg;
+    return 0;
+}
+
+/**
+ * Resets the HCI UART transport to a clean state.  Frees all buffers and
+ * reconfigures the UART.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_reset(void)
+{
+    struct ble_hci_emspi_pkt *pkt;
+
+    hal_gpio_write(MYNEWT_VAL(BLE_HCI_EMSPI_RESET_PIN), 1);
+
+    while ((pkt = STAILQ_FIRST(&ble_hci_emspi_tx_q)) != NULL) {
+        STAILQ_REMOVE(&ble_hci_emspi_tx_q, pkt, ble_hci_emspi_pkt, next);
+        ble_hci_emspi_free_pkt(pkt->type, pkt->data, pkt->data);
+        os_memblock_put(&ble_hci_emspi_pkt_pool, pkt);
+    }
+
+    return 0;
+}
+
+static void
+ble_hci_emspi_event_txrx(struct os_event *ev)
+{
+    int rc;
+
+    rc = ble_hci_emspi_rx_pkt();
+
+    do {
+        rc = ble_hci_emspi_tx_pkt();
+    } while (rc == 0);
+}
+
+static void
+ble_hci_emspi_loop(void *unused)
+{
+    while (1) {
+        os_eventq_run(&ble_hci_emspi_evq);
+    }
+}
+
+static void
+ble_hci_emspi_init_hw(void)
+{
+    struct hal_spi_settings spi_cfg;
+    int rc;
+
+    rc = hal_gpio_init_out(MYNEWT_VAL(BLE_HCI_EMSPI_RESET_PIN), 0);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = hal_gpio_init_out(MYNEWT_VAL(BLE_HCI_EMSPI_SS_PIN), 1);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    spi_cfg.data_order = HAL_SPI_MSB_FIRST;
+    spi_cfg.data_mode = HAL_SPI_MODE0;
+    spi_cfg.baudrate = MYNEWT_VAL(BLE_HCI_EMSPI_BAUD);
+    spi_cfg.word_size = HAL_SPI_WORD_SIZE_8BIT;
+
+    rc = hal_spi_config(MYNEWT_VAL(BLE_HCI_EMSPI_SPI_NUM), &spi_cfg);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = hal_gpio_irq_init(MYNEWT_VAL(BLE_HCI_EMSPI_RDY_PIN),
+                           ble_hci_emspi_rdy_isr, NULL,
+                           HAL_GPIO_TRIG_RISING, HAL_GPIO_PULL_DOWN);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = hal_spi_enable(MYNEWT_VAL(BLE_HCI_EMSPI_SPI_NUM));
+    assert(rc == 0);
+
+    hal_gpio_write(MYNEWT_VAL(BLE_HCI_EMSPI_RESET_PIN), 1);
+}
+
+/**
+ * Initializes the UART HCI transport module.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+void
+ble_hci_emspi_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = os_mempool_ext_init(&ble_hci_emspi_acl_pool,
+                             MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                             ACL_BLOCK_SIZE,
+                             ble_hci_emspi_acl_buf,
+                             "ble_hci_emspi_acl_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mbuf_pool_init(&ble_hci_emspi_acl_mbuf_pool,
+                           &ble_hci_emspi_acl_pool.mpe_mp,
+                           ACL_BLOCK_SIZE,
+                           MYNEWT_VAL(BLE_ACL_BUF_COUNT));
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    /*
+     * Create memory pool of HCI command buffers. NOTE: we currently dont
+     * allow this to be configured. The controller will only allow one
+     * outstanding command. We decided to keep this a pool in case we allow
+     * allow the controller to handle more than one outstanding command.
+     */
+    rc = os_mempool_init(&ble_hci_emspi_cmd_pool,
+                         1,
+                         BLE_HCI_TRANS_CMD_SZ,
+                         ble_hci_emspi_cmd_buf,
+                         "ble_hci_emspi_cmd_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_emspi_evt_hi_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_emspi_evt_hi_buf,
+                         "ble_hci_emspi_evt_hi_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_emspi_evt_lo_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_emspi_evt_lo_buf,
+                         "ble_hci_emspi_evt_lo_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    /*
+     * Create memory pool of packet list nodes. NOTE: the number of these
+     * buffers should be, at least, the total number of event buffers (hi
+     * and lo), the number of command buffers (currently 1) and the total
+     * number of buffers that the controller could possibly hand to the host.
+     */
+    rc = os_mempool_init(&ble_hci_emspi_pkt_pool,
+                         BLE_HCI_EMSPI_PKT_EVT_COUNT + 1 +
+                         MYNEWT_VAL(BLE_HCI_ACL_OUT_COUNT),
+                         sizeof (struct ble_hci_emspi_pkt),
+                         ble_hci_emspi_pkt_buf,
+                         "ble_hci_emspi_pkt_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    STAILQ_INIT(&ble_hci_emspi_tx_q);
+
+    ble_hci_emspi_init_hw();
+
+    /* Initialize the LL task */
+    os_eventq_init(&ble_hci_emspi_evq);
+    rc = os_task_init(&ble_hci_emspi_task, "ble_hci_emspi", ble_hci_emspi_loop,
+                      NULL, MYNEWT_VAL(BLE_HCI_EMSPI_PRIO), OS_WAIT_FOREVER,
+                      ble_hci_emspi_stack,
+                      MYNEWT_VAL(BLE_HCI_EMSPI_STACK_SIZE));
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/transport/emspi/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/emspi/syscfg.yml
new file mode 100644
index 00000000..4751271b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/emspi/syscfg.yml
@@ -0,0 +1,99 @@
+# 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_HCI_EMSPI:
+        description: 'Indicates that the emspi host HCI transport is present.'
+        value: 1
+        restrictions:
+            # XXX: This package only builds with the apollo2 MCU.
+            # MCU-dependencies need to be removed.
+            - MCU_APOLLO2
+
+            # This is a host-only transport.
+            - BLE_HOST
+
+    BLE_HCI_EVT_HI_BUF_COUNT:
+        description: 'Number of high-priority event buffers.'
+        value:  2
+
+    BLE_HCI_EVT_LO_BUF_COUNT:
+        description: 'Number of low-priority event buffers.'
+        value:  8
+
+    BLE_HCI_EVT_BUF_SIZE:
+        description: 'Size of each event buffer, in bytes.'
+        value:  70
+
+    BLE_ACL_BUF_COUNT:
+        description: 'The number of ACL data buffers'
+        value: 4
+
+    BLE_ACL_BUF_SIZE:
+        description: >
+            This is the maximum size of the data portion of HCI ACL data
+            packets. It does not include the HCI data header (of 4 bytes).
+        value: 255
+
+    BLE_HCI_ACL_OUT_COUNT:
+        description: >
+            This count is used in creating a pool of elements used by the
+            code to enqueue various elements. In the case of the controller
+            only HCI, this number should be equal to the number of mbufs in
+            the msys pool. For host only, it is really dependent on the
+            number of ACL buffers that the controller tells the host it
+            has.
+        value: 12
+
+    BLE_HCI_EMSPI_SPI_NUM:
+        description: The index of the SPI device to use for HCI.
+        value: 5
+
+    BLE_HCI_EMSPI_BAUD:
+        description: The SPI baud rate.
+        value: 8000000
+
+    BLE_HCI_EMSPI_RESET_PIN:
+        description: The GPIO pin that resets the EM controller.
+        value: 42
+
+    BLE_HCI_EMSPI_SS_PIN:
+        description: The GPIO pin to use for SPI slave select.
+        value: 45
+
+    BLE_HCI_EMSPI_RDY_PIN:
+        description: >
+            The GPIO pin that the EM controller uses to indicate data ready.
+        value: 26
+
+    BLE_HCI_EMSPI_PRIO:
+        description: The priority of the emspi task.
+        type: task_priority
+        value: 5
+
+    BLE_HCI_EMSPI_STACK_SIZE:
+        description: 'The size of the emspi task (units: 4-byte words).'
+        value: 256
+
+    BLE_HCI_EMSPI_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the EMSPI BLE transport.
+        value: 100
+
+syscfg.vals.BLE_EXT_ADV:
+    BLE_HCI_EVT_BUF_SIZE: 257
diff --git a/src/libs/mynewt-nimble/nimble/transport/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/pkg.yml
new file mode 100644
index 00000000..2bc4ac29
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/pkg.yml
@@ -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.
+#
+
+pkg.name: nimble/transport
+pkg.description: Meta-package for NimBLE HCI transport
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps.'BLE_HCI_TRANSPORT == "builtin"':
+    - nimble/transport/ram
+    - nimble/controller
+
+pkg.deps.'BLE_HCI_TRANSPORT == "emspi"':
+    - nimble/transport/emspi
+
+pkg.deps.'BLE_HCI_TRANSPORT == "ram"':
+    - nimble/transport/ram
+
+pkg.deps.'BLE_HCI_TRANSPORT == "socket"':
+    - nimble/transport/socket
+
+pkg.deps.'BLE_HCI_TRANSPORT == "uart"':
+    - nimble/transport/uart
+
+pkg.deps.'BLE_HCI_TRANSPORT == "da1469x"':
+    - nimble/transport/da1469x
diff --git a/src/libs/mynewt-nimble/nimble/transport/ram/include/transport/ram/ble_hci_ram.h b/src/libs/mynewt-nimble/nimble/transport/ram/include/transport/ram/ble_hci_ram.h
new file mode 100644
index 00000000..3c37e329
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/ram/include/transport/ram/ble_hci_ram.h
@@ -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.
+ */
+
+#ifndef H_BLE_HCI_RAM_
+#define H_BLE_HCI_RAM_
+
+#include "nimble/ble_hci_trans.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_hci_ram_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/transport/ram/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/ram/pkg.yml
new file mode 100644
index 00000000..bb8397bf
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/ram/pkg.yml
@@ -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.
+#
+
+pkg.name: nimble/transport/ram
+pkg.description: XXX
+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"
+    - nimble
+
+pkg.apis:
+    - ble_transport
+
+pkg.init:
+    ble_hci_ram_init: 'MYNEWT_VAL(BLE_TRANS_RAM_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/transport/ram/src/ble_hci_ram.c b/src/libs/mynewt-nimble/nimble/transport/ram/src/ble_hci_ram.c
new file mode 100644
index 00000000..3f10e9df
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/ram/src/ble_hci_ram.c
@@ -0,0 +1,238 @@
+/*
+ * 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 <stddef.h>
+#include "syscfg/syscfg.h"
+#include "sysinit/sysinit.h"
+#include "os/os.h"
+#include "mem/mem.h"
+#include "nimble/ble.h"
+#include "nimble/ble_hci_trans.h"
+#include "transport/ram/ble_hci_ram.h"
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_ram_rx_cmd_hs_cb;
+static void *ble_hci_ram_rx_cmd_hs_arg;
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_ram_rx_cmd_ll_cb;
+static void *ble_hci_ram_rx_cmd_ll_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_ram_rx_acl_hs_cb;
+static void *ble_hci_ram_rx_acl_hs_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_ram_rx_acl_ll_cb;
+static void *ble_hci_ram_rx_acl_ll_arg;
+
+static struct os_mempool ble_hci_ram_cmd_pool;
+static os_membuf_t ble_hci_ram_cmd_buf[
+        OS_MEMPOOL_SIZE(1, BLE_HCI_TRANS_CMD_SZ)
+];
+
+static struct os_mempool ble_hci_ram_evt_hi_pool;
+static os_membuf_t ble_hci_ram_evt_hi_buf[
+    OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                    MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_ram_evt_lo_pool;
+static os_membuf_t ble_hci_ram_evt_lo_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_ram_rx_cmd_hs_cb = cmd_cb;
+    ble_hci_ram_rx_cmd_hs_arg = cmd_arg;
+    ble_hci_ram_rx_acl_hs_cb = acl_cb;
+    ble_hci_ram_rx_acl_hs_arg = acl_arg;
+}
+
+void
+ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_ram_rx_cmd_ll_cb = cmd_cb;
+    ble_hci_ram_rx_cmd_ll_arg = cmd_arg;
+    ble_hci_ram_rx_acl_ll_cb = acl_cb;
+    ble_hci_ram_rx_acl_ll_arg = acl_arg;
+}
+
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_cmd_ll_cb != NULL);
+
+    rc = ble_hci_ram_rx_cmd_ll_cb(cmd, ble_hci_ram_rx_cmd_ll_arg);
+    return rc;
+}
+
+int
+ble_hci_trans_ll_evt_tx(uint8_t *hci_ev)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_cmd_hs_cb != NULL);
+
+    rc = ble_hci_ram_rx_cmd_hs_cb(hci_ev, ble_hci_ram_rx_cmd_hs_arg);
+    return rc;
+}
+
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_acl_ll_cb != NULL);
+
+    rc = ble_hci_ram_rx_acl_ll_cb(om, ble_hci_ram_rx_acl_ll_arg);
+    return rc;
+}
+
+int
+ble_hci_trans_ll_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_acl_hs_cb != NULL);
+
+    rc = ble_hci_ram_rx_acl_hs_cb(om, ble_hci_ram_rx_acl_hs_arg);
+    return rc;
+}
+
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_CMD:
+        buf = os_memblock_get(&ble_hci_ram_cmd_pool);
+        break;
+
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&ble_hci_ram_evt_hi_pool);
+        if (buf == NULL) {
+            /* If no high-priority event buffers remain, try to grab a
+             * low-priority one.
+             */
+            buf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+        }
+        break;
+
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+        buf = os_memblock_get(&ble_hci_ram_evt_lo_pool);
+        break;
+
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    /* XXX: this may look a bit odd, but the controller uses the command
+    * buffer to send back the command complete/status as an immediate
+    * response to the command. This was done to insure that the controller
+    * could always send back one of these events when a command was received.
+    * Thus, we check to see which pool the buffer came from so we can free
+    * it to the appropriate pool
+    */
+    if (os_memblock_from(&ble_hci_ram_evt_hi_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_ram_evt_hi_pool, buf);
+        assert(rc == 0);
+    } else if (os_memblock_from(&ble_hci_ram_evt_lo_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_ram_evt_lo_pool, buf);
+        assert(rc == 0);
+    } else {
+        assert(os_memblock_from(&ble_hci_ram_cmd_pool, buf));
+        rc = os_memblock_put(&ble_hci_ram_cmd_pool, buf);
+        assert(rc == 0);
+    }
+}
+
+/**
+ * Unsupported; the RAM transport does not have a dedicated ACL data packet
+ * pool.
+ */
+int
+ble_hci_trans_set_acl_free_cb(os_mempool_put_fn *cb, void *arg)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_hci_trans_reset(void)
+{
+    /* No work to do.  All allocated buffers are owned by the host or
+     * controller, and they will get freed by their owners.
+     */
+    return 0;
+}
+
+void
+ble_hci_ram_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    /*
+     * Create memory pool of HCI command buffers. NOTE: we currently dont
+     * allow this to be configured. The controller will only allow one
+     * outstanding command. We decided to keep this a pool in case we allow
+     * allow the controller to handle more than one outstanding command.
+     */
+    rc = os_mempool_init(&ble_hci_ram_cmd_pool,
+                         1,
+                         BLE_HCI_TRANS_CMD_SZ,
+                         ble_hci_ram_cmd_buf,
+                         "ble_hci_ram_cmd_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_ram_evt_hi_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_ram_evt_hi_buf,
+                         "ble_hci_ram_evt_hi_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_ram_evt_lo_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_ram_evt_lo_buf,
+                         "ble_hci_ram_evt_lo_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/transport/ram/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/ram/syscfg.yml
new file mode 100644
index 00000000..3b822fcc
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/ram/syscfg.yml
@@ -0,0 +1,48 @@
+# 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_HCI_EVT_HI_BUF_COUNT:
+        description: 'Number of high-priority event buffers.'
+        value:  2
+
+    BLE_HCI_EVT_LO_BUF_COUNT:
+        description: 'Number of low-priority event buffers.'
+        value:  8
+
+    BLE_HCI_EVT_BUF_SIZE:
+        description: 'Size of each event buffer, in bytes.'
+        value:  70
+
+    BLE_ACL_BUF_COUNT:
+        description: 'The number of ACL data buffers'
+        value: 4
+
+    BLE_ACL_BUF_SIZE:
+        description: >
+            This is the maximum size of the data portion of HCI ACL data
+            packets. It does not include the HCI data header (of 4 bytes).
+        value: 255
+
+    BLE_TRANS_RAM_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the RAM BLE transport.
+        value: 100
+
+syscfg.vals.BLE_EXT_ADV:
+    BLE_HCI_EVT_BUF_SIZE: 257
diff --git a/src/libs/mynewt-nimble/nimble/transport/socket/include/socket/ble_hci_socket.h b/src/libs/mynewt-nimble/nimble/transport/socket/include/socket/ble_hci_socket.h
new file mode 100644
index 00000000..d17074f5
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/socket/include/socket/ble_hci_socket.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 _BLE_HCI_SOCKET_H_
+#define _BLE_HCI_SOCKET_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct os_eventq;
+void ble_hci_sock_set_evq(struct os_eventq *);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/transport/socket/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/socket/pkg.yml
new file mode 100644
index 00000000..fb4144d3
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/socket/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/transport/socket
+pkg.description: Provides HCI transport over socket interface
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - "@apache-mynewt-core/hw/hal"
+    - "@apache-mynewt-core/kernel/os"
+    - "@apache-mynewt-core/util/mem"
+    - nimble
+
+pkg.apis:
+    - ble_transport
+
+pkg.init:
+    ble_hci_sock_init: 'MYNEWT_VAL(BLE_SOCK_CLI_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/transport/socket/src/ble_hci_socket.c b/src/libs/mynewt-nimble/nimble/transport/socket/src/ble_hci_socket.c
new file mode 100644
index 00000000..8bf56f39
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/socket/src/ble_hci_socket.c
@@ -0,0 +1,886 @@
+/*
+ * 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.
+ */
+
+/*
+ * Provides HCI transport over sockets. Either over
+ * TCP sockets, or Linux bluetooth socket.
+ *
+ * E.g. to connect to TCP target, start in another window
+ * socat -x PIPE:/dev/ttyACM1 TCP4-LISTEN:14433,fork,reuseaddr
+ * When building this package, set BLE_SOCK_USE_TCP=1 and
+ * BLE_SOCK_TCP_PORT controls the target port this transport
+ * connects to.
+ *
+ * To use Linux Bluetooth sockets, create an interface:
+ * sudo hciattach -r -n /dev/ttyUSB0 any 57600
+ * And build this package with BLE_SOCK_USE_LINUX_BLUE=1,
+ * BLE_SOCK_LINUX_DEV=<interface index of the target interface>
+ *
+ */
+#include "syscfg/syscfg.h"
+
+#include <assert.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/uio.h>
+#include <unistd.h>
+#include <sys/socket.h>
+
+#if MYNEWT_VAL(BLE_SOCK_USE_TCP)
+#include <sys/errno.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#endif
+
+#if MYNEWT_VAL(BLE_SOCK_USE_LINUX_BLUE)
+#include <sys/errno.h>
+#define BTPROTO_HCI 1
+#define HCI_CHANNEL_RAW		0
+#define HCI_CHANNEL_USER	1
+#define HCIDEVUP	_IOW('H', 201, int)
+#define HCIDEVDOWN	_IOW('H', 202, int)
+#define HCIDEVRESET	_IOW('H', 203, int)
+#define HCIGETDEVLIST	_IOR('H', 210, int)
+
+struct sockaddr_hci {
+        sa_family_t    hci_family;
+        unsigned short hci_dev;
+        unsigned short hci_channel;
+};
+#endif
+
+#include <fcntl.h>
+#include <sys/ioctl.h>
+
+#include "sysinit/sysinit.h"
+#include "os/os.h"
+#include "mem/mem.h"
+
+#include "stats/stats.h"
+
+/* BLE */
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/nimble_npl.h"
+#include "nimble/ble_hci_trans.h"
+#include "socket/ble_hci_socket.h"
+
+/***
+ * NOTES:
+ * The UART HCI transport doesn't use event buffer priorities.  All incoming
+ * and outgoing events use buffers from the same pool.
+ *
+ * The "skip" definitions are here so that when buffers cannot be allocated,
+ * the command or acl packets are simply skipped so that the HCI interface
+ * does not lose synchronization and resets dont (necessarily) occur.
+ */
+
+STATS_SECT_START(hci_sock_stats)
+    STATS_SECT_ENTRY(imsg)
+    STATS_SECT_ENTRY(icmd)
+    STATS_SECT_ENTRY(ievt)
+    STATS_SECT_ENTRY(iacl)
+    STATS_SECT_ENTRY(ibytes)
+    STATS_SECT_ENTRY(ierr)
+    STATS_SECT_ENTRY(imem)
+    STATS_SECT_ENTRY(omsg)
+    STATS_SECT_ENTRY(oacl)
+    STATS_SECT_ENTRY(ocmd)
+    STATS_SECT_ENTRY(oevt)
+    STATS_SECT_ENTRY(obytes)
+    STATS_SECT_ENTRY(oerr)
+STATS_SECT_END
+
+STATS_SECT_DECL(hci_sock_stats) hci_sock_stats;
+STATS_NAME_START(hci_sock_stats)
+    STATS_NAME(hci_sock_stats, imsg)
+    STATS_NAME(hci_sock_stats, icmd)
+    STATS_NAME(hci_sock_stats, ievt)
+    STATS_NAME(hci_sock_stats, iacl)
+    STATS_NAME(hci_sock_stats, ibytes)
+    STATS_NAME(hci_sock_stats, ierr)
+    STATS_NAME(hci_sock_stats, imem)
+    STATS_NAME(hci_sock_stats, omsg)
+    STATS_NAME(hci_sock_stats, oacl)
+    STATS_NAME(hci_sock_stats, ocmd)
+    STATS_NAME(hci_sock_stats, oevt)
+    STATS_NAME(hci_sock_stats, obytes)
+    STATS_NAME(hci_sock_stats, oerr)
+STATS_NAME_END(hci_sock_stats)
+
+/***
+ * NOTES:
+ * The "skip" definitions are here so that when buffers cannot be allocated,
+ * the command or acl packets are simply skipped so that the HCI interface
+ * does not lose synchronization and resets dont (necessarily) occur.
+ */
+#define BLE_HCI_UART_H4_NONE        0x00
+#define BLE_HCI_UART_H4_CMD         0x01
+#define BLE_HCI_UART_H4_ACL         0x02
+#define BLE_HCI_UART_H4_SCO         0x03
+#define BLE_HCI_UART_H4_EVT         0x04
+#define BLE_HCI_UART_H4_SYNC_LOSS   0x80
+#define BLE_HCI_UART_H4_SKIP_CMD    0x81
+#define BLE_HCI_UART_H4_SKIP_ACL    0x82
+
+#if MYNEWT
+
+#define BLE_SOCK_STACK_SIZE         \
+    OS_STACK_ALIGN(MYNEWT_VAL(BLE_SOCK_STACK_SIZE))
+
+struct os_task ble_sock_task;
+
+#endif
+
+static struct os_mempool ble_hci_sock_evt_hi_pool;
+static os_membuf_t ble_hci_sock_evt_hi_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_sock_evt_lo_pool;
+static os_membuf_t ble_hci_sock_evt_lo_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_sock_cmd_pool;
+static os_membuf_t ble_hci_sock_cmd_buf[
+        OS_MEMPOOL_SIZE(1, BLE_HCI_TRANS_CMD_SZ)
+];
+
+static struct os_mempool ble_hci_sock_acl_pool;
+static struct os_mbuf_pool ble_hci_sock_acl_mbuf_pool;
+
+#define ACL_BLOCK_SIZE  OS_ALIGN(MYNEWT_VAL(BLE_ACL_BUF_SIZE) \
+                                 + BLE_MBUF_MEMBLOCK_OVERHEAD \
+                                 + BLE_HCI_DATA_HDR_SZ, OS_ALIGNMENT)
+/*
+ * The MBUF payload size must accommodate the HCI data header size plus the
+ * maximum ACL data packet length. The ACL block size is the size of the
+ * mbufs we will allocate.
+ */
+
+static os_membuf_t ble_hci_sock_acl_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                        ACL_BLOCK_SIZE)
+];
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_sock_rx_cmd_cb;
+static void *ble_hci_sock_rx_cmd_arg;
+static ble_hci_trans_rx_acl_fn *ble_hci_sock_rx_acl_cb;
+static void *ble_hci_sock_rx_acl_arg;
+
+static struct ble_hci_sock_state {
+    int sock;
+    struct ble_npl_eventq evq;
+    struct ble_npl_event ev;
+    struct ble_npl_callout timer;
+
+    uint16_t rx_off;
+    uint8_t rx_data[512];
+} ble_hci_sock_state;
+
+#if MYNEWT_VAL(BLE_SOCK_USE_TCP)
+static int s_ble_hci_device = MYNEWT_VAL(BLE_SOCK_TCP_PORT);
+#elif MYNEWT_VAL(BLE_SOCK_USE_LINUX_BLUE)
+static int s_ble_hci_device = MYNEWT_VAL(BLE_SOCK_LINUX_DEV);
+#endif
+
+/**
+ * Allocates a buffer (mbuf) for ACL operation.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+static struct os_mbuf *
+ble_hci_trans_acl_buf_alloc(void)
+{
+    struct os_mbuf *m;
+
+    /*
+     * XXX: note that for host only there would be no need to allocate
+     * a user header. Address this later.
+     */
+    m = os_mbuf_get_pkthdr(&ble_hci_sock_acl_mbuf_pool,
+                           sizeof(struct ble_mbuf_hdr));
+    return m;
+}
+
+static int
+ble_hci_sock_acl_tx(struct os_mbuf *om)
+{
+    struct msghdr msg;
+    struct iovec iov[8];
+    int i;
+    struct os_mbuf *m;
+    uint8_t ch;
+
+    memset(&msg, 0, sizeof(msg));
+    memset(iov, 0, sizeof(iov));
+
+    msg.msg_iov = iov;
+
+    ch = BLE_HCI_UART_H4_ACL;
+    iov[0].iov_len = 1;
+    iov[0].iov_base = &ch;
+    i = 1;
+    for (m = om; m; m = SLIST_NEXT(m, om_next)) {
+        iov[i].iov_base = m->om_data;
+        iov[i].iov_len = m->om_len;
+        i++;
+    }
+    msg.msg_iovlen = i;
+
+    STATS_INC(hci_sock_stats, omsg);
+    STATS_INC(hci_sock_stats, oacl);
+    STATS_INCN(hci_sock_stats, obytes, OS_MBUF_PKTLEN(om) + 1);
+    i = sendmsg(ble_hci_sock_state.sock, &msg, 0);
+    os_mbuf_free_chain(om);
+    if (i != OS_MBUF_PKTLEN(om) + 1) {
+        if (i < 0) {
+            dprintf(1, "sendmsg() failed : %d\n", errno);
+        } else {
+            dprintf(1, "sendmsg() partial write: %d\n", i);
+        }
+        STATS_INC(hci_sock_stats, oerr);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+    return 0;
+}
+
+static int
+ble_hci_sock_cmdevt_tx(uint8_t *hci_ev, uint8_t h4_type)
+{
+    struct msghdr msg;
+    struct iovec iov[8];
+    int len;
+    int i;
+    uint8_t ch;
+
+    memset(&msg, 0, sizeof(msg));
+    memset(iov, 0, sizeof(iov));
+
+    msg.msg_iov = iov;
+    msg.msg_iovlen = 2;
+
+    ch = h4_type;
+    iov[0].iov_len = 1;
+    iov[0].iov_base = &ch;
+    iov[1].iov_base = hci_ev;
+    if (h4_type == BLE_HCI_UART_H4_CMD) {
+        len = sizeof(struct ble_hci_cmd) + hci_ev[2];
+        STATS_INC(hci_sock_stats, ocmd);
+    } else if (h4_type == BLE_HCI_UART_H4_EVT) {
+        len = sizeof(struct ble_hci_ev) + hci_ev[1];
+        STATS_INC(hci_sock_stats, oevt);
+    } else {
+        assert(0);
+    }
+    iov[1].iov_len = len;
+
+    STATS_INC(hci_sock_stats, omsg);
+    STATS_INCN(hci_sock_stats, obytes, len + 1);
+
+    i = sendmsg(ble_hci_sock_state.sock, &msg, 0);
+    ble_hci_trans_buf_free(hci_ev);
+    if (i != len + 1) {
+        if (i < 0) {
+            dprintf(1, "sendmsg() failed : %d\n", errno);
+        } else {
+            dprintf(1, "sendmsg() partial write: %d\n", i);
+        }
+        STATS_INC(hci_sock_stats, oerr);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    return 0;
+}
+
+static int
+ble_hci_sock_rx_msg(void)
+{
+    struct ble_hci_sock_state *bhss;
+    int len;
+    struct os_mbuf *m;
+    uint8_t *data;
+    int sr;
+    int rc;
+
+    bhss = &ble_hci_sock_state;
+    if (bhss->sock < 0) {
+        return -1;
+    }
+    len = read(bhss->sock, bhss->rx_data + bhss->rx_off,
+               sizeof(bhss->rx_data) - bhss->rx_off);
+    if (len < 0) {
+        return -2;
+    }
+    if (len == 0) {
+        return -1;
+    }
+    bhss->rx_off += len;
+    STATS_INCN(hci_sock_stats, ibytes, len);
+
+    while (bhss->rx_off > 0) {
+        switch (bhss->rx_data[0]) {
+#if MYNEWT_VAL(BLE_CONTROLLER)
+        case BLE_HCI_UART_H4_CMD:
+            if (bhss->rx_off < sizeof(struct ble_hci_cmd)) {
+                return -1;
+            }
+            len = 1 + sizeof(struct ble_hci_cmd) + bhss->rx_data[3];
+            if (bhss->rx_off < len) {
+                return -1;
+            }
+            STATS_INC(hci_sock_stats, imsg);
+            STATS_INC(hci_sock_stats, icmd);
+            data = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+            if (!data) {
+                STATS_INC(hci_sock_stats, ierr);
+                break;
+            }
+            memcpy(data, &bhss->rx_data[1], len - 1);
+            OS_ENTER_CRITICAL(sr);
+            rc = ble_hci_sock_rx_cmd_cb(data, ble_hci_sock_rx_cmd_arg);
+            OS_EXIT_CRITICAL(sr);
+            if (rc) {
+                ble_hci_trans_buf_free(data);
+                STATS_INC(hci_sock_stats, ierr);
+                break;
+            }
+            break;
+#endif
+#if MYNEWT_VAL(BLE_HOST)
+        case BLE_HCI_UART_H4_EVT:
+            if (bhss->rx_off < sizeof(struct ble_hci_ev)) {
+                return -1;
+            }
+            len = 1 + sizeof(struct ble_hci_ev) + bhss->rx_data[2];
+            if (bhss->rx_off < len) {
+                return -1;
+            }
+            STATS_INC(hci_sock_stats, imsg);
+            STATS_INC(hci_sock_stats, ievt);
+            data = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (!data) {
+                STATS_INC(hci_sock_stats, ierr);
+                break;
+            }
+            memcpy(data, &bhss->rx_data[1], len - 1);
+            OS_ENTER_CRITICAL(sr);
+            rc = ble_hci_sock_rx_cmd_cb(data, ble_hci_sock_rx_cmd_arg);
+            OS_EXIT_CRITICAL(sr);
+            if (rc) {
+                ble_hci_trans_buf_free(data);
+                STATS_INC(hci_sock_stats, ierr);
+                return 0;
+            }
+            break;
+#endif
+        case BLE_HCI_UART_H4_ACL:
+            if (bhss->rx_off < BLE_HCI_DATA_HDR_SZ) {
+                return -1;
+            }
+            len = 1 + BLE_HCI_DATA_HDR_SZ + (bhss->rx_data[4] << 8) +
+              bhss->rx_data[3];
+            if (bhss->rx_off < len) {
+                return -1;
+            }
+            STATS_INC(hci_sock_stats, imsg);
+            STATS_INC(hci_sock_stats, iacl);
+            m = ble_hci_trans_acl_buf_alloc();
+            if (!m) {
+                STATS_INC(hci_sock_stats, imem);
+                break;
+            }
+            if (os_mbuf_append(m, &bhss->rx_data[1], len - 1)) {
+                STATS_INC(hci_sock_stats, imem);
+                os_mbuf_free_chain(m);
+                break;
+            }
+            OS_ENTER_CRITICAL(sr);
+            ble_hci_sock_rx_acl_cb(m, ble_hci_sock_rx_acl_arg);
+            OS_EXIT_CRITICAL(sr);
+            break;
+        default:
+            STATS_INC(hci_sock_stats, ierr);
+            break;
+        }
+        memmove(bhss->rx_data, &bhss->rx_data[len], bhss->rx_off - len);
+        bhss->rx_off -= len;
+    }
+    return 0;
+}
+
+static void
+ble_hci_sock_rx_ev(struct ble_npl_event *ev)
+{
+    int rc;
+    ble_npl_time_t timeout;
+
+    rc = ble_hci_sock_rx_msg();
+    if (rc == 0) {
+        ble_npl_eventq_put(&ble_hci_sock_state.evq, &ble_hci_sock_state.ev);
+    } else {
+        rc = ble_npl_time_ms_to_ticks(10, &timeout);
+        ble_npl_callout_reset(&ble_hci_sock_state.timer, timeout);
+    }
+}
+
+#if MYNEWT_VAL(BLE_SOCK_USE_TCP)
+static int
+ble_hci_sock_config(void)
+{
+    struct ble_hci_sock_state *bhss = &ble_hci_sock_state;
+    struct sockaddr_in sin;
+    ble_npl_time_t timeout;
+    int s;
+    int rc;
+
+    memset(&sin, 0, sizeof(sin));
+    sin.sin_family = AF_INET;
+    sin.sin_port = htons(s_ble_hci_device);
+    sin.sin_addr.s_addr = inet_addr("127.0.0.1");
+#ifdef MN_OSX
+    sin.sin_len = sizeof(sin);
+#endif
+
+#if 0
+    if (bhss->sock >= 0) {
+        close(bhss->sock);
+        bhss->sock = -1;
+    }
+#endif
+    if (bhss->sock < 0) {
+        s = socket(PF_INET, SOCK_STREAM, 0);
+        if (s < 0) {
+            goto err;
+        }
+
+        rc = connect(s, (struct sockaddr *)&sin, sizeof(sin));
+        if (rc) {
+            dprintf(1, "connect() failed: %d\n", errno);
+            goto err;
+        }
+
+        rc = 1;
+
+        rc = ioctl(s, FIONBIO, (char *)&rc);
+        if (rc) {
+            goto err;
+        }
+        bhss->sock = s;
+    }
+    rc = ble_npl_time_ms_to_ticks(10, &timeout);
+    if (rc) {
+        goto err;
+    }
+    ble_npl_callout_reset(&ble_hci_sock_state.timer, timeout);
+
+    return 0;
+err:
+    if (s >= 0) {
+        close(s);
+    }
+    return BLE_ERR_HW_FAIL;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_SOCK_USE_LINUX_BLUE)
+static int
+ble_hci_sock_config(void)
+{
+    struct sockaddr_hci shci;
+    int s;
+    int rc;
+    ble_npl_time_t timeout;
+
+    memset(&shci, 0, sizeof(shci));
+    shci.hci_family = AF_BLUETOOTH;
+    shci.hci_dev = s_ble_hci_device;
+    shci.hci_channel = HCI_CHANNEL_USER;
+
+    if (ble_hci_sock_state.sock >= 0) {
+        close(ble_hci_sock_state.sock);
+        ble_hci_sock_state.sock = -1;
+    }
+
+    s = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
+    if (s < 0) {
+        dprintf(1, "socket() failed %d\n", errno);
+        goto err;
+    }
+
+    /* 
+     * HCI User Channel requires exclusive access to the device.
+     * The device has to be down at the time of binding.
+     */
+    ioctl(s, HCIDEVDOWN, shci.hci_dev);
+
+    rc = bind(s, (struct sockaddr *)&shci, sizeof(shci));
+    if (rc) {
+        dprintf(1, "bind() failed %d hci%d\n", errno, shci.hci_dev);
+        goto err;
+    }
+
+    rc = 1;
+
+    rc = ioctl(s, FIONBIO, (char *)&rc);
+    if (rc) {
+        goto err;
+    }
+    ble_hci_sock_state.sock = s;
+
+    rc = ble_npl_time_ms_to_ticks(10, &timeout);
+    if (rc) {
+        goto err;
+    }
+    ble_npl_callout_reset(&ble_hci_sock_state.timer, timeout);
+
+    return 0;
+err:
+    if (s >= 0) {
+        close(s);
+    }
+    return BLE_ERR_HW_FAIL;
+}
+#endif
+/**
+ * Sends an HCI event from the controller to the host.
+ *
+ * @param cmd                   The HCI event to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_ll_evt_tx(uint8_t *cmd)
+{
+    return ble_hci_sock_cmdevt_tx(cmd, BLE_HCI_UART_H4_EVT);
+}
+
+/**
+ * Sends ACL data from controller to host.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_ll_acl_tx(struct os_mbuf *om)
+{
+    return ble_hci_sock_acl_tx(om);
+}
+
+/**
+ * Sends an HCI command from the host to the controller.
+ *
+ * @param cmd                   The HCI command to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    return ble_hci_sock_cmdevt_tx(cmd, BLE_HCI_UART_H4_CMD);
+}
+
+/**
+ * Sends ACL data from host to controller.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+    return ble_hci_sock_acl_tx(om);
+}
+
+/**
+ * Configures the HCI transport to call the specified callback upon receiving
+ * HCI packets from the controller.  This function should only be called by by
+ * host.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_sock_rx_cmd_cb = cmd_cb;
+    ble_hci_sock_rx_cmd_arg = cmd_arg;
+    ble_hci_sock_rx_acl_cb = acl_cb;
+    ble_hci_sock_rx_acl_arg = acl_arg;
+}
+
+/**
+ * Configures the HCI transport to operate with a host.  The transport will
+ * execute specified callbacks upon receiving HCI packets from the controller.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_sock_rx_cmd_cb = cmd_cb;
+    ble_hci_sock_rx_cmd_arg = cmd_arg;
+    ble_hci_sock_rx_acl_cb = acl_cb;
+    ble_hci_sock_rx_acl_arg = acl_arg;
+}
+
+/**
+ * Allocates a flat buffer of the specified type.
+ *
+ * @param type                  The type of buffer to allocate; one of the
+ *                                  BLE_HCI_TRANS_BUF_[...] constants.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_CMD:
+        buf = os_memblock_get(&ble_hci_sock_cmd_pool);
+        break;
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&ble_hci_sock_evt_hi_pool);
+        if (buf == NULL) {
+            /* If no high-priority event buffers remain, try to grab a
+             * low-priority one.
+             */
+            buf = os_memblock_get(&ble_hci_sock_evt_lo_pool);
+        }
+        break;
+
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+        buf = os_memblock_get(&ble_hci_sock_evt_lo_pool);
+        break;
+
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+/**
+ * Frees the specified flat buffer.  The buffer must have been allocated via
+ * ble_hci_trans_buf_alloc().
+ *
+ * @param buf                   The buffer to free.
+ */
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    /*
+     * XXX: this may look a bit odd, but the controller uses the command
+     * buffer to send back the command complete/status as an immediate
+     * response to the command. This was done to insure that the controller
+     * could always send back one of these events when a command was received.
+     * Thus, we check to see which pool the buffer came from so we can free
+     * it to the appropriate pool
+     */
+    if (os_memblock_from(&ble_hci_sock_evt_hi_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_sock_evt_hi_pool, buf);
+        assert(rc == 0);
+    } else if (os_memblock_from(&ble_hci_sock_evt_lo_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_sock_evt_lo_pool, buf);
+        assert(rc == 0);
+    } else {
+        assert(os_memblock_from(&ble_hci_sock_cmd_pool, buf));
+        rc = os_memblock_put(&ble_hci_sock_cmd_pool, buf);
+        assert(rc == 0);
+    }
+}
+
+/**
+ * Resets the HCI UART transport to a clean state.  Frees all buffers and
+ * reconfigures the UART.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_reset(void)
+{
+    int rc;
+
+    ble_npl_callout_stop(&ble_hci_sock_state.timer);
+
+    /* Reopen the UART. */
+    rc = ble_hci_sock_config();
+    if (rc != 0) {
+        dprintf(1, "Failure restarting socket HCI\n");
+        return rc;
+    }
+
+    return 0;
+}
+
+void
+ble_hci_sock_ack_handler(void *arg)
+{
+    struct ble_npl_event *ev;
+
+    while (1) {
+        ev = ble_npl_eventq_get(&ble_hci_sock_state.evq, BLE_NPL_TIME_FOREVER);
+        ble_npl_event_run(ev);
+    }
+}
+
+static void
+ble_hci_sock_init_task(void)
+{
+    ble_npl_eventq_init(&ble_hci_sock_state.evq);
+    ble_npl_callout_stop(&ble_hci_sock_state.timer);
+    ble_npl_callout_init(&ble_hci_sock_state.timer, &ble_hci_sock_state.evq,
+                    ble_hci_sock_rx_ev, NULL);
+
+#if MYNEWT
+    {
+        os_stack_t *pstack;
+
+        pstack = malloc(sizeof(os_stack_t)*BLE_SOCK_STACK_SIZE);
+        assert(pstack);
+        os_task_init(&ble_sock_task, "hci_sock", ble_hci_sock_ack_handler, NULL,
+                     MYNEWT_VAL(BLE_SOCK_TASK_PRIO), BLE_NPL_TIME_FOREVER, pstack,
+                     BLE_SOCK_STACK_SIZE);
+    }
+#else
+/*
+ * For non-Mynewt OS it is required that OS creates task for HCI SOCKET
+ * to run ble_hci_sock_ack_handler.
+ */
+
+#endif
+
+}
+
+void
+ble_hci_sock_set_device(int dev)
+{
+    s_ble_hci_device = dev;
+}
+
+/**
+ * Initializes the UART HCI transport module.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+void
+ble_hci_sock_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    memset(&ble_hci_sock_state, 0, sizeof(ble_hci_sock_state));
+    ble_hci_sock_state.sock = -1;
+
+    ble_hci_sock_init_task();
+    ble_npl_event_init(&ble_hci_sock_state.ev, ble_hci_sock_rx_ev, NULL);
+
+    rc = os_mempool_init(&ble_hci_sock_acl_pool,
+                         MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                         ACL_BLOCK_SIZE,
+                         ble_hci_sock_acl_buf,
+                         "ble_hci_sock_acl_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mbuf_pool_init(&ble_hci_sock_acl_mbuf_pool,
+                           &ble_hci_sock_acl_pool,
+                           ACL_BLOCK_SIZE,
+                           MYNEWT_VAL(BLE_ACL_BUF_COUNT));
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    /*
+     * Create memory pool of HCI command buffers. NOTE: we currently dont
+     * allow this to be configured. The controller will only allow one
+     * outstanding command. We decided to keep this a pool in case we allow
+     * allow the controller to handle more than one outstanding command.
+     */
+    rc = os_mempool_init(&ble_hci_sock_cmd_pool,
+                         1,
+                         BLE_HCI_TRANS_CMD_SZ,
+                         &ble_hci_sock_cmd_buf,
+                         "ble_hci_sock_cmd_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_sock_evt_hi_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         &ble_hci_sock_evt_hi_buf,
+                         "ble_hci_sock_evt_hi_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_sock_evt_lo_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_sock_evt_lo_buf,
+                         "ble_hci_sock_evt_lo_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_hci_sock_config();
+    SYSINIT_PANIC_ASSERT_MSG(rc == 0, "Failure configuring socket HCI");
+
+    rc = stats_init_and_reg(STATS_HDR(hci_sock_stats),
+                            STATS_SIZE_INIT_PARMS(hci_sock_stats, STATS_SIZE_32),
+                            STATS_NAME_INIT_PARMS(hci_sock_stats), "hci_socket");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/src/libs/mynewt-nimble/nimble/transport/socket/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/socket/syscfg.yml
new file mode 100644
index 00000000..2050f646
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/socket/syscfg.yml
@@ -0,0 +1,79 @@
+# 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_HCI_EVT_BUF_SIZE:
+        description: 'The size of the allocated event buffers'
+        value: 70
+    BLE_HCI_EVT_HI_BUF_COUNT:
+        description: 'The number of high priority event buffers'
+        value: 8
+    BLE_HCI_EVT_LO_BUF_COUNT:
+        description: 'The number of low priority event buffers'
+        value: 8
+    BLE_ACL_BUF_COUNT:
+        description: 'The number of ACL data buffers'
+        value: 24
+    BLE_ACL_BUF_SIZE:
+        description: >
+            This is the maximum size of the data portion of HCI ACL data
+            packets. It does not include the HCI data header (of 4 bytes).
+        value: 255
+
+    BLE_HCI_ACL_OUT_COUNT:
+        description: >
+            This count is used in creating a pool of elements used by the
+            code to enqueue various elements. In the case of the controller
+            only HCI, this number should be equal to the number of mbufs in
+            the msys pool. For host only, it is really dependent on the
+            number of ACL buffers that the controller tells the host it
+            has.
+        value: 12
+
+    BLE_SOCK_USE_TCP:
+        description: 'Use TCP socket, connects to BLE_SOCK_TCP_PORT'
+        value: 1
+
+    BLE_SOCK_TCP_PORT:
+        description: 'ipv4 tcp port to connect to'
+        value: 14433
+
+    BLE_SOCK_USE_LINUX_BLUE:
+        description: 'Use Linux bluetooth raw socket'
+        value: 0
+
+    BLE_SOCK_LINUX_DEV:
+        description: 'linux kernel device'
+        value: 0
+
+    BLE_SOCK_TASK_PRIO:
+        description: 'Priority of the HCI socket task.'
+        type: task_priority
+        value: 9
+
+    BLE_SOCK_STACK_SIZE:
+        description: 'Size of the HCI socket stack (units=words).'
+        value: 80
+
+    BLE_SOCK_CLI_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the socket BLE transport.
+        value: 500
+
+syscfg.vals.BLE_EXT_ADV:
+    BLE_HCI_EVT_BUF_SIZE: 257
diff --git a/src/libs/mynewt-nimble/nimble/transport/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/syscfg.yml
new file mode 100644
index 00000000..137d6e94
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/syscfg.yml
@@ -0,0 +1,68 @@
+# 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_HCI_TRANSPORT:
+        description: >
+            Selects HCI transport to be included in build.
+            This has virtually the same effect as including package dependency
+            manually, but it allows to easily override HCI transport package in
+            application or target settings.
+        value: builtin
+        restrictions: $notnull
+        choices:
+            - builtin   # Built-in NimBLE controller and RAM transport
+            - custom    # Custom transport, has to be included manually by user
+            - ram       # RAM transport
+            - uart      # UART HCI H4 transport
+            - socket    # Socket transport (for native builds)
+            - emspi     # SPI transport for EM Microelectionic controllers
+            - da1469x   # Dialog DA1469x integrated controller
+
+# Deprecated settings
+    BLE_HCI_TRANSPORT_NIMBLE_BUILTIN:
+        description: Use BLE_HCI_TRANSPORT instead.
+        value: 0
+        deprecated: 1
+    BLE_HCI_TRANSPORT_EMSPI:
+        description: Use BLE_HCI_TRANSPORT instead.
+        value: 0
+        deprecated: 1
+    BLE_HCI_TRANSPORT_RAM:
+        description: Use BLE_HCI_TRANSPORT instead.
+        value: 0
+        deprecated: 1
+    BLE_HCI_TRANSPORT_SOCKET:
+        description: Use BLE_HCI_TRANSPORT instead.
+        value: 0
+        deprecated: 1
+    BLE_HCI_TRANSPORT_UART:
+        description: Use BLE_HCI_TRANSPORT instead.
+        value: 0
+        deprecated: 1
+
+syscfg.vals.BLE_HCI_TRANSPORT_NIMBLE_BUILTIN:
+    BLE_HCI_TRANSPORT: builtin
+syscfg.vals.BLE_HCI_TRANSPORT_RAM:
+    BLE_HCI_TRANSPORT: ram
+syscfg.vals.BLE_HCI_TRANSPORT_UART:
+    BLE_HCI_TRANSPORT: uart
+syscfg.vals.BLE_HCI_TRANSPORT_SOCKET:
+    BLE_HCI_TRANSPORT: socket
+syscfg.vals.BLE_HCI_TRANSPORT_EMSPI:
+    BLE_HCI_TRANSPORT: emspi
diff --git a/src/libs/mynewt-nimble/nimble/transport/uart/include/transport/uart/ble_hci_uart.h b/src/libs/mynewt-nimble/nimble/transport/uart/include/transport/uart/ble_hci_uart.h
new file mode 100644
index 00000000..e5e10841
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/uart/include/transport/uart/ble_hci_uart.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_HCI_UART_
+#define H_BLE_HCI_UART_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_hci_uart_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/libs/mynewt-nimble/nimble/transport/uart/pkg.yml b/src/libs/mynewt-nimble/nimble/transport/uart/pkg.yml
new file mode 100644
index 00000000..95681373
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/uart/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/transport/uart
+pkg.description: XXX
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - "@apache-mynewt-core/hw/hal"
+    - "@apache-mynewt-core/kernel/os"
+    - "@apache-mynewt-core/util/mem"
+    - nimble
+
+pkg.apis:
+    - ble_transport
+
+pkg.init:
+    ble_hci_uart_init: 'MYNEWT_VAL(BLE_TRANS_UART_SYSINIT_STAGE)'
diff --git a/src/libs/mynewt-nimble/nimble/transport/uart/src/ble_hci_uart.c b/src/libs/mynewt-nimble/nimble/transport/uart/src/ble_hci_uart.c
new file mode 100644
index 00000000..ac6af28e
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/uart/src/ble_hci_uart.c
@@ -0,0 +1,1187 @@
+/*
+ * 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 <stdio.h>
+#include <errno.h>
+#include <stdint.h>
+#include "sysinit/sysinit.h"
+#include "syscfg/syscfg.h"
+#include "os/os_cputime.h"
+#include "bsp/bsp.h"
+#include "os/os.h"
+#include "mem/mem.h"
+#include "hal/hal_gpio.h"
+#include "hal/hal_uart.h"
+
+/* BLE */
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+
+#include "transport/uart/ble_hci_uart.h"
+
+#define BLE_HCI_UART_EVT_COUNT  \
+    (MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT) + MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT))
+
+/***
+ * NOTES:
+ * The UART HCI transport doesn't use event buffer priorities.  All incoming
+ * and outgoing events use buffers from the same pool.
+ *
+ * The "skip" definitions are here so that when buffers cannot be allocated,
+ * the command or acl packets are simply skipped so that the HCI interface
+ * does not lose synchronization and resets dont (necessarily) occur.
+ */
+
+/* XXX: for now, define this here */
+#if MYNEWT_VAL(BLE_CONTROLLER)
+extern void ble_ll_data_buffer_overflow(void);
+extern void ble_ll_hw_error(uint8_t err);
+
+static const uint8_t ble_hci_uart_reset_cmd[4] = { 0x01, 0x03, 0x0C, 0x00 };
+#endif
+
+/***
+ * NOTES:
+ * The "skip" definitions are here so that when buffers cannot be allocated,
+ * the command or acl packets are simply skipped so that the HCI interface
+ * does not lose synchronization and resets dont (necessarily) occur.
+ */
+#define BLE_HCI_UART_H4_NONE        0x00
+#define BLE_HCI_UART_H4_CMD         0x01
+#define BLE_HCI_UART_H4_ACL         0x02
+#define BLE_HCI_UART_H4_SCO         0x03
+#define BLE_HCI_UART_H4_EVT         0x04
+#define BLE_HCI_UART_H4_SYNC_LOSS   0x80
+#define BLE_HCI_UART_H4_SKIP_CMD    0x81
+#define BLE_HCI_UART_H4_SKIP_ACL    0x82
+#define BLE_HCI_UART_H4_LE_EVT      0x83
+#define BLE_HCI_UART_H4_SKIP_EVT    0x84
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_uart_rx_cmd_cb;
+static void *ble_hci_uart_rx_cmd_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_uart_rx_acl_cb;
+static void *ble_hci_uart_rx_acl_arg;
+
+static struct os_mempool ble_hci_uart_evt_hi_pool;
+static os_membuf_t ble_hci_uart_evt_hi_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_uart_evt_lo_pool;
+static os_membuf_t ble_hci_uart_evt_lo_buf[
+        OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                        MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE))
+];
+
+static struct os_mempool ble_hci_uart_cmd_pool;
+static os_membuf_t ble_hci_uart_cmd_buf[
+	OS_MEMPOOL_SIZE(1, BLE_HCI_TRANS_CMD_SZ)
+];
+
+static struct os_mbuf_pool ble_hci_uart_acl_mbuf_pool;
+static struct os_mempool_ext ble_hci_uart_acl_pool;
+
+/*
+ * The MBUF payload size must accommodate the HCI data header size plus the
+ * maximum ACL data packet length. The ACL block size is the size of the
+ * mbufs we will allocate.
+ */
+#define ACL_BLOCK_SIZE  OS_ALIGN(MYNEWT_VAL(BLE_ACL_BUF_SIZE) \
+                                 + BLE_MBUF_MEMBLOCK_OVERHEAD \
+                                 + BLE_HCI_DATA_HDR_SZ, OS_ALIGNMENT)
+
+static os_membuf_t ble_hci_uart_acl_buf[
+	OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                        ACL_BLOCK_SIZE)
+];
+
+/**
+ * A packet to be sent over the UART.  This can be a command, an event, or ACL
+ * data.
+ */
+struct ble_hci_uart_pkt {
+    STAILQ_ENTRY(ble_hci_uart_pkt) next;
+    void *data;
+    uint8_t type;
+};
+
+static struct os_mempool ble_hci_uart_pkt_pool;
+static os_membuf_t ble_hci_uart_pkt_buf[
+        OS_MEMPOOL_SIZE(BLE_HCI_UART_EVT_COUNT + 1 +
+                        MYNEWT_VAL(BLE_HCI_ACL_OUT_COUNT),
+                        sizeof (struct ble_hci_uart_pkt))
+];
+
+/**
+ * An incoming or outgoing command or event.
+ */
+struct ble_hci_uart_cmd {
+    uint8_t *data;      /* Pointer to ble_hci_uart_cmd data */
+    uint16_t cur;       /* Number of bytes read/written */
+    uint16_t len;       /* Total number of bytes to read/write */
+};
+
+/**
+ * An incoming ACL data packet.
+ */
+struct ble_hci_uart_acl {
+    struct os_mbuf *buf; /* Buffer containing the data */
+    uint8_t *dptr;       /* Pointer to where bytes should be placed */
+    uint16_t len;        /* Target size when buf is considered complete */
+    uint16_t rxd_bytes;  /* current count of bytes received for packet */
+};
+
+/**
+ * Structure for transmitting ACL packets over UART
+ *
+ */
+struct ble_hci_uart_h4_acl_tx
+{
+    uint8_t *dptr;
+    struct os_mbuf *tx_acl;
+};
+
+static struct {
+    /*** State of data received over UART. */
+    uint8_t rx_type;    /* Pending packet type. 0 means nothing pending */
+    union {
+        struct ble_hci_uart_cmd rx_cmd;
+        struct ble_hci_uart_acl rx_acl;
+    };
+
+    /*** State of data transmitted over UART. */
+    uint8_t tx_type;    /* Pending packet type. 0 means nothing pending */
+    uint16_t rem_tx_len; /* Used for acl tx only currently */
+    union {
+        struct ble_hci_uart_cmd tx_cmd;
+        struct ble_hci_uart_h4_acl_tx tx_pkt;
+    };
+    STAILQ_HEAD(, ble_hci_uart_pkt) tx_pkts; /* Packet queue to send to UART */
+} ble_hci_uart_state;
+
+/**
+ * Allocates a buffer (mbuf) for ACL operation.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+static struct os_mbuf *
+ble_hci_trans_acl_buf_alloc(void)
+{
+    struct os_mbuf *m;
+    uint8_t usrhdr_len;
+
+#if MYNEWT_VAL(BLE_CONTROLLER)
+    usrhdr_len = sizeof(struct ble_mbuf_hdr);
+#elif MYNEWT_VAL(BLE_HS_FLOW_CTRL)
+    usrhdr_len = BLE_MBUF_HS_HDR_LEN;
+#else
+    usrhdr_len = 0;
+#endif
+
+    m = os_mbuf_get_pkthdr(&ble_hci_uart_acl_mbuf_pool, usrhdr_len);
+    return m;
+}
+
+static int
+ble_hci_uart_acl_tx(struct os_mbuf *om)
+{
+    struct ble_hci_uart_pkt *pkt;
+    os_sr_t sr;
+
+    /* If this packet is zero length, just free it */
+    if (OS_MBUF_PKTLEN(om) == 0) {
+        os_mbuf_free_chain(om);
+        return 0;
+    }
+
+    pkt = os_memblock_get(&ble_hci_uart_pkt_pool);
+    if (pkt == NULL) {
+        os_mbuf_free_chain(om);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    pkt->type = BLE_HCI_UART_H4_ACL;
+    pkt->data = om;
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&ble_hci_uart_state.tx_pkts, pkt, next);
+    OS_EXIT_CRITICAL(sr);
+
+    hal_uart_start_tx(MYNEWT_VAL(BLE_HCI_UART_PORT));
+
+    return 0;
+}
+
+static int
+ble_hci_uart_cmdevt_tx(uint8_t *hci_ev, uint8_t h4_type)
+{
+    struct ble_hci_uart_pkt *pkt;
+    os_sr_t sr;
+
+    pkt = os_memblock_get(&ble_hci_uart_pkt_pool);
+    if (pkt == NULL) {
+        ble_hci_trans_buf_free(hci_ev);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    pkt->type = h4_type;
+    pkt->data = hci_ev;
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&ble_hci_uart_state.tx_pkts, pkt, next);
+    OS_EXIT_CRITICAL(sr);
+
+    hal_uart_start_tx(MYNEWT_VAL(BLE_HCI_UART_PORT));
+
+    return 0;
+}
+
+/**
+ * @return                      The packet type to transmit on success;
+ *                              -1 if there is nothing to send.
+ */
+static int
+ble_hci_uart_tx_pkt_type(void)
+{
+    struct ble_hci_uart_pkt *pkt;
+    struct os_mbuf *om;
+    os_sr_t sr;
+    int rc;
+
+    OS_ENTER_CRITICAL(sr);
+
+    pkt = STAILQ_FIRST(&ble_hci_uart_state.tx_pkts);
+    if (!pkt) {
+        OS_EXIT_CRITICAL(sr);
+        return -1;
+    }
+
+    STAILQ_REMOVE(&ble_hci_uart_state.tx_pkts, pkt, ble_hci_uart_pkt, next);
+
+    OS_EXIT_CRITICAL(sr);
+
+    rc = pkt->type;
+    switch (pkt->type) {
+    case BLE_HCI_UART_H4_CMD:
+        ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_CMD;
+        ble_hci_uart_state.tx_cmd.data = pkt->data;
+        ble_hci_uart_state.tx_cmd.cur = 0;
+        ble_hci_uart_state.tx_cmd.len = ble_hci_uart_state.tx_cmd.data[2] +
+                                        sizeof(struct ble_hci_cmd);
+        break;
+
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_EVT;
+        ble_hci_uart_state.tx_cmd.data = pkt->data;
+        ble_hci_uart_state.tx_cmd.cur = 0;
+        ble_hci_uart_state.tx_cmd.len = ble_hci_uart_state.tx_cmd.data[1] +
+                                        sizeof(struct ble_hci_ev);
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_ACL;
+        om = (struct os_mbuf *)pkt->data;
+        /* NOTE: first mbuf must have non-zero length */
+        os_mbuf_trim_front(om);
+        ble_hci_uart_state.tx_pkt.tx_acl = om;
+        ble_hci_uart_state.tx_pkt.dptr = om->om_data;
+        ble_hci_uart_state.rem_tx_len = OS_MBUF_PKTLEN(om);
+        break;
+
+    default:
+        rc = -1;
+        break;
+    }
+
+    os_memblock_put(&ble_hci_uart_pkt_pool, pkt);
+
+    return rc;
+}
+
+/**
+ * @return                      The byte to transmit on success;
+ *                              -1 if there is nothing to send.
+ */
+static int
+ble_hci_uart_tx_char(void *arg)
+{
+    uint8_t u8;
+    int rc;
+    struct os_mbuf *om;
+
+    switch (ble_hci_uart_state.tx_type) {
+    case BLE_HCI_UART_H4_NONE: /* No pending packet, pick one from the queue */
+        rc = ble_hci_uart_tx_pkt_type();
+        break;
+
+    case BLE_HCI_UART_H4_CMD:
+    case BLE_HCI_UART_H4_EVT:
+        rc = ble_hci_uart_state.tx_cmd.data[ble_hci_uart_state.tx_cmd.cur++];
+
+        if (ble_hci_uart_state.tx_cmd.cur == ble_hci_uart_state.tx_cmd.len) {
+            ble_hci_trans_buf_free(ble_hci_uart_state.tx_cmd.data);
+            ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+        }
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        /* Copy the first unsent byte from the tx buffer and remove it from the
+         * source.
+         */
+        u8 = ble_hci_uart_state.tx_pkt.dptr[0];
+        --ble_hci_uart_state.rem_tx_len;
+        if (ble_hci_uart_state.rem_tx_len == 0) {
+            os_mbuf_free_chain(ble_hci_uart_state.tx_pkt.tx_acl);
+            ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+        } else {
+            om = ble_hci_uart_state.tx_pkt.tx_acl;
+            --om->om_len;
+            if (om->om_len == 0) {
+                /* Remove and free any zero mbufs */
+                while ((om != NULL) && (om->om_len == 0)) {
+                    ble_hci_uart_state.tx_pkt.tx_acl = SLIST_NEXT(om, om_next);
+                    os_mbuf_free(om);
+                    om = ble_hci_uart_state.tx_pkt.tx_acl;
+                }
+                /* NOTE: om should never be NULL! What to do? */
+                if (om == NULL) {
+                    assert(0);
+                    ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+                } else {
+                    ble_hci_uart_state.tx_pkt.dptr = om->om_data;
+                }
+            } else {
+                ble_hci_uart_state.tx_pkt.dptr++;
+            }
+        }
+        rc = u8;
+        break;
+    default:
+        rc = -1;
+        break;
+    }
+
+    return rc;
+}
+
+#if MYNEWT_VAL(BLE_CONTROLLER)
+/**
+ * HCI uart sync lost.
+ *
+ * This occurs when the controller receives an invalid packet type or a length
+ * field that is out of range. The controller needs to send a HW error to the
+ * host and wait to find a LL reset command.
+ */
+static void
+ble_hci_uart_sync_lost(void)
+{
+    ble_hci_uart_state.rx_cmd.len = 0;
+    ble_hci_uart_state.rx_cmd.cur = 0;
+    ble_hci_uart_state.rx_cmd.data =
+        ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+    ble_ll_hw_error(BLE_HW_ERR_HCI_SYNC_LOSS);
+    ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_SYNC_LOSS;
+}
+#endif
+
+/**
+ * @return                      The type of packet to follow success;
+ *                              -1 if there is no valid packet to receive.
+ */
+static int
+ble_hci_uart_rx_pkt_type(uint8_t data)
+{
+    struct os_mbuf *m;
+
+    ble_hci_uart_state.rx_type = data;
+
+    switch (ble_hci_uart_state.rx_type) {
+    /* Host should never receive a command! */
+#if MYNEWT_VAL(BLE_CONTROLLER)
+    case BLE_HCI_UART_H4_CMD:
+        ble_hci_uart_state.rx_cmd.len = 0;
+        ble_hci_uart_state.rx_cmd.cur = 0;
+        ble_hci_uart_state.rx_cmd.data =
+            ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+        if (ble_hci_uart_state.rx_cmd.data == NULL) {
+            ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_SKIP_CMD;
+        }
+        break;
+#endif
+
+        /* Controller should never receive an event */
+#if MYNEWT_VAL(BLE_HOST)
+    case BLE_HCI_UART_H4_EVT:
+        /*
+         * The event code is unknown at the moment. Depending on event priority,
+         * buffer *shall* be allocated from ble_hci_uart_evt_hi_pool
+         * or "may* be allocated from ble_hci_uart_evt_lo_pool.
+         * Thus do not allocate the buffer yet.
+         */
+        ble_hci_uart_state.rx_cmd.data = NULL;
+        ble_hci_uart_state.rx_cmd.len = 0;
+        ble_hci_uart_state.rx_cmd.cur = 0;
+        break;
+#endif
+
+    case BLE_HCI_UART_H4_ACL:
+        ble_hci_uart_state.rx_acl.len = 0;
+        ble_hci_uart_state.rx_acl.rxd_bytes = 0;
+        m = ble_hci_trans_acl_buf_alloc();
+        if (m) {
+            ble_hci_uart_state.rx_acl.dptr = m->om_data;
+        } else {
+            ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_SKIP_ACL;
+        }
+        ble_hci_uart_state.rx_acl.buf = m;
+        break;
+
+    default:
+#if MYNEWT_VAL(BLE_CONTROLLER)
+        /*
+         * If we receive an unknown HCI packet type this is considered a loss
+         * of sync.
+         */
+        ble_hci_uart_sync_lost();
+#else
+        /*
+         * XXX: not sure what to do about host in this case. Just go back to
+         * none for now.
+         */
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+#endif
+        break;
+    }
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_CONTROLLER)
+/**
+ * HCI uart sync loss.
+ *
+ * Find a LL reset command in the byte stream. The LL reset command is a
+ * sequence of 4 bytes:
+ *  0x01    HCI Packet Type = HCI CMD
+ *  0x03    OCF for reset command
+ *  0x0C    OGF for reset command (0x03 shifted left by two bits as the OGF
+ *          occupies the uopper 6 bits of this byte.
+ *  0x00    Parameter length of reset command (no parameters).
+ *
+ * @param data Byte received over serial port
+ */
+void
+ble_hci_uart_rx_sync_loss(uint8_t data)
+{
+    int rc;
+    int index;
+
+    /*
+     * If we couldnt allocate a command buffer (should not occur but
+     * possible) try to allocate one on each received character. If we get
+     * a reset and buffer is not available we have to ignore reset.
+     */
+    if (ble_hci_uart_state.rx_cmd.data == NULL) {
+        ble_hci_uart_state.rx_cmd.data =
+            ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+    }
+
+    index = ble_hci_uart_state.rx_cmd.cur;
+    if (data == ble_hci_uart_reset_cmd[index]) {
+        if (index == 3) {
+            if (ble_hci_uart_state.rx_cmd.data == NULL) {
+                index = 0;
+            } else {
+                assert(ble_hci_uart_rx_cmd_cb != NULL);
+                ble_hci_uart_state.rx_cmd.data[0] = 0x03;
+                ble_hci_uart_state.rx_cmd.data[1] = 0x0C;
+                ble_hci_uart_state.rx_cmd.data[2] = 0x00;
+                rc = ble_hci_uart_rx_cmd_cb(ble_hci_uart_state.rx_cmd.data,
+                                            ble_hci_uart_rx_cmd_arg);
+                if (rc != 0) {
+                    ble_hci_trans_buf_free(ble_hci_uart_state.rx_cmd.data);
+                }
+                ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+            }
+        } else {
+            ++index;
+        }
+    } else {
+        index = 0;
+    }
+
+    ble_hci_uart_state.rx_cmd.cur = index;
+}
+
+static void
+ble_hci_uart_rx_cmd(uint8_t data)
+{
+    int rc;
+
+    ble_hci_uart_state.rx_cmd.data[ble_hci_uart_state.rx_cmd.cur++] = data;
+
+    if (ble_hci_uart_state.rx_cmd.cur < sizeof(struct ble_hci_cmd)) {
+        return;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == sizeof(struct ble_hci_cmd)) {
+        ble_hci_uart_state.rx_cmd.len = ble_hci_uart_state.rx_cmd.data[2] +
+                                        sizeof(struct ble_hci_cmd);
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == ble_hci_uart_state.rx_cmd.len) {
+        assert(ble_hci_uart_rx_cmd_cb != NULL);
+        rc = ble_hci_uart_rx_cmd_cb(ble_hci_uart_state.rx_cmd.data,
+                                    ble_hci_uart_rx_cmd_arg);
+        if (rc != 0) {
+            ble_hci_trans_buf_free(ble_hci_uart_state.rx_cmd.data);
+        }
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static void
+ble_hci_uart_rx_skip_cmd(uint8_t data)
+{
+    ble_hci_uart_state.rx_cmd.cur++;
+
+    if (ble_hci_uart_state.rx_cmd.cur < sizeof(struct ble_hci_cmd)) {
+        return;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == sizeof(struct ble_hci_cmd)) {
+        ble_hci_uart_state.rx_cmd.len = data + sizeof(struct ble_hci_cmd);
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == ble_hci_uart_state.rx_cmd.len) {
+        /*
+         * XXX: for now we simply skip the command and do nothing. This
+         * should not happen but at least we retain HCI synch. The host
+         * can decide what to do in this case. It may be appropriate for
+         * the controller to attempt to send back a command complete or
+         * command status in this case.
+         */
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+#endif
+
+#if MYNEWT_VAL(BLE_HOST)
+static inline void
+ble_hci_uart_rx_evt_cb(void)
+{
+    int rc;
+
+    if (ble_hci_uart_state.rx_cmd.cur == ble_hci_uart_state.rx_cmd.len) {
+        assert(ble_hci_uart_rx_cmd_cb != NULL);
+        rc = ble_hci_uart_rx_cmd_cb(ble_hci_uart_state.rx_cmd.data,
+                                    ble_hci_uart_rx_cmd_arg);
+        if (rc != 0) {
+            ble_hci_trans_buf_free(ble_hci_uart_state.rx_cmd.data);
+        }
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static void
+ble_hci_uart_rx_evt(uint8_t data)
+{
+    /* Determine event priority to allocate buffer */
+    if (!ble_hci_uart_state.rx_cmd.data) {
+        /* In case of LE Meta Event priority might be still unknown */
+        if (data == BLE_HCI_EVCODE_LE_META) {
+            ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_LE_EVT;
+            ble_hci_uart_state.rx_cmd.cur++;
+            return;
+        }
+
+        ble_hci_uart_state.rx_cmd.data =
+            ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        assert(ble_hci_uart_state.rx_cmd.data != NULL);
+    }
+
+    ble_hci_uart_state.rx_cmd.data[ble_hci_uart_state.rx_cmd.cur++] = data;
+
+    if (ble_hci_uart_state.rx_cmd.cur < sizeof(struct ble_hci_ev)) {
+        return;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == sizeof(struct ble_hci_ev)) {
+        ble_hci_uart_state.rx_cmd.len = ble_hci_uart_state.rx_cmd.data[1] +
+                                        sizeof(struct ble_hci_ev);
+    }
+
+    ble_hci_uart_rx_evt_cb();
+}
+
+static void
+ble_hci_uart_rx_le_evt(uint8_t data)
+{
+    ble_hci_uart_state.rx_cmd.cur++;
+
+    if (ble_hci_uart_state.rx_cmd.cur == sizeof(struct ble_hci_ev)) {
+        /* LE Meta Event parameter length is never 0 */
+        assert(data != 0);
+        ble_hci_uart_state.rx_cmd.len = data + sizeof(struct ble_hci_ev);
+        return;
+    }
+
+    /* Determine event priority to allocate buffer */
+    if (!ble_hci_uart_state.rx_cmd.data) {
+        /* Determine event priority to allocate buffer */
+        if (data == BLE_HCI_LE_SUBEV_ADV_RPT ||
+                data == BLE_HCI_LE_SUBEV_EXT_ADV_RPT) {
+            ble_hci_uart_state.rx_cmd.data =
+                ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+            if (ble_hci_uart_state.rx_cmd.data == NULL) {
+                ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_SKIP_EVT;
+                return;
+            }
+        } else {
+            ble_hci_uart_state.rx_cmd.data =
+                ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            assert(ble_hci_uart_state.rx_cmd.data != NULL);
+        }
+
+        ble_hci_uart_state.rx_cmd.data[0] = BLE_HCI_EVCODE_LE_META;
+        ble_hci_uart_state.rx_cmd.data[1] =
+                ble_hci_uart_state.rx_cmd.len - sizeof(struct ble_hci_ev);
+    }
+
+    ble_hci_uart_state.rx_cmd.data[ble_hci_uart_state.rx_cmd.cur - 1] = data;
+    ble_hci_uart_rx_evt_cb();
+}
+
+static void
+ble_hci_uart_rx_skip_evt(uint8_t data)
+{
+    if (++ble_hci_uart_state.rx_cmd.cur == ble_hci_uart_state.rx_cmd.len) {
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+#endif
+
+static void
+ble_hci_uart_rx_acl(uint8_t data)
+{
+    uint16_t rxd_bytes;
+    uint16_t pktlen;
+
+    rxd_bytes = ble_hci_uart_state.rx_acl.rxd_bytes;
+    ble_hci_uart_state.rx_acl.dptr[rxd_bytes] = data;
+    ++rxd_bytes;
+    ble_hci_uart_state.rx_acl.rxd_bytes = rxd_bytes;
+
+    if (rxd_bytes < BLE_HCI_DATA_HDR_SZ) {
+        return;
+    }
+
+    if (rxd_bytes == BLE_HCI_DATA_HDR_SZ) {
+        pktlen = ble_hci_uart_state.rx_acl.dptr[3];
+        pktlen = (pktlen << 8) + ble_hci_uart_state.rx_acl.dptr[2];
+        ble_hci_uart_state.rx_acl.len = pktlen + BLE_HCI_DATA_HDR_SZ;
+
+        /*
+         * Data portion cannot exceed data length of acl buffer. If it does
+         * this is considered to be a loss of sync.
+         */
+        if (pktlen > MYNEWT_VAL(BLE_ACL_BUF_SIZE)) {
+            os_mbuf_free_chain(ble_hci_uart_state.rx_acl.buf);
+#if MYNEWT_VAL(BLE_CONTROLLER)
+            ble_hci_uart_sync_lost();
+#else
+        /*
+         * XXX: not sure what to do about host in this case. Just go back to
+         * none for now.
+         */
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+#endif
+        }
+    }
+
+    if (rxd_bytes == ble_hci_uart_state.rx_acl.len) {
+        assert(ble_hci_uart_rx_acl_cb != NULL);
+        /* XXX: can this callback fail? What if it does? */
+        OS_MBUF_PKTLEN(ble_hci_uart_state.rx_acl.buf) = rxd_bytes;
+        ble_hci_uart_state.rx_acl.buf->om_len = rxd_bytes;
+        ble_hci_uart_rx_acl_cb(ble_hci_uart_state.rx_acl.buf,
+                               ble_hci_uart_rx_acl_arg);
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static void
+ble_hci_uart_rx_skip_acl(uint8_t data)
+{
+    uint16_t rxd_bytes;
+    uint16_t pktlen;
+
+    rxd_bytes = ble_hci_uart_state.rx_acl.rxd_bytes;
+    ++rxd_bytes;
+    ble_hci_uart_state.rx_acl.rxd_bytes = rxd_bytes;
+
+    if (rxd_bytes == (BLE_HCI_DATA_HDR_SZ - 1)) {
+        ble_hci_uart_state.rx_acl.len = data;
+        return;
+    }
+
+    if (rxd_bytes == BLE_HCI_DATA_HDR_SZ) {
+        pktlen = data;
+        pktlen = (pktlen << 8) + ble_hci_uart_state.rx_acl.len;
+        ble_hci_uart_state.rx_acl.len = pktlen + BLE_HCI_DATA_HDR_SZ;
+    }
+
+    if (rxd_bytes == ble_hci_uart_state.rx_acl.len) {
+/* XXX: I dont like this but for now this denotes controller only */
+#if MYNEWT_VAL(BLE_CONTROLLER)
+        ble_ll_data_buffer_overflow();
+#endif
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static int
+ble_hci_uart_rx_char(void *arg, uint8_t data)
+{
+    switch (ble_hci_uart_state.rx_type) {
+    case BLE_HCI_UART_H4_NONE:
+        return ble_hci_uart_rx_pkt_type(data);
+#if MYNEWT_VAL(BLE_CONTROLLER)
+    case BLE_HCI_UART_H4_CMD:
+        ble_hci_uart_rx_cmd(data);
+        return 0;
+    case BLE_HCI_UART_H4_SKIP_CMD:
+        ble_hci_uart_rx_skip_cmd(data);
+        return 0;
+    case BLE_HCI_UART_H4_SYNC_LOSS:
+        ble_hci_uart_rx_sync_loss(data);
+        return 0;
+#endif
+#if MYNEWT_VAL(BLE_HOST)
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_uart_rx_evt(data);
+        return 0;
+    case BLE_HCI_UART_H4_LE_EVT:
+        ble_hci_uart_rx_le_evt(data);
+        return 0;
+    case BLE_HCI_UART_H4_SKIP_EVT:
+        ble_hci_uart_rx_skip_evt(data);
+        return 0;
+#endif
+    case BLE_HCI_UART_H4_ACL:
+        ble_hci_uart_rx_acl(data);
+        return 0;
+    case BLE_HCI_UART_H4_SKIP_ACL:
+        ble_hci_uart_rx_skip_acl(data);
+        return 0;
+    default:
+        /* This should never happen! */
+        assert(0);
+        return 0;
+    }
+}
+
+static void
+ble_hci_uart_set_rx_cbs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                        void *cmd_arg,
+                        ble_hci_trans_rx_acl_fn *acl_cb,
+                        void *acl_arg)
+{
+    ble_hci_uart_rx_cmd_cb = cmd_cb;
+    ble_hci_uart_rx_cmd_arg = cmd_arg;
+    ble_hci_uart_rx_acl_cb = acl_cb;
+    ble_hci_uart_rx_acl_arg = acl_arg;
+}
+
+static void
+ble_hci_uart_free_pkt(uint8_t type, uint8_t *cmdevt, struct os_mbuf *acl)
+{
+    switch (type) {
+    case BLE_HCI_UART_H4_NONE:
+        break;
+
+    case BLE_HCI_UART_H4_CMD:
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_trans_buf_free(cmdevt);
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        os_mbuf_free_chain(acl);
+        break;
+
+    default:
+        assert(0);
+        break;
+    }
+}
+
+static int
+ble_hci_uart_config(void)
+{
+    int rc;
+
+    rc = hal_uart_init_cbs(MYNEWT_VAL(BLE_HCI_UART_PORT),
+                           ble_hci_uart_tx_char, NULL,
+                           ble_hci_uart_rx_char, NULL);
+    if (rc != 0) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    rc = hal_uart_config(MYNEWT_VAL(BLE_HCI_UART_PORT),
+                         MYNEWT_VAL(BLE_HCI_UART_BAUD),
+                         MYNEWT_VAL(BLE_HCI_UART_DATA_BITS),
+                         MYNEWT_VAL(BLE_HCI_UART_STOP_BITS),
+                         MYNEWT_VAL(BLE_HCI_UART_PARITY),
+                         MYNEWT_VAL(BLE_HCI_UART_FLOW_CTRL));
+    if (rc != 0) {
+        return BLE_ERR_HW_FAIL;
+    }
+
+    return 0;
+}
+
+/**
+ * Sends an HCI event from the controller to the host.
+ *
+ * @param cmd                   The HCI event to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_ll_evt_tx(uint8_t *cmd)
+{
+    int rc;
+
+    rc = ble_hci_uart_cmdevt_tx(cmd, BLE_HCI_UART_H4_EVT);
+    return rc;
+}
+
+/**
+ * Sends ACL data from controller to host.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_ll_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    rc = ble_hci_uart_acl_tx(om);
+    return rc;
+}
+
+/**
+ * Sends an HCI command from the host to the controller.
+ *
+ * @param cmd                   The HCI command to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    int rc;
+
+    rc = ble_hci_uart_cmdevt_tx(cmd, BLE_HCI_UART_H4_CMD);
+    return rc;
+}
+
+/**
+ * Sends ACL data from host to controller.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    rc = ble_hci_uart_acl_tx(om);
+    return rc;
+}
+
+/**
+ * Configures the HCI transport to call the specified callback upon receiving
+ * HCI packets from the controller.  This function should only be called by by
+ * host.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_uart_set_rx_cbs(cmd_cb, cmd_arg, acl_cb, acl_arg);
+}
+
+/**
+ * Configures the HCI transport to operate with a host.  The transport will
+ * execute specified callbacks upon receiving HCI packets from the controller.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_uart_set_rx_cbs(cmd_cb, cmd_arg, acl_cb, acl_arg);
+}
+
+/**
+ * Allocates a flat buffer of the specified type.
+ *
+ * @param type                  The type of buffer to allocate; one of the
+ *                                  BLE_HCI_TRANS_BUF_[...] constants.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_CMD:
+        buf = os_memblock_get(&ble_hci_uart_cmd_pool);
+        break;
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&ble_hci_uart_evt_hi_pool);
+        if (buf == NULL) {
+            /* If no high-priority event buffers remain, try to grab a
+             * low-priority one.
+             */
+            buf = os_memblock_get(&ble_hci_uart_evt_lo_pool);
+        }
+        break;
+
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+        buf = os_memblock_get(&ble_hci_uart_evt_lo_pool);
+        break;
+
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+/**
+ * Frees the specified flat buffer.  The buffer must have been allocated via
+ * ble_hci_trans_buf_alloc().
+ *
+ * @param buf                   The buffer to free.
+ */
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    /*
+     * XXX: this may look a bit odd, but the controller uses the command
+     * buffer to send back the command complete/status as an immediate
+     * response to the command. This was done to insure that the controller
+     * could always send back one of these events when a command was received.
+     * Thus, we check to see which pool the buffer came from so we can free
+     * it to the appropriate pool
+     */
+    if (os_memblock_from(&ble_hci_uart_evt_hi_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_uart_evt_hi_pool, buf);
+        assert(rc == 0);
+    } else if (os_memblock_from(&ble_hci_uart_evt_lo_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_uart_evt_lo_pool, buf);
+        assert(rc == 0);
+    } else {
+        assert(os_memblock_from(&ble_hci_uart_cmd_pool, buf));
+        rc = os_memblock_put(&ble_hci_uart_cmd_pool, buf);
+        assert(rc == 0);
+    }
+}
+
+/**
+ * Configures a callback to get executed whenever an ACL data packet is freed.
+ * The function is called in lieu of actually freeing the packet.
+ *
+ * @param cb                    The callback to configure.
+ *
+ * @return                      0 on success.
+ */
+int
+ble_hci_trans_set_acl_free_cb(os_mempool_put_fn *cb, void *arg)
+{
+    ble_hci_uart_acl_pool.mpe_put_cb = cb;
+    ble_hci_uart_acl_pool.mpe_put_arg = arg;
+    return 0;
+}
+
+/**
+ * Resets the HCI UART transport to a clean state.  Frees all buffers and
+ * reconfigures the UART.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_reset(void)
+{
+    struct ble_hci_uart_pkt *pkt;
+    int rc;
+
+    /* Close the UART to prevent race conditions as the buffers are freed. */
+    rc = hal_uart_close(MYNEWT_VAL(BLE_HCI_UART_PORT));
+    if (rc != 0) {
+        return BLE_ERR_HW_FAIL;
+    }
+
+    ble_hci_uart_free_pkt(ble_hci_uart_state.rx_type,
+                          ble_hci_uart_state.rx_cmd.data,
+                          ble_hci_uart_state.rx_acl.buf);
+    ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+
+    ble_hci_uart_free_pkt(ble_hci_uart_state.tx_type,
+                          ble_hci_uart_state.tx_cmd.data,
+                          ble_hci_uart_state.tx_pkt.tx_acl);
+    ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+
+    while ((pkt = STAILQ_FIRST(&ble_hci_uart_state.tx_pkts)) != NULL) {
+        STAILQ_REMOVE(&ble_hci_uart_state.tx_pkts, pkt, ble_hci_uart_pkt,
+                      next);
+        ble_hci_uart_free_pkt(pkt->type, pkt->data, pkt->data);
+        os_memblock_put(&ble_hci_uart_pkt_pool, pkt);
+    }
+
+    /* Reopen the UART. */
+    rc = ble_hci_uart_config();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Initializes the UART HCI transport module.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+void
+ble_hci_uart_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = os_mempool_ext_init(&ble_hci_uart_acl_pool,
+                             MYNEWT_VAL(BLE_ACL_BUF_COUNT),
+                             ACL_BLOCK_SIZE,
+                             ble_hci_uart_acl_buf,
+                             "ble_hci_uart_acl_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mbuf_pool_init(&ble_hci_uart_acl_mbuf_pool,
+                           &ble_hci_uart_acl_pool.mpe_mp,
+                           ACL_BLOCK_SIZE,
+                           MYNEWT_VAL(BLE_ACL_BUF_COUNT));
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    /*
+     * Create memory pool of HCI command buffers. NOTE: we currently dont
+     * allow this to be configured. The controller will only allow one
+     * outstanding command. We decided to keep this a pool in case we allow
+     * allow the controller to handle more than one outstanding command.
+     */
+    rc = os_mempool_init(&ble_hci_uart_cmd_pool,
+                         1,
+                         BLE_HCI_TRANS_CMD_SZ,
+                         ble_hci_uart_cmd_buf,
+                         "ble_hci_uart_cmd_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_uart_evt_hi_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_HI_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_uart_evt_hi_buf,
+                         "ble_hci_uart_evt_hi_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_mempool_init(&ble_hci_uart_evt_lo_pool,
+                         MYNEWT_VAL(BLE_HCI_EVT_LO_BUF_COUNT),
+                         MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE),
+                         ble_hci_uart_evt_lo_buf,
+                         "ble_hci_uart_evt_lo_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    /*
+     * Create memory pool of packet list nodes. NOTE: the number of these
+     * buffers should be, at least, the total number of event buffers (hi
+     * and lo), the number of command buffers (currently 1) and the total
+     * number of buffers that the controller could possibly hand to the host.
+     */
+    rc = os_mempool_init(&ble_hci_uart_pkt_pool,
+                         BLE_HCI_UART_EVT_COUNT + 1 +
+                         MYNEWT_VAL(BLE_HCI_ACL_OUT_COUNT),
+                         sizeof (struct ble_hci_uart_pkt),
+                         ble_hci_uart_pkt_buf,
+                         "ble_hci_uart_pkt_pool");
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_hci_uart_config();
+    SYSINIT_PANIC_ASSERT_MSG(rc == 0, "Failure configuring UART HCI");
+
+    memset(&ble_hci_uart_state, 0, sizeof ble_hci_uart_state);
+    STAILQ_INIT(&ble_hci_uart_state.tx_pkts);
+}
diff --git a/src/libs/mynewt-nimble/nimble/transport/uart/syscfg.yml b/src/libs/mynewt-nimble/nimble/transport/uart/syscfg.yml
new file mode 100644
index 00000000..43486a8b
--- /dev/null
+++ b/src/libs/mynewt-nimble/nimble/transport/uart/syscfg.yml
@@ -0,0 +1,72 @@
+# 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_HCI_EVT_BUF_SIZE:
+        description: 'The size of the allocated event buffers'
+        value: 70
+    BLE_HCI_EVT_HI_BUF_COUNT:
+        description: 'The number of high priority event buffers'
+        value: 8
+    BLE_HCI_EVT_LO_BUF_COUNT:
+        description: 'The number of low priority event buffers'
+        value: 8
+    BLE_ACL_BUF_COUNT:
+        description: 'The number of ACL data buffers'
+        value: 12
+    BLE_ACL_BUF_SIZE:
+        description: >
+            This is the maximum size of the data portion of HCI ACL data
+            packets. It does not include the HCI data header (of 4 bytes).
+        value: 255
+
+    BLE_HCI_ACL_OUT_COUNT:
+        description: >
+            This count is used in creating a pool of elements used by the
+            code to enqueue various elements. In the case of the controller
+            only HCI, this number should be equal to the number of mbufs in
+            the msys pool. For host only, it is really dependent on the
+            number of ACL buffers that the controller tells the host it
+            has.
+        value: 12
+
+    BLE_HCI_UART_PORT:
+        description: 'The uart to use for the HCI uart interface'
+        value:            0
+    BLE_HCI_UART_BAUD:
+        description: 'The baud rate of the HCI uart interface'
+        value:            1000000
+    BLE_HCI_UART_DATA_BITS:
+        description: 'Number of data bits used for HCI uart interface'
+        value:       8
+    BLE_HCI_UART_STOP_BITS:
+        description: 'Number of stop bits used for HCI uart interface'
+        value:       1
+    BLE_HCI_UART_PARITY:
+        description: 'Parity used for HCI uart interface'
+        value:          HAL_UART_PARITY_NONE
+    BLE_HCI_UART_FLOW_CTRL:
+        description: 'Flow control used for HCI uart interface'
+        value:       HAL_UART_FLOW_CTL_RTS_CTS
+    BLE_TRANS_UART_SYSINIT_STAGE:
+        description: >
+            Sysinit stage for the UART BLE transport.
+        value: 500
+
+syscfg.vals.BLE_EXT_ADV:
+    BLE_HCI_EVT_BUF_SIZE: 257