From c4f113bd0e9231cbf85021d49647ac3e6b52e335 Mon Sep 17 00:00:00 2001 From: Rahul Tank Date: Wed, 6 May 2026 15:18:55 +0530 Subject: [PATCH] fix(nimble): Pending critical fixes --- apps/bttester/src/btp_gatt.c | 4 +- apps/bttester/src/gatt.c | 6 +- nimble/host/include/host/ble_esp_gap.h | 2 + nimble/host/include/host/ble_gatt.h | 6 + nimble/host/include/host/ble_hs_iso.h | 2 + nimble/host/services/hid/src/ble_svc_hid.c | 10 + .../htp/include/services/htp/ble_svc_htp.h | 1 + nimble/host/services/htp/src/ble_svc_htp.c | 6 + nimble/host/services/sps/src/ble_svc_sps.c | 20 +- nimble/host/src/ble_att.c | 7 +- nimble/host/src/ble_att_priv.h | 2 +- nimble/host/src/ble_att_svr.c | 333 +++++++++++------ nimble/host/src/ble_gap.c | 82 ++--- nimble/host/src/ble_gattc.c | 91 ++--- nimble/host/src/ble_gatts.c | 146 +++++--- nimble/host/src/ble_hs.c | 6 +- nimble/host/src/ble_hs_hci.c | 10 +- nimble/host/src/ble_hs_hci_evt.c | 22 +- nimble/host/src/ble_hs_hci_priv.h | 2 +- nimble/host/src/ble_hs_iso.c | 6 + nimble/host/src/ble_hs_pvcy.c | 82 ++++- nimble/host/src/ble_hs_resolv.c | 17 +- nimble/host/src/ble_l2cap_sig.c | 137 ++++--- nimble/host/src/ble_sm.c | 241 ++++++++++--- nimble/host/src/ble_sm_priv.h | 2 + .../store/config/src/ble_store_config_conf.c | 135 ++++--- nimble/transport/common/hci_h4/src/hci_h4.c | 16 +- nimble/transport/src/monitor.c | 78 ++-- porting/nimble/src/os_mbuf.c | 33 +- porting/nimble/src/os_msys_init.c | 339 ++++++++++++++++++ porting/npl/freertos/src/npl_os_freertos.c | 53 ++- 31 files changed, 1368 insertions(+), 529 deletions(-) create mode 100644 porting/nimble/src/os_msys_init.c diff --git a/apps/bttester/src/btp_gatt.c b/apps/bttester/src/btp_gatt.c index a3d58f16e..b75bcb010 100644 --- a/apps/bttester/src/btp_gatt.c +++ b/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; diff --git a/apps/bttester/src/gatt.c b/apps/bttester/src/gatt.c index 412227d8c..a27eeb8dc 100644 --- a/apps/bttester/src/gatt.c +++ b/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,8 +1750,8 @@ 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, - &out_att_err); + handle, 0, &buf, + &out_att_err); rp->att_response = out_att_err; rp->value_length = os_mbuf_len(buf) - sizeof(*rp); diff --git a/nimble/host/include/host/ble_esp_gap.h b/nimble/host/include/host/ble_esp_gap.h index e4a95c922..a738d5e73 100644 --- a/nimble/host/include/host/ble_esp_gap.h +++ b/nimble/host/include/host/ble_esp_gap.h @@ -7,6 +7,8 @@ #ifndef H_BLE_ESP_GAP_ #define H_BLE_ESP_GAP_ +#include + #ifdef __cplusplus extern "C" { #endif diff --git a/nimble/host/include/host/ble_gatt.h b/nimble/host/include/host/ble_gatt.h index 8fd515d8f..e73fa75fe 100644 --- a/nimble/host/include/host/ble_gatt.h +++ b/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 diff --git a/nimble/host/include/host/ble_hs_iso.h b/nimble/host/include/host/ble_hs_iso.h index 3c0aa1093..6bc521e29 100644 --- a/nimble/host/include/host/ble_hs_iso.h +++ b/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); diff --git a/nimble/host/services/hid/src/ble_svc_hid.c b/nimble/host/services/hid/src/ble_svc_hid.c index 45699182e..a6e523b94 100644 --- a/nimble/host/services/hid/src/ble_svc_hid.c +++ b/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; diff --git a/nimble/host/services/htp/include/services/htp/ble_svc_htp.h b/nimble/host/services/htp/include/services/htp/ble_svc_htp.h index d8741c325..626b76994 100644 --- a/nimble/host/services/htp/include/services/htp/ble_svc_htp.h +++ b/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); diff --git a/nimble/host/services/htp/src/ble_svc_htp.c b/nimble/host/services/htp/src/ble_svc_htp.c index be165d29d..06c729764 100644 --- a/nimble/host/services/htp/src/ble_svc_htp.c +++ b/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 * diff --git a/nimble/host/services/sps/src/ble_svc_sps.c b/nimble/host/services/sps/src/ble_svc_sps.c index b69d26ccd..b3edb1687 100644 --- a/nimble/host/services/sps/src/ble_svc_sps.c +++ b/nimble/host/services/sps/src/ble_svc_sps.c @@ -21,6 +21,7 @@ #include #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_scan_window = 0; + ble_svc_sps_reset(); } /** @@ -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. */ diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c index b9d033296..71fee0a8b 100644 --- a/nimble/host/src/ble_att.c +++ b/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_ERR_REQ_NOT_SUPPORTED) == 0) { - *om = NULL; - } + ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, + BLE_ATT_ERR_REQ_NOT_SUPPORTED); + *om = NULL; #else os_mbuf_free_chain(*om); *om = NULL; diff --git a/nimble/host/src/ble_att_priv.h b/nimble/host/src/ble_att_priv.h index ed9272622..94eda134e 100644 --- a/nimble/host/src/ble_att_priv.h +++ b/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); diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c index 0b5e9f472..dc63822a9 100644 --- a/nimble/host/src/ble_att_svr.c +++ b/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. */ - BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX); + if (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 || - !sec_state.encrypted) { - *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) { - *out_att_err = BLE_ATT_ERR_INSUFFICIENT_KEY_SZ; + if (!sec_state.authenticated || !sec_state.encrypted || sec_state.key_size != 16) { + 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); + } + + 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, - NULL); + rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, &om, + &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; - - ble_hs_lock(); + txom = NULL; 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; - mtu = 0; + txom = NULL; 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) { - rc = BLE_HS_ENOMEM; + 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; - att_err = 0; + txom = NULL; 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) { - return 0; + 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. */ - if (attr_om == NULL) { - 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; + uint16_t req_val_len = OS_MBUF_PKTLEN(rxom) - + sizeof(struct ble_att_find_type_value_req); + + rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf, + &attr_len, out_att_err); + if (rc == 0 && attr_len == req_val_len) { + /* value is at the end of req */ + rc = os_mbuf_cmpf(rxom, sizeof(struct ble_att_find_type_value_req), + buf, attr_len); + if (rc == 0) { + first = ha->ha_handle_id; + prev = ha->ha_handle_id; } - } else { - os_mbuf_adj(attr_om, OS_MBUF_PKTLEN(attr_om)); - } - - /* Read attribute value into temporary mbuf. */ - rc = ble_att_svr_read(conn_handle, ha, 0, attr_om, out_att_err); - if (rc != 0) { + } 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; } - - attr_len = OS_MBUF_PKTLEN(attr_om); - - /* 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; - } } } @@ -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; - att_err = 0; + txom = NULL; 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; - attr_om = NULL; + prev_attr_len = -1; /* 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; - err_handle = 0; + txom = NULL; 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; - att_err = 0; + txom = NULL; 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; - att_err = 0; + txom = NULL; 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; - err_handle = 0; + txom = NULL; 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; - err_handle = 0; + txom = NULL; 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; - err_handle = 0; + txom = NULL; 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; - att_err = 0; + txom = NULL; 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; - att_err = 0; + txom = NULL; 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; - err_handle = 0; + txom = NULL; 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; - att_err = 0; + txom = NULL; 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,14 +3710,16 @@ 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) - 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); if (!ble_att_svr_prep_entry_mem) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + 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); + if (!ble_att_svr_prep_entry_mem) { + 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,18 +3794,22 @@ 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) { - size += 4; // Updated - continue; + if (next_entry != NULL) { + /* Advance entry to skip the value attribute which was just accounted for */ + entry = next_entry; } - /* 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 || @@ -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)) { - sec_level = 0x03; //Authenticated pairing with encryption - /* This is the highest currently supported value. - * Break here. - */ - highest_security_level = 0x03; - break; + if (ble_hs_cfg.sm_sc_only) { + sec_level = 0x04; //Authenticated LE Secure Connections + } else { + sec_level = 0x03; //Authenticated pairing with encryption + } } 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; } diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index c5cecbbb7..205a7f9d7 100644 --- a/nimble/host/src/ble_gap.c +++ b/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; } - rc = os_mbuf_append(mbuf, data, data_len); - if (rc) { - os_mbuf_free_chain(mbuf); - 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)) || - (data_len > BLE_HCI_MAX_ADV_DATA_LEN)) { + if (data_len < 0 || + ((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; } - rc = os_mbuf_append(mbuf, data, data_len); - if (rc) { - os_mbuf_free_chain(mbuf); - 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; } - memcpy(cmd.scan_rsp, data, data_len); + 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 diff --git a/nimble/host/src/ble_gattc.c b/nimble/host/src/ble_gattc.c index 28c80052a..25fbff2ba 100644 --- a/nimble/host/src/ble_gattc.c +++ b/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) - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); + 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) - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); + 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,11 +4233,11 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status, attr[i].offset = 0; } - if (status == 0) { - for (i = 0; i < proc->read_mult.num_handles; i++) { - if (OS_MBUF_PKTLEN(*om) < 2) { - break; - } + *om = os_mbuf_pullup(*om, 2); + if (*om == NULL) { + status = BLE_HS_ENOMEM; + break; + } *om = os_mbuf_pullup(*om, 2); if (*om == NULL) { @@ -4234,23 +4247,21 @@ 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) { - break; - } + 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; - } + attr[i].om = os_msys_get_pkthdr(attr_len, 0); + if (!attr[i].om) { + status = BLE_HS_ENOMEM; + break; + } - rc = os_mbuf_appendfrom(attr[i].om, *om, 0, attr_len); - if (rc) { - break; - } - - os_mbuf_adj(*om, attr_len); + rc = os_mbuf_appendfrom(attr[i].om, *om, 0, attr_len); + if (rc) { + status = BLE_HS_ENOMEM; + break; } /* failed to correctly parse response, @@ -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); diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index dc082293f..2de59ace1 100644 --- a/nimble/host/src/ble_gatts.c +++ b/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); - BLE_HS_DBG_ASSERT(om != NULL); + *om = os_mbuf_pullup(*om, 2); + if (*om == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); + return BLE_ATT_ERR_INSUFFICIENT_RES; + } - flags = get_le16(om->om_data); + flags = get_le16((*om)->om_data); if ((flags & ~clt_cfg->allowed) != 0) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_REQ_NOT_SUPPORTED); return BLE_ATT_ERR_REQ_NOT_SUPPORTED; @@ -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) { - memset(&key_csfc, 0, sizeof key_csfc); - key_csfc.peer_addr = conn->bhc_peer_addr; - - rc = ble_store_delete_csfc(&key_csfc); - if (rc != 0) { - goto done; - } - - memset(&value_csfc, 0, sizeof value_csfc); - value_csfc.peer_addr = conn->bhc_peer_addr; - memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); - - rc = ble_store_write_csfc(&value_csfc); - if (rc != 0) { - goto done; - } - } + bonded = conn->bhc_sec_state.bonded; + peer_addr = conn->bhc_peer_addr; done: ble_hs_unlock(); + + if (rc == 0 && bonded) { + memset(&key_csfc, 0, sizeof key_csfc); + key_csfc.peer_addr = peer_addr; + + rc = ble_store_delete_csfc(&key_csfc); + if (rc != 0) { + 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 -arg[0] : added/removed -arg[1] : affected 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]; +struct ble_gatts_conn_clt_cfg_arg { + uint16_t action; + uint16_t chr_val_handle; uint16_t allowed_flags; - switch(action) { - case 1: + int rc; +}; + +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]; - ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle, - allowed_flags, 0); + allowed_flags = clt_cfg_arg->allowed_flags; + clt_cfg_arg->rc = ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + 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, - chr_val_handle); - if (rc == 0) { + clt_cfg_arg->rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + clt_cfg_arg->chr_val_handle); + 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[1] = ha->ha_handle_id + 1; - arg[2] = allowed_flags; - ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg); + arg.action = CONN_CLT_CFG_ADD; + arg.chr_val_handle = ha->ha_handle_id + 1; + arg.allowed_flags = allowed_flags; + 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[1] = chr_val_handle; - ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg); + arg.action = CONN_CLT_CFG_REMOVE; + arg.chr_val_handle = chr_val_handle; + 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 diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index 34b3e6f55..649ccfb5e 100644 --- a/nimble/host/src/ble_hs.c +++ b/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(); diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c index 5ae17db0a..d05da07a7 100644 --- a/nimble/host/src/ble_hs_hci.c +++ b/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; + return BLE_HS_ENOMEM; } } @@ -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; + return BLE_HS_ENOMEM; } } #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) diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index 589681dc9..2a8186835 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/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 diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h index 246551eb5..57ac35a64 100644 --- a/nimble/host/src/ble_hs_hci_priv.h +++ b/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); diff --git a/nimble/host/src/ble_hs_iso.c b/nimble/host/src/ble_hs_iso.c index 3022f39fa..3f54e4720 100644 --- a/nimble/host/src/ble_hs_iso.c +++ b/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) { diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index 263f5d706..ad65c9f94 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/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, - BLE_HCI_OCF_LE_CLR_RESOLV_LIST), - NULL, 0, NULL, 0); + 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) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (addr->type > BLE_ADDR_RANDOM_ID) { + 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 diff --git a/nimble/host/src/ble_hs_resolv.c b/nimble/host/src/ble_hs_resolv.c index 92d918af5..b913b55fd 100644 --- a/nimble/host/src/ble_hs_resolv.c +++ b/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]) && - (ecb.cipher_text[13] == rpa[2])) { + if (memcmp(ecb.cipher_text, rpa, 3) == 0) { rc = 0; } else { rc = BLE_HS_ENOENT; diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c index 3e031e455..9325f6816 100644 --- a/nimble/host/src/ble_l2cap_sig.c +++ b/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) && - (rsp->result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) && - (rsp->result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) { + if ((result != BLE_L2CAP_COC_ERR_NO_RESOURCES) && + (result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) && + (result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) { goto done; } } @@ -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++) { - proc->connect.chan[i]->cb = NULL; - ble_l2cap_chan_free(conn, proc->connect.chan[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) { - ble_hs_conn_delete_chan(conn, chan); - } else { - ble_l2cap_chan_free(NULL, chan); + chan = ble_hs_conn_chan_find_by_scid(conn, proc->disconnect.scid); + if (chan) { + ble_hs_conn_delete_chan(conn, 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); diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index abc162f12..03d9a05ce 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -711,13 +711,15 @@ ble_sm_persist_keys(struct ble_sm_proc *proc) &value_sec); ble_store_write_peer_sec(&value_sec); - value_rpa_rec.peer_addr.type = peer_addr.type; - memcpy(value_rpa_rec.peer_addr.val, peer_addr.val, sizeof peer_addr.val); + 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); - value_rpa_rec.peer_rpa_addr.type = peer_rpa_addr.type; - memcpy(value_rpa_rec.peer_rpa_addr.val, peer_rpa_addr.val, sizeof peer_rpa_addr.val); + value_rpa_rec.peer_rpa_addr.type = peer_rpa_addr.type; + 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); + 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 | - ble_hs_cfg.sm_mitm << 2 | - ble_hs_cfg.sm_sc << 3 | - ble_hs_cfg.sm_keypress << 4; + uint8_t authreq; + + 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); - rc = ble_store_read_our_sec(&key_sec, value_sec); - if (rc != 0) { - return rc; + 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) { + return 0; + } } - if (value_sec->ediv != ediv || value_sec->rand_num != rand) { - return BLE_HS_ENOENT; - } - 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 - * to not support SC pairing, let us make sure legacy pairing is supported - * on our side. If not, we can fail right away. + /* If SC-Only mode or Security Level 4 is enabled, the SC bit must be set. */ - if (!(cmd & BLE_SM_PAIR_AUTHREQ_SC)) { - if (MYNEWT_VAL(BLE_SM_LEGACY) == 0) { + 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 (ble_hs_cfg.sm_sc_only && !(req->authreq & BLE_SM_PAIR_AUTHREQ_SC)) { + } 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)) { - /* Fail if Secure Connections Only mode is on and key size is too small + } else if ((ble_hs_cfg.sm_sc_only || ble_hs_cfg.sm_sec_lvl == 4) && + (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,30 +2237,47 @@ 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)) { - /* Fail if Secure Connections Only mode is on and remote does not meet - * key size requirements - MITM was checked in last step - */ + } else if (!(rsp->authreq & BLE_SM_PAIR_AUTHREQ_BOND) && + (rsp->init_key_dist != 0 || rsp->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); + } 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 { - ble_sm_pair_cfg(proc); + 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) { - rc = ble_sm_io_action(proc, &ioact); - if (rc != 0) { - res->sm_err = BLE_SM_ERR_AUTHREQ; - res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ); - res->enc_cb = 1; + res->sm_err = BLE_SM_ERR_INVAL; + res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); } else { - proc->state = ble_sm_state_after_pair(proc); - if (ble_sm_ioact_state(ioact) == proc->state) { - res->passkey_params.action = ioact; - } - if (ble_sm_proc_can_advance(proc)) { - res->execute = 1; + ble_sm_pair_cfg(proc); + + rc = ble_sm_io_action(proc, &ioact); + if (rc != 0) { + res->sm_err = BLE_SM_ERR_AUTHREQ; + res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ); + res->enc_cb = 1; + } else { + proc->state = ble_sm_state_after_pair(proc); + if (ble_sm_ioact_state(ioact) == proc->state) { + res->passkey_params.action = ioact; + } + if (ble_sm_proc_can_advance(proc)) { + res->execute = 1; + } } } } @@ -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,8 +2936,14 @@ 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); - res->sm_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(); - ble_sm_process_result(conn_handle, &res, true); + 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 - * 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. + * starting pairing. */ 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; diff --git a/nimble/host/src/ble_sm_priv.h b/nimble/host/src/ble_sm_priv.h index 48b662731..6c1fb4ca8 100644 --- a/nimble/host/src/ble_sm_priv.h +++ b/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 diff --git a/nimble/host/store/config/src/ble_store_config_conf.c b/nimble/host/store/config/src/ble_store_config_conf.c index be56d42fd..821d27217 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/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 sec[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ]; - char cccd[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ]; - char rpa_rec[BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ]; + char *buf; + int buf_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, - sizeof buf.sec); - func("ble_hs/our_sec", buf.sec); + buf, + buf_sz); + 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, - sizeof buf.sec); - func("ble_hs/peer_sec", buf.sec); + buf, + buf_sz); + 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, - sizeof buf.cccd); - func("ble_hs/cccd", buf.cccd); + buf, + buf_sz); + 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, - sizeof buf.csfc); - func("ble_hs/csfc", buf.csfc); + buf, + buf_sz); + 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, - sizeof buf.ead); - func("ble_hs/ead", buf.ead); + buf, + buf_sz); + 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, - sizeof buf.rpa_rec); - func("ble_hs/rpa_rec", buf.rpa_rec); + buf, + buf_sz); + 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) { - /* - * 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]; + char *buf; 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); - /* NOTE: RAM-NVS consistency pattern - see system-wide TODO above */ + buf, BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ); + 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 diff --git a/nimble/transport/common/hci_h4/src/hci_h4.c b/nimble/transport/common/hci_h4/src/hci_h4.c index 6cc66c0b4..f87d0bd6c 100644 --- a/nimble/transport/common/hci_h4/src/hci_h4.c +++ b/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); - 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->frame_cb(h4sm->pkt_type, h4sm->buf); 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); - if (rc != 0) { - os_mbuf_free_chain(h4sm->om); - } + h4sm->frame_cb(h4sm->pkt_type, h4sm->om); h4sm->om = NULL; } break; diff --git a/nimble/transport/src/monitor.c b/nimble/transport/src/monitor.c index 13c3850e9..4a42a07f7 100644 --- a/nimble/transport/src/monitor.c +++ b/nimble/transport/src/monitor.c @@ -42,14 +42,12 @@ #include #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 uint8_t tx_ringbuf_tail; +static volatile int tx_ringbuf_head; +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 */ - while (inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf)) == - tx_ringbuf_tail) { - uart_start_tx(uart); + /* If buffer is full, we must discard data to avoid hanging the system, + * especially when called from ISR or critical section. + */ + 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()) { - os_time_delay(1); - } - OS_ENTER_CRITICAL(sr); + return; } 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 */ diff --git a/porting/nimble/src/os_mbuf.c b/porting/nimble/src/os_mbuf.c index bec1be89c..2f0337ae3 100644 --- a/porting/nimble/src/os_mbuf.c +++ b/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); - if (rc != 0) { - goto done; + tmp_rc = os_mbuf_free(om); + if (tmp_rc != 0) { + 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 diff --git a/porting/nimble/src/os_msys_init.c b/porting/nimble/src/os_msys_init.c new file mode 100644 index 000000000..5dfd966dc --- /dev/null +++ b/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 +#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 +} diff --git a/porting/npl/freertos/src/npl_os_freertos.c b/porting/npl/freertos/src/npl_os_freertos.c index 4ad38e6ef..481020056 100644 --- a/porting/npl/freertos/src/npl_os_freertos.c +++ b/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; @@ -391,16 +406,18 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) BLE_LL_ASSERT(ret == pdPASS || ret == errQUEUE_EMPTY); if (ev) { - struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event; - if (event) { + 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;