diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h
index 616ad91bc..6005f3fbe 100644
--- a/nimble/host/include/host/ble_gap.h
+++ b/nimble/host/include/host/ble_gap.h
@@ -98,8 +98,8 @@ struct hci_conn_update;
 
 #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_INITIAL_CONN_MIN_CE_LEN     0x0000
+#define BLE_GAP_INITIAL_CONN_MAX_CE_LEN     0x0000
 
 #define BLE_GAP_ROLE_MASTER                 0
 #define BLE_GAP_ROLE_SLAVE                  1
@@ -1239,6 +1239,20 @@ int ble_gap_unpair(const ble_addr_t *peer_addr);
  */
 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
 int ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode);
diff --git a/nimble/host/src/ble_att_cmd.c b/nimble/host/src/ble_att_cmd.c
index 999f57a2b..bad192df5 100644
--- a/nimble/host/src/ble_att_cmd.c
+++ b/nimble/host/src/ble_att_cmd.c
@@ -66,11 +66,10 @@ ble_att_tx(uint16_t conn_handle, struct os_mbuf *txom)
 
     ble_hs_lock();
 
-    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_ATT, &conn,
-                                    &chan);
-    if (chan == NULL) {
+    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_ATT, &conn,
+                                         &chan);
+    if (rc != 0) {
         os_mbuf_free_chain(txom);
-        rc = BLE_HS_ENOTCONN;
     } else {
         ble_att_truncate_to_mtu(chan, txom);
         rc = ble_l2cap_tx(conn, chan, txom);
diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c
index a6b228542..ebd237fee 100644
--- a/nimble/host/src/ble_gap.c
+++ b/nimble/host/src/ble_gap.c
@@ -1674,7 +1674,6 @@ ble_gap_update_timer(void)
         ble_hs_unlock();
 
         if (entry != NULL) {
-            ble_gap_update_notify(conn_handle, BLE_HS_ETIMEOUT);
             ble_gap_update_entry_free(entry);
         }
     } while (entry != NULL);
@@ -4496,6 +4495,36 @@ ble_gap_unpair_oldest_peer(void)
     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)
@@ -4517,7 +4546,8 @@ ble_gap_passkey_event(uint16_t conn_handle,
 }
 
 void
-ble_gap_enc_event(uint16_t conn_handle, int status, int security_restored)
+ble_gap_enc_event(uint16_t conn_handle, int status,
+                  int security_restored, int bonded)
 {
 #if !NIMBLE_BLE_SM
     return;
@@ -4533,12 +4563,24 @@ ble_gap_enc_event(uint16_t conn_handle, int status, int security_restored)
     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);
-        }
+    if (status != 0) {
+        return;
+    }
+
+    /* If encryption succeded and encryption has been restored for bonded device,
+     * notify gatt server so it has chance to send notification/indication if needed.
+     */
+    if (security_restored) {
+        ble_gatts_bonding_restored(conn_handle);
+        return;
+    }
+
+    /* If this is fresh pairing and bonding has been established,
+     * notify gatt server about that so previous subscriptions (before bonding)
+     * can be stored.
+     */
+    if (bonded) {
+        ble_gatts_bonding_established(conn_handle);
     }
 }
 
diff --git a/nimble/host/src/ble_gap_priv.h b/nimble/host/src/ble_gap_priv.h
index 74d219cd0..b8af959df 100644
--- a/nimble/host/src/ble_gap_priv.h
+++ b/nimble/host/src/ble_gap_priv.h
@@ -93,7 +93,7 @@ int ble_gap_rx_l2cap_update_req(uint16_t conn_handle,
                                 struct ble_gap_upd_params *params);
 void ble_gap_rx_phy_update_complete(struct hci_le_phy_upd_complete *evt);
 void ble_gap_enc_event(uint16_t conn_handle, int status,
-                       int security_restored);
+                       int security_restored, int bonded);
 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,
diff --git a/nimble/host/src/ble_hs_conn.c b/nimble/host/src/ble_hs_conn.c
index 035150b98..eb65e3288 100644
--- a/nimble/host/src/ble_hs_conn.c
+++ b/nimble/host/src/ble_hs_conn.c
@@ -410,22 +410,19 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn,
 
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
     /* RPA: Override peer address information. */
-    struct ble_hs_resolv_entry *rl = NULL;
-
     ble_addr_t bhc_peer_addr;
     bhc_peer_addr.type = conn->bhc_peer_addr.type;
     memcpy(bhc_peer_addr.val, conn->bhc_peer_addr.val, BLE_DEV_ADDR_LEN);
-    if (ble_host_rpa_enabled()) {
 
-        uint8_t *local_id = NULL;
-        ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL);
+    struct ble_hs_resolv_entry *rl = NULL;
+    rl = ble_hs_resolv_list_find(bhc_peer_addr.val);
+    if (rl != NULL) {
+        memcpy(addrs->peer_id_addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN);
+        addrs->peer_id_addr.type = rl->rl_addr_type;
 
-        rl = ble_hs_resolv_list_find(bhc_peer_addr.val);
-        if (rl != NULL) {
-            memcpy(addrs->peer_ota_addr.val, addrs->peer_id_addr.val, BLE_DEV_ADDR_LEN);
-            memcpy(addrs->peer_id_addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN);
-
-            addrs->peer_id_addr.type = rl->rl_addr_type;
+        if (ble_host_rpa_enabled()) {
+            uint8_t *local_id = NULL;
+            ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL);
 
             /* RL is present: populate our id addr with public ID */
             memcpy(addrs->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN);
diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c
index 90ac71321..df3d0094b 100644
--- a/nimble/host/src/ble_hs_hci.c
+++ b/nimble/host/src/ble_hs_hci.c
@@ -239,7 +239,7 @@ ble_hs_hci_process_ack(uint16_t expected_opcode,
     }
 
     if (rc == 0) {
-        if (params_buf == NULL) {
+        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) {
diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c
index cb9ed82e7..5152c26f7 100644
--- a/nimble/host/src/ble_hs_hci_evt.c
+++ b/nimble/host/src/ble_hs_hci_evt.c
@@ -342,16 +342,11 @@ ble_hs_hci_evt_le_conn_complete(uint8_t subevent, uint8_t *data, int len)
             if (ble_host_rpa_enabled()) {
                 uint8_t *local_id_rpa = ble_hs_get_rpa_local();
                 memcpy(evt.local_rpa, local_id_rpa, 6);
-
-                struct ble_hs_resolv_entry *rl = NULL;
-                ble_rpa_replace_peer_params_with_rl(evt.peer_addr,
-                                                    &evt.peer_addr_type, &rl);
-                if (rl == NULL) {
-                    if (ble_rpa_resolv_add_peer_rec(evt.peer_addr) != 0) {
-                        BLE_HS_LOG(DEBUG, "Memory unavailable for new peer record\n");
-                    }
-                }
             }
+
+            struct ble_hs_resolv_entry *rl = NULL;
+            ble_rpa_replace_peer_params_with_rl(evt.peer_addr,
+                                                &evt.peer_addr_type, &rl);
 #endif
         } else {
             memset(evt.local_rpa, 0, BLE_DEV_ADDR_LEN);
@@ -448,14 +443,6 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, uint8_t *data, int len)
         memcpy(desc.addr.val, data + off, 6);
         off += 6;
 
-#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
-        if (ble_host_rpa_enabled()) {
-            /* Now RPA to be resolved here, since controller is unaware of the
-             * address is RPA  */
-            ble_rpa_replace_peer_params_with_rl(desc.addr.val,
-                                                &desc.addr.type, NULL);
-        }
-#endif
         desc.length_data = data[off];
         ++off;
 
diff --git a/nimble/host/src/ble_hs_misc.c b/nimble/host/src/ble_hs_misc.c
index e6bb3825b..00200d316 100644
--- a/nimble/host/src/ble_hs_misc.c
+++ b/nimble/host/src/ble_hs_misc.c
@@ -56,7 +56,7 @@ ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
     return rc;
 }
 
-void
+int
 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)
@@ -66,7 +66,9 @@ ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
     int rc;
 
     rc = ble_hs_misc_conn_chan_find(conn_handle, cid, &conn, &chan);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    if (rc != 0) {
+        return rc;
+    }
 
     if (out_conn != NULL) {
         *out_conn = conn;
@@ -74,6 +76,8 @@ ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
     if (out_chan != NULL) {
         *out_chan = chan;
     }
+
+    return 0;
 }
 
 uint8_t
diff --git a/nimble/host/src/ble_hs_priv.h b/nimble/host/src/ble_hs_priv.h
index f7d3cd43a..640a7e6c9 100644
--- a/nimble/host/src/ble_hs_priv.h
+++ b/nimble/host/src/ble_hs_priv.h
@@ -114,9 +114,9 @@ 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);
+int 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_addr_type_to_id(uint8_t addr_type);
 int ble_hs_misc_restore_irks(void);
 
diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c
index 07a338ae6..bc73d954a 100644
--- a/nimble/host/src/ble_l2cap_sig.c
+++ b/nimble/host/src/ble_l2cap_sig.c
@@ -473,8 +473,13 @@ ble_l2cap_sig_update(uint16_t conn_handle,
     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);
+    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                         &conn, &chan);
+    if (rc != 0) {
+        ble_hs_unlock();
+        goto done;
+    }
+
     master = conn->bhc_flags & BLE_HS_CONN_F_MASTER;
     ble_hs_unlock();
 
diff --git a/nimble/host/src/ble_l2cap_sig_cmd.c b/nimble/host/src/ble_l2cap_sig_cmd.c
index 366dde625..510420f09 100644
--- a/nimble/host/src/ble_l2cap_sig_cmd.c
+++ b/nimble/host/src/ble_l2cap_sig_cmd.c
@@ -28,9 +28,11 @@ ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom)
     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);
+    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                         &conn, &chan);
+    if (rc == 0) {
+        rc = ble_l2cap_tx(conn, chan, txom);
+    }
     ble_hs_unlock();
 
     return rc;
diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c
index ea55e4a75..b70198953 100644
--- a/nimble/host/src/ble_sm.c
+++ b/nimble/host/src/ble_sm.c
@@ -538,6 +538,11 @@ ble_sm_persist_keys(struct ble_sm_proc *proc)
             case BLE_ADDR_PUBLIC:
             case BLE_ADDR_PUBLIC_ID:
                 conn->bhc_peer_addr.type = BLE_ADDR_PUBLIC_ID;
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+                /* In case of Host based privacy, we should not be changing
+                 * peer address type to BLE_ADDR_PUBLIC_ID */
+                conn->bhc_peer_addr.type = BLE_ADDR_PUBLIC;
+#endif
                 break;
 
             case BLE_ADDR_RANDOM:
@@ -548,26 +553,23 @@ ble_sm_persist_keys(struct ble_sm_proc *proc)
 
             identity_ev = 1;
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
-            if (ble_host_rpa_enabled())
-            {
-                struct ble_hs_dev_records *p_dev_rec =
-                                          ble_rpa_find_peer_dev_rec(conn->bhc_peer_rpa_addr.val);
-                if (p_dev_rec == NULL) {
-                    if (!ble_rpa_resolv_add_peer_rec(conn->bhc_peer_rpa_addr.val)) {
-                        p_dev_rec = ble_rpa_find_peer_dev_rec(conn->bhc_peer_rpa_addr.val);
-                    }
+            struct ble_hs_dev_records *p_dev_rec =
+                                      ble_rpa_find_peer_dev_rec(conn->bhc_peer_rpa_addr.val);
+            if (p_dev_rec == NULL) {
+                if (!ble_rpa_resolv_add_peer_rec(conn->bhc_peer_rpa_addr.val)) {
+                    p_dev_rec = ble_rpa_find_peer_dev_rec(conn->bhc_peer_rpa_addr.val);
                 }
+            }
 
-                if (p_dev_rec != NULL) {
-                    /* Once bonded, copy the peer device records */
-                    swap_buf(p_dev_rec->peer_sec.irk, proc->peer_keys.irk, 16);
-                    p_dev_rec->peer_sec.irk_present = proc->peer_keys.irk_valid;
-                    memcpy(p_dev_rec->peer_sec.peer_addr.val,
-                           proc->peer_keys.addr, 6);
-                    p_dev_rec->peer_sec.peer_addr.type = proc->peer_keys.addr_type;
+            if (p_dev_rec != NULL) {
+                /* Once bonded, copy the peer device records */
+                swap_buf(p_dev_rec->peer_sec.irk, proc->peer_keys.irk, 16);
+                p_dev_rec->peer_sec.irk_present = proc->peer_keys.irk_valid;
+                memcpy(p_dev_rec->peer_sec.peer_addr.val,
+                       proc->peer_keys.addr, 6);
+                p_dev_rec->peer_sec.peer_addr.type = proc->peer_keys.addr_type;
 
-                    ble_store_persist_peer_records();
-                }
+                ble_store_persist_peer_records();
             }
 #endif
         }
@@ -947,7 +949,7 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res)
 
         if (res->enc_cb) {
             BLE_HS_DBG_ASSERT(proc == NULL || rm);
-            ble_gap_enc_event(conn_handle, res->app_status, res->restore);
+            ble_gap_enc_event(conn_handle, res->app_status, res->restore, res->bonded);
         }
 
         if (res->app_status == 0 &&
@@ -1201,6 +1203,7 @@ ble_sm_enc_event_rx(uint16_t conn_handle, uint8_t evt_status, int encrypted)
 
     ble_hs_unlock();
 
+    res.bonded = bonded;
     ble_sm_process_result(conn_handle, &res);
 }
 
@@ -1781,8 +1784,21 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
      */
     proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, &prev);
     if (proc != NULL) {
-        /* Pairing already in progress; abort old procedure and start new. */
-        /* XXX: Check the spec on this. */
+        /* 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);
     }
@@ -2016,7 +2032,10 @@ 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, 0, 1, proc->key_size);
+    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;
@@ -2441,7 +2460,7 @@ ble_sm_timer(void)
      * procedures without reconnect.
      */
     while ((proc = STAILQ_FIRST(&exp_list)) != NULL) {
-        ble_gap_enc_event(proc->conn_handle, BLE_HS_ETIMEOUT, 0);
+        ble_gap_enc_event(proc->conn_handle, BLE_HS_ETIMEOUT, 0, 0);
 
         STAILQ_REMOVE_HEAD(&exp_list, next);
         ble_sm_proc_free(proc);
diff --git a/nimble/host/src/ble_sm_cmd.c b/nimble/host/src/ble_sm_cmd.c
index b5e674e75..e12e109d5 100644
--- a/nimble/host/src/ble_sm_cmd.c
+++ b/nimble/host/src/ble_sm_cmd.c
@@ -52,14 +52,19 @@ ble_sm_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(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);
+    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SM,
+                                         &conn, &chan);
+    if (rc == 0) {
+        rc = ble_l2cap_tx(conn, chan, txom);
+    }
+
+    return rc;
 }
 
 #if NIMBLE_BLE_SM
diff --git a/nimble/host/src/ble_sm_priv.h b/nimble/host/src/ble_sm_priv.h
index 74205bd0c..bbb4b03ae 100644
--- a/nimble/host/src/ble_sm_priv.h
+++ b/nimble/host/src/ble_sm_priv.h
@@ -277,10 +277,10 @@ struct ble_sm_result {
     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;
+    unsigned execute : 1;
+    unsigned enc_cb : 1;
+    unsigned bonded : 1;
+    unsigned restore : 1;
 };
 
 #if MYNEWT_VAL(BLE_HS_DEBUG)
diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c
index 05c22176c..73c71d93b 100644
--- a/nimble/host/src/ble_store_util.c
+++ b/nimble/host/src/ble_store_util.c
@@ -253,13 +253,15 @@ 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:
-            case BLE_STORE_OBJ_TYPE_CCCD:
-                return ble_gap_unpair_oldest_peer();
+        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;
+        default:
+            return BLE_HS_EUNKNOWN;
         }
 
     case BLE_STORE_EVENT_FULL:
diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c
index 8cbb231b6..6a72e667a 100644
--- a/nimble/host/store/config/src/ble_store_nvs.c
+++ b/nimble/host/store/config/src/ble_store_nvs.c
@@ -208,11 +208,11 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value)
                     err = get_nvs_matching_index(&p_dev_rec, value, num_value,
                                                  sizeof(struct ble_hs_dev_records));
                 } else {
-                    if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) {
+                    if (obj_type != BLE_STORE_OBJ_TYPE_CCCD) {
                         err = get_nvs_matching_index(&cur.sec, value, num_value,
                                                      sizeof(struct ble_store_value_sec));
                     } else {
-                        err = get_nvs_matching_index(&cur.sec, value, num_value,
+                        err = get_nvs_matching_index(&cur.cccd, value, num_value,
                                                      sizeof(struct ble_store_value_cccd));
                     }
                 }
diff --git a/nimble/host/test/src/ble_gap_test.c b/nimble/host/test/src/ble_gap_test.c
index 4902821ad..2de4351b4 100644
--- a/nimble/host/test/src/ble_gap_test.c
+++ b/nimble/host/test/src/ble_gap_test.c
@@ -2033,70 +2033,6 @@ ble_gap_test_util_update_l2cap(struct ble_gap_upd_params *params,
                        peer_addr, 6) == 0);
 }
 
-static void
-ble_gap_test_util_update_no_l2cap_tmo(struct ble_gap_upd_params *params,
-                                      int master)
-{
-    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, 0);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(!ble_gap_master_in_progress());
-
-    /* Verify tx of connection update command. */
-    ble_gap_test_util_verify_tx_update_conn(params);
-
-    /* Ensure no update event reported. */
-    TEST_ASSERT(ble_gap_test_event.type == 0xff);
-
-    /* Advance 39 seconds; ensure no timeout reported. */
-    os_time_advance(39 * OS_TICKS_PER_SEC);
-    ble_gap_timer();
-    TEST_ASSERT(ble_gap_test_event.type == 0xff);
-
-    /* Advance 40th second; ensure timeout reported. */
-    os_time_advance(1 * OS_TICKS_PER_SEC);
-
-    /* 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();
-
-    /* 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);
-}
-
 static void
 ble_gap_test_util_update_l2cap_tmo(struct ble_gap_upd_params *params,
                                    uint8_t hci_status, uint8_t event_status,
@@ -2967,9 +2903,6 @@ TEST_CASE_SELF(ble_gap_test_case_update_timeout)
         .max_ce_len = 456,
     };
 
-    /* No L2CAP. */
-    ble_gap_test_util_update_no_l2cap_tmo(&params, 1);
-
     /* L2CAP - Local unsupported; L2CAP timeout. */
     ble_gap_test_util_update_l2cap_tmo(&params, BLE_ERR_UNKNOWN_HCI_CMD, 0, 0);
 
diff --git a/porting/nimble/include/modlog/modlog.h b/porting/nimble/include/modlog/modlog.h
index 55b6b3247..fc063b773 100644
--- a/porting/nimble/include/modlog/modlog.h
+++ b/porting/nimble/include/modlog/modlog.h
@@ -41,20 +41,24 @@ modlog_dummy(const char *msg, ...)
 #endif
 
 #ifdef ESP_PLATFORM
+#define MODLOG_ESP_LOCAL(level, ml_msg_, ...) do { \
+    if (LOG_LOCAL_LEVEL >= level) esp_log_write(level, "NimBLE", ml_msg_, ##__VA_ARGS__); \
+} while(0)
+
 #define MODLOG_DEBUG(ml_mod_, ml_msg_, ...) \
-    esp_log_write(ESP_LOG_DEBUG, "NimBLE",ml_msg_, ##__VA_ARGS__)
+    MODLOG_ESP_LOCAL(ESP_LOG_DEBUG, ml_msg_, ##__VA_ARGS__)
 
 #define MODLOG_INFO(ml_mod_, ml_msg_, ...) \
-    esp_log_write(ESP_LOG_INFO, "NimBLE",ml_msg_, ##__VA_ARGS__)
+    MODLOG_ESP_LOCAL(ESP_LOG_INFO, ml_msg_, ##__VA_ARGS__)
 
 #define MODLOG_WARN(ml_mod_, ml_msg_, ...) \
-    esp_log_write(ESP_LOG_WARN, "NimBLE",ml_msg_, ##__VA_ARGS__)
+    MODLOG_ESP_LOCAL(ESP_LOG_WARN, ml_msg_, ##__VA_ARGS__)
 
 #define MODLOG_ERROR(ml_mod_, ml_msg_, ...) \
-    esp_log_write(ESP_LOG_ERROR, "NimBLE",ml_msg_, ##__VA_ARGS__)
+    MODLOG_ESP_LOCAL(ESP_LOG_ERROR, ml_msg_, ##__VA_ARGS__)
 
 #define MODLOG_CRITICAL(ml_mod_, ml_msg_, ...) \
-    esp_log_write(ESP_LOG_ERROR, "NimBLE",ml_msg_, ##__VA_ARGS__)
+    MODLOG_ESP_LOCAL(ESP_LOG_ERROR, ml_msg_, ##__VA_ARGS__)
 
 #else