fix(nimble): Fixes for AI reported issues

apps/btshell/src/cmd.c

@@ -1845,7 +1845,11 @@ cmd_set_adv_data_or_scan_rsp(int argc, char **argv, bool scan_rsp,
             rc = ble_gap_ext_adv_rsp_set_data(instance, adv_data);
 #if MYNEWT_VAL(BLE_PERIODIC_ADV)
         } else if (periodic) {
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_ENH)
+            rc = ble_gap_periodic_adv_set_data(instance, adv_data, NULL);
+#else
             rc = ble_gap_periodic_adv_set_data(instance, adv_data);
+#endif
 #endif
         } else {
             rc = ble_gap_ext_adv_set_data(instance, adv_data);

nimble/controller/src/ble_ll_sync.c

@@ -1898,7 +1898,7 @@ ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len)
         return BLE_ERR_UNK_ADV_INDENT;
     }
 
-    if (cmd->enable) {
+    if (cmd->enable & 0x01) {
         sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DISABLED;
     } else {
         sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;

nimble/host/include/host/ble_ead.h

@@ -88,9 +88,10 @@ struct key_material {
  *
  * @return              0 on success;
  *                      BLE_HS_EINVAL if the specified value is not
- *                      within the allowed range.
- *                      BLE_HS_ECANCEL if error occurred during the random number
- *                      generation
+ *                      within the allowed range;
+ *                      other nonzero BLE host error codes on internal
+ *                      failure (e.g., HCI error during randomizer
+ *                      generation).
  */
 int ble_ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE],
                     const uint8_t iv[BLE_EAD_IV_SIZE], const uint8_t *payload,

nimble/host/include/host/ble_esp_gap.h

@@ -347,8 +347,10 @@ ble_gap_dev_authorization(uint16_t conn_handle, bool authorized);
  * @return                      0 on success;
  *                              A nonzero value indicating an error if the command fails.
  */
+#if MYNEWT_VAL(BLE_HCI_VS)
 int
 ble_hs_send_vs_event_mask(uint32_t event_mask);
+#endif
 
 void ble_gap_rx_test_evt(const void *buf, uint8_t len);
 void ble_gap_tx_test_evt(const void *buf, uint8_t len);

nimble/host/include/host/ble_gap.h

@@ -2749,14 +2749,6 @@ struct ble_gap_set_periodic_adv_subev_data_params {
 };
 #endif
 
-#if MYNEWT_VAL(BLE_ADV_SEND_CONSTANT_DID)
-struct ble_gap_adv_const_did_cmd_params {
-    uint16_t handle;
-    uint8_t enable;
-    uint16_t did;
-};
-#endif // MYNEWT_VAL(BLE_ADV_SEND_CONSTANT_DID)
-
 /**
  * Configure periodic advertising for specified advertising instance
  *
@@ -3553,37 +3545,6 @@ int ble_gap_read_sugg_def_data_len(uint16_t *out_sugg_max_tx_octets,
 int ble_gap_write_sugg_def_data_len(uint16_t sugg_max_tx_octets,
                                     uint16_t sugg_max_tx_time);
 
-/**
- * Read LE Suggested Default Data Length in controller (OGF = 0x08, OCF = 0x0024).
- *
- * @param out_sugg_max_tx_octets    The Host's suggested value for the Controller's maximum transmitted
- *                                  number of payload octets in LL Data PDUs to be used for new
- *                                  connections. (Range 0x001B-0x00FB).
- * @param out_sugg_max_tx_time      The Host's suggested value for the Controller's maximum packet
- *                                  transmission time for packets containing LL Data PDUs to be used
- *                                  for new connections. (Range 0x0148-0x4290).
- *
- * @return                          0 on success,
- *                                  other error code on failure.
- */
-int ble_gap_read_sugg_def_data_len(uint16_t *out_sugg_max_tx_octets,
-                                   uint16_t *out_sugg_max_tx_time);
-
-/**
- * Configure LE Suggested Default Data Length in controller (OGF = 0x08, OCF = 0x0024).
- *
- * @param sugg_max_tx_octets    The Host's suggested value for the Controller's maximum transmitted
- *                              number of payload octets in LL Data PDUs to be used for new
- *                              connections. (Range 0x001B-0x00FB).
- * @param sugg_max_tx_time      The Host's suggested value for the Controller's maximum packet
- *                              transmission time for packets containing LL Data PDUs to be used
- *                              for new connections. (Range 0x0148-0x4290).
- *
- * @return                      0 on success,
- *                              other error code on failure.
- */
-int ble_gap_write_sugg_def_data_len(uint16_t sugg_max_tx_octets, uint16_t sugg_max_tx_time);
-
 /**
  * Initiates the GAP security procedure.
  *
@@ -4224,7 +4185,6 @@ int ble_gap_rd_local_resolv_addr(uint8_t peer_addr_type, const ble_addr_t *peer_
  */
 int ble_gap_read_local_irk(uint8_t * out_irk);
 
-#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
 /**
  * Set or clear a bit controlled by the host in the link layer featureSet
  * stored in the Controller
@@ -4236,7 +4196,9 @@ int ble_gap_read_local_irk(uint8_t * out_irk);
  * return                0 on success; nonzero on failure
  */
 int ble_gap_set_host_feat(uint8_t bit_num,uint8_t bit_val);
-#endif
+
+int ble_gap_rd_all_local_supp_features(uint8_t *status, uint8_t *max_page, uint8_t *le_features);
+int ble_gap_rd_all_remote_features(uint16_t conn_handle, uint8_t page_requested);
 
 #if MYNEWT_VAL(BLE_FRAME_SPACE_UPDATE)
 /* * Request a Frame Space Update.

nimble/host/include/host/ble_hs_hci.h

@@ -102,6 +102,17 @@ int ble_hs_hci_set_chan_class(const uint8_t *chan_map);
  */
 int ble_hs_hci_util_rand(void *dst, int len);
 
+/**
+ * Returns the number of available ACL transmit buffers on the controller.
+ *
+ * Applications can use this to implement notification throttling — only
+ * enqueue new notifications when available buffers are above a waterline.
+ *
+ * @return  The number of ACL packet buffers the controller can currently
+ *          accept from the host.
+ */
+uint16_t ble_hs_hci_get_avail_pkts(void);
+
 #if MYNEWT_VAL(BLE_HCI_VS)
 /**
  * Send an arbitrary HCI command to the controller.

nimble/host/include/host/ble_hs_pvcy.h

@@ -68,7 +68,9 @@ int ble_hs_pvcy_rpa_config(uint8_t enable);
 void ble_hs_resolv_deinit(void);
 #endif
 
+#if (!MYNEWT_VAL(BLE_HOST_BASED_PRIVACY))
 int ble_hs_pvcy_set_resolve_enabled(int enable);
+#endif
 int ble_hs_set_rpa_timeout(uint16_t timeout);
 
 #ifdef __cplusplus

nimble/host/services/bas/include/services/bas/ble_svc_bas.h

@@ -31,6 +31,7 @@ extern "C" {
 #define BLE_SVC_BAS_CHR_UUID16_BATTERY_LEVEL                 0x2A19
 
 int ble_svc_bas_battery_level_set(uint8_t level);
+uint8_t ble_svc_bas_battery_level_get(void);
 
 void ble_svc_bas_init(void);
 void ble_svc_bas_deinit(void);

nimble/host/services/bas/src/ble_svc_bas.c

@@ -134,6 +134,10 @@ ble_svc_bas_battery_level_set(uint8_t level)
 void
 ble_svc_bas_deinit(void)
 {
+    ble_svc_bas_battery_level = 0;
+#if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0
+    ble_svc_bas_battery_handle = 0;
+#endif
     ble_gatts_free_svcs();
 }
 

nimble/host/services/bleuart/src/bleuart.c

@@ -147,13 +147,21 @@ bleuart_uart_read(void)
 
     off = 0;
     while (1) {
-        rc = console_read(console_buf + off,
-                          MYNEWT_VAL(BLEUART_MAX_INPUT) - off, &full_line);
-        if (rc <= 0 && !full_line) {
-            vTaskDelay(pdMS_TO_TICKS(10));
-            continue;
+        if (off >= MYNEWT_VAL(BLEUART_MAX_INPUT)) {
+            full_line = 1;
+        } else {
+            rc = console_read(console_buf + off,
+                              MYNEWT_VAL(BLEUART_MAX_INPUT) - off, &full_line);
+            if (rc < 0) {
+                off = 0;
+                break;
+            }
+            if (rc <= 0 && !full_line) {
+                vTaskDelay(pdMS_TO_TICKS(10));
+                continue;
+            }
+            off += rc;
         }
-        off += rc;
         if (!full_line) {
             continue;
         }
@@ -192,10 +200,10 @@ bleuart_init(void)
     /* Ensure this function only gets called by sysinit. */
     SYSINIT_ASSERT_ACTIVE();
 
-    rc = console_init(bleuart_uart_read);
-    SYSINIT_PANIC_ASSERT(rc == 0);
-
     console_buf = nimble_platform_mem_calloc(1, MYNEWT_VAL(BLEUART_MAX_INPUT));
     SYSINIT_PANIC_ASSERT(console_buf != NULL);
+
+    rc = console_init(bleuart_uart_read);
+    SYSINIT_PANIC_ASSERT(rc == 0);
 }
 #endif

nimble/host/services/cte/src/ble_svc_cte.c

@@ -190,6 +190,10 @@ static const struct ble_gatt_svc_def ble_svc_cte_defs[] = {
  * @return A pointer to the matching CTE configuration instance, or NULL if not found.
  */
 static cte_instance_config_t* cte_find_config_by_conn_handle(uint16_t conn_handle) {
+    if (conn_handle == 0xffff) {
+        return NULL;
+    }
+
     for (int i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) {
         if (cte_config[i].conn_handle == conn_handle) {
             return &cte_config[i];
@@ -303,6 +307,7 @@ ble_svc_cte_two_octet_chr_write(struct os_mbuf *om,
     if (rc != 0) {
         return BLE_ATT_ERR_UNLIKELY;
     }
+    value = le16toh(value);
 
     // Check if the value is within the allowed range
     if (value < min_value || value > max_value) {
@@ -366,18 +371,19 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle,
                         if(ble_gap_set_conn_cte_transmit_param(conn_handle, BLE_GAP_CTE_RSP_ALLOW_AOA_MASK, 0, NULL) != 0)
                         {
                             config->cte_enable = old_enable;
-                            rc = 0xFC;
+                            rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED;
                             break;
                         }
                         if(ble_gap_conn_cte_rsp_enable(conn_handle, true) != 0) {
                             config->cte_enable = old_enable;
-                            rc = 0xFC;
+                            rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED;
                             break;
                         }
-                    } else if ((old_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) {
+                    } else if ((old_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION &&
+                               (config->cte_enable & CTE_ENABLE_AOA_CONNECTION) == 0) {
                         if(ble_gap_conn_cte_rsp_enable(conn_handle, false) != 0) {
                             config->cte_enable = old_enable;
-                            rc = 0xFC;
+                            rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED;
                             break;
                         }
                     }
@@ -509,11 +515,13 @@ static int ble_svc_cte_adv_cte_interval_access(uint16_t conn_handle, uint16_t at
     }
     
     switch (ctxt->op) {
-        case BLE_GATT_ACCESS_OP_READ_CHR:
+        case BLE_GATT_ACCESS_OP_READ_CHR: {
             // Handle read characteristic request
-            rc = os_mbuf_append(ctxt->om, &config->cte_interval, sizeof(config->cte_interval)) == 0 ?
+            uint16_t interval_le = htole16(config->cte_interval);
+            rc = os_mbuf_append(ctxt->om, &interval_le, sizeof(interval_le)) == 0 ?
                     0 : BLE_ATT_ERR_INSUFFICIENT_RES;
             break;
+        }
 
         case BLE_GATT_ACCESS_OP_WRITE_CHR:
             // Handle write characteristic request

nimble/host/services/cts/src/ble_svc_cts.c

@@ -114,7 +114,8 @@ ble_svc_cts_chr_write(struct os_mbuf *om, uint16_t min_len,
 
 int ble_svc_cts_curr_time_validate(struct ble_svc_cts_curr_time curr_time) {
     if(curr_time.et_256.d_d_t.day_of_week > 7  ||
-       (curr_time.et_256.d_d_t.d_t.year < 1582)||
+       (curr_time.et_256.d_d_t.d_t.year != 0 &&
+        curr_time.et_256.d_d_t.d_t.year < 1582)||
        curr_time.et_256.d_d_t.d_t.year > 9999  ||
        curr_time.et_256.d_d_t.d_t.month > 12   ||
        curr_time.et_256.d_d_t.d_t.day > 31     ||

nimble/host/services/dis/src/ble_svc_dis.c

@@ -176,7 +176,8 @@ const struct ble_gatt_svc_def ble_svc_dis_include_def[] = {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid = &ble_svc_dis_include_uuid.u,
         .includes = included_services,
-    }
+    },
+    { 0 }
 };
 
 /**
@@ -484,6 +485,17 @@ ble_svc_dis_deinit(void)
         nimble_platform_mem_free(ble_svc_dis_data_ptr);
         ble_svc_dis_data_ptr = NULL;
     }
+#else
+    ble_svc_dis_data.model_number     = MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_DEFAULT);
+    ble_svc_dis_data.serial_number     = MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_DEFAULT);
+    ble_svc_dis_data.firmware_revision = MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_DEFAULT);
+    ble_svc_dis_data.hardware_revision = MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_DEFAULT);
+    ble_svc_dis_data.software_revision = MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_DEFAULT);
+    ble_svc_dis_data.manufacturer_name = MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_DEFAULT);
+    ble_svc_dis_data.system_id         = MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_DEFAULT);
+    ble_svc_dis_data.pnp_id            = MYNEWT_VAL(BLE_SVC_DIS_PNP_ID_DEFAULT);
+    ble_svc_dis_data.ieee              = "dummy_data";
+    ble_svc_dis_data.udi               = NULL;
 #endif
 }
 

nimble/host/services/gap/src/ble_svc_gap.c

@@ -238,8 +238,8 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt)
 
     ble_hs_gap_svc_ctx->svc_gap_name = nimble_platform_mem_calloc(1, om_len + 1);
     if (!ble_hs_gap_svc_ctx->svc_gap_name) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
-        return BLE_HS_ENOMEM;
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES);
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
     }
 #endif
 
@@ -251,8 +251,11 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt)
 
     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);
+    {
+        ble_svc_gap_chr_changed_fn *cb = ble_svc_gap_chr_changed_cb_fn;
+        if (cb) {
+            cb(BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME);
+        }
     }
 
     return rc;
@@ -292,8 +295,11 @@ ble_svc_gap_appearance_write_access(struct ble_gatt_access_ctxt *ctxt)
 
     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);
+    {
+        ble_svc_gap_chr_changed_fn *cb = ble_svc_gap_chr_changed_cb_fn;
+        if (cb) {
+            cb(BLE_SVC_GAP_CHR_UUID16_APPEARANCE);
+        }
     }
 
     return rc;
@@ -518,6 +524,14 @@ ble_svc_gap_device_key_material_set(uint8_t *session_key, uint8_t *iv)
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    int rc;
+
+    rc = ble_svc_gap_appearance_init();
+    if (rc != 0) {
+        return rc;
+    }
+#endif
     memcpy(&ble_svc_gap_km.session_key, session_key, BLE_EAD_KEY_SIZE);
     memcpy(&ble_svc_gap_km.iv, iv, BLE_EAD_IV_SIZE);
     ble_gatts_chr_updated(ble_svc_gap_enc_adv_data_handle);
@@ -588,7 +602,7 @@ ble_svc_gap_deinit_name(void)
 void
 ble_svc_gap_init(void)
 {
-#if NIMBLE_BLE_CONNECT
+#if NIMBLE_BLE_CONNECT || MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     int rc;
 #endif
     /* Ensure this function only gets called by sysinit. */

nimble/host/services/gatt/src/ble_svc_gatt.c

@@ -144,7 +144,16 @@ ble_svc_gatt_cl_sup_feat_access(uint16_t conn_handle, uint16_t attr_handle,
         return 0;
     }
     if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-        return ble_gatts_peer_cl_sup_feat_update(conn_handle, ctxt->om);
+        rc = ble_gatts_peer_cl_sup_feat_update(conn_handle, ctxt->om);
+        if (rc == 0) {
+            return 0;
+        }
+        if (rc == BLE_ATT_ERR_INSUFFICIENT_RES ||
+            rc == BLE_ATT_ERR_UNLIKELY ||
+            rc == BLE_ATT_ERR_VALUE_NOT_ALLOWED) {
+            return rc;
+        }
+        return BLE_ATT_ERR_UNLIKELY;
     }
 
     return 0;
@@ -270,6 +279,8 @@ ble_svc_gatt_init(void)
 void
 ble_svc_gatt_deinit(void)
 {
+    ble_svc_gatt_local_srv_sup_feat = 0;
+    ble_svc_gatt_local_cl_sup_feat = 0;
     ble_gatts_free_svcs();
 }
 #endif

nimble/host/services/hid/src/ble_svc_hid.c

@@ -300,6 +300,10 @@ fill_boot_mouse_inp(uint8_t instance)
         return;
     }
 
+    if (hid_instances[instance].mouse_inp_rpt_len == 0) {
+        hid_instances[instance].mouse_inp_rpt_len = 3;
+    }
+
     write_flags = BLE_GATT_CHR_F_WRITE |
 #if MYNEWT_VAL(BLE_SM_LVL) == 2
                   BLE_GATT_CHR_F_WRITE_ENC |
@@ -538,7 +542,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
     uint16_t out_rpt_len = 0;
     uint8_t instances = get_curr_svc_idx();
     uint16_t handle;
-    uint8_t val;
+    uint8_t val = 0xFF;
 
     for (int instance = 0; instance < instances; instance++) {
         switch (uuid16) {
@@ -597,7 +601,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
                         s_char_write_cb) {
                     s_char_write_cb(attr_handle, BLE_SVC_HID_CHR_UUID16_HID_CTRL_PT, hid_instances[instance].ctrl_pt);
                 }
-                return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+                return rc;
             }
             return BLE_ATT_ERR_UNLIKELY;
 
@@ -619,7 +623,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
                                       &hid_instances[instance].kbd_out_rpt,
                                       out_rpt_len);
                 }
-                return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+                return rc;
             }
             return 0;
         case BLE_SVC_HID_CHR_UUID16_BOOT_KBD_INP:
@@ -636,7 +640,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
                 if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) {
                     ble_gatts_chr_updated(*(ctxt->chr->val_handle));
                 }
-                return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+                return rc;
             }
             return 0;
         case BLE_SVC_HID_CHR_UUID16_BOOT_MOUSE_INP:
@@ -681,7 +685,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
                             s_char_write_cb) {
                         s_char_write_cb(attr_handle, BLE_SVC_HID_CHR_UUID16_PROTOCOL_MODE, hid_instances[instance].proto_mode);
                     }
-                    return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+                    return rc;
                 }
                 return BLE_ATT_ERR_UNLIKELY;
             }
@@ -709,7 +713,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
                 if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) {
                     ble_gatts_chr_updated(*(ctxt->chr->val_handle));
                 }
-                return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+                return rc;
             }
             return 0;
 
@@ -861,6 +865,7 @@ ble_svc_hid_reset(void)
 void
 ble_svc_hid_deinit(void)
 {
+    ble_svc_hid_reset();
     ble_gatts_free_svcs();
 }
 

nimble/host/services/hr/include/services/hr/ble_svc_hr.h

@@ -30,6 +30,8 @@ void ble_svc_hr_on_gap_disconnect(uint16_t conn_handle);
 
 void ble_svc_hr_init(void);
 
+int ble_svc_hr_notify_measurement(void);
+
 #ifdef __cplusplus
 }
 #endif

nimble/host/services/hr/src/ble_svc_hr.c

@@ -31,7 +31,7 @@ static int
 ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle,
                   struct ble_gatt_access_ctxt *ctxt,
                   void *arg);
-static int ble_svc_hr_notify_measurement(void);
+int ble_svc_hr_notify_measurement(void);
 static int
 ble_svc_hr_chr_write(struct os_mbuf *om, uint16_t min_len,
                      uint16_t max_len, void *dst,
@@ -107,7 +107,8 @@ ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle,
 
     switch (uuid16) {
     case BLE_SVC_HR_CHR_UUID16_MEASUREMENT:
-        rc = ble_svc_hr_notify_measurement();
+        rc = os_mbuf_append(ctxt->om, &ble_svc_hr_measurement,
+                            sizeof(ble_svc_hr_measurement));
         return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
     case BLE_SVC_HR_CHR_UUID16_BODY_SENSOR_LOC:
@@ -122,7 +123,7 @@ ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle,
                                       &ble_svc_hr_ctrl_pt,
                                       NULL);
 
-            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+            return rc;
         }
         return BLE_SVC_HS_ERR_CMD_NOT_SUPPORTED;
 
@@ -172,26 +173,34 @@ int
 ble_svc_hr_notify_measurement(void)
 {
     int rc;
+    int first_err = 0;
     struct os_mbuf *txom = NULL;
 
-    for (int i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) {
+    for (int i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) {
         if (ble_svc_hr_conn_handle[i] != -1) {
 
             txom = ble_hs_mbuf_from_flat(&ble_svc_hr_measurement,
                                          sizeof(ble_svc_hr_measurement));
             if (!txom) {
                 BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
-                return BLE_HS_ENOMEM;
+                if (first_err == 0) {
+                    first_err = BLE_HS_ENOMEM;
+                }
+                continue;
             }
 
             rc = ble_gatts_notify_custom(ble_svc_hr_conn_handle[i],
                                          ble_svc_hr_measurement_val_handle, txom);
             if (rc != 0) {
-                return rc;
+                BLE_HS_LOG(ERROR, "%s conn=%d rc=%d\n", __func__,
+                           ble_svc_hr_conn_handle[i], rc);
+                if (first_err == 0) {
+                    first_err = rc;
+                }
             }
         }
     }
-    return 0;
+    return first_err;
 }
 
 /**

nimble/host/services/htp/src/ble_svc_htp.c

@@ -115,7 +115,7 @@ static const struct ble_gatt_svc_def ble_svc_htp_defs[] = {
                 {
                     {
                         .uuid = BLE_UUID16_DECLARE(BLE_SVC_HTP_DSC_UUID16_VALID_RANGE),
-                        .att_flags = BLE_ATT_F_READ | BLE_ATT_F_WRITE,
+                        .att_flags = BLE_ATT_F_READ,
                         .access_cb = ble_svc_htp_access,
                     }, {
                         0,
@@ -166,7 +166,7 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle,
 
     case BLE_SVC_HTP_CHR_UUID16_MEASUREMENT_ITVL:
         if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = ble_svc_htp_chr_write(ctxt->om, 0, sizeof(ble_svc_htp_temp_msr_itvl),
+            rc = ble_svc_htp_chr_write(ctxt->om, sizeof(ble_svc_htp_temp_msr_itvl), sizeof(ble_svc_htp_temp_msr_itvl),
                                        &ble_svc_htp_temp_msr_itvl,
                                        NULL);
             return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
@@ -178,8 +178,16 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle,
             return BLE_SVC_HS_ERR_OUT_OF_RANGE;
         }
 
+    case BLE_SVC_HTP_DSC_UUID16_VALID_RANGE:
+        /* Valid Range descriptor is read-only; minimum and maximum interval values */
+        if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) {
+            static const uint16_t valid_range[2] = {1, 65535};
+            rc = os_mbuf_append(ctxt->om, valid_range, sizeof(valid_range));
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        return BLE_ATT_ERR_WRITE_NOT_PERMITTED;
+
     default:
-        assert(0);
         return BLE_ATT_ERR_UNLIKELY;
     }
 }
@@ -216,6 +224,9 @@ ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr)
     if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) {
         return false;
     }
+    if (chr < 0 || chr >= 3) {
+        return false;
+    }
     return conn_chr_subs[conn_handle].chr_subs[chr];
 }
 
@@ -276,7 +287,7 @@ ble_svc_htp_notify(uint16_t conn_handle, float temp, bool temp_unit)
 
     txom = ble_hs_mbuf_from_flat(&flags, sizeof(flags));
     if (!txom) {
-        return ESP_FAIL;
+        return BLE_HS_ENOMEM;
     }
 
     rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp));
@@ -323,7 +334,7 @@ ble_svc_htp_indicate(uint16_t conn_handle, float temp, bool temp_unit)
 
     txom = ble_hs_mbuf_from_flat(&flags, sizeof(flags));
     if (!txom) {
-        return ESP_FAIL;
+        return BLE_HS_ENOMEM;
     }
 
     rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp));

nimble/host/services/ipss/src/ble_svc_ipss.c

@@ -52,5 +52,6 @@ ble_svc_ipss_init(void)
 
     rc = ble_gatts_add_svcs(ble_svc_ipss_defs);
     SYSINIT_PANIC_ASSERT(rc == 0);
+    (void)rc;
 }
 #endif

nimble/host/services/lls/src/ble_svc_lls.c

@@ -111,7 +111,7 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
                             sizeof ble_svc_lls_alert_level);
         return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
-    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+    case BLE_GATT_ACCESS_OP_WRITE_CHR: {
         uint8_t level;
         rc = ble_svc_lls_chr_write(ctxt->om,
                                    sizeof level,
@@ -124,6 +124,7 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
             ble_svc_lls_alert_level = level;
         }
         return rc;
+    }
 
     default:
         assert(0);
@@ -150,9 +151,11 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
 void
 ble_svc_lls_on_gap_disconnect(int reason)
 {
+    ble_svc_lls_event_fn *cb = ble_svc_lls_cb_fn;
+
     if (reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_SPVN_TMO)) {
-        if (ble_svc_lls_cb_fn != NULL) {
-            ble_svc_lls_cb_fn(ble_svc_lls_alert_level);
+        if (cb != NULL) {
+            cb(ble_svc_lls_alert_level);
         }
     }
 }

nimble/host/services/prox/include/services/prox/ble_svc_prox.h

@@ -36,6 +36,7 @@ void ble_svc_prox_ctx_deinit(void);
  * @brief  Initializes proximity service.
  */
 void ble_svc_prox_init(void);
+void ble_svc_prox_deinit(void);
 
 #ifdef __cplusplus
 }

nimble/host/services/prox/src/ble_svc_prox.c

@@ -222,15 +222,28 @@ ble_svc_prox_link_loss_access(uint16_t conn_handle, uint16_t attr_handle,
     uint16_t uuid16;
     int rc;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_svc_prox_ctx == NULL) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+#endif
+
     uuid16 = ble_uuid_u16(ctxt->chr->uuid);
     assert(uuid16 != 0);
 
     switch (uuid16) {
     case BLE_SVC_PROX_CHR_UUID16_ALERT_LVL:
         if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = ble_svc_prox_chr_write(ctxt->om, 1, sizeof(ble_svc_prox_link_loss_alert),
-                                        &ble_svc_prox_link_loss_alert, NULL);
-            return rc;
+            uint8_t tmp_alert;
+            rc = ble_svc_prox_chr_write(ctxt->om, 1, sizeof(tmp_alert), &tmp_alert, NULL);
+            if (rc != 0) {
+                return rc;
+            }
+            if (tmp_alert > 2) {
+                return BLE_ATT_ERR_VALUE_NOT_ALLOWED;
+            }
+            ble_svc_prox_link_loss_alert = tmp_alert;
+            return 0;
         } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
             rc = os_mbuf_append(ctxt->om, &ble_svc_prox_link_loss_alert,
                                 sizeof(ble_svc_prox_link_loss_alert));
@@ -251,6 +264,12 @@ ble_svc_prox_imm_alert_access(uint16_t conn_handle, uint16_t attr_handle,
 {
     uint16_t uuid16;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_svc_prox_ctx == NULL) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+#endif
+
     uuid16 = ble_uuid_u16(ctxt->chr->uuid);
     assert(uuid16 != 0);
 
@@ -259,7 +278,7 @@ ble_svc_prox_imm_alert_access(uint16_t conn_handle, uint16_t attr_handle,
         if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
             int rc = ble_svc_prox_chr_write(ctxt->om, 1, 1, &ble_svc_prox_alert, NULL);
             if (rc != 0) {
-                return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+                return rc;
             }
             MODLOG_DFLT(INFO, "Path loss = %d", ble_svc_prox_alert);
 
@@ -282,6 +301,12 @@ ble_svc_prox_tx_pwr_access(uint16_t conn_handle, uint16_t attr_handle,
     uint16_t uuid16;
     int rc;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_svc_prox_ctx == NULL) {
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+#endif
+
     uuid16 = ble_uuid_u16(ctxt->chr->uuid);
     assert(uuid16 != 0);
 
@@ -361,4 +386,18 @@ ble_svc_prox_init(void)
     BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", 4096, NULL, 10, &ble_prox_task_handle);
     SYSINIT_PANIC_ASSERT(ret == pdPASS);
 }
+
+void
+ble_svc_prox_deinit(void)
+{
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    /* ble_svc_prox_ctx_deinit handles NULL check, task deletion, and ctx free */
+    ble_svc_prox_ctx_deinit();
+#else
+    if (ble_prox_task_handle != NULL) {
+        vTaskDelete(ble_prox_task_handle);
+        ble_prox_task_handle = NULL;
+    }
+#endif
+}
 #endif

nimble/host/services/ras/include/services/ras/ble_svc_ras.h

@@ -173,6 +173,18 @@ struct ranging_buffer {
     /** Index for tracking the current write position in subevent data. */
     uint16_t subevent_cursor;
 
+    /** Offset of the most recently written subevent_header within the
+     *  subevents array. Used to update num_steps_reported when a
+     *  RESULT_CONTINUE event appends additional steps to the same subevent. */
+    uint16_t last_subevent_hdr_offset;
+
+    /** True only when last_subevent_hdr_offset points to a successfully written
+     *  subevent header for the current subevent. Reset to false at the start of
+     *  every RESULT event (before the overflow check) so that a RESULT_CONTINUE
+     *  arriving after a failed/overflow RESULT does not corrupt the previous
+     *  subevent's header via a stale offset. */
+    bool last_subevent_hdr_valid;
+
     /** Reference count to ensure safe access and prevent premature reuse. */
     uint8_t refcount;
 
@@ -227,6 +239,7 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent);
 void ble_gatts_indicate_control_point_response(uint16_t attr_handle , uint16_t ranging_counter);
 struct ranging_buffer *ranging_buffer_alloc(uint16_t conn_handle , uint16_t ranging_counter);
 void ble_svc_ras_init(void);
+void ble_svc_ras_deinit(void);
 
 
 #endif

nimble/host/services/ras/src/ble_svc_ras.c

@@ -92,6 +92,9 @@ static void ranging_buffer_init(uint16_t conn_handle, struct ranging_buffer *buf
     buf->isbusy = true;
     buf->isacked = false;
     buf->subevent_cursor = 0;
+    buf->last_subevent_hdr_offset = 0;
+    buf->last_subevent_hdr_valid = false;
+    memset(&buf->ranging_data, 0, sizeof(buf->ranging_data));
 }
 
 static void reset_ranging_buffer(void)
@@ -104,6 +107,8 @@ static void reset_ranging_buffer(void)
         ranging_buffers[i].isbusy = false;
         ranging_buffers[i].isacked = false;
         ranging_buffers[i].subevent_cursor = 0;
+        ranging_buffers[i].last_subevent_hdr_offset = 0;
+        ranging_buffers[i].last_subevent_hdr_valid = false;
     }
 }
 
@@ -113,9 +118,10 @@ ble_svc_ras_ensure_ctx_init()
 {
     if (ble_svc_ras_ctx == NULL) {
         ble_svc_ras_ctx = nimble_platform_mem_calloc(1, sizeof(ble_svc_ras_ctx_t));
+        if (ble_svc_ras_ctx != NULL) {
+            reset_ranging_buffer();
+        }
     }
-
-    reset_ranging_buffer();
 }
 
 void
@@ -377,9 +383,11 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl
                         sizeof(ble_svc_ras_rt_rd_val),
                         sizeof(ble_svc_ras_rt_rd_val),
                         &ble_svc_ras_rt_rd_val, NULL);
-                ble_gatts_chr_updated(attr_handle);
-                MODLOG_DFLT(INFO, "Notification/Indication scheduled for "
-                "all subscribed peers.\n");
+                if (rc == 0) {
+                    ble_gatts_chr_updated(attr_handle);
+                    MODLOG_DFLT(INFO, "Notification/Indication scheduled for "
+                    "all subscribed peers.\n");
+                }
                 return rc;
             } else if (attr_handle == ble_svc_ras_od_rd_val_handle) {
                 /* Ensure the buffer is allocated before writing to it */
@@ -420,10 +428,23 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl
                         ble_svc_ras_od_rd_val = NULL;
                         ble_svc_ras_od_rd_seg_len = 0;
                     }
-                    /* Reset the ranging buffers */
+                    /* Reset the ranging buffer for this connection */
+                    for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) {
+                        if (ranging_buffers[i].conn == (int)conn_handle) {
+                            ranging_buffers[i].conn = -1;
+                            ranging_buffers[i].ranging_counter = 0;
+                            ranging_buffers[i].isready = false;
+                            ranging_buffers[i].isbusy = false;
+                            ranging_buffers[i].isacked = false;
+                            ranging_buffers[i].subevent_cursor = 0;
+                            ranging_buffers[i].last_subevent_hdr_offset = 0;
+                            ranging_buffers[i].last_subevent_hdr_valid = false;
+                        }
+                    }
                     ble_svc_ras_cp_val[0]= 0x02;
                     /*Table 3.12. Response Code Values associated with Op Code 0x02*/
                     ble_svc_ras_cp_val[1]=0x01; // Success
+                    ble_svc_ras_cp_val[2]=0x01; // Response value: Success
                     ble_gatts_chr_updated(ble_svc_ras_cp_val_handle);
                     MODLOG_DFLT(INFO, "Successfully completed the Ranging procedure\n");
 
@@ -440,18 +461,22 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl
                         sizeof(ble_svc_ras_rd_val),
                         sizeof(ble_svc_ras_rd_val),
                         &ble_svc_ras_rd_val, NULL);
-                ble_gatts_chr_updated(attr_handle);
-                MODLOG_DFLT(INFO, "Notification/Indication scheduled for "
-                "all subscribed peers.\n");
+                if (rc == 0) {
+                    ble_gatts_chr_updated(attr_handle);
+                    MODLOG_DFLT(INFO, "Notification/Indication scheduled for "
+                    "all subscribed peers.\n");
+                }
                 return rc;
             } else if (attr_handle == ble_svc_ras_rd_ov_val_handle) {
                 rc = gatt_svr_write(ctxt->om,
                         sizeof(ble_svc_ras_rd_ov_val),
                         sizeof(ble_svc_ras_rd_ov_val),
                         &ble_svc_ras_rd_ov_val, NULL);
-                ble_gatts_chr_updated(attr_handle);
-                MODLOG_DFLT(INFO, "Notification/Indication scheduled for "
-                "all subscribed peers.\n");
+                if (rc == 0) {
+                    ble_gatts_chr_updated(attr_handle);
+                    MODLOG_DFLT(INFO, "Notification/Indication scheduled for "
+                    "all subscribed peers.\n");
+                }
                 return rc;
             }
 
@@ -472,6 +497,48 @@ unknown:
 
 void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) {
     struct ranging_buffer *buf = NULL;
+    uint16_t conn_handle;
+    uint16_t procedure_counter;
+    uint8_t config_id;
+    uint8_t procedure_done_status;
+    uint8_t subevent_done_status;
+    uint8_t abort_reason;
+    uint8_t num_antenna_paths;
+    uint8_t num_steps_reported;
+    const struct cs_steps_data *steps;
+    uint16_t start_acl_conn_event_counter = 0;
+    uint16_t frequency_compensation = 0;
+    uint8_t reference_power_level = 0;
+
+    /* Extract fields from the correct union member based on event type */
+    if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) {
+        conn_handle = ranging_subevent.subev_result.conn_handle;
+        procedure_counter = ranging_subevent.subev_result.procedure_counter;
+        config_id = ranging_subevent.subev_result.config_id;
+        procedure_done_status = ranging_subevent.subev_result.procedure_done_status;
+        subevent_done_status = ranging_subevent.subev_result.subevent_done_status;
+        abort_reason = ranging_subevent.subev_result.abort_reason;
+        num_antenna_paths = ranging_subevent.subev_result.num_antenna_paths;
+        num_steps_reported = ranging_subevent.subev_result.num_steps_reported;
+        steps = ranging_subevent.subev_result.steps;
+        start_acl_conn_event_counter = ranging_subevent.subev_result.start_acl_conn_event_counter;
+        frequency_compensation = ranging_subevent.subev_result.frequency_compensation;
+        reference_power_level = ranging_subevent.subev_result.reference_power_level;
+    } else if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE) {
+        conn_handle = ranging_subevent.subev_result_continue.conn_handle;
+        /* subev_result_continue has no procedure_counter; reuse conn to find buf */
+        procedure_counter = 0;
+        config_id = ranging_subevent.subev_result_continue.config_id;
+        procedure_done_status = ranging_subevent.subev_result_continue.procedure_done_status;
+        subevent_done_status = ranging_subevent.subev_result_continue.subevent_done_status;
+        abort_reason = ranging_subevent.subev_result_continue.abort_reason;
+        num_antenna_paths = ranging_subevent.subev_result_continue.num_antenna_paths;
+        num_steps_reported = ranging_subevent.subev_result_continue.num_steps_reported;
+        steps = ranging_subevent.subev_result_continue.steps;
+    } else {
+        MODLOG_DFLT(ERROR, "Unknown CS event type %d\n", ranging_subevent.type);
+        return;
+    }
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     ble_svc_ras_ensure_ctx_init();
@@ -480,70 +547,162 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) {
     }
 #endif
 
-    /* Check if the subevent is already stored */
-    for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) {
-        if (ranging_buffers[i].conn == ranging_subevent.subev_result.conn_handle &&
-            ranging_buffers[i].ranging_counter == ranging_subevent.subev_result.procedure_counter) {
-            buf = &ranging_buffers[i];
-            break;
+    /* Find the buffer for this event.
+     * RESULT:          match on conn_handle + ranging_counter (unique per procedure).
+     * RESULT_CONTINUE: procedure_counter not available per BT Core Spec §7.7.65.45;
+     *                  match on conn_handle + config_id. config_id is only 4 bits
+     *                  (0-3), so successive procedures on the same connection share
+     *                  the same config_id. Among all matching buffers pick the one
+     *                  with the highest ranging_counter — that is the most recently
+     *                  started procedure and the correct target for RESULT_CONTINUE. */
+    if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE) {
+        uint16_t best_counter = 0;
+        bool found = false;
+        for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) {
+            if (ranging_buffers[i].conn != (int)conn_handle) {
+                continue;
+            }
+            if (ranging_buffers[i].ranging_data.ranging_header.config_id == config_id) {
+                if (!found || ranging_buffers[i].ranging_counter > best_counter) {
+                    buf = &ranging_buffers[i];
+                    best_counter = ranging_buffers[i].ranging_counter;
+                    found = true;
+                }
+            }
+        }
+    } else {
+        for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) {
+            if (ranging_buffers[i].conn != (int)conn_handle) {
+                continue;
+            }
+            if (ranging_buffers[i].ranging_counter == procedure_counter) {
+                buf = &ranging_buffers[i];
+                break;
+            }
         }
     }
 
     if (buf == NULL) {
-        /* Allocate a new buffer if not found */
-        buf = ranging_buffer_alloc(ranging_subevent.subev_result.conn_handle, ranging_subevent.subev_result.procedure_counter);
+        /* Per BT spec §7.7.65.45, RESULT_CONTINUE shall only follow a prior
+         * RESULT event. Receiving RESULT_CONTINUE without an existing buffer
+         * is a protocol error — reject to avoid producing corrupt data. */
+        if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE) {
+            MODLOG_DFLT(ERROR, "RESULT_CONTINUE without prior RESULT for conn 0x%04x\n",
+                        conn_handle);
+            return;
+        }
+        buf = ranging_buffer_alloc(conn_handle, procedure_counter);
         if (buf == NULL) {
             MODLOG_DFLT(ERROR,"No available buffer for storing ranging data\n");
             return;
         }
     }
 
-    buf->ranging_data.ranging_header.config_id = ranging_subevent.subev_result.config_id;
-    buf->ranging_data.ranging_header.ranging_counter = ranging_subevent.subev_result.procedure_counter;
-  //  buf->ranging_data.ranging_header.selected_tx_power = ranging_subevent.subev_result.selected_tx_power;
-    /* convert antenna path mask using bitmask */
-    buf->ranging_data.ranging_header.antenna_paths_mask = ranging_subevent.subev_result.num_antenna_paths;
+    /* Only set header fields for initial RESULT event; RESULT_CONTINUE appends
+     * to an existing buffer and must not overwrite the valid ranging_counter. */
+    if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) {
+        buf->ranging_data.ranging_header.config_id = config_id;
+        buf->ranging_data.ranging_header.ranging_counter = procedure_counter;
+        /* convert antenna path mask using bitmask */
+        buf->ranging_data.ranging_header.antenna_paths_mask = num_antenna_paths;
+    }
 
     uint16_t max_subevent_data = BLE_RAS_PROCEDURE_MEM - sizeof(struct ranging_header);
 
-    if (buf->subevent_cursor + sizeof(struct subevent_header) > max_subevent_data) {
-        MODLOG_DFLT(ERROR, "Ranging buffer overflow on subevent header\n");
+    /* RESULT_CONTINUE appends step data to the same logical subevent; no new
+     * subevent_header is written. Only RESULT events open a new subevent. */
+    if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) {
+        /* Invalidate the prior subevent's header pointer before the overflow
+         * check. If this RESULT overflows and returns early, any subsequent
+         * RESULT_CONTINUE must not corrupt the previous subevent's header. */
+        buf->last_subevent_hdr_valid = false;
+
+        if (buf->subevent_cursor + sizeof(struct subevent_header) > max_subevent_data) {
+            MODLOG_DFLT(ERROR, "Ranging buffer overflow on subevent header\n");
+            return;
+        }
+
+        /* Save offset so RESULT_CONTINUE can locate and update this header. */
+        buf->last_subevent_hdr_offset = buf->subevent_cursor;
+        buf->last_subevent_hdr_valid = true;
+
+        struct subevent_header *subevent_hdr = (struct subevent_header *)(buf->ranging_data.subevents + buf->subevent_cursor);
+        buf->subevent_cursor += sizeof(struct subevent_header);
+        subevent_hdr->start_acl_conn_event = start_acl_conn_event_counter;
+        subevent_hdr->freq_compensation = frequency_compensation;
+        subevent_hdr->ranging_done_status = procedure_done_status;
+        subevent_hdr->subevent_done_status = subevent_done_status;
+        /* Abort_Reason per BT Core Spec §7.7.65.44: bits 0-3 = procedure abort,
+         * bits 4-7 = subevent abort. Extract each nibble separately. */
+        subevent_hdr->ranging_abort_reason  = abort_reason & 0x0F;
+        subevent_hdr->subevent_abort_reason = (abort_reason >> 4) & 0x0F;
+        subevent_hdr->ref_power_level = reference_power_level;
+        /* num_steps_reported set after the step loop to reflect actual written count. */
+    }
+
+    /* For RESULT_CONTINUE: if the corresponding RESULT event failed (overflow),
+     * last_subevent_hdr_valid is false. Writing step data into the buffer with
+     * no subevent header to account for it produces a corrupt RAS segment.
+     * Reject early, before the loop advances subevent_cursor. */
+    if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE &&
+            !buf->last_subevent_hdr_valid) {
+        MODLOG_DFLT(ERROR, "RESULT_CONTINUE with no valid subevent header; dropping\n");
         return;
     }
 
-    struct subevent_header *subevent_hdr = (struct subevent_header *)(buf->ranging_data.subevents + buf->subevent_cursor);
-    buf->subevent_cursor += sizeof(struct subevent_header);
-    subevent_hdr->start_acl_conn_event = ranging_subevent.subev_result.start_acl_conn_event_counter;
-    subevent_hdr->freq_compensation = ranging_subevent.subev_result.frequency_compensation;
-    subevent_hdr->ranging_done_status = ranging_subevent.subev_result.procedure_done_status;
-    subevent_hdr->subevent_done_status = ranging_subevent.subev_result.subevent_done_status;
-    subevent_hdr->ranging_abort_reason = ranging_subevent.subev_result.abort_reason;
-    subevent_hdr->subevent_abort_reason = ranging_subevent.subev_result.abort_reason;
-    subevent_hdr->ref_power_level = ranging_subevent.subev_result.reference_power_level;
-    subevent_hdr->num_steps_reported = ranging_subevent.subev_result.num_steps_reported;
-
-
-    /* Add step data to buf using manual pointer advancement for flexible array member */
-    const uint8_t *step_ptr = (const uint8_t *)ranging_subevent.subev_result.steps;
-    for (int i = 0; i < ranging_subevent.subev_result.num_steps_reported; i++) {
+    /* Add step data; track actual steps written so the header reflects reality
+     * even if the buffer overflows mid-loop and we return early. */
+    uint8_t steps_written = 0;
+    const uint8_t *step_ptr = (const uint8_t *)steps;
+    for (int i = 0; i < num_steps_reported; i++) {
         const struct cs_steps_data *step = (const struct cs_steps_data *)step_ptr;
 
         if (buf->subevent_cursor + BLE_RAS_STEP_MODE_LEN + step->data_len > max_subevent_data) {
             MODLOG_DFLT(ERROR, "Ranging buffer overflow on step data\n");
-            return;
+            break;
         }
 
         buf->ranging_data.subevents[buf->subevent_cursor] = step->mode;
         buf->subevent_cursor += BLE_RAS_STEP_MODE_LEN;
         memcpy(&buf->ranging_data.subevents[buf->subevent_cursor], step->data, step->data_len);
         buf->subevent_cursor += step->data_len;
-
         step_ptr += sizeof(struct cs_steps_data) + step->data_len;
+        steps_written++;
+    }
+
+    /* Update the subevent header with the count of steps actually written.
+     * For RESULT: correct any pre-set count in case of overflow.
+     * For RESULT_CONTINUE: accumulate only the steps that fit, and overwrite
+     * the status/abort fields with the final values from this last event
+     * (BT Core Spec v6.2 §7.7.65.44 — the Continue event carries the terminal
+     * done/abort status that supersedes the partial status set by RESULT).
+     * Guard with last_subevent_hdr_valid: a RESULT_CONTINUE arriving after an
+     * overflowed RESULT must not corrupt the previous subevent's header. */
+    if (buf->last_subevent_hdr_valid) {
+        struct subevent_header *subevent_hdr =
+            (struct subevent_header *)(buf->ranging_data.subevents +
+                                       buf->last_subevent_hdr_offset);
+        if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) {
+            subevent_hdr->num_steps_reported = steps_written;
+        } else {
+            subevent_hdr->num_steps_reported += steps_written;
+            subevent_hdr->ranging_done_status  = procedure_done_status;
+            subevent_hdr->subevent_done_status = subevent_done_status;
+            subevent_hdr->ranging_abort_reason  = abort_reason & 0x0F;
+            subevent_hdr->subevent_abort_reason = (abort_reason >> 4) & 0x0F;
+        }
     }
     /* Create RAS segment*/
     struct segment *ras_segment;
 
-    uint16_t max_data_len = ble_att_mtu(ranging_subevent.subev_result.conn_handle) - sizeof(struct segment_header) - 4;
+    uint16_t att_mtu = ble_att_mtu(conn_handle);
+    uint16_t overhead = (uint16_t)(sizeof(struct segment_header) + 4);
+    if (att_mtu == 0 || att_mtu <= overhead) {
+        MODLOG_DFLT(INFO, "MTU (%d) too small for RAS segment overhead (%d)\n",
+                    att_mtu, overhead);
+        return;
+    }
+    uint16_t max_data_len = att_mtu - overhead;
     MODLOG_DFLT(INFO, "Max data len : %d\n", max_data_len);
     ras_segment= nimble_platform_mem_calloc(1,sizeof(struct segment)+ max_data_len);
     if (ras_segment == NULL) {
@@ -621,4 +780,20 @@ ble_svc_ras_init(void) {
     reset_ranging_buffer();
 
 }
+
+void
+ble_svc_ras_deinit(void)
+{
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_svc_ras_od_rd_val != NULL) {
+        nimble_platform_mem_free(ble_svc_ras_od_rd_val);
+        ble_svc_ras_od_rd_val = NULL;
+    }
+    ble_svc_ras_od_rd_seg_len = 0;
+#else
+    /* ble_svc_ras_ctx_deinit handles NULL check, frees inner buffer,
+     * frees ctx itself, and sets ble_svc_ras_ctx = NULL. */
+    ble_svc_ras_ctx_deinit();
+#endif
+}
 #endif

nimble/host/services/ras/src/rrsp/ble_svc_ras_rrsp.c

@@ -27,7 +27,7 @@
 static uint32_t ble_svc_ras_feat_val;
 static uint16_t ble_svc_ras_rd_val;
 static uint16_t ble_svc_ras_rd_ov_val;
-static uint16_t ble_svc_ras_cp_val;
+static uint8_t ble_svc_ras_cp_val[RASCP_CMD_OPCODE_LEN + sizeof(uint16_t)];
 
 static uint16_t ble_svc_ras_feat_val_handle;
 static uint16_t ble_svc_ras_od_val_handle;

nimble/host/services/sps/src/ble_svc_sps.c

@@ -20,6 +20,7 @@
 #include <assert.h>
 #include <string.h>
 #include "sysinit/sysinit.h"
+#include "host/ble_att.h"
 #include "host/ble_hs.h"
 #include "services/sps/ble_svc_sps.h"
 #include "host/ble_hs_log.h"
@@ -113,6 +114,11 @@ ble_svc_sps_chr_write(struct os_mbuf *om, uint16_t min_len,
 }
 
 void ble_svc_sps_scan_refresh() {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_svc_sps_static_vars == NULL) {
+        return;
+    }
+#endif
     /* spec allows only value 0 to send */
     ble_scan_refresh = 0;
     ble_gatts_chr_updated(ble_scan_refresh_handle);
@@ -131,8 +137,16 @@ ble_svc_sps_access(uint16_t conn_handle, uint16_t attr_handle,
         assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR);
         rc = ble_svc_sps_chr_write(ctxt->om, 4, 4, buf, NULL);
         if(rc == 0) {
-            ble_scan_itvl = get_le16(buf);
-            ble_scan_window = get_le16(buf + 2);
+            uint16_t itvl = get_le16(buf);
+            uint16_t window = get_le16(buf + 2);
+
+            if (itvl < 0x0004 || itvl > 0x4000 ||
+                window < 0x0004 || window > 0x4000 ||
+                window > itvl) {
+                return BLE_ATT_ERR_VALUE_NOT_ALLOWED;
+            }
+            ble_scan_itvl = itvl;
+            ble_scan_window = window;
             if (ble_svc_sps_cb_fn) {
                 ble_svc_sps_cb_fn(ble_scan_itvl, ble_scan_window);
             }
@@ -198,8 +212,17 @@ ble_svc_sps_set_cb(ble_svc_sps_event_fn *cb)
 void ble_svc_sps_deinit(void)
 {
     ble_gatts_free_svcs();
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_svc_sps_static_vars != NULL) {
+        ble_scan_itvl = 0;
+        ble_scan_window = 0;
+        nimble_platform_mem_free(ble_svc_sps_static_vars);
+        ble_svc_sps_static_vars = NULL;
+    }
+#else
     ble_scan_itvl = 0;
     ble_scan_window = 0;
+#endif
 }
 
 /**

nimble/host/src/ble_aes_ccm.c

@@ -64,8 +64,7 @@ ble_aes_ccm_hex(const void *buf, size_t len)
     char *str;
     int i;
 
-    str = hexbufs[curbuf++];
-    curbuf %= ARRAY_SIZE(hexbufs);
+    str = hexbufs[curbuf++ % ARRAY_SIZE(hexbufs)];
 
     len = min(len, (sizeof(hexbufs[0]) - 1) / 2);
 
@@ -142,18 +141,26 @@ int
 ble_aes_ccm_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data)
 {
     struct tc_aes_key_sched_struct s = {0};
+    int rc = 0;
 
     if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
-        return BLE_HS_EUNKNOWN;
+        rc = BLE_HS_EUNKNOWN;
+        goto done;
     }
 
     if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
-        return BLE_HS_EUNKNOWN;
+        rc = BLE_HS_EUNKNOWN;
     }
 
-    return 0;
+done:
+    memset(&s, 0, sizeof(s));
+    /* Compiler barrier: prevent the optimizer from eliminating the memset above
+     * as a dead store. The key schedule in s is sensitive cryptographic material
+     * and must actually be erased from the stack before the frame is released. */
+    __asm__ volatile("" : : "r"(&s) : "memory");
+    return rc;
 }
 #endif
 
@@ -361,14 +368,36 @@ int ble_aes_ccm_decrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t
 
     rc = ble_aes_ccm_crypt(key_reversed, nonce, enc_msg, out_msg, msg_len);
     if (rc != 0) {
+        memset(out_msg, 0, msg_len);
+        memset(key_reversed, 0, sizeof(key_reversed));
+        __asm__ volatile("" : : "r"(key_reversed) : "memory");
         return rc;
     }
 
     rc = ble_aes_ccm_auth(key_reversed, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size);
+    memset(key_reversed, 0, sizeof(key_reversed));
+    __asm__ volatile("" : : "r"(key_reversed) : "memory");
     if (rc != 0) {
+        memset(out_msg, 0, msg_len);
+        memset(mic, 0, sizeof(mic));
+        __asm__ volatile("" : : "r"(mic) : "memory");
         return rc;
     }
 
+    /* Constant-time MIC comparison to prevent timing side-channel attacks */
+    volatile uint8_t diff = 0;
+    for (size_t i = 0; i < mic_size; i++) {
+        diff |= mic[i] ^ enc_msg[msg_len + i];
+    }
+    if (diff != 0) {
+        memset(out_msg, 0, msg_len);
+        memset(mic, 0, sizeof(mic));
+        __asm__ volatile("" : : "r"(mic) : "memory");
+        return BLE_HS_EAUTHEN;
+    }
+
+    memset(mic, 0, sizeof(mic));
+    __asm__ volatile("" : : "r"(mic) : "memory");
     return 0;
 }
 
@@ -400,11 +429,15 @@ int ble_aes_ccm_encrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t
     /** Calculating MIC */
     int rc = ble_aes_ccm_auth(key_reversed, nonce, msg, msg_len, aad, aad_len, mic, mic_size);
     if (rc != 0) {
+        memset(key_reversed, 0, sizeof(key_reversed));
+        __asm__ volatile("" : : "r"(key_reversed) : "memory");
         return rc;
     }
 
     /** Encrypting advertisement */
     rc = ble_aes_ccm_crypt(key_reversed, nonce, msg, out_msg, msg_len);
+    memset(key_reversed, 0, sizeof(key_reversed));
+    __asm__ volatile("" : : "r"(key_reversed) : "memory");
     if (rc != 0) {
         return rc;
     }

nimble/host/src/ble_att.c

@@ -550,6 +550,16 @@ ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle,
         return;
     }
 #if MYNEWT_VAL(BLE_GATTS)
+    /*
+     * Responses and notifications have bit 0 set (includes Handle Value
+     * Indication 0x1D). Handle Value Confirmation (0x1E) has bit 0 clear.
+     */
+    if ((op & 0x01) || op == BLE_ATT_OP_INDICATE_RSP) {
+        os_mbuf_free_chain(*om);
+        *om = NULL;
+        return;
+    }
+
     os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om));
     ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0,
                              BLE_ATT_ERR_REQ_NOT_SUPPORTED);
@@ -690,10 +700,6 @@ process_att_normally:
         return BLE_HS_EMSGSIZE;
     }
 
-    if (cid == BLE_L2CAP_CID_ATT && ble_att_is_response_op(op)) {
-        ble_att_send_outstanding_after_response(conn_handle);
-    }
-
     entry = ble_att_rx_dispatch_entry_find(op);
     if (entry == NULL) {
         BLE_HS_LOG(INFO, "ATT handler not found; op=0x%02x conn_handle=0x%04x "
@@ -708,6 +714,9 @@ process_att_normally:
     os_mbuf_adj(*om, 1);
 
     rc = entry->bde_fn(conn_handle, cid, om);
+    if (cid == BLE_L2CAP_CID_ATT && ble_att_is_response_op(op)) {
+        ble_att_send_outstanding_after_response(conn_handle);
+    }
     if (rc != 0) {
         if (rc == BLE_HS_ENOTSUP) {
             ble_att_rx_handle_unknown_request(op, conn_handle, cid, om);
@@ -760,11 +769,11 @@ ble_att_set_preferred_mtu(uint16_t mtu)
         return BLE_HS_EINVAL;
     }
 
-    ble_att_preferred_mtu_val = mtu;
-
     /* Set my_mtu for established connections that haven't exchanged. */
     ble_hs_lock();
 
+    ble_att_preferred_mtu_val = mtu;
+
     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);

nimble/host/src/ble_att_clt.c

@@ -94,6 +94,12 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu)
 
     req = ble_att_cmd_get(BLE_ATT_OP_MTU_REQ, sizeof(*req), &txom);
     if (req == NULL) {
+        ble_hs_lock();
+        if (ble_att_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT,
+                                   &conn, &chan) == 0) {
+            chan->flags &= ~BLE_L2CAP_CHAN_F_TXED_MTU;
+        }
+        ble_hs_unlock();
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
         return BLE_HS_ENOMEM;
     }
@@ -124,11 +130,9 @@ ble_att_clt_rx_mtu(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
 
     mtu = 0;
 
-#if MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0
-    if (ble_hs_cfg.eatt && cid != BLE_L2CAP_CID_ATT) {
+    if (cid != BLE_L2CAP_CID_ATT) {
         return BLE_HS_ENOTSUP;
     }
-#endif
 
     rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*cmd));
     if (rc == 0) {
@@ -334,7 +338,7 @@ ble_att_clt_parse_find_type_value_hinfo(
 
     rc = ble_hs_mbuf_pullup_base(om, sizeof(*group));
     if (rc != 0) {
-        return BLE_HS_EBADDATA;
+        return rc;
     }
 
     group = (struct ble_att_handle_group *)(*om)->om_data;
@@ -371,7 +375,7 @@ ble_att_clt_rx_find_type_value(uint16_t conn_handle, uint16_t cid, struct os_mbu
     /* Notify GATT client that the full response has been parsed. */
     ble_gattc_rx_find_type_value_complete(conn_handle, cid, rc);
 
-    return 0;
+    return rc;
 }
 
 /*****************************************************************************
@@ -555,7 +559,7 @@ int
 ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid,
                          const uint16_t *handles, int num_handles, bool variable)
 {
-#if !NIMBLE_BLE_ATT_CLT_READ_MULT
+#if !NIMBLE_BLE_ATT_CLT_READ_MULT && !NIMBLE_BLE_ATT_CLT_READ_MULT_VAR
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -564,7 +568,7 @@ ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid,
     int i;
     uint8_t op;
 
-    if (num_handles < 1) {
+    if (num_handles < 2) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
@@ -625,6 +629,7 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle, uint16_t cid,
 
     struct ble_att_read_group_type_req *req;
     struct os_mbuf *txom;
+    int rc;
 
     if (start_handle == 0 || start_handle > end_handle) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
@@ -640,7 +645,11 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle, uint16_t cid,
 
     req->bagq_start_handle = htole16(start_handle);
     req->bagq_end_handle = htole16(end_handle);
-    ble_uuid_flat(uuid, req->uuid);
+    rc = ble_uuid_flat(uuid, req->uuid);
+    if (rc != 0) {
+        os_mbuf_free_chain(txom);
+        return rc;
+    }
 
     return ble_att_tx(conn_handle, cid, txom);
 }
@@ -898,14 +907,16 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han
         nimble_platform_mem_free(message);
         message = NULL;
     }
+    /* After concat, txom is owned by txom2 chain; caller must not free txom */
     os_mbuf_concat(txom2, txom);
+    txom = NULL;
     return ble_att_tx(conn_handle, cid, txom2);
 err:
     if (message != NULL) {
         nimble_platform_mem_free(message);
         message = NULL;
     }
-    /* Do not free txom here; the caller ble_gattc_signed_write is responsible for it on error. */
+    os_mbuf_free_chain(txom);
     os_mbuf_free_chain(txom2);
     return rc;
 }
@@ -920,6 +931,7 @@ ble_att_clt_tx_prep_write(uint16_t conn_handle, uint16_t cid, uint16_t handle,
                           uint16_t offset, struct os_mbuf *txom)
 {
 #if !NIMBLE_BLE_ATT_CLT_PREP_WRITE
+    os_mbuf_free_chain(txom);
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -1072,7 +1084,9 @@ ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle,
 
     cid = ble_eatt_get_available_chan_cid(conn_handle, BLE_GATT_OP_DUMMY);
     rc = ble_att_tx(conn_handle, cid, txom2);
-    ble_eatt_release_chan(conn_handle, BLE_GATT_OP_DUMMY);
+    if (cid != BLE_L2CAP_CID_ATT) {
+        ble_eatt_release_chan(conn_handle, BLE_GATT_OP_DUMMY);
+    }
     return rc;
 
 err:
@@ -1138,6 +1152,7 @@ int
 ble_att_clt_tx_notify_mult(uint16_t conn_handle, struct os_mbuf *txom)
 {
 #if !NIMBLE_BLE_ATT_CLT_NOTIFY_MULT
+    os_mbuf_free_chain(txom);
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -1147,6 +1162,7 @@ ble_att_clt_tx_notify_mult(uint16_t conn_handle, struct os_mbuf *txom)
 
     if (ble_att_cmd_get(BLE_ATT_OP_NOTIFY_MULTI_REQ, 0, &txom2) == NULL) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
+        os_mbuf_free_chain(txom);
         return BLE_HS_ENOMEM;
     }
 

nimble/host/src/ble_att_cmd.c

@@ -36,13 +36,12 @@ ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom)
         return NULL;
     }
 
-    if (os_mbuf_extend(txom, sizeof(*hdr) + len) == NULL) {
+    hdr = os_mbuf_extend(txom, sizeof(*hdr) + len);
+    if (hdr == NULL) {
         os_mbuf_free_chain(txom);
         return NULL;
     }
 
-    hdr = (struct ble_att_hdr *)(txom)->om_data;
-
     hdr->opcode = opcode;
 
     return hdr->data;
@@ -231,7 +230,10 @@ ble_att_mtu_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_MTU_REQ, payload, BLE_ATT_MTU_CMD_SZ,
                              len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->bamc_mtu = le16toh(src->bamc_mtu);
 }
 
@@ -243,7 +245,10 @@ ble_att_mtu_rsp_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_MTU_RSP, payload, BLE_ATT_MTU_CMD_SZ,
                              len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->bamc_mtu = le16toh(src->bamc_mtu);
 }
 
@@ -267,6 +272,9 @@ ble_att_mtu_rsp_write(void *payload, int len,
 
     dst = ble_att_init_write(BLE_ATT_OP_MTU_RSP, payload,
                              BLE_ATT_MTU_CMD_SZ, len);
+    if (dst == NULL) {
+        return;
+    }
     dst->bamc_mtu = htole16(src->bamc_mtu);
 }
 
@@ -278,7 +286,10 @@ ble_att_find_info_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_REQ, payload,
                              BLE_ATT_FIND_INFO_REQ_SZ, len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->bafq_start_handle = le16toh(src->bafq_start_handle);
     dst->bafq_end_handle = le16toh(src->bafq_end_handle);
 }
@@ -304,6 +315,10 @@ ble_att_find_info_rsp_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_RSP, payload,
                              BLE_ATT_FIND_INFO_RSP_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->bafp_format = src->bafp_format;
 }
@@ -316,6 +331,9 @@ ble_att_find_info_rsp_write(void *payload, int len,
 
     dst = ble_att_init_write(BLE_ATT_OP_FIND_INFO_RSP, payload,
                              BLE_ATT_FIND_INFO_RSP_BASE_SZ, len);
+    if (dst == NULL) {
+        return;
+    }
 
     dst->bafp_format = src->bafp_format;
 }
@@ -328,7 +346,10 @@ ble_att_find_type_value_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, payload,
                              BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     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);
@@ -356,7 +377,10 @@ ble_att_read_type_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_REQ, payload,
                              BLE_ATT_READ_TYPE_REQ_BASE_SZ, len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->batq_start_handle = le16toh(src->batq_start_handle);
     dst->batq_end_handle = le16toh(src->batq_end_handle);
 }
@@ -382,7 +406,10 @@ ble_att_read_type_rsp_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_RSP, payload,
                              BLE_ATT_READ_TYPE_RSP_BASE_SZ, len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->batp_length = src->batp_length;
 }
 
@@ -394,7 +421,9 @@ ble_att_read_type_rsp_write(void *payload, int len,
 
     dst = ble_att_init_write(BLE_ATT_OP_READ_TYPE_RSP, payload,
                              BLE_ATT_READ_TYPE_RSP_BASE_SZ, len);
-
+    if (dst == NULL) {
+        return;
+    }
     dst->batp_length = src->batp_length;
 }
 
@@ -406,6 +435,10 @@ ble_att_read_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_REQ, payload,
                              BLE_ATT_READ_REQ_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->barq_handle = le16toh(src->barq_handle);
 }
@@ -430,6 +463,10 @@ ble_att_read_blob_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_BLOB_REQ, payload,
                              BLE_ATT_READ_BLOB_REQ_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->babq_handle = le16toh(src->babq_handle);
     dst->babq_offset = le16toh(src->babq_offset);
@@ -484,7 +521,10 @@ ble_att_read_group_type_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload,
                              BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len);
-
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->bagq_start_handle = le16toh(src->bagq_start_handle);
     dst->bagq_end_handle = le16toh(src->bagq_end_handle);
 }
@@ -497,7 +537,10 @@ ble_att_read_group_type_req_write(
 
     dst = ble_att_init_write(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload,
                              BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len);
-
+    if (dst == NULL) {
+        BLE_HS_DBG_ASSERT(dst != NULL);
+        return;
+    }
     dst->bagq_start_handle = htole16(src->bagq_start_handle);
     dst->bagq_end_handle = htole16(src->bagq_end_handle);
 }
@@ -510,6 +553,10 @@ ble_att_read_group_type_rsp_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload,
                              BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->bagp_length = src->bagp_length;
 }
@@ -522,7 +569,9 @@ ble_att_read_group_type_rsp_write(
 
     dst = ble_att_init_write(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload,
                              BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len);
-
+    if (dst == NULL) {
+        return;
+    }
     dst->bagp_length = src->bagp_length;
 }
 
@@ -534,6 +583,10 @@ ble_att_write_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_WRITE_REQ, payload,
                              BLE_ATT_WRITE_REQ_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->bawq_handle = le16toh(src->bawq_handle);
 }
@@ -546,6 +599,10 @@ ble_att_write_cmd_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_WRITE_CMD, payload,
                              BLE_ATT_WRITE_REQ_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
     dst->bawq_handle = le16toh(src->bawq_handle);
 }
 
@@ -568,6 +625,10 @@ ble_att_write_cmd_write(void *payload, int len,
 
     dst = ble_att_init_write(BLE_ATT_OP_WRITE_CMD, payload,
                              BLE_ATT_WRITE_REQ_BASE_SZ, len);
+    if (dst == NULL) {
+        BLE_HS_DBG_ASSERT(dst != NULL);
+        return;
+    }
     dst->bawq_handle = htole16(src->bawq_handle);
 }
 
@@ -579,6 +640,10 @@ ble_att_prep_write_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_REQ, payload,
                              BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->bapc_handle = le16toh(src->bapc_handle);
     dst->bapc_offset = le16toh(src->bapc_offset);
@@ -605,6 +670,10 @@ ble_att_prep_write_rsp_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_RSP, payload,
                              BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->bapc_handle = le16toh(src->bapc_handle);
     dst->bapc_offset = le16toh(src->bapc_offset);
@@ -631,6 +700,10 @@ ble_att_exec_write_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_REQ, payload,
                              BLE_ATT_EXEC_WRITE_REQ_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->baeq_flags = src->baeq_flags;
 }
@@ -669,6 +742,10 @@ ble_att_notify_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_NOTIFY_REQ, payload,
                              BLE_ATT_NOTIFY_REQ_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->banq_handle = le16toh(src->banq_handle);
 }
@@ -693,6 +770,10 @@ ble_att_indicate_req_parse(const void *payload, int len,
 
     src = ble_att_init_parse(BLE_ATT_OP_INDICATE_REQ, payload,
                              BLE_ATT_INDICATE_REQ_BASE_SZ, len);
+    if (src == NULL) {
+        memset(dst, 0, sizeof(*dst));
+        return;
+    }
 
     dst->baiq_handle = le16toh(src->baiq_handle);
 }

nimble/host/src/ble_att_priv.h

@@ -148,9 +148,7 @@ 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);
 
-#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
 int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle);
-#endif
 #if MYNEWT_VAL(BLE_GATT_CACHING)
 int ble_att_get_database_size(int *out_size);
 int ble_att_fill_database_info(uint8_t *out_data);

nimble/host/src/ble_att_svr.c

@@ -108,6 +108,9 @@ ble_att_svr_ensure_ctx(void)
         return BLE_HS_ENOMEM;
     }
 
+    STAILQ_INIT(&ble_att_svr_ctx->list);
+    STAILQ_INIT(&ble_att_svr_ctx->hidden_list);
+
     return 0;
 }
 #endif
@@ -156,6 +159,9 @@ ble_att_svr_next_id(void)
 {
     /* Rollover is fatal. */
     BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX);
+    if (ble_att_svr_id == UINT16_MAX) {
+        return 0; /* Invalid handle; caller must check */
+    }
     return ++ble_att_svr_id;
 }
 
@@ -187,6 +193,11 @@ ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags,
     entry->ha_flags = flags;
     entry->ha_min_key_size = min_key_size;
     entry->ha_handle_id = ble_att_svr_next_id();
+    if (entry->ha_handle_id == 0) {
+        ble_att_svr_entry_free(entry);
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
+        return BLE_HS_ENOMEM;
+    }
     entry->ha_cb = cb;
     entry->ha_cb_arg = cb_arg;
 
@@ -199,7 +210,6 @@ ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags,
     return 0;
 }
 
-#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
 /**
  * Deregister a host attribute with the BLE stack.
  *
@@ -229,7 +239,6 @@ int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle) {
     }
     return 0;
 }
-#endif
 
 uint16_t
 ble_att_svr_prev_handle(void)
@@ -972,7 +981,10 @@ ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
 
             put_le16(buf + 0, ha->ha_handle_id);
 
-            ble_uuid_flat(ha->ha_uuid, buf + 2);
+            rc = ble_uuid_flat(ha->ha_uuid, buf + 2);
+            if (rc != 0) {
+                goto done;
+            }
 
             num_entries++;
         }
@@ -1010,6 +1022,7 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle, uint16_t cid,
     if (rsp == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
+        txom = NULL;
         goto done;
     }
 
@@ -1020,8 +1033,12 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle, uint16_t cid,
     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;
+        if (rc == BLE_HS_ENOMEM) {
+            *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        } else {
+            *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND;
+            rc = BLE_HS_ENOENT;
+        }
         goto done;
     }
 
@@ -1161,13 +1178,6 @@ ble_att_svr_is_valid_group_end(const ble_uuid_t *uuid_group,
         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:
@@ -1263,12 +1273,22 @@ ble_att_svr_fill_type_value(uint16_t conn_handle,
         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 == BLE_HS_EMSGSIZE) {
+                /* Attribute value longer than search buffer; treat as non-match. */
+                *out_att_err = 0;
+                rc = 0;
+                continue;
+            }
             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);
+            /* value is at the end of req; require exact length match */
+            if ((uint16_t)(OS_MBUF_PKTLEN(rxom) - sizeof(struct ble_att_find_type_value_req)) != attr_len) {
+                rc = 1; /* length mismatch - treat as non-match */
+            } else {
+                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;
@@ -1324,6 +1344,7 @@ ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle, uint16_t cid,
     if (buf == NULL) {
         *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
+        txom = NULL;
         goto done;
     }
 
@@ -1429,7 +1450,7 @@ static void ble_att_svr_make_conn_aware(uint16_t conn_handle) {
 
 static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) {
     struct ble_hs_conn *conn;
-    struct ble_hs_conn_addrs addrs;
+    struct ble_hs_conn_addrs addrs = {0};
 
     conn = ble_hs_conn_find_assert(conn_handle);
     BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
@@ -1502,6 +1523,7 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid,
 
     rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_TYPE_RSP, sizeof(*rsp), txom);
     if (rsp == NULL) {
+        txom = NULL; /* ble_att_cmd_prepare freed txom on failure */
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         *err_handle = 0;
         rc = BLE_HS_ENOMEM;
@@ -1721,7 +1743,7 @@ ble_att_svr_rx_read(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     ble_hs_lock();
     if((ble_att_svr_get_csfs(conn_handle)[0] & 1)
-        && ble_svc_gatt_csf_handle() != err_handle ) {
+        && ble_svc_gatt_csf_handle() != err_handle) {
         if (!ble_att_svr_check_conn_aware(conn_handle)) {
             att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC;
             rc = BLE_HS_EREJECT;
@@ -1740,6 +1762,7 @@ ble_att_svr_rx_read(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
     if (ble_att_cmd_prepare(BLE_ATT_OP_READ_RSP, 0, txom) == NULL) {
         att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
+        txom = NULL;
         goto done;
     }
 
@@ -1785,7 +1808,7 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     ble_hs_lock();
     if((ble_att_svr_get_csfs(conn_handle)[0] & 1)
-        && ble_svc_gatt_csf_handle() != err_handle ) {
+        && ble_svc_gatt_csf_handle() != err_handle) {
         if (!ble_att_svr_check_conn_aware(conn_handle)) {
             att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC;
             rc = BLE_HS_EREJECT;
@@ -1805,6 +1828,7 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
     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;
+        txom = NULL;
         goto done;
     }
 
@@ -1846,6 +1870,7 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle, uint16_t cid,
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         *err_handle = 0;
         rc = BLE_HS_ENOMEM;
+        txom = NULL;
         goto done;
     }
 
@@ -1952,6 +1977,7 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid,
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         *err_handle = 0;
         rc = BLE_HS_ENOMEM;
+        txom = NULL;
         goto done;
     }
 
@@ -1996,12 +2022,16 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid,
         uint16_t le_len = htole16(tuple_len);
         rc = os_mbuf_append(txom, &le_len, sizeof(le_len));
         if (rc != 0) {
+            *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+            rc = BLE_HS_ENOMEM;
             *err_handle = handle;
             goto done;
         }
         if (tuple_len != 0) {
             rc = os_mbuf_appendfrom(txom, tmp, 0, tuple_len);
             if (rc != 0) {
+                *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+                rc = BLE_HS_ENOMEM;
                 *err_handle = handle;
                 goto done;
             }
@@ -2102,6 +2132,9 @@ ble_att_svr_service_uuid(struct ble_att_svr_entry *entry,
         rc = ble_uuid_init_from_buf(uuid, val, attr_len);
     }
 
+    if (rc != 0 && out_att_err != NULL) {
+        *out_att_err = BLE_ATT_ERR_UNLIKELY;
+    }
     return rc;
 }
 
@@ -2131,7 +2164,11 @@ ble_att_svr_read_group_type_entry_write(struct os_mbuf *om, uint16_t mtu,
 
     put_le16(buf + 0, start_group_handle);
     put_le16(buf + 2, end_group_handle);
-    ble_uuid_flat(service_uuid, buf + 4);
+    int rc2 = ble_uuid_flat(service_uuid, buf + 4);
+    if (rc2 != 0) {
+        os_mbuf_adj(om, -len);
+        return rc2;
+    }
 
     return 0;
 }
@@ -2181,6 +2218,7 @@ ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle, uint16_t cid,
     rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_GROUP_TYPE_RSP, sizeof(*rsp),
                               txom);
     if (rsp == NULL) {
+        txom = NULL; /* ble_att_cmd_prepare freed txom on failure */
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
         goto done;
@@ -2192,7 +2230,6 @@ ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle, uint16_t cid,
             continue;
         }
         if (entry->ha_handle_id > end_handle) {
-            /* The full input range has been searched. */
             rc = 0;
             goto done;
         }
@@ -2407,8 +2444,6 @@ ble_att_svr_build_write_rsp(struct os_mbuf **rxom, struct os_mbuf **out_txom,
      */
     rc = ble_att_svr_pkt(rxom, &txom, att_err);
     if (rc != 0) {
-        os_mbuf_free_chain(txom);
-	txom = NULL;
         goto done;
     }
 
@@ -2497,6 +2532,7 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf *
 {
 #if !MYNEWT_VAL(BLE_ATT_SVR_WRITE_NO_RSP)
     os_mbuf_free_chain(*rxom);
+    *rxom = NULL;
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -2506,6 +2542,7 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf *
         !ble_att_svr_check_conn_aware(conn_handle)) {
         ble_hs_unlock();
         os_mbuf_free_chain(*rxom);
+        *rxom = NULL;
         return BLE_HS_EREJECT;
     }
     ble_hs_unlock();
@@ -2575,6 +2612,12 @@ ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf *
         return rc;
     }
 
+    /* Ensure packet is large enough to hold the handle and 12-byte signature. */
+    if (OS_MBUF_PKTLEN(*rxom) < (uint16_t)(sizeof(*req) + (BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ - BLE_ATT_SIGNED_WRITE_DATA_OFFSET))) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
     req = (struct ble_att_signed_write_cmd *)(*rxom)->om_data;
 
     handle = le16toh(req->handle);
@@ -2656,6 +2699,10 @@ ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om)
 {
     int rc;
 
+    if (om == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
     rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0,
                                   &om, NULL);
 
@@ -2844,10 +2891,16 @@ ble_att_svr_prep_write(uint16_t conn_handle,
         ble_att_svr_prep_extract(prep_list, &attr_handle, &om);
 
         /* Attribute existence was verified during prepare-write request
-         * processing.
+         * processing.  Guard against dynamic deletion between prepare and
+         * execute.
          */
         attr = ble_att_svr_find_by_handle(attr_handle);
         BLE_HS_DBG_ASSERT(attr != NULL);
+        if (attr == NULL) {
+            os_mbuf_free_chain(om);
+            *err_handle = attr_handle;
+            return BLE_ATT_ERR_INVALID_HANDLE;
+        }
 
         rc = ble_att_svr_write(conn_handle, attr, 0, &om, &att_err);
         os_mbuf_free_chain(om);
@@ -2949,7 +3002,7 @@ ble_att_svr_rx_prep_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     ble_hs_lock();
     if((ble_att_svr_get_csfs(conn_handle)[0] & 1)
-        && ble_svc_gatt_csf_handle() != err_handle ) {
+        && ble_svc_gatt_csf_handle() != err_handle) {
         if (!ble_att_svr_check_conn_aware(conn_handle)) {
             att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC;
             rc = BLE_HS_EREJECT;
@@ -3071,6 +3124,7 @@ ble_att_svr_rx_exec_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r
     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;
+        txom = NULL;
         goto done;
     }
 
@@ -3124,8 +3178,8 @@ ble_att_svr_rx_notify(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL);
     if (rc != 0) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
-        return BLE_HS_ENOMEM;
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc);
+        return rc;
     }
 
     req = (struct ble_att_notify_req *)(*rxom)->om_data;
@@ -3175,7 +3229,6 @@ ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid, struct os_mbuf *
     while (pkt_len > 0) {
         rc = ble_att_svr_pullup_req_base(rxom, sizeof(struct ble_att_tuple_list), NULL);
         if (rc != 0) {
-            rc = BLE_HS_ENOMEM;
             goto done;
         }
 
@@ -3193,6 +3246,13 @@ ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid, struct os_mbuf *
             goto done;
         }
 
+        if (attr_len > OS_MBUF_PKTLEN(*rxom)) {
+            BLE_HS_LOG_ERROR("attr_len (%d) > pkt_len (%d)",
+                             attr_len, OS_MBUF_PKTLEN(*rxom));
+            rc = BLE_HS_EBADDATA;
+            goto done;
+        }
+
         tmp = os_msys_get_pkthdr(attr_len, 0);
         if (!tmp) {
             BLE_HS_LOG_ERROR("not enough resources, aborting");
@@ -3260,6 +3320,7 @@ ble_att_svr_build_indicate_rsp(struct os_mbuf **rxom,
     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;
+        txom = NULL;
         goto done;
     }
 
@@ -3480,9 +3541,7 @@ ble_att_svr_free_start_mem(void)
         ble_att_svr_entry_mem = NULL;
     }
     os_mempool_unregister(&ble_att_svr_entry_pool);
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     memset(&ble_att_svr_entry_pool, 0, sizeof(ble_att_svr_entry_pool));
-#endif
 }
 
 int
@@ -3536,6 +3595,8 @@ ble_att_svr_deinit(void)
         return;
     }
 
+    ble_att_svr_reset();
+
     if (ble_att_svr_prep_entry_mem) {
         nimble_platform_mem_free(ble_att_svr_prep_entry_mem);
         ble_att_svr_prep_entry_mem = NULL;
@@ -3552,6 +3613,7 @@ ble_att_svr_deinit(void)
 void
 ble_att_svr_stop(void)
 {
+    ble_att_svr_reset();
     ble_att_svr_free_start_mem();
 }
 
@@ -3652,8 +3714,17 @@ int ble_att_get_database_size(int *out_size)
 
             /* uuid is stored in the value attribute */
             struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next);
+            uint8_t val[20];
+            uint16_t attr_len;
+
             if (next_entry == NULL) {
-                size += 4; // Updated
+                rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE,
+                                           entry, 0, sizeof(val), val,
+                                           &attr_len, &att_error);
+                if (rc != 0) {
+                    return rc;
+                }
+                size += (4 + attr_len);
                 continue;
             }
             /* handle(2 bytes) + type(2 bytes) + properties(1 byte)
@@ -3708,7 +3779,10 @@ int ble_att_fill_database_info(uint8_t *out_data)
             put_le16(data, entry->ha_handle_id);
             uuid = BLE_UUID16(entry->ha_uuid);
             put_le16(data + 2, uuid->value);
-            ble_uuid_flat(&service_uuid.u, data + 4);
+            rc = ble_uuid_flat(&service_uuid.u, data + 4);
+            if (rc != 0) {
+                return rc;
+            }
             data += (4 + ble_uuid_length(&service_uuid.u));
         }
         else if(uuid->value == BLE_ATT_UUID_INCLUDE) {
@@ -3728,6 +3802,22 @@ int ble_att_fill_database_info(uint8_t *out_data)
             data += (4 + attr_len);
         }
         else if(uuid->value == BLE_ATT_UUID_CHARACTERISTIC) {
+            struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next);
+            if (next_entry == NULL) {
+                put_le16(data, entry->ha_handle_id);
+                uuid = BLE_UUID16(entry->ha_uuid);
+                put_le16(data + 2, uuid->value);
+
+                rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE,
+                                           entry, 0, sizeof(val), val,
+                                           &attr_len, &att_error);
+                if (rc != 0) {
+                    return rc;
+                }
+                memcpy(data + 4, val, attr_len);
+                data += (4 + attr_len);
+                continue;
+            }
 
             /* handle(2 bytes) + type(2 bytes) + properties(1 byte)
                       + val_handle(2 bytes) + uuid */
@@ -3743,6 +3833,7 @@ int ble_att_fill_database_info(uint8_t *out_data)
             }
             memcpy(data + 4, val, attr_len);
             data += (4 + attr_len);
+            entry = next_entry;
         }
         else if(uuid->value == 0x2901 ||
                 uuid->value == 0x2902 ||

nimble/host/src/ble_cs.c

@@ -693,7 +693,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data,
     struct ble_gap_conn_desc desc;
 
     if (len != sizeof(*ev) || ev->status) {
-        BLE_HS_LOG(INFO, "Failed to enable CS security BLE_HS_ECNOTEROLLER");
+        BLE_HS_LOG(INFO, "Failed to enable CS security");
 
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
         return BLE_HS_ECONTROLLER;
@@ -715,7 +715,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data,
     cmd.min_procedure_interval = 10;
     cmd.max_procedure_interval = 10;
     /* Minimum/maximum suggested durations for each CS subevent in microseconds.
-     * 1250us and 5000us selected.
+     * 60000us selected for both min and max.
      */
     cmd.min_subevent_len = 60000;
     cmd.max_subevent_len = 60000;
@@ -825,6 +825,11 @@ ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data,
     int steps_remaining = 0;
     int step_size = 0;
 
+    if (len < sizeof(*event)) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
+        return BLE_HS_ECONTROLLER;
+    }
+
     expected_len += sizeof(*event);
     steps_remaining = event->num_steps_reported;
     step_ptr = (void *)event->steps;
@@ -869,6 +874,10 @@ ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data
     void *step_ptr = NULL;
     int step_size = 0;
 
+    if (len < sizeof(*event)) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
+        return BLE_HS_ECONTROLLER;
+    }
     expected_len = sizeof(*event);
     steps_remaining = event->num_steps_reported;
     step_ptr = (void *)event->steps;
@@ -889,7 +898,7 @@ ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data
         }
 
         expected_len += step_size;
-        step_ptr += step_size;
+        step_ptr = (uint8_t *)step_ptr + step_size;
         steps_remaining--;
     }
 
@@ -929,6 +938,10 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p
         return rc;
     }
 #endif
+    if (params == NULL) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
 
     /* Channel Sounding setup phase:
      * 1. Set local default CS settings
@@ -944,7 +957,7 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p
     cmd.conn_handle = params->conn_handle;
     rc = ble_cs_rd_rem_supp_cap(&cmd);
     if (rc) {
-        BLE_HS_LOG(DEBUG, "Failed to read local supported CS capabilities,"
+        BLE_HS_LOG(DEBUG, "Failed to read remote supported CS capabilities, "
                    "err %d", rc);
     }
 

nimble/host/src/ble_ead.c

@@ -123,6 +123,11 @@ int ble_ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i
                    "Randomizer and the MIC.");
     }
 
+    if (payload_size > 0xffff) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
+
     /* Ensure payload_size isn't too large to wrap around when adding overhead */
     if (payload_size > SIZE_MAX - (BLE_EAD_RANDOMIZER_SIZE + BLE_EAD_MIC_SIZE)) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);

nimble/host/src/ble_eatt.c

@@ -235,9 +235,7 @@ ble_eatt_alloc(void)
     struct ble_eatt *eatt;
 
     eatt = os_memblock_get(&ble_eatt_conn_pool);
-    if (eatt) {
-        SLIST_INSERT_HEAD(&g_ble_eatt_list, eatt, next);
-    } else {
+    if (!eatt) {
         BLE_EATT_LOG_DEBUG("eatt: Failed to allocate new eatt context\n");
         return NULL;
     }
@@ -249,6 +247,8 @@ ble_eatt_alloc(void)
     STAILQ_INIT(&eatt->eatt_tx_q);
     ble_npl_event_init(&eatt->setup_ev, ble_eatt_setup_cb, eatt);
     ble_npl_event_init(&eatt->wakeup_ev, ble_eatt_wakeup_cb, eatt);
+
+    SLIST_INSERT_HEAD(&g_ble_eatt_list, eatt, next);
     return eatt;
 }
 
@@ -257,6 +257,15 @@ ble_eatt_free(struct ble_eatt *eatt)
 {
     struct os_mbuf_pkthdr *omp;
 
+    /* Remove pending events and release internal event resources. On FreeRTOS,
+     * ble_npl_event_init allocates memory that event_deinit must free. */
+    ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(),
+                          &eatt->setup_ev);
+    ble_npl_event_deinit(&eatt->setup_ev);
+    ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(),
+                          &eatt->wakeup_ev);
+    ble_npl_event_deinit(&eatt->wakeup_ev);
+
     while ((omp = STAILQ_FIRST(&eatt->eatt_tx_q)) != NULL) {
         STAILQ_REMOVE_HEAD(&eatt->eatt_tx_q, omp_next);
         os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp));
@@ -399,7 +408,6 @@ ble_eatt_setup_cb(struct ble_npl_event *ev)
     if (rc) {
         BLE_EATT_LOG_ERROR("eatt: Failed to connect EATT on conn_handle 0x%04x (status=%d)\n",
                            eatt->conn_handle, rc);
-        os_mbuf_free_chain(om);
         ble_eatt_free(eatt);
     }
 }
@@ -459,28 +467,30 @@ ble_gatt_eatt_read_uuid_cb(uint16_t conn_handle,
 
     if (error == NULL || (error->status != 0 && error->status != BLE_HS_EDONE)) {
         BLE_EATT_LOG_DEBUG("eatt: Cannot find Server Supported features on peer device\n");
-        return BLE_HS_EDONE;
+        return 0;
     }
 
-    if (attr == NULL) {
-        BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n");
-        return BLE_HS_EDONE;
-    }
+    if (error->status == 0) {
+        if (attr == NULL) {
+            BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n");
+            return 0;
+        }
 
-    rc = os_mbuf_copydata(attr->om, 0, 1, &supported_features);
-    if (rc) {
-        BLE_EATT_LOG_ERROR("eatt: Cannot read srv supported features \n");
-        return BLE_HS_EDONE;
-    }
+        rc = os_mbuf_copydata(attr->om, 0, 1, &supported_features);
+        if (rc) {
+            BLE_EATT_LOG_ERROR("eatt: Cannot read srv supported features \n");
+            return 0;
+        }
 
-    if (supported_features & 0x01) {
-        struct ble_npl_event *ev = (struct ble_npl_event *)nimble_platform_mem_calloc(1, sizeof(struct ble_npl_event));
-        if (ev) {
-            ble_npl_event_init(ev, ble_gatt_eatt_read_cl_uuid, (void *)((uintptr_t) conn_handle));
-            ble_npl_eventq_put(ble_hs_evq_get(), ev);
+        if (supported_features & 0x01) {
+            struct ble_npl_event *ev = (struct ble_npl_event *)nimble_platform_mem_calloc(1, sizeof(struct ble_npl_event));
+            if (ev) {
+                ble_npl_event_init(ev, ble_gatt_eatt_read_cl_uuid, (void *)((uintptr_t) conn_handle));
+                ble_npl_eventq_put(ble_hs_evq_get(), ev);
+            }
         }
     }
-    return BLE_HS_EDONE;
+    return 0;
 }
 
 static void
@@ -679,11 +689,18 @@ ble_eatt_start(uint16_t conn_handle)
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 void ble_eatt_deinit(void)
 {
+    struct ble_eatt *eatt;
+    struct ble_eatt *next;
+
     if (ble_eatt_ctx == NULL)
     {
         return;
     }
 
+    SLIST_FOREACH_SAFE(eatt, &g_ble_eatt_list, next, next) {
+        ble_eatt_free(eatt);
+    }
+
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
     if (ble_eatt_sdu_coc_mem) {
         nimble_platform_mem_free(ble_eatt_sdu_coc_mem);
@@ -735,9 +752,9 @@ ble_eatt_init(ble_eatt_att_rx_fn att_rx_cb)
         if (!ble_eatt_sdu_coc_mem) {
             // free the allocated memory
             nimble_platform_mem_free(ble_eatt_conn_mem);
+            ble_eatt_conn_mem = NULL;
             nimble_platform_mem_free(ble_eatt_ctx);
-	    ble_eatt_conn_mem = NULL;
-	    ble_eatt_ctx = NULL;
+            ble_eatt_ctx = NULL;
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
             return BLE_HS_ENOMEM;
         }
@@ -781,6 +798,8 @@ ble_eatt_init(ble_eatt_att_rx_fn att_rx_cb)
 
 err:
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    os_mempool_unregister(&ble_eatt_sdu_mbuf_mempool);
+    os_mempool_unregister(&ble_eatt_conn_pool);
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
     nimble_platform_mem_free(ble_eatt_sdu_coc_mem);
     ble_eatt_sdu_coc_mem = NULL;

nimble/host/src/ble_gattc.c

@@ -741,12 +741,14 @@ ble_gattc_log_write(uint16_t att_handle, uint16_t len, int expecting_rsp)
     BLE_HS_LOG(INFO, "att_handle=%d len=%d\n", att_handle, len);
 }
 
+#if NIMBLE_BLE_ATT_CLT_SIGNED_WRITE
 static void
 ble_gattc_log_signed_write(uint16_t att_handle, uint16_t len)
 {
     ble_gattc_log_proc_init("signed write; ");
     BLE_HS_LOG(INFO, "att_handle=%d len=%d\n", att_handle, len);
 }
+#endif
 
 static void
 ble_gattc_log_write_long(struct ble_gattc_proc *proc)
@@ -794,7 +796,7 @@ 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);
+    BLE_HS_LOG(INFO, "att_handle=%u\n", att_handle);
 }
 
 #if MYNEWT_VAL(BLE_GATTC)
@@ -1014,6 +1016,11 @@ static ble_gattc_err_fn *
 ble_gattc_err_dispatch_get(uint8_t op)
 {
     BLE_HS_DBG_ASSERT(op < BLE_GATT_OP_CNT);
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gattc_ctx == NULL || ble_gattc_err_dispatch == NULL) {
+        return NULL;
+    }
+#endif
     return ble_gattc_err_dispatch[op];
 }
 
@@ -1350,24 +1357,11 @@ ble_gattc_extract_with_rx_entry(uint16_t conn_handle, uint16_t cid,
  * @return                      The matching proc entry on success;
  *                                  null on failure.
  */
-#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 #define BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle, cid, rx_entries, out_rx_entry)  \
     ble_gattc_extract_with_rx_entry(                                          \
         (conn_handle), (cid), (rx_entries),                                   \
         sizeof (rx_entries) / sizeof (rx_entries)[0],                         \
         (const void **)(out_rx_entry))
-#else
-/*
- * When memory optimization flag is enabled:
- * RX entry table is dynamically allocated with 4 entries
- * to optimize memory usage and reduce static allocation.
- */
-#define BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle, cid, rx_entries, out_rx_entry)  \
-    ble_gattc_extract_with_rx_entry(                                          \
-        (conn_handle), (cid), (rx_entries),                                   \
-        4,                                                                    \
-        (const void **)(out_rx_entry))
-#endif
 #endif
 
 /**
@@ -1391,7 +1385,9 @@ ble_gattc_fail_procs(uint16_t conn_handle, uint8_t op, int status)
      */
     while ((proc = STAILQ_FIRST(&temp_list)) != NULL) {
         err_cb = ble_gattc_err_dispatch_get(proc->op);
-        err_cb(proc, status, 0);
+        if (err_cb != NULL) {
+            err_cb(proc, status, 0);
+        }
 
         STAILQ_REMOVE_HEAD(&temp_list, next);
         ble_gattc_proc_free(proc);
@@ -1448,7 +1444,9 @@ ble_gattc_resume_procs(void)
 
     ble_gattc_extract_stalled(&stall_list);
 
-    STAILQ_FOREACH(proc, &stall_list, next) {
+    while ((proc = STAILQ_FIRST(&stall_list)) != NULL) {
+        STAILQ_REMOVE_HEAD(&stall_list, next);
+
         resume_cb = ble_gattc_resume_dispatch_get(proc->op);
         BLE_HS_DBG_ASSERT(resume_cb != NULL);
 
@@ -1734,8 +1732,8 @@ ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *cb_arg)
     if (proc == NULL) {
         BLE_HS_LOG(INFO, "GATTC proc alloc failed; conn_handle=0x%04x op=mtu\n",
                    conn_handle);
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        STATS_INC(ble_gattc_stats, mtu_fail);
+        return BLE_HS_ENOMEM;
     }
 
     proc->op = BLE_GATT_OP_MTU;
@@ -1747,15 +1745,9 @@ ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *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;
 }
@@ -1891,7 +1883,8 @@ ble_gattc_disc_all_svcs_rx_adata(struct ble_gattc_proc *proc,
         goto done;
     }
 
-    if (adata->end_group_handle <= proc->disc_all_svcs.prev_handle) {
+    if (adata->att_handle > adata->end_group_handle ||
+        adata->end_group_handle <= proc->disc_all_svcs.prev_handle) {
         /* Peer sent services out of order; terminate procedure. */
         rc = BLE_HS_EBADDATA;
         goto done;
@@ -1905,7 +1898,7 @@ ble_gattc_disc_all_svcs_rx_adata(struct ble_gattc_proc *proc,
     rc = 0;
 
 done:
-    cbrc = ble_gattc_disc_all_svcs_cb(proc, rc, 0, &service);
+    cbrc = ble_gattc_disc_all_svcs_cb(proc, rc, 0, rc == 0 ? &service : NULL);
     if (rc != 0 || cbrc != 0) {
         return BLE_HS_EDONE;
     } else {
@@ -1991,13 +1984,19 @@ ble_gattc_disc_all_svcs(uint16_t conn_handle, ble_gatt_disc_svc_fn *cb,
     }
 
 done:
+    /* One ble_gattc_proc cannot be part of multiple linked lists.
+     * Hence it needs to be removed before going into ble_gattc_process_status.
+     */
+#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT)
+    if (proc != NULL) {
+        ble_hs_lock();
+        STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next);
+        ble_hs_unlock();
+    }
+#endif
+
     if (rc != 0) {
         STATS_INC(ble_gattc_stats, disc_all_svcs_fail);
-#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT)
-        ble_hs_lock();
-        STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next);
-        ble_hs_unlock();
-#endif
     }
 
     ble_gattc_process_status(proc, rc);
@@ -2061,7 +2060,10 @@ ble_gattc_disc_svc_uuid_tx(struct ble_gattc_proc *proc)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    ble_uuid_flat(&proc->disc_svc_uuid.service_uuid.u, val);
+    rc = ble_uuid_flat(&proc->disc_svc_uuid.service_uuid.u, val);
+    if (rc != 0) {
+        return rc;
+    }
     rc = ble_att_clt_tx_find_type_value(proc->conn_handle, proc->cid,
                                         proc->disc_svc_uuid.prev_handle + 1,
                                         0xffff, BLE_ATT_UUID_PRIMARY_SERVICE,
@@ -2137,7 +2139,7 @@ ble_gattc_disc_svc_uuid_rx_hinfo(struct ble_gattc_proc *proc,
     rc = 0;
 
 done:
-    cbrc = ble_gattc_disc_svc_uuid_cb(proc, rc, 0, &service);
+    cbrc = ble_gattc_disc_svc_uuid_cb(proc, rc, 0, rc == 0 ? &service : NULL);
     if (rc != 0 || cbrc != 0) {
         return BLE_HS_EDONE;
     } else {
@@ -2203,7 +2205,10 @@ ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, const ble_uuid_t *uuid,
 
     ble_gattc_proc_prepare(proc, conn_handle, BLE_GATT_OP_DISC_SVC_UUID);
 
-    ble_uuid_to_any(uuid, &proc->disc_svc_uuid.service_uuid);
+    rc = ble_uuid_to_any(uuid, &proc->disc_svc_uuid.service_uuid);
+    if (rc != 0) {
+        goto done;
+    }
     proc->disc_svc_uuid.prev_handle = 0x0000;
     proc->disc_svc_uuid.cb = cb;
     proc->disc_svc_uuid.cb_arg = cb_arg;
@@ -2398,6 +2403,10 @@ ble_gattc_find_inc_svcs_rx_read_rsp(struct ble_gattc_proc *proc, int status,
     /* Proceed to the next service. */
     proc->find_inc_svcs.cur_start = 0;
     proc->find_inc_svcs.cur_end = 0;
+    if (proc->find_inc_svcs.prev_handle >= proc->find_inc_svcs.end_handle) {
+        ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
     rc = ble_gattc_find_inc_svcs_resume(proc);
     if (rc != 0) {
         goto err;
@@ -2474,8 +2483,8 @@ ble_gattc_find_inc_svcs_rx_adata(struct ble_gattc_proc *proc,
 
 done:
     if (call_cb) {
-        cbrc = ble_gattc_find_inc_svcs_cb(proc, 0, 0, &service);
-        if (rc != 0) {
+        cbrc = ble_gattc_find_inc_svcs_cb(proc, rc, 0, rc == 0 ? &service : NULL);
+        if (rc == 0) {
             rc = cbrc;
         }
     } else {
@@ -2505,7 +2514,8 @@ ble_gattc_find_inc_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
         return BLE_HS_EDONE;
     }
 
-    if (proc->find_inc_svcs.prev_handle == 0xffff) {
+    if (proc->find_inc_svcs.prev_handle >= proc->find_inc_svcs.end_handle &&
+        proc->find_inc_svcs.cur_start == 0) {
         /* Procedure complete. */
         ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL);
         return BLE_HS_EDONE;
@@ -2712,7 +2722,8 @@ ble_gattc_disc_all_chrs_rx_adata(struct ble_gattc_proc *proc,
     chr.properties = adata->value[0];
     chr.val_handle = get_le16(adata->value + 1);
 
-    if (adata->att_handle <= proc->disc_all_chrs.prev_handle) {
+    if (adata->att_handle <= proc->disc_all_chrs.prev_handle ||
+        adata->att_handle > proc->disc_all_chrs.end_handle) {
         /* Peer sent characteristics out of order; terminate procedure. */
         rc = BLE_HS_EBADDATA;
         goto done;
@@ -2806,13 +2817,19 @@ ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle,
     }
 
 done:
+    /* One ble_gattc_proc cannot be part of multiple linked lists.
+     * Hence it needs to be removed before going into ble_gattc_process_status.
+     */
+#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT)
+    if (proc != NULL) {
+        ble_hs_lock();
+        STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next);
+        ble_hs_unlock();
+    }
+#endif
+
     if (rc != 0) {
         STATS_INC(ble_gattc_stats, disc_all_chrs_fail);
-#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT)
-        ble_hs_lock();
-        STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next);
-        ble_hs_unlock();
-#endif
     }
 
     ble_gattc_process_status(proc, rc);
@@ -3001,7 +3018,7 @@ ble_gattc_disc_chr_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
         return BLE_HS_EDONE;
     }
 
-    if (proc->disc_chr_uuid.prev_handle == proc->disc_chr_uuid.end_handle) {
+    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;
@@ -3027,6 +3044,14 @@ ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle,
     struct ble_gattc_proc *proc;
     int rc;
 
+    if (uuid == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (start_handle == 0 || start_handle > end_handle) {
+        return BLE_HS_EINVAL;
+    }
+
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     rc = ble_gattc_cache_conn_search_chrs_by_uuid(conn_handle, start_handle, end_handle, uuid, cb, cb_arg);
     if(rc == 0) {
@@ -3243,6 +3268,16 @@ ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t start_handle,
     struct ble_gattc_proc *proc;
     int rc;
 
+    if (start_handle >= end_handle) {
+        /* No descriptors to discover. Use a local error struct — ble_gattc_error()
+         * uses a static variable and is non-reentrant (ble_hs task only). */
+        if (cb != NULL) {
+            struct ble_gatt_error err = { .status = BLE_HS_EDONE, .att_handle = 0 };
+            cb(conn_handle, &err, start_handle, NULL, cb_arg);
+        }
+        return 0;
+    }
+
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     rc = ble_gattc_cache_conn_search_all_dscs(conn_handle, start_handle, end_handle, cb, cb_arg);
     if(rc == 0) {
@@ -3350,7 +3385,7 @@ static void ble_gattc_fill_gatt_db_conversion(uint16_t *count, uint16_t num, esp
             }
             break;
         }
-        case BLE_GATT_DB_INCLUDED_SERVICE: {
+        case ESP_BLE_GATT_DB_INCLUDED_SERVICE: {
             ble_gattc_included_svc_elem_t *incl_result = (ble_gattc_included_svc_elem_t *)result;
             for (int i = 0; i < db_size; i++) {
                 incl_result->handle = db[offset + i].handle;
@@ -3460,6 +3495,10 @@ int ble_gattc_get_service(uint16_t conn_handle,
                           ble_gattc_service_elem_t *result,
                           uint16_t *count, uint16_t offset)
 {
+      if (result == NULL || count == NULL) {
+          return BLE_HS_EINVAL;
+      }
+
       int rc;
       ble_gattc_db_elem_t *db = NULL;
       uint16_t svc_num = 0;
@@ -3489,6 +3528,10 @@ int ble_gattc_get_all_char(uint16_t conn_handle,
                            ble_gattc_char_elem_t *result,
                            uint16_t *count, uint16_t offset)
 {
+      if (result == NULL || count == NULL) {
+          return BLE_HS_EINVAL;
+      }
+
       int rc;
       ble_gattc_db_elem_t *db = NULL;
       uint16_t char_num = 0;
@@ -3517,6 +3560,10 @@ int ble_gattc_get_all_descr(uint16_t conn_handle,
                             ble_gattc_descr_elem_t *result,
                             uint16_t *count, uint16_t offset)
 {
+      if (result == NULL || count == NULL) {
+          return BLE_HS_EINVAL;
+      }
+
       int rc;
       ble_gattc_db_elem_t *db = NULL;
       uint16_t descr_num = 0;
@@ -3546,9 +3593,13 @@ int ble_gattc_get_char_by_uuid(uint16_t conn_handle,
                                ble_uuid_t *char_uuid,
                                ble_gattc_char_elem_t *result, uint16_t *count)
 {
-      int rc;
-      ble_gattc_db_elem_t *db = NULL;
-      uint16_t char_num = 0;
+    if (result == NULL || count == NULL || char_uuid == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
+    int rc;
+    ble_gattc_db_elem_t *db = NULL;
+    uint16_t char_num = 0;
 
       ble_gattc_get_cached_char_by_uuid_db(conn_handle, start_handle, end_handle, char_uuid, &db, &char_num);
 
@@ -3578,6 +3629,10 @@ int ble_gattc_get_descr_by_uuid(uint16_t conn_handle,
                                 ble_uuid_t *descr_uuid,
                                 ble_gattc_descr_elem_t *result, uint16_t *count)
 {
+      if (result == NULL || count == NULL || char_uuid == NULL) {
+          return BLE_HS_EINVAL;
+      }
+
       int rc;
       ble_gattc_db_elem_t *db = NULL;
       uint16_t descr_num = 0;
@@ -3669,6 +3724,11 @@ int ble_gattc_get_attr_count(uint16_t conn_handle, esp_ble_gatt_db_attr_type_t t
                              uint16_t start_handle, uint16_t end_handle,
                              uint16_t char_handle, uint16_t *count)
 {
+    if (count == NULL || type < ESP_BLE_GATT_DB_PRIMARY_SERVICE ||
+        type > ESP_BLE_GATT_DB_ALL) {
+        return BLE_HS_EINVAL;
+    }
+
     if (type == ESP_BLE_GATT_DB_ALL) {
         ble_gattc_get_db_size(conn_handle, start_handle, end_handle, count);
     } else {
@@ -3681,12 +3741,23 @@ int ble_gattc_get_db(uint16_t conn_handle,
                      uint16_t start_handle, uint16_t end_handle,
                      ble_gattc_db_elem_t *result, uint16_t *count)
 {
+    if (result == NULL || count == NULL) {
+        if (count != NULL) {
+            *count = 0;
+        }
+        return BLE_HS_EINVAL;
+    }
+
     uint16_t num = 0;
     ble_gattc_db_elem_t *db = NULL;
 
     ble_gattc_get_cached_gatt_db(conn_handle, start_handle, end_handle, &db, &num, count);
 
     if (num == 0) {
+        if (db != NULL) {
+            nimble_platform_mem_free(db);
+        }
+        *count = 0;
         return BLE_ATT_ERR_ATTR_NOT_FOUND;
     }
     if (db) {
@@ -3917,11 +3988,10 @@ ble_gattc_read_uuid_rx_adata(struct ble_gattc_proc *proc,
     attr.offset = 0;
     attr.om = ble_hs_mbuf_from_flat(adata->value, adata->value_len);
     if (attr.om == NULL) {
-        rc = BLE_HS_ENOMEM;
+        rc = ble_gattc_read_uuid_cb(proc, BLE_HS_ENOMEM, 0, NULL);
     } else {
-        rc = 0;
+        rc = ble_gattc_read_uuid_cb(proc, 0, 0, &attr);
     }
-    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);
@@ -3957,6 +4027,8 @@ ble_gattc_read_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
 static int
 ble_gattc_read_uuid_tx(struct ble_gattc_proc *proc)
 {
+    ble_gattc_dbg_assert_proc_not_inserted(proc);
+
     return ble_att_clt_tx_read_type(proc->conn_handle, proc->cid,
                                     proc->read_uuid.start_handle,
                                     proc->read_uuid.end_handle,
@@ -3985,7 +4057,10 @@ ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle,
 
     ble_gattc_proc_prepare(proc, conn_handle, BLE_GATT_OP_READ_UUID);
 
-    ble_uuid_to_any(uuid, &proc->read_uuid.chr_uuid);
+    rc = ble_uuid_to_any(uuid, &proc->read_uuid.chr_uuid);
+    if (rc != 0) {
+        goto done;
+    }
     proc->read_uuid.start_handle = start_handle;
     proc->read_uuid.end_handle = end_handle;
     proc->read_uuid.cb = cb;
@@ -4146,6 +4221,7 @@ ble_gattc_read_long_rx_read_rsp(struct ble_gattc_proc *proc, int status,
     mtu = ble_att_mtu_by_cid(proc->conn_handle, proc->cid);
     if (mtu == 0) {
         /* No longer connected. */
+        ble_gattc_read_long_cb(proc, BLE_HS_ENOTCONN, 0, NULL);
         return BLE_HS_EDONE;
     }
 
@@ -4229,12 +4305,14 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status,
     for (i = 0; i < proc->read_mult.num_handles; i++) {
         attr[i].handle = proc->read_mult.handles[i];
         attr[i].offset = 0;
-        if (om == NULL || OS_MBUF_PKTLEN(*om) == 0) {
+        if (om == NULL || OS_MBUF_PKTLEN(*om) < 2) {
             continue;
         }
 
         *om = os_mbuf_pullup(*om, 2);
-        assert(*om);
+        if (!*om) {
+            break;
+        }
 
         attr_len = get_le16((*om)->om_data);
 
@@ -4260,9 +4338,6 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status,
         os_mbuf_adj(*om, attr_len);
     }
 
-    /*FIXME Testing assert */
-    assert(i == proc->read_mult.num_handles);
-
     proc->read_mult.cb_mult(proc->conn_handle,
                     ble_gattc_error(status, att_handle), &attr[0],
                     i,
@@ -4396,7 +4471,7 @@ ble_gattc_read_mult_internal(uint16_t conn_handle, const uint16_t *handles,
                              ble_gatt_attr_mult_fn *cb_mult,
                              void *cb_arg)
 {
-#if !MYNEWT_VAL(BLE_GATT_READ_MULT)
+#if !MYNEWT_VAL(BLE_GATT_READ_MULT) && !MYNEWT_VAL(BLE_GATT_READ_MULT_VAR)
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -4407,7 +4482,8 @@ ble_gattc_read_mult_internal(uint16_t conn_handle, const uint16_t *handles,
 
     STATS_INC(ble_gattc_stats, read_mult);
 
-    if (num_handles > MYNEWT_VAL(BLE_GATT_READ_MAX_ATTRS)) {
+    if (num_handles < 2 ||
+        num_handles > MYNEWT_VAL(BLE_GATT_READ_MAX_ATTRS)) {
         rc = BLE_HS_EINVAL;
         goto done;
     }
@@ -4448,8 +4524,12 @@ 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_gattc_read_mult_internal(conn_handle, handles,
                                         num_handles, false, cb, NULL, cb_arg);
+#else
+    return BLE_HS_ENOTSUP;
+#endif
 }
 
 int
@@ -4474,6 +4554,7 @@ 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)
+    os_mbuf_free_chain(txom);
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -4562,8 +4643,11 @@ ble_gattc_signed_write(uint16_t conn_handle, uint16_t attr_handle,
     /* Converting the csrk to little endian */
     swap_buf(csrk, value_sec.csrk, 16);
 
-    rc = ble_att_clt_tx_signed_write_cmd(conn_handle, BLE_L2CAP_CID_ATT, attr_handle,
-                                         csrk, value_sec.sign_counter, txom);
+    rc = ble_att_clt_tx_signed_write_cmd(conn_handle, BLE_L2CAP_CID_ATT,
+                                         attr_handle, csrk,
+                                         value_sec.sign_counter, txom);
+    /* callee frees txom on both success and error paths */
+    txom = NULL;
     if (rc != 0) {
         goto err;
     }
@@ -4645,6 +4729,7 @@ 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)
+    os_mbuf_free_chain(txom);
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -4669,6 +4754,10 @@ ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle,
     proc->write.cb_arg = cb_arg;
 #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR)
     proc->write.om = os_mbuf_dup(txom);
+    if (proc->write.om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
 #endif
 
     ble_gattc_log_write(attr_handle, OS_MBUF_PKTLEN(txom), 1);
@@ -4832,6 +4921,10 @@ ble_gattc_write_long_resume(struct ble_gattc_proc *proc)
     status = ble_gattc_write_long_tx(proc);
     rc = ble_gattc_process_resume_status(proc, status);
     if (rc != 0) {
+        if (proc->write_long.attr.offset > 0) {
+            ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid,
+                                      BLE_ATT_EXEC_WRITE_F_CANCEL);
+        }
         ble_gattc_write_long_cb(proc, rc, 0);
         return rc;
     }
@@ -4876,6 +4969,8 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc,
 {
     struct os_mbuf *om;
     int rc;
+    bool exec_canceled = false;
+    bool prepare_acked = false;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
@@ -4886,6 +4981,7 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc,
         rc = status;
         goto err;
     }
+    prepare_acked = true;
 
     /* Verify the response. */
     if (proc->write_long.attr.offset >=
@@ -4924,20 +5020,24 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc,
         /* if data doesn't match up send cancel write */
         ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid,
                                   BLE_ATT_EXEC_WRITE_F_CANCEL);
+        exec_canceled = true;
         goto err;
     } else {
         /* 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 BLE_HS_EDONE;
         }
 
         return 0;
     }
 
 err:
-    /* XXX: Might need to cancel pending writes. */
+    if (!exec_canceled && (prepare_acked || proc->write_long.attr.offset > 0)) {
+        ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid,
+                                  BLE_ATT_EXEC_WRITE_F_CANCEL);
+    }
     ble_gattc_write_long_cb(proc, rc, 0);
     return BLE_HS_EDONE;
 }
@@ -4957,7 +5057,8 @@ ble_gattc_write_long_rx_exec(struct ble_gattc_proc *proc, int status)
         /* Expecting an execute write response, not a prepare write
          * response.
          */
-        return BLE_HS_EBADDATA;
+        ble_gattc_write_long_cb(proc, BLE_HS_EBADDATA, 0);
+        return BLE_HS_EDONE;
     }
 
     ble_gattc_write_long_cb(proc, status, 0);
@@ -4970,12 +5071,17 @@ ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle,
                      ble_gatt_attr_fn *cb, void *cb_arg)
 {
 #if !MYNEWT_VAL(BLE_GATT_WRITE_LONG)
+    os_mbuf_free_chain(txom);
     return BLE_HS_ENOTSUP;
 #endif
 
     struct ble_gattc_proc *proc;
     int rc;
 
+    if (txom == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
     STATS_INC(ble_gattc_stats, write_long);
 
     proc = ble_gattc_proc_alloc();
@@ -5079,7 +5185,7 @@ ble_gattc_write_reliable_tx(struct ble_gattc_proc *proc)
 {
     struct ble_gatt_attr *attr;
     struct os_mbuf *om;
-    uint16_t max_sz;
+    int max_sz;
     int attr_idx;
     int rc;
 
@@ -5157,16 +5263,20 @@ 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.
+     * we could send the execute write command, send cancel before notifying
+     * the application, matching the pattern of ble_gattc_write_long_err.
      */
-    if (proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) {
-
+    if (proc->write_reliable.cur_attr < proc->write_reliable.num_attrs &&
+        (proc->write_reliable.cur_attr > 0 ||
+         proc->write_reliable.attrs[proc->write_reliable.cur_attr].offset > 0 ||
+         status == BLE_HS_EBADDATA)) {
         ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid,
                                   BLE_ATT_EXEC_WRITE_F_CANCEL);
     }
+
+    ble_gattc_write_reliable_cb(proc, status, att_handle);
 }
 
 /**
@@ -5244,7 +5354,13 @@ 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);
+
+    if (status == 0 &&
+        proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) {
+        ble_gattc_write_reliable_cb(proc, BLE_HS_EBADDATA, 0);
+    } else {
+        ble_gattc_write_reliable_cb(proc, status, 0);
+    }
     return BLE_HS_EDONE;
 }
 
@@ -5266,7 +5382,7 @@ ble_gattc_write_reliable(uint16_t conn_handle,
 
     STATS_INC(ble_gattc_stats, write_reliable);
 
-    if (num_attrs > MYNEWT_VAL(BLE_GATT_WRITE_MAX_ATTRS)) {
+    if (num_attrs <= 0 || num_attrs > MYNEWT_VAL(BLE_GATT_WRITE_MAX_ATTRS)) {
         rc = BLE_HS_EINVAL;
         goto done;
     }
@@ -5336,6 +5452,9 @@ ble_gatts_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle,
                         struct os_mbuf *txom)
 {
 #if !MYNEWT_VAL(BLE_GATT_NOTIFY)
+    if (txom != NULL) {
+        os_mbuf_free_chain(txom);
+    }
     return BLE_HS_ENOTSUP;
 #endif
 #if MYNEWT_VAL(BLE_GATT_CACHING)
@@ -5372,8 +5491,6 @@ ble_gatts_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle,
         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;
         }
     }
@@ -5462,6 +5579,7 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle,
     if (peer_supports_multi_notify == 0) {
         for (i = 0; i < chr_count; i++) {
             rc = ble_att_clt_tx_notify(conn_handle, tuples[i].handle, tuples[i].value);
+            tuples[i].value = NULL; /* callee always consumes mbuf */
             if (rc != 0) {
                 goto done;
             }
@@ -5471,14 +5589,32 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle,
 
     for (i = 0; i < chr_count; i++) {
         if (OS_MBUF_PKTLEN(txom) + OS_MBUF_PKTLEN(tuples[i].value) > mtu && cur_chr_cnt < 2) {
+            if (cur_chr_cnt == 1) {
+                os_mbuf_adj(txom, sizeof(uint16_t) + sizeof(uint16_t));
+                rc = ble_att_clt_tx_notify(conn_handle,
+                                           tuples[last_appended_idx].handle,
+                                           txom);
+                txom = NULL;
+                if (rc != 0) {
+                    goto done;
+                }
+                cur_chr_cnt = 0;
+                txom = ble_hs_mbuf_att_pkt();
+                if (txom == NULL) {
+                    rc = BLE_HS_ENOMEM;
+                    goto done;
+                }
+            }
             rc = ble_att_clt_tx_notify(conn_handle, tuples[i].handle,
                                        tuples[i].value);
+            tuples[i].value = NULL; /* callee always consumes mbuf */
             if (rc != 0) {
                 goto done;
             }
             continue;
         } else if (OS_MBUF_PKTLEN(txom) + OS_MBUF_PKTLEN(tuples[i].value) > mtu) {
             rc = ble_att_clt_tx_notify_mult(conn_handle, txom);
+            txom = NULL; /* callee always consumes txom */
             if (rc != 0) {
                 goto done;
             }
@@ -5500,16 +5636,24 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle,
 
         /* Value */
         os_mbuf_concat(txom, tuples[i].value);
+        tuples[i].value = NULL; /* txom now owns this mbuf */
         cur_chr_cnt++;
         last_appended_idx = i;  /* Track the last appended index */
     }
 
     if (cur_chr_cnt == 1) {
-        /* Use the last appended index, not chr_count which may be out of bounds */
+        /* Strip the handle+length header prepended in the loop; the
+         * remaining data is the attribute value, which ble_att_clt_tx_notify
+         * expects.  tuples[last_appended_idx].value is NULL (cleared in the
+         * loop after os_mbuf_concat) so we cannot reuse it here.
+         */
+        os_mbuf_adj(txom, sizeof(uint16_t) + sizeof(uint16_t));
         rc = ble_att_clt_tx_notify(conn_handle, tuples[last_appended_idx].handle,
-                                   tuples[last_appended_idx].value);
-    } else {
+                                   txom);
+        txom = NULL;
+    } else if (cur_chr_cnt > 1) {
         rc = ble_att_clt_tx_notify_mult(conn_handle, txom);
+        txom = NULL;
     }
 
 done:
@@ -5529,7 +5673,9 @@ done:
         }
     }
 
-    os_mbuf_free_chain(txom);
+    if (txom != NULL) {
+        os_mbuf_free_chain(txom);
+    }
     /* Tell the application that multiple notification transmissions were attempted. */
     for (i = 0; i < chr_count; i++) {
         ble_gap_notify_tx_event(rc, conn_handle, tuples[i].handle, 0);
@@ -5546,7 +5692,12 @@ int
 ble_gattc_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle,
                         struct os_mbuf *txom)
 {
+#if MYNEWT_VAL(BLE_GATTS)
     return ble_gatts_notify_custom(conn_handle, chr_val_handle, txom);
+#else
+    os_mbuf_free_chain(txom);
+    return BLE_HS_ENOTSUP;
+#endif
 }
 
 #if MYNEWT_VAL(BLE_GATTC)
@@ -5593,6 +5744,7 @@ static int ble_gattc_cccd_register_cb(uint16_t conn_handle, const struct ble_gat
                                       ble_gattc_cccd_write_complete_cb, NULL);
             if (rc != 0) {
                 BLE_HS_LOG(WARN, "Failed to Register for Notification, status = %d", rc);
+                rc = 0;
             }
         }
      } else {
@@ -5663,6 +5815,7 @@ static int ble_gattc_cccd_unregister_cb(uint16_t conn_handle, const struct ble_g
                                       ble_gattc_cccd_write_complete_cb, NULL);
             if (rc != 0) {
                 BLE_HS_LOG(WARN, "Failed to Unregister for Notification, status = %d", rc);
+                rc = 0;
             }
         }
     } else {
@@ -5820,6 +5973,7 @@ ble_gatts_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle,
                           struct os_mbuf *txom)
 {
 #if !MYNEWT_VAL(BLE_GATT_INDICATE)
+    os_mbuf_free_chain(txom);
     return BLE_HS_ENOTSUP;
 #endif
 
@@ -5858,6 +6012,20 @@ ble_gatts_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle,
         }
     }
 #endif
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        ble_hs_unlock();
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+    if (conn->bhc_gatt_svr.indicate_val_handle != 0) {
+        ble_hs_unlock();
+        rc = BLE_HS_EALREADY;
+        goto done;
+    }
+    ble_hs_unlock();
+
     if (txom == NULL) {
         /* No custom attribute data; read the value from the specified
          * attribute.
@@ -5871,9 +6039,6 @@ ble_gatts_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle,
         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;
         }
     }
@@ -5948,14 +6113,14 @@ ble_gattc_rx_err(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t s
     struct ble_gattc_proc *proc;
     ble_gattc_err_fn *err_cb;
 #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR)
-    struct ble_gap_conn_desc desc;
+    struct ble_gap_conn_desc desc = {0};
     int rc;
 #endif
 
     proc = ble_gattc_extract_first_by_conn_cid_op(conn_handle, cid, BLE_GATT_OP_NONE);
 #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR)
     if (proc != NULL) {
-        ble_gap_conn_find(conn_handle, &desc);
+        rc = ble_gap_conn_find(conn_handle, &desc);
         proc->error.att_handle = handle;
         proc->error.status = status;
     }
@@ -5963,7 +6128,7 @@ ble_gattc_rx_err(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t s
 
     if (proc != NULL) {
 #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR)
-        if (proc != NULL && desc.sec_state.encrypted == 0 &&
+        if (proc != NULL && rc == 0 && desc.sec_state.encrypted == 0 &&
             (status == BLE_ATT_ERR_INSUFFICIENT_ENC ||
              status == BLE_ATT_ERR_INSUFFICIENT_AUTHEN)) {
             rc = ble_gap_security_initiate(conn_handle);
@@ -6238,7 +6403,7 @@ void
 ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, uint16_t cid, int status,
                            struct os_mbuf **om, bool variable)
 {
-#if !NIMBLE_BLE_ATT_CLT_READ_MULT
+#if !NIMBLE_BLE_ATT_CLT_READ_MULT && !NIMBLE_BLE_ATT_CLT_READ_MULT_VAR
     return;
 #endif
 
@@ -6404,6 +6569,10 @@ ble_gattc_err_dispatch_init(void)
         return BLE_HS_EINVAL;
     }
 
+    if (ble_gattc_err_dispatch != NULL) {
+        return 0;
+    }
+
     ble_gattc_err_dispatch =  nimble_platform_mem_calloc(1, BLE_GATT_OP_CNT * sizeof(ble_gattc_err_fn *));
 
     if (!ble_gattc_err_dispatch) {
@@ -6456,6 +6625,10 @@ ble_gattc_resume_dispatch_init(void)
         return BLE_HS_EINVAL;
     }
 
+    if (ble_gattc_resume_dispatch != NULL) {
+        return 0;
+    }
+
     ble_gattc_resume_dispatch =  nimble_platform_mem_calloc(1, BLE_GATT_OP_CNT * sizeof(ble_gattc_resume_fn *));
 
     if (!ble_gattc_resume_dispatch) {

nimble/host/src/ble_gattc_cache.c

@@ -28,7 +28,7 @@
 #include "host/ble_gatt.h"
 #include "esp_nimble_mem.h"
 #include "host/ble_hs_log.h"
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+#if MYNEWT_VAL(BLE_GATT_CACHING)
 #include "nvs.h"
 #endif
 
@@ -138,8 +138,21 @@ cacheEraseItem(cache_handle_t handle, const char *key)
 static int
 cacheErase(cache_handle_t handle)
 {
+    int rc;
+
     if (cache_fn.erase_all) {
-        return cache_fn.erase_all(handle);
+        rc = cache_fn.erase_all(handle);
+        if (rc != 0) {
+            return rc;
+        }
+
+        if (cache_fn.commit) {
+            rc = cache_fn.commit(handle);
+            if (rc != 0) {
+                return rc;
+            }
+        }
+        return 0;
     }
     return -1;
 }
@@ -147,8 +160,21 @@ cacheErase(cache_handle_t handle)
 static int
 cacheWrite(cache_handle_t handle, const char * key, const void* value, size_t length)
 {
+    int rc;
+
     if (cache_fn.write) {
-        return cache_fn.write(handle, key, value, length);
+        rc = cache_fn.write(handle, key, value, length);
+        if (rc != 0) {
+            return rc;
+        }
+
+        if (cache_fn.commit) {
+            rc = cache_fn.commit(handle);
+            if (rc != 0) {
+                return rc;
+            }
+        }
+        return 0;
     }
     return -1;
 }
@@ -247,6 +273,10 @@ ble_gattc_cacheReset(ble_addr_t *addr)
         /* Reduced the number address counter also */
         cache_env->num_addr--;
 
+        /* Clear the stale trailing element to prevent is_open/cache_fp reuse */
+        memset(&cache_env->cache_addr[cache_env->num_addr], 0,
+               sizeof(cache_addr_info_t));
+
         /* Update addr list to storage flash */
         if (cache_env->num_addr > 0) {
             uint8_t *p_buf = nimble_platform_mem_calloc(1,MAX_ADDR_LIST_CACHE_BUF);
@@ -320,10 +350,7 @@ ble_gattc_cache_find_addr(ble_addr_t addr)
 #endif
 
     if (cache_env == NULL) {
-        cache_env = nimble_platform_mem_calloc(1, sizeof(cache_env_t));
-        if (cache_env == NULL) {
-            return INVALID_ADDR_NUM;
-        }
+        return INVALID_ADDR_NUM;
     }
 
     uint8_t num = cache_env->num_addr;
@@ -340,13 +367,25 @@ ble_gattc_cache_find_addr(ble_addr_t addr)
 
 void ble_gattc_cache_get_addr_list(ble_addr_t *addr_list, uint8_t *out_num)
 {
-    uint8_t num = cache_env->num_addr;
+    uint8_t num;
 
     if (addr_list == NULL || out_num == NULL) {
         BLE_HS_LOG(WARN, "Invalid input to ble_gattc_cache_get_addr_list.");
         return;
     }
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gattc_cache_static_vars == NULL || cache_env == NULL) {
+#else
+    if (cache_env == NULL) {
+#endif
+        BLE_HS_LOG(WARN, "ble_gattc_cache_get_addr_list: cache not initialized.");
+        *out_num = 0;
+        return;
+    }
+
+    num = cache_env->num_addr;
+
     for (uint8_t i = 0; i < num; i++) {
         memcpy(&addr_list[i], &cache_env->cache_addr[i].addr, sizeof(ble_addr_t));
     }
@@ -578,7 +617,8 @@ ble_gattc_cache_addr_save(uint8_t *out_index, ble_addr_t addr, uint8_t * hash_ke
 static int
 handle_compare(const void *s1, const void *s2)
 {
-    return ((struct ble_gatt_nv_attr *)s1)->s_handle - ((struct ble_gatt_nv_attr *)s2)->s_handle;
+    return ((const struct ble_gatt_nv_attr *)s1)->s_handle -
+           ((const struct ble_gatt_nv_attr *)s2)->s_handle;
 }
 
 static void
@@ -646,16 +686,31 @@ ble_gattc_cache_load_nv_attr(uint8_t index, int *num_attr)
         return NULL;
     }
 
+    if (length % sizeof(ble_gatt_nv_attr) != 0) {
+        BLE_HS_LOG(ERROR, "Cache data length %zu not aligned to attr size %zu",
+                   length, sizeof(ble_gatt_nv_attr));
+        return NULL;
+    }
+
     *num_attr = length / (sizeof(ble_gatt_nv_attr));
+    if (*num_attr == 0) {
+        return NULL;
+    }
 
     nv_attr = (struct ble_gatt_nv_attr *) nimble_platform_mem_calloc(1,(*num_attr) * sizeof(struct ble_gatt_nv_attr));
     if (nv_attr == NULL) {
         return NULL;
     }
 
+    /* Pass the actual allocated size to prevent heap overflow */
+    length = (size_t)(*num_attr) * sizeof(struct ble_gatt_nv_attr);
     rc = cacheRead(cache_env->cache_addr[index].cache_fp, getKeyname(&cache_env->cache_addr[index].addr), nv_attr, &length);
 
     BLE_HS_LOG(INFO, "%s, rc = %d, length = %d index = %d", __func__, rc, length, index);
+    if (rc != 0) {
+        nimble_platform_mem_free(nv_attr);
+        return NULL;
+    }
     return nv_attr;
 }
 
@@ -761,6 +816,7 @@ ble_gattc_cache_assoc_load(ble_addr_t src_addr, uint8_t src_index, ble_addr_t as
     nv_attr = ble_gattc_cache_load_nv_attr(src_index, &num_attr);
     if (nv_attr == NULL) {
         BLE_HS_LOG(ERROR, "Failed to load nv_attr from source index %d", src_index);
+        cacheClose(src_addr);
         return BLE_HS_EINVAL;
     }
 
@@ -798,21 +854,30 @@ ble_gattc_cache_assoc_load(ble_addr_t src_addr, uint8_t src_index, ble_addr_t as
 
     BLE_HS_LOG(DEBUG, "Successfully associated cache from src_addr to assoc_addr.");
 
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
+    cacheClose(src_addr);
+    return rc;
 }
 
 int
 ble_gattc_cache_find_source(struct ble_gattc_cache_conn *cache_conn, uint8_t *database_hash)
 {
     uint8_t addr_index = 0;
-    uint8_t num = cache_env->num_addr;
-    cache_addr_info_t *addr_info = &cache_env->cache_addr[0];
+    uint8_t num;
+    cache_addr_info_t *addr_info;
     int rc = ESP_FAIL;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gattc_cache_static_vars == NULL || cache_env == NULL) {
+#else
+    if (cache_env == NULL) {
+#endif
+        ble_gap_assoc_event(cache_conn->conn_handle, rc, cache_conn->cache_state);
+        return rc;
+    }
+
+    num = cache_env->num_addr;
+    addr_info = &cache_env->cache_addr[0];
+
     /* Iterate through all cached addresses to find a matching database hash */
     for (addr_index = 0; addr_index < num; addr_index++, addr_info++) {
         /* Compare stored hash with the provided database_hash */
@@ -844,14 +909,12 @@ ble_gattc_cache_load(ble_addr_t peer_addr)
 
     if (!cacheOpen(peer_addr, true, &index)) {
         BLE_HS_LOG(INFO, "gattc cache open fail");
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
 
     if ((nv_attr = ble_gattc_cache_load_nv_attr(index, &num_attr)) == NULL) {
         BLE_HS_LOG(INFO, "%s, gattc cache nv_attr load fail", __func__);
         cacheClose(peer_addr);
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
 
@@ -948,7 +1011,6 @@ ble_gattc_cache_init(void *storage_cb)
     uint8_t *p_buf = NULL;
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-   int no_cached_blob = 0;
    rc = ble_gattc_cache_static_vars_init();
    if (rc != 0) {
     return rc;
@@ -985,18 +1047,15 @@ ble_gattc_cache_init(void *storage_cb)
             if ((rc = cacheRead(fp, cache_key, p_buf, &length)) != 0) {
                 BLE_HS_LOG(DEBUG, "%s, Line = %d, storage flash get blob data fail, err_code = 0x%x",
                            __func__, __LINE__, rc);
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
                 /*
                  * This is expected scenario during first boot or
                  * when no GATT cache exists yet and should not
                  * be treated as a fatal error.
                  */
-                if (rc == ESP_ERR_NVS_NOT_FOUND){
-                    no_cached_blob = 1;
+                if (rc == ESP_ERR_NVS_NOT_FOUND) {
                     nimble_platform_mem_free(p_buf);
                     return 0;
                 }
-#endif
                 goto error;
             }
 
@@ -1038,9 +1097,6 @@ error:
         nimble_platform_mem_free(p_buf);
         p_buf = NULL;
     }
-    if (cache_env && cache_env->is_open && cache_fn.close) {
-        cache_fn.close(cache_env->addr_fp);
-    }
     if (cache_env) {
         /* Close NVS handle if it was opened before freeing cache_env */
         if (cache_env->is_open && cache_fn.close) {
@@ -1052,7 +1108,7 @@ error:
     }
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    if (ble_gattc_cache_static_vars && !no_cached_blob) {
+    if (ble_gattc_cache_static_vars) {
         nimble_platform_mem_free(ble_gattc_cache_static_vars);
         ble_gattc_cache_static_vars = NULL;
     }

nimble/host/src/ble_gattc_cache_conn.c

@@ -44,7 +44,6 @@
     switch(cache_state) { \
     case SVC_DISC_IN_PROGRESS: \
         if((void*)ble_gattc_cache_conn_svc_disced == cb) { \
-            BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \
             return BLE_HS_EINVAL; \
         } \
         ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \
@@ -52,7 +51,6 @@
         return 0; \
     case CHR_DISC_IN_PROGRESS: \
        if((void*)ble_gattc_cache_conn_chr_disced == cb) { \
-           BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \
            return BLE_HS_EINVAL; \
        } \
         ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \
@@ -60,7 +58,6 @@
         return 0; \
     case INC_DISC_IN_PROGRESS: \
        if((void *)ble_gattc_cache_conn_inc_disced == cb) { \
-           BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \
            return BLE_HS_EINVAL; \
         } \
         ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \
@@ -68,7 +65,6 @@
         return 0; \
     case DSC_DISC_IN_PROGRESS: \
        if((void*)ble_gattc_cache_conn_dsc_disced == cb) { \
-           BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \
            return BLE_HS_EINVAL; \
        } \
         ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \
@@ -175,10 +171,13 @@ int
 ble_gattc_cache_conn_chr_is_empty(const struct ble_gattc_cache_conn_svc *svc, const struct ble_gattc_cache_conn_chr *chr);
 
 static struct ble_gattc_cache_conn_chr *
-ble_gattc_cache_conn_chr_find(const struct ble_gattc_cache_conn_svc *svc, uint16_t chr_def_handle,
+ble_gattc_cache_conn_chr_find(const struct ble_gattc_cache_conn_svc *svc, uint16_t chr_val_handle,
                               struct ble_gattc_cache_conn_chr **out_prev);
 
 #if MYNEWT_VAL(BLE_GATTC)
+static int
+ble_gattc_cache_conn_disc(struct ble_gattc_cache_conn *peer);
+
 static void
 ble_gattc_cache_conn_disc_chrs(struct ble_gattc_cache_conn *ble_gattc_cache_conn);
 
@@ -187,6 +186,12 @@ ble_gattc_cache_conn_disc_incs(struct ble_gattc_cache_conn *ble_gattc_cache_conn
 
 static void
 ble_gattc_cache_conn_disc_dscs(struct ble_gattc_cache_conn *peer);
+
+static void
+ble_gattc_cache_conn_pending_op_fail(struct ble_gattc_cache_conn *peer, int status);
+
+static struct ble_gatt_error *
+ble_gattc_cache_error(int status, uint16_t att_handle);
 #endif
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
@@ -234,6 +239,11 @@ struct ble_gattc_cache_conn *
 ble_gattc_cache_conn_find_by_addr(ble_addr_t peer_addr)
 {
     struct ble_gattc_cache_conn *ble_gattc_cache_conn;
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gattc_cache_conn_static_vars == NULL) {
+        return NULL;
+    }
+#endif
     SLIST_FOREACH(ble_gattc_cache_conn, &ble_gattc_cache_conns, next) {
         if (memcmp(&ble_gattc_cache_conn->ble_gattc_cache_conn_addr, &peer_addr, sizeof(peer_addr)) == 0) {
             return ble_gattc_cache_conn;
@@ -377,7 +387,11 @@ ble_gattc_cache_conn_chr_end_handle(const struct ble_gattc_cache_conn_svc *svc,
 
     next_chr = SLIST_NEXT(chr, next);
     if (next_chr != NULL) {
-        return next_chr->chr.def_handle - 1;
+        uint16_t end = next_chr->chr.def_handle - 1;
+        if (end < chr->chr.val_handle) {
+            end = chr->chr.val_handle;
+        }
+        return end;
     } else {
         return svc->svc.end_handle;
     }
@@ -471,7 +485,7 @@ ble_gattc_cache_conn_db_hash_read(uint16_t conn_handle,
 }
 #endif
 
-const struct ble_gattc_cache_conn_svc *
+static const struct ble_gattc_cache_conn_svc *
 ble_gattc_cache_conn_svc_find_uuid(const struct ble_gattc_cache_conn *ble_gattc_cache_conn, const ble_uuid_t *uuid)
 {
     const struct ble_gattc_cache_conn_svc *svc;
@@ -536,7 +550,7 @@ ble_gattc_cache_conn_chr_add(ble_addr_t peer_addr, uint16_t svc_start_handle,
         return BLE_HS_EUNKNOWN;
     }
 
-    chr = ble_gattc_cache_conn_chr_find(svc, gatt_chr->def_handle, &prev);
+    chr = ble_gattc_cache_conn_chr_find(svc, gatt_chr->val_handle, &prev);
     if (chr != NULL) {
         /* Characteristic already discovered. */
         return 0;
@@ -922,7 +936,9 @@ ble_gattc_get_db_size_with_handle(struct ble_gattc_cache_conn *peer, uint16_t st
             break;
         }
 
-        db_size++;
+        if (svc->svc.start_handle >= start_handle) {
+            db_size++;
+        }
 #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
         SLIST_FOREACH(included_svc, &svc->incl_svc, next) {
             if (included_svc->svc.handle < start_handle) {
@@ -985,16 +1001,14 @@ ble_gattc_get_db_size_with_type(struct ble_gattc_cache_conn *peer, uint8_t type,
         switch (type) {
             case BLE_GATT_DB_PRIMARY_SERVICE:
                 if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY &&
-                    svc->svc.start_handle >= start_handle &&
-                    svc->svc.end_handle <= end_handle) {
+                    svc->svc.start_handle >= start_handle) {
                     db_size++;
                 }
                 break;
 
             case BLE_GATT_DB_SECONDARY_SERVICE:
                 if (svc->type == BLE_GATT_SVC_TYPE_SECONDARY &&
-                    svc->svc.start_handle >= start_handle &&
-                    svc->svc.end_handle <= end_handle) {
+                    svc->svc.start_handle >= start_handle) {
                     db_size++;
                 }
                 break;
@@ -1011,7 +1025,7 @@ ble_gattc_get_db_size_with_type(struct ble_gattc_cache_conn *peer, uint8_t type,
             case BLE_GATT_DB_CHARACTERISTIC:
                 // Iterate through characteristics.
                 SLIST_FOREACH(chr, &svc->chrs, next) {
-                      if (chr->chr.def_handle >= start_handle && chr->chr.def_handle <= end_handle) { //Updated
+                      if (chr->chr.val_handle >= start_handle && chr->chr.val_handle <= end_handle) {
                           db_size++;
                       }
                 }
@@ -1266,6 +1280,10 @@ void ble_gattc_get_db_size_with_type_handle(uint16_t conn_handle,
         *count = 0;
         return ;
     }
+    if (cache_conn->cache_state != CACHE_VERIFIED) {
+        *count = 0;
+        return;
+    }
 
     *count =  ble_gattc_get_db_size_with_type(cache_conn, type, start_handle, end_handle, char_handle);
 }
@@ -1282,6 +1300,10 @@ void ble_gattc_get_db_size_handle(uint16_t conn_handle,
         *count = 0;
         return;
     }
+    if (cache_conn->cache_state != CACHE_VERIFIED) {
+        *count = 0;
+        return;
+    }
     *count = ble_gattc_get_db_size_with_handle(cache_conn, start_handle, end_handle);
 }
 
@@ -1317,6 +1339,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
     }
 
     ble_gattc_db_elem_t *curr_db_attr = buffer;
+    size_t written = 0;
     struct ble_gattc_cache_conn_svc *svc;
 #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
     struct ble_gattc_cache_conn_incl_svc *included_svc;
@@ -1333,16 +1356,22 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
         if (svc->svc.start_handle > end_handle) {
             break;
         }
-        ble_gattc_fill_gatt_db_el(curr_db_attr,
-                                  svc->type == BLE_GATT_SVC_TYPE_PRIMARY ? /* attr type */
-                                  BLE_GATT_DB_PRIMARY_SERVICE:
-                                  BLE_GATT_DB_SECONDARY_SERVICE,
-                                  0,                                      /* attr handle */
-                                  svc->svc.start_handle,                  /* start handle*/
-                                  svc->svc.end_handle,                    /* end handle  */
-                                  0,                                      /* property    */
-                                  svc->svc.uuid);                         /* uuid        */
-        curr_db_attr++;
+        if (written >= db_size) {
+            break;
+        }
+        if (svc->svc.start_handle >= start_handle) {
+            ble_gattc_fill_gatt_db_el(curr_db_attr,
+                                      svc->type == BLE_GATT_SVC_TYPE_PRIMARY ? /* attr type */
+                                      BLE_GATT_DB_PRIMARY_SERVICE:
+                                      BLE_GATT_DB_SECONDARY_SERVICE,
+                                      0,                                      /* attr handle */
+                                      svc->svc.start_handle,                  /* start handle*/
+                                      svc->svc.end_handle,                    /* end handle  */
+                                      0,                                      /* property    */
+                                      svc->svc.uuid);                         /* uuid        */
+            curr_db_attr++;
+            written++;
+        }
 #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
         // Iterate over included services
         SLIST_FOREACH(included_svc, &svc->incl_svc, next) {
@@ -1352,10 +1381,13 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
 
             if (included_svc->svc.handle > end_handle) {
                 *db = buffer;
-                *count = db_size;
+                *count = (uint16_t)written;
                 return;
             }
 
+            if (written >= db_size) {
+                break;
+            }
             ble_gattc_fill_gatt_db_el(curr_db_attr,
                                       BLE_GATT_DB_INCLUDED_SERVICE,    /* attr type   */
                                       included_svc->svc.handle,        /* attr handle */
@@ -1364,6 +1396,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
                                       0,                               /* property    */
                                       included_svc->svc.uuid);         /* uuid        */
             curr_db_attr++;
+            written++;
         }
 #endif
         // Iterate over characterstic
@@ -1374,9 +1407,12 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
 
             if (chr->chr.val_handle > end_handle) {
                 *db = buffer;
-                *count = db_size;
+                *count = (uint16_t)written;
                 return;
             }
+            if (written >= db_size) {
+                break;
+            }
             ble_gattc_fill_gatt_db_el(curr_db_attr,
                                       BLE_GATT_DB_CHARACTERISTIC,     /* attr type   */
                                       chr->chr.val_handle,            /* attr handle */
@@ -1385,6 +1421,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
                                       chr->chr.properties,            /* property    */
                                       chr->chr.uuid);                 /* uuid        */
               curr_db_attr++;
+              written++;
                // Iterate over descriptors
                SLIST_FOREACH(dsc, &chr->dscs, next) {
                     if (dsc->dsc.handle < start_handle) {
@@ -1393,9 +1430,12 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
 
                     if (dsc->dsc.handle > end_handle) {
                         *db = buffer;
-                        *count = db_size;
+                        *count = (uint16_t)written;
                         return;
                     }
+                    if (written >= db_size) {
+                        break;
+                    }
                     ble_gattc_fill_gatt_db_el(curr_db_attr,
                                               BLE_GATT_DB_DESCRIPTOR,   /* attr type   */
                                               dsc->dsc.handle,          /* attr handle */
@@ -1404,12 +1444,13 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer,
                                               0,                        /* property    */
                                               dsc->dsc.uuid);           /* uuid        */
                     curr_db_attr++;
+                    written++;
               }
         }
     }
 
     *db = buffer;
-    *count = db_size;
+    *count = (uint16_t)written;
 
     return;
 }
@@ -1465,6 +1506,7 @@ ble_gattc_cache_conn_broken(uint16_t conn_handle)
 {
     struct ble_gattc_cache_conn_svc *svc;
     struct ble_gattc_cache_conn *conn;
+    bool was_queued;
 
     conn = ble_gattc_cache_conn_find(conn_handle);
     if (conn == NULL) {
@@ -1475,6 +1517,25 @@ ble_gattc_cache_conn_broken(uint16_t conn_handle)
     /* clean the cache_conn */
     SLIST_REMOVE(&ble_gattc_cache_conns, conn, ble_gattc_cache_conn, next);
 
+    /* Remove any pending discovery event to prevent UAF after conn is freed.
+     * Use portable NPL API — disc_ev.event is a struct on esp-idf port, not a
+     * pointer, so direct != NULL comparison does not compile there. */
+    was_queued = ble_npl_event_is_queued(&conn->disc_ev);
+    if (was_queued) {
+        ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(),
+                              &conn->disc_ev);
+    }
+    if (conn->pending_op.cb != NULL &&
+        (was_queued || conn->cache_state != CACHE_VERIFIED)) {
+#if MYNEWT_VAL(BLE_GATTC)
+        ble_gattc_cache_conn_pending_op_fail(conn, BLE_HS_ENOTCONN);
+#endif
+    }
+    /* Always deinit to free the internal FreeRTOS event allocation even when
+     * the event was already dispatched (no longer queued). deinit is a no-op
+     * if disc_ev was never initialized (ev->event == NULL). */
+    ble_npl_event_deinit(&conn->disc_ev);
+
     while ((svc = SLIST_FIRST(&conn->svcs)) != NULL) {
         SLIST_REMOVE_HEAD(&conn->svcs, next);
         ble_gattc_cache_conn_svc_delete(svc);
@@ -1575,6 +1636,71 @@ static void service_sanity_check(struct ble_gattc_cache_conn_svc_list *svcs)
     }
 }
 
+static void
+ble_gattc_cache_conn_pending_op_fail(struct ble_gattc_cache_conn *peer, int status)
+{
+    struct ble_gattc_cache_conn_op *op;
+    uint16_t conn_handle;
+    ble_gatt_disc_svc_fn *disc_svc_cb;
+#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
+    ble_gatt_disc_incl_svc_fn *disc_incl_svc_cb;
+#endif
+    ble_gatt_chr_fn *chr_cb;
+    ble_gatt_dsc_fn *dsc_cb;
+    void *cb_arg;
+    uint8_t cb_type;
+
+    op = &peer->pending_op;
+    if (op->cb == NULL) {
+        return;
+    }
+
+    conn_handle = peer->conn_handle;
+    cb_type = op->cb_type;
+    cb_arg = op->cb_arg;
+    disc_svc_cb = (ble_gatt_disc_svc_fn *)op->cb;
+#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
+    disc_incl_svc_cb = (ble_gatt_disc_incl_svc_fn *)op->cb;
+#endif
+    chr_cb = (ble_gatt_chr_fn *)op->cb;
+    dsc_cb = (ble_gatt_dsc_fn *)op->cb;
+    memset(op, 0, sizeof(*op));
+
+    switch (cb_type) {
+    case BLE_GATT_OP_DISC_ALL_SVCS:
+        disc_svc_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), NULL, cb_arg);
+        break;
+    case BLE_GATT_OP_DISC_SVC_UUID:
+        disc_svc_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), NULL, cb_arg);
+        break;
+    case BLE_GATT_OP_FIND_INC_SVCS:
+#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
+        disc_incl_svc_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), NULL, cb_arg);
+#else
+        disc_svc_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), NULL, cb_arg);
+#endif
+        break;
+    case BLE_GATT_OP_DISC_ALL_CHRS:
+        chr_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), NULL, cb_arg);
+        break;
+    case BLE_GATT_OP_DISC_CHR_UUID:
+        chr_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), NULL, cb_arg);
+        break;
+    case BLE_GATT_OP_DISC_ALL_DSCS:
+        dsc_cb(conn_handle,
+            ble_gattc_cache_error(status, 0), 0, NULL, cb_arg);
+        break;
+    default:
+        break;
+    }
+}
+
 static void
 ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc)
 {
@@ -1583,8 +1709,26 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc)
     const struct ble_gattc_cache_conn_chr *chr;
     bool bonded;
 
+    int rediscover;
+
     peer->disc_prev_chr_val = 0;
+    rediscover = peer->needs_rediscovery;
+    peer->needs_rediscovery = 0;
     if (rc == 0) {
+        if (rediscover) {
+            peer->cache_state = CACHE_INVALID;
+            if (!MYNEWT_VAL(BLE_GATT_CACHING_DISABLE_AUTO)) {
+                rc = ble_gattc_cache_conn_disc(peer);
+                if (rc == 0) {
+                    return;
+                }
+                peer->cache_state = CACHE_INVALID;
+            } else {
+                rc = BLE_HS_EAGAIN;
+            }
+            ble_gattc_cache_conn_pending_op_fail(peer, rc);
+            return;
+        }
         /* discovery complete */
         peer->cache_state = CACHE_VERIFIED;
         service_sanity_check(&peer->svcs);
@@ -1594,18 +1738,21 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc)
         ble_addr_t peer_addr;
         ble_hs_lock();
         hs_conn = ble_hs_conn_find(peer->conn_handle);
-        BLE_HS_DBG_ASSERT(hs_conn != NULL);
-        bonded = hs_conn->bhc_sec_state.bonded;
-        peer_addr = hs_conn->bhc_peer_addr; /* Copy address while holding lock */
-        ble_hs_unlock();
+        if (hs_conn != NULL) {
+            bonded = hs_conn->bhc_sec_state.bonded;
+            peer_addr = hs_conn->bhc_peer_addr; /* Copy address while holding lock */
+            ble_hs_unlock();
 
-        chr = ble_gattc_cache_conn_chr_find_uuid(peer,
-                                                 BLE_UUID16_DECLARE(BLE_GATT_SVC_UUID16),
-                                                 BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_DATABASE_HASH_UUID16));
-        if (bonded || chr != NULL) {
-            /* persist the cache */
-            ble_gattc_cacheReset(&peer_addr);
-            ble_gattc_cache_conn_cache_peer(peer); /* TODO */
+            chr = ble_gattc_cache_conn_chr_find_uuid(peer,
+                                                     BLE_UUID16_DECLARE(BLE_GATT_SVC_UUID16),
+                                                     BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_DATABASE_HASH_UUID16));
+            if (bonded || chr != NULL) {
+                /* persist the cache */
+                ble_gattc_cacheReset(&peer_addr);
+                ble_gattc_cache_conn_cache_peer(peer); /* TODO */
+            }
+        } else {
+            ble_hs_unlock();
         }
     } else {
         peer->cache_state = CACHE_INVALID;
@@ -1613,6 +1760,10 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc)
     /* respond to the pending gatt op */
     op = &peer->pending_op;
     if (op->cb) {
+        if (rc != 0) {
+            ble_gattc_cache_conn_pending_op_fail(peer, rc);
+            return;
+        }
         switch (op->cb_type) {
         case BLE_GATT_OP_DISC_ALL_SVCS :
             rc = ble_gattc_cache_conn_search_all_svcs(peer->conn_handle, op->cb, op->cb_arg);
@@ -1672,6 +1823,7 @@ ble_gattc_cache_conn_undisc_all(ble_addr_t peer_addr)
         SLIST_REMOVE_HEAD(&peer->svcs, next);
         ble_gattc_cache_conn_svc_delete(svc);
     }
+    peer->cur_svc = NULL;
 }
 
 #if MYNEWT_VAL(BLE_GATTC)
@@ -1765,6 +1917,10 @@ ble_gattc_cache_conn_disc(struct ble_gattc_cache_conn *peer)
     BLE_HS_LOG(INFO, "Initiating Remote Service Discovery");
     peer->cache_state = SVC_DISC_IN_PROGRESS;
     rc = ble_gattc_disc_all_svcs(peer->conn_handle, ble_gattc_cache_conn_svc_disced, peer);
+    if (rc != 0) {
+        peer->cache_state = CACHE_INVALID;
+        peer->disc_prev_chr_val = 0;
+    }
     return rc;
 }
 
@@ -1793,7 +1949,13 @@ ble_gattc_cache_conn_on_read(uint16_t conn_handle,
         ble_gattc_cache_conn_disc_complete((struct ble_gattc_cache_conn *)arg, res);
         return 0;
     } else {
-        res = ble_gattc_cache_conn_disc((struct ble_gattc_cache_conn *)arg);
+        struct ble_gattc_cache_conn *peer = arg;
+
+        peer->needs_rediscovery = 0;
+        res = ble_gattc_cache_conn_disc(peer);
+        if (res != 0) {
+            ble_gattc_cache_conn_disc_complete((struct ble_gattc_cache_conn *)arg, res);
+        }
         return res;
     }
 }
@@ -1813,6 +1975,11 @@ ble_gattc_cache_conn_create(uint16_t conn_handle, ble_addr_t ble_gattc_cache_con
         return 0;
     }
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gattc_cache_conn_static_vars == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+#endif
     cache_conn = os_memblock_get(&ble_gattc_cache_conn_pool);
     if (cache_conn == NULL) {
         /* Out of memory. */
@@ -1919,6 +2086,7 @@ ble_gattc_cache_conn_disc_dscs(struct ble_gattc_cache_conn *peer)
                                              ble_gattc_cache_conn_dsc_disced, peer);
                 if (rc != 0) {
                     ble_gattc_cache_conn_disc_complete(peer, rc);
+                    return;
                 }
 
                 peer->disc_prev_chr_val = chr->chr.val_handle;
@@ -1945,11 +2113,11 @@ ble_gattc_cache_conn_chr_disced(uint16_t conn_handle, const struct ble_gatt_erro
     case 0:
         rc = ble_gattc_cache_conn_chr_add(peer->ble_gattc_cache_conn_addr, peer->cur_svc->svc.start_handle, chr);
 
-        if (chr->uuid.u16.value == BLE_GATTC_DATABASE_HASH_UUID128) {
-            rc = ble_gattc_read(peer->conn_handle, chr->val_handle,
-                                ble_gattc_cache_conn_db_hash_read, peer);
-            if (rc != 0) {
-                BLE_HS_LOG(ERROR, "Failed to read Database Hash %d", rc);
+        if (rc == 0 && chr->uuid.u.type == BLE_UUID_TYPE_16 && chr->uuid.u16.value == BLE_GATTC_DATABASE_HASH_UUID128) {
+            int read_rc = ble_gattc_read(peer->conn_handle, chr->val_handle,
+                                         ble_gattc_cache_conn_db_hash_read, peer);
+            if (read_rc != 0) {
+                BLE_HS_LOG(ERROR, "Failed to read Database Hash %d", read_rc);
             }
         }
         break;
@@ -2019,8 +2187,8 @@ ble_gattc_cache_conn_disc_incs(struct ble_gattc_cache_conn *peer)
         if (peer->cur_svc == NULL) {
             if (peer->disc_prev_chr_val > 0) {
                 ble_gattc_cache_conn_disc_chrs(peer);
-                return;
             }
+            return;
         }
     }
     svc = peer->cur_svc;
@@ -2057,6 +2225,14 @@ ble_gattc_cache_conn_update(uint16_t conn_handle, uint16_t start_handle, uint16_
         return; //Update
     }
 
+    if (peer->cache_state == SVC_DISC_IN_PROGRESS ||
+        peer->cache_state == CHR_DISC_IN_PROGRESS ||
+        peer->cache_state == INC_DISC_IN_PROGRESS ||
+        peer->cache_state == DSC_DISC_IN_PROGRESS ||
+        peer->cache_state == VERIFY_IN_PROGRESS) {
+        peer->needs_rediscovery = 1;
+        return;
+    }
     peer->cache_state = CACHE_INVALID;
     if (MYNEWT_VAL(BLE_GATT_CACHING_DISABLE_AUTO)) {
 	    /* Do not automatically re-discover and correct cache */
@@ -2067,7 +2243,6 @@ ble_gattc_cache_conn_update(uint16_t conn_handle, uint16_t start_handle, uint16_
         peer->cache_state = CACHE_INVALID;
     }
 }
-#endif
 
 int ble_gattc_cache_refresh(ble_addr_t peer_addr)
 {
@@ -2130,8 +2305,10 @@ ble_gattc_cache_conn_assoc_on_read(uint16_t conn_handle,
         BLE_HS_LOG(WARN, "Failed to copy database hash from attr->om (rc=%d)", rc);
         return rc;
     }
-    rc =  ble_gattc_cache_find_source((struct ble_gattc_cache_conn *)arg, database_hash);
-
+    rc = ble_gattc_cache_find_source((struct ble_gattc_cache_conn *)arg, database_hash);
+    if (rc == 0) {
+        return BLE_HS_EDONE;
+    }
     return rc;
 }
 
@@ -2147,12 +2324,14 @@ int ble_gattc_cache_assoc(ble_addr_t peer_addr)
         return BLE_HS_EUNKNOWN;
     }
 
-    if (cache_conn->cache_state == CACHE_LOADED) {
+    if (cache_conn->cache_state == CACHE_LOADED ||
+        cache_conn->cache_state == CACHE_VERIFIED) {
 
         BLE_HS_LOG(INFO, "Cache already loaded for conn_handle=%d; "
                          "cache state=%d. Skipping association.",
                           cache_conn->conn_handle, cache_conn->cache_state);
         ble_gap_assoc_event(cache_conn->conn_handle, 0, cache_conn->cache_state);
+        return 0;
     }
 
     if (cache_conn->cache_state == CACHE_INVALID) {
@@ -2243,6 +2422,7 @@ ble_gattc_cache_conn_get_svc_changed_handle(uint16_t conn_handle)
     }
     return chr->chr.val_handle;
 }
+#endif
 
 void
 ble_gattc_cache_conn_free_mem(void)
@@ -2428,6 +2608,9 @@ ble_gattc_cache_conn_init(void)
 
     storage_cb = NULL;
     rc = ble_gattc_cache_init(storage_cb);
+    if (rc != 0) {
+        goto err;
+    }
     return 0;
 
 err:
@@ -2459,14 +2642,6 @@ static int ble_gattc_cache_conn_verify(struct ble_gattc_cache_conn *conn)
 {
     struct ble_hs_conn *gap_conn;
     int rc;
-#if MYNEWT_VAL(BLE_GATT_CACHING_ASSOC_ENABLE)
-    if (conn->assoc_success && conn->cache_state == CACHE_LOADED) {
-        conn->cache_state = CACHE_VERIFIED;
-        BLE_HS_LOG(INFO, "Associate complete, skipping Discovery");
-        ble_gattc_cache_conn_disc_complete(conn, 0);
-        return 0;
-    }
-#endif
 
     if (conn->cache_state == CACHE_VERIFIED) {
         return 0;
@@ -2484,6 +2659,14 @@ static int ble_gattc_cache_conn_verify(struct ble_gattc_cache_conn *conn)
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOTCONN);
         return BLE_HS_ENOTCONN;
     }
+#if MYNEWT_VAL(BLE_GATT_CACHING_ASSOC_ENABLE)
+    if (conn->assoc_success && conn->cache_state == CACHE_LOADED) {
+        conn->cache_state = CACHE_VERIFIED;
+        BLE_HS_LOG(INFO, "Associate complete, skipping Discovery");
+        ble_gattc_cache_conn_disc_complete(conn, 0);
+        return 0;
+    }
+#endif
     if (conn->cache_state == CACHE_LOADED) {
         if (bonded) {
             conn->cache_state = CACHE_VERIFIED;
@@ -2513,6 +2696,7 @@ static void ble_gattc_cache_search_all_svcs_cb(struct ble_npl_event *ev)
     int status = 0;
     uint16_t conn_handle;
     ble_gatt_disc_svc_fn *dcb;
+    bool aborted;
 
     conn_handle = *(uint16_t*)ble_npl_event_get_arg(ev);
     conn = ble_gattc_cache_conn_find(conn_handle);
@@ -2530,13 +2714,19 @@ static void ble_gattc_cache_search_all_svcs_cb(struct ble_npl_event *ev)
         return;
     }
 
+    aborted = false;
     SLIST_FOREACH(svc, &conn->svcs, next) {
         if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY) {
-            dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg);
+            if (dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg) != 0) {
+                aborted = true;
+                break;
+            }
         }
     }
-    status = BLE_HS_EDONE;
-    dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg);
+    if (!aborted) {
+        status = BLE_HS_EDONE;
+        dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg);
+    }
 
     return;
 }
@@ -2764,6 +2954,9 @@ ble_gattc_cache_conn_search_all_chrs_cb(struct ble_npl_event *ev)
 
     op = &conn->pending_op;
     dcb = op->cb;
+    if (dcb == NULL) {
+        return;
+    }
     svc = ble_gattc_cache_conn_svc_find_range(conn, op->start_handle);
 
     if (svc == NULL) {
@@ -2773,6 +2966,10 @@ ble_gattc_cache_conn_search_all_chrs_cb(struct ble_npl_event *ev)
 
     /* return all chrs */
     SLIST_FOREACH(chr, &svc->chrs, next) {
+        if (chr->chr.def_handle < op->start_handle ||
+            chr->chr.def_handle > op->end_handle) {
+            continue;
+        }
         dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, op->cb_arg);
     }
     status = BLE_HS_EDONE;
@@ -2894,6 +3091,7 @@ ble_gattc_cache_conn_search_all_dscs_cb(struct ble_npl_event *ev)
     int status = 0;
     uint16_t conn_handle;
     ble_gatt_dsc_fn *dcb;
+    bool aborted;
 
     conn_handle = *(uint16_t*)ble_npl_event_get_arg(ev);
     conn = ble_gattc_cache_conn_find(conn_handle);
@@ -2917,11 +3115,20 @@ ble_gattc_cache_conn_search_all_dscs_cb(struct ble_npl_event *ev)
         return;
     }
 
+    aborted = false;
     SLIST_FOREACH(dsc, &chr->dscs, next) {
-        dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, op->cb_arg);
+        if (dsc->dsc.handle < op->start_handle || dsc->dsc.handle > op->end_handle) {
+            continue;
+        }
+        if (dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, op->cb_arg) != 0) {
+            aborted = true;
+            break;
+        }
+    }
+    if (!aborted) {
+        status = BLE_HS_EDONE;
+        dcb(conn_handle, ble_gattc_cache_error(status, 0), 0, NULL, op->cb_arg);
     }
-    status = BLE_HS_EDONE;
-    dcb(conn_handle, ble_gattc_cache_error(status, 0), 0, NULL, op->cb_arg);
 
     return;
 }

nimble/host/src/ble_gattc_cache_priv.h

@@ -126,6 +126,8 @@ struct ble_gattc_cache_conn {
     struct ble_gattc_cache_conn_svc_list svcs;
 
     uint8_t cache_state;
+    /** Set when a DB change arrives during in-progress discovery. */
+    uint8_t needs_rediscovery;
     /** Keeps track of where we are in the service discovery process. */
     uint16_t disc_prev_chr_val;
     struct ble_gattc_cache_conn_svc *cur_svc;
@@ -308,7 +310,7 @@ int ble_gattc_cache_conn_search_all_svcs(uint16_t conn_handle,
                                          ble_gatt_disc_svc_fn *cb, void *cb_arg);
 int ble_gattc_cache_conn_search_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_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES))
+#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)
 int ble_gattc_cache_conn_search_inc_svcs(uint16_t conn_handle, uint16_t start_handle,
                                          uint16_t end_handle,
                                          ble_gatt_disc_incl_svc_fn *cb, void *cb_arg);

nimble/host/src/ble_gatts.c

@@ -49,6 +49,13 @@ enum {
     CONN_CLT_CFG_ADD = 1,
     CONN_CLT_CFG_REMOVE = 2,
 };
+
+struct ble_gatts_conn_clt_cfg_upd_arg {
+    uint16_t action;
+    uint16_t chr_val_handle;
+    uint16_t allowed_flags;
+    int rc;
+};
 #endif
 #endif
 
@@ -298,15 +305,26 @@ ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle,
     BLE_HS_DBG_ASSERT(op == BLE_ATT_ACCESS_OP_READ);
 
     svc = arg;
+    {
+        uint8_t uuid_buf[16];
+        int uuid_len;
 
-    buf = os_mbuf_extend(*om, ble_uuid_length(svc->uuid));
-    if (buf == NULL) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES);
-        return BLE_ATT_ERR_INSUFFICIENT_RES;
+        uuid_len = ble_uuid_length(svc->uuid);
+        if (offset > uuid_len) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        ble_uuid_flat(svc->uuid, uuid_buf);
+
+        buf = os_mbuf_extend(*om, uuid_len - offset);
+        if (buf == NULL) {
+            BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES);
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+
+        memcpy(buf, uuid_buf + offset, uuid_len - offset);
     }
 
-    ble_uuid_flat(svc->uuid, buf);
-
     return 0;
 }
 
@@ -609,7 +627,9 @@ ble_gatts_val_access(uint16_t conn_handle, uint16_t attr_handle,
             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);
+                    if (os_mbuf_appendfrom(*om, gatt_ctxt->om, offset, attr_len) != 0) {
+                        rc = BLE_ATT_ERR_INSUFFICIENT_RES;
+                    }
                 }
             } else {
                 rc = BLE_ATT_ERR_INVALID_OFFSET;
@@ -845,6 +865,17 @@ ble_gatts_dsc_is_sane(const struct ble_gatt_dsc_def *dsc)
         return 0;
     }
 
+    if ((dsc->att_flags & (BLE_ATT_F_READ | BLE_ATT_F_WRITE |
+                          BLE_ATT_F_READ_ENC | BLE_ATT_F_WRITE_ENC |
+                          BLE_ATT_F_READ_AUTHEN | BLE_ATT_F_WRITE_AUTHEN |
+                          BLE_ATT_F_READ_AUTHOR | BLE_ATT_F_WRITE_AUTHOR)) == 0) {
+        return 0;
+    }
+
+    if (dsc->min_key_size > 16) {
+        return 0;
+    }
+
     return 1;
 }
 
@@ -939,6 +970,10 @@ ble_gatts_clt_cfg_find_idx(struct ble_gatts_clt_cfg *cfgs,
     struct ble_gatts_clt_cfg *cfg;
     int i;
 
+    if (cfgs == NULL) {
+        return -1;
+    }
+
     for (i = 0; i < ble_gatts_num_cfgable_chrs; i++) {
         cfg = cfgs + i;
         if (cfg->chr_val_handle == chr_val_handle) {
@@ -1003,7 +1038,7 @@ ble_gatts_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
 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 os_mbuf **om,
                                 struct ble_store_value_cccd *out_cccd,
                                 uint8_t *out_prev_clt_cfg_flags,
                                 uint8_t *out_cur_clt_cfg_flags)
@@ -1048,7 +1083,7 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle,
     switch (gatt_op) {
     case BLE_GATT_ACCESS_OP_READ_DSC:
         STATS_INC(ble_gatts_stats, dsc_reads);
-        buf = os_mbuf_extend(om, 2);
+        buf = os_mbuf_extend(*om, 2);
         if (buf == NULL) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES);
             return BLE_ATT_ERR_INSUFFICIENT_RES;
@@ -1058,15 +1093,18 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle,
 
     case BLE_GATT_ACCESS_OP_WRITE_DSC:
         STATS_INC(ble_gatts_stats, dsc_writes);
-        if (OS_MBUF_PKTLEN(om) != 2) {
+        if (OS_MBUF_PKTLEN(*om) != 2) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN);
             return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
         }
 
-        om = os_mbuf_pullup(om, 2);
-        BLE_HS_DBG_ASSERT(om != NULL);
+        *om = os_mbuf_pullup(*om, 2);
+        if (*om == NULL) {
+            BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES);
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
 
-        flags = get_le16(om->om_data);
+        flags = get_le16((*om)->om_data);
         if ((flags & ~clt_cfg->allowed) != 0) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_REQ_NOT_SUPPORTED);
             return BLE_ATT_ERR_REQ_NOT_SUPPORTED;
@@ -1117,7 +1155,7 @@ ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle,
         rc = BLE_ATT_ERR_UNLIKELY;
     } else {
         rc = ble_gatts_clt_cfg_access_locked(conn, attr_handle, op, offset,
-                                             *om, &cccd_value, &prev_flags,
+                                             om, &cccd_value, &prev_flags,
                                              &cur_flags);
     }
 
@@ -1205,13 +1243,22 @@ ble_gatts_cpfd_access(uint16_t conn_handle, uint16_t attr_handle,
     STATS_INC(ble_gatts_stats, dsc_reads);
 
     cpfd = arg;
-    
-    rc = 0;
-    rc += os_mbuf_append(*om, &(cpfd->format), sizeof(cpfd->format));
-    rc += os_mbuf_append(*om, &(cpfd->exponent), sizeof(cpfd->exponent));
-    rc += os_mbuf_append(*om, &(cpfd->unit), sizeof(cpfd->unit));
-    rc += os_mbuf_append(*om, &(cpfd->name_space), sizeof(cpfd->name_space));
-    rc += os_mbuf_append(*om, &(cpfd->description), sizeof(cpfd->description));
+    {
+        uint8_t cpfd_bytes[7];
+
+        cpfd_bytes[0] = cpfd->format;
+        cpfd_bytes[1] = (uint8_t)cpfd->exponent;
+        put_le16(&cpfd_bytes[2], cpfd->unit);
+        cpfd_bytes[4] = cpfd->name_space;
+        put_le16(&cpfd_bytes[5], cpfd->description);
+
+        if (offset > sizeof(cpfd_bytes)) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        rc = os_mbuf_append(*om, cpfd_bytes + offset,
+                            sizeof(cpfd_bytes) - offset);
+    }
 
     return ((rc == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES);
 }
@@ -1546,6 +1593,59 @@ ble_gatts_register_round(int *out_num_registered, ble_gatt_register_fn *cb,
  *                              BLE_HS_EINVAL if the service definition table
  *                                  contains an invalid element.
  */
+#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+static void
+ble_gatts_unregister_svcs(const struct ble_gatt_svc_def *svcs, int num_svcs)
+{
+    struct ble_gatts_svc_entry *entry;
+    int i;
+    int rc;
+
+    if (svcs == NULL || num_svcs <= 0) {
+        return;
+    }
+
+    for (i = num_svcs - 1; i >= 0; i--) {
+        entry = ble_gatts_find_svc_entry(&svcs[i]);
+        if (entry == NULL) {
+            continue;
+        }
+        if (entry->handle != 0) {
+            rc = ble_att_svr_deregister(entry->handle, entry->end_group_handle);
+            BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+        }
+        STAILQ_REMOVE(&ble_gatts_svc_entries, entry, ble_gatts_svc_entry, next);
+        ble_gatts_svc_entry_free(entry);
+    }
+}
+#else
+static void
+ble_gatts_unregister_svcs_entries(int start_idx, int num_svcs)
+{
+    int i;
+    int rc;
+
+    if (num_svcs <= 0) {
+        return;
+    }
+
+    for (i = start_idx + num_svcs - 1; i >= start_idx; i--) {
+        if (ble_gatts_svc_entries[i].handle != 0) {
+            rc = ble_att_svr_deregister(
+                ble_gatts_svc_entries[i].handle,
+                ble_gatts_svc_entries[i].end_group_handle);
+            BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+        }
+        memset(&ble_gatts_svc_entries[i], 0,
+               sizeof ble_gatts_svc_entries[i]);
+    }
+
+    if (ble_gatts_num_svc_entries == start_idx + num_svcs) {
+        ble_gatts_num_svc_entries = start_idx;
+    }
+}
+#endif
+
 int
 ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
                         ble_gatt_register_fn *cb, void *cb_arg)
@@ -1563,6 +1663,11 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
     }
 #else
     int idx;
+    int start_idx;
+#endif
+
+#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+    start_idx = ble_gatts_num_svc_entries;
 #endif
 
     for (i = 0; svcs[i].type != BLE_GATT_SVC_TYPE_END; i++) {
@@ -1570,6 +1675,7 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
         entry = ble_gatts_svc_entry_alloc();
         if (entry == NULL) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
+            ble_gatts_unregister_svcs(svcs, i);
             return BLE_HS_ENOMEM;
         }
         entry -> svc = svcs + i;
@@ -1577,9 +1683,10 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
         entry -> end_group_handle = 0xffff;
         STAILQ_INSERT_TAIL(&ble_gatts_svc_entries, entry, next);
 #else
-        idx = ble_gatts_num_svc_entries + i;
+        idx = start_idx + i;
         if (idx >= ble_hs_max_services) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
+            ble_gatts_unregister_svcs_entries(start_idx, i);
             return BLE_HS_ENOMEM;
         }
 
@@ -1597,6 +1704,11 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
     while (total_registered < num_svcs) {
         rc = ble_gatts_register_round(&cur_registered, cb, cb_arg);
         if (rc != 0) {
+#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+            ble_gatts_unregister_svcs(svcs, num_svcs);
+#else
+            ble_gatts_unregister_svcs_entries(start_idx, num_svcs);
+#endif
             return rc;
         }
         total_registered += cur_registered;
@@ -1750,6 +1862,12 @@ ble_gatts_free_svc_defs(void)
 static void
 ble_gatts_free_mem(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gatts_static_vars == NULL) {
+        return;
+    }
+#endif
+
 #if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
     int rc;
 #endif
@@ -1826,9 +1944,6 @@ ble_gatts_stop(void)
     ble_hs_max_client_configs = 0;
 
     ble_gatts_free_svc_defs();
-#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
-    ble_att_svr_reset();
-#endif
     ble_att_svr_stop();
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
@@ -1855,6 +1970,7 @@ ble_gatts_start(void)
     uint16_t allowed_flags;
     ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
     int num_elems;
+
 #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
     if (STAILQ_EMPTY(&ble_gatts_clt_cfgs)) {
         STAILQ_INIT(&ble_gatts_clt_cfgs);
@@ -1877,11 +1993,6 @@ ble_gatts_start(void)
 
     ble_gatts_free_mem();
 
-    rc = ble_att_svr_start();
-    if (rc != 0) {
-        goto done;
-    }
-
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_gatts_ensure_ctx()) {
         rc = BLE_HS_ENOMEM;
@@ -1889,8 +2000,14 @@ ble_gatts_start(void)
     }
 #endif
 
+    rc = ble_att_svr_start();
+    if (rc != 0) {
+        goto done;
+    }
+
 #if MYNEWT_VAL(BLE_GATT_CACHING)
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+#if MYNEWT_VAL(BLE_STORE_MAX_BONDS)
     if (ble_gatts_conn_aware_states == NULL) {
         ble_gatts_conn_aware_states = nimble_platform_mem_calloc(1, sizeof(struct ble_gatts_aware_state) * MYNEWT_VAL(BLE_STORE_MAX_BONDS));
         if (ble_gatts_conn_aware_states == NULL) {
@@ -1898,6 +2015,7 @@ ble_gatts_start(void)
             goto done;
         }
     }
+#endif
 #else
     memset(ble_gatts_conn_aware_states, 0, sizeof ble_gatts_conn_aware_states);
 #endif
@@ -2017,7 +2135,10 @@ ble_gatts_start(void)
         if (allowed_flags != 0) {
 #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
             clt_cfg = ble_gatts_clt_cfg_alloc();
-            BLE_HS_DBG_ASSERT_EVAL(clt_cfg != NULL);
+            if (clt_cfg == NULL) {
+                rc = BLE_HS_ENOMEM;
+                goto done;
+            }
 
             clt_cfg->chr_val_handle = ha->ha_handle_id + 1;
             clt_cfg->allowed = allowed_flags;
@@ -2053,8 +2174,20 @@ ble_gatts_conn_can_alloc(void)
     }
 #endif
 
-    return ble_gatts_num_cfgable_chrs == 0 ||
+    if (ble_gatts_num_cfgable_chrs == 0) {
+        return 1;
+    }
+
+#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+    return ble_gatts_clt_cfg_pool.mp_num_free >= ble_gatts_num_cfgable_chrs;
+#else
+    return 1;
+#endif
+#else
+    return ble_gatts_clt_cfg_pool.mp_num_blocks == 0 ||
            ble_gatts_clt_cfg_pool.mp_num_free > 0;
+#endif
 }
 
 int
@@ -2372,11 +2505,11 @@ ble_gatts_chr_updated(uint16_t chr_val_handle)
                                          chr_val_handle);
 
 	if (clt_cfg == NULL) {
-	    break;
+	    continue;
 	}
 #else
 	if (conn->bhc_gatt_svr.clt_cfgs == NULL ) {
-	    break;
+	    continue;
 	}
         BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
                                clt_cfg_idx);
@@ -2462,12 +2595,16 @@ ble_gatts_peer_cl_sup_feat_get(uint16_t conn_handle, uint8_t *out_supported_feat
         goto done;
     }
 
+    uint8_t orig_len = len;
     if (MYNEWT_VAL(BLE_GATT_CSFC_SIZE) < len) {
         len = MYNEWT_VAL(BLE_GATT_CSFC_SIZE);
     }
 
     memcpy(out_supported_feat, conn->bhc_gatt_svr.peer_cl_sup_feat,
            sizeof(uint8_t) * len);
+    if (len < orig_len) {
+        memset(out_supported_feat + len, 0, orig_len - len);
+    }
 
 done:
     ble_hs_unlock();
@@ -2480,8 +2617,10 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om)
     struct ble_hs_conn *conn;
     struct ble_store_value_csfc value_csfc;
     struct ble_store_key_csfc key_csfc;
+    ble_addr_t peer_addr;
     uint8_t feat[MYNEWT_VAL(BLE_GATT_CSFC_SIZE)] = {};
     uint16_t len;
+    int bonded = 0;
     int rc = 0;
     int i;
 
@@ -2503,9 +2642,10 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om)
         return BLE_ATT_ERR_UNLIKELY;
     }
 
-    /* clear RFU bits */
-    for (i = 0; i < MYNEWT_VAL(BLE_GATT_CSFC_SIZE); i++) {
-        feat[i] &= (BLE_GATT_CHR_CLI_SUP_FEAT_MASK >> (8 * i));
+    /* clear RFU bits - only first byte has defined bits, rest are RFU */
+    feat[0] &= (uint8_t)BLE_GATT_CHR_CLI_SUP_FEAT_MASK;
+    for (i = 1; i < MYNEWT_VAL(BLE_GATT_CSFC_SIZE); i++) {
+        feat[i] = 0;
     }
 
     ble_hs_lock();
@@ -2529,27 +2669,30 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om)
 
     memcpy(conn->bhc_gatt_svr.peer_cl_sup_feat, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE));
 
-    if (conn->bhc_sec_state.bonded) {
-        memset(&key_csfc, 0, sizeof key_csfc);
-        key_csfc.peer_addr = conn->bhc_peer_addr;
-
-        rc = ble_store_delete_csfc(&key_csfc);
-        if (rc != 0) {
-            goto done;
-        }
-
-        memset(&value_csfc, 0, sizeof value_csfc);
-        value_csfc.peer_addr = conn->bhc_peer_addr;
-        memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE));
-
-        rc = ble_store_write_csfc(&value_csfc);
-        if (rc != 0) {
-            goto done;
-        }
+    bonded = conn->bhc_sec_state.bonded;
+    if (bonded) {
+        peer_addr = conn->bhc_peer_addr;
     }
 
 done:
     ble_hs_unlock();
+
+    if (rc == 0 && bonded) {
+        memset(&key_csfc, 0, sizeof key_csfc);
+        key_csfc.peer_addr = peer_addr;
+
+        rc = ble_store_delete_csfc(&key_csfc);
+        if (rc != 0 && rc != BLE_HS_ENOENT) {
+            return rc;
+        }
+
+        memset(&value_csfc, 0, sizeof value_csfc);
+        value_csfc.peer_addr = peer_addr;
+        memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE));
+
+        rc = ble_store_write_csfc(&value_csfc);
+    }
+
     return rc;
 }
 
@@ -2594,6 +2737,10 @@ ble_gatts_tx_notifications_one_chr(uint16_t chr_val_handle)
 #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
             clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs,
                                              chr_val_handle);
+            if (clt_cfg == NULL) {
+                ble_hs_unlock();
+                continue;
+            }
 #else
             BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
                                    clt_cfg_idx);
@@ -2966,7 +3113,7 @@ ble_gatts_find_svc_chr_attr(const ble_uuid_t *svc_uuid,
         }
         next = STAILQ_NEXT(cur, ha_next);
 
-        if (cur->ha_handle_id == svc_entry->end_group_handle) {
+        if (cur->ha_handle_id >= svc_entry->end_group_handle) {
             /* Reached end of service without a match. */
             return BLE_HS_ENOENT;
         }
@@ -3074,15 +3221,18 @@ ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
 }
 
 #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
-static void ble_gatts_add_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle, uint16_t allowed_flags, uint8_t flags) {
+static int ble_gatts_add_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle, uint16_t allowed_flags, uint8_t flags) {
     struct ble_gatts_clt_cfg *cfg;
     cfg = ble_gatts_clt_cfg_alloc();
-    BLE_HS_DBG_ASSERT_EVAL(cfg != NULL);
+    if (cfg == NULL) {
+        return BLE_HS_ENOMEM;
+    }
 
     cfg->chr_val_handle = chr_val_handle;
     cfg->allowed = allowed_flags;
     cfg->flags = flags;
     STAILQ_INSERT_TAIL(clt_cfgs, cfg, next);
+    return 0;
 }
 
 static int ble_gatts_remove_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle) {
@@ -3112,28 +3262,37 @@ ble_gatts_conn_unaware(struct ble_hs_conn *conn, void *arg) {
 }
 #endif
 
-/* takes two arguments
-arg[0] : added/removed
-arg[1] : affected chr_val_handle
-arg[2] : allowed_flags
-*/
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+static int
+ble_gatts_conn_foreach_count(struct ble_hs_conn *conn, void *arg)
+{
+    (void)conn;
+    (*(int *)arg)++;
+    return 0;
+}
+#endif
+
 static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) {
+    struct ble_gatts_conn_clt_cfg_upd_arg *upd_arg = arg;
     int rc;
-    uint16_t action = ((uint16_t *) arg)[0];
-    uint16_t chr_val_handle = ((uint16_t *) arg)[1];
-    uint16_t allowed_flags;
-    switch(action) {
-    case 1:
-        /* added */
-        allowed_flags = ((uint16_t *) arg)[2];
-        ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle,
-                                allowed_flags, 0);
-        (conn->bhc_gatt_svr.num_clt_cfgs)++;
+
+    switch (upd_arg->action) {
+    case CONN_CLT_CFG_ADD:
+        rc = ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs,
+                                   upd_arg->chr_val_handle,
+                                   upd_arg->allowed_flags, 0);
+        if (rc == 0) {
+            (conn->bhc_gatt_svr.num_clt_cfgs)++;
+        } else {
+            BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc);
+            if (upd_arg->rc == 0) {
+                upd_arg->rc = rc;
+            }
+        }
         return 0;
-    case 2:
-        /* removed */
+    case CONN_CLT_CFG_REMOVE:
         rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs,
-                                chr_val_handle);
+                                      upd_arg->chr_val_handle);
         if (rc == 0) {
             (conn->bhc_gatt_svr.num_clt_cfgs)--;
         }
@@ -3153,35 +3312,86 @@ static struct ble_gatts_clt_cfg * ble_gatts_get_last_cfg(struct ble_gatts_clt_cf
     }
     return prev;
 }
+
+#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+static void
+ble_gatts_rollback_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs,
+                                    int num_svcs,
+                                    struct ble_gatts_clt_cfg *clt_cfg_start,
+                                    int cfgable_chrs_start)
+#else
+static void
+ble_gatts_rollback_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs,
+                                    int num_svcs,
+                                    int svc_start_idx,
+                                    struct ble_gatts_clt_cfg *clt_cfg_start,
+                                    int cfgable_chrs_start)
+#endif
+{
+    struct ble_gatts_clt_cfg *cfg;
+    struct ble_gatts_conn_clt_cfg_upd_arg upd_arg;
+
+    while ((cfg = ble_gatts_get_last_cfg(&ble_gatts_clt_cfgs)) != clt_cfg_start) {
+        upd_arg.action = CONN_CLT_CFG_REMOVE;
+        upd_arg.chr_val_handle = cfg->chr_val_handle;
+        upd_arg.rc = 0;
+        ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &upd_arg);
+        ble_gatts_remove_clt_cfg(&ble_gatts_clt_cfgs, cfg->chr_val_handle);
+    }
+
+#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+    ble_gatts_unregister_svcs(svcs, num_svcs);
+#else
+    ble_gatts_unregister_svcs_entries(svc_start_idx, num_svcs);
+#endif
+    ble_gatts_num_cfgable_chrs = cfgable_chrs_start;
+    ble_gatts_free_svc_defs();
+}
+
 int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) {
     void *p;
     int i;
     int rc = 0;
+    int num_svcs;
+    int cfgable_chrs_start;
     struct ble_att_svr_entry *ha;
     struct ble_gatt_chr_def *chr;
     struct ble_gatts_svc_entry *entry;
     ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
     uint16_t allowed_flags;
     struct ble_gatts_clt_cfg *cfg;
-    uint16_t arg[3];
+    struct ble_gatts_clt_cfg *clt_cfg_start;
+    struct ble_gatts_conn_clt_cfg_upd_arg upd_arg;
     uint16_t start_handle, end_handle;
+#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+    int svc_start_idx;
+#endif
+
+    num_svcs = 0;
+    while (svcs[num_svcs].type != BLE_GATT_SVC_TYPE_END) {
+        num_svcs++;
+    }
 
     p = nimble_platform_mem_calloc(1,sizeof *ble_gatts_svc_defs);
     if (p == NULL) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
     ble_hs_lock();
 
+    clt_cfg_start = ble_gatts_get_last_cfg(&ble_gatts_clt_cfgs);
+#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+    svc_start_idx = ble_gatts_num_svc_entries;
+#endif
+
     ble_gatts_svc_defs = p;
     ble_gatts_svc_defs[0] = svcs;
+    cfgable_chrs_start = ble_gatts_num_cfgable_chrs;
     rc = ble_gatts_register_svcs(ble_gatts_svc_defs[0],
                                  ble_hs_cfg.gatts_register_cb,
                                  ble_hs_cfg.gatts_register_arg);
     if (rc != 0) {
-#if BLE_HS_DEBUG
-        BLE_HS_DBG_ASSERT(0); /* memory leak expected */
-#endif
+        ble_gatts_num_cfgable_chrs = cfgable_chrs_start;
+        ble_gatts_free_svc_defs();
         goto done;
     }
     ble_gatts_free_svc_defs();
@@ -3191,24 +3401,45 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) {
         ha = NULL;
     } else {
         ha = ble_att_svr_find_by_handle(cfg->chr_val_handle - 1);
+        if (ha == NULL) {
+            rc = BLE_HS_EUNKNOWN;
+            goto rollback;
+        }
     }
     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_gatts_add_clt_cfg(&ble_gatts_clt_cfgs, ha->ha_handle_id + 1, allowed_flags, 0);
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+            int conn_cnt = 0;
+
+            ble_hs_conn_foreach(ble_gatts_conn_foreach_count, &conn_cnt);
+            if (ble_gatts_clt_cfg_pool.mp_num_free < 1 + conn_cnt) {
+                rc = BLE_HS_ENOMEM;
+                goto rollback;
+            }
+#endif
+            rc = ble_gatts_add_clt_cfg(&ble_gatts_clt_cfgs, ha->ha_handle_id + 1, allowed_flags, 0);
+            if (rc != 0) {
+                goto rollback;
+            }
             /* update connections */
-            arg[0] = CONN_CLT_CFG_ADD;
-            arg[1] = ha->ha_handle_id + 1;
-            arg[2] = allowed_flags;
-            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg);
+            upd_arg.action = CONN_CLT_CFG_ADD;
+            upd_arg.chr_val_handle = ha->ha_handle_id + 1;
+            upd_arg.allowed_flags = allowed_flags;
+            upd_arg.rc = 0;
+            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &upd_arg);
+            if (upd_arg.rc != 0) {
+                rc = upd_arg.rc;
+                goto rollback;
+            }
         }
     }
     i = 0;
     entry = ble_gatts_find_svc_entry(&svcs[i]);
     if (entry == NULL) {
         rc = BLE_HS_ENOENT;
-        goto done;
+        goto rollback;
     }
     start_handle = entry->handle;
     while(svcs[i].type != BLE_GATT_SVC_TYPE_END) {
@@ -3217,7 +3448,7 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) {
     entry = ble_gatts_find_svc_entry(&svcs[i - 1]);
     if (entry == NULL) {
         rc = BLE_HS_ENOENT;
-        goto done;
+        goto rollback;
     }
     end_handle = entry->end_group_handle;
 #if MYNEWT_VAL(BLE_GATT_CACHING)
@@ -3229,14 +3460,24 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) {
         ble_gatts_conn_aware_states[i].half_aware = 0;
         ble_gatts_conn_aware_states[i].aware = false;
     }
-    ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL);
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     }
 #endif
+    ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL);
 #endif
 
     /* send service change indication */
     ble_svc_gatt_changed(start_handle, end_handle);
+rollback:
+    if (rc != 0) {
+#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
+        ble_gatts_rollback_add_dynamic_svcs(svcs, num_svcs, clt_cfg_start,
+                                            cfgable_chrs_start);
+#else
+        ble_gatts_rollback_add_dynamic_svcs(svcs, num_svcs, svc_start_idx,
+                                            clt_cfg_start, cfgable_chrs_start);
+#endif
+    }
 done:
     ble_hs_unlock();
     return rc;
@@ -3284,10 +3525,10 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) {
     int chr_val_handle;
     struct ble_gatt_chr_def *chr;
     uint16_t allowed_flags;
-    uint16_t arg[2];
+    struct ble_gatts_conn_clt_cfg_upd_arg upd_arg;
     ble_uuid16_t uuid_chr = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC);
     struct ble_att_svr_entry *ha;
-    uint16_t start_handle, end_handle;
+    uint16_t start_handle = 0, end_handle = 0;
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     int i;
 #endif
@@ -3301,7 +3542,8 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) {
     }
     ha = ble_att_svr_find_by_handle(entry->handle);
     if (ha == NULL) {
-        rc = BLE_HS_ENOENT;
+        /* Service not yet registered; remove entry without ATT cleanup */
+        rc = 0;
         goto done;
     }
     while ((ha = ble_att_svr_find_by_uuid(ha, &uuid_chr.u, entry->end_group_handle)) != NULL) {
@@ -3312,9 +3554,10 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) {
             ble_gatts_remove_clt_cfg(&ble_gatts_clt_cfgs, chr_val_handle);
 
             /* update connections */
-            arg[0] = CONN_CLT_CFG_REMOVE;
-            arg[1] = chr_val_handle;
-            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg);
+            upd_arg.action = CONN_CLT_CFG_REMOVE;
+            upd_arg.chr_val_handle = chr_val_handle;
+            upd_arg.rc = 0;
+            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &upd_arg);
         }
     }
     /* keep the start handle and end handle before deleting the service */
@@ -3328,22 +3571,24 @@ done:
     if (rc == 0) {
         rc = ble_gatts_remove_svc_entry(uuid);
 #if MYNEWT_VAL(BLE_GATT_CACHING)
-        /* make all bonded connections them unaware */
+        if (start_handle != 0 || end_handle != 0) {
+            /* make all bonded connections them unaware */
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-        if (ble_gatts_conn_aware_states != NULL) {
+            if (ble_gatts_conn_aware_states != NULL) {
 #endif
-        for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) {
-            ble_gatts_conn_aware_states[i].aware = false;
-            ble_gatts_conn_aware_states[i].half_aware = 0;
-        }
-        ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL);
+            for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) {
+                ble_gatts_conn_aware_states[i].aware = false;
+                ble_gatts_conn_aware_states[i].half_aware = 0;
+            }
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+            }
+#endif
+            ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL);
         }
 #endif
-#endif
-
-        /* send service change indication */
-        ble_svc_gatt_changed(start_handle, end_handle);
+        if (start_handle != 0 || end_handle != 0) {
+            ble_svc_gatt_changed(start_handle, end_handle);
+        }
     }
     ble_hs_unlock();
     return rc;
@@ -3362,6 +3607,11 @@ ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs)
         goto done;
     }
 
+    if (svcs == NULL) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_gatts_ensure_ctx()) {
         rc = BLE_HS_ENOMEM;
@@ -3594,17 +3844,28 @@ int
 ble_gatts_get_cfgable_chrs(void)
 {
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    if (ble_gatts_static_vars == NULL) {
+    ble_gatts_static_vars_t *svars = ble_gatts_static_vars;
+    if (svars == NULL) {
         return 0;
     }
-#endif
+    return svars->_ble_gatts_num_cfgable_chrs;
+#else
     return ble_gatts_num_cfgable_chrs;
+#endif
 }
 #endif
 
 void
 ble_gatts_lcl_svc_foreach(ble_gatt_svc_foreach_fn cb, void *arg)
 {
+    if (cb == NULL) {
+        return;
+    }
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_gatts_static_vars == NULL) {
+        return;
+    }
+#endif
 #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE)
     struct ble_gatts_svc_entry *entry;
 
@@ -3644,9 +3905,8 @@ ble_gatts_reset(void)
 #endif
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    if (ble_gatts_ensure_ctx()) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
-        return BLE_HS_ENOMEM;
+    if (ble_gatts_static_vars == NULL) {
+        return 0;
     }
 #endif
 
@@ -3669,15 +3929,16 @@ ble_gatts_reset(void)
 #endif
 
         /* Note: gatts memory gets freed on next call to ble_gatts_start(). */
-    }
 
 #if MYNEWT_VAL(BLE_SVC_HID_SERVICE)
-    ble_svc_hid_reset();
+        ble_svc_hid_reset();
 #endif
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) && CONFIG_BT_NIMBLE_SPS_SERVICE
-    ble_svc_sps_reset();
+        ble_svc_sps_reset();
 #endif
+    }
+
     ble_hs_unlock();
 
     return rc;

nimble/host/src/ble_gatts_lcl.c

@@ -54,6 +54,9 @@ static const char * const ble_gatt_chr_f_names[] = {
     "WRITE_ENC",
     "WRITE_AUTHEN",
     "WRITE_AUTHOR",
+    "NOTIFY_INDICATE_ENC",
+    "NOTIFY_INDICATE_AUTHEN",
+    "NOTIFY_INDICATE_AUTHOR",
     NULL
 };
 
@@ -69,7 +72,7 @@ static const char * const ble_gatt_dsc_f_names[] = {
     NULL
 };
 
-#define BLE_CHR_FLAGS_STR_LEN 180
+#define BLE_CHR_FLAGS_STR_LEN 256
 
 static char *
 ble_gatts_flags_to_str(uint32_t flags, char *buf,
@@ -269,7 +272,9 @@ ble_gatt_show_local_svc(const struct ble_gatt_svc_def *svc,
 void
 ble_gatts_show_local(void)
 {
+    ble_hs_lock();
     ble_gatts_lcl_svc_foreach(ble_gatt_show_local_svc, NULL);
+    ble_hs_unlock();
 }
 
 #endif

nimble/host/src/ble_hs.c

@@ -238,6 +238,11 @@ ble_hs_evq_get(void)
 void
 ble_hs_evq_set(struct ble_npl_eventq *evq)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hs_ctx == NULL) {
+        return;
+    }
+#endif
     ble_hs_evq = evq;
 }
 
@@ -288,11 +293,14 @@ ble_hs_lock_nested(void)
     rc = ble_npl_mutex_pend(&ble_hs_mutex, 0xffffffff);
 
 #if MYNEWT_VAL(BLE_HS_DEBUG)
+    BLE_HS_DBG_ASSERT(ble_hs_task_handle_index < MAX_NESTED_LOCKS);
     counter_lock++;
     ble_hs_mutex_locked = 1;
     ble_hs_task_handle = xTaskGetCurrentTaskHandle();
-    ble_hs_task_handles[ble_hs_task_handle_index] = xTaskGetCurrentTaskHandle();
-    ble_hs_task_handle_index++;
+    if (ble_hs_task_handle_index < MAX_NESTED_LOCKS) {
+        ble_hs_task_handles[ble_hs_task_handle_index] = ble_hs_task_handle;
+        ble_hs_task_handle_index++;
+    }
 #endif
     BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
 }
@@ -315,10 +323,13 @@ ble_hs_unlock_nested(void)
         if (counter_lock == 0) {
             ble_hs_mutex_locked = 0;
         }
-        if (ble_hs_task_handles[ble_hs_task_handle_index - 1] == xTaskGetCurrentTaskHandle()) {
+        if (counter_lock < MAX_NESTED_LOCKS &&
+            ble_hs_task_handle_index > 0 &&
+            ble_hs_task_handles[ble_hs_task_handle_index - 1] == xTaskGetCurrentTaskHandle()) {
             ble_hs_task_handle_index--;
             ble_hs_task_handles[ble_hs_task_handle_index] = NULL;
-            ble_hs_task_handle = ble_hs_task_handles[ble_hs_task_handle_index -1];
+            ble_hs_task_handle = (ble_hs_task_handle_index > 0) ?
+                ble_hs_task_handles[ble_hs_task_handle_index - 1] : NULL;
         }
     }
 #endif
@@ -388,6 +399,8 @@ ble_hs_wakeup_tx_conn(struct ble_hs_conn *conn)
              */
             STAILQ_INSERT_HEAD(&conn->bhc_tx_q, OS_MBUF_PKTHDR(om), omp_next);
             return BLE_HS_EAGAIN;
+        } else if (rc != 0) {
+            return rc;
         }
     }
 
@@ -453,12 +466,22 @@ ble_hs_clear_rx_queue(void)
 int
 ble_hs_is_enabled(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hs_state_ctx) {
+        return 0;
+    }
+#endif
     return ble_hs_enabled_state == BLE_HS_ENABLED_STATE_ON;
 }
 
 int
 ble_hs_synced(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hs_state_ctx) {
+        return 0;
+    }
+#endif
     return ble_hs_sync_state == BLE_HS_SYNC_STATE_GOOD;
 }
 
@@ -486,10 +509,12 @@ ble_hs_sync(void)
 
     if (rc == 0) {
 #if NIMBLE_BLE_CONNECT
-        rc = ble_hs_misc_restore_irks();
-        if (rc != 0) {
+        int irk_rc;
+
+        irk_rc = ble_hs_misc_restore_irks();
+        if (irk_rc != 0) {
             BLE_HS_LOG(INFO, "Failed to restore IRKs from store; status=%d\n",
-                       rc);
+                       irk_rc);
         }
 #endif
         if (ble_hs_cfg.sync_cb != NULL) {
@@ -506,6 +531,7 @@ static int
 ble_hs_reset(void)
 {
     int rc;
+    int reset_reason;
 
     STATS_INC(ble_hs_stats, reset);
 
@@ -513,16 +539,23 @@ ble_hs_reset(void)
 
     ble_hs_clear_rx_queue();
 
+    /* Atomically retrieve and clear reset reason under lock to avoid race
+     * with concurrent ble_hs_sched_reset() calls.
+     */
+    ble_hs_lock_nested();
+    reset_reason = ble_hs_reset_reason;
+    ble_hs_reset_reason = 0;
+    ble_hs_unlock_nested();
+
     /* Clear adverising and scanning states. */
-    ble_gap_reset_state(ble_hs_reset_reason);
+    ble_gap_reset_state(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);
+    if (ble_hs_cfg.reset_cb != NULL && reset_reason != 0) {
+        ble_hs_cfg.reset_cb(reset_reason);
     }
-    ble_hs_reset_reason = 0;
 
     rc = ble_hs_sync();
     return rc;
@@ -580,7 +613,6 @@ ble_hs_timer_reset(uint32_t ticks)
 
     if (!ble_hs_is_enabled()) {
         ble_npl_callout_stop(&ble_hs_timer);
-        ble_npl_callout_deinit(&ble_hs_timer);
     } else {
         rc = ble_npl_callout_reset(&ble_hs_timer, ticks);
         BLE_HS_DBG_ASSERT_EVAL(rc == 0);
@@ -596,6 +628,10 @@ ble_hs_timer_sched(int32_t ticks_from_now)
         return;
     }
 
+    if (!ble_hs_is_enabled()) {
+        return;
+    }
+
     /* Reset timer if it is not currently scheduled or if the specified time is
      * sooner than the previous expiration time.
      */
@@ -605,8 +641,8 @@ ble_hs_timer_sched(int32_t ticks_from_now)
                                ble_npl_callout_get_ticks(&ble_hs_timer))) < 0) {
         ble_hs_timer_reset(ticks_from_now);
     }
-    else if (ble_npl_callout_get_ticks(&ble_hs_timer) <= ble_npl_time_get()) {
-        /* Reset timer if currect time is later than expiration time. */
+    else if ((ble_npl_stime_t)(ble_npl_time_get() - ble_npl_callout_get_ticks(&ble_hs_timer)) >= 0) {
+        /* Reset timer if current time is later than expiration time. */
         BLE_HS_LOG(DEBUG,"exp_time:%d.now:%d.ticks:%d.active:%d.Need reset.",ble_npl_callout_get_ticks(&ble_hs_timer),ble_npl_time_get(),ticks_from_now,ble_npl_callout_is_active(&ble_hs_timer));
         ble_hs_timer_reset(ticks_from_now);
     }
@@ -716,12 +752,15 @@ ble_hs_enqueue_hci_event(uint8_t *hci_evt)
 
     ev = os_memblock_get(&ble_hs_hci_ev_pool);
 
-    if (ev) {
+    if (ev && ble_hs_evq) {
         memset (ev, 0, sizeof *ev);
         ble_npl_event_init(ev, ble_hs_event_rx_hci_ev, hci_evt);
         ble_npl_eventq_put(ble_hs_evq, ev);
     } else {
         /* Either ev is NULL or queue doesn't exist */
+        if (ev) {
+            os_memblock_put(&ble_hs_hci_ev_pool, ev);
+        }
 #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
         ble_transport_free(BLE_HCI_EVT, hci_evt);
 #else
@@ -883,11 +922,12 @@ ble_hs_tx_data(struct os_mbuf *om)
     if (pkt_len + 1 <= BLE_HS_HCI_LOG_BUF_SIZE) {
         uint8_t data[BLE_HS_HCI_LOG_BUF_SIZE];
         data[0] = 0x02;
-        os_mbuf_copydata(om, 0, pkt_len, &data[1]);
-        bt_hci_log_record_hci_data(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len);
+        if (os_mbuf_copydata(om, 0, pkt_len, &data[1]) == 0) {
+            bt_hci_log_record_hci_data(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len);
 #if BT_HCI_INSIGHTS_INCLUDED
-        bt_hci_log_record_insights(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len);
+            bt_hci_log_record_insights(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len);
 #endif
+        }
     }
 #endif
 
@@ -925,6 +965,10 @@ ble_hs_init(void)
         ble_hs_state_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_state_ctx));
         if (!ble_hs_state_ctx) {
             MODLOG_DFLT(ERROR, "Failed to allocate ble_hs_state_ctx (%zu bytes)\n", sizeof(*ble_hs_state_ctx));
+            nimble_platform_mem_free(ble_hs_ctx->hci_os_event_buf);
+            ble_hs_ctx->hci_os_event_buf = NULL;
+            nimble_platform_mem_free(ble_hs_ctx);
+            ble_hs_ctx = NULL;
             return;
         }
     }
@@ -1086,6 +1130,12 @@ ble_transport_hs_init(void)
 void
 ble_hs_deinit(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hs_ctx == NULL) {
+        return;
+    }
+#endif
+
     ble_hs_flow_deinit();
 
 #if BLE_MONITOR
@@ -1143,6 +1193,12 @@ ble_hs_deinit(void)
 #if (MYNEWT_VAL(BLE_HOST_BASED_PRIVACY))
     ble_hs_resolv_deinit();
 #endif
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) && MYNEWT_VAL(BLE_PERIODIC_ADV)
+    ble_hs_periodic_sync_deinit();
+#endif
+#if MYNEWT_VAL(BLE_HS_PVCY)
+    ble_hs_pvcy_irk_deinit();
+#endif
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_ctx) {
         ble_hs_ctx->parent_task = NULL;
@@ -1169,10 +1225,6 @@ ble_hs_deinit(void)
 
     ble_hs_adv_parse_free();
 
-#if MYNEWT_VAL(BLE_HS_PVCY)
-    ble_hs_pvcy_irk_deinit();
-#endif
-
     ble_hs_id_ctx_free();
 
     ble_hs_hci_ctx_free();

nimble/host/src/ble_hs_adv.c

@@ -42,6 +42,11 @@ typedef struct{
     ble_uuid16_t _ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2];
     ble_uuid32_t _ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4];
     ble_uuid128_t _ble_hs_adv_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16];
+#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
+    ble_uuid16_t _ble_hs_adv_sol_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2];
+    ble_uuid32_t _ble_hs_adv_sol_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4];
+    ble_uuid128_t _ble_hs_adv_sol_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16];
+#endif
 }ble_hs_adv_uuids_ctx;
 
 static ble_hs_adv_uuids_ctx *ble_hs_adv_uuids;
@@ -49,11 +54,21 @@ static ble_hs_adv_uuids_ctx *ble_hs_adv_uuids;
 #define ble_hs_adv_uuids16        (ble_hs_adv_uuids->_ble_hs_adv_uuids16)
 #define ble_hs_adv_uuids32        (ble_hs_adv_uuids->_ble_hs_adv_uuids32)
 #define ble_hs_adv_uuids128       (ble_hs_adv_uuids->_ble_hs_adv_uuids128)
+#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
+#define ble_hs_adv_sol_uuids16    (ble_hs_adv_uuids->_ble_hs_adv_sol_uuids16)
+#define ble_hs_adv_sol_uuids32    (ble_hs_adv_uuids->_ble_hs_adv_sol_uuids32)
+#define ble_hs_adv_sol_uuids128   (ble_hs_adv_uuids->_ble_hs_adv_sol_uuids128)
+#endif
 
 #else
 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];
+#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
+static ble_uuid16_t ble_hs_adv_sol_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2];
+static ble_uuid32_t ble_hs_adv_sol_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4];
+static ble_uuid128_t ble_hs_adv_sol_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16];
+#endif
 #endif
 
 static int
@@ -62,6 +77,10 @@ ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len,
 {
     int rc;
 
+    if (data_len > 254) {
+        return BLE_HS_EMSGSIZE;
+    }
+
     if (om ) {
         data_len++;
         rc = os_mbuf_append(om, &data_len, sizeof(data_len));
@@ -430,7 +449,7 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
 #if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
 
     /*** 0x14 - 16 bit service solicitaion */
-    if (adv_fields->sol_uuids16 != NULL) {
+    if (adv_fields->sol_uuids16 != NULL && adv_fields->sol_num_uuids16 > 0) {
         rc = ble_hs_adv_set_array_uuid16(BLE_HS_ADV_TYPE_SOL_UUIDS16, adv_fields->sol_num_uuids16,
                                          adv_fields->sol_uuids16, dst, &dst_len_local,
                                          max_len, om);
@@ -440,7 +459,7 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
     }
 
     /*** 0x15 - 128-bit service solicitation. */
-    if (adv_fields->sol_uuids128 != NULL) {
+    if (adv_fields->sol_uuids128 != NULL && adv_fields->sol_num_uuids128 > 0) {
         rc = ble_hs_adv_set_array_uuid128(BLE_HS_ADV_TYPE_SOL_UUIDS128, adv_fields->sol_num_uuids128,
                                           adv_fields->sol_uuids128, dst, &dst_len_local,
                                           max_len, om);
@@ -664,6 +683,9 @@ ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields,
 #if !NIMBLE_BLE_ADVERTISE
     return BLE_HS_ENOTSUP;
 #endif
+    if (om == NULL) {
+        return BLE_HS_EINVAL;
+    }
     return adv_set_fields(adv_fields, NULL, NULL, 0, om);
 }
 
@@ -714,6 +736,92 @@ ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields *adv_fields,
     return 0;
 }
 
+#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
+static int
+ble_hs_adv_parse_sol_uuids16(struct ble_hs_adv_fields *adv_fields,
+                             const uint8_t *data, uint8_t data_len)
+{
+    ble_uuid_any_t uuid;
+    int uuid_cnt;
+    int i;
+
+    if (data_len % 2 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    uuid_cnt = data_len / 2;
+    if (uuid_cnt > (BLE_HS_ADV_MAX_FIELD_SZ / 2)) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    adv_fields->sol_uuids16 = ble_hs_adv_sol_uuids16;
+    adv_fields->sol_num_uuids16 = uuid_cnt;
+
+    for (i = 0; i < uuid_cnt; i++) {
+        ble_uuid_init_from_buf(&uuid, data + i * 2, 2);
+        ble_hs_adv_sol_uuids16[i] = uuid.u16;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_parse_sol_uuids32(struct ble_hs_adv_fields *adv_fields,
+                             const uint8_t *data, uint8_t data_len)
+{
+    ble_uuid_any_t uuid;
+    int uuid_cnt;
+    int i;
+
+    if (data_len % 4 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    uuid_cnt = data_len / 4;
+    if (uuid_cnt > (BLE_HS_ADV_MAX_FIELD_SZ / 4)) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    adv_fields->sol_uuids32 = ble_hs_adv_sol_uuids32;
+    adv_fields->sol_num_uuids32 = uuid_cnt;
+
+    for (i = 0; i < uuid_cnt; i++) {
+        ble_uuid_init_from_buf(&uuid, data + i * 4, 4);
+        ble_hs_adv_sol_uuids32[i] = uuid.u32;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_parse_sol_uuids128(struct ble_hs_adv_fields *adv_fields,
+                              const uint8_t *data, uint8_t data_len)
+{
+    ble_uuid_any_t uuid;
+    int uuid_cnt;
+    int i;
+
+    if (data_len % 16 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    uuid_cnt = data_len / 16;
+    if (uuid_cnt > (BLE_HS_ADV_MAX_FIELD_SZ / 16)) {
+        return BLE_HS_EMSGSIZE;
+    }
+
+    adv_fields->sol_uuids128 = ble_hs_adv_sol_uuids128;
+    adv_fields->sol_num_uuids128 = uuid_cnt;
+
+    for (i = 0; i < uuid_cnt; i++) {
+        ble_uuid_init_from_buf(&uuid, data + i * 16, 16);
+        ble_hs_adv_sol_uuids128[i] = uuid.u128;
+    }
+
+    return 0;
+}
+#endif
+
 static int
 ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields *adv_fields,
                          const uint8_t *data, uint8_t data_len)
@@ -844,6 +952,9 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
     *total_len = src[0] + 1;
 
     if (*total_len < 2) {
+        if (*total_len == 1 && src[0] == 0) {
+            return 0;
+        }
         return BLE_HS_EBADDATA;
     }
 
@@ -962,18 +1073,10 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
 
 #if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
     case BLE_HS_ADV_TYPE_SOL_UUIDS16:
-        rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
-        if (rc != 0) {
-            return rc;
-        }
-        break;
+        return ble_hs_adv_parse_sol_uuids16(adv_fields, data, data_len);
 
     case BLE_HS_ADV_TYPE_SOL_UUIDS128:
-        rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len);
-        if (rc != 0) {
-            return rc;
-        }
-        break;
+        return ble_hs_adv_parse_sol_uuids128(adv_fields, data, data_len);
 #endif
 
     case BLE_HS_ADV_TYPE_SVC_DATA_UUID16:
@@ -1019,6 +1122,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
         }
         adv_fields->device_addr = data;
         adv_fields->device_addr_type = data[6];
+        adv_fields->device_addr_is_present = 1;
         break;
 
     case BLE_HS_ADV_TYPE_LE_ROLE:
@@ -1061,11 +1165,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
 
 #if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS)
     case BLE_HS_ADV_TYPE_SOL_UUIDS32:
-        rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
-        if (rc != 0) {
-            return rc;
-        }
-        break;
+        return ble_hs_adv_parse_sol_uuids32(adv_fields, data, data_len);
 #endif
 
 #if MYNEWT_VAL(ENC_ADV_DATA)
@@ -1125,6 +1225,10 @@ ble_hs_adv_parse(const uint8_t *data, uint8_t length,
     while (length > 1) {
         field = (const void *) data;
 
+        if (field->length == 0) {
+            return 0;
+        }
+
         if (field->length >= length) {
             return BLE_HS_EBADDATA;
         }

nimble/host/src/ble_hs_conn.c

@@ -361,7 +361,7 @@ ble_hs_conn_insert(struct ble_hs_conn *conn)
 
     BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
 
-    BLE_HS_DBG_ASSERT_EVAL(ble_hs_conn_find(conn->bhc_handle) == NULL);
+    BLE_HS_DBG_ASSERT(ble_hs_conn_find(conn->bhc_handle) == NULL);
     SLIST_INSERT_HEAD(&ble_hs_conns, conn, bhc_next);
 }
 
@@ -433,14 +433,20 @@ ble_hs_conn_find_by_addr(const ble_addr_t *addr)
             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 */
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+            /* Also match against the peer identity address when available. */
             ble_hs_conn_addrs(conn, &addrs);
             if (ble_addr_cmp(&addrs.peer_id_addr, addr) == 0) {
                 return conn;
             }
+#else
+            if (conn->bhc_peer_addr.type >= BLE_ADDR_PUBLIC_ID) {
+                ble_hs_conn_addrs(conn, &addrs);
+                if (ble_addr_cmp(&addrs.peer_id_addr, addr) == 0) {
+                    return conn;
+                }
+            }
+#endif
         }
     }
 
@@ -500,6 +506,9 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn,
 {
     const uint8_t *our_id_addr_val;
     int rc;
+
+    memset(addrs, 0, sizeof(*addrs));
+
     /* Determine our address information. */
     addrs->our_id_addr.type =
         ble_hs_misc_own_addr_type_to_id(conn->bhc_our_addr_type);
@@ -626,8 +635,14 @@ ble_hs_conn_timer(void)
                 time_diff = conn->rx_frag_tmo - now;
 
                 if (time_diff <= 0) {
+                    int term_rc;
+
                     /* ACL reassembly has timed out.*/
-                    ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM);
+                    term_rc = ble_gap_terminate_with_conn(conn,
+                            BLE_ERR_REM_USER_CONN_TERM);
+                    if (term_rc != 0 && next_exp_in > 1) {
+                        next_exp_in = 1;
+                    }
                     continue;
                 }
 
@@ -645,8 +660,14 @@ ble_hs_conn_timer(void)
              */
             time_diff = ble_att_svr_ticks_until_tmo(&conn->bhc_att_svr, now);
             if (time_diff <= 0) {
+                int term_rc;
+
                 /* Queued write has timed out.*/
-                ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM);
+                term_rc = ble_gap_terminate_with_conn(conn,
+                        BLE_ERR_REM_USER_CONN_TERM);
+                if (term_rc != 0 && next_exp_in > 1) {
+                    next_exp_in = 1;
+                }
                 continue;
             }
 
@@ -702,10 +723,11 @@ ble_hs_conn_init(void)
 #endif
         os_mempool_unregister(&ble_hs_conn_pool);
         if (ble_hs_conn_ctx) {
+            /* Clear pool before freeing ctx to avoid UAF via the macro */
+            memset(&ble_hs_conn_pool, 0, sizeof(ble_hs_conn_pool));
             nimble_platform_mem_free(ble_hs_conn_ctx);
             ble_hs_conn_ctx = NULL;
         }
-        memset(&ble_hs_conn_pool, 0, sizeof(ble_hs_conn_pool));
 #endif
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EOS);
         return BLE_HS_EOS;

nimble/host/src/ble_hs_hci.c

@@ -138,10 +138,10 @@ static struct err_code core_err_code_list[] = {
       { BLE_HS_EAUTHEN,                               ": BLE_HS_EAUTHEN (Insufficient authentication)" },
       { BLE_HS_EAUTHOR,                               ": BLE_HS_EAUTHOR (Insufficient authorization)" },
       { BLE_HS_EENCRYPT,                              ": BLE_HS_EENCRYPT (Insufficient encryption level)" },
-      { BLE_HS_EENCRYPT_KEY_SZ,                       ": BLE_HS_EENCRYPT_KEY_SZ (Insufficient key size" },
-      { BLE_HS_ESTORE_CAP,                            ": BLE_HS_ESTORE_CAP (BLE_HS_ESTORE_FAIL,)" },
+      { BLE_HS_EENCRYPT_KEY_SZ,                       ": BLE_HS_EENCRYPT_KEY_SZ (Insufficient key size)" },
+      { BLE_HS_ESTORE_CAP,                            ": BLE_HS_ESTORE_CAP (Storage capacity exceeded)" },
       { BLE_HS_ESTORE_FAIL,                           ": BLE_HS_ESTORE_FAIL (Storage IO error)" },
-      { BLE_HS_EPREEMPTED,                            ": BLE_HS_EPREEMPTED (ation was preempted)" },
+      { BLE_HS_EPREEMPTED,                            ": BLE_HS_EPREEMPTED (Operation was preempted)" },
       { BLE_HS_EDISABLED,                             ": BLE_HS_EDISABLED (Operation disabled)" },
       { BLE_HS_ESTALLED,                              ": BLE_HS_ESTALLED (Operation stalled)" }
 };
@@ -268,7 +268,7 @@ typedef struct {
     uint16_t             hci_max_pkts;
     uint64_t             hci_sup_feat;
     uint8_t              hci_version;
-    uint16_t             hci_avial_pkts;
+    uint16_t             hci_avail_pkts;
 
     struct os_mempool    hci_frag_mempool;      /* Memory pool for HCI fragments */
     os_membuf_t         *hci_frag_data;         /* Memory buffer backing HCI pool */
@@ -286,7 +286,8 @@ static ble_hs_hci_ctx_t *ble_hs_hci_ctx = NULL;
 #define ble_hs_hci_max_pkts           (ble_hs_hci_ctx->hci_max_pkts)
 #define ble_hs_hci_sup_feat           (ble_hs_hci_ctx->hci_sup_feat)
 #define ble_hs_hci_version            (ble_hs_hci_ctx->hci_version)
-#define ble_hs_hci_avail_pkts         (ble_hs_hci_ctx->hci_avial_pkts)
+#undef ble_hs_hci_avail_pkts
+#define ble_hs_hci_avail_pkts         (ble_hs_hci_ctx->hci_avail_pkts)
 
 #define ble_hs_hci_frag_mempool     (ble_hs_hci_ctx->hci_frag_mempool)
 #define ble_hs_hci_frag_data        (ble_hs_hci_ctx->hci_frag_data)
@@ -312,6 +313,9 @@ uint16_t ble_hs_hci_avail_pkts;
 static struct ble_hci_ev *l_ble_hs_hci_ack;
 #endif //BLE_STATIC_TO_DYNAMIC
 
+/* Indicates HCI mutex / state has been initialized and can be safely read. */
+static uint8_t ble_hs_hci_initialized;
+
 #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE
 #define BLE_HS_HCI_FRAG_DATABUF_SIZE    \
     (BLE_ACL_MAX_PKT_SIZE +             \
@@ -365,13 +369,19 @@ ble_hs_hci_set_phony_ack_cb(ble_hs_hci_phony_ack_fn *cb)
 }
 #endif
 
-static void
+static int
 ble_hs_hci_lock(void)
 {
     int rc;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hs_hci_ctx == NULL) {
+        return BLE_HS_ENOTSYNCED;
+    }
+#endif
     rc = ble_npl_mutex_pend(&ble_hs_hci_mutex, BLE_NPL_TIME_FOREVER);
     BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+    return 0;
 }
 
 static void
@@ -379,6 +389,11 @@ ble_hs_hci_unlock(void)
 {
     int rc;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hs_hci_ctx == NULL) {
+        return;
+    }
+#endif
     rc = ble_npl_mutex_release(&ble_hs_hci_mutex);
     BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
 }
@@ -391,6 +406,8 @@ ble_hs_hci_set_buf_sz(uint16_t pktlen, uint16_t max_pkts)
         return BLE_HS_EINVAL;
     }
 
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
     ble_hs_hci_buf_sz = pktlen;
     ble_hs_hci_max_pkts = max_pkts;
     ble_hs_hci_avail_pkts = max_pkts;
@@ -398,6 +415,26 @@ ble_hs_hci_set_buf_sz(uint16_t pktlen, uint16_t max_pkts)
     return 0;
 }
 
+/**
+ * Returns the number of available ACL transmit buffers on the controller.
+ * This can be used by applications to throttle notification enqueuing.
+ */
+uint16_t
+ble_hs_hci_get_avail_pkts(void)
+{
+    if (!ble_hs_hci_initialized) {
+        return 0;
+    }
+
+    uint16_t avail_pkts;
+
+    ble_hs_lock_nested();
+    avail_pkts = ble_hs_hci_avail_pkts;
+    ble_hs_unlock_nested();
+
+    return avail_pkts;
+}
+
 /**
  * Increases the count of available controller ACL buffers.
  */
@@ -406,7 +443,13 @@ 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) {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hs_hci_ctx) {
+        return;
+    }
+#endif
+
+    if (delta > ble_hs_hci_max_pkts - ble_hs_hci_avail_pkts) {
         ble_hs_sched_reset(BLE_HS_ECONTROLLER);
     } else {
         ble_hs_hci_avail_pkts += delta;
@@ -437,6 +480,7 @@ ble_hs_hci_rx_cmd_complete(const void *data, int len,
 
         /* TODO Process num_pkts field. */
 
+        out_ack->bha_opcode = opcode;
         out_ack->bha_status = 0;
         out_ack->bha_params = NULL;
         out_ack->bha_params_len = 0;
@@ -547,9 +591,10 @@ ble_hs_hci_wait_for_ack(void)
     if (ble_hs_hci_phony_ack_cb == NULL) {
         rc = BLE_HS_ETIMEOUT_HCI;
     } else {
-        l_ble_hs_hci_ack = ble_transport_alloc_cmd();
+        l_ble_hs_hci_ack = (struct ble_hci_ev *)ble_transport_alloc_evt(0);
         BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack != NULL);
-        rc = ble_hs_hci_phony_ack_cb((void *)l_ble_hs_hci_ack, 260);
+        rc = ble_hs_hci_phony_ack_cb((void *)l_ble_hs_hci_ack,
+                                     MYNEWT_VAL(BLE_TRANSPORT_EVT_SIZE));
     }
 #else
     rc = ble_npl_sem_pend(&ble_hs_hci_sem,
@@ -575,7 +620,10 @@ ble_hs_hci_cmd_tx_no_rsp(uint16_t opcode, const void *cmd, uint8_t cmd_len)
 {
     int rc;
 
-    ble_hs_hci_lock();
+    rc = ble_hs_hci_lock();
+    if (rc != 0) {
+        return rc;
+    }
 
     rc = ble_hs_hci_cmd_send_buf(opcode, cmd, cmd_len);
 
@@ -591,8 +639,14 @@ ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len,
     struct ble_hs_hci_ack ack;
     int rc;
 
+    rc = ble_hs_hci_lock();
+    if (rc != 0) {
+        return rc;
+    }
+    /* Assert after lock: when BLE_STATIC_TO_DYNAMIC is enabled, l_ble_hs_hci_ack
+     * expands to (ble_hs_hci_ctx->hci_ack). Evaluating this before ble_hs_hci_lock()
+     * would dereference ble_hs_hci_ctx before its NULL guard is checked. */
     BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack == NULL);
-    ble_hs_hci_lock();
 
     rc = ble_hs_hci_cmd_send_buf(opcode, cmd, cmd_len);
     if (rc != 0) {
@@ -1002,6 +1056,11 @@ ble_hs_hci_set_hci_version(uint8_t hci_version)
 uint8_t
 ble_hs_hci_get_hci_version(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hs_hci_ctx) {
+        return 0;
+    }
+#endif
     return ble_hs_hci_version;
 }
 
@@ -1045,11 +1104,15 @@ ble_hs_hci_init(void)
     }
 #endif
 
+    int init_ok = 1;
+
     rc = ble_npl_sem_init(&ble_hs_hci_sem, 0);
     BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    init_ok = init_ok && (rc == 0);
 
     rc = ble_npl_mutex_init(&ble_hs_hci_mutex);
     BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    init_ok = init_ok && (rc == 0);
 
     rc = mem_init_mbuf_pool(ble_hs_hci_frag_data,
                             &ble_hs_hci_frag_mempool,
@@ -1059,12 +1122,16 @@ ble_hs_hci_init(void)
                             "ble_hs_hci_frag");
 
     BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    init_ok = init_ok && (rc == 0);
+    ble_hs_hci_initialized = init_ok;
 }
 
 void ble_hs_hci_deinit(void)
 {
     int rc;
 
+    ble_hs_hci_initialized = 0;
+
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_hci_ctx == NULL) {
         return;

nimble/host/src/ble_hs_hci_cmd.c

@@ -54,8 +54,11 @@ ble_hs_hci_cmd_transport(struct ble_hci_cmd *cmd)
         return BLE_HS_ENOMEM_EVT;
 
     default:
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
-        return BLE_HS_EUNKNOWN;
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc);
+        /* BLE_HS_HCI_ERR is defined for positive HCI error codes (0x01-0xFF).
+         * Negative or out-of-range codes (e.g. -ENOMEM from UART transport)
+         * must not be passed to the macro or they produce an invalid error value. */
+        return (rc > 0 && rc <= 0xFF) ? BLE_HS_HCI_ERR(rc) : BLE_HS_EUNKNOWN;
     }
 }
 

nimble/host/src/ble_hs_hci_evt.c

@@ -126,7 +126,7 @@ 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;
 #if NIMBLE_BLE_CONNECT
 static ble_hs_hci_evt_fn ble_hs_hci_evt_disconn_complete;
-#if MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_SM
 static ble_hs_hci_evt_fn ble_hs_hci_evt_encrypt_change;
 static ble_hs_hci_evt_fn ble_hs_hci_evt_enc_key_refresh;
 #endif
@@ -258,7 +258,7 @@ static const struct ble_hs_hci_evt_dispatch_entry ble_hs_hci_evt_dispatch[] = {
 #if NIMBLE_BLE_CONNECT
     { BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP, ble_hs_hci_evt_rd_rem_ver_complete },
     { BLE_HCI_EVCODE_DISCONN_CMP, ble_hs_hci_evt_disconn_complete },
-#if MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_SM
     { BLE_HCI_EVCODE_ENCRYPT_CHG, ble_hs_hci_evt_encrypt_change },
     { BLE_HCI_EVCODE_ENC_KEY_REFRESH, ble_hs_hci_evt_enc_key_refresh },
 #endif
@@ -520,7 +520,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data,
 		/* Post event to interested application */
 		ble_gap_reattempt_count(handle, reattempt_conn.count);
 #if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_OBSERVER)
-                rc = ble_gap_master_connect_reattempt(ev->conn_handle);
+                rc = ble_gap_master_connect_reattempt(handle);
 		if (rc != 0) {
 		    BLE_HS_LOG(INFO, "Master reconnect attempt failed; rc = %d", rc);
 		}
@@ -593,7 +593,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data,
     return 0;
 }
 
-#if MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_SM
 static int
 ble_hs_hci_evt_encrypt_change(uint8_t event_code, const void *data,
                               unsigned int len)
@@ -626,7 +626,7 @@ ble_hs_hci_evt_hw_error(uint8_t event_code, const void *data, unsigned int len)
     return 0;
 }
 
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT && NIMBLE_BLE_SM
 static int
 ble_hs_hci_evt_enc_key_refresh(uint8_t event_code, const void *data,
                                unsigned int len)
@@ -715,6 +715,9 @@ ble_hs_hci_evt_vs(uint8_t event_code, const void *data, unsigned int len)
 static int
 ble_hs_hci_evt_rx_test(uint8_t event_code, const void *data, unsigned int len)
 {
+    if (len < sizeof(struct ble_hci_ev_command_complete)) {
+        return BLE_HS_ECONTROLLER;
+    }
     ble_gap_rx_test_evt(data, len);
 
     return 0;
@@ -799,7 +802,7 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data,
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
         /* RPA needs to be resolved here, as controller is not aware of the
          * address is RPA in Host based RPA  */
-        if (ble_host_rpa_enabled() && ((!memcmp(evt.local_rpa, ble_hs_conn_null_addr, 6)) == 0)) {
+        if (ble_host_rpa_enabled() && (memcmp(evt.local_rpa, ble_hs_conn_null_addr, 6) == 0)) {
             uint8_t *local_id_rpa = ble_hs_get_rpa_local();
             if (local_id_rpa) {
                 memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN);
@@ -952,7 +955,7 @@ ble_hs_hci_evt_le_adv_rpt_first_pass(const void *data, unsigned int len)
 
         rpt = data;
 
-        if (rpt->data_len > len) {
+        if (sizeof(*rpt) + 1 + rpt->data_len > len) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
             return BLE_HS_ECONTROLLER;
         }
@@ -992,12 +995,18 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len)
 
     /* BLE Queue Congestion check*/
 #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK)
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hci_ctx) {
+#endif
     if (ble_get_adv_list_length() > BLE_ADV_LIST_MAX_LENGTH || ble_adv_list_count > BLE_ADV_LIST_MAX_COUNT) {
         ble_adv_list_refresh();
     }
     ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER);
     ble_adv_list_count++;
     ble_npl_mutex_release(&adv_list_lock);
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    }
+#endif
 #endif
 
     for (i = 0; i < ev->num_reports; i++) {
@@ -1005,7 +1014,12 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len)
 
     /* Avoiding further processing, if the adv report is from the same device*/
 #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK)
-    if (ble_check_adv_list(rpt->addr, rpt->addr_type) == true) {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hci_ctx &&
+#else
+    if (
+#endif
+        ble_check_adv_list(rpt->addr, rpt->addr_type) == true) {
         data += sizeof(*rpt) + rpt->data_len + 1;
         continue;
     }
@@ -1150,11 +1164,11 @@ int
 ble_hs_iso_evt_rx_cb_set(void *cb)
 {
     if (cb == NULL) {
-        return -BLE_HS_EINVAL;
+        return BLE_HS_EINVAL;
     }
 
     if (iso_evt_cb) {
-        return -BLE_HS_EALREADY;
+        return BLE_HS_EALREADY;
     }
 
     iso_evt_cb = cb;
@@ -1370,7 +1384,7 @@ ble_hs_hci_evt_le_ext_adv_rpt_first_pass(const void *data, unsigned int len)
 
         report = data;
 
-        if (report->data_len > len) {
+        if (sizeof(*report) + report->data_len > len) {
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
             return BLE_HS_ECONTROLLER;
         }
@@ -1413,7 +1427,7 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data,
 
         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);
+            legacy_event_type = ble_hs_hci_decode_legacy_type(desc.props);
             if (legacy_event_type < 0) {
                 report = (const void *) &report->data[report->data_len];
                 continue;
@@ -1550,13 +1564,22 @@ ble_hs_hci_evt_le_periodic_adv_sync_transfer(uint8_t subevent, const void *data,
 {
 #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
     const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev = data;
+    struct ble_hci_ev_le_subev_periodic_adv_sync_transfer ev_copy;
 
+#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES)
+    /* V1 event is 4 bytes shorter (lacks PAwR fields); accept both lengths */
+    unsigned int v1_len = sizeof(*ev) - 4;
+    if (len != sizeof(*ev) && len != v1_len) {
+#else
     if (len != sizeof(*ev)) {
+#endif
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBADDATA);
         return BLE_HS_EBADDATA;
     }
 
-    ble_gap_rx_periodic_adv_sync_transfer(ev);
+    memset(&ev_copy, 0, sizeof(ev_copy));
+    memcpy(&ev_copy, ev, len);
+    ble_gap_rx_periodic_adv_sync_transfer(&ev_copy);
 
 #endif
     return 0;
@@ -1634,6 +1657,22 @@ ble_hs_hci_evt_le_adv_set_terminated(uint8_t subevent, const void *data,
             ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle);
             pend_conn_complete_valid = false;
         }
+    } else {
+        /* Advertising set terminated with error. Clear a stale pending
+         * connection-complete only if it is itself an error (e.g. directed
+         * advertising timeout, status != 0). A real connection (status == 0)
+         * may belong to a *different* advertising set whose Advertising Set
+         * Terminated (success) event has not arrived yet; clearing it here
+         * would silently drop that connection at the host level. */
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+        if (ble_hci_ctx) {
+#endif
+            if (!pend_conn_complete_valid || pend_conn_complete.status != 0) {
+                pend_conn_complete_valid = false;
+            }
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+        }
+#endif
     }
     ble_gap_rx_adv_set_terminated(ev);
 #endif
@@ -1772,7 +1811,6 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data
     const struct ble_hci_ev_le_subev_periodic_adv_resp_rep *ev = data;
     const struct periodic_adv_response *response;
     struct ble_gap_periodic_adv_response resp;
-    uint32_t size;
 
     if (len < 5) {  // minimum size for fixed fields
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
@@ -1797,16 +1835,6 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data
         return BLE_HS_ECONTROLLER;
     }
 
-    /* TODO: compare with the total length including the response data. */
-    size = sizeof(*ev);
-    for (uint8_t i = 0; i < ev->num_responses; i ++) {
-        size += sizeof(struct periodic_adv_response) + ev->responses[i].data_length;
-    }
-    if (len < size) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER);
-        return BLE_HS_ECONTROLLER;
-    }
-
     len -= sizeof(*ev);
     data += sizeof(*ev);
 
@@ -2019,13 +2047,23 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev)
     STATS_INC(ble_hs_stats, hci_event);
 
     if(ev->opcode == BLE_HCI_EVCODE_COMMAND_COMPLETE) {
-        if (ev->length < sizeof(struct ble_hci_ev_command_complete)) {
+        if (ev->length < sizeof(struct ble_hci_ev_command_complete_nop)) {
              STATS_INC(ble_hs_stats, hci_unknown_event);
              rc = BLE_HS_ECONTROLLER;
              goto done;
         }
         /* Check if this Command complete has a parsable opcode */
         struct ble_hci_ev_command_complete *cmd_complete = (void *) ev->data;
+        /* NOP command complete has no status byte; skip dispatch for it */
+        if (le16toh(cmd_complete->opcode) == BLE_HCI_OPCODE_NOP) {
+             rc = 0;
+             goto done;
+        }
+        if (ev->length < sizeof(struct ble_hci_ev_command_complete)) {
+             STATS_INC(ble_hs_stats, hci_unknown_event);
+             rc = BLE_HS_ECONTROLLER;
+             goto done;
+        }
         entry = ble_hs_hci_evt_dispatch_find(cmd_complete->opcode);
     }
     else {
@@ -2042,7 +2080,7 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev)
            BLE_HS_LOG(DEBUG, "ble_hs_event_rx_hci_ev; opcode=0x%x ", ev->opcode);
 
 	   /* For LE Meta, print subevent code */
-           if(ev->opcode == 0x3e) {
+           if(ev->opcode == 0x3e && ev->length >= 1) {
               BLE_HS_LOG(DEBUG, "subevent: 0x%x", ev->data[0]);
            }
 
@@ -2139,12 +2177,11 @@ void ble_adv_list_init(void)
 void ble_adv_list_deinit(void)
 {
     struct ble_addr_list_entry *device;
-    struct ble_addr_list_entry *temp;
 
     ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER);
 
-    SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) {
-        SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next);
+    while ((device = SLIST_FIRST(&ble_adv_list)) != NULL) {
+        SLIST_REMOVE_HEAD(&ble_adv_list, next);
         nimble_platform_mem_free(device);
     }
 
@@ -2162,6 +2199,13 @@ void ble_adv_list_add_packet(void *data)
         return;
     }
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hci_ctx) {
+        /* List takes ownership of data; free here to avoid leak when ctx absent. */
+        nimble_platform_mem_free(data);
+        return;
+    }
+#endif
     ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER);
 
     device = (struct ble_addr_list_entry *)data;
@@ -2175,6 +2219,11 @@ uint32_t ble_get_adv_list_length(void)
     uint32_t length = 0;
     struct ble_addr_list_entry *device;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hci_ctx) {
+        return 0;
+    }
+#endif
     ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER);
     SLIST_FOREACH(device, &ble_adv_list, next) {
         length++;
@@ -2187,7 +2236,6 @@ uint32_t ble_get_adv_list_length(void)
 void ble_adv_list_refresh(void)
 {
     struct ble_addr_list_entry *device;
-    struct ble_addr_list_entry *temp;
 
     ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER);
 
@@ -2195,8 +2243,8 @@ void ble_adv_list_refresh(void)
         BLE_HS_LOG(DEBUG, "%s: ble_adv_list empty — reinitializing", __func__);
         SLIST_INIT(&ble_adv_list);
     } else {
-        SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) {
-            SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next);
+        while ((device = SLIST_FIRST(&ble_adv_list)) != NULL) {
+            SLIST_REMOVE_HEAD(&ble_adv_list, next);
             nimble_platform_mem_free(device);
         }
     }
@@ -2216,6 +2264,12 @@ bool ble_check_adv_list(const uint8_t *addr, uint8_t addr_type)
         return found;
     }
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_hci_ctx) {
+        return false;
+    }
+#endif
+
     ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER);
 
     SLIST_FOREACH(device, &ble_adv_list, next) {

nimble/host/src/ble_hs_hci_priv.h

@@ -83,7 +83,13 @@ struct ble_hs_hci_sup_cmd {
     uint8_t commands[64];
 };
 
+uint16_t ble_hs_hci_get_avail_pkts(void);
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+#define ble_hs_hci_avail_pkts         (ble_hs_hci_get_avail_pkts())
+#else
 extern uint16_t ble_hs_hci_avail_pkts;
+#endif
 
 /* This function is not waiting for command status/complete HCI events */
 int ble_hs_hci_cmd_tx_no_rsp(uint16_t opcode, const void *cmd, uint8_t cmd_len);
@@ -145,9 +151,9 @@ int ble_hs_hci_rd_all_local_supp_features(uint8_t *status, uint8_t *max_page,
 int ble_hs_hci_rd_all_remote_features(uint16_t conn_handle, uint8_t page_requested);
 
 #if MYNEWT_VAL(BLE_MONITOR_ADV)
-int ble_hs_hci_add_monitor_adv_list(uint8_t addr_type, uint8_t *addr, uint8_t rssi_low,
-                                    uint8_t rssi_high, uint8_t timeout);
-int ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, uint8_t *addr);
+int ble_hs_hci_add_monitor_adv_list(uint8_t addr_type, uint8_t *addr, int8_t rssi_low,
+                                    int8_t rssi_high, uint8_t timeout);
+int ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, const uint8_t *addr);
 int ble_hs_hci_clear_monitor_adv_list(void);
 int ble_hs_hci_read_monitor_adv_list_size(uint8_t *out_number);
 int ble_hs_hci_enable_monitor_adv(uint8_t enable);

nimble/host/src/ble_hs_hci_util.c

@@ -80,6 +80,11 @@ ble_hs_hci_util_rand(void *dst, int len)
         return BLE_HS_EINVAL;
     }
 
+    if (dst == NULL) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
+
     u8ptr = dst;
     while (len > 0) {
         rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RAND),
@@ -495,7 +500,7 @@ ble_hs_hci_add_monitor_adv_list(uint8_t addr_type, uint8_t *addr, int8_t rssi_lo
 }
 
 int
-ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, uint8_t *addr)
+ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, const uint8_t *addr)
 {
     struct ble_hci_le_rmv_monitor_adv_list_cp cmd;
 

nimble/host/src/ble_hs_id.c

@@ -88,13 +88,14 @@ ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type)
 int
 ble_hs_id_set_pub(const uint8_t *pub_addr)
 {
+    ble_hs_lock();
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_id_ensure_ctx()) {
+        ble_hs_unlock();
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
         return BLE_HS_ENOMEM;
     }
 #endif
-    ble_hs_lock();
     memcpy(ble_hs_id_pub, pub_addr, 6);
     ble_hs_unlock();
     return 0;
@@ -136,19 +137,19 @@ ble_hs_id_set_nrpa_rnd(void)
     ble_addr_t nrpa_addr;
     int rc;
 
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    if (ble_hs_id_ensure_ctx()) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
-        return BLE_HS_ENOMEM;
-    }
-#endif
-
     rc = ble_hs_id_gen_rnd(1, &nrpa_addr);
     if (rc != 0) {
         return rc;
     }
 
     ble_hs_lock();
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hs_id_ensure_ctx()) {
+        ble_hs_unlock();
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
+        return BLE_HS_ENOMEM;
+    }
+#endif
 
     /* set the NRPA address as pseudo random address in controller */
     rc = ble_hs_hci_util_set_random_addr(nrpa_addr.val);
@@ -182,15 +183,15 @@ ble_hs_id_set_pseudo_rnd(const uint8_t *rnd_addr)
     int rc;
     int ones;
 
+    ble_hs_lock();
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_id_ensure_ctx()) {
+        ble_hs_unlock();
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
         return BLE_HS_ENOMEM;
     }
 #endif
 
-    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;
@@ -226,15 +227,16 @@ ble_hs_id_set_rnd(const uint8_t *rnd_addr)
     int rc;
     int ones;
 
+    ble_hs_lock();
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    if (ble_hs_id_ensure_ctx()) {
-        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
-        return BLE_HS_EINVAL;
+    rc = ble_hs_id_ensure_ctx();
+    if (rc != 0) {
+        ble_hs_unlock();
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc);
+        return rc;
     }
 #endif
 
-    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;
@@ -471,12 +473,13 @@ done:
 void
 ble_hs_id_reset(void)
 {
+    ble_hs_lock();
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_id_ensure_ctx()) {
+        ble_hs_unlock();
         return;
     }
 #endif
-    ble_hs_lock();
     memset(ble_hs_id_pub, 0, sizeof ble_hs_id_pub);
     memset(ble_hs_id_rnd, 0, sizeof ble_hs_id_rnd);
     ble_hs_unlock();

nimble/host/src/ble_hs_iso.c

@@ -183,6 +183,10 @@ ble_hs_hci_iso_tx_now(uint16_t conn_handle, const uint8_t *sdu, uint16_t sdu_len
     uint8_t *frag;
     int rc;
 
+    if (sdu == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
 #if MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL)
     /* Get the Controller ISO buffer needed for the SDU */
     uint8_t count = ble_hs_hci_iso_buf_needed(sdu_len, ts_flag);
@@ -222,7 +226,8 @@ ble_hs_hci_iso_tx_now(uint16_t conn_handle, const uint8_t *sdu, uint16_t sdu_len
      */
     rc = ble_hci_trans_hs_iso_tx(frag, BLE_HCI_ISO_DATA_HDR_SZ + dlh_len + sdu_len, NULL);
     if (rc) {
-        return BLE_HS_EDONE;
+        nimble_platform_mem_free(frag);
+        return BLE_HS_ECONTROLLER;
     }
 
 #if MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL)
@@ -267,8 +272,10 @@ ble_hs_iso_pkt_rx_cb_set(ble_hs_iso_pkt_rx_fn cb)
 int
 ble_hs_rx_iso_data(const uint8_t *data, uint16_t len, void *arg)
 {
+    int rc = 0;
+
     if (ble_hs_iso_pkt_rx_cb) {
-        ble_hs_iso_pkt_rx_cb(data, len, arg);
+        rc = ble_hs_iso_pkt_rx_cb(data, len, arg);
     } else {
         BLE_HS_LOG(WARN, "ISO RX: no callback registered");
     }
@@ -277,7 +284,7 @@ ble_hs_rx_iso_data(const uint8_t *data, uint16_t len, void *arg)
     // TODO: add adapter for controller free.
     free((void *)data);
 
-    return 0;
+    return rc;
 }
 
 #endif /* MYNEWT_VAL(BLE_ISO) */

nimble/host/src/ble_hs_iso_hci.c

@@ -49,8 +49,8 @@ ble_hs_hci_read_local_supp_codec_caps(uint8_t coding_fmt, uint16_t company_id,
     }
 
     cmd.coding_fmt = coding_fmt;
-    cmd.company_id = company_id;
-    cmd.vs_codec_id = vs_codec_id;
+    cmd.company_id = htole16(company_id);
+    cmd.vs_codec_id = htole16(vs_codec_id);
     cmd.logical_tpt_type = logical_transport_type;
     cmd.direction = direction;
 
@@ -85,8 +85,8 @@ ble_hs_hci_read_local_supp_controller_delay(uint8_t coding_fmt, uint16_t company
     cmd = (void *)cmd_buf;
 
     cmd->coding_fmt = coding_fmt;
-    cmd->company_id = company_id;
-    cmd->vs_codec_id = vs_codec_id;
+    cmd->company_id = htole16(company_id);
+    cmd->vs_codec_id = htole16(vs_codec_id);
     cmd->logical_tpt_type = logical_transport_type;
     cmd->direction = direction;
     cmd->codec_cfg_len = codec_cfg_len;
@@ -262,7 +262,7 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin
     }
 
     if (rsp_buf == NULL||
-        rsp_len < sizeof(struct ble_hci_le_set_cig_params_rp) + cis_cnt * 2) {
+        rsp_len < sizeof(struct ble_hci_le_set_cig_params_test_rp) + cis_cnt * 2) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
@@ -624,7 +624,7 @@ ble_hs_hci_iso_test_end(uint16_t conn_handle, uint32_t *received_sdu_count,
     int rc;
 
     if (!received_sdu_count || !missed_sdu_count || !failed_sdu_count) {
-        return BLE_ERR_INV_HCI_CMD_PARMS;
+        return BLE_HS_EINVAL;
     }
 
     cmd.conn_handle = htole16(conn_handle);
@@ -636,6 +636,10 @@ ble_hs_hci_iso_test_end(uint16_t conn_handle, uint32_t *received_sdu_count,
         return rc;
     }
 
+    if (le16toh(rsp.conn_handle) != conn_handle) {
+        return BLE_HS_ECONTROLLER;
+    }
+
     *received_sdu_count = le32toh(rsp.received_sdu_count);
     *missed_sdu_count = le32toh(rsp.missed_sdu_count);
     *failed_sdu_count = le32toh(rsp.failed_sdu_count);

nimble/host/src/ble_hs_log.c

@@ -73,7 +73,9 @@ ble_hs_log_mbuf(const struct os_mbuf *om)
             chunk_len = BLE_HS_LOG_HEX_BYTES_PER_LINE;
         }
 
-        os_mbuf_copydata(om, offset, chunk_len, buf);
+        if (os_mbuf_copydata(om, offset, chunk_len, buf) != 0) {
+            break;
+        }
         ble_hs_log_debug_hex_chunk(buf, chunk_len);
     }
 #endif

nimble/host/src/ble_hs_periodic_sync.c

@@ -79,6 +79,8 @@ ble_hs_periodic_sync_alloc(void)
 {
     struct ble_hs_periodic_sync *psync;
 
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
     psync = os_memblock_get(&ble_hs_periodic_sync_pool);
     if (psync) {
         memset(psync, 0, sizeof(*psync));
@@ -119,8 +121,8 @@ 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);
+    BLE_HS_DBG_ASSERT(
+        ble_hs_periodic_sync_find_by_handle(psync->sync_handle) == NULL);
 
     SLIST_INSERT_HEAD(&g_ble_hs_periodic_sync_handles, psync, next);
 }
@@ -128,10 +130,23 @@ 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 *cur;
+    struct ble_hs_periodic_sync *prev;
+
     BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
 
-    SLIST_REMOVE(&g_ble_hs_periodic_sync_handles, psync, ble_hs_periodic_sync,
-                 next);
+    prev = NULL;
+    SLIST_FOREACH(cur, &g_ble_hs_periodic_sync_handles, next) {
+        if (cur == psync) {
+            if (prev == NULL) {
+                SLIST_REMOVE_HEAD(&g_ble_hs_periodic_sync_handles, next);
+            } else {
+                SLIST_NEXT(prev, next) = SLIST_NEXT(cur, next);
+            }
+            return;
+        }
+        prev = cur;
+    }
 }
 
 struct ble_hs_periodic_sync *

nimble/host/src/ble_hs_pvcy.c

@@ -80,7 +80,7 @@ ble_hs_pvcy_set_addr_timeout(uint16_t timeout)
     struct ble_hci_le_set_rpa_tmo_cp cmd;
 
     if (timeout == 0 || timeout > BLE_MAX_RPA_TIMEOUT_VAL) {
-        return BLE_ERR_INV_HCI_CMD_PARMS;
+        return BLE_HS_EINVAL;
     }
 
     cmd.rpa_timeout = htole16(timeout);
@@ -99,9 +99,12 @@ int ble_hs_set_rpa_timeout(uint16_t timeout)
         return BLE_HS_ENOMEM;
     }
 #endif
-    l_rpa_timeout = timeout;
+    int rc = ble_hs_pvcy_set_addr_timeout(timeout);
+    if (rc == 0) {
+        l_rpa_timeout = timeout;
+    }
 
-    return ble_hs_pvcy_set_addr_timeout(l_rpa_timeout);
+    return rc;
 }
 
 uint16_t ble_hs_get_rpa_timeout(void)
@@ -260,6 +263,7 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t 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) {
+        ble_hs_pvcy_remove_entry(addr_type, addr);
         return rc;
     }
 #endif
@@ -308,6 +312,13 @@ ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type,
 {
     int rc;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_hs_pvcy_ctx == NULL) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
+        return BLE_HS_ENOMEM;
+    }
+#endif
+
     STATS_INC(ble_hs_stats, pvcy_add_entry);
 
     /* No GAP procedures can be active when adding an entry to the resolving
@@ -420,8 +431,13 @@ void ble_hs_pvcy_set_default_irk(void)
         }
     }
 
-    void ble_store_config_init(void);
-    ble_store_config_init();
+    /* Only call ble_store_config_init on the first initialization.
+     * Calling it at runtime resets all in-memory bonds/CCCDs.
+     */
+    if (!ble_hs_pvcy_ctx->pvcy_started) {
+        void ble_store_config_init(void);
+        ble_store_config_init();
+    }
 #endif
 
     rc = ble_store_read_local_irk(&key_local_irk, &value_local_irk);
@@ -457,16 +473,18 @@ void ble_hs_pvcy_set_default_irk(void)
     }
 }
 
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 void
 ble_hs_pvcy_irk_deinit(void)
 {
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_pvcy_ctx) {
         nimble_platform_mem_free(ble_hs_pvcy_ctx);
         ble_hs_pvcy_ctx = NULL;
     }
-}
+#else
+    ble_hs_pvcy_started = 0;
 #endif
+}
 
 int
 ble_hs_pvcy_set_our_irk(const uint8_t *irk)
@@ -574,12 +592,15 @@ ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode)
         return BLE_HS_EINVAL;
     }
 
-    if (addr->type > BLE_ADDR_RANDOM) {
-        return BLE_ERR_INV_HCI_CMD_PARMS;
+    /* Reject address types that are not in the expected set */
+    if (addr->type != BLE_ADDR_PUBLIC && addr->type != BLE_ADDR_RANDOM &&
+        addr->type != BLE_ADDR_PUBLIC_ID && addr->type != BLE_ADDR_RANDOM_ID) {
+        return BLE_HS_EINVAL;
     }
 
     cmd.mode = priv_mode;
-    cmd.peer_id_addr_type = addr->type;
+    /* HCI only accepts public (0) or random (1); map identity types to base */
+    cmd.peer_id_addr_type = addr->type % 2;
     memcpy(cmd.peer_id_addr, addr->val, BLE_DEV_ADDR_LEN);
 
     return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
@@ -603,6 +624,14 @@ int
 ble_hs_pvcy_rpa_config(uint8_t enable)
 {
     int rc = 0;
+    bool was_resolv_enabled;
+    bool was_nrpa;
+
+    if (enable != NIMBLE_HOST_DISABLE_PRIVACY &&
+        enable != NIMBLE_HOST_ENABLE_RPA &&
+        enable != NIMBLE_HOST_ENABLE_NRPA) {
+        return BLE_HS_EINVAL;
+    }
 
     if (enable != NIMBLE_HOST_DISABLE_PRIVACY) {
         rc = ble_hs_pvcy_ensure_started();
@@ -610,6 +639,9 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
             return rc;
         }
 
+        was_resolv_enabled = is_ble_hs_resolv_enabled();
+        was_nrpa = was_resolv_enabled && !ble_host_rpa_enabled();
+
         ble_hs_resolv_enable(true);
 
         /* Configure NRPA address related flags according to input parameter */
@@ -621,6 +653,16 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
 
         /* Generate local RPA address and set it in controller */
         rc = ble_hs_gen_own_private_rnd();
+        if (rc != 0) {
+            if (!was_resolv_enabled) {
+                ble_hs_resolv_enable(false);
+            }
+            if (was_nrpa) {
+                ble_hs_resolv_nrpa_enable();
+            } else {
+                ble_hs_resolv_nrpa_disable();
+            }
+        }
     } else {
         ble_hs_resolv_enable(false);
     }

nimble/host/src/ble_hs_pvcy_priv.h

@@ -50,9 +50,7 @@ bool ble_hs_pvcy_enabled(void);
 #endif
 bool ble_hs_pvcy_resolve_with_irk(const uint8_t rpa[6], const uint8_t irk[16]);
 
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 void ble_hs_pvcy_irk_deinit(void);
-#endif
 #ifdef __cplusplus
 }
 #endif

nimble/host/src/ble_hs_resolv.c

@@ -367,6 +367,7 @@ ble_rpa_resolv_add_peer_rec(uint8_t *peer_addr)
     p_dev_rec = &peer_dev_rec[ble_store_num_peer_dev_rec];
 
     p_dev_rec->rec_used = 1;
+    p_dev_rec->rand_addr_type = BLE_ADDR_RANDOM;
     memcpy(p_dev_rec->pseudo_addr, peer_addr, BLE_DEV_ADDR_LEN);
     memcpy(p_dev_rec->rand_addr, peer_addr, BLE_DEV_ADDR_LEN);
     memcpy(p_dev_rec->identity_addr, peer_addr, BLE_DEV_ADDR_LEN);
@@ -525,9 +526,9 @@ ble_hs_rand_prand_get(uint8_t *prand)
 {
     uint16_t sum;
     int rc;
-    int retry = 100;
+    int i;
 
-    while (retry--) {
+    for (i = 0; i < 100; i++) {
         /* Get 24 bits of random data */
         rc = ble_hs_hci_util_rand(prand, 3);
         if (rc != 0) {
@@ -537,20 +538,15 @@ ble_hs_rand_prand_get(uint8_t *prand)
         /* 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;
+            return 0;
         }
     }
 
-    if (retry <= 0) {
-        BLE_HS_LOG(ERROR, "Failed to generate random prand\n");
-        return BLE_HS_ETIMEOUT;
-    }
-
-    /* Upper two bits must be 01 */
-    prand[2] &= ~0xc0;
-    prand[2] |= 0x40;
-
-    return 0;
+    BLE_HS_LOG(ERROR, "Failed to generate random prand\n");
+    return BLE_HS_ETIMEOUT;
 }
 
 static bool
@@ -674,9 +670,9 @@ ble_hs_get_rpa_local(void)
 static void
 ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev)
 {
-#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     int rc;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_resolv_ctx == NULL) {
         rc = ble_hs_resolv_ctx_ensure_init();
         if (rc != 0) {
@@ -689,7 +685,10 @@ ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev)
         BLE_HS_LOG(DEBUG, "RPA/NRPA Timeout; start active adv & scan with new Private address \n");
         ble_gap_preempt();
         /* Generate local private address */
-        ble_hs_gen_own_private_rnd();
+        rc = ble_hs_gen_own_private_rnd();
+        if (rc != 0) {
+            BLE_HS_LOG(ERROR, "RPA/NRPA generation failed; rc=%d\n", rc);
+        }
         ble_npl_callout_reset(&g_ble_hs_resolv_data.rpa_timer,
                               (int32_t)g_ble_hs_resolv_data.rpa_tmo);
         ble_gap_preempt_done();
@@ -717,7 +716,9 @@ ble_hs_is_on_resolv_list(uint8_t *addr, uint8_t addr_type)
     struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[1];
 
     for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) {
-        if ((!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN)) || (!memcmp(rl->rl_peer_rpa, addr, BLE_DEV_ADDR_LEN))) {
+        if (rl->rl_addr_type == addr_type &&
+            ((!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN)) ||
+             (!memcmp(rl->rl_peer_rpa, addr, BLE_DEV_ADDR_LEN)))) {
             return i;
         }
         ++rl;
@@ -871,17 +872,17 @@ ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr)
     if (position) {
         memmove(&g_ble_hs_resolv_list[position],
                 &g_ble_hs_resolv_list[position + 1],
-                (g_ble_hs_resolv_data.rl_cnt - position) * sizeof (struct
+                (g_ble_hs_resolv_data.rl_cnt - position - 1) * sizeof (struct
                         ble_hs_resolv_entry));
         --g_ble_hs_resolv_data.rl_cnt;
 
         rc = 0;
     }
 
-    /* As we are removing the RL record, it is needed to change
-     * peer_address to its latest received OTA address, this helps when existing bond at
-     * peer side is removed */
-    ble_rpa_replace_id_with_rand_addr(&addr_type, ident_addr);
+    /* Only replace identity addr with RPA if the entry was actually removed */
+    if (rc == 0) {
+        ble_rpa_replace_id_with_rand_addr(&addr_type, ident_addr);
+    }
 #endif
 
     return rc;
@@ -1078,7 +1079,6 @@ ble_hs_resolv_rpa_addr(uint8_t *addr, uint8_t addr_type) {
     for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) {
         if(ble_hs_resolv_rpa(addr, rl->rl_peer_irk) == 0) {
             memcpy(g_ble_hs_resolv_list[i].rl_peer_rpa, addr, BLE_DEV_ADDR_LEN);
-            g_ble_hs_resolv_list[i].rl_addr_type = addr_type;
             return rl;
         }
 

nimble/host/src/ble_hs_resolv_priv.h

@@ -109,6 +109,8 @@ int ble_hs_resolv_rpa(uint8_t *rpa, uint8_t *irk);
 /* Initialize resolv*/
 void ble_hs_resolv_init(void);
 
+uint8_t is_ble_hs_resolv_enabled(void);
+
 #ifdef __cplusplus
 }
 #endif

nimble/host/src/ble_hs_startup.c

@@ -209,17 +209,21 @@ ble_hs_startup_read_buf_sz(void)
         }
     }
 
+    ble_hs_lock();
     rc = ble_hs_hci_set_buf_sz(pktlen, max_pkts);
+    ble_hs_unlock();
     if (rc != 0) {
         return rc;
     }
 
 #if MYNEWT_VAL(BLE_ISO)
-    ble_hs_lock();
-    rc = ble_hs_hci_set_iso_buf_sz(iso_pktlen, iso_max_pkts);
-    ble_hs_unlock();
-    if (rc != 0) {
-        return rc;
+    if (iso_pktlen != 0 && iso_max_pkts != 0) {
+        ble_hs_lock();
+        rc = ble_hs_hci_set_iso_buf_sz(iso_pktlen, iso_max_pkts);
+        ble_hs_unlock();
+        if (rc != 0) {
+            return rc;
+        }
     }
 #endif /* MYNEWT_VAL(BLE_ISO) */
 
@@ -400,7 +404,7 @@ ble_hs_startup_le_set_evmask_tx(void)
 #endif /* MYNEWT_VAL(BLE_ISO) */
 
 #if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
-    if (version >= BLE_HCI_VER_BCS_5_4) {
+    if (version >= BLE_HCI_VER_BCS_6_0) {
         /**
          * Enable the following LE events:
          * 0x0000080000000000 LE CS Read Remote Supported Capabilities Complete event

nimble/host/src/ble_hs_stop.c

@@ -90,11 +90,20 @@ ble_hs_stop_done(int status)
     slist = ble_hs_stop_listeners;
     SLIST_INIT(&ble_hs_stop_listeners);
 
-    ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
+    if (status == 0) {
+        ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF;
+    } else {
+        ble_hs_enabled_state = BLE_HS_ENABLED_STATE_ON;
+    }
 
     ble_hs_unlock();
 
-    SLIST_FOREACH(listener, &slist, link) {
+    if (status != 0) {
+        ble_hs_timer_resched();
+    }
+
+    struct ble_hs_stop_listener *listener_next;
+    SLIST_FOREACH_SAFE(listener, &slist, link, listener_next) {
         listener->fn(status, listener->arg);
     }
 }
@@ -169,8 +178,7 @@ 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);
+    ble_hs_stop_done(BLE_HS_ETIMEOUT);
 }
 
 /**
@@ -208,8 +216,20 @@ static void
 ble_hs_stop_register_listener(struct ble_hs_stop_listener *listener,
                               ble_hs_stop_fn *fn, void *arg)
 {
+    struct ble_hs_stop_listener *cur;
+
     BLE_HS_DBG_ASSERT(fn != NULL);
 
+    /* Prevent duplicate insertion which would create a cycle in the list. */
+    SLIST_FOREACH(cur, &ble_hs_stop_listeners, link) {
+        if (cur == listener) {
+            /* Update fn/arg in case caller re-registers with new values */
+            listener->fn = fn;
+            listener->arg = arg;
+            return;
+        }
+    }
+
     listener->fn = fn;
     listener->arg = arg;
     SLIST_INSERT_HEAD(&ble_hs_stop_listeners, listener, link);
@@ -219,6 +239,10 @@ static int
 ble_hs_stop_begin(struct ble_hs_stop_listener *listener,
                    ble_hs_stop_fn *fn, void *arg)
 {
+    if (listener != NULL && fn == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
     switch (ble_hs_enabled_state) {
     case BLE_HS_ENABLED_STATE_ON:
         /* Host is enabled; proceed with the stop procedure. */
@@ -339,12 +363,14 @@ ble_hs_stop_init(void)
 void
 ble_hs_stop_deinit(void)
 {
-    ble_npl_callout_deinit(&ble_hs_stop_terminate_tmo);
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_stop_ctx) {
+        ble_npl_callout_deinit(&ble_hs_stop_terminate_tmo);
         nimble_platform_mem_free(ble_hs_stop_ctx);
         ble_hs_stop_ctx = NULL;
     }
+#else
+    ble_npl_callout_deinit(&ble_hs_stop_terminate_tmo);
 #endif
 
 }

nimble/host/src/ble_ibeacon.c

@@ -64,16 +64,19 @@ ble_ibeacon_set_adv_data(const void *uuid128, uint16_t major,
         return BLE_HS_EINVAL;
     }
 
+    ble_hs_lock();
+
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-if (buf == NULL) {
-    /* Since the data used by buf is needed for adv reattempts,
-     * this data shall stay in the heap and not be freed.
-     */
-    buf = nimble_platform_mem_calloc(BLE_IBEACON_MFG_DATA_SIZE, sizeof(uint8_t));
     if (buf == NULL) {
-        return BLE_HS_ENOMEM;
+        /* Since the data used by buf is needed for adv reattempts,
+         * this data shall stay in the heap and not be freed.
+         */
+        buf = nimble_platform_mem_calloc(BLE_IBEACON_MFG_DATA_SIZE, sizeof(uint8_t));
+        if (buf == NULL) {
+            ble_hs_unlock();
+            return BLE_HS_ENOMEM;
+        }
     }
-}
 #endif
 
     /** Company identifier (Apple). */
@@ -106,5 +109,6 @@ if (buf == NULL) {
                    BLE_HS_ADV_F_BREDR_UNSUP;
 
     rc = ble_gap_adv_set_fields(&fields);
+    ble_hs_unlock();
     return rc;
 }

nimble/host/src/ble_l2cap.c

@@ -117,15 +117,15 @@ ble_l2cap_chan_free(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
 
     ble_l2cap_coc_cleanup_chan(conn, chan);
 
-#if MYNEWT_VAL(BLE_HS_DEBUG)
-    memset(chan, 0xff, sizeof *chan);
-#endif
-
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_l2cap_ctx == NULL) {
         return;
     }
 #endif
+
+#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);
@@ -241,9 +241,18 @@ ble_l2cap_reconfig(struct ble_l2cap_chan *chans[], uint8_t num, uint16_t new_mtu
         return BLE_HS_EINVAL;
     }
 
+    if (chans[0] == NULL) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
+
     conn_handle = chans[0]->conn_handle;
 
     for (i = 1; i < num; i++) {
+        if (chans[i] == NULL) {
+            BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+            return BLE_HS_EINVAL;
+        }
         if (conn_handle != chans[i]->conn_handle) {
             BLE_HS_LOG(ERROR, "All channels should have same conn handle\n");
             return BLE_HS_EINVAL;
@@ -582,11 +591,16 @@ ble_l2cap_init(void)
     return 0;
 done:
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    ble_l2cap_sig_deinit();
+    ble_l2cap_coc_deinit();
+    ble_sm_deinit();
+
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
     nimble_platform_mem_free(ble_l2cap_chan_mem);
     ble_l2cap_chan_mem = NULL;
 #endif
     os_mempool_unregister(&ble_l2cap_chan_pool);
+    memset(&ble_l2cap_chan_pool, 0, sizeof(ble_l2cap_chan_pool));
     nimble_platform_mem_free(ble_l2cap_ctx);
     ble_l2cap_ctx = NULL;
 #endif

nimble/host/src/ble_l2cap_coc.c

@@ -106,11 +106,21 @@ ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu,
 {
     struct ble_l2cap_coc_srv *srv;
 
+    if (psm < 0x0001 || psm > 0x00FF) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
+
     if (cb == NULL) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
 
+    if (mtu < BLE_L2CAP_COC_MIN_MTU) {
+        BLE_HS_LOG(ERROR, "%s mtu too small rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
+
     ble_hs_lock();
 
     if (ble_l2cap_coc_srv_find(psm) != NULL) {
@@ -142,13 +152,13 @@ ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu,
 static inline void
 ble_l2cap_set_used_cid(uint32_t *cid_mask, int bit)
 {
-    cid_mask[bit / 32] |= (1 << (bit % 32));
+    cid_mask[bit / 32] |= (1U << (bit % 32));
 }
 
 static inline void
 ble_l2cap_clear_used_cid(uint32_t *cid_mask, int bit)
 {
-    cid_mask[bit / 32] &= ~(1 << (bit % 32));
+    cid_mask[bit / 32] &= ~(1U << (bit % 32));
 }
 
 static inline int
@@ -183,7 +193,7 @@ ble_l2cap_coc_get_cid(uint32_t *cid_mask)
     int bit;
 
     bit = ble_l2cap_get_first_available_bit(cid_mask);
-    if (bit < 0) {
+    if (bit < 0 || bit >= MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)) {
         return -1;
     }
 
@@ -219,6 +229,7 @@ ble_l2cap_event_coc_received_data(struct ble_l2cap_chan *chan,
 {
     struct ble_l2cap_event event;
 
+    memset(&event, 0, sizeof event);
     event.type = BLE_L2CAP_EVENT_COC_DATA_RECEIVED;
     event.receive.conn_handle = chan->conn_handle;
     event.receive.chan = chan;
@@ -244,7 +255,16 @@ ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan, struct os_mbuf **om)
     BLE_HS_DBG_ASSERT(rx != NULL);
 
     rx_sdu = rx->sdus[chan->coc_rx.current_sdu_idx];
-    BLE_HS_DBG_ASSERT(rx_sdu != NULL);
+    if (rx_sdu == NULL) {
+        /* Peer used a credit while no RX SDU buffer is armed (app deferred
+         * ble_l2cap_recv_ready). This is a credit/buffer asymmetry — disconnect
+         * rather than dereference a NULL pointer.
+         */
+        BLE_HS_LOG(ERROR, "CoC RX: no SDU buffer armed (idx=%d), disconnecting\n",
+                   chan->coc_rx.current_sdu_idx);
+        ble_l2cap_disconnect(chan);
+        return BLE_HS_ENOMEM;
+    }
 
     om_total = OS_MBUF_PKTLEN(*om);
 
@@ -522,8 +542,8 @@ 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;
+    uint32_t left_to_send;
+    struct os_mbuf *txom = NULL;
     struct ble_hs_conn *conn;
     uint16_t sdu_size_offset;
     int rc;
@@ -549,7 +569,13 @@ ble_l2cap_coc_continue_tx(struct ble_l2cap_chan *chan)
         }
 
         /* Take into account peer MTU */
-        len = min(left_to_send, chan->peer_coc_mps);
+        len = min(left_to_send, (uint32_t)chan->peer_coc_mps);
+
+        /* Guard against underflow: first packet needs at least SDU_SIZE bytes */
+        if (len < sdu_size_offset) {
+            rc = BLE_HS_EINVAL;
+            goto failed;
+        }
 
         /* Prepare packet */
         txom = ble_hs_mbuf_l2cap_pkt();
@@ -690,9 +716,7 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx)
 
     ble_hs_lock();
 
-    if (chan->coc_rx.sdus[0] != NULL &&
-        chan->coc_rx.next_sdu_alloc_idx == chan->coc_rx.current_sdu_idx &&
-        BLE_L2CAP_SDU_BUFF_CNT != 1) {
+    if (chan->coc_rx.sdus[chan->coc_rx.next_sdu_alloc_idx] != NULL) {
         ble_hs_unlock();
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBUSY);
         return BLE_HS_EBUSY;
@@ -724,13 +748,18 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx)
                                  credits_to_send);
         ble_hs_lock();
         /* Re-validate connection and channel after relock — they could have
-         * been freed while the lock was released. */
-        /* Re-validate connection and channel after relock — they could have
-         * been freed while the lock was released. Credits were already updated
-         * at line 714 before unlocking; no further update needed here. */
+         * been freed while the lock was released. coc_rx.credits was already
+         * updated before unlocking; no further update needed here. */
         conn = ble_hs_conn_find(cached_conn_handle);
         if (conn != NULL) {
             c = ble_hs_conn_chan_find_by_scid(conn, cached_scid);
+        } else {
+            c = NULL;
+        }
+
+        if (c == NULL) {
+            ble_hs_unlock();
+            return 0;
         }
     }
 
@@ -824,6 +853,14 @@ void ble_l2cap_coc_deinit(void)
 {
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
      if (ble_l2cap_coc_ctx) {
+        struct ble_l2cap_coc_srv *srv;
+
+        /* Free all active server entries before destroying the pool */
+        while ((srv = STAILQ_FIRST(&ble_l2cap_coc_srvs)) != NULL) {
+            STAILQ_REMOVE_HEAD(&ble_l2cap_coc_srvs, next);
+            os_memblock_put(&ble_l2cap_coc_srv_pool, srv);
+        }
+
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
         if (ble_l2cap_coc_srv_mem) {
              nimble_platform_mem_free(ble_l2cap_coc_srv_mem);

nimble/host/src/ble_l2cap_coc_priv.h

@@ -33,6 +33,9 @@ extern "C" {
 #define BLE_L2CAP_COC_CID_START                 0x0040
 #define BLE_L2CAP_COC_CID_END                   0x007F
 
+/* Core Spec Vol 3 Part A §4.22 — LE Credit Based Flow Control min MTU */
+#define BLE_L2CAP_COC_MIN_MTU                   23
+
 struct ble_l2cap_chan;
 
 #define BLE_L2CAP_COC_FLAG_STALLED              0x01

nimble/host/src/ble_l2cap_sig.c

@@ -410,12 +410,12 @@ static int
 ble_l2cap_sig_check_conn_params(const struct ble_gap_upd_params *params)
 {
     /* Check connection interval min */
-    if ((params->itvl_min < BLE_HCI_CONN_ITVL_MIN) ||
+    if ((params->itvl_min < BLE_HOST_CONN_PARAM_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) ||
+    if ((params->itvl_max < BLE_HOST_CONN_PARAM_ITVL_MIN) ||
         (params->itvl_max > BLE_HCI_CONN_ITVL_MAX) ||
         (params->itvl_max < params->itvl_min)) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -432,6 +432,12 @@ ble_l2cap_sig_check_conn_params(const struct ble_gap_upd_params *params)
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
+    /* supervision_timeout must be greater than (1+latency)*itvl_max*2 (in units) */
+    if ((uint32_t)params->supervision_timeout * 4 <=
+        (uint32_t)(1 + params->latency) * params->itvl_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;
@@ -571,7 +577,8 @@ ble_l2cap_sig_update_rsp_rx(uint16_t conn_handle,
 done:
     ble_l2cap_sig_update_call_cb(proc, cb_status);
     ble_l2cap_sig_proc_free(proc);
-    return rc;
+    /* Silently ignore errors as this is a response signal */
+    return 0;
 }
 
 int
@@ -649,6 +656,29 @@ done:
 
 #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
 
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+static int
+ble_l2cap_sig_reconfig_err2ble_hs_err(uint16_t reconfig_err)
+{
+    switch (reconfig_err) {
+    case BLE_L2CAP_ERR_RECONFIG_SUCCEED:
+        return 0;
+    case BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED:
+    case BLE_L2CAP_ERR_RECONFIG_REDUCTION_MPS_NOT_ALLOWED:
+    case BLE_L2CAP_ERR_RECONFIG_INVALID_DCID:
+        return BLE_HS_EREJECT;
+    case BLE_L2CAP_ERR_RECONFIG_UNACCEPTED_PARAM:
+        BLE_HS_LOG(ERROR, "%s reconfig_err=0x%04x rc=%d\n", __func__,
+                   reconfig_err, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    default:
+        BLE_HS_LOG(ERROR, "%s reconfig_err=0x%04x rc=%d\n", __func__,
+                   reconfig_err, BLE_HS_EUNKNOWN);
+        return BLE_HS_EUNKNOWN;
+    }
+}
+#endif
+
 static int
 ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err)
 {
@@ -679,6 +709,9 @@ ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err)
     case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS:
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
+    case BLE_L2CAP_COC_ERR_INVALID_PARAMETERS:
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
     default:
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
         return BLE_HS_EUNKNOWN;
@@ -866,7 +899,7 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle,
     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) {
+    if (cid_cnt == 0 || cid_cnt > BLE_L2CAP_MAX_COC_CONN_REQ) {
         rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCEPTED_PARAM);
         goto failed;
     }
@@ -878,7 +911,7 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle,
          * Core Specification Vol 3 Part A 4.28
          */
         chan[i] = ble_hs_conn_chan_find_by_scid(conn, host_dcid);
-        if (!chan[i]) {
+        if (!chan[i] || chan[i]->psm == 0) {
              rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_INVALID_DCID);
              goto failed;
         }
@@ -973,7 +1006,8 @@ ble_l2cap_sig_credit_base_reconfig_rsp_rx(uint16_t conn_handle,
     }
 
     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);
+    ble_l2cap_sig_coc_reconfig_cb(proc,
+        ble_l2cap_sig_reconfig_err2ble_hs_err(le16toh(rsp->result)));
 
 done:
     if (rc != 0) {
@@ -1213,21 +1247,26 @@ ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle,
     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));
+        uint16_t result = le16toh(rsp->result);
+        rc = ble_l2cap_sig_coc_err2ble_hs_err(result);
         /* Below results means that some of the channels has not been created
          * and we have to look closer into the response.
          * Any other results means that all the connections has been refused.
          */
-        if ((rsp->result != BLE_L2CAP_COC_ERR_NO_RESOURCES) &&
-            (rsp->result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) &&
-            (rsp->result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) {
+        if ((result != BLE_L2CAP_COC_ERR_NO_RESOURCES) &&
+            (result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) &&
+            (result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) {
             goto done;
         }
     }
 
     ble_hs_lock();
     conn = ble_hs_conn_find(conn_handle);
-    assert(conn != NULL);
+    if (conn == NULL) {
+        ble_hs_unlock();
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
 
     for (i = 0; i < proc->connect.chan_cnt; i++) {
         chan = proc->connect.chan[i];
@@ -1375,6 +1414,10 @@ ble_l2cap_sig_coc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
 
     rc = ble_l2cap_sig_tx(conn_handle, txom);
     if (rc != 0) {
+        /* Notify app before suppressing callback during channel teardown. */
+        ble_l2cap_event_coc_connected(chan, 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);
@@ -1432,6 +1475,14 @@ ble_l2cap_sig_coc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
         goto done;
     }
 
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn == NULL) {
+        ble_hs_unlock();
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
     /* Fill up remote configuration
      * Note MPS is the L2CAP MTU
      */
@@ -1440,9 +1491,6 @@ ble_l2cap_sig_coc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
     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();
 
@@ -1469,6 +1517,9 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     int rc;
 
     if (!sdu_rx || !cb) {
+        if (sdu_rx != NULL) {
+            os_mbuf_free_chain(sdu_rx);
+        }
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
@@ -1477,8 +1528,8 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     conn = ble_hs_conn_find(conn_handle);
 
     if (!conn) {
-        ble_hs_unlock();
         os_mbuf_free_chain(sdu_rx);
+        ble_hs_unlock();
         return BLE_HS_ENOTCONN;
     }
 
@@ -1491,6 +1542,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
 
     proc = ble_l2cap_sig_proc_alloc();
     if (!proc) {
+        chan->cb = NULL;
         ble_l2cap_chan_free(conn, chan);
         ble_hs_unlock();
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
@@ -1506,6 +1558,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ, proc->id,
                                 sizeof(*req), &txom);
     if (!req) {
+        chan->cb = NULL;
         ble_l2cap_chan_free(conn, chan);
         ble_hs_unlock();
         rc = BLE_HS_ENOMEM;
@@ -1525,6 +1578,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     if (rc != 0) {
         ble_hs_lock();
         conn = ble_hs_conn_find(conn_handle);
+        chan->cb = NULL;
         ble_l2cap_chan_free(conn, chan);
         ble_hs_unlock();
     }
@@ -1549,8 +1603,23 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     int rc;
     int i;
     int j;
+    bool sdu_rx_freed = false;
+
+    if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) {
+        if (sdu_rx != NULL) {
+            for (i = 0; i < num; i++) {
+                os_mbuf_free_chain(sdu_rx[i]);
+            }
+        }
+        return BLE_HS_EINVAL;
+    }
 
     if (!sdu_rx || !cb) {
+        if (sdu_rx != NULL) {
+            for (i = 0; i < num; i++) {
+                os_mbuf_free_chain(sdu_rx[i]);
+            }
+        }
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
@@ -1559,23 +1628,23 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     conn = ble_hs_conn_find(conn_handle);
 
     if (!conn) {
+        for (i = 0; i < num; i++) {
+            os_mbuf_free_chain(sdu_rx[i]);
+        }
         ble_hs_unlock();
         return BLE_HS_ENOTCONN;
     }
 
     proc = ble_l2cap_sig_proc_alloc();
     if (!proc) {
+        for (i = 0; i < num; i++) {
+            os_mbuf_free_chain(sdu_rx[i]);
+        }
         ble_hs_unlock();
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
         return BLE_HS_ENOMEM;
     }
 
-    if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) {
-        ble_hs_unlock();
-        rc = BLE_HS_EINVAL;
-        goto done;
-    }
-
     proc->op = BLE_L2CAP_SIG_PROC_OP_CONNECT;
     proc->id = ble_l2cap_sig_next_id();
     proc->conn_handle = conn_handle;
@@ -1600,6 +1669,12 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
                 proc->connect.chan[j]->cb = NULL;
                 ble_l2cap_chan_free(conn, proc->connect.chan[j]);
             }
+            /* sdu_rx[i] was already freed internally by ble_l2cap_coc_chan_alloc;
+             * only free the remaining unprocessed buffers sdu_rx[i+1..num-1]. */
+            for (j = i + 1; j < num; j++) {
+                os_mbuf_free_chain(sdu_rx[j]);
+            }
+            sdu_rx_freed = true;
             ble_hs_unlock();
             rc = BLE_HS_ENOMEM;
             goto done;
@@ -1620,8 +1695,23 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
     ble_hs_unlock();
 
     rc = ble_l2cap_sig_tx(proc->conn_handle, txom);
+    if (rc != 0) {
+        ble_hs_lock();
+        conn = ble_hs_conn_find(conn_handle);
+        for (i = 0; i < proc->connect.chan_cnt; i++) {
+            proc->connect.chan[i]->cb = NULL;
+            ble_l2cap_chan_free(conn, proc->connect.chan[i]);
+            proc->connect.chan[i] = NULL;
+        }
+        ble_hs_unlock();
+    }
 
 done:
+    if (rc != 0 && proc != NULL && proc->connect.chan_cnt == 0 && !sdu_rx_freed) {
+        for (i = 0; i < num; i++) {
+            os_mbuf_free_chain(sdu_rx[i]);
+        }
+    }
     ble_l2cap_sig_process_status(proc, rc);
 
     return rc;
@@ -1664,7 +1754,7 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
             proc->reconfig.cids[i] = chans[i]->scid;
         } else {
             ble_hs_unlock();
-            rc = BLE_HS_ENOMEM;
+            rc = BLE_HS_ENOENT;
             goto done;
         }
     }
@@ -1751,7 +1841,8 @@ ble_l2cap_sig_disc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
     if (!chan) {
         os_mbuf_free_chain(txom);
         ble_hs_unlock();
-        ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, hdr->identifier, req->dcid, req->scid);
+        ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, hdr->identifier,
+                                            le16toh(req->scid), le16toh(req->dcid));
         return 0;
     }
     if (le16toh(req->scid) != chan->dcid) {
@@ -1850,7 +1941,8 @@ ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
 
 done:
     ble_l2cap_sig_proc_free(proc);
-    return rc;
+    /* Silently ignore errors as this is a response signal */
+    return 0;
 }
 
 int
@@ -1907,7 +1999,7 @@ ble_l2cap_sig_le_credits_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
 
     rc = ble_hs_mbuf_pullup_base(om, sizeof(*req));
     if (rc != 0) {
-        return 0;
+        return rc;
     }
 
     if (OS_MBUF_PKTLEN(*om) != sizeof(*req)) {
@@ -2090,6 +2182,7 @@ ble_l2cap_sig_extract_expired(struct ble_l2cap_sig_proc_list *dst_list)
             if (time_diff < next_exp_in) {
                 next_exp_in = time_diff;
             }
+            prev = proc;
         }
 
         proc = next;
@@ -2186,6 +2279,11 @@ ble_l2cap_sig_timer(void)
             case BLE_L2CAP_SIG_PROC_OP_DISCONNECT:
                 ble_l2cap_sig_coc_disconnect_cb(proc, BLE_HS_ETIMEOUT);
             break;
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+            case BLE_L2CAP_SIG_PROC_OP_RECONFIG:
+                ble_l2cap_sig_coc_reconfig_cb(proc, BLE_HS_ETIMEOUT);
+            break;
+#endif
 #endif
         }
 

nimble/host/src/ble_l2cap_sig_cmd.c

@@ -71,7 +71,9 @@ ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason,
     }
 
     cmd->reason = htole16(reason);
-    memcpy(cmd->data, data, data_len);
+    if (data_len > 0) {
+        memcpy(cmd->data, data, data_len);
+    }
 
     STATS_INC(ble_l2cap_stats, sig_tx);
     return ble_l2cap_sig_tx(conn_handle, txom);
@@ -85,8 +87,8 @@ ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id,
         uint16_t local_cid;
         uint16_t remote_cid;
     } data = {
-        .local_cid = dst_cid,
-        .remote_cid = src_cid,
+        .local_cid = htole16(dst_cid),
+        .remote_cid = htole16(src_cid),
     };
 
     return ble_l2cap_sig_reject_tx(conn_handle, id,
@@ -105,14 +107,13 @@ ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len,
         return NULL;
     }
 
-    if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) {
+    hdr = os_mbuf_extend(*txom, sizeof(*hdr) + len);
+    if (hdr == NULL) {
         os_mbuf_free_chain(*txom);
         *txom = NULL;
         return NULL;
     }
 
-    hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data;
-
     hdr->op = opcode;
     hdr->identifier = id;
     hdr->length = htole16(len);

nimble/host/src/ble_sm.c

@@ -267,6 +267,11 @@ 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)
 {
+    if (next_ltk == NULL) {
+        ble_sm_dbg_next_ltk_set = 0;
+        return;
+    }
+
     memcpy(ble_sm_dbg_next_ltk, next_ltk,
            sizeof ble_sm_dbg_next_ltk);
     ble_sm_dbg_next_ltk_set = 1;
@@ -994,7 +999,9 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle,
     struct ble_gap_repeat_pairing rp;
     struct ble_store_value_sec bond;
     int rc;
+    int delete_retries;
 
+    delete_retries = 0;
     do {
 #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS)
         int defer_repeat_chk;
@@ -1020,6 +1027,7 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle,
         case 0:
             break;
         case BLE_HS_ENOENT:
+        case BLE_HS_ENOTSUP:
             return 0;
         default:
             return rc;
@@ -1056,6 +1064,19 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle,
         rp.new_bonding = !!(proc_flags & BLE_SM_PROC_F_BONDING);
 
         rc = ble_gap_repeat_pairing_event(&rp);
+        if (rc == BLE_GAP_REPEAT_PAIRING_RETRY) {
+            delete_retries++;
+            if (delete_retries >= 2) {
+                rc = ble_sm_read_bond(conn_handle, &bond);
+                if (rc == BLE_HS_ENOENT || rc == BLE_HS_ENOTSUP) {
+                    return 0;
+                }
+                if (rc != 0) {
+                    return rc;
+                }
+                rc = BLE_GAP_REPEAT_PAIRING_IGNORE;
+            }
+        }
     } while (rc == BLE_GAP_REPEAT_PAIRING_RETRY);
 
     BLE_HS_LOG(DEBUG, "silently ignoring pair request from bonded peer");
@@ -1192,12 +1213,14 @@ 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_req;
     struct ble_sm_pair_cmd *pair_rsp;
 
+    pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1];
     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;
+    *out_init_key_dist = pair_req->init_key_dist & pair_rsp->init_key_dist;
+    *out_resp_key_dist = pair_req->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) {
@@ -1315,6 +1338,10 @@ ble_sm_enc_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
     BLE_HS_DBG_ASSERT(cmd != NULL);
 
     res->app_status = ble_sm_start_encrypt_tx(cmd);
+    if (res->app_status != 0) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->enc_cb = 1;
+    }
 }
 
 static void
@@ -1449,7 +1476,8 @@ ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type,
     if (rc != 0) {
         return rc;
     }
-    if (value_sec->ediv != ediv || value_sec->rand_num != rand) {
+    if (!value_sec->ltk_present ||
+        value_sec->ediv != ediv || value_sec->rand_num != rand) {
         return BLE_HS_ENOENT;
     }
     return rc;
@@ -1484,7 +1512,7 @@ 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;
+    struct ble_hci_le_lt_key_req_neg_reply_rp rsp;
     int rc;
 
     cmd.conn_handle = htole16(conn_handle);
@@ -1514,6 +1542,7 @@ ble_sm_ltk_start_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
         proc->state = BLE_SM_PROC_STATE_ENC_START;
     } else {
         res->enc_cb = 1;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
     }
 }
 
@@ -1590,6 +1619,8 @@ ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev)
         proc = ble_sm_proc_alloc();
         if (proc == NULL) {
             res.app_status = BLE_HS_ENOMEM;
+            ble_sm_ltk_req_neg_reply_tx(conn_handle);
+            restore = 0;
         } else {
             proc->conn_handle = conn_handle;
             proc->state = BLE_SM_PROC_STATE_LTK_RESTORE;
@@ -1618,9 +1649,19 @@ ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev)
     }
 
     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);
+        conn = ble_hs_conn_find(conn_handle);
+        if (conn == NULL) {
+            /* Connection gone; inform controller so it does not wait for an
+             * LTK reply until supervision timeout fires. */
+            ble_sm_ltk_req_neg_reply_tx(conn_handle);
+            /* proc was inserted into the list; let ble_sm_process_result
+             * remove and free it via non-zero app_status. */
+            res.app_status = BLE_HS_ENOTCONN;
+            res.execute = 0;
+        } else {
+            ble_hs_conn_addrs(conn, &addrs);
+            memcpy(peer_id_addr, addrs.peer_id_addr.val, 6);
+        }
     }
 
     ble_hs_unlock();
@@ -1798,11 +1839,12 @@ ble_sm_confirm_rx(uint16_t conn_handle, struct os_mbuf **om,
 static uint8_t
 ble_sm_state_after_pair(struct ble_sm_proc *proc)
 {
+#if MYNEWT_VAL(BLE_SM_SC)
     if (proc->flags & BLE_SM_PROC_F_SC) {
         return BLE_SM_PROC_STATE_PUBLIC_KEY;
-    } else {
-        return BLE_SM_PROC_STATE_CONFIRM;
     }
+#endif
+    return BLE_SM_PROC_STATE_CONFIRM;
 }
 
 static void
@@ -1979,7 +2021,9 @@ ble_sm_verify_auth_requirements(uint8_t cmd)
      * on our side. If not, we can fail right away.
      */
     if (!(cmd & BLE_SM_PAIR_AUTHREQ_SC)) {
-        if (MYNEWT_VAL(BLE_SM_LEGACY) == 0) {
+        if (MYNEWT_VAL(BLE_SM_LEGACY) == 0 ||
+            ble_hs_cfg.sm_sc_only ||
+            ble_hs_cfg.sm_sec_lvl == 4) {
             return false;
         }
     }
@@ -2058,7 +2102,10 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
     ble_hs_lock();
 
     proc = ble_sm_proc_alloc();
-    if (proc != NULL) {
+    if (proc == NULL) {
+        res->app_status = BLE_HS_ENOMEM;
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    } else {
         proc->conn_handle = conn_handle;
         proc->state = BLE_SM_PROC_STATE_PAIR;
         ble_sm_insert(proc);
@@ -2076,9 +2123,11 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
         } 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);
+            res->enc_cb = 1;
         } 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);
+            res->enc_cb = 1;
         } else if (ble_hs_cfg.sm_sc_only  && !(req->authreq & BLE_SM_PAIR_AUTHREQ_SC)) {
             /* Fail if Secure Connections Only mode is on and SC is not supported by peer
              */
@@ -2094,6 +2143,7 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
         } 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);
+            res->enc_cb = 1;
         } else {
             /* The request looks good.  Precalculate our pairing response and
              * determine some properties of the imminent link.  We need this
@@ -2158,18 +2208,22 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om,
         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);
+            res->enc_cb = 1;
         } 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);
+            res->enc_cb = 1;
         } else if (ble_hs_cfg.sm_sc_only && (rsp->max_enc_key_size != BLE_SM_PAIR_KEY_SZ_MAX)) {
             /* Fail if Secure Connections Only mode is on and remote does not meet
             * key size requirements - MITM was checked in last step
             */
             res->sm_err = BLE_SM_ERR_ENC_KEY_SZ;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ);
+            res->enc_cb = 1;
         } else if (!ble_sm_verify_auth_requirements(rsp->authreq)) {
             res->sm_err = BLE_SM_ERR_AUTHREQ;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ);
+            res->enc_cb = 1;
         } else {
             ble_sm_pair_cfg(proc);
 
@@ -2259,8 +2313,8 @@ ble_sm_sec_req_rx(uint16_t conn_handle, struct os_mbuf **om,
 
     conn = ble_hs_conn_find_assert(conn_handle);
 
-    /* Check if pairing procedure is already in progress */
-    if (ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_PAIR, 1, NULL)) {
+    /* Check if any SM procedure is already in progress */
+    if (ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL)) {
         ble_hs_unlock();
         res->app_status = 0;
         return;
@@ -2745,6 +2799,13 @@ ble_sm_id_addr_info_rx(uint16_t conn_handle, struct os_mbuf **om,
 
     cmd = (struct ble_sm_id_addr_info *)(*om)->om_data;
 
+    if (cmd->addr_type != BLE_ADDR_PUBLIC && cmd->addr_type != BLE_ADDR_RANDOM) {
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
+        res->sm_err = BLE_SM_ERR_INVAL;
+        res->enc_cb = 1;
+        return;
+    }
+
     ble_hs_lock();
 
     proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL);
@@ -2830,6 +2891,8 @@ ble_sm_fail_rx(uint16_t conn_handle, struct os_mbuf **om,
 
         res->app_status = BLE_HS_SM_PEER_ERR(cmd->reason);
         res->sm_err =  cmd->reason;
+    } else {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
     }
 }
 
@@ -2876,7 +2939,7 @@ ble_sm_incr_our_sign_counter(uint16_t conn_handle)
     }
 
     rc = ble_store_delete_our_sec(&key_sec);
-    if (rc != 0) {
+    if (rc != 0 && rc != BLE_HS_ENOENT) {
         return rc;
     }
 
@@ -2926,11 +2989,6 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle)
         return BLE_HS_ENOMEM;
     }
 
-    rc = ble_store_delete_peer_sec(&key_sec);
-    if (rc != 0) {
-        return rc;
-    }
-
 #if MYNEWT_VAL(BLE_HS_PVCY)
 #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS)
     /*
@@ -3101,6 +3159,14 @@ ble_sm_enc_initiate(uint16_t conn_handle, uint8_t key_size,
     struct ble_sm_proc *proc;
     struct hci_start_encrypt cmd;
 
+    if (key_size < BLE_SM_PAIR_KEY_SZ_MIN || key_size > BLE_SM_PAIR_KEY_SZ_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (ltk == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
     memset(&res, 0, sizeof res);
 
     /* Make sure a procedure isn't already in progress for this connection. */
@@ -3328,6 +3394,10 @@ ble_sm_connection_broken(uint16_t conn_handle)
 static int
 ble_sm_state_dispatch_init(void)
 {
+    if (ble_sm_state_dispatch != NULL) {
+        return 0;
+    }
+
     ble_sm_state_dispatch = nimble_platform_mem_calloc(1, BLE_SM_PROC_STATE_CNT * sizeof(ble_sm_state_fn *));
 
     if (!ble_sm_state_dispatch) {
@@ -3460,7 +3530,7 @@ ble_sm_rx(struct ble_l2cap_chan *chan, struct os_mbuf **om)
     int rc;
 
     handle = ble_l2cap_get_conn_handle(chan);
-    if (!handle) {
+    if (handle == BLE_HS_CONN_HANDLE_NONE) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOTCONN);
         return BLE_HS_ENOTCONN;
     }

nimble/host/src/ble_sm_alg.c

@@ -93,6 +93,17 @@ static const char * const TAG = "ble_sm_alg";
 #endif
 #endif
 
+static void
+ble_sm_alg_clear_buf(void *buf, size_t len)
+{
+    volatile uint8_t *vp = buf;
+    size_t i;
+
+    for (i = 0; i < len; i++) {
+        vp[i] = 0;
+    }
+}
+
 static void
 ble_sm_alg_xor_128(const uint8_t *p, const uint8_t *q, uint8_t *r)
 {
@@ -123,7 +134,9 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
     status = psa_import_key(&key_attributes, tmp, 16, &key_id);
     if (status != PSA_SUCCESS) {
         ESP_LOGE(TAG, "Failed to import AES key: %d", status);
+        psa_reset_key_attributes(&key_attributes);
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
         return BLE_HS_EUNKNOWN;
     }
     psa_reset_key_attributes(&key_attributes);
@@ -137,6 +150,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
         ESP_LOGE(TAG, "Encryption failed: %d", status);
         psa_destroy_key(key_id);
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
         return BLE_HS_EUNKNOWN;
     }
     psa_destroy_key(key_id);
@@ -147,6 +161,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
     if (mbedtls_aes_setkey_enc(&s, tmp, 128) != 0) {
         mbedtls_aes_free(&s);
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
         return BLE_HS_EUNKNOWN;
     }
 
@@ -155,6 +170,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
     if (mbedtls_aes_crypt_ecb(&s, MBEDTLS_AES_ENCRYPT, tmp, enc_data) != 0) {
         mbedtls_aes_free(&s);
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
         return BLE_HS_EUNKNOWN;
     }
 
@@ -165,6 +181,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
 
     if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
         return BLE_HS_EUNKNOWN;
     }
 
@@ -172,12 +189,14 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
 
     if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
         return BLE_HS_EUNKNOWN;
     }
 #endif
 
     swap_in_place(enc_data, 16);
 
+        ble_sm_alg_clear_buf(tmp, sizeof(tmp));
     return 0;
 }
 
@@ -395,20 +414,32 @@ ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len,
     struct tc_cmac_struct state;
 
     if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) {
+        memset(&sched, 0, sizeof(sched));
+        memset(&state, 0, sizeof(state));
+        __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory");
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
         return BLE_HS_EUNKNOWN;
     }
 
     if (tc_cmac_update(&state, in, len) == TC_CRYPTO_FAIL) {
+        memset(&sched, 0, sizeof(sched));
+        memset(&state, 0, sizeof(state));
+        __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory");
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
         return BLE_HS_EUNKNOWN;
     }
 
     if (tc_cmac_final(out, &state) == TC_CRYPTO_FAIL) {
+        memset(&sched, 0, sizeof(sched));
+        memset(&state, 0, sizeof(state));
+        __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory");
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
         return BLE_HS_EUNKNOWN;
     }
 
+    memset(&sched, 0, sizeof(sched));
+    memset(&state, 0, sizeof(state));
+    __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory");
     return 0;
 }
 #endif
@@ -455,6 +486,9 @@ ble_sm_alg_f4(const uint8_t *u, const uint8_t *v, const uint8_t *x,
     swap_buf(xs, x, 16);
 
     rc = ble_sm_alg_aes_cmac(xs, m, sizeof(m), out_enc_data);
+    memset(xs, 0, sizeof(xs));
+    /* Prevent compiler from optimizing away the sensitive-data clear */
+    __asm__ volatile("" : : "r"(xs) : "memory");
     if (rc != 0) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
         return BLE_HS_EUNKNOWN;
@@ -586,6 +620,8 @@ ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
 
     /* Zero sensitive key material from stack */
     memset(ws, 0, sizeof(ws));
+    /* Use a memory barrier to prevent compiler from optimizing out the memset */
+    __asm__ volatile("" : : "r"(ws) : "memory");
 
     if (rc != 0) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
@@ -621,6 +657,9 @@ ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x,
     /* reuse xs (key) as buffer for result */
     rc = ble_sm_alg_aes_cmac(xs, m, sizeof(m), xs);
     if (rc != 0) {
+        memset(xs, 0, sizeof(xs));
+        /* Prevent compiler from optimizing away the sensitive-data clear */
+        __asm__ volatile("" : : "r"(xs) : "memory");
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN);
         return BLE_HS_EUNKNOWN;
     }
@@ -628,6 +667,9 @@ ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x,
     ble_sm_alg_log_buf("res", xs, 16);
 
     *passkey = get_be32(xs + 12) % 1000000;
+    memset(xs, 0, sizeof(xs));
+    /* Prevent compiler from optimizing away the sensitive-data clear */
+    __asm__ volatile("" : : "r"(xs) : "memory");
     BLE_HS_LOG(DEBUG, "    passkey=%u\n", *passkey);
 
     return 0;
@@ -751,6 +793,9 @@ exit:
     if (rc != 0) {
 #if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
         if (keypair_ptr) {
+#ifndef CONFIG_MBEDTLS_VER_4_X_SUPPORT
+            mbedtls_ecp_keypair_free(keypair_ptr);
+#endif
             nimble_platform_mem_free(keypair_ptr);
             keypair_ptr = NULL;
         }
@@ -910,7 +955,16 @@ exit:
 void mbedtls_free_keypair(void)
 {
 #ifndef CONFIG_MBEDTLS_VER_4_X_SUPPORT
+#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (keypair_ptr == NULL) {
+        return;
+    }
+    mbedtls_ecp_keypair_free(keypair_ptr);
+    nimble_platform_mem_free(keypair_ptr);
+    keypair_ptr = NULL;
+#else
     mbedtls_ecp_keypair_free(&keypair);
+#endif
 #endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT
 }
 #endif

nimble/host/src/ble_sm_lgcy.c

@@ -193,6 +193,7 @@ ble_sm_gen_stk(struct ble_sm_proc *proc)
 
     /* Zero sensitive key material from stack */
     memset(key, 0, sizeof(key));
+    __asm__ volatile("" : : "r"(key) : "memory");
 
     return 0;
 }

nimble/host/src/ble_sm_sc.c

@@ -29,6 +29,10 @@
 #if NIMBLE_BLE_CONNECT
 #if MYNEWT_VAL(BLE_SM_SC)
 
+#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS)
+void mbedtls_free_keypair(void);
+#endif
+
 #define BLE_SM_SC_PASSKEY_BYTES     4
 #define BLE_SM_SC_PASSKEY_BITS      20
 
@@ -158,20 +162,16 @@ ble_sm_sc_io_action(struct ble_sm_proc *proc, uint8_t *action)
     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;
 #if MYNEWT_VAL(STATIC_PASSKEY)
-    /* Check if static passkey is enabled - if so, use static passkey action */
-    if (ble_hs_cfg.sm_static_passkey)
-    {
+    } else if (ble_hs_cfg.sm_static_passkey) {
         *action = BLE_SM_IOACT_STATIC;
         proc->pair_alg = BLE_SM_PAIR_ALG_PASSKEY;
         proc->flags |= BLE_SM_PROC_F_AUTHENTICATED;
         return 0;
-    }
 #endif
-
-    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)) {
 
@@ -780,7 +780,7 @@ 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_addrs addrs = {0};
     struct ble_hs_conn *conn;
 
     BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
@@ -1009,25 +1009,28 @@ ble_sm_sc_oob_data_check(struct ble_sm_proc *proc,
 int
 ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data)
 {
+    uint8_t pub_key[64];
     int rc;
 
+    ble_hs_lock();
+
     rc = ble_sm_sc_ensure_keys_generated();
-    if (rc) {
+    if (rc == 0) {
+        memcpy(pub_key, ble_sm_sc_pub_key, sizeof(pub_key));
+    }
+
+    ble_hs_unlock();
+
+    if (rc != 0) {
         return rc;
     }
 
     rc = ble_hs_hci_util_rand(oob_data->r, 16);
-    if (rc) {
+    if (rc != 0) {
         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;
+    return ble_sm_alg_f4(pub_key, pub_key, oob_data->r, 0, oob_data->c);
 }
 
 void
@@ -1042,9 +1045,13 @@ void
 ble_sm_sc_deinit(void)
 {
     if (ble_sm_sc_ctx) {
+#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS)
+        mbedtls_free_keypair();
+#endif
         nimble_platform_mem_free(ble_sm_sc_ctx);
         ble_sm_sc_ctx = NULL;
     }
+    ble_sm_sc_keys_generated = 0;
 }
 #endif
 

nimble/host/src/ble_store.c

@@ -27,7 +27,7 @@ int
 ble_store_read(int obj_type, const union ble_store_key *key,
                union ble_store_value *val)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     int rc;
 
@@ -50,7 +50,7 @@ ble_store_read(int obj_type, const union ble_store_key *key,
 int
 ble_store_write(int obj_type, const union ble_store_value *val)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
     int rc;
 
     while (1) {
@@ -91,7 +91,7 @@ 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)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     int rc;
 
@@ -111,7 +111,7 @@ ble_store_delete(int obj_type, const union ble_store_key *key)
 #endif
 }
 
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 static int
 ble_store_status(struct ble_store_status_event *event)
 {
@@ -132,9 +132,9 @@ ble_store_status(struct ble_store_status_event *event)
 int
 ble_store_overflow_event(int obj_type, const union ble_store_value *value)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
-    struct ble_store_status_event event;
+    struct ble_store_status_event event = {0};
 
     event.event_code = BLE_STORE_EVENT_OVERFLOW;
     event.overflow.obj_type = obj_type;
@@ -149,9 +149,9 @@ 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)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
-    struct ble_store_status_event event;
+    struct ble_store_status_event event = {0};
 
     event.event_code = BLE_STORE_EVENT_FULL;
     event.full.obj_type = obj_type;
@@ -228,11 +228,11 @@ ble_store_delete_our_sec(const struct ble_store_key_sec *key_sec)
 {
 #if NIMBLE_BLE_CONNECT
 
-    union ble_store_key *store_key;
+    union ble_store_key store_key = {0};
     int rc;
 
-    store_key = (void *)key_sec;
-    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, store_key);
+    store_key.sec = *key_sec;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, &store_key);
     return rc;
 #else
     return BLE_HS_ENOTSUP;
@@ -258,16 +258,16 @@ int
 ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec,
                         struct ble_store_value_sec *value_sec)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     union ble_store_value *store_value;
-    union ble_store_key *store_key;
+    const 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_key = (const union ble_store_key *)key_sec;
     store_value = (void *)value_sec;
     rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_SEC, store_key, store_value);
 
@@ -280,7 +280,7 @@ ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec,
 int
 ble_store_write_peer_sec(const struct ble_store_value_sec *value_sec)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     int rc;
 #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY)
@@ -390,7 +390,7 @@ ble_store_read_cccd(const struct ble_store_key_cccd *key,
 int
 ble_store_write_cccd(const struct ble_store_value_cccd *value)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     union ble_store_value *store_value;
     int rc;
@@ -406,7 +406,7 @@ ble_store_write_cccd(const struct ble_store_value_cccd *value)
 int
 ble_store_delete_cccd(const struct ble_store_key_cccd *key)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     union ble_store_key *store_key;
     int rc;
@@ -443,11 +443,11 @@ ble_store_write_csfc(const struct ble_store_value_csfc *value)
 {
 #if NIMBLE_BLE_CONNECT
 
-    union ble_store_value *store_value;
+    union ble_store_value store_value = {0};
     int rc;
 
-    store_value = (void *)value;
-    rc = ble_store_write(BLE_STORE_OBJ_TYPE_CSFC, store_value);
+    store_value.csfc = *value;
+    rc = ble_store_write(BLE_STORE_OBJ_TYPE_CSFC, &store_value);
     return rc;
 #else
     return BLE_HS_ENOTSUP;
@@ -474,7 +474,7 @@ void
 ble_store_key_from_value_cccd(struct ble_store_key_cccd *out_key,
                               const struct ble_store_value_cccd *value)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     out_key->peer_addr = value->peer_addr;
     out_key->chr_val_handle = value->chr_val_handle;
@@ -503,10 +503,10 @@ ble_store_read_ead(const struct ble_store_key_ead *key,
 #if NIMBLE_BLE_CONNECT
 
     union ble_store_value *store_value;
-    union ble_store_key *store_key;
+    const union ble_store_key *store_key;
     int rc;
 
-    store_key = (void *)key;
+    store_key = (const union ble_store_key *)key;
     store_value = (void *)out_value;
     rc = ble_store_read(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, store_key, store_value);
     return rc;
@@ -536,11 +536,11 @@ ble_store_delete_ead(const struct ble_store_key_ead *key)
 {
 #if NIMBLE_BLE_CONNECT
 
-    union ble_store_key *store_key;
+    union ble_store_key store_key = {0};
     int rc;
 
-    store_key = (void *)key;
-    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, store_key);
+    store_key.ead = *key;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, &store_key);
     return rc;
 #else
     return BLE_HS_ENOTSUP;
@@ -563,7 +563,7 @@ int
 ble_store_read_local_irk(const struct ble_store_key_local_irk *key,
                    struct ble_store_value_local_irk *out_value)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     union ble_store_value *store_value;
     union ble_store_key *store_key;
@@ -583,13 +583,13 @@ ble_store_read_local_irk(const struct ble_store_key_local_irk *key,
 int
 ble_store_write_local_irk(const struct ble_store_value_local_irk *value)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
-    union ble_store_value *store_value;
+    union ble_store_value store_value = {0};
     int rc;
 
-    store_value = (void *)value;
-    rc = ble_store_write(BLE_STORE_OBJ_TYPE_LOCAL_IRK, store_value);
+    store_value.local_irk = *value;
+    rc = ble_store_write(BLE_STORE_OBJ_TYPE_LOCAL_IRK, &store_value);
     return rc;
 #else
     return BLE_HS_ENOTSUP;
@@ -602,11 +602,11 @@ ble_store_delete_local_irk(const struct ble_store_key_local_irk *key)
 {
 #if NIMBLE_BLE_CONNECT
 
-    union ble_store_key *store_key;
+    union ble_store_key store_key = {0};
     int rc;
 
-    store_key = (void *)key;
-    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_LOCAL_IRK, store_key);
+    store_key.local_irk = *key;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_LOCAL_IRK, &store_key);
     return rc;
 #else
     return BLE_HS_ENOTSUP;
@@ -664,11 +664,11 @@ ble_store_delete_rpa_rec(const struct ble_store_key_rpa_rec *key)
 {
 #if NIMBLE_BLE_CONNECT
 
-    union ble_store_key *store_key;
+    union ble_store_key store_key = {0};
     int rc;
 
-    store_key = (void *)key;
-    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_PEER_ADDR, store_key);
+    store_key.rpa_rec = *key;
+    rc = ble_store_delete(BLE_STORE_OBJ_TYPE_PEER_ADDR, &store_key);
     return rc;
 #else
     return BLE_HS_ENOTSUP;
@@ -743,7 +743,7 @@ ble_store_iterate(int obj_type,
                   ble_store_iterator_fn *callback,
                   void *cookie)
 {
-#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC)
+#if NIMBLE_BLE_CONNECT
 
     union ble_store_key key;
     union ble_store_value value;

nimble/host/src/ble_store_util.c

@@ -385,11 +385,12 @@ ble_store_util_ead_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers,
        return rc;
    }
 
-   if (set.status != 0 && set.status != BLE_HS_ENOMEM) {
+   *out_num_peers = set.num_peers;
+
+   if (set.status != 0) {
        return set.status;
    }
 
-   *out_num_peers = set.num_peers;
    return 0;
 #else
     return BLE_HS_ENOTSUP;
@@ -400,7 +401,7 @@ ble_store_util_ead_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers,
 int
 ble_store_util_delete_ead_oldest_peer(void)
 {
-#if NIMBLE_BLE_CONNECT
+#if NIMBLE_BLE_CONNECT && (MYNEWT_VAL(BLE_STORE_MAX_EADS) > 0)
 
    ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_EADS)];
    int num_peers;

nimble/host/src/ble_uuid.c

@@ -133,7 +133,13 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2)
 
     switch (uuid1->type) {
     case BLE_UUID_TYPE_16:
-        return (int) BLE_UUID16(uuid1)->value - (int) BLE_UUID16(uuid2)->value;
+        if (BLE_UUID16(uuid1)->value < BLE_UUID16(uuid2)->value) {
+            return -1;
+        }
+        if (BLE_UUID16(uuid1)->value > BLE_UUID16(uuid2)->value) {
+            return 1;
+        }
+        return 0;
     case BLE_UUID_TYPE_32:
         if (BLE_UUID32(uuid1)->value < BLE_UUID32(uuid2)->value) {
             return -1;
@@ -143,7 +149,7 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2)
         }
         return 0;
     case BLE_UUID_TYPE_128:
-        return memcmp(&BLE_UUID128(uuid1)->value, &BLE_UUID128(uuid2)->value, 16);
+        return memcmp(BLE_UUID128(uuid1)->value, BLE_UUID128(uuid2)->value, 16);
     }
 
     BLE_HS_DBG_ASSERT(0);
@@ -154,6 +160,9 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2)
 void
 ble_uuid_copy(ble_uuid_any_t *dst, const ble_uuid_t *src)
 {
+    if (src == NULL || dst == NULL) {
+        return;
+    }
     VERIFY_UUID(src);
 
     switch (src->type) {
@@ -212,16 +221,15 @@ ble_uuid_base_init(void)
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
             return BLE_HS_ENOMEM;
         }
+        ble_uuid_base[0] = 0xfb;
+        ble_uuid_base[1] = 0x34;
+        ble_uuid_base[2] = 0x9b;
+        ble_uuid_base[3] = 0x5f;
+        ble_uuid_base[4] = 0x80;
+        ble_uuid_base[7] = 0x80;
+        ble_uuid_base[9] = 0x10;
     }
 
-    ble_uuid_base[0] = 0xfb;
-    ble_uuid_base[1] = 0x34;
-    ble_uuid_base[2] = 0x9b;
-    ble_uuid_base[3] = 0x5f;
-    ble_uuid_base[4] = 0x80;
-    ble_uuid_base[7] = 0x80;
-    ble_uuid_base[9] = 0x10;
-
     return 0;
 }
 #endif
@@ -249,13 +257,21 @@ ble_uuid_from_str(ble_uuid_any_t *uuid, const char *str)
         return BLE_HS_EINVAL;
     }
 
+    /* For 16-bit (6 chars: 0xXXXX) and 32-bit (10 chars: 0xXXXXXXXX) strings,
+     * require the "0x" prefix */
+    if ((len == 6 || len == 10) &&
+        !(str[0] == '0' && (str[1] == 'x' || str[1] == 'X'))) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
+    }
+
     str_ptr = &str[len - 2];
 
     if (len <= BLE_UUID16_STR_MAX_LEN) {
         uuid->u.type = BLE_UUID_TYPE_16;
     } else if (len <= BLE_UUID32_STR_MAX_LEN) {
         uuid->u.type = BLE_UUID_TYPE_32;
-    } else if (len <= BLE_UUID128_STR_MAX_LEN) {
+    } else if (len == BLE_UUID128_STR_MAX_LEN || len == 32) {
         uuid->u.type = BLE_UUID_TYPE_128;
     } else {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
@@ -422,6 +438,7 @@ int
 ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om)
 {
     int len;
+    int rc;
     void *buf;
 
     VERIFY_UUID(uuid);
@@ -439,7 +456,11 @@ ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om)
         return BLE_HS_ENOMEM;
     }
 
-    ble_uuid_flat(uuid, buf);
+    rc = ble_uuid_flat(uuid, buf);
+    if (rc != 0) {
+        os_mbuf_adj(om, -len);
+        return rc;
+    }
 
     return 0;
 }

nimble/host/store/config/src/ble_store_config.c

@@ -18,6 +18,7 @@
  */
 
 #include <inttypes.h>
+#include <stdbool.h>
 #include <string.h>
 
 #include "sysinit/sysinit.h"
@@ -115,7 +116,7 @@ int ble_restore_our_sec_nvs(void)
 
         err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, &key);
 
-        if (err != ESP_OK) {
+        if (err != 0) {
             BLE_HS_LOG(DEBUG, "Error deleting from nvs");
             return err;
         }
@@ -128,7 +129,7 @@ int ble_restore_our_sec_nvs(void)
 
         err = ble_store_config_write(BLE_STORE_OBJ_TYPE_OUR_SEC, &val);
 
-        if (err != ESP_OK) {
+        if (err != 0) {
             BLE_HS_LOG(DEBUG, "Error writing to nvs");
             return err;
         }
@@ -164,7 +165,7 @@ int ble_restore_peer_sec_nvs(void)
 
         err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_PEER_SEC, &key);
 
-        if (err != ESP_OK) {
+        if (err != 0) {
             BLE_HS_LOG(DEBUG, "Error deleting from nvs %d ",err);
             return err;
         }
@@ -176,7 +177,7 @@ int ble_restore_peer_sec_nvs(void)
 
         err = ble_store_config_write(BLE_STORE_OBJ_TYPE_PEER_SEC, &val);
 
-        if (err != ESP_OK) {
+        if (err != 0) {
             BLE_HS_LOG(DEBUG, "Error writing to nvs %d ",err);
             return err;
         }
@@ -640,6 +641,7 @@ ble_store_config_find_ead(const struct ble_store_key_ead *key)
 static int
 ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead)
 {
+    struct ble_store_value_ead backup;
     int idx;
     int rc;
 
@@ -648,6 +650,8 @@ ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead)
         return BLE_HS_ENOENT;
     }
 
+    backup = ble_store_config_eads[idx];
+
     rc = ble_store_config_delete_obj(ble_store_config_eads,
                                      sizeof *ble_store_config_eads,
                                      idx,
@@ -658,6 +662,12 @@ ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead)
 
     rc = ble_store_config_persist_eads();
     if (rc != 0) {
+        /* Restore the deleted entry to keep RAM consistent with storage */
+        memmove(&ble_store_config_eads[idx + 1],
+                &ble_store_config_eads[idx],
+                (ble_store_config_num_eads - idx) * sizeof(*ble_store_config_eads));
+        ble_store_config_eads[idx] = backup;
+        ble_store_config_num_eads++;
         return rc;
     }
 
@@ -685,11 +695,13 @@ ble_store_config_write_ead(const struct ble_store_value_ead *value_ead)
     struct ble_store_key_ead key_ead;
     int idx;
     int rc;
+    bool is_new;
 
     ble_store_key_from_value_ead(&key_ead, value_ead);
     idx = ble_store_config_find_ead(&key_ead);
+    is_new = (idx == -1);
 
-    if (idx == -1) {
+    if (is_new) {
         if (ble_store_config_num_eads >= MYNEWT_VAL(BLE_STORE_MAX_EADS)) {
             BLE_HS_LOG(DEBUG, "error persisting ead; too many entries (%d)\n",
                        ble_store_config_num_eads);
@@ -705,6 +717,9 @@ ble_store_config_write_ead(const struct ble_store_value_ead *value_ead)
 
     rc = ble_store_config_persist_eads();
     if (rc != 0) {
+        if (is_new) {
+            ble_store_config_num_eads--;
+        }
         return rc;
     }
 
@@ -852,9 +867,11 @@ ble_store_config_find_rpa_rec(const struct ble_store_key_rpa_rec *key)
     for(i = 0; i < ble_store_config_num_rpa_recs; i++){
         rpa_rec = ble_store_config_rpa_recs + i;
 
-        if (ble_addr_cmp(&rpa_rec->peer_rpa_addr, &key->peer_rpa_addr) &&
-            ble_addr_cmp(&rpa_rec->peer_addr, &key->peer_rpa_addr)) {
-            continue;
+        if (ble_addr_cmp(&key->peer_rpa_addr, BLE_ADDR_ANY)) {
+            if (ble_addr_cmp(&rpa_rec->peer_rpa_addr, &key->peer_rpa_addr) &&
+                ble_addr_cmp(&rpa_rec->peer_addr, &key->peer_rpa_addr)) {
+                continue;
+            }
         }
         if (key->idx > skipped) {
             skipped++;
@@ -961,9 +978,14 @@ ble_store_config_find_csfc(const struct ble_store_key_csfc *key,
 
     /* If peer_addr is specified, search by peer_addr (common for write/read/delete) */
     if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) {
+        int skipped = 0;
         for (i = 0; i < num_value_csfc; i++) {
             cur = &value_csfc[i];
             if (!ble_addr_cmp(&cur->peer_addr, &key->peer_addr)) {
+                if (skipped < key->idx) {
+                    skipped++;
+                    continue;
+                }
                 return i;
             }
         }
@@ -1258,5 +1280,8 @@ ble_store_config_deinit(void)
         nimble_platform_mem_free(ble_store_config_vars);
         ble_store_config_vars = NULL;
     }
+    ble_hs_cfg.store_read_cb = NULL;
+    ble_hs_cfg.store_write_cb = NULL;
+    ble_hs_cfg.store_delete_cb = NULL;
 }
 #endif

nimble/host/store/config/src/ble_store_config_conf.c

@@ -88,7 +88,7 @@ ble_store_config_serialize_arr(const void *arr, int obj_sz, int num_objs,
     int arr_size;
 
     arr_size = obj_sz * num_objs;
-    assert(arr_size <= buf_sz);
+    assert(BASE64_ENCODE_SIZE(arr_size) <= buf_sz);
 
     base64_encode(arr, arr_size, out_buf, 1);
 }

nimble/host/store/config/src/ble_store_nvs.c

@@ -159,7 +159,10 @@ get_nvs_peer_record(char *key_string, struct ble_hs_dev_records *p_dev_rec)
     size_t required_size = 0;
     nvs_handle_t nimble_handle;
 
-    err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READWRITE, &nimble_handle);
+    err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READONLY, &nimble_handle);
+    if (err == ESP_ERR_NVS_NOT_FOUND) {
+        return ESP_ERR_NVS_NOT_FOUND;
+    }
     if (err != ESP_OK) {
         ESP_LOGE(TAG, "NVS open operation failed");
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL);
@@ -221,7 +224,10 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val)
     size_t expected_size;
     nvs_handle_t nimble_handle;
 
-    err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READWRITE, &nimble_handle);
+    err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READONLY, &nimble_handle);
+    if (err == ESP_ERR_NVS_NOT_FOUND) {
+        return ESP_ERR_NVS_NOT_FOUND;
+    }
     if (err != ESP_OK) {
         ESP_LOGE(TAG, "NVS open operation failed");
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL);
@@ -450,6 +456,10 @@ ble_nvs_write_key_value(char *key, const void *value, size_t required_size)
     return 0;
 error:
     nvs_close(nimble_handle);
+    if (err == ESP_ERR_NVS_NOT_ENOUGH_SPACE || err == ESP_ERR_NVS_NO_FREE_PAGES) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_CAP);
+        return BLE_HS_ESTORE_CAP;
+    }
     BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL);
     return BLE_HS_ESTORE_FAIL;
 }
@@ -1063,17 +1073,22 @@ int ble_store_persist_peer_records(void)
         peer_rec = peer_dev_rec[ble_store_num_peer_dev_rec - 1];
         return ble_store_nvs_peer_records(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, &peer_rec);
     } else if (nvs_count > ble_store_num_peer_dev_rec) {
-        /* NVS db count more than RAM count, delete operation */
-        nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0,
-                                       peer_dev_rec,
-                                       ble_store_num_peer_dev_rec);
-        if (nvs_idx == -1) {
-            ESP_LOGE(TAG, "NVS delete operation failed for peer records");
-            BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL);
-            return BLE_HS_ESTORE_FAIL;
+        /* NVS db count more than RAM count, delete all non-matching records */
+        while (nvs_count > ble_store_num_peer_dev_rec) {
+            nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0,
+                                           peer_dev_rec,
+                                           ble_store_num_peer_dev_rec);
+            if (nvs_idx == -1) {
+                ESP_LOGE(TAG, "NVS delete operation failed for peer records");
+                BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL);
+                return BLE_HS_ESTORE_FAIL;
+            }
+            ESP_LOGD(TAG, "Deleting peer record, nvs idx = %d", nvs_idx);
+            if (ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, nvs_idx) != 0) {
+                return BLE_HS_ESTORE_FAIL;
+            }
+            nvs_count--;
         }
-        ESP_LOGD(TAG, "Deleting peer record, nvs idx = %d", nvs_idx);
-        return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, nvs_idx);
     }
     return 0;
 }

nimble/host/store/ram/src/ble_store_ram.c

@@ -80,7 +80,7 @@ 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=",
+        BLE_HS_LOG(DEBUG, "ediv=%u rand=%" PRIu64 " authenticated=%d ltk=",
                        sec->ediv, sec->rand_num, sec->authenticated);
         ble_hs_log_flat_buf(sec->ltk, 16);
         BLE_HS_LOG(DEBUG, " ");
@@ -124,12 +124,18 @@ ble_store_ram_find_sec(const struct ble_store_key_sec *key_sec,
         if (key_sec->idx < num_value_secs) {
             return key_sec->idx;
         }
-    } else if (key_sec->idx == 0) {
+    } else {
+        int skipped;
+
+        skipped = 0;
         for (i = 0; i < num_value_secs; i++) {
             cur = &value_secs[i];
 
             if (!ble_addr_cmp(&cur->peer_addr, &key_sec->peer_addr)) {
-                return i;
+                if (skipped == key_sec->idx) {
+                    return i;
+                }
+                skipped++;
             }
         }
     }
@@ -166,6 +172,10 @@ ble_store_ram_write_our_sec(const struct ble_store_value_sec *value_sec)
     struct ble_store_key_sec key_sec;
     int idx;
 
+    if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) == 0) {
+        return BLE_HS_EINVAL;
+    }
+
     BLE_HS_LOG(DEBUG, "persisting our sec; ");
     ble_store_ram_print_value_sec(value_sec);
 
@@ -213,6 +223,10 @@ ble_store_ram_delete_obj(void *values, int value_size, int idx,
         memmove(dst, src, (size_t)move_count * value_size);
     }
 
+    /* Zero the now-unused slot at the end to prevent stale key data in RAM */
+    dst = (uint8_t *)values + (*num_values) * value_size;
+    memset(dst, 0, value_size);
+
     return 0;
 }
 #endif
@@ -305,6 +319,10 @@ ble_store_ram_write_peer_sec(const struct ble_store_value_sec *value_sec)
     struct ble_store_key_sec key_sec;
     int idx;
 
+    if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) == 0) {
+        return BLE_HS_EINVAL;
+    }
+
     BLE_HS_LOG(DEBUG, "persisting peer sec; ");
     ble_store_ram_print_value_sec(value_sec);
 
@@ -529,7 +547,7 @@ static int
 ble_store_ram_write_csfc(const struct ble_store_value_csfc *value_csfc)
 {
 #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS)
-    struct ble_store_key_csfc key_csfc;
+    struct ble_store_key_csfc key_csfc = {0};
     int idx;
 
     ble_store_key_from_value_csfc(&key_csfc, value_csfc);
@@ -624,7 +642,7 @@ ble_store_ram_read_ead(const struct ble_store_key_ead *key_ead,
 static int
 ble_store_ram_write_ead(const struct ble_store_value_ead *value_ead)
 {
-    struct ble_store_key_ead key_ead;
+    struct ble_store_key_ead key_ead = {0};
     int idx;
 
     ble_store_key_from_value_ead(&key_ead, value_ead);

nimble/host/test/src/ble_hs_hci_test.c

@@ -105,7 +105,9 @@ TEST_CASE_SELF(ble_hs_hci_acl_one_conn)
     ble_hs_test_util_init();
 
     /* The controller has room for five 20-byte payloads. */
+    ble_hs_lock();
     rc = ble_hs_hci_set_buf_sz(20, 5);
+    ble_hs_unlock();
     TEST_ASSERT_FATAL(rc == 0);
     TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5);
 
@@ -190,7 +192,9 @@ TEST_CASE_SELF(ble_hs_hci_acl_two_conn)
     ble_hs_test_util_init();
 
     /* The controller has room for five 20-byte payloads*/
+    ble_hs_lock();
     rc = ble_hs_hci_set_buf_sz(20, 5);
+    ble_hs_unlock();
     TEST_ASSERT_FATAL(rc == 0);
     TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5);
 

nimble/host/test/src/ble_hs_pvcy_test.c

@@ -236,7 +236,7 @@ ble_hs_pvcy_test_util_add_arbitrary_irk(bool scanning, bool connecting)
     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,
+        ble_hs_pvcy_get_default_irk(),
         scanning,
         connecting);
 }
@@ -255,7 +255,7 @@ ble_hs_pvcy_test_util_restore_irk(const struct ble_store_value_sec *value_sec,
 
     ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec->peer_addr,
                                             value_sec->irk,
-                                            ble_hs_pvcy_default_irk,
+                                            ble_hs_pvcy_get_default_irk(),
                                             scanning,
                                             connecting);
 }
@@ -287,7 +287,7 @@ TEST_CASE_SELF(ble_hs_pvcy_test_case_restore_irks)
     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,
+                                            ble_hs_pvcy_get_default_irk(),
                                             false, false);
 
     /* Two persisted IRKs. */
@@ -305,11 +305,11 @@ TEST_CASE_SELF(ble_hs_pvcy_test_case_restore_irks)
     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,
+                                            ble_hs_pvcy_get_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,
+                                            ble_hs_pvcy_get_default_irk(),
                                             false, false);
 
     ble_hs_test_util_assert_mbufs_freed(NULL);

nimble/include/nimble/hci_common.h

@@ -1559,8 +1559,8 @@ struct ble_hci_le_cs_test_cp {
 struct ble_hci_le_add_monitor_adv_list_cp {
     uint8_t addr_type;
     uint8_t address[6];
-    uint8_t rssi_low_threshold;
-    uint8_t rssi_high_threshold;
+    int8_t rssi_low_threshold;
+    int8_t rssi_high_threshold;
     uint8_t timeout;
 } __attribute__((packed));
 

nimble/transport/cdc/src/cdc_hci.c

@@ -306,7 +306,9 @@ cdc_hci_acl_packet_write(struct usb_in_packet *packet, size_t offset)
 
         for (;;) {
             mb = os_mbuf_off(om, (int)new_offset - 1, &mbuf_offset);
-            assert(mb);
+            if (mb == NULL || new_offset < 1) {
+                break;
+            }
             /* mbuf_offset is == om_len when new_offset reached end of mbuf data */
             if (mb->om_len == mbuf_offset) {
                 break;

nimble/transport/src/transport.c

@@ -419,6 +419,14 @@ ble_transport_acl_put(struct os_mempool_ext *mpe, void *data, void *arg)
     struct os_mbuf *om;
     struct os_mbuf_pkthdr *pkthdr;
     bool from_ll;
+    uint16_t omp_flags;
+#endif
+
+#if MYNEWT_VAL(BLE_TRANSPORT_INT_FLOW_CTL)
+    om = data;
+    pkthdr = OS_MBUF_PKTHDR(om);
+    omp_flags = pkthdr->omp_flags;
+    from_ll = (omp_flags & OMP_FLAG_FROM_MASK) == OMP_FLAG_FROM_LL;
 #endif
 
 #if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
@@ -430,10 +438,6 @@ ble_transport_acl_put(struct os_mempool_ext *mpe, void *data, void *arg)
 #endif
 
 #if MYNEWT_VAL(BLE_TRANSPORT_INT_FLOW_CTL)
-    om = data;
-    pkthdr = OS_MBUF_PKTHDR(om);
-
-    from_ll = (pkthdr->omp_flags & OMP_FLAG_FROM_MASK) == OMP_FLAG_FROM_LL;
 
     if (from_ll && !err) {
         hci_ipc_put(HCI_IPC_TYPE_ACL);

porting/nimble/src/hal_timer.c

@@ -190,6 +190,9 @@ nrf_timer_set_ocmp(struct nrf52_hal_timer *bsptimer, uint32_t expiry)
                 /* CC too far ahead. Just make sure we set compare far ahead */
                 rtctimer->CC[NRF_RTC_TIMER_CC_INT] = cntr + (1UL << 23);
             }
+            /* Clear any stale TICK and COMPARE events before enabling interrupt */
+            rtctimer->EVENTS_TICK = 0;
+            rtctimer->EVENTS_COMPARE[NRF_RTC_TIMER_CC_INT] = 0;
             rtctimer->INTENSET = NRF_TIMER_INT_MASK(NRF_RTC_TIMER_CC_INT);
         }
     } else {
@@ -535,6 +538,7 @@ hal_timer_init(int timer_num, void *cfg)
 
     bsptimer->tmr_reg = hwtimer;
     bsptimer->tmr_irq_num = irq_num;
+    TAILQ_INIT(&bsptimer->hal_timer_q);
 
     /* Disable IRQ, set priority and set vector in table */
     NVIC_DisableIRQ(irq_num);
@@ -591,6 +595,7 @@ hal_timer_config(int timer_num, uint32_t freq_hz)
 
         bsptimer->tmr_freq = freq_hz;
         bsptimer->tmr_enabled = 1;
+        bsptimer->tmr_cntr = 0;
 
         OS_ENTER_CRITICAL(sr);
 
@@ -619,6 +624,11 @@ hal_timer_config(int timer_num, uint32_t freq_hz)
 #endif
 
     /* Set timer to desired frequency */
+    if (freq_hz == 0) {
+        rc = EINVAL;
+        goto err;
+    }
+
     div = NRF52_MAX_TIMER_FREQ / freq_hz;
 
     /*
@@ -707,6 +717,12 @@ hal_timer_deinit(int timer_num)
     NRF52_HAL_TIMER_RESOLVE(timer_num, bsptimer);
 
     OS_ENTER_CRITICAL(sr);
+    if (bsptimer->tmr_reg == NULL) {
+        /* Already deinitialized */
+        OS_EXIT_CRITICAL(sr);
+        rc = EINVAL;
+        goto err;
+    }
     if (bsptimer->tmr_rtc) {
         rtctimer = (NRF_RTC_Type *)bsptimer->tmr_reg;
         rtctimer->INTENCLR = NRF_TIMER_INT_MASK(NRF_RTC_TIMER_CC_INT);
@@ -748,6 +764,9 @@ hal_timer_get_resolution(int timer_num)
 
     NRF52_HAL_TIMER_RESOLVE(timer_num, bsptimer);
 
+    if (bsptimer->tmr_freq == 0) {
+        return 0;
+    }
     resolution = 1000000000 / bsptimer->tmr_freq;
     return resolution;
 
@@ -781,11 +800,12 @@ hal_timer_read(int timer_num)
 
     return tcntr;
 
-    /* Assert here since there is no invalid return code */
+    /* Abort on invalid timer_num since there is no valid return code.
+     * rc is set by NRF52_HAL_TIMER_RESOLVE but not used after abort(). */
     err:
-    assert(0);
-    rc = 0;
-    return rc;
+    (void)rc;
+    abort();
+    return 0;
 }
 
 /**
@@ -803,6 +823,10 @@ hal_timer_delay(int timer_num, uint32_t ticks)
 {
     uint32_t until;
 
+    if (ticks == 0) {
+        return 0;
+    }
+
     until = hal_timer_read(timer_num) + ticks;
     while ((int32_t)(hal_timer_read(timer_num) - until) <= 0) {
         /* Loop here till finished */
@@ -847,6 +871,10 @@ hal_timer_start(struct hal_timer *timer, uint32_t ticks)
     uint32_t tick;
     struct nrf52_hal_timer *bsptimer;
 
+    if (timer == NULL) {
+        return EINVAL;
+    }
+
     /* Set the tick value at which the timer should expire */
     bsptimer = (struct nrf52_hal_timer *)timer->bsp_timer;
     if (bsptimer->tmr_rtc) {

porting/nimble/src/modlog.c

@@ -9,9 +9,14 @@ void MODLOG_INFO(int mod, char * msg, ...) {
 
     memset(buffer, 0, 1000);
     va_start(args, msg);
-    len = sprintf(buffer, args);
+    len = vsnprintf(buffer, sizeof(buffer), msg, args);
     va_end(args);
 
+    if (len < 0) {
+        len = 0;
+    } else if (len >= (int)sizeof(buffer)) {
+        len = (int)sizeof(buffer) - 1;
+    }
     ble_log_write_hex(BLE_LOG_SRC_HOST, (uint8_t *)buffer, len);
     ble_log_flush();
 }

porting/nimble/src/nimble_port.c

@@ -94,6 +94,8 @@ static struct ble_hs_stop_listener stop_listener;
 
 #endif
 
+static bool nimble_port_run_active;
+
 extern void os_msys_init(void);
 extern void os_mempool_module_init(void);
 #if MYNEWT_VAL(MP_RUNTIME_ALLOC)
@@ -167,7 +169,17 @@ esp_err_t esp_nimble_init(void)
     /* Initialize the function pointers for OS porting */
     npl_freertos_funcs_init();
 
-    npl_freertos_mempool_init();
+    if (npl_freertos_mempool_init() != 0) {
+        ESP_LOGE(NIMBLE_PORT_LOG_TAG, "npl mempool init failed\n");
+        npl_freertos_funcs_deinit();
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+        if (ble_npl_ctx) {
+            nimble_platform_mem_free(ble_npl_ctx);
+            ble_npl_ctx = NULL;
+        }
+#endif
+        return ESP_FAIL;
+    }
 
 #if CONFIG_BT_CONTROLLER_ENABLED
     if(esp_nimble_hci_init() != ESP_OK) {
@@ -219,6 +231,9 @@ esp_err_t esp_nimble_init(void)
 esp_err_t esp_nimble_deinit(void)
 {
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_npl_ctx == NULL) {
+        return ESP_OK;
+    }
     ble_npl_deiniting = true;
 #endif
 
@@ -248,10 +263,10 @@ esp_err_t esp_nimble_deinit(void)
 
 #endif
 
-    ble_npl_eventq_deinit(&g_eventq_dflt);
-
     ble_hs_deinit();
 
+    ble_npl_eventq_deinit(&g_eventq_dflt);
+
 #if !SOC_ESP_NIMBLE_CONTROLLER || !CONFIG_BT_CONTROLLER_ENABLED
     npl_freertos_funcs_deinit();
 #endif
@@ -392,13 +407,19 @@ nimble_port_stop(void)
     err = ble_hs_stop(&stop_listener, ble_hs_stop_cb,
                      NULL);
     if (err != 0) {
-        ble_npl_sem_deinit(&ble_hs_stop_sem);
-        return err;
+        if (err != BLE_HS_EALREADY) {
+            ble_npl_sem_deinit(&ble_hs_stop_sem);
+            return err;
+        }
+        if (!nimble_port_run_active) {
+            ble_npl_sem_deinit(&ble_hs_stop_sem);
+            return ESP_OK;
+        }
+    } else {
+        /* Wait till the host stop procedure is complete */
+        ble_npl_sem_pend(&ble_hs_stop_sem, BLE_NPL_TIME_FOREVER);
     }
 
-    /* Wait till the host stop procedure is complete */
-    ble_npl_sem_pend(&ble_hs_stop_sem, BLE_NPL_TIME_FOREVER);
-
     ble_npl_event_init(&ble_hs_ev_stop, nimble_port_stop_cb,
                        NULL);
     ble_npl_eventq_put(&g_eventq_dflt, &ble_hs_ev_stop);
@@ -417,14 +438,25 @@ IRAM_ATTR
 void nimble_port_run(void)
 {
     struct ble_npl_event *ev;
+    /* Snapshot stop_ev and stop_sem addresses before entering the loop.
+     * When BLE_STATIC_TO_DYNAMIC is enabled, after ble_npl_sem_release(stop_sem)
+     * at the end of stop-event handling, the caller of nimble_port_stop() can
+     * wake up and free ble_npl_ctx. Any subsequent use of the &ble_hs_ev_stop
+     * or &ble_hs_stop_sem macros (which dereference ble_npl_ctx) would be a
+     * use-after-free. Using local snapshots avoids that.
+     */
+    struct ble_npl_event *stop_ev  = &ble_hs_ev_stop;
+    struct ble_npl_sem  *stop_sem  = &ble_hs_stop_sem;
 
+    nimble_port_run_active = true;
     while (1) {
         ev = ble_npl_eventq_get(&g_eventq_dflt, BLE_NPL_TIME_FOREVER);
         if (ev) {
             ble_npl_event_run(ev);
-            if (ev == &ble_hs_ev_stop) {
-                ble_npl_event_deinit(&ble_hs_ev_stop);
-                ble_npl_sem_release(&ble_hs_stop_sem);
+            if (ev == stop_ev) {
+                ble_npl_event_deinit(stop_ev);
+                nimble_port_run_active = false;
+                ble_npl_sem_release(stop_sem);
                 break;
             }
         }
@@ -438,18 +470,18 @@ struct ble_npl_eventq *
 nimble_port_get_dflt_eventq(void)
 {
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    /* Check deiniting first: context may still be non-NULL during the deinit
+     * window after ble_npl_deiniting is set but before ble_npl_ctx is freed.
+     * Return the fallback to prevent callers from using a deinitialized queue. */
+    if (ble_npl_deiniting) {
+        return &g_eventq_shutdown_fallback;
+    }
+
     /* If context exists, return the real eventq */
     if (ble_npl_ctx != NULL) {
         return &g_eventq_dflt;
     }
 
-    /* Context is NULL - either not yet allocated or already freed during deinit.
-     * If we're in shutdown, return the static fallback to prevent crashes.
-     * The fallback eventq is not functional but callers can safely reference it. */
-    if (ble_npl_deiniting) {
-        return &g_eventq_shutdown_fallback;
-    }
-
     /* Normal case: try to allocate context */
     if (ble_npl_ensure_ctx()) {
         return NULL;

porting/nimble/src/os_mbuf.c

@@ -121,7 +121,7 @@ os_mqueue_put(struct os_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *
     int rc;
 
     /* Can only place the head of a chained mbuf on the queue. */
-    if (!OS_MBUF_IS_PKTHDR(m)) {
+    if (m == NULL || !OS_MBUF_IS_PKTHDR(m)) {
         rc = OS_EINVAL;
         goto err;
     }
@@ -153,15 +153,23 @@ os_msys_register(struct os_mbuf_pool *new_pool)
 
     pool = NULL;
     STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) {
-        if (new_pool->omp_databuf_len > pool->omp_databuf_len) {
+        if (new_pool->omp_databuf_len < pool->omp_databuf_len) {
             break;
         }
     }
 
-    if (pool) {
-        STAILQ_INSERT_AFTER(&g_msys_pool_list, pool, new_pool, omp_next);
-    } else {
+    if (pool == NULL) {
         STAILQ_INSERT_TAIL(&g_msys_pool_list, new_pool, omp_next);
+    } else if (pool == STAILQ_FIRST(&g_msys_pool_list)) {
+        STAILQ_INSERT_HEAD(&g_msys_pool_list, new_pool, omp_next);
+    } else {
+        struct os_mbuf_pool *prev;
+
+        prev = STAILQ_FIRST(&g_msys_pool_list);
+        while (STAILQ_NEXT(prev, omp_next) != pool) {
+            prev = STAILQ_NEXT(prev, omp_next);
+        }
+        STAILQ_INSERT_AFTER(&g_msys_pool_list, prev, new_pool, omp_next);
     }
 
     return (0);
@@ -216,7 +224,7 @@ os_msys_get(uint16_t dsize, uint16_t leadingspace)
 err:
     log_count ++;
     if ((log_count % 100) == 0) {
-        ESP_LOGI("ESP_LOG_INFO","_os_msys_find_pool failed (size %u)\n",dsize);
+        ESP_LOGE("nimble_mbuf", "_os_msys_find_pool failed (size %u)", dsize);
         log_count = 0;
     }
 
@@ -241,7 +249,7 @@ os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len)
 err:
     log_count ++;
     if ((log_count % 100) == 0) {
-        ESP_LOGI("ESP_LOG_INFO","_os_msys_find_pool failed (size %u)\n",dsize);
+        ESP_LOGE("nimble_mbuf", "_os_msys_find_pool failed (size %u)", dsize);
         log_count = 0;
     }
     return (NULL);
@@ -570,6 +578,8 @@ os_mbuf_dup(struct os_mbuf *om)
 
             if (OS_MBUF_IS_PKTHDR(om)) {
                 _os_mbuf_copypkthdr(head, om);
+                /* Restore leading space that _os_mbuf_copypkthdr overwrote */
+                head->om_data += OS_MBUF_LEADINGSPACE(om);
             }
             copy = head;
         }
@@ -590,6 +600,10 @@ os_mbuf_off(const struct os_mbuf *om, int off, uint16_t *out_off)
     struct os_mbuf *next;
     struct os_mbuf *cur;
 
+    if (off < 0) {
+        return NULL;
+    }
+
     /* Cast away const. */
     cur = (struct os_mbuf *)om;
 
@@ -618,7 +632,11 @@ os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst)
     unsigned int count;
     uint8_t *udst;
 
-    if (!len) {
+    if (off < 0 || len < 0) {
+        return -1;
+    }
+
+    if (len == 0) {
         return 0;
     }
 
@@ -872,6 +890,10 @@ os_mbuf_prepend(struct os_mbuf *om, int len)
 struct os_mbuf *
 os_mbuf_prepend_pullup(struct os_mbuf *om, uint16_t len)
 {
+    if (om == NULL) {
+        return NULL;
+    }
+
     om = os_mbuf_prepend(om, len);
     if (om == NULL) {
         return NULL;
@@ -894,6 +916,7 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len)
     uint16_t cur_off;
     int copylen;
     int rc;
+    int orig_len = len;
 
     /* Find the mbuf,offset pair for the start of the destination. */
     cur = os_mbuf_off(om, off, &cur_off);
@@ -936,7 +959,7 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len)
     /* Fix up the packet header, if one is present. */
     if (OS_MBUF_IS_PKTHDR(om)) {
         OS_MBUF_PKTHDR(om)->omp_len =
-            max(OS_MBUF_PKTHDR(om)->omp_len, off + len);
+            max(OS_MBUF_PKTHDR(om)->omp_len, off + orig_len);
     }
 
     return 0;
@@ -1052,6 +1075,7 @@ os_mbuf_pullup(struct os_mbuf *om, uint16_t len)
 
         if (OS_MBUF_IS_PKTHDR(om)) {
             _os_mbuf_copypkthdr(om2, om);
+            om->om_pkthdr_len = 0;
         }
     }
     space = OS_MBUF_TRAILINGSPACE(om2);
@@ -1202,6 +1226,11 @@ os_mbuf_widen(struct os_mbuf *om, uint16_t off, uint16_t len)
     }
     edge_om->om_len = sub_off;
 
+    /* Find actual tail of gap chain (os_mbuf_append may have extended it). */
+    while (SLIST_NEXT(prev, om_next) != NULL) {
+        prev = SLIST_NEXT(prev, om_next);
+    }
+
     /* Insert the gap into the chain. */
     SLIST_NEXT(prev, om_next) = SLIST_NEXT(edge_om, om_next);
     SLIST_NEXT(edge_om, om_next) = first_new;

porting/nimble/src/os_msys_init.c

@@ -149,7 +149,7 @@ os_msys_sanity_min_count(int idx)
 
     default:
         BLE_LL_ASSERT(0);
-        return ESP_OK;
+        return 0;
     }
 }
 
@@ -291,6 +291,8 @@ os_msys_buf_free(void)
     os_mempool_unregister(&os_msys_init_2_mempool);
 #endif
 #endif
+
+    os_msys_reset();
 }
 
 void os_msys_init(void)

porting/npl/dummy/src/npl_os_dummy.c

@@ -101,7 +101,7 @@ 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)
 {
-    return BLE_NPL_ENOENT;
+    return BLE_NPL_OK;
 }
 
 ble_npl_error_t
@@ -128,10 +128,11 @@ ble_npl_sem_get_count(struct ble_npl_sem *sem)
     return 0;
 }
 
-void
+int
 ble_npl_callout_init(struct ble_npl_callout *c, struct ble_npl_eventq *evq,
                      ble_npl_event_fn *ev_cb, void *ev_arg)
 {
+    return 0;
 }
 
 ble_npl_error_t
@@ -167,7 +168,8 @@ ble_npl_time_get(void)
 ble_npl_error_t
 ble_npl_time_ms_to_ticks(uint32_t ms, ble_npl_time_t *out_ticks)
 {
-    return BLE_NPL_ENOENT;
+    *out_ticks = ms;
+    return 0;
 }
 
 ble_npl_error_t

porting/npl/esp-idf/src/nimble_port_freertos.c

@@ -21,10 +21,13 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "nimble/nimble_port.h"
+#include "esp_log.h"
 #if CONFIG_BT_CONTROLLER_ENABLED
 #include "esp_bt.h"
 #endif
 
+#define NIMBLE_PORT_LOG_TAG "BLE_INIT"
+
 static TaskHandle_t host_task_h = NULL;
 
 /**
@@ -35,13 +38,28 @@ static TaskHandle_t host_task_h = NULL;
  */
 esp_err_t esp_nimble_enable(void *host_task)
 {
+    if (host_task == NULL) {
+        ESP_LOGE(NIMBLE_PORT_LOG_TAG, "esp_nimble_enable: host_task is NULL");
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    if (host_task_h != NULL) {
+        return ESP_ERR_INVALID_STATE;
+    }
+
     /*
      * Create task where NimBLE host will run. It is not strictly necessary to
      * have separate task for NimBLE host, but since something needs to handle
      * default queue it is just easier to make separate task which does this.
      */
-    xTaskCreatePinnedToCore(host_task, "nimble_host", NIMBLE_HS_STACK_SIZE,
-                            NULL, (configMAX_PRIORITIES - 4), &host_task_h, NIMBLE_CORE);
+    BaseType_t ret = xTaskCreatePinnedToCore(host_task, "nimble_host",
+                                             NIMBLE_HS_STACK_SIZE, NULL,
+                                             (configMAX_PRIORITIES - 4),
+                                             &host_task_h, NIMBLE_CORE);
+    if (ret != pdPASS) {
+        host_task_h = NULL;
+        return ESP_ERR_NO_MEM;
+    }
     return ESP_OK;
 
 }
@@ -54,8 +72,9 @@ esp_err_t esp_nimble_enable(void *host_task)
 esp_err_t esp_nimble_disable(void)
 {
     if (host_task_h) {
-        vTaskDelete(host_task_h);
+        TaskHandle_t tmp = host_task_h;
         host_task_h = NULL;
+        vTaskDelete(tmp);
     }
     return ESP_OK;
 }
@@ -69,7 +88,14 @@ esp_err_t esp_nimble_disable(void)
 void
 nimble_port_freertos_init(TaskFunction_t host_task_fn)
 {
-    esp_nimble_enable(host_task_fn);
+    esp_err_t err = esp_nimble_enable(host_task_fn);
+    if (err != ESP_OK) {
+        ESP_LOGE(NIMBLE_PORT_LOG_TAG,
+                 "nimble_port_freertos_init: esp_nimble_enable failed (%d)", err);
+        /* BLE host task creation is critical; abort so the system does not
+         * continue in a broken state where all BLE operations will fail. */
+        abort();
+    }
 }
 
 /**

porting/npl/freertos/src/nimble_port_freertos.c

@@ -24,6 +24,9 @@
 #if CONFIG_BT_CONTROLLER_ENABLED
 #include "esp_bt.h"
 #endif
+#include "esp_log.h"
+
+#define NIMBLE_PORT_LOG_TAG "BLE_INIT"
 
 static TaskHandle_t host_task_h = NULL;
 
@@ -35,13 +38,27 @@ static TaskHandle_t host_task_h = NULL;
  */
 esp_err_t esp_nimble_enable(void *host_task)
 {
+    if (host_task == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    if (host_task_h != NULL) {
+        return ESP_ERR_INVALID_STATE;
+    }
+
     /*
      * Create task where NimBLE host will run. It is not strictly necessary to
      * have separate task for NimBLE host, but since something needs to handle
      * default queue it is just easier to make separate task which does this.
      */
-    xTaskCreatePinnedToCore(host_task, "nimble_host", NIMBLE_HS_STACK_SIZE,
-                            NULL, (configMAX_PRIORITIES - 4), &host_task_h, NIMBLE_CORE);
+    BaseType_t ret = xTaskCreatePinnedToCore(host_task, "nimble_host",
+                                             NIMBLE_HS_STACK_SIZE, NULL,
+                                             (configMAX_PRIORITIES - 4),
+                                             &host_task_h, NIMBLE_CORE);
+    if (ret != pdPASS) {
+        host_task_h = NULL;
+        return ESP_ERR_NO_MEM;
+    }
     return ESP_OK;
 
 }
@@ -54,8 +71,9 @@ esp_err_t esp_nimble_enable(void *host_task)
 esp_err_t esp_nimble_disable(void)
 {
     if (host_task_h) {
-        vTaskDelete(host_task_h);
+        TaskHandle_t tmp = host_task_h;
         host_task_h = NULL;
+        vTaskDelete(tmp);
     }
     return ESP_OK;
 }
@@ -69,7 +87,12 @@ esp_err_t esp_nimble_disable(void)
 void
 nimble_port_freertos_init(TaskFunction_t host_task_fn)
 {
-    esp_nimble_enable(host_task_fn);
+    esp_err_t err = esp_nimble_enable(host_task_fn);
+    if (err != ESP_OK) {
+        ESP_LOGE(NIMBLE_PORT_LOG_TAG,
+                 "nimble_port_freertos_init: esp_nimble_enable failed (%d)", err);
+        abort();
+    }
 }
 
 /**

porting/npl/freertos/src/npl_os_freertos.c

@@ -305,7 +305,7 @@ npl_freertos_eventq_init(struct ble_npl_eventq *evq)
         BLE_LL_ASSERT(eventq);
 
         memset(eventq, 0, sizeof(*eventq));
-        eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_eventq *));
+        eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_event *));
         BLE_LL_ASSERT(eventq->q);
     }
 #else
@@ -315,7 +315,7 @@ npl_freertos_eventq_init(struct ble_npl_eventq *evq)
         BLE_LL_ASSERT(eventq);
 
         memset(eventq, 0, sizeof(*eventq));
-        eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_eventq *));
+        eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_event *));
         BLE_LL_ASSERT(eventq->q);
     }
 #endif
@@ -326,7 +326,9 @@ npl_freertos_eventq_deinit(struct ble_npl_eventq *evq)
 {
     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
 
-    BLE_LL_ASSERT(eventq);
+    if (!eventq) {
+        return;
+    }
     vQueueDelete(eventq->q);
 #if OS_MEM_ALLOC
     os_memblock_put(&ble_freertos_evq_pool,eventq);
@@ -401,28 +403,6 @@ npl_eventq_queued_get_isr(struct ble_npl_event_freertos *event)
     return queued;
 }
 
-static void IRAM_ATTR
-npl_eventq_queued_set_isr(struct ble_npl_event_freertos *event, bool queued)
-{
-    portENTER_CRITICAL_ISR(&ble_port_mutex);
-    event->queued = queued;
-    portEXIT_CRITICAL_ISR(&ble_port_mutex);
-}
-
-static bool IRAM_ATTR
-npl_eventq_queued_claim_isr(struct ble_npl_event_freertos *event)
-{
-    bool already;
-
-    portENTER_CRITICAL_ISR(&ble_port_mutex);
-    already = event->queued;
-    if (!already) {
-        event->queued = true;
-    }
-    portEXIT_CRITICAL_ISR(&ble_port_mutex);
-    return already;
-}
-
 static void IRAM_ATTR
 npl_eventq_queued_set_task(struct ble_npl_event_freertos *event, bool queued)
 {
@@ -478,23 +458,27 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo)
 {
     struct ble_npl_event *ev = NULL;
     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
-    BaseType_t woken;
+    BaseType_t woken = pdFALSE;
     BaseType_t ret;
 
     if (in_isr()) {
         BLE_LL_ASSERT(tmo == 0);
+        woken = pdFALSE;
+
+        portENTER_CRITICAL_ISR(&ble_port_mutex);
         ret = xQueueReceiveFromISR(eventq->q, &ev, &woken);
-        if( woken == pdTRUE ) {
+        if (ret == pdPASS && ev != NULL) {
+            struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
+            if (event) {
+                event->queued = false;
+            }
+        }
+        portEXIT_CRITICAL_ISR(&ble_port_mutex);
+
+        if (woken == pdTRUE) {
             portYIELD_FROM_ISR();
         }
         BLE_LL_ASSERT(ret == pdPASS || ret == errQUEUE_EMPTY);
-
-        if (ev) {
-            struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
-            if (event) {
-                npl_eventq_queued_set_isr(event, false);
-            }
-        }
     } else if (tmo == 0) {
         bool locked = npl_eventq_lock();
 
@@ -555,22 +539,30 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo)
 void
 npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
 {
-    BaseType_t woken;
+    BaseType_t woken = pdFALSE;
     BaseType_t ret;
     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
 
     if (in_isr()) {
-        if (npl_eventq_queued_claim_isr(event)) {
+        woken = pdFALSE;
+
+        portENTER_CRITICAL_ISR(&ble_port_mutex);
+        if (event->queued) {
+            portEXIT_CRITICAL_ISR(&ble_port_mutex);
             return;
         }
+        event->queued = true;
 
         ret = xQueueSendToBackFromISR(eventq->q, &ev, &woken);
         if (ret != pdPASS) {
-            npl_eventq_queued_set_isr(event, false);
+            event->queued = false;
+            portEXIT_CRITICAL_ISR(&ble_port_mutex);
             return;
         }
-        if( woken == pdTRUE ) {
+        portEXIT_CRITICAL_ISR(&ble_port_mutex);
+
+        if (woken == pdTRUE) {
             portYIELD_FROM_ISR();
         }
         return;
@@ -591,7 +583,7 @@ npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
     }
 }
 
-void
+IRAM_ATTR void
 npl_freertos_eventq_remove(struct ble_npl_eventq *evq,
                        struct ble_npl_event *ev)
 {
@@ -600,7 +592,7 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq,
     int i;
     int count;
     bool removed;
-    BaseType_t woken, woken2;
+    BaseType_t woken = pdFALSE, woken2 = pdFALSE;
     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
 
@@ -649,8 +641,6 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq,
             portYIELD_FROM_ISR();
         }
     } else {
-        removed = false;
-
         bool locked = npl_eventq_lock();
 
         if (!npl_eventq_queued_get_task(event)) {
@@ -658,6 +648,7 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq,
             return;
         }
 
+        removed = false;
         portENTER_CRITICAL(&ble_port_mutex);
         count = uxQueueMessagesWaiting(eventq->q);
         for (i = 0; i < count; i++) {
@@ -695,12 +686,16 @@ npl_freertos_mutex_init(struct ble_npl_mutex *mu)
         mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
 
         if (!mutex) {
-            return BLE_NPL_INVALID_PARAM;
+            return BLE_NPL_ENOMEM;
         }
 
         memset(mutex, 0, sizeof(*mutex));
         mutex->handle = xSemaphoreCreateRecursiveMutex();
-        BLE_LL_ASSERT(mutex->handle);
+        if (!mutex->handle) {
+            os_memblock_put(&ble_freertos_mutex_pool, mutex);
+            mu->mutex = NULL;
+            return BLE_NPL_ENOMEM;
+        }
     }
 #else
     if(!mu->mutex) {
@@ -708,12 +703,16 @@ npl_freertos_mutex_init(struct ble_npl_mutex *mu)
         mutex = (struct ble_npl_mutex_freertos *)mu->mutex;
 
         if (!mutex) {
-            return BLE_NPL_INVALID_PARAM;
+            return BLE_NPL_ENOMEM;
         }
 
         memset(mutex, 0, sizeof(*mutex));
         mutex->handle = xSemaphoreCreateRecursiveMutex();
-        BLE_LL_ASSERT(mutex->handle);
+        if (!mutex->handle) {
+            nimble_platform_mem_free((void *)mutex);
+            mu->mutex = NULL;
+            return BLE_NPL_ENOMEM;
+        }
     }
 #endif
 
@@ -822,7 +821,7 @@ npl_freertos_mutex_release(struct ble_npl_mutex *mu)
     BLE_LL_ASSERT(mutex->handle);
 
     if (in_isr()) {
-        BLE_LL_ASSERT(0);
+        return BLE_NPL_ERR_IN_ISR;
     } else {
         if (xSemaphoreGiveRecursive(mutex->handle) != pdPASS) {
             return BLE_NPL_BAD_MUTEX;
@@ -842,12 +841,16 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
         semaphor = (struct ble_npl_sem_freertos *)sem->sem;
 
         if (!semaphor) {
-            return BLE_NPL_INVALID_PARAM;
+            return BLE_NPL_ENOMEM;
         }
 
         memset(semaphor, 0, sizeof(*semaphor));
         semaphor->handle = xSemaphoreCreateCounting(128, tokens);
-        BLE_LL_ASSERT(semaphor->handle);
+        if (!semaphor->handle) {
+            os_memblock_put(&ble_freertos_sem_pool, semaphor);
+            sem->sem = NULL;
+            return BLE_NPL_ENOMEM;
+        }
     }
 #else
     if(!sem->sem) {
@@ -855,12 +858,16 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
         semaphor = (struct ble_npl_sem_freertos *)sem->sem;
 
         if (!semaphor) {
-            return BLE_NPL_INVALID_PARAM;
+            return BLE_NPL_ENOMEM;
         }
 
         memset(semaphor, 0, sizeof(*semaphor));
         semaphor->handle = xSemaphoreCreateCounting(128, tokens);
-        BLE_LL_ASSERT(semaphor->handle);
+        if (!semaphor->handle) {
+            nimble_platform_mem_free((void *)semaphor);
+            sem->sem = NULL;
+            return BLE_NPL_ENOMEM;
+        }
     }
 #endif
 
@@ -892,7 +899,7 @@ npl_freertos_sem_deinit(struct ble_npl_sem *sem)
 ble_npl_error_t
 npl_freertos_sem_pend(struct ble_npl_sem *sem, ble_npl_time_t timeout)
 {
-    BaseType_t woken;
+    BaseType_t woken = pdFALSE;
     BaseType_t ret;
     struct ble_npl_sem_freertos *semaphor = (struct ble_npl_sem_freertos *)sem->sem;
 
@@ -919,7 +926,7 @@ ble_npl_error_t
 npl_freertos_sem_release(struct ble_npl_sem *sem)
 {
     BaseType_t ret;
-    BaseType_t woken;
+    BaseType_t woken = pdFALSE;
     struct ble_npl_sem_freertos *semaphor = (struct ble_npl_sem_freertos *)sem->sem;
 
     if (!semaphor) {
@@ -952,11 +959,13 @@ ble_npl_event_fn_wrapper(void *arg)
         ble_npl_eventq_put(callout->evq, &callout->ev);
     } else {
         struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)callout->ev.event;
-        event->fn(&callout->ev);
+        if (event && event->fn) {
+            event->fn(&callout->ev);
+        }
     }
 }
 
-static
+static IRAM_ATTR
 ble_npl_error_t esp_err_to_npl_error(esp_err_t err)
 {
     switch(err) {
@@ -1024,6 +1033,7 @@ npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq
         }
 
 #else
+	callout->evq = evq;
 	callout->handle = xTimerCreate("co", 1, pdFALSE, callout, os_callout_timer_cb);
 
         if (!callout->handle) {
@@ -1069,6 +1079,7 @@ npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq
             return -1;
         }
 #else
+	callout->evq = evq;
 	callout->handle = xTimerCreate("co", 1, pdFALSE, callout, os_callout_timer_cb);
 
         if (!callout->handle) {
@@ -1125,11 +1136,17 @@ uint16_t
 npl_freertos_sem_get_count(struct ble_npl_sem *sem)
 {
     struct ble_npl_sem_freertos *semaphor = (struct ble_npl_sem_freertos *)sem->sem;
+    if (!semaphor || !semaphor->handle) {
+        return 0;
+    }
+    if (in_isr()) {
+        return uxQueueMessagesWaitingFromISR(semaphor->handle);
+    }
     return uxSemaphoreGetCount(semaphor->handle);
 }
 
 
-ble_npl_error_t
+IRAM_ATTR ble_npl_error_t
 npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks)
 {
     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
@@ -1139,10 +1156,10 @@ npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks)
         npl_freertos_eventq_remove(callout->evq, &callout->ev);
     }
 
-    return esp_err_to_npl_error(esp_timer_start_once(callout->handle, ticks*1000));
+    return esp_err_to_npl_error(esp_timer_start_once(callout->handle, (uint64_t)ticks * 1000));
 #else
 
-    BaseType_t woken1, woken2, woken3;
+    BaseType_t woken1 = pdFALSE, woken2 = pdFALSE, woken3 = pdFALSE;
 
     if (ticks == 0) {
         ticks = 1;
@@ -1164,14 +1181,13 @@ npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks)
             npl_freertos_eventq_remove(callout->evq, &callout->ev);
         }
         xTimerChangePeriod(callout->handle, ticks, portMAX_DELAY);
-        xTimerReset(callout->handle, portMAX_DELAY);
     }
 
     return BLE_NPL_OK;
 #endif
 }
 
-void
+IRAM_ATTR void
 npl_freertos_callout_stop(struct ble_npl_callout *co)
 {
     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
@@ -1181,9 +1197,19 @@ npl_freertos_callout_stop(struct ble_npl_callout *co)
     }
 
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
-    esp_timer_stop(callout->handle);
+    if (!in_isr()) {
+        esp_timer_stop(callout->handle);
+    }
 #else
-    xTimerStop(callout->handle, portMAX_DELAY);
+    if (in_isr()) {
+        BaseType_t woken = pdFALSE;
+        xTimerStopFromISR(callout->handle, &woken);
+        if (woken == pdTRUE) {
+            portYIELD_FROM_ISR();
+        }
+    } else {
+        xTimerStop(callout->handle, portMAX_DELAY);
+    }
 #endif
 
     if (callout->evq) {
@@ -1195,6 +1221,9 @@ bool
 npl_freertos_callout_is_active(struct ble_npl_callout *co)
 {
     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
+    if (!callout) {
+        return false;
+    }
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
     return esp_timer_is_active(callout->handle);
 #else
@@ -1206,15 +1235,28 @@ ble_npl_time_t
 npl_freertos_callout_get_ticks(struct ble_npl_callout *co)
 {
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
-   /* Currently, esp_timer does not support an API which gets the expiry time for
-    * current timer.
-    * Returning 0 from here should not cause any effect.
-    * Drawback of this approach is that existing code to reset timer would be called
-    * more often (since the if condition to invoke reset timer would always succeed if
-    * timer is active).
-    */
+    uint32_t exp = 0;
 
-    return 0;
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
+    struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
+    if (!callout) {
+        return 0;
+    }
+
+    uint64_t expiry = 0;
+    esp_err_t err;
+
+    err = esp_timer_get_expiry_time((esp_timer_handle_t)(callout->handle), &expiry);
+    if (err != ESP_OK) {
+        return 0;
+    }
+
+    npl_freertos_time_ms_to_ticks((uint32_t)(expiry / 1000), &exp);
+#else
+    exp = 0;
+#endif
+
+    return exp;
 #else
     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
     return xTimerGetExpiryTime(callout->handle);
@@ -1253,8 +1295,9 @@ npl_freertos_callout_remaining_ticks(struct ble_npl_callout *co,
     exp = xTimerGetExpiryTime(callout->handle);
 #endif
 
-    if (exp > now) {
-        rt = exp - now;
+    int32_t diff = (int32_t)(exp - now);
+    if (diff > 0) {
+        rt = (ble_npl_time_t)diff;
     } else {
         rt = 0;
     }
@@ -1276,7 +1319,11 @@ npl_freertos_time_get(void)
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
     return esp_timer_get_time() / 1000;
 #else
-    return xTaskGetTickCountFromISR();
+    if (xPortInIsrContext()) {
+        return xTaskGetTickCountFromISR();
+    } else {
+        return xTaskGetTickCount();
+    }
 #endif
 }
 
@@ -1322,7 +1369,7 @@ npl_freertos_time_ms_to_ticks32(uint32_t ms)
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
     return ms;
 #else
-    return ms * configTICK_RATE_HZ / 1000;
+    return (ble_npl_time_t)((uint64_t)ms * configTICK_RATE_HZ / 1000);
 #endif
 }
 
@@ -1332,7 +1379,7 @@ npl_freertos_time_ticks_to_ms32(ble_npl_time_t ticks)
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
     return ticks;
 #else
-    return ticks * 1000 / configTICK_RATE_HZ;
+    return (uint32_t)((uint64_t)ticks * 1000 / configTICK_RATE_HZ);
 #endif
 }
 
@@ -1457,6 +1504,9 @@ struct npl_funcs_t * npl_freertos_funcs_get(void)
 
 void npl_freertos_funcs_init(void)
 {
+    if (npl_funcs != NULL) {
+        return;
+    }
     npl_funcs = (struct npl_funcs_t *)nimble_platform_mem_calloc(1,sizeof(struct npl_funcs_t));
     if(!npl_funcs) {
         assert(0);
@@ -1589,7 +1639,6 @@ _error:
     }
 #endif
 
-   BLE_LL_ASSERT(rc == 0);
    return rc;
 }
 
@@ -1600,6 +1649,12 @@ void npl_freertos_mempool_deinit(void)
         npl_eventq_sync = NULL;
     }
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (!ble_freertos_ctx) {
+        return;
+    }
+#endif
+
 #if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED
     if (ble_freertos_ev_buf) {
         nimble_platform_mem_free(ble_freertos_ev_buf);