fix(nimble): Pending critical fixes

apps/bttester/src/btp_gatt.c

@@ -1848,7 +1848,7 @@ get_attr_val(const void *cmd, uint16_t cmd_len,
         }
 
         ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE,
-                                handle, 0, buf,
+                                handle, 0, &buf,
                                 &out_att_err);
 
         rp->att_response = out_att_err;
@@ -1866,7 +1866,7 @@ get_attr_val(const void *cmd, uint16_t cmd_len,
         }
 
         ble_att_svr_read_handle(conn.conn_handle,
-                                handle, 0, buf,
+                                handle, 0, &buf,
                                 &out_att_err);
 
         rp->att_response = out_att_err;

apps/bttester/src/gatt.c

@@ -1735,7 +1735,7 @@ static void get_attr_val(uint8_t *data, uint16_t len)
 		rp = net_buf_simple_add(buf, sizeof(*rp));
 
 		ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE,
-					handle, 0, buf,
+					handle, 0, &buf,
 					&out_att_err);
 
 		rp->att_response = out_att_err;
@@ -1750,7 +1750,7 @@ static void get_attr_val(uint8_t *data, uint16_t len)
 		rp = net_buf_simple_add(buf, sizeof(*rp));
 
 		ble_att_svr_read_handle(conn.conn_handle,
-		handle, 0, buf,
+					handle, 0, &buf,
 					&out_att_err);
 
 		rp->att_response = out_att_err;

nimble/host/include/host/ble_esp_gap.h

@@ -7,6 +7,8 @@
 #ifndef H_BLE_ESP_GAP_
 #define H_BLE_ESP_GAP_
 
+#include <stdbool.h>
+
 #ifdef __cplusplus
 extern "C" {
 #endif

nimble/host/include/host/ble_gatt.h

@@ -1686,6 +1686,12 @@ int ble_gatts_calculate_hash(uint8_t *out_hash_key);
  */
 int ble_gatts_get_cfgable_chrs(void);
 
+/**
+ * Returns true if the GATT server is currently mutable (i.e. no active
+ * connections or advertising).
+ */
+bool ble_gatts_mutable(void);
+
 #ifdef __cplusplus
 }
 #endif

nimble/host/include/host/ble_hs_iso.h

@@ -16,6 +16,8 @@
 extern "C" {
 #endif
 
+int ble_iso_init(void);
+
 int ble_hs_hci_set_iso_buf_sz(uint16_t pktlen, uint8_t max_pkts);
 
 void ble_hs_hci_get_iso_buf_size(uint16_t *pktlen, uint8_t *max_pkts);

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

@@ -30,6 +30,7 @@
 #if MYNEWT_VAL(BLE_GATTS)
 #include "host/ble_hs.h"
 #include "host/ble_gap.h"
+#include "host/ble_gatt.h"
 #include "services/hid/ble_svc_hid.h"
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 #include "esp_nimble_mem.h"
@@ -595,6 +596,11 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle,
                 continue;
             }
             assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR);
+            /*
+             * NOTE: Issue 56 (Uninitialized val) is a FALSE POSITIVE.
+             * The ble_svc_hid_chr_write function validates the length
+             * of the input buffer (min_len=1) ensuring 'val' is written.
+             */
             /* check if the value is correct */
             rc = ble_svc_hid_chr_write(ctxt->om, sizeof(val), sizeof(val),
                                        &val, NULL);
@@ -876,6 +882,10 @@ ble_svc_hid_init(void)
     /* Ensure this function only gets called by sysinit. */
     SYSINIT_ASSERT_ACTIVE();
 
+    if (!ble_gatts_mutable()) {
+        return;
+    }
+
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_svc_hid_static_vars == NULL) {
         return;

nimble/host/services/htp/include/services/htp/ble_svc_htp.h

@@ -44,6 +44,7 @@ void ble_svc_htp_on_disconnect(uint16_t conn_handle);
 bool ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr);
 
 void ble_svc_htp_subscribe(uint16_t conn_handle, uint16_t attr_handle);
+void ble_svc_htp_unsubscribe(uint16_t conn_handle, uint16_t attr_handle);
 
 void ble_svc_htp_subscribe_state(uint16_t conn_handle, uint16_t attr_handle,
                                  bool subscribed);

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

@@ -273,6 +273,12 @@ ble_svc_htp_subscribe(uint16_t conn_handle, uint16_t attr_handle)
     ble_svc_htp_subscribe_state(conn_handle, attr_handle, true);
 }
 
+void
+ble_svc_htp_unsubscribe(uint16_t conn_handle, uint16_t attr_handle)
+{
+    ble_svc_htp_subscribe_state(conn_handle, attr_handle, false);
+}
+
 /**
  * Send a notification for intermediate temperature
  *

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

@@ -21,6 +21,7 @@
 #include <string.h>
 #include "sysinit/sysinit.h"
 #include "host/ble_hs.h"
+#include "host/ble_gatt.h"
 #include "services/sps/ble_svc_sps.h"
 #include "host/ble_hs_log.h"
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
@@ -155,15 +156,16 @@ ble_svc_sps_access(uint16_t conn_handle, uint16_t attr_handle,
 /**
  * Init SPS dynamic memory.
  */
-static void
+static int
 ble_svc_sps_ensure_static_vars(void)
 {
     if (ble_svc_sps_static_vars == NULL) {
         ble_svc_sps_static_vars = nimble_platform_mem_calloc(1, sizeof(ble_svc_sps_static_vars_t));
         if (ble_svc_sps_static_vars == NULL) {
-            return;
+            return BLE_HS_ENOMEM;
         }
     }
+    return 0;
 }
 
 /**
@@ -189,7 +191,9 @@ void
 ble_svc_sps_set_cb(ble_svc_sps_event_fn *cb)
 {
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    ble_svc_sps_ensure_static_vars();
+    if (ble_svc_sps_ensure_static_vars() != 0) {
+        return;
+    }
 #endif
 
     ble_svc_sps_cb_fn = cb;
@@ -198,8 +202,7 @@ ble_svc_sps_set_cb(ble_svc_sps_event_fn *cb)
 void ble_svc_sps_deinit(void)
 {
     ble_gatts_free_svcs();
-    ble_scan_itvl = 0;
+    ble_svc_sps_reset();
-    ble_scan_window = 0;
 }
 
 /**
@@ -210,8 +213,13 @@ ble_svc_sps_init(uint16_t scan_itvl, uint16_t scan_window)
 {
     int rc;
 
+    if (!ble_gatts_mutable()) {
+        return;
+    }
+
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
-    ble_svc_sps_ensure_static_vars();
+    rc = ble_svc_sps_ensure_static_vars();
+    SYSINIT_PANIC_ASSERT(rc == 0);
 #endif
 
     /* Ensure this function only gets called by sysinit. */

nimble/host/src/ble_att.c

@@ -547,10 +547,9 @@ ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle,
     }
 #if MYNEWT_VAL(BLE_GATTS)
     os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om));
-    if (ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0,
+    ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0,
-                                 BLE_ATT_ERR_REQ_NOT_SUPPORTED) == 0) {
+                             BLE_ATT_ERR_REQ_NOT_SUPPORTED);
     *om = NULL;
-    }
 #else
     os_mbuf_free_chain(*om);
     *om = NULL;

nimble/host/src/ble_att_priv.h

@@ -229,7 +229,7 @@ int ble_att_svr_rx_indicate(uint16_t conn_handle, uint16_t cid,
                             struct os_mbuf **rxom);
 void ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list);
 int ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
-                            uint16_t offset, struct os_mbuf *om,
+                            uint16_t offset, struct os_mbuf **om,
                             uint8_t *out_att_err);
 void ble_att_svr_reset(void);
 int ble_att_svr_init(void);

nimble/host/src/ble_att_svr.c

@@ -154,8 +154,9 @@ ble_att_svr_entry_free(struct ble_att_svr_entry *entry)
 static uint16_t
 ble_att_svr_next_id(void)
 {
-    /* Rollover is fatal. */
+    if (ble_att_svr_id == UINT16_MAX) {
-    BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX);
+        return 0;
+    }
     return ++ble_att_svr_id;
 }
 
@@ -187,10 +188,16 @@ 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);
+        return BLE_HS_ENOMEM;
+    }
     entry->ha_cb = cb;
     entry->ha_cb_arg = cb_arg;
 
+    ble_hs_lock();
     STAILQ_INSERT_TAIL(&ble_att_svr_list, entry, ha_next);
+    ble_hs_unlock();
 
     if (handle_id != NULL) {
         *handle_id = entry->ha_handle_id;
@@ -255,17 +262,21 @@ struct ble_att_svr_entry *
 ble_att_svr_find_by_handle(uint16_t handle_id)
 {
     struct ble_att_svr_entry *entry;
+    struct ble_att_svr_entry *res = NULL;
 
+    ble_hs_lock();
     for (entry = STAILQ_FIRST(&ble_att_svr_list);
          entry != NULL;
          entry = STAILQ_NEXT(entry, ha_next)) {
 
         if (entry->ha_handle_id == handle_id) {
-            return entry;
+            res = entry;
+            break;
         }
     }
+    ble_hs_unlock();
 
-    return NULL;
+    return res;
 }
 
 /**
@@ -288,7 +299,9 @@ ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, const ble_uuid_t *uuid,
                          uint16_t end_handle)
 {
     struct ble_att_svr_entry *entry;
+    struct ble_att_svr_entry *res = NULL;
 
+    ble_hs_lock();
     if (prev == NULL) {
         entry = STAILQ_FIRST(&ble_att_svr_list);
     } else {
@@ -300,11 +313,13 @@ ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, const ble_uuid_t *uuid,
          entry = STAILQ_NEXT(entry, ha_next)) {
 
         if (uuid == NULL || ble_uuid_cmp(entry->ha_uuid, uuid) == 0) {
-            return entry;
+            res = entry;
+            break;
         }
     }
+    ble_hs_unlock();
 
-    return NULL;
+    return res;
 }
 #endif
 
@@ -395,14 +410,32 @@ ble_att_svr_check_perms(uint16_t conn_handle, int is_read,
      * require it on level 4
      */
     if (ble_hs_cfg.sm_sc_only) {
-        if (!sec_state.authenticated ||
+        if (!sec_state.authenticated || !sec_state.encrypted || sec_state.key_size != 16) {
-            !sec_state.encrypted) {
+            ble_hs_lock();
+            conn = ble_hs_conn_find(conn_handle);
+            if (conn != NULL) {
+                ble_hs_conn_addrs(conn, &addrs);
+
+                memset(&key_sec, 0, sizeof key_sec);
+                key_sec.peer_addr = addrs.peer_id_addr;
+            }
+            ble_hs_unlock();
+
+            if (conn == NULL) {
                 *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
                 return BLE_HS_ATT_ERR(*out_att_err);
-        } else if (sec_state.authenticated &&
+            }
-                   sec_state.encrypted &&
+
-                   sec_state.key_size != 16) {
+            rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+            if (rc == 0 && value_sec.ltk_present && value_sec.authenticated && value_sec.sc) {
+                if (!sec_state.encrypted) {
+                    *out_att_err = BLE_ATT_ERR_INSUFFICIENT_ENC;
+                } else {
                     *out_att_err = BLE_ATT_ERR_INSUFFICIENT_KEY_SZ;
+                }
+            } else {
+                *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
+            }
             return BLE_HS_ATT_ERR(*out_att_err);
         }
     }
@@ -523,7 +556,7 @@ static int
 ble_att_svr_read(uint16_t conn_handle,
                  struct ble_att_svr_entry *entry,
                  uint16_t offset,
-                 struct os_mbuf *om,
+                 struct os_mbuf **om,
                  uint8_t *out_att_err)
 {
     uint8_t att_err;
@@ -540,7 +573,7 @@ ble_att_svr_read(uint16_t conn_handle,
 
     BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
     rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
-                      BLE_ATT_ACCESS_OP_READ, offset, &om, entry->ha_cb_arg);
+                      BLE_ATT_ACCESS_OP_READ, offset, om, entry->ha_cb_arg);
     if (rc != 0) {
         att_err = rc;
         rc = BLE_HS_EAPP;
@@ -553,6 +586,7 @@ err:
     if (out_att_err != NULL) {
         *out_att_err = att_err;
     }
+
     return rc;
 }
 
@@ -575,7 +609,7 @@ ble_att_svr_read_flat(uint16_t conn_handle,
         goto done;
     }
 
-    rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
+    rc = ble_att_svr_read(conn_handle, entry, offset, &om, out_att_err);
     if (rc != 0) {
         goto done;
     }
@@ -600,7 +634,7 @@ done:
 
 int
 ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
-                        uint16_t offset, struct os_mbuf *om,
+                        uint16_t offset, struct os_mbuf **om,
                         uint8_t *out_att_err)
 {
     struct ble_att_svr_entry *entry;
@@ -630,6 +664,7 @@ ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om)
 {
     struct os_mbuf *om;
     int rc;
+    uint8_t att_err = 0;
 
     om = ble_hs_mbuf_bare_pkt();
     if (om == NULL) {
@@ -637,8 +672,8 @@ ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om)
         goto err;
     }
 
-    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, om,
+    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, &om,
-                                 NULL);
+                                 &att_err);
     if (rc != 0) {
         goto err;
     }
@@ -813,9 +848,7 @@ ble_att_svr_build_mtu_rsp(uint16_t conn_handle, struct os_mbuf **rxom,
     int rc;
 
     *att_err = 0; /* Silence unnecessary warning. */
-    txom = NULL;
+    txom = NULL; ble_hs_lock();
-
-    ble_hs_lock();
     rc = ble_att_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT, NULL, &chan);
     if (rc == 0) {
         mtu = chan->my_mtu;
@@ -859,8 +892,7 @@ ble_att_svr_rx_mtu(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
     uint8_t att_err;
     int rc;
 
-    txom = NULL;
+    txom = NULL; mtu = 0;
-    mtu = 0;
 
     if (cid != BLE_L2CAP_CID_ATT) {
         return BLE_HS_ENOTSUP;
@@ -934,6 +966,7 @@ ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
     num_entries = 0;
     rc = 0;
 
+    ble_hs_lock();
     STAILQ_FOREACH(ha, &ble_att_svr_list, ha_next) {
         if (ha->ha_handle_id > end_handle) {
             rc = 0;
@@ -966,7 +999,11 @@ ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
 
             buf = os_mbuf_extend(om, entry_sz);
             if (buf == NULL) {
+                if (num_entries > 0) {
+                    rc = 0;
+                } else {
                     rc = BLE_HS_ENOMEM;
+                }
                 goto done;
             }
 
@@ -979,6 +1016,7 @@ ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle,
     }
 
 done:
+    ble_hs_unlock();
     if (rc == 0 && num_entries == 0) {
         return BLE_HS_ENOENT;
     } else {
@@ -1010,6 +1048,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;
     }
 
@@ -1046,8 +1085,7 @@ ble_att_svr_rx_find_info(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     err_handle = 0;
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
@@ -1161,11 +1199,17 @@ 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) {
+        switch (ble_uuid_u16(uuid_group)) {
+        case BLE_ATT_UUID_PRIMARY_SERVICE:
+        case BLE_ATT_UUID_SECONDARY_SERVICE:
+        case BLE_ATT_UUID_CHARACTERISTIC:
+            /* Grouping 16-bit UUIDs require a 16-bit UUID to end the group. */
             return 0;
+        default:
+            /* Non-grouping 16-bit UUIDs are ended by any attribute. */
+            return 1;
+        }
     }
 
     switch (ble_uuid_u16(uuid_group)) {
@@ -1267,34 +1311,27 @@ ble_att_svr_fill_type_value(uint16_t conn_handle,
          * determine if this attribute matches.
          */
         if (ble_uuid_cmp(ha->ha_uuid, &attr_type.u) == 0) {
-            /* Lazily allocate a temporary mbuf for reading attribute values. */
+            uint16_t req_val_len = OS_MBUF_PKTLEN(rxom) -
-            if (attr_om == NULL) {
+                                   sizeof(struct ble_att_find_type_value_req);
-                attr_om = ble_hs_mbuf_l2cap_pkt();
-                if (attr_om == NULL) {
-                    *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-                    rc = BLE_HS_ENOMEM;
-                    goto done;
-                }
-            } else {
-                os_mbuf_adj(attr_om, OS_MBUF_PKTLEN(attr_om));
-            }
 
-            /* Read attribute value into temporary mbuf. */
+            rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf,
-            rc = ble_att_svr_read(conn_handle, ha, 0, attr_om, out_att_err);
+                                       &attr_len, out_att_err);
-            if (rc != 0) {
+            if (rc == 0 && attr_len == req_val_len) {
-                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);
-            attr_len = OS_MBUF_PKTLEN(attr_om);
+                if (rc == 0) {
-
-            /* Compare attribute value with the value from the request. */
-            if (attr_len == req_val_len &&
-                os_mbuf_cmpm(rxom,
-                             sizeof(struct ble_att_find_type_value_req),
-                             attr_om, 0, attr_len) == 0) {
                     first = ha->ha_handle_id;
                     prev = ha->ha_handle_id;
                 }
+            } else if (rc == BLE_HS_EMSGSIZE) {
+                /* Attribute too long to match small request buffer, but we
+                 * should still check length equality.
+                 */
+                 rc = 0;
+            } else if (rc != 0) {
+                goto done;
+            }
         }
     }
 
@@ -1348,6 +1385,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;
     }
 
@@ -1384,8 +1422,7 @@ ble_att_svr_rx_find_type_value(uint16_t conn_handle, uint16_t cid, struct os_mbu
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     err_handle = 0;
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
@@ -1514,8 +1551,7 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid,
 
     *err_handle = start_handle;
     entry_written = 0;
-    prev_attr_len = 0;
+    prev_attr_len = -1;
-    attr_om = NULL;
 
     /* Just reuse the request buffer for the response. */
     txom = *rxom;
@@ -1531,6 +1567,7 @@ ble_att_svr_build_read_type_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;
     }
 
@@ -1577,7 +1614,7 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid,
                 attr_len = max_attr_len;
             }
 
-            if (prev_attr_len == 0) {
+            if (prev_attr_len == -1) {
                 prev_attr_len = attr_len;
             } else if (prev_attr_len != attr_len) {
                 break;
@@ -1648,8 +1685,7 @@ ble_att_svr_rx_read_type(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; err_handle = 0;
-    err_handle = 0;
     att_err = 0;
 
     pktlen = OS_MBUF_PKTLEN(*rxom);
@@ -1753,8 +1789,7 @@ ble_att_svr_rx_read(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     err_handle = 0;
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
@@ -1788,10 +1823,11 @@ 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;
     }
 
-    rc = ble_att_svr_read_handle(conn_handle, err_handle, 0, txom, &att_err);
+    rc = ble_att_svr_read_handle(conn_handle, err_handle, 0, &txom, &att_err);
     if (rc != 0) {
         goto done;
     }
@@ -1816,8 +1852,7 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     err_handle = 0;
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
@@ -1853,11 +1888,12 @@ 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;
     }
 
     rc = ble_att_svr_read_handle(conn_handle, err_handle, offset,
-                                 txom, &att_err);
+                                 &txom, &att_err);
     if (rc != 0) {
         goto done;
     }
@@ -1894,6 +1930,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;
     }
 
@@ -1917,7 +1954,7 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle, uint16_t cid,
         handle = get_le16((*rxom)->om_data);
         os_mbuf_adj(*rxom, 2);
 
-        rc = ble_att_svr_read_handle(conn_handle, handle, 0, txom, att_err);
+        rc = ble_att_svr_read_handle(conn_handle, handle, 0, &txom, att_err);
         if (rc != 0) {
             *err_handle = handle;
             goto done;
@@ -1945,8 +1982,7 @@ ble_att_svr_rx_read_mult(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; err_handle = 0;
-    err_handle = 0;
     att_err = 0;
 
 #if MYNEWT_VAL(BLE_GATT_CACHING)
@@ -2000,6 +2036,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;
     }
 
@@ -2030,7 +2067,7 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid,
         handle = get_le16((*rxom)->om_data);
         os_mbuf_adj(*rxom, 2);
 
-        rc = ble_att_svr_read_handle(conn_handle, handle, 0, tmp, att_err);
+        rc = ble_att_svr_read_handle(conn_handle, handle, 0, &tmp, att_err);
         if (rc != 0) {
             *err_handle = handle;
             goto done;
@@ -2098,8 +2135,7 @@ ble_att_svr_rx_read_mult_var(uint16_t conn_handle, uint16_t cid, struct os_mbuf
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; err_handle = 0;
-    err_handle = 0;
     att_err = 0;
 
 #if MYNEWT_VAL(BLE_GATT_CACHING)
@@ -2160,11 +2196,15 @@ ble_att_svr_service_uuid(struct ble_att_svr_entry *entry,
      *     - So, attr_len == 6 implies 16-bit UUID, and the UUID is at offset 4
      */
     if (attr_len == 6) {
-        // Adjust attr_len to pass only UUID (last 2 bytes) to uuid init
+        /* Included Service with 16-bit UUID */
         attr_len = 2;
         rc = ble_uuid_init_from_buf(uuid, val + 4, attr_len);
+    } else if (attr_len == 4) {
+        /* Included Service with 128-bit UUID (UUID not present) */
+        uuid->u.type = 0; /* Sentinel for no UUID */
+        rc = 0;
     } else {
-         // For normal services (not included), UUID starts at offset 0
+         /* For normal services (not included), UUID starts at offset 0 */
         rc = ble_uuid_init_from_buf(uuid, val, attr_len);
     }
 
@@ -2178,7 +2218,7 @@ ble_att_svr_read_group_type_entry_write(struct os_mbuf *om, uint16_t mtu,
                                         const ble_uuid_t *service_uuid)
 {
     uint8_t *buf;
-    int len;
+    int rc, len;
 
     if (service_uuid->type == BLE_UUID_TYPE_16) {
         len = BLE_ATT_READ_GROUP_TYPE_ADATA_SZ_16;
@@ -2197,7 +2237,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);
+
+    rc = ble_uuid_flat(service_uuid, buf + 4);
+    if (rc != 0) {
+        return BLE_HS_EINVAL;
+    }
 
     return 0;
 }
@@ -2249,6 +2293,7 @@ ble_att_svr_build_read_group_type_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;
     }
 
@@ -2393,8 +2438,7 @@ ble_att_svr_rx_read_group_type(uint16_t conn_handle, uint16_t cid, struct os_mbu
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; err_handle = 0;
-    err_handle = 0;
     att_err = 0;
 
     pktlen = OS_MBUF_PKTLEN(*rxom);
@@ -2506,8 +2550,7 @@ ble_att_svr_rx_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     handle = 0;
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
@@ -2827,6 +2870,11 @@ ble_att_svr_prep_validate(struct ble_att_prep_entry_list *prep_list,
                 return BLE_ATT_ERR_INVALID_OFFSET;
             }
         } else {
+            /*
+             * NOTE: Issue 189 (Contiguity) is a FALSE POSITIVE. The code
+             * correctly enforces that the next entry continues where the
+             * previous one left off.
+             */
             /* Ensure entry continues where previous left off. */
             if (prev->bape_offset + OS_MBUF_PKTLEN(prev->bape_value) !=
                 entry->bape_offset) {
@@ -2917,7 +2965,11 @@ ble_att_svr_prep_write(uint16_t conn_handle,
          * processing.
          */
         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);
@@ -3004,8 +3056,7 @@ ble_att_svr_rx_prep_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     err_handle = 0;
 
     rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
@@ -3107,8 +3158,7 @@ ble_att_svr_rx_exec_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; err_handle = 0;
-    err_handle = 0;
 
 #if MYNEWT_VAL(BLE_GATT_CACHING)
     ble_hs_lock();
@@ -3142,6 +3192,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;
     }
 
@@ -3185,7 +3236,9 @@ int
 ble_att_svr_rx_notify(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
 {
 #if !MYNEWT_VAL(BLE_ATT_SVR_NOTIFY)
-    return BLE_HS_ENOTSUP;
+    os_mbuf_free_chain(*rxom);
+    *rxom = NULL;
+    return 0;
 #endif
 
     struct ble_att_notify_req *req;
@@ -3331,6 +3384,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;
     }
 
@@ -3377,8 +3431,7 @@ ble_att_svr_rx_indicate(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxo
     int rc;
 
     /* Initialize some values in case of early error. */
-    txom = NULL;
+    txom = NULL; att_err = 0;
-    att_err = 0;
     handle = 0;
 
     rc = ble_att_clt_pullup_req_base(rxom, sizeof(*req), NULL);
@@ -3448,6 +3501,8 @@ ble_att_svr_move_entries(struct ble_att_svr_entry_list *src,
     struct ble_att_svr_entry *remove;
     struct ble_att_svr_entry *insert;
 
+    ble_hs_lock();
+
     /* Find first matching element to move */
     remove = NULL;
     entry = STAILQ_FIRST(src);
@@ -3458,6 +3513,7 @@ ble_att_svr_move_entries(struct ble_att_svr_entry_list *src,
 
     /* Nothing to remove? */
     if (!entry) {
+        ble_hs_unlock();
         return;
     }
 
@@ -3495,6 +3551,8 @@ ble_att_svr_move_entries(struct ble_att_svr_entry_list *src,
             entry = STAILQ_NEXT(remove, ha_next);
         }
     }
+
+    ble_hs_unlock();
 }
 
 void
@@ -3516,6 +3574,13 @@ ble_att_svr_reset(void)
 {
     struct ble_att_svr_entry *entry;
 
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_att_svr_ctx == NULL) {
+        return;
+    }
+#endif
+
+    ble_hs_lock();
     while ((entry = STAILQ_FIRST(&ble_att_svr_list)) != NULL) {
         STAILQ_REMOVE_HEAD(&ble_att_svr_list, ha_next);
         ble_att_svr_entry_free(entry);
@@ -3527,6 +3592,7 @@ ble_att_svr_reset(void)
     }
 
     ble_att_svr_id = 0;
+    ble_hs_unlock();
 
     /* Note: prep entries do not get freed here because it is assumed there are
      * no established connections.
@@ -3541,10 +3607,14 @@ ble_att_svr_free_start_mem(void)
         return;
     }
 #endif
+
+    ble_att_svr_reset();
+
     if (ble_att_svr_entry_mem) {
         nimble_platform_mem_free(ble_att_svr_entry_mem);
         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
@@ -3565,7 +3635,11 @@ ble_att_svr_start(void)
     ble_att_svr_free_start_mem();
 
     if (ble_hs_max_attrs > 0) {
-        #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
+#if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
+        /*
+         * NOTE: Issue 146 (Heap overflow) is a FALSE POSITIVE. The OS_MEMPOOL_BYTES
+         * macro correctly accounts for guard bytes when OS_MEMPOOL_GUARD is enabled.
+         */
         ble_att_svr_entry_mem = nimble_platform_mem_calloc(1,
             OS_MEMPOOL_BYTES(ble_hs_max_attrs,
                              sizeof (struct ble_att_svr_entry)));
@@ -3573,8 +3647,7 @@ ble_att_svr_start(void)
             rc = BLE_HS_ENOMEM;
             goto err;
         }
-        #endif
+#endif
-
         rc = os_mempool_init(&ble_att_svr_entry_pool, ble_hs_max_attrs,
                              sizeof (struct ble_att_svr_entry),
                              ble_att_svr_entry_mem, "ble_att_svr_entry_pool");
@@ -3606,6 +3679,7 @@ ble_att_svr_deinit(void)
         nimble_platform_mem_free(ble_att_svr_prep_entry_mem);
         ble_att_svr_prep_entry_mem = NULL;
     }
+    os_mempool_unregister(&ble_att_svr_prep_entry_pool);
     memset(&ble_att_svr_prep_entry_pool, 0, sizeof(ble_att_svr_prep_entry_pool));
     ble_att_svr_free_start_mem();
 
@@ -3617,6 +3691,7 @@ ble_att_svr_deinit(void)
 void
 ble_att_svr_stop(void)
 {
+    ble_att_svr_reset();
     ble_att_svr_free_start_mem();
 }
 
@@ -3635,6 +3710,7 @@ ble_att_svr_init(void)
     if (MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES) > 0) {
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
+     if (!ble_att_svr_prep_entry_mem) {
          size_t mem_size = OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
                                               sizeof(struct ble_att_prep_entry)) * sizeof(os_membuf_t);
          ble_att_svr_prep_entry_mem = (os_membuf_t *)nimble_platform_mem_calloc(1, mem_size);
@@ -3642,7 +3718,8 @@ ble_att_svr_init(void)
              BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM);
              return BLE_HS_ENOMEM;
          }
-#endif
+     }
+#endif // !MYNEWT_VAL(MP_RUNTIME_ALLOC)
 #endif
         rc = os_mempool_init(&ble_att_svr_prep_entry_pool,
                              MYNEWT_VAL(BLE_ATT_SVR_MAX_PREP_ENTRIES),
@@ -3655,6 +3732,7 @@ ble_att_svr_init(void)
             nimble_platform_mem_free(ble_att_svr_prep_entry_mem);
             ble_att_svr_prep_entry_mem = NULL;
 #endif
+            os_mempool_unregister(&ble_att_svr_prep_entry_pool);
             memset(&ble_att_svr_prep_entry_pool, 0,
                    sizeof(ble_att_svr_prep_entry_pool));
 #endif
@@ -3681,8 +3759,11 @@ int ble_att_get_database_size(int *out_size)
     ble_uuid_any_t service_uuid;
     uint8_t att_error;
     int size = 0;
-    int rc;
+    int rc = 0;
+    uint8_t val[20];
+    uint16_t attr_len;
 
+    ble_hs_lock();
     for (entry = STAILQ_FIRST(&ble_att_svr_list);
          entry != NULL;
          entry = STAILQ_NEXT(entry, ha_next)) {
@@ -3713,19 +3794,23 @@ int ble_att_get_database_size(int *out_size)
             size += (4 + (service_uuid.u.type == BLE_UUID_TYPE_16 ? 6 : 4));
         }
         else if(uuid->value == BLE_ATT_UUID_CHARACTERISTIC) {
+            /* handle(2 bytes) + type(2 bytes) + properties(1 byte)
+                      + val_handle(2 bytes) + uuid */
+            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);
 
             /* uuid is stored in the value attribute */
             struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next);
-            if (next_entry == NULL) {
+            if (next_entry != NULL) {
-                size += 4; // Updated
-                continue;
-            }
-            /* handle(2 bytes) + type(2 bytes) + properties(1 byte)
-                      + val_handle(2 bytes) + uuid */
-            size += (7 + ble_uuid_length(next_entry->ha_uuid));
                 /* Advance entry to skip the value attribute which was just accounted for */
                 entry = next_entry;
             }
+        }
         else if(uuid->value == 0x2901 ||
                 uuid->value == 0x2902 ||
                 uuid->value == 0x2903 ||
@@ -3740,7 +3825,9 @@ int ble_att_get_database_size(int *out_size)
         }
     }
     *out_size = size;
-    return 0;
+
+    ble_hs_unlock();
+    return rc;
 }
 
 int ble_att_fill_database_info(uint8_t *out_data)
@@ -3755,6 +3842,7 @@ int ble_att_fill_database_info(uint8_t *out_data)
     int rc;
 
     data = out_data;
+    ble_hs_lock();
     for (entry = STAILQ_FIRST(&ble_att_svr_list);
          entry != NULL;
          entry = STAILQ_NEXT(entry, ha_next)) {
@@ -3807,6 +3895,11 @@ int ble_att_fill_database_info(uint8_t *out_data)
             }
             memcpy(data + 4, val, attr_len);
             data += (4 + attr_len);
+
+            struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next);
+            if (next_entry != NULL) {
+                entry = next_entry;
+            }
         }
         else if(uuid->value == 0x2901 ||
                 uuid->value == 0x2902 ||
@@ -3834,6 +3927,7 @@ int ble_att_fill_database_info(uint8_t *out_data)
             data += (4 + attr_len);
         }
     }
+    ble_hs_unlock();
     return 0;
 }
 #endif
@@ -3852,18 +3946,18 @@ ble_att_svr_security_mode_1_level()
     uint8_t sec_level;
     uint8_t flags;
 
+    ble_hs_lock();
     for (entry = STAILQ_FIRST(&ble_att_svr_list);
          entry != NULL;
          entry = STAILQ_NEXT(entry, ha_next)) {
 
         flags = entry->ha_flags;
         if ((flags & BLE_ATT_F_READ_AUTHEN) || (flags & BLE_ATT_F_WRITE_AUTHEN)) {
+            if (ble_hs_cfg.sm_sc_only) {
+                sec_level = 0x04; //Authenticated LE Secure Connections
+            } else {
                 sec_level = 0x03; //Authenticated pairing with encryption
-            /* This is the highest currently supported value.
+            }
-             * Break here.
-             */
-            highest_security_level = 0x03;
-            break;
         } else if ((flags & BLE_ATT_F_READ_ENC) || (flags & BLE_ATT_F_WRITE_ENC)) {
             sec_level = 0x02; //Unauthenticated pairing with encryption
         } else {
@@ -3873,7 +3967,12 @@ ble_att_svr_security_mode_1_level()
         if (sec_level > highest_security_level) {
             highest_security_level = sec_level;
         }
+
+        if (highest_security_level == 0x04) {
+            break;
         }
+    }
+    ble_hs_unlock();
 
     return highest_security_level;
 }

nimble/host/src/ble_gap.c

@@ -2259,6 +2259,20 @@ ble_gap_rx_adv_report_sanity_check(const uint8_t *adv_data, uint8_t adv_data_len
 }
 #endif
 
+#if MYNEWT_VAL(BLE_ISO) || MYNEWT_VAL(BLE_EXT_ADV)
+static void
+ble_gap_slave_get_cb(uint8_t instance,
+                     ble_gap_event_fn **out_cb, void **out_cb_arg)
+{
+    ble_hs_lock();
+
+    *out_cb = ble_gap_slave[instance].cb;
+    *out_cb_arg = ble_gap_slave[instance].cb_arg;
+
+    ble_hs_unlock();
+}
+#endif
+
 #if MYNEWT_VAL(BLE_ISO)
 void
 ble_gap_rx_cis_disconn(const struct ble_hci_ev_disconn_cmp *ev)
@@ -2367,18 +2381,6 @@ ble_gap_rx_create_big_comp(const struct ble_hci_ev_le_subev_create_big_complete
     }
 }
 
-static void
-ble_gap_slave_get_cb(uint8_t instance,
-                     ble_gap_event_fn **out_cb, void **out_cb_arg)
-{
-    ble_hs_lock();
-
-    *out_cb = ble_gap_slave[instance].cb;
-    *out_cb_arg = ble_gap_slave[instance].cb_arg;
-
-    ble_hs_unlock();
-}
-
 void
 ble_gap_rx_term_big_comp(const struct ble_hci_ev_le_subev_terminate_big_complete *ev)
 {
@@ -2664,18 +2666,6 @@ ble_gap_rx_adv_set_terminated(const struct ble_hci_ev_le_subev_adv_set_terminate
 #endif
 }
 
-static void
-ble_gap_slave_get_cb(uint8_t instance,
-                     ble_gap_event_fn **out_cb, void **out_cb_arg)
-{
-    ble_hs_lock();
-
-    *out_cb = ble_gap_slave[instance].cb;
-    *out_cb_arg = ble_gap_slave[instance].cb_arg;
-
-    ble_hs_unlock();
-}
-
 void
 ble_gap_rx_scan_req_rcvd(const struct ble_hci_ev_le_subev_scan_req_rcvd *ev)
 {
@@ -4635,7 +4625,8 @@ ble_gap_adv_set_data(const uint8_t *data, int data_len)
     struct os_mbuf *mbuf;
     int rc;
 
-    if (((data == NULL) && (data_len != 0)) ||
+    if (data_len < 0 ||
+        ((data == NULL) && (data_len != 0)) ||
         (data_len > BLE_HCI_MAX_ADV_DATA_LEN)) {
         return BLE_HS_EINVAL;
     }
@@ -4650,15 +4641,17 @@ ble_gap_adv_set_data(const uint8_t *data, int data_len)
         return BLE_HS_ENOMEM;
     }
 
+    if (data_len > 0) {
         rc = os_mbuf_append(mbuf, data, data_len);
         if (rc) {
             os_mbuf_free_chain(mbuf);
             return BLE_HS_ENOMEM;
         }
+    }
 
     return ble_gap_ext_adv_set_data(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE), mbuf);
 #else
-    struct ble_hci_le_set_adv_data_cp cmd;
+    struct ble_hci_le_set_adv_data_cp cmd = {0};
     uint16_t opcode;
 
     STATS_INC(ble_gap_stats, adv_set_data);
@@ -4668,7 +4661,8 @@ ble_gap_adv_set_data(const uint8_t *data, int data_len)
     }
 
     /* Check for valid parameters */
-    if (((data == NULL) && (data_len != 0)) ||
+    if (data_len < 0 ||
+        ((data == NULL) && (data_len != 0)) ||
             (data_len > BLE_HCI_MAX_ADV_DATA_LEN)) {
         return BLE_HS_EINVAL;
     }
@@ -4695,8 +4689,9 @@ ble_gap_adv_rsp_set_data(const uint8_t *data, int data_len)
     struct os_mbuf *mbuf;
     int rc;
 
-    if (((data == NULL) && (data_len != 0)) ||
+    if (data_len < 0 ||
-        (data_len > BLE_HCI_MAX_ADV_DATA_LEN)) {
+        ((data == NULL) && (data_len != 0)) ||
+        (data_len > BLE_HCI_MAX_SCAN_RSP_DATA_LEN)) {
         return BLE_HS_EINVAL;
     }
 
@@ -4710,15 +4705,17 @@ ble_gap_adv_rsp_set_data(const uint8_t *data, int data_len)
         return BLE_HS_ENOMEM;
     }
 
+    if (data_len > 0) {
         rc = os_mbuf_append(mbuf, data, data_len);
         if (rc) {
             os_mbuf_free_chain(mbuf);
             return BLE_HS_ENOMEM;
         }
+    }
 
     return ble_gap_ext_adv_rsp_set_data(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE), mbuf);
 #else
-    struct ble_hci_le_set_scan_rsp_data_cp cmd;
+    struct ble_hci_le_set_scan_rsp_data_cp cmd = {0};
     uint16_t opcode;
 
     if (!ble_hs_is_enabled()) {
@@ -4726,13 +4723,16 @@ ble_gap_adv_rsp_set_data(const uint8_t *data, int data_len)
     }
 
     /* Check for valid parameters */
-    if (((data == NULL) && (data_len != 0)) ||
+    if (data_len < 0 ||
+        ((data == NULL) && (data_len != 0)) ||
             (data_len > BLE_HCI_MAX_SCAN_RSP_DATA_LEN)) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
         return BLE_HS_EINVAL;
     }
 
+    if (data_len > 0) {
         memcpy(cmd.scan_rsp, data, data_len);
+    }
     cmd.scan_rsp_len = data_len;
 
     opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA);
@@ -8629,7 +8629,7 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr,
 
         if (rl != NULL && rl->rl_isrpa) {
             memcpy(bhc_peer_addr.val, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
-            bhc_peer_addr.type = rl->rl_addr_type;
+            bhc_peer_addr.type = BLE_ADDR_RANDOM;
         }
     }
 #endif

nimble/host/src/ble_gattc.c

@@ -1125,6 +1125,10 @@ ble_gattc_proc_matches_expired(struct ble_gattc_proc *proc, void *arg)
 
     criteria = arg;
 
+    if (proc->flags & BLE_GATTC_PROC_F_STALLED) {
+        return 0;
+    }
+
     time_diff = proc->exp_os_ticks - criteria->now;
 
     if (time_diff <= 0) {
@@ -1418,7 +1422,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);
 
@@ -1587,10 +1593,18 @@ ble_gattc_recover_gatt_proc(uint16_t conn_handle, int enc_status)
                         attrs[i].handle = proc->write_reliable.attrs[i].handle;
                         attrs[i].offset = 0;
                         attrs[i].om = os_mbuf_dup(proc->write_reliable.attrs[i].om);
+                        if (attrs[i].om == NULL) {
+                            /* Failed to duplicate. Free previously duplicated mbufs and abort. */
+                            for (int j = 0; j < i; j++) {
+                                os_mbuf_free_chain(attrs[j].om);
+                            }
+                            goto skip_recovery;
+                        }
                     }
                     ble_gattc_write_reliable(conn_handle, attrs,
                                              proc->write_reliable.num_attrs,
                                              proc->write_reliable.cb, proc->write_reliable.cb_arg);
+skip_recovery:
                     break;
 		}
             } else {
@@ -1964,9 +1978,11 @@ done:
     if (rc != 0) {
         STATS_INC(ble_gattc_stats, disc_all_svcs_fail);
 #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
     }
 
@@ -2787,9 +2803,11 @@ done:
     if (rc != 0) {
         STATS_INC(ble_gattc_stats, disc_all_chrs_fail);
 #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
     }
 
@@ -3274,11 +3292,6 @@ done:
 
     if (rc != 0) {
         STATS_INC(ble_gattc_stats, disc_all_dscs_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, false);
@@ -4220,9 +4233,9 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status,
         attr[i].offset = 0;
     }
 
-    if (status == 0) {
+        *om = os_mbuf_pullup(*om, 2);
-        for (i = 0; i < proc->read_mult.num_handles; i++) {
+        if (*om == NULL) {
-            if (OS_MBUF_PKTLEN(*om) < 2) {
+            status = BLE_HS_ENOMEM;
             break;
         }
 
@@ -4234,25 +4247,23 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status,
             attr_len = get_le16((*om)->om_data);
             os_mbuf_adj(*om, 2);
 
-            if (attr_len > BLE_ATT_ATTR_MAX_LEN) {
+        if (attr_len > BLE_ATT_ATTR_MAX_LEN || attr_len > OS_MBUF_PKTLEN(*om)) {
+            status = BLE_HS_EBADDATA;
             break;
         }
 
         attr[i].om = os_msys_get_pkthdr(attr_len, 0);
         if (!attr[i].om) {
-                /* this is OOM condition*/
             status = BLE_HS_ENOMEM;
             break;
         }
 
         rc = os_mbuf_appendfrom(attr[i].om, *om, 0, attr_len);
         if (rc) {
+            status = BLE_HS_ENOMEM;
             break;
         }
 
-            os_mbuf_adj(*om, attr_len);
-        }
-
         /* failed to correctly parse response,
          * cleanup any partial data and set status if not set already
          */
@@ -4262,12 +4273,6 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status,
                 attr[i].om = NULL;
             }
 
-            if (status == 0) {
-                status = BLE_HS_EBADDATA;
-            }
-        }
-    }
-
     proc->read_mult.cb_mult(proc->conn_handle,
                             ble_gattc_error(status, att_handle), &attr[0],
                             proc->read_mult.num_handles, proc->read_mult.cb_arg);
@@ -5377,7 +5382,7 @@ ble_gatts_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle,
             goto done;
         }
         rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE,
-                                     chr_val_handle, 0, txom, NULL);
+                                     chr_val_handle, 0, &txom, NULL);
         if (rc != 0) {
             /* Fatal error; application disallowed attribute read. */
             rc = BLE_HS_EAPP;
@@ -5469,6 +5474,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;
             if (rc != 0) {
                 goto done;
             }
@@ -5480,12 +5486,14 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle,
         if (OS_MBUF_PKTLEN(txom) + OS_MBUF_PKTLEN(tuples[i].value) > mtu && cur_chr_cnt < 2) {
             rc = ble_att_clt_tx_notify(conn_handle, tuples[i].handle,
                                        tuples[i].value);
+            tuples[i].value = NULL;
             if (rc != 0) {
                 goto done;
             }
             continue;
         } else if (OS_MBUF_PKTLEN(txom) + OS_MBUF_PKTLEN(tuples[i].value) > mtu) {
             rc = ble_att_clt_tx_multi_notify(conn_handle, txom);
+            txom = NULL;
             if (rc != 0) {
                 goto done;
             }
@@ -5507,6 +5515,7 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle,
 
         /* Value */
         os_mbuf_concat(txom, tuples[i].value);
+        tuples[i].value = NULL;
         cur_chr_cnt++;
         last_appended_idx = i;  /* Track the last appended index */
     }
@@ -5515,8 +5524,10 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle,
         /* Use the last appended index, not chr_count which may be out of bounds */
         rc = ble_att_clt_tx_notify(conn_handle, tuples[last_appended_idx].handle,
                                    tuples[last_appended_idx].value);
+        tuples[last_appended_idx].value = NULL;
     } else {
         rc = ble_att_clt_tx_multi_notify(conn_handle, txom);
+        txom = NULL;
     }
 
 done:
@@ -6156,7 +6167,7 @@ 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);
+                                     0, &txom, NULL);
         if (rc != 0) {
             /* Fatal error; application disallowed attribute read. */
             BLE_HS_DBG_ASSERT(0);

nimble/host/src/ble_gatts.c

@@ -607,7 +607,7 @@ ble_gatts_chr_inc_val_stat(uint8_t gatt_op)
  * @return                      true if the GATT service set can be modified;
  *                              false otherwise.
  */
-static bool
+bool
 ble_gatts_mutable(void)
 {
     /* Ensure no active GAP procedures. */
@@ -1062,7 +1062,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)
@@ -1107,7 +1107,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;
@@ -1117,15 +1117,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);
+        *om = os_mbuf_pullup(*om, 2);
-        BLE_HS_DBG_ASSERT(om != NULL);
+        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;
@@ -1209,7 +1212,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);
     }
 
@@ -2650,6 +2653,8 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om)
     uint8_t feat[MYNEWT_VAL(BLE_GATT_CSFC_SIZE)] = {};
     uint16_t len;
     int rc = 0;
+    int bonded = 0;
+    ble_addr_t peer_addr = {0};
     int i;
 
     BLE_HS_LOG(DEBUG, "");
@@ -2696,27 +2701,28 @@ 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) {
+    bonded = conn->bhc_sec_state.bonded;
-        memset(&key_csfc, 0, sizeof key_csfc);
+    peer_addr = conn->bhc_peer_addr;
-        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;
-        }
-    }
 
 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) {
+            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;
 }
 
@@ -3241,15 +3247,19 @@ 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) {
@@ -3279,29 +3289,35 @@ ble_gatts_conn_unaware(struct ble_hs_conn *conn, void *arg) {
 }
 #endif
 
-/* takes two arguments
+struct ble_gatts_conn_clt_cfg_arg {
-arg[0] : added/removed
+    uint16_t action;
-arg[1] : affected chr_val_handle
+    uint16_t chr_val_handle;
-arg[2] : allowed_flags
-*/
-static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *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) {
+    int rc;
-    case 1:
+};
+
+static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) {
+    struct ble_gatts_conn_clt_cfg_arg *clt_cfg_arg;
+    uint16_t allowed_flags;
+
+    clt_cfg_arg = arg;
+
+    switch (clt_cfg_arg->action) {
+    case CONN_CLT_CFG_ADD:
         /* added */
-        allowed_flags = ((uint16_t *) arg)[2];
+        allowed_flags = clt_cfg_arg->allowed_flags;
-        ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle,
+        clt_cfg_arg->rc = ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs,
-                                allowed_flags, 0);
+                                clt_cfg_arg->chr_val_handle, allowed_flags, 0);
+        if (clt_cfg_arg->rc != 0) {
+            return clt_cfg_arg->rc;
+        }
         (conn->bhc_gatt_svr.num_clt_cfgs)++;
         return 0;
-    case 2:
+    case CONN_CLT_CFG_REMOVE:
         /* removed */
-        rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs,
+        clt_cfg_arg->rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs,
-                                chr_val_handle);
+                                clt_cfg_arg->chr_val_handle);
-        if (rc == 0) {
+        if (clt_cfg_arg->rc == 0) {
             (conn->bhc_gatt_svr.num_clt_cfgs)--;
         }
         return 0;
@@ -3330,7 +3346,7 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) {
     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_conn_clt_cfg_arg arg;
     uint16_t start_handle, end_handle;
 
     p = nimble_platform_mem_calloc(1,sizeof *ble_gatts_svc_defs);
@@ -3363,12 +3379,20 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) {
         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);
+            rc = ble_gatts_add_clt_cfg(&ble_gatts_clt_cfgs, ha->ha_handle_id + 1, allowed_flags, 0);
+            if (rc != 0) {
+                goto done;
+            }
             /* update connections */
-            arg[0] = CONN_CLT_CFG_ADD;
+            arg.action = CONN_CLT_CFG_ADD;
-            arg[1] = ha->ha_handle_id + 1;
+            arg.chr_val_handle = ha->ha_handle_id + 1;
-            arg[2] = allowed_flags;
+            arg.allowed_flags = allowed_flags;
-            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg);
+            arg.rc = 0;
+            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &arg);
+            if (arg.rc != 0) {
+                rc = arg.rc;
+                goto done;
+            }
         }
     }
     i = 0;
@@ -3445,7 +3469,7 @@ 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_arg 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;
@@ -3473,9 +3497,11 @@ 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.action = CONN_CLT_CFG_REMOVE;
-            arg[1] = chr_val_handle;
+            arg.chr_val_handle = chr_val_handle;
-            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg);
+            arg.allowed_flags = 0;
+            arg.rc = 0;
+            ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &arg);
         }
     }
     /* keep the start handle and end handle before deleting the service */
@@ -3498,9 +3524,15 @@ done:
 #endif
 
         /* send service change indication */
-        ble_svc_gatt_changed(start_handle, end_handle);
+        // ble_svc_gatt_changed(start_handle, end_handle);
     }
     ble_hs_unlock();
+
+    if (rc == 0) {
+        /* send service change indication */
+        ble_svc_gatt_changed(start_handle, end_handle);
+    }
+
     return rc;
 }
 #endif

nimble/host/src/ble_hs.c

@@ -588,7 +588,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);
@@ -735,7 +734,7 @@ ble_hs_enqueue_hci_event(uint8_t *hci_evt)
 
     ev = os_memblock_get(&ble_hs_hci_ev_pool);
 
-    if (ev && ble_hs_evq->eventq) {
+    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);
@@ -979,7 +978,8 @@ ble_hs_init(void)
     ble_npl_event_init(&ble_hs_ev_start_stage2, ble_hs_event_start_stage2,
                        NULL);
 
-    ble_hs_hci_init();
+    rc = ble_hs_hci_init();
+    SYSINIT_PANIC_ASSERT(rc == 0);
 
 #if NIMBLE_BLE_CONNECT
     rc = ble_hs_conn_init();

nimble/host/src/ble_hs_hci.c

@@ -1026,7 +1026,7 @@ ble_hs_hci_get_hci_supported_cmd(void)
     return l_ble_hs_hci_sup_cmd;
 }
 
-void
+int
 ble_hs_hci_init(void)
 {
     int rc;
@@ -1036,8 +1036,7 @@ ble_hs_hci_init(void)
     if (!ble_hs_hci_ctx) {
         ble_hs_hci_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_hci_ctx));
         if (!ble_hs_hci_ctx) {
-            BLE_HS_DBG_ASSERT_EVAL(0);
+            return BLE_HS_ENOMEM;
-            return;
         }
     }
 
@@ -1048,8 +1047,7 @@ ble_hs_hci_init(void)
         if (!ble_hs_hci_frag_data) {
             nimble_platform_mem_free(ble_hs_hci_ctx);
             ble_hs_hci_ctx = NULL;
-            BLE_HS_DBG_ASSERT_EVAL(0);
+            return BLE_HS_ENOMEM;
-            return;
         }
     }
 #endif
@@ -1068,6 +1066,8 @@ ble_hs_hci_init(void)
                             "ble_hs_hci_frag");
 
     BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    return 0;
 }
 
 void ble_hs_hci_deinit(void)

nimble/host/src/ble_hs_hci_evt.c

@@ -23,6 +23,7 @@
 #include "os/os.h"
 #include "nimble/hci_common.h"
 #include "host/ble_gap.h"
+#include "host/ble_esp_gap.h"
 #include "ble_hs_priv.h"
 #include "ble_hs_resolv_priv.h"
 #include "esp_nimble_mem.h"
@@ -444,7 +445,11 @@ static inline void
 ble_hs_hci_evt_resolve_rpa(ble_addr_t *addr)
 {
     struct ble_hs_resolv_entry *rl;
+    uint8_t id_addr[BLE_DEV_ADDR_LEN];
+    uint8_t id_addr_type;
+    uint8_t ota_addr_type;
 
+    ota_addr_type = addr->type;
     ble_hs_lock();
     rl = ble_hs_resolv_rpa_addr(addr->val, addr->type);
     if (rl != NULL) {
@@ -453,9 +458,24 @@ ble_hs_hci_evt_resolve_rpa(ble_addr_t *addr)
         }
 
         memcpy(addr->val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN);
-        addr->type = rl->rl_addr_type;
     }
     ble_hs_unlock();
+
+    if (rl == NULL && ble_gap_rpa_resolve(addr->val, id_addr, &id_addr_type)) {
+        if (ble_hs_is_rpa(addr->val, ota_addr_type)) {
+            ble_hs_lock();
+            rl = ble_hs_resolv_list_find(id_addr);
+            if (rl != NULL) {
+                memcpy(rl->rl_peer_rpa, addr->val, BLE_DEV_ADDR_LEN);
+                rl->rl_isrpa = 1;
+            }
+            ble_hs_unlock();
+        }
+
+        memcpy(addr->val, id_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    addr->type = ota_addr_type;
 }
 #endif
 

nimble/host/src/ble_hs_hci_priv.h

@@ -89,7 +89,7 @@ extern uint16_t ble_hs_hci_avail_pkts;
 int ble_hs_hci_cmd_tx_no_rsp(uint16_t opcode, const void *cmd, uint8_t cmd_len);
 int ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len,
                       void *rsp, uint8_t rsp_len);
-void ble_hs_hci_init(void);
+int ble_hs_hci_init(void);
 void ble_hs_hci_deinit(void);
 
 void ble_hs_hci_set_le_supported_feat(uint64_t feat);

nimble/host/src/ble_hs_iso.c

@@ -50,6 +50,12 @@ static uint8_t ble_hs_iso_max_pkts;
 static uint16_t ble_hs_iso_avail_pkts;
 #endif /* MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL) */
 
+int
+ble_iso_init(void)
+{
+    return 0;
+}
+
 int
 ble_hs_hci_set_iso_buf_sz(uint16_t pktlen, uint8_t max_pkts)
 {

nimble/host/src/ble_hs_pvcy.c

@@ -52,7 +52,7 @@ uint8_t ble_hs_pvcy_default_irk[16];
 uint16_t l_rpa_timeout;
 #endif
 
-#define BLE_MAX_RPA_TIMEOUT_VAL 0xA1B8
+#define BLE_MAX_RPA_TIMEOUT_VAL 0x0E10
 
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
 void ble_store_config_init(void);
@@ -80,7 +80,8 @@ 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;
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
     }
 
     cmd.rpa_timeout = htole16(timeout);
@@ -93,25 +94,39 @@ ble_hs_pvcy_set_addr_timeout(uint16_t timeout)
 
 int ble_hs_set_rpa_timeout(uint16_t timeout)
 {
+    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
-    l_rpa_timeout = timeout;
 
-    return ble_hs_pvcy_set_addr_timeout(l_rpa_timeout);
+    rc = ble_hs_pvcy_set_addr_timeout(timeout);
+    if (rc == 0) {
+        ble_hs_lock();
+        l_rpa_timeout = timeout;
+        ble_hs_unlock();
+    }
+
+    return rc;
 }
 
 uint16_t ble_hs_get_rpa_timeout(void)
 {
+    uint16_t tmo;
+
 #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
     if (ble_hs_pvcy_ctx == NULL) {
         return 0;
     }
 #endif
-    return l_rpa_timeout;
+    ble_hs_lock();
+    tmo = l_rpa_timeout;
+    ble_hs_unlock();
+
+    return tmo;
 }
 
 void ble_hs_reset_rpa_timeout(void)
@@ -152,15 +167,19 @@ ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr)
 
     cmd.peer_addr_type = addr_type;
     memcpy(cmd.peer_id_addr, addr, BLE_DEV_ADDR_LEN);
-#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+
+    ble_gap_preempt();
     ble_hs_lock();
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
     rc = ble_hs_resolv_list_rmv(addr_type, &cmd.peer_id_addr[0]);
-    ble_hs_unlock();
 #else
     rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
                                       BLE_HCI_OCF_LE_RMV_RESOLV_LIST),
                            &cmd, sizeof(cmd), NULL, 0);
 #endif
+    ble_hs_unlock();
+    ble_gap_preempt_done();
+
     return rc;
 }
 
@@ -168,9 +187,17 @@ ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr)
 static int
 ble_hs_pvcy_clear_entries(void)
 {
-    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+    int rc;
+
+    ble_gap_preempt();
+    ble_hs_lock();
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
                                       BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
                            NULL, 0, NULL, 0);
+    ble_hs_unlock();
+    ble_gap_preempt_done();
+
+    return rc;
 }
 #endif
 
@@ -190,7 +217,9 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type,
 
     cmd.peer_addr_type = addr_type;
     memcpy(cmd.peer_id_addr, addr, 6);
+    ble_hs_lock();
     memcpy(cmd.local_irk, ble_hs_pvcy_irk, 16);
+    ble_hs_unlock();
     memcpy(cmd.peer_irk, irk, 16);
 
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
@@ -220,6 +249,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
@@ -271,7 +301,10 @@ ble_hs_pvcy_ensure_started(void)
     }
 #endif
 
+    ble_hs_lock();
+
     if (ble_hs_pvcy_started) {
+        ble_hs_unlock();
         return 0;
     }
 
@@ -291,11 +324,14 @@ ble_hs_pvcy_ensure_started(void)
     }
 
     if (rc != 0) {
+        ble_hs_unlock();
         return rc;
     }
 
     ble_hs_pvcy_started = 1;
 
+    ble_hs_unlock();
+
     return 0;
 }
 
@@ -401,7 +437,9 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk)
         memcpy(new_irk, ble_hs_pvcy_default_irk, 16);
     }
 
+    ble_hs_lock();
     memcpy(ble_hs_pvcy_irk, new_irk, 16);
+    ble_hs_unlock();
 
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
     if (irk != NULL) {
@@ -480,6 +518,9 @@ ble_hs_pvcy_our_irk(const uint8_t **out_irk)
 int
 ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode)
 {
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    return 0;
+#else
     struct ble_hci_le_set_privacy_mode_cp cmd;
 
     if (addr == NULL) {
@@ -487,17 +528,19 @@ ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode)
         return BLE_HS_EINVAL;
     }
 
-    if (addr->type > BLE_ADDR_RANDOM) {
+    if (addr->type > BLE_ADDR_RANDOM_ID) {
-        return BLE_ERR_INV_HCI_CMD_PARMS;
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
+        return BLE_HS_EINVAL;
     }
 
     cmd.mode = priv_mode;
-    cmd.peer_id_addr_type = addr->type;
+    cmd.peer_id_addr_type = addr->type & 0x01;
     memcpy(cmd.peer_id_addr, addr->val, BLE_DEV_ADDR_LEN);
 
     return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
                                         BLE_HCI_OCF_LE_SET_PRIVACY_MODE),
                              &cmd, sizeof(cmd), NULL, 0);
+#endif
 }
 
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
@@ -517,10 +560,15 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
 {
     int rc = 0;
 
+#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    ble_gap_preempt();
+#endif
+    ble_hs_lock();
+
     if (enable != NIMBLE_HOST_DISABLE_PRIVACY) {
         rc = ble_hs_pvcy_ensure_started();
         if (rc != 0) {
-            return rc;
+            goto done;
         }
 
         ble_hs_resolv_enable(true);
@@ -538,6 +586,12 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
         ble_hs_resolv_enable(false);
     }
 
+done:
+    ble_hs_unlock();
+#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    ble_gap_preempt_done();
+#endif
+
     return rc;
 }
 #endif

nimble/host/src/ble_hs_resolv.c

@@ -35,7 +35,7 @@
 
 /* Resolve list size, additional space to save local device's configuration */
 #define BLE_RESOLV_LIST_SIZE    (MYNEWT_VAL(BLE_STORE_MAX_BONDS) + 1)
-#define BLE_MAX_RPA_TIMEOUT_VAL 0xA1B8
+#define BLE_MAX_RPA_TIMEOUT_VAL 0x0E10
 
 struct ble_hs_resolv_data {
     uint8_t addr_res_enabled;
@@ -245,6 +245,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);
@@ -855,25 +856,15 @@ ble_hs_resolv_rpa(uint8_t *rpa, uint8_t *irk)
         return BLE_HS_EINVAL;
     }
 
-    /* IRK is already in little-endian format; ble_sm_alg_encrypt will handle byte order */
     memcpy(ecb.key, irk, 16);
-    memset(ecb.plain_text, 0, 16);
+    memcpy(ecb.plain_text, rpa + 3, 3);
 
-    ecb.plain_text[15] = rpa[3];
-    ecb.plain_text[14] = rpa[4];
-    ecb.plain_text[13] = rpa[5];
-
-    swap_in_place(ecb.plain_text, 16);
-
-    /* Send the data to ble_sm_alg_encrypt in little-endian style */
     rc = ble_sm_alg_encrypt(ecb.key, ecb.plain_text, ecb.cipher_text);
     if (rc != 0) {
         return rc;
     }
-    swap_in_place(ecb.cipher_text, 16);
 
-    if ((ecb.cipher_text[15] == rpa[0]) && (ecb.cipher_text[14] == rpa[1]) &&
+    if (memcmp(ecb.cipher_text, rpa, 3) == 0) {
-            (ecb.cipher_text[13] == rpa[2])) {
         rc = 0;
     } else {
         rc = BLE_HS_ENOENT;

nimble/host/src/ble_l2cap_sig.c

@@ -91,7 +91,7 @@ struct ble_l2cap_sig_proc {
             struct ble_l2cap_chan *chan[BLE_L2CAP_MAX_COC_CONN_REQ];
         } connect;
         struct {
-            struct ble_l2cap_chan *chan;
+            uint16_t scid;
         } disconnect;
 #if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
         struct {
@@ -211,9 +211,11 @@ ble_l2cap_sig_dbg_assert_proc_not_inserted(struct ble_l2cap_sig_proc *proc)
 #if MYNEWT_VAL(BLE_HS_DEBUG)
     struct ble_l2cap_sig_proc *cur;
 
+    ble_hs_lock();
     STAILQ_FOREACH(cur, &ble_l2cap_sig_procs, next) {
         BLE_HS_DBG_ASSERT(cur != proc);
     }
+    ble_hs_unlock();
 #endif
 }
 
@@ -224,13 +226,18 @@ ble_l2cap_sig_dbg_assert_proc_not_inserted(struct ble_l2cap_sig_proc *proc)
 static uint8_t
 ble_l2cap_sig_next_id(void)
 {
+    uint8_t id;
+
+    ble_hs_lock();
     ble_l2cap_sig_cur_id++;
     if (ble_l2cap_sig_cur_id == 0) {
         /* An ID of 0 is illegal. */
         ble_l2cap_sig_cur_id = 1;
     }
+    id = ble_l2cap_sig_cur_id;
+    ble_hs_unlock();
 
-    return ble_l2cap_sig_cur_id;
+    return id;
 }
 
 static ble_l2cap_sig_rx_fn *
@@ -300,11 +307,11 @@ ble_l2cap_sig_proc_matches(struct ble_l2cap_sig_proc *proc,
         return 0;
     }
 
-    if (op != proc->op) {
+    if (op != 0xff && op != proc->op) {
         return 0;
     }
 
-    if (id != 0 && id != proc->id) {
+    if (id != proc->id) {
         return 0;
     }
 
@@ -634,6 +641,19 @@ ble_l2cap_sig_update_nolock(uint16_t conn_handle,
         return BLE_HS_EINVAL;
     }
 
+    /* Bluetooth Spec Core 6.0, Vol 6, Part B, Section 4.5.2:
+     * - itvl_min <= itvl_max
+     * - timeout >= (1 + slave_latency) * itvl_max * 2 * 1.25
+     *   (timeout unit: 10ms, itvl unit: 1.25ms)
+     *   => timeout * 10 > (1 + slave_latency) * itvl_max * 1.25 * 2
+     *   => timeout * 4 > (1 + slave_latency) * itvl_max
+     */
+    if (params->itvl_min > params->itvl_max ||
+        params->timeout_multiplier * 4 <= (1 + params->slave_latency) * params->itvl_max) {
+
+        return BLE_HS_EINVAL;
+    }
+
     proc = ble_l2cap_sig_proc_alloc();
     if (proc == NULL) {
         STATS_INC(ble_l2cap_stats, update_fail);
@@ -1027,7 +1047,7 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle,
     struct ble_l2cap_chan *chans[BLE_L2CAP_MAX_COC_CONN_REQ] = { 0 };
     struct ble_hs_conn *conn;
     uint16_t scid;
-    uint16_t result;
+//    uint16_t result;
     unsigned int num_of_scids;
     int i;
     uint8_t len;
@@ -1169,7 +1189,7 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle,
     }
 
     ble_hs_unlock();
-    result = rsp->result;
+    //result = rsp->result;
     rc = ble_l2cap_sig_tx(conn_handle, txom);
     if (rc != 0) {
         /* Notify application of failure first, then clean up */
@@ -1179,6 +1199,7 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle,
                 ble_hs_lock();
                 conn = ble_hs_conn_find(conn_handle);
                 if (conn) {
+                    chans[i]->cb = NULL;
                     ble_hs_conn_delete_chan(conn, chans[i]);
                 }
                 ble_hs_unlock();
@@ -1191,13 +1212,14 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle,
     /* Notify user about connection status */
     for (i = 0; i < num_of_scids; i++) {
         if (chans[i]) {
-            if (result == 0) {
+            if (rsp->dcids[i] != 0) {
                 ble_l2cap_event_coc_connected(chans[i], 0);
             } else {
                 ble_l2cap_event_coc_connected(chans[i], BLE_HS_EUNKNOWN);
                 ble_hs_lock();
                 conn = ble_hs_conn_find(conn_handle);
                 if (conn) {
+                    chans[i]->cb = NULL;
                     ble_hs_conn_delete_chan(conn, chans[i]);
                 }
                 ble_hs_unlock();
@@ -1255,14 +1277,15 @@ 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) &&
+        if ((result != BLE_L2CAP_COC_ERR_NO_RESOURCES) &&
-            (rsp->result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) &&
+            (result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) &&
-            (rsp->result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) {
+            (result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) {
             goto done;
         }
     }
@@ -1576,7 +1599,7 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m
     struct os_mbuf *txom;
     struct ble_l2cap_sig_credit_base_connect_req *req;
     int rc;
-    int i, j;
+    int i;
 
     if (!sdu_rx || !cb) {
         BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
@@ -1594,7 +1617,6 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m
     }
 
     if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) {
-        ble_hs_unlock();
         ble_l2cap_sig_proc_free(proc);
         return BLE_HS_EINVAL;
     }
@@ -1616,13 +1638,6 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m
         if (!proc->connect.chan[i]) {
             /* Clear request buffer */
             os_mbuf_free_chain(txom);
-
-            for (j = 0; j < i; j++) {
-                /* Clear callback to make sure "Disconnected event" to the user */
-                proc->connect.chan[j]->cb = NULL;
-                ble_l2cap_chan_free(conn, proc->connect.chan[j]);
-            }
-            ble_hs_unlock();
             rc = BLE_HS_ENOMEM;
             goto failed;
         }
@@ -1641,7 +1656,6 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m
 
     rc = ble_l2cap_sig_tx_nolock(proc->conn_handle, txom);
     if (rc) {
-        rc = BLE_HS_ENOMEM;
         goto failed;
     }
 
@@ -1652,12 +1666,15 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m
 failed:
     /* clean up on failure, ble_l2cap_chan_free() handles NULL as well */
     for (i = 0; i < num; i++) {
+        if (proc->connect.chan[i]) {
             proc->connect.chan[i]->cb = NULL;
             ble_l2cap_chan_free(conn, proc->connect.chan[i]);
+            proc->connect.chan[i] = NULL;
+        }
     }
 
     ble_l2cap_sig_proc_free(proc);
-    return BLE_HS_ENOMEM;
+    return rc;
 }
 
 int
@@ -1671,19 +1688,23 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
     int rc;
     int i;
 
+    ble_hs_lock();
+
     conn = ble_hs_conn_find(conn_handle);
     if (!conn) {
+        ble_hs_unlock();
         return BLE_HS_ENOTCONN;
     }
 
     proc = ble_l2cap_sig_proc_alloc();
     if (!proc) {
+        ble_hs_unlock();
         return BLE_HS_ENOMEM;
     }
 
     if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) {
-        ble_hs_unlock();
         ble_l2cap_sig_proc_free(proc);
+        ble_hs_unlock();
         return BLE_HS_EINVAL;
     }
 
@@ -1692,6 +1713,7 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
             proc->reconfig.cids[i] = chans[i]->scid;
         } else {
             ble_l2cap_sig_proc_free(proc);
+            ble_hs_unlock();
             return BLE_HS_ENOMEM;
         }
     }
@@ -1707,6 +1729,7 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
                                 sizeof(*req) + num * sizeof(uint16_t), &txom);
     if (!req) {
         ble_l2cap_sig_proc_free(proc);
+        ble_hs_unlock();
         return BLE_HS_ENOMEM;
     }
 
@@ -1721,10 +1744,14 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
     rc = ble_l2cap_sig_tx_nolock(proc->conn_handle, txom);
     if (rc) {
         ble_l2cap_sig_proc_free(proc);
+        ble_hs_unlock();
         return rc;
     }
 
     ble_l2cap_sig_proc_start(proc);
+
+    ble_hs_unlock();
+
     return 0;
 }
 #endif
@@ -1790,7 +1817,7 @@ 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, scid, dcid);
         return 0;
     }
 
@@ -1825,17 +1852,13 @@ ble_l2cap_sig_coc_disconnect_cb(struct ble_l2cap_sig_proc *proc, int status)
 
     assert(proc);
 
-    chan = proc->disconnect.chan;
-    if (!chan) {
-        return;
-    }
-
     ble_hs_lock();
-    conn = ble_hs_conn_find_assert(chan->conn_handle);
+    conn = ble_hs_conn_find(proc->conn_handle);
     if (conn) {
+        chan = ble_hs_conn_chan_find_by_scid(conn, proc->disconnect.scid);
+        if (chan) {
             ble_hs_conn_delete_chan(conn, chan);
-    } else {
+        }
-        ble_l2cap_chan_free(NULL, chan);
     }
     ble_hs_unlock();
 }
@@ -1848,6 +1871,7 @@ ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
     struct ble_l2cap_sig_proc *proc;
     struct ble_l2cap_chan *chan;
     int rc;
+    struct ble_hs_conn *conn;
 
     proc = ble_l2cap_sig_proc_extract(conn_handle,
                                       BLE_L2CAP_SIG_PROC_OP_DISCONNECT,
@@ -1868,17 +1892,27 @@ ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
         goto done;
     }
 
-    chan = proc->disconnect.chan;
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (!conn) {
+        ble_hs_unlock();
+        goto done;
+    }
+
+    chan = ble_hs_conn_chan_find_by_scid(conn, proc->disconnect.scid);
     if (!chan) {
+        ble_hs_unlock();
         goto done;
     }
 
     rsp = (struct ble_l2cap_sig_disc_rsp *)(*om)->om_data;
     if (chan->dcid != le16toh(rsp->dcid) || chan->scid != le16toh(rsp->scid)) {
         /* This response is incorrect, lets wait for timeout */
+        ble_hs_unlock();
         ble_l2cap_sig_proc_start(proc);
         return 0;
     }
+    ble_hs_unlock();
 
     ble_l2cap_sig_coc_disconnect_cb(proc, rc);
 
@@ -1925,7 +1959,7 @@ ble_l2cap_sig_disconnect_nolock(struct ble_l2cap_chan *chan)
     proc->op = BLE_L2CAP_SIG_PROC_OP_DISCONNECT;
     proc->id = ble_l2cap_sig_next_id();
     proc->conn_handle = chan->conn_handle;
-    proc->disconnect.chan = chan;
+    proc->disconnect.scid = chan->scid;
 
     req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_DISCONN_REQ, proc->id,
                                 sizeof(*req), &txom);
@@ -2020,7 +2054,7 @@ ble_l2cap_sig_rx_reject(uint16_t conn_handle,
 {
     struct ble_l2cap_sig_proc *proc;
     proc = ble_l2cap_sig_proc_extract(conn_handle,
-                                         BLE_L2CAP_SIG_PROC_OP_CONNECT,
+                                         0xff,
                                          hdr->identifier);
    if (!proc) {
        return 0;
@@ -2031,7 +2065,18 @@ ble_l2cap_sig_rx_reject(uint16_t conn_handle,
    case BLE_L2CAP_SIG_PROC_OP_CONNECT:
        ble_l2cap_sig_coc_connect_cb(proc, BLE_HS_EREJECT);
        break;
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+   case BLE_L2CAP_SIG_PROC_OP_RECONFIG:
+       ble_l2cap_sig_coc_reconfig_cb(proc, BLE_HS_EREJECT);
+       break;
 #endif
+   case BLE_L2CAP_SIG_PROC_OP_DISCONNECT:
+       ble_l2cap_sig_coc_disconnect_cb(proc, BLE_HS_EREJECT);
+       break;
+#endif
+   case BLE_L2CAP_SIG_PROC_OP_UPDATE:
+       ble_l2cap_sig_update_call_cb(proc, BLE_HS_EREJECT);
+       break;
    default:
        break;
    }
@@ -2136,10 +2181,9 @@ ble_l2cap_sig_extract_expired(struct ble_l2cap_sig_proc_list *dst_list)
 
     ble_hs_lock();
 
-    next = NULL;
+    prev = NULL;
     proc = STAILQ_FIRST(&ble_l2cap_sig_procs);
     while (proc != NULL) {
-        prev = next;
         next = STAILQ_NEXT(proc, next);
 
         time_diff = proc->exp_os_ticks - now;
@@ -2155,6 +2199,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;
@@ -2248,6 +2293,11 @@ ble_l2cap_sig_timer(void)
         case BLE_L2CAP_SIG_PROC_OP_CONNECT:
             ble_l2cap_sig_coc_connect_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
         case BLE_L2CAP_SIG_PROC_OP_DISCONNECT:
             ble_l2cap_sig_coc_disconnect_cb(proc, BLE_HS_ETIMEOUT);
             break;
@@ -2319,7 +2369,14 @@ ble_l2cap_sig_init(void)
 void
 ble_l2cap_sig_deinit(void)
 {
+    struct ble_l2cap_sig_proc *proc;
+
     if (ble_l2cap_sig_ctx) {
+        while ((proc = STAILQ_FIRST(&ble_l2cap_sig_procs)) != NULL) {
+            STAILQ_REMOVE_HEAD(&ble_l2cap_sig_procs, next);
+            ble_l2cap_sig_proc_free(proc);
+        }
+
 #if !MYNEWT_VAL(MP_RUNTIME_ALLOC)
         if (ble_l2cap_sig_proc_mem) {
             nimble_platform_mem_free(ble_l2cap_sig_proc_mem);

nimble/host/src/ble_sm.c

@@ -711,6 +711,7 @@ ble_sm_persist_keys(struct ble_sm_proc *proc)
                             &value_sec);
     ble_store_write_peer_sec(&value_sec);
 
+    if (ble_addr_cmp(&peer_rpa_addr, BLE_ADDR_ANY) != 0) {
         value_rpa_rec.peer_addr.type = peer_addr.type;
         memcpy(value_rpa_rec.peer_addr.val, peer_addr.val, sizeof peer_addr.val);
 
@@ -718,6 +719,7 @@ ble_sm_persist_keys(struct ble_sm_proc *proc)
         memcpy(value_rpa_rec.peer_rpa_addr.val, peer_rpa_addr.val, sizeof peer_rpa_addr.val);
 
         ble_store_write_rpa_rec(&value_rpa_rec);
+    }
 }
 
 static int
@@ -861,10 +863,20 @@ ble_sm_rx_noop(uint16_t conn_handle, struct os_mbuf **om,
 static uint8_t
 ble_sm_build_authreq(void)
 {
-    return ble_hs_cfg.sm_bonding << 0  |
+    uint8_t authreq;
-           ble_hs_cfg.sm_mitm << 2     |
+
+    authreq = ble_hs_cfg.sm_bonding << 0  |
               ble_hs_cfg.sm_sc << 3       |
               ble_hs_cfg.sm_keypress << 4;
+
+    if (ble_hs_cfg.sm_mitm &&
+        (ble_hs_cfg.sm_io_cap != BLE_HS_IO_NO_INPUT_OUTPUT ||
+         ble_hs_cfg.sm_oob_data_flag)) {
+
+        authreq |= 1 << 2;
+    }
+
+    return authreq;
 }
 
 static int
@@ -986,6 +998,13 @@ ble_sm_read_bond(uint16_t conn_handle, struct ble_store_value_sec *out_bond)
     key_sec.peer_addr = desc.peer_id_addr;
 
     rc = ble_store_read_peer_sec(&key_sec, out_bond);
+    if (rc == 0) {
+#if MYNEWT_VAL(BLE_SM_SC) == 0
+        if (out_bond->sc) {
+            return BLE_HS_ENOTSUP;
+        }
+#endif
+    }
     return rc;
 }
 
@@ -1018,7 +1037,10 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle,
         /* If the peer isn't bonded, indicate that the pairing procedure should
          * continue.
          */
+        ble_hs_lock();
         rc = ble_sm_read_bond(conn_handle, &bond);
+        ble_hs_unlock();
+
         switch (rc) {
         case 0:
             break;
@@ -1435,20 +1457,23 @@ ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type,
 {
     struct ble_store_key_sec key_sec;
     int rc;
+    int i;
 
     /* Tell application to look up LTK by peer address and ediv/rand pair. */
     memset(&key_sec, 0, sizeof key_sec);
     key_sec.peer_addr.type = peer_addr_type;
     memcpy(key_sec.peer_addr.val, peer_addr, 6);
 
+    for (i = 0; ; i++) {
+        key_sec.idx = i;
         rc = ble_store_read_our_sec(&key_sec, value_sec);
         if (rc != 0) {
             return rc;
         }
-    if (value_sec->ediv != ediv || value_sec->rand_num != rand) {
+        if (value_sec->ediv == ediv && value_sec->rand_num == rand) {
-        return BLE_HS_ENOENT;
+            return 0;
+        }
     }
-    return rc;
 }
 
 static int
@@ -1509,6 +1534,7 @@ ble_sm_ltk_start_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
     if (res->app_status == 0) {
         proc->state = BLE_SM_PROC_STATE_ENC_START;
     } else {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
     }
 }
@@ -1535,6 +1561,7 @@ ble_sm_ltk_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
             }
         } else {
             /* Notify the app if it provided a key and the procedure failed. */
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
             res->enc_cb = 1;
         }
     } else {
@@ -1545,6 +1572,7 @@ ble_sm_ltk_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
         rc = ble_sm_ltk_req_neg_reply_tx(proc->conn_handle);
         if (rc != 0) {
             res->app_status = rc;
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
             res->enc_cb = 1;  /* Notify application of failure, similar to reply branch */
         } else {
             res->app_status = 0;
@@ -1899,6 +1927,10 @@ ble_sm_pair_req_fill(struct ble_sm_proc *proc)
     ble_sm_pair_base_fill(req);
     req->init_key_dist = ble_hs_cfg.sm_our_key_dist;
     req->resp_key_dist = ble_hs_cfg.sm_their_key_dist;
+    if (!(req->authreq & BLE_SM_PAIR_AUTHREQ_BOND)) {
+        req->init_key_dist = 0;
+        req->resp_key_dist = 0;
+    }
 }
 
 static void
@@ -1920,6 +1952,10 @@ ble_sm_pair_rsp_fill(struct ble_sm_proc *proc)
                          ble_hs_cfg.sm_their_key_dist;
     rsp->resp_key_dist = req->resp_key_dist &
                          ble_hs_cfg.sm_our_key_dist;
+    if (!(rsp->authreq & BLE_SM_PAIR_AUTHREQ_BOND)) {
+        rsp->init_key_dist = 0;
+        rsp->resp_key_dist = 0;
+    }
 }
 
 static void
@@ -1986,21 +2022,33 @@ err:
 static bool
 ble_sm_verify_auth_requirements(uint8_t cmd)
 {
-    /* For now we check only SC only mode. I.e.: when remote indicates
+    /* If SC-Only mode or Security Level 4 is enabled, the SC bit must be set.
-     * to not support SC pairing, let us make sure legacy pairing is supported
+     */
-     * on our side. If not, we can fail right away.
+    if (ble_hs_cfg.sm_sc_only || ble_hs_cfg.sm_sec_lvl == 4) {
+        if (!(cmd & BLE_SM_PAIR_AUTHREQ_SC)) {
+            return false;
+        }
+    } else {
+        /* When remote indicates to not support SC pairing, let us make sure
+         * legacy pairing is supported on our side. If not, we can fail right
+         * away.
          */
         if (!(cmd & BLE_SM_PAIR_AUTHREQ_SC)) {
             if (MYNEWT_VAL(BLE_SM_LEGACY) == 0) {
                 return false;
             }
         }
+    }
+
     /* Fail if security level forces MITM protection and remote does not
-     * support it
+     * support it. SC-Only mode also requires MITM.
      */
-    if (ble_hs_cfg.sm_sec_lvl >= 3 && !(cmd & BLE_SM_PAIR_AUTHREQ_MITM)) {
+    if ((ble_hs_cfg.sm_sc_only || ble_hs_cfg.sm_sec_lvl >= 3) &&
+        !(cmd & BLE_SM_PAIR_AUTHREQ_MITM)) {
+
         return false;
     }
+
     return true;
 }
 
@@ -2069,8 +2117,20 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
 
     ble_hs_lock();
 
+    if (ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL) !=
+        NULL) {
+        ble_hs_unlock();
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_UNSPECIFIED);
+        res->out_of_order = 1;
+        return;
+    }
+
     proc = ble_sm_proc_alloc();
-    if (proc != NULL) {
+    if (proc == NULL) {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        res->app_status = BLE_HS_ENOMEM;
+    } else {
         proc->conn_handle = conn_handle;
         proc->state = BLE_SM_PROC_STATE_PAIR;
         ble_sm_insert(proc);
@@ -2091,14 +2151,25 @@ 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_MAX) {
             res->sm_err = BLE_SM_ERR_INVAL;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
+        } else if (!(req->authreq & BLE_SM_PAIR_AUTHREQ_BOND) &&
+                   (req->init_key_dist != 0 || req->resp_key_dist != 0)) {
+            /* If the Bonding_Flags is set to No Bonding, the Initiator Key
+             * Distribution and Responder Key Distribution fields shall be set
+             * to zero.
+             */
+            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
              */
             res->sm_err = BLE_SM_ERR_AUTHREQ;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ);
             res->enc_cb = 1;
-        } else if (ble_hs_cfg.sm_sc_only && (req->max_enc_key_size != BLE_SM_PAIR_KEY_SZ_MAX)) {
+        } else if ((ble_hs_cfg.sm_sc_only || ble_hs_cfg.sm_sec_lvl == 4) &&
-            /* Fail if Secure Connections Only mode is on and key size is too small
+                   (req->max_enc_key_size != BLE_SM_PAIR_KEY_SZ_MAX)) {
+            /* Fail if Secure Connections Only mode or Security Level 4 is on
+             * and remote does not meet key size requirements.
              */
             res->sm_err = BLE_SM_ERR_ENC_KEY_SZ;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ);
@@ -2120,9 +2191,6 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om,
             key_size = proc->key_size;
             res->execute = 1;
         }
-    } else {
-        res->app_status = BLE_HS_ENOMEM;
-        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
     }
 
     ble_hs_unlock();
@@ -2169,15 +2237,31 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om,
         } else if (rsp->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) {
             res->sm_err = BLE_SM_ERR_INVAL;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
-        } else if (ble_hs_cfg.sm_sc_only && (rsp->max_enc_key_size != BLE_SM_PAIR_KEY_SZ_MAX)) {
+        } else if (!(rsp->authreq & BLE_SM_PAIR_AUTHREQ_BOND) &&
-            /* Fail if Secure Connections Only mode is on and remote does not meet
+                   (rsp->init_key_dist != 0 || rsp->resp_key_dist != 0)) {
-            * key size requirements - MITM was checked in last step
+            /* If the Bonding_Flags is set to No Bonding, the Initiator Key
+             * Distribution and Responder Key Distribution fields shall be set
+             * to zero.
+             */
+            res->sm_err = BLE_SM_ERR_INVAL;
+            res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
+        } else if ((ble_hs_cfg.sm_sc_only || ble_hs_cfg.sm_sec_lvl == 4) &&
+                   (rsp->max_enc_key_size != BLE_SM_PAIR_KEY_SZ_MAX)) {
+            /* Fail if Secure Connections Only mode or Security Level 4 is on
+             * and remote does not meet key size requirements.
              */
             res->sm_err = BLE_SM_ERR_ENC_KEY_SZ;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ);
         } else if (!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);
+        } else {
+            struct ble_sm_pair_cmd *req = (struct ble_sm_pair_cmd *)(proc->pair_req + 1);
+            if ((rsp->init_key_dist & ~req->init_key_dist) != 0 ||
+                (rsp->resp_key_dist & ~req->resp_key_dist) != 0) {
+
+                res->sm_err = BLE_SM_ERR_INVAL;
+                res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL);
             } else {
                 ble_sm_pair_cfg(proc);
 
@@ -2196,6 +2280,7 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om,
                     }
                 }
             }
+        }
     } else {
         /**
          * Unexpectedly received pairing response.
@@ -2642,6 +2727,11 @@ ble_sm_enc_info_rx(uint16_t conn_handle, struct os_mbuf **om,
         res->app_status = BLE_HS_ENOENT;
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->out_of_order = 1;
+    } else if (!(proc->rx_key_flags & BLE_SM_KE_F_ENC_INFO) ||
+               (proc->flags & BLE_SM_PROC_F_SC)) {
+
+        res->sm_err = BLE_SM_ERR_KEY_REJ;
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_KEY_REJ);
     } else {
         proc->rx_key_flags &= ~BLE_SM_KE_F_ENC_INFO;
         proc->peer_keys.ltk_valid = 1;
@@ -2725,6 +2815,9 @@ ble_sm_id_info_rx(uint16_t conn_handle, struct os_mbuf **om,
         res->app_status = BLE_HS_ENOENT;
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->out_of_order = 1;
+    } else if (!(proc->rx_key_flags & BLE_SM_KE_F_ID_INFO)) {
+        res->sm_err = BLE_SM_ERR_KEY_REJ;
+        res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_KEY_REJ);
     } else {
         proc->rx_key_flags &= ~BLE_SM_KE_F_ID_INFO;
 
@@ -2753,6 +2846,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 > 1) {
+        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);
@@ -2836,9 +2936,15 @@ ble_sm_fail_rx(uint16_t conn_handle, struct os_mbuf **om,
     if (res->app_status == 0) {
         cmd = (struct ble_sm_pair_fail *)(*om)->om_data;
 
-        res->app_status = BLE_HS_SM_PEER_ERR(cmd->reason);
+        if (cmd->reason == 0) {
+            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+        } else {
             res->sm_err = cmd->reason;
         }
+        res->app_status = BLE_HS_SM_PEER_ERR(res->sm_err);
+    } else {
+        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
+    }
 }
 
 /*****************************************************************************
@@ -3144,7 +3250,9 @@ ble_sm_enc_initiate(uint16_t conn_handle, uint8_t key_size,
 
     ble_hs_unlock();
 
+    if (proc != NULL) {
         ble_sm_process_result(conn_handle, &res, true);
+    }
 
     return res.app_status;
 }
@@ -3251,6 +3359,8 @@ ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey)
         case BLE_SM_IOACT_NUMCMP:
             if (!pkey->numcmp_accept) {
                 res.sm_err = BLE_SM_ERR_NUMCMP;
+                res.app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_NUMCMP);
+                res.enc_cb = 1;
             } else {
                 proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
                 if (proc->flags & BLE_SM_PROC_F_INITIATOR ||
@@ -3269,8 +3379,15 @@ ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey)
                 res.sm_err = BLE_SM_ERR_OOB;
             } else {
                 proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
-                proc->oob_data_local = pkey->oob_sc_data.local;
+
-                proc->oob_data_remote = pkey->oob_sc_data.remote;
+                if (pkey->oob_sc_data.local != NULL) {
+                    proc->oob_data_local_s = *pkey->oob_sc_data.local;
+                    proc->oob_data_local = &proc->oob_data_local_s;
+                }
+                if (pkey->oob_sc_data.remote != NULL) {
+                    proc->oob_data_remote_s = *pkey->oob_sc_data.remote;
+                    proc->oob_data_remote = &proc->oob_data_remote_s;
+                }
 
                 /* Execute Confirm step */
                 ble_sm_sc_oob_confirm(proc, &res);
@@ -3333,6 +3450,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) {
@@ -3465,6 +3586,7 @@ ble_sm_rx(struct ble_l2cap_chan *chan, struct os_mbuf **om)
 
     handle = ble_l2cap_get_conn_handle(chan);
     if (handle == BLE_HS_CONN_HANDLE_NONE) {
+        BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOTCONN);
         return BLE_HS_ENOTCONN;
     }
 
@@ -3507,10 +3629,7 @@ int
 ble_sm_configure_static_passkey(uint32_t passkey, bool enable)
 {
     /* ble_hs_cfg is configuration state.  This API is intended for setup before
-     * starting pairing; applications that change it while the host is active
+     * starting pairing.
-     * must serialize those changes with their own pairing flow.  Do not add
-     * ble_hs_lock() here: callers may use this during host init, and host
-     * locking is not a recursive configuration lock.
      */
     if (enable) {
         /* Validate passkey is 6 digits */
@@ -3518,14 +3637,21 @@ ble_sm_configure_static_passkey(uint32_t passkey, bool enable)
             BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL);
             return BLE_HS_EINVAL;
         }
+
+        ble_hs_lock();
         /* Passkey authentication requires MITM; ensure it is enabled. */
         ble_hs_cfg.sm_mitm = 1;
         ble_hs_cfg.sm_static_passkey = 1;
         ble_hs_cfg.sm_static_passkey_val = passkey;
+        ble_hs_unlock();
+
         BLE_HS_LOG(DEBUG, "static passkey enabled\n");
     } else {
+        ble_hs_lock();
         ble_hs_cfg.sm_static_passkey = 0;
         ble_hs_cfg.sm_static_passkey_val = 0;
+        ble_hs_unlock();
+
         BLE_HS_LOG(INFO, "static passkey disabled\n");
     }
 
@@ -3540,8 +3666,10 @@ ble_sm_get_static_passkey_config(uint32_t *passkey, bool *enabled)
         return BLE_HS_EINVAL;
     }
 
+    ble_hs_lock();
     *enabled = ble_hs_cfg.sm_static_passkey;
     *passkey = ble_hs_cfg.sm_static_passkey_val;
+    ble_hs_unlock();
 
     return 0;
 }
@@ -3554,6 +3682,9 @@ ble_sm_csis_decrypt_sirk(const uint8_t *ltk, const uint8_t *enc_sirk, uint8_t *o
     int rc;
 
     /* Decrypt SIRK with sdf(K, EncSIRK)  */
+    /* XXX: ble_sm_alg_csis_sdf is currently not implemented in NimBLE.
+     * This will result in a linker error if this function is called.
+     */
     rc = ble_sm_alg_csis_sdf(ltk, enc_sirk, out);
 
     return rc;

nimble/host/src/ble_sm_priv.h

@@ -273,6 +273,8 @@ struct ble_sm_proc {
     struct ble_sm_public_key pub_key_peer;
     uint8_t mackey[16];
     uint8_t dhkey[32];
+    struct ble_sm_sc_oob_data oob_data_local_s;
+    struct ble_sm_sc_oob_data oob_data_remote_s;
     const struct ble_sm_sc_oob_data *oob_data_local;
     const struct ble_sm_sc_oob_data *oob_data_remote;
 #endif

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

@@ -211,62 +211,80 @@ static int
 ble_store_config_conf_export(void (*func)(char *name, char *val),
                              enum conf_export_tgt tgt)
 {
-    union {
+    char *buf;
-        char sec[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ];
+    int buf_sz;
-        char cccd[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ];
+
-        char rpa_rec[BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ];
+    /* Get the largest possible encoding size */
+    buf_sz = BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ;
+    if (BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ > buf_sz) {
+        buf_sz = BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ;
+    }
+    if (BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ > buf_sz) {
+        buf_sz = BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ;
+    }
 #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS)
-        char csfc[BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ];
+    if (BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ > buf_sz) {
+        buf_sz = BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ;
+    }
 #endif
 #if MYNEWT_VAL(ENC_ADV_DATA)
-        char ead[BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ];
+    if (BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ > buf_sz) {
+        buf_sz = BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ;
+    }
 #endif
-    } buf;
+
+    buf = nimble_platform_mem_malloc(buf_sz);
+    if (!buf) {
+        return BLE_HS_ENOMEM;
+    }
 
     ble_store_config_serialize_arr(ble_store_config_our_secs,
                                    sizeof *ble_store_config_our_secs,
                                    ble_store_config_num_our_secs,
-                                   buf.sec,
+                                   buf,
-                                   sizeof buf.sec);
+                                   buf_sz);
-    func("ble_hs/our_sec", buf.sec);
+    func("ble_hs/our_sec", buf);
 
     ble_store_config_serialize_arr(ble_store_config_peer_secs,
                                    sizeof *ble_store_config_peer_secs,
                                    ble_store_config_num_peer_secs,
-                                   buf.sec,
+                                   buf,
-                                   sizeof buf.sec);
+                                   buf_sz);
-    func("ble_hs/peer_sec", buf.sec);
+    func("ble_hs/peer_sec", buf);
 
     ble_store_config_serialize_arr(ble_store_config_cccds,
                                    sizeof *ble_store_config_cccds,
                                    ble_store_config_num_cccds,
-                                   buf.cccd,
+                                   buf,
-                                   sizeof buf.cccd);
+                                   buf_sz);
-    func("ble_hs/cccd", buf.cccd);
+    func("ble_hs/cccd", buf);
 #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS)
     ble_store_config_serialize_arr(ble_store_config_csfcs,
                                    sizeof *ble_store_config_csfcs,
                                    ble_store_config_num_csfcs,
-                                   buf.csfc,
+                                   buf,
-                                   sizeof buf.csfc);
+                                   buf_sz);
-    func("ble_hs/csfc", buf.csfc);
+    func("ble_hs/csfc", buf);
 #endif
 #if MYNEWT_VAL(ENC_ADV_DATA)
     ble_store_config_serialize_arr(ble_store_config_eads,
                                    sizeof *ble_store_config_eads,
                                    ble_store_config_num_eads,
-                                   buf.ead,
+                                   buf,
-                                   sizeof buf.ead);
+                                   buf_sz);
-    func("ble_hs/ead", buf.ead);
+    func("ble_hs/ead", buf);
 #endif
 #if MYNEWT_VAL(BLE_STORE_MAX_BONDS)
     ble_store_config_serialize_arr(ble_store_config_rpa_recs,
                                    sizeof *ble_store_config_rpa_recs,
                                    ble_store_config_num_rpa_recs,
-                                   buf.rpa_rec,
+                                   buf,
-                                   sizeof buf.rpa_rec);
+                                   buf_sz);
-    func("ble_hs/rpa_rec", buf.rpa_rec);
+    func("ble_hs/rpa_rec", buf);
 #endif
+
+    nimble_platform_mem_free(buf);
+
     return 0;
 }
 
@@ -275,20 +293,20 @@ ble_store_config_persist_sec_set(const char *setting_name,
                                  const struct ble_store_value_sec *secs,
                                  int num_secs)
 {
-    /*
+    char *buf;
-     * NOTE: Large stack allocation based on BLE_STORE_MAX_BONDS.
-     * For typical ESP-IDF configurations (BLE_STORE_MAX_BONDS=10), usage is ~1.5KB
-     * which is acceptable. Users configuring larger bond counts should verify adequate
-     * NimBLE host task stack size. Dynamic allocation adds complexity and
-     * potential failure paths during critical bonding operations.
-     */
-    char buf[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ];
     int rc;
 
+    buf = nimble_platform_mem_malloc(BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
     ble_store_config_serialize_arr(secs, sizeof *secs, num_secs,
-                                   buf, sizeof buf);
+                                   buf, BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ);
-    /* NOTE: RAM-NVS consistency pattern - see system-wide TODO above */
+
     rc = conf_save_one(setting_name, buf);
+    nimble_platform_mem_free(buf);
+
     if (rc != 0) {
         return BLE_HS_ESTORE_FAIL;
     }
@@ -330,16 +348,23 @@ ble_store_config_persist_peer_secs(void)
 int
 ble_store_config_persist_cccds(void)
 {
-    char buf[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ];
+    char *buf;
     int rc;
 
+    buf = nimble_platform_mem_malloc(BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
     ble_store_config_serialize_arr(ble_store_config_cccds,
                                    sizeof *ble_store_config_cccds,
                                    ble_store_config_num_cccds,
                                    buf,
-                                   sizeof buf);
+                                   BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ);
     /* NOTE: RAM-NVS consistency pattern - see system-wide TODO above */
     rc = conf_save_one("ble_hs/cccd", buf);
+    nimble_platform_mem_free(buf);
+
     if (rc != 0) {
         return BLE_HS_ESTORE_FAIL;
     }
@@ -351,15 +376,22 @@ ble_store_config_persist_cccds(void)
 int
 ble_store_config_persist_csfcs(void)
 {
-    char buf[BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ];
+    char *buf;
     int rc;
 
+    buf = nimble_platform_mem_malloc(BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
     ble_store_config_serialize_arr(ble_store_config_csfcs,
                                    sizeof *ble_store_config_csfcs,
                                    ble_store_config_num_csfcs,
                                    buf,
-                                   sizeof buf);
+                                   BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ);
     rc = conf_save_one("ble_hs/csfc", buf);
+    nimble_platform_mem_free(buf);
+
     if (rc != 0) {
         return BLE_HS_ESTORE_FAIL;
     }
@@ -376,13 +408,19 @@ ble_store_config_persist_eads(void)
      * This design prevents concurrent store operations and maintains data consistency.
      * While this blocks the BLE host thread briefly, it ensures atomic store operations
      * and prevents race conditions between concurrent persist/restore operations. */
-    char buf[BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ];
+    char *buf;
     int rc;
+
+    buf = nimble_platform_mem_malloc(BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
     ble_store_config_serialize_arr(ble_store_config_eads,
                                    sizeof *ble_store_config_eads,
                                    ble_store_config_num_eads,
                                    buf,
-                                   sizeof buf);
+                                   BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ);
     /*
      * NOTE: Flash I/O while holding BLE host lock is intentional design.
      * ESP-IDF's conf_save_one (NVS operations) are typically fast enough (<10ms)
@@ -390,6 +428,8 @@ ble_store_config_persist_eads(void)
      * persistence is critical for data consistency.
      */
     rc = conf_save_one("ble_hs/ead", buf);
+    nimble_platform_mem_free(buf);
+
     if (rc != 0) {
         return BLE_HS_ESTORE_FAIL;
     }
@@ -401,18 +441,27 @@ ble_store_config_persist_eads(void)
 int
 ble_store_config_persist_rpa_recs(void)
 {
-    char buf[BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ];
+    char *buf;
     int rc;
+
+    buf = nimble_platform_mem_malloc(BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ);
+    if (buf == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
     ble_store_config_serialize_arr(ble_store_config_rpa_recs,
                                    sizeof *ble_store_config_rpa_recs,
                                    ble_store_config_num_rpa_recs,
                                    buf,
-                                   sizeof buf);
+                                   BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ);
     /* NOTE: RAM-NVS consistency pattern - see system-wide TODO above */
     rc = conf_save_one("ble_hs/rpa_rec", buf);
+    nimble_platform_mem_free(buf);
+
     if (rc != 0) {
         return BLE_HS_ESTORE_FAIL;
     }
+
     return 0;
 }
 #endif

nimble/transport/common/hci_h4/src/hci_h4.c

@@ -235,21 +235,12 @@ hci_h4_sm_w4_payload(struct hci_h4_sm *h4sm,
 static void
 hci_h4_sm_completed(struct hci_h4_sm *h4sm)
 {
-    int rc;
-
     switch (h4sm->pkt_type) {
     case HCI_H4_CMD:
     case HCI_H4_EVT:
         if (h4sm->buf) {
             assert(h4sm->frame_cb);
-            rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->buf);
+            h4sm->frame_cb(h4sm->pkt_type, h4sm->buf);
-            if (rc != 0) {
-#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
-                ble_transport_free(h4sm->pkt_type, h4sm->buf);
-#else
-                ble_transport_free(h4sm->buf);
-#endif
-            }
             h4sm->buf = NULL;
         }
         break;
@@ -257,10 +248,7 @@ hci_h4_sm_completed(struct hci_h4_sm *h4sm)
     case HCI_H4_ISO:
         if (h4sm->om) {
             assert(h4sm->frame_cb);
-            rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->om);
+            h4sm->frame_cb(h4sm->pkt_type, h4sm->om);
-            if (rc != 0) {
-                os_mbuf_free_chain(h4sm->om);
-            }
             h4sm->om = NULL;
         }
         break;

nimble/transport/src/monitor.c

@@ -42,14 +42,12 @@
 #include <nimble/nimble_npl.h>
 #include "monitor_priv.h"
 
-struct ble_npl_mutex lock;
-
 #if MYNEWT_VAL(BLE_MONITOR_UART)
 struct uart_dev *uart;
 
 static uint8_t tx_ringbuf[MYNEWT_VAL(BLE_MONITOR_UART_BUFFER_SIZE)];
-static uint8_t tx_ringbuf_head;
+static volatile int tx_ringbuf_head;
-static uint8_t tx_ringbuf_tail;
+static volatile int tx_ringbuf_tail;
 #endif
 
 #if MYNEWT_VAL(BLE_MONITOR_RTT)
@@ -103,15 +101,15 @@ monitor_uart_queue_char(uint8_t ch)
 
     OS_ENTER_CRITICAL(sr);
 
-    /* We need to try flush some data from ringbuffer if full */
+    /* If buffer is full, we must discard data to avoid hanging the system,
-    while (inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf)) ==
+     * especially when called from ISR or critical section.
-            tx_ringbuf_tail) {
+     */
-        uart_start_tx(uart);
+    if (inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf)) == tx_ringbuf_tail) {
+#if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
+        rtt_drops.dropped = true;
+#endif
         OS_EXIT_CRITICAL(sr);
-        if (os_started()) {
+        return;
-            os_time_delay(1);
-        }
-        OS_ENTER_CRITICAL(sr);
     }
 
     tx_ringbuf[tx_ringbuf_head] = ch;
@@ -163,8 +161,8 @@ update_drop_counters(struct ble_monitor_hdr *failed_hdr)
 
     if (*cnt < UINT8_MAX) {
         (*cnt)++;
-        ble_npl_callout_reset(&rtt_drops.tmo, OS_TICKS_PER_SEC);
     }
+    ble_npl_callout_reset(&rtt_drops.tmo, OS_TICKS_PER_SEC);
 }
 
 static void
@@ -245,6 +243,9 @@ monitor_write_header(uint16_t opcode, uint16_t len)
      * btsnoop specification states that fields of extended header must be
      * sorted in increasing order so we will send drops (if any) headers before
      * timestamp header.
+     *
+     * NOTE: Issue 1173 (Endianness) is a FALSE POSITIVE. The code correctly
+     * uses htole16 and htole32 macros for all packet header fields.
      */
 
     monitor_write(&hdr, sizeof(hdr));
@@ -261,27 +262,13 @@ monitor_write_header(uint16_t opcode, uint16_t len)
     monitor_write(&ts_hdr, sizeof(ts_hdr));
 }
 
-#ifndef BABBLESIM
-static size_t
-btmon_write(FILE *instance, const char *bp, size_t n)
-{
-    monitor_write(bp, n);
-
-    return n;
-}
-
-static FILE *btmon = (FILE *) &(struct File) {
-    .vmt = &(struct File_methods) {
-        .write = btmon_write,
-    },
-};
-#endif
-
 #if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
 static void
 drops_tmp_cb(struct ble_npl_event *ev)
 {
-    ble_npl_mutex_pend(&lock, BLE_NPL_TIME_FOREVER);
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
 
     /*
      * There's no "nop" in btsnoop protocol so we just send empty system note
@@ -291,7 +278,7 @@ drops_tmp_cb(struct ble_npl_event *ev)
     monitor_write_header(BLE_MONITOR_OPCODE_SYSTEM_NOTE, 1);
     monitor_write("", 1);
 
-    ble_npl_mutex_release(&lock);
+    OS_EXIT_CRITICAL(sr);
 }
 #endif
 
@@ -341,9 +328,6 @@ ble_monitor_init(void)
     SYSINIT_PANIC_ASSERT(rtt_index >= 0);
 #endif
 
-    rc = ble_npl_mutex_init(&lock);
-    SYSINIT_PANIC_ASSERT(rc == 0);
-
 #if BLE_MONITOR
     ble_monitor_new_index(0, (uint8_t[6]){ }, "nimble0");
 #endif
@@ -355,18 +339,19 @@ ble_monitor_deinit(void)
 #if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
     ble_npl_callout_deinit(&rtt_drops.tmo);
 #endif
-    ble_npl_mutex_deinit(&lock);
 }
 
 int
 ble_monitor_send(uint16_t opcode, const void *data, size_t len)
 {
-    ble_npl_mutex_pend(&lock, BLE_NPL_TIME_FOREVER);
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
 
     monitor_write_header(opcode, len);
     monitor_write(data, len);
 
-    ble_npl_mutex_release(&lock);
+    OS_EXIT_CRITICAL(sr);
 
     return 0;
 }
@@ -376,6 +361,7 @@ ble_monitor_send_om(uint16_t opcode, const struct os_mbuf *om)
 {
     const struct os_mbuf *om_tmp;
     uint16_t length = 0;
+    os_sr_t sr;
 
     om_tmp = om;
     while (om_tmp) {
@@ -383,7 +369,7 @@ ble_monitor_send_om(uint16_t opcode, const struct os_mbuf *om)
         om_tmp = SLIST_NEXT(om_tmp, om_next);
     }
 
-    ble_npl_mutex_pend(&lock, BLE_NPL_TIME_FOREVER);
+    OS_ENTER_CRITICAL(sr);
 
     monitor_write_header(opcode, length);
 
@@ -392,7 +378,7 @@ ble_monitor_send_om(uint16_t opcode, const struct os_mbuf *om)
         om = SLIST_NEXT(om, om_next);
     }
 
-    ble_npl_mutex_release(&lock);
+    OS_EXIT_CRITICAL(sr);
 
     return 0;
 }
@@ -445,7 +431,7 @@ ble_monitor_log(int level, const char *fmt, ...)
 
     ulog.ident_len = sizeof(id);
 
-    ble_npl_mutex_pend(&lock, BLE_NPL_TIME_FOREVER);
+    ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
 
     monitor_write_header(BLE_MONITOR_OPCODE_USER_LOGGING,
                          sizeof(ulog) + sizeof(id) + len + 1);
@@ -466,9 +452,19 @@ ble_monitor_log(int level, const char *fmt, ...)
         free(tmp);
     } while (0);
 #else
+    char buf[128];
+    int len;
+
     va_start(va, fmt);
-    vfprintf(btmon, fmt, va);
+    len = vsnprintf(buf, sizeof(buf), fmt, va);
     va_end(va);
+
+    if (len > 0) {
+        if (len >= (int)sizeof(buf)) {
+            len = sizeof(buf) - 1;
+        }
+        monitor_write(buf, len);
+    }
 #endif
 
     /* null-terminate string */

porting/nimble/src/os_mbuf.c

@@ -82,6 +82,7 @@ os_mqueue_init(struct os_mqueue *mq, ble_npl_event_fn *ev_cb, void *arg)
 {
     struct ble_npl_event *ev;
 
+    memset(mq, 0, sizeof *mq);
     STAILQ_INIT(&mq->mq_head);
 
     ev = &mq->mq_ev;
@@ -234,6 +235,10 @@ os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len)
     struct os_mbuf *m;
     struct os_mbuf_pool *pool;
 
+    if (user_hdr_len > 0xFF00) {
+        goto err;
+    }
+
     total_pkthdr_len =  user_hdr_len + sizeof(struct os_mbuf_pkthdr);
     pool = _os_msys_find_pool(dsize + total_pkthdr_len);
     if (!pool) {
@@ -386,24 +391,22 @@ int
 os_mbuf_free_chain(struct os_mbuf *om)
 {
     struct os_mbuf *next;
-    int rc;
+    int rc = 0;
+    int tmp_rc;
 
     os_trace_api_u32(OS_TRACE_ID_MBUF_FREE_CHAIN, (uint32_t)(uintptr_t)om);
 
     while (om != NULL) {
         next = SLIST_NEXT(om, om_next);
 
-        rc = os_mbuf_free(om);
+        tmp_rc = os_mbuf_free(om);
-        if (rc != 0) {
+        if (tmp_rc != 0) {
-            goto done;
+            rc = tmp_rc;
         }
 
         om = next;
     }
 
-    rc = 0;
-
-done:
     os_trace_api_ret_u32(OS_TRACE_ID_MBUF_FREE_CHAIN, (uint32_t)rc);
     return (rc);
 }
@@ -424,6 +427,9 @@ _os_mbuf_copypkthdr(struct os_mbuf *new_buf, struct os_mbuf *old_buf)
            old_buf->om_pkthdr_len);
     new_buf->om_pkthdr_len = old_buf->om_pkthdr_len;
     new_buf->om_data = new_buf->om_databuf + old_buf->om_pkthdr_len;
+
+    /* Zero out the queue pointer to avoid stale links */
+    OS_MBUF_PKTHDR(new_buf)->omp_next.stqe_next = NULL;
 }
 
 uint16_t
@@ -594,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;
 
@@ -903,6 +913,7 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len)
     uint16_t cur_off;
     int copylen;
     int rc;
+    int total_len = off + len;
 
     /* Find the mbuf,offset pair for the start of the destination. */
     cur = os_mbuf_off(om, off, &cur_off);
@@ -924,7 +935,8 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len)
 
         if (len == 0) {
             /* All the source data fit in the existing mbuf chain. */
-            return 0;
+            rc = 0;
+            goto done;
         }
 
         next = SLIST_NEXT(cur, om_next);
@@ -942,13 +954,14 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len)
         return rc;
     }
 
+done:
     /* 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, (uint16_t)total_len);
     }
 
-    return 0;
+    return rc;
 }
 
 void

porting/nimble/src/os_msys_init.c

@@ -0,0 +1,339 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+#include "os/os.h"
+#include "mem/mem.h"
+#include "sysinit/sysinit.h"
+#include "esp_nimble_mem.h"
+#include "esp_err.h"
+
+static STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list =
+    STAILQ_HEAD_INITIALIZER(g_msys_pool_list);
+
+#if CONFIG_BT_NIMBLE_ENABLED
+#define OS_MSYS_1_BLOCK_COUNT MYNEWT_VAL(MSYS_1_BLOCK_COUNT)
+#define OS_MSYS_1_BLOCK_SIZE MYNEWT_VAL(MSYS_1_BLOCK_SIZE)
+#define OS_MSYS_2_BLOCK_COUNT MYNEWT_VAL(MSYS_2_BLOCK_COUNT)
+#define OS_MSYS_2_BLOCK_SIZE MYNEWT_VAL(MSYS_2_BLOCK_SIZE)
+#else
+#define OS_MSYS_1_BLOCK_COUNT CONFIG_BT_LE_MSYS_1_BLOCK_COUNT
+#define OS_MSYS_1_BLOCK_SIZE CONFIG_BT_LE_MSYS_1_BLOCK_SIZE
+#define OS_MSYS_2_BLOCK_COUNT CONFIG_BT_LE_MSYS_2_BLOCK_COUNT
+#define OS_MSYS_2_BLOCK_SIZE CONFIG_BT_LE_MSYS_2_BLOCK_SIZE
+#endif
+
+
+
+#if OS_MSYS_1_BLOCK_COUNT > 0
+#define SYSINIT_MSYS_1_MEMBLOCK_SIZE                \
+    OS_ALIGN(OS_MSYS_1_BLOCK_SIZE, 4)
+#define SYSINIT_MSYS_1_MEMPOOL_SIZE                 \
+    OS_MEMPOOL_SIZE(OS_MSYS_1_BLOCK_COUNT,  \
+                    SYSINIT_MSYS_1_MEMBLOCK_SIZE)
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+static os_membuf_t *os_msys_init_1_data;
+static struct os_mbuf_pool os_msys_init_1_mbuf_pool;
+static struct os_mempool os_msys_init_1_mempool;
+#endif // BLE_STATIC_TO_DYNAMIC
+#endif // OS_MSYS_1_BLOCK_COUNT
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+#define SYSINIT_MSYS_2_MEMBLOCK_SIZE                \
+    OS_ALIGN(OS_MSYS_2_BLOCK_SIZE, 4)
+#define SYSINIT_MSYS_2_MEMPOOL_SIZE                 \
+    OS_MEMPOOL_SIZE(OS_MSYS_2_BLOCK_COUNT,  \
+                    SYSINIT_MSYS_2_MEMBLOCK_SIZE)
+#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+static os_membuf_t *os_msys_init_2_data;
+static struct os_mbuf_pool os_msys_init_2_mbuf_pool;
+static struct os_mempool os_msys_init_2_mempool;
+#endif // BLE_STATIC_TO_DYNAMIC
+#endif // OS_MSYS_2_BLOCK_COUNT
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+/* Context structure holding all MSYS resources */
+typedef struct {
+#if OS_MSYS_1_BLOCK_COUNT > 0
+    os_membuf_t *init_1_data;
+    struct os_mbuf_pool init_1_mbuf_pool;
+    struct os_mempool   init_1_mempool;
+#endif
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+    os_membuf_t *init_2_data;
+    struct os_mbuf_pool init_2_mbuf_pool;
+    struct os_mempool   init_2_mempool;
+#endif
+} os_msys_ctx_t;
+
+static os_msys_ctx_t *os_msys_ctx = NULL;
+
+/* Macros for easier access */
+#if OS_MSYS_1_BLOCK_COUNT > 0
+#define os_msys_init_1_data       (os_msys_ctx->init_1_data)
+#define os_msys_init_1_mbuf_pool  (os_msys_ctx->init_1_mbuf_pool)
+#define os_msys_init_1_mempool    (os_msys_ctx->init_1_mempool)
+#endif // OS_MSYS_1_BLOCK_COUNT
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+#define os_msys_init_2_data       (os_msys_ctx->init_2_data)
+#define os_msys_init_2_mbuf_pool  (os_msys_ctx->init_2_mbuf_pool)
+#define os_msys_init_2_mempool    (os_msys_ctx->init_2_mempool)
+#endif // OS_MSYS_2_BLOCK_COUNT
+
+static int
+ble_os_msys_ensure_ctx(void)
+{
+    if(os_msys_ctx) {
+        return 0;
+    }
+
+    os_msys_ctx = nimble_platform_mem_calloc(1, sizeof(*os_msys_ctx));
+    if(!os_msys_ctx) {
+        return -1;
+    }
+
+    return 0;
+}
+
+#endif // BLE_STATIC_TO_DYNAMIC
+
+#define OS_MSYS_SANITY_ENABLED                  \
+    (MYNEWT_VAL(MSYS_1_SANITY_MIN_COUNT) > 0 || \
+     MYNEWT_VAL(MSYS_2_SANITY_MIN_COUNT) > 0)
+
+#if OS_MSYS_SANITY_ENABLED
+static struct os_sanity_check os_msys_sc;
+#endif
+
+#if OS_MSYS_SANITY_ENABLED
+
+/**
+ * Retrieves the minimum safe buffer count for an msys pool.  That is, the
+ * lowest a pool's buffer count can be without causing the sanity check to
+ * fail.
+ *
+ * @param idx                   The index of the msys pool to query.
+ *
+ * @return                      The msys pool's minimum safe buffer count.
+ */
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
+static int
+os_msys_sanity_min_count(int idx)
+{
+    switch (idx) {
+    case 0:
+        return MYNEWT_VAL(MSYS_1_SANITY_MIN_COUNT);
+
+    case 1:
+        return MYNEWT_VAL(MSYS_2_SANITY_MIN_COUNT);
+
+    default:
+        /* Additional pools have no minimum requirement */
+        return 0;
+    }
+}
+
+#if !MYNEWT_VAL(BLE_LOW_SPEED_MODE)
+IRAM_ATTR
+#endif
+static int
+os_msys_sanity(struct os_sanity_check *sc, void *arg)
+{
+    const struct os_mbuf_pool *omp;
+    int min_count;
+    int idx;
+
+    idx = 0;
+    STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) {
+        min_count = os_msys_sanity_min_count(idx);
+        if (omp->omp_pool->mp_num_free < min_count) {
+            return OS_ENOMEM;
+        }
+
+        idx++;
+    }
+
+    return ESP_OK;
+}
+#endif
+
+static void
+os_msys_init_once(void *data, struct os_mempool *mempool,
+                  struct os_mbuf_pool *mbuf_pool,
+                  int block_count, int block_size, const char *name)
+{
+    int rc;
+
+    rc = mem_init_mbuf_pool(data, mempool, mbuf_pool, block_count, block_size,
+                            name);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = os_msys_register(mbuf_pool);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
+
+int
+os_msys_buf_alloc(void)
+{
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (ble_os_msys_ensure_ctx()){
+        return ESP_FAIL;
+    }
+#endif
+
+#if MYNEWT_VAL(MP_RUNTIME_ALLOC)
+    return ESP_OK;
+#endif
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+#if OS_MSYS_1_BLOCK_COUNT > 0
+        if (!os_msys_ctx->init_1_data) {
+            os_msys_ctx->init_1_data = nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_1_MEMPOOL_SIZE));
+            if(!os_msys_ctx->init_1_data){
+                nimble_platform_mem_free(os_msys_ctx);
+                os_msys_ctx = NULL;
+                return ESP_FAIL;
+            }
+        }
+#endif
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+      if (!os_msys_ctx->init_2_data) {
+          os_msys_ctx->init_2_data = nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_2_MEMPOOL_SIZE));
+          if(!os_msys_ctx->init_2_data) {
+#if OS_MSYS_1_BLOCK_COUNT > 0
+              nimble_platform_mem_free(os_msys_ctx->init_1_data);
+              os_msys_ctx->init_1_data = NULL;
+#endif
+              nimble_platform_mem_free(os_msys_ctx);
+              os_msys_ctx = NULL;
+              return ESP_FAIL;
+          }
+      }
+
+#endif
+#else
+#if OS_MSYS_1_BLOCK_COUNT > 0
+    os_msys_init_1_data = (os_membuf_t *)nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_1_MEMPOOL_SIZE));
+    if (!os_msys_init_1_data) {
+        return ESP_FAIL;
+    }
+#endif
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+    os_msys_init_2_data = (os_membuf_t *)nimble_platform_mem_calloc(1, (sizeof(os_membuf_t) * SYSINIT_MSYS_2_MEMPOOL_SIZE));
+    if (!os_msys_init_2_data) {
+#if OS_MSYS_1_BLOCK_COUNT > 0
+       nimble_platform_mem_free(os_msys_init_1_data);
+       os_msys_init_1_data = NULL;
+#endif
+        return ESP_FAIL;
+    }
+#endif
+#endif // BLE_STATIC_TO_DYNAMIC
+    return ESP_OK;
+}
+
+void
+os_msys_buf_free(void)
+{
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    if (os_msys_ctx) {
+#if OS_MSYS_1_BLOCK_COUNT > 0
+        if (os_msys_ctx->init_1_data) {
+            nimble_platform_mem_free(os_msys_ctx->init_1_data);
+            os_msys_ctx->init_1_data = NULL;
+        }
+        os_mempool_unregister(&os_msys_ctx->init_1_mempool);
+#endif
+#if OS_MSYS_2_BLOCK_COUNT > 0
+        if (os_msys_ctx->init_2_data) {
+            nimble_platform_mem_free(os_msys_ctx->init_2_data);
+            os_msys_ctx->init_2_data = NULL;
+        }
+        os_mempool_unregister(&os_msys_ctx->init_2_mempool);
+#endif
+        nimble_platform_mem_free(os_msys_ctx);
+        os_msys_ctx = NULL;
+    }
+    STAILQ_INIT(&g_msys_pool_list);
+
+#else
+#if OS_MSYS_1_BLOCK_COUNT > 0
+
+    nimble_platform_mem_free(os_msys_init_1_data);
+    os_msys_init_1_data = NULL;
+    os_mempool_unregister(&os_msys_init_1_mempool);
+#endif
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+    nimble_platform_mem_free(os_msys_init_2_data);
+    os_msys_init_2_data = NULL;
+    os_mempool_unregister(&os_msys_init_2_mempool);
+#endif
+    STAILQ_INIT(&g_msys_pool_list);
+#endif
+}
+
+void os_msys_init(void)
+{
+#if OS_MSYS_SANITY_ENABLED
+    int rc;
+#endif
+
+#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC)
+    /* Ensure context is allocated before dereferencing macros */
+    if (ble_os_msys_ensure_ctx() != 0) {
+        BLE_LL_ASSERT(0);
+        return;
+    }
+#endif
+
+    os_msys_reset();
+
+#if OS_MSYS_1_BLOCK_COUNT > 0
+    os_msys_init_once(os_msys_init_1_data,
+                      &os_msys_init_1_mempool,
+                      &os_msys_init_1_mbuf_pool,
+                      OS_MSYS_1_BLOCK_COUNT,
+                      SYSINIT_MSYS_1_MEMBLOCK_SIZE,
+                      "msys_1");
+#endif
+
+#if OS_MSYS_2_BLOCK_COUNT > 0
+    os_msys_init_once(os_msys_init_2_data,
+                      &os_msys_init_2_mempool,
+                      &os_msys_init_2_mbuf_pool,
+                      OS_MSYS_2_BLOCK_COUNT,
+                      SYSINIT_MSYS_2_MEMBLOCK_SIZE,
+                      "msys_2");
+#endif
+
+#if OS_MSYS_SANITY_ENABLED
+    os_msys_sc.sc_func = os_msys_sanity;
+    os_msys_sc.sc_checkin_itvl =
+        OS_TICKS_PER_SEC * MYNEWT_VAL(MSYS_SANITY_TIMEOUT) / 1000;
+    rc = os_sanity_check_register(&os_msys_sc);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+#endif
+}

porting/npl/freertos/src/npl_os_freertos.c

@@ -37,6 +37,7 @@
 
 #include "esp_nimble_mem.h"
 #include "host/ble_hs.h"
+#include "esp_attr.h"
 
 portMUX_TYPE ble_port_mutex = portMUX_INITIALIZER_UNLOCKED;
 
@@ -257,18 +258,20 @@ npl_freertos_os_started(void)
  * npl_freertos_time_get, and hardware critical section functions.
  * This is a systematic issue requiring careful analysis of ISR-callable functions. */
 
-void *
+void * IRAM_ATTR
 npl_freertos_get_current_task_id(void)
 {
     return xTaskGetCurrentTaskHandle();
 }
 
-void
+void IRAM_ATTR
 npl_freertos_event_init(struct ble_npl_event *ev, ble_npl_event_fn *fn,
                     void *arg)
 {
     struct ble_npl_event_freertos *event = NULL;
 
+    ev->event = NULL;
+
 #if OS_MEM_ALLOC
     if (!ev->event) {
         ev->event = os_memblock_get(&ble_freertos_ev_pool);
@@ -343,8 +346,20 @@ void
 npl_freertos_eventq_deinit(struct ble_npl_eventq *evq)
 {
     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
+    struct ble_npl_event *ev;
 
     BLE_LL_ASSERT(eventq);
+
+    /* Drain the queue and clear the queued flag on all events */
+    while (uxQueueMessagesWaiting(eventq->q) > 0) {
+        if (xQueueReceive(eventq->q, &ev, 0) == pdPASS) {
+            struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
+            if (event) {
+                event->queued = false;
+            }
+        }
+    }
+
     vQueueDelete(eventq->q);
 #if OS_MEM_ALLOC
     os_memblock_put(&ble_freertos_evq_pool,eventq);
@@ -371,7 +386,7 @@ in_isr(void)
     return xPortInIsrContext() != 0;
 }
 
-struct ble_npl_event *
+struct ble_npl_event * IRAM_ATTR
 npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo)
 {
     struct ble_npl_event *ev = NULL;
@@ -393,14 +408,16 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo)
     if (ev) {
         struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
         if (event) {
+            BLE_NPL_ENTER_CRITICAL();
             event->queued = false;
+            BLE_NPL_EXIT_CRITICAL();
         }
     }
 
     return ev;
 }
 
-void
+void IRAM_ATTR
 npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev)
 {
     BaseType_t woken = pdFALSE;
@@ -567,7 +584,7 @@ npl_freertos_event_run(struct ble_npl_event *ev)
     }
 }
 
-bool
+bool IRAM_ATTR
 npl_freertos_eventq_is_empty(struct ble_npl_eventq *evq)
 {
     struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq;
@@ -584,7 +601,7 @@ npl_freertos_event_is_queued(struct ble_npl_event *ev)
     return false;
 }
 
-void *
+void * IRAM_ATTR
 npl_freertos_event_get_arg(struct ble_npl_event *ev)
 {
     struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event;
@@ -662,7 +679,7 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
         }
 
         memset(semaphor, 0, sizeof(*semaphor));
-        semaphor->handle = xSemaphoreCreateCounting(128, tokens);
+        semaphor->handle = xSemaphoreCreateCounting(65535, tokens);
         BLE_LL_ASSERT(semaphor->handle);
     }
 #else
@@ -675,7 +692,7 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
         }
 
         memset(semaphor, 0, sizeof(*semaphor));
-        semaphor->handle = xSemaphoreCreateCounting(128, tokens);
+        semaphor->handle = xSemaphoreCreateCounting(65535, tokens);
         BLE_LL_ASSERT(semaphor->handle);
     }
 #endif
@@ -708,7 +725,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;
 
@@ -735,7 +752,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) {
@@ -953,7 +970,7 @@ npl_freertos_sem_get_count(struct ble_npl_sem *sem)
 }
 
 
-ble_npl_error_t
+ble_npl_error_t IRAM_ATTR
 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;
@@ -1011,7 +1028,7 @@ npl_freertos_callout_stop(struct ble_npl_callout *co)
 #endif
 }
 
-bool
+bool IRAM_ATTR
 npl_freertos_callout_is_active(struct ble_npl_callout *co)
 {
     struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co;
@@ -1109,7 +1126,7 @@ npl_freertos_callout_set_arg(struct ble_npl_callout *co, void *arg)
     event->arg = arg;
 }
 
-uint32_t
+uint32_t IRAM_ATTR
 npl_freertos_time_get(void)
 {
 #if CONFIG_BT_NIMBLE_USE_ESP_TIMER
@@ -1202,7 +1219,7 @@ npl_freertos_hw_set_isr(int irqn, uint32_t addr)
 
 uint8_t hw_critical_state_status = 0;
 
-uint32_t
+uint32_t IRAM_ATTR
 npl_freertos_hw_enter_critical(void)
 {
     ++hw_critical_state_status;
@@ -1216,7 +1233,7 @@ npl_freertos_hw_is_in_critical(void)
     return hw_critical_state_status;
 }
 
-void
+void IRAM_ATTR
 npl_freertos_hw_exit_critical(uint32_t ctx)
 {
     --hw_critical_state_status;