From 79316193f942060e9c79200ddd032157453b041d Mon Sep 17 00:00:00 2001 From: Rahul Tank Date: Sat, 25 Jul 2026 18:44:32 +0530 Subject: [PATCH] fix(nimble): Add changes for nimble-1.9.0-idf migration Co-authored-by: Cursor --- .rat-excludes | 12 + docs/ble_hs/ble_att.rst | 4 +- nimble/drivers/dialog_cmac/src/ble_phy.c | 3 +- nimble/host/include/host/ble_att.h | 6 +- nimble/host/include/host/ble_esp_gap.h | 2 + nimble/host/include/host/ble_esp_hs.h | 4 +- nimble/host/include/host/ble_gap.h | 13 +- nimble/host/include/host/ble_gatt.h | 33 +- nimble/host/include/host/ble_hs.h | 2 + nimble/host/include/host/ble_hs_iso.h | 2 - nimble/host/include/host/ble_hs_log.h | 9 +- nimble/host/include/host/ble_iso.h | 2 +- nimble/host/mesh/include/mesh/glue.h | 12 + nimble/host/mesh/src/msg.c | 3 + nimble/host/services/ans/src/ble_svc_ans.c | 11 +- nimble/host/services/bas/src/ble_svc_bas.c | 14 +- .../cte/include/services/cte/ble_svc_cte.h | 1 + nimble/host/services/cte/src/ble_svc_cte.c | 80 +- nimble/host/services/cts/src/ble_svc_cts.c | 9 +- nimble/host/services/dis/src/ble_svc_dis.c | 9 +- nimble/host/services/gap/src/ble_svc_gap.c | 59 +- nimble/host/services/hid/src/ble_svc_hid.c | 5 + nimble/host/services/htp/src/ble_svc_htp.c | 12 +- nimble/host/services/lls/src/ble_svc_lls.c | 13 +- nimble/host/services/prox/src/ble_svc_prox.c | 24 +- nimble/host/services/ras/src/ble_svc_ras.c | 15 +- nimble/host/services/tps/src/ble_svc_tps.c | 13 +- nimble/host/src/ble_aes_ccm.c | 22 +- nimble/host/src/ble_att.c | 25 +- nimble/host/src/ble_att_clt.c | 21 +- nimble/host/src/ble_att_cmd.c | 94 ++- nimble/host/src/ble_att_svr.c | 62 +- nimble/host/src/ble_cs.c | 33 +- nimble/host/src/ble_ead.c | 10 +- nimble/host/src/ble_eatt.c | 83 +- nimble/host/src/ble_eddystone.c | 48 +- nimble/host/src/ble_gap.c | 781 ++++++++++++------ nimble/host/src/ble_gap_priv.h | 6 +- nimble/host/src/ble_gatt_priv.h | 3 +- nimble/host/src/ble_gattc.c | 568 +++++-------- nimble/host/src/ble_gattc_cache.c | 15 +- nimble/host/src/ble_gattc_cache_conn.c | 374 +++++---- nimble/host/src/ble_gattc_cache_priv.h | 3 +- nimble/host/src/ble_gatts.c | 101 ++- nimble/host/src/ble_gatts_lcl.c | 4 +- nimble/host/src/ble_hs.c | 105 ++- nimble/host/src/ble_hs_adv.c | 38 +- nimble/host/src/ble_hs_conn.c | 19 +- nimble/host/src/ble_hs_flow.c | 15 +- nimble/host/src/ble_hs_hci.c | 20 +- nimble/host/src/ble_hs_hci_cmd.c | 15 +- nimble/host/src/ble_hs_hci_evt.c | 183 ++-- nimble/host/src/ble_hs_id.c | 12 +- nimble/host/src/ble_hs_iso.c | 33 +- nimble/host/src/ble_hs_iso_hci.c | 4 +- nimble/host/src/ble_hs_pvcy.c | 57 +- nimble/host/src/ble_hs_resolv.c | 16 +- nimble/host/src/ble_hs_resolv_priv.h | 2 +- nimble/host/src/ble_hs_startup.c | 21 +- nimble/host/src/ble_hs_stop.c | 79 +- nimble/host/src/ble_iso.c | 227 +++-- nimble/host/src/ble_iso_priv.h | 3 + nimble/host/src/ble_l2cap.c | 44 +- nimble/host/src/ble_l2cap_coc.c | 62 +- nimble/host/src/ble_l2cap_coc_priv.h | 7 - nimble/host/src/ble_l2cap_sig.c | 65 +- nimble/host/src/ble_l2cap_sig_priv.h | 17 + nimble/host/src/ble_sm.c | 199 +++-- nimble/host/src/ble_sm_alg.c | 704 +++++++++++++--- nimble/host/src/ble_sm_priv.h | 10 +- nimble/host/src/ble_sm_sc.c | 15 +- nimble/host/src/ble_store.c | 4 + .../host/store/config/src/ble_store_config.c | 18 +- .../store/config/src/ble_store_config_conf.c | 6 + .../store/config/src/ble_store_config_priv.h | 5 +- nimble/host/store/config/src/ble_store_nvs.c | 82 +- nimble/host/test/src/ble_gatts_reg_test.c | 5 +- nimble/host/test/src/ble_hs_test_util.c | 29 +- nimble/host/test/src/ble_sm_test.c | 2 +- nimble/include/nimble/ble.h | 1 - nimble/include/nimble/hci_common.h | 9 +- nimble/transport/cdc/src/cdc_hci.c | 4 + nimble/transport/common/hci_ipc/src/hci_ipc.c | 9 +- .../transport/esp_ipc_btdm/src/hci_esp_ipc.c | 16 + .../transport/nrf5340/src/nrf5340_ble_hci.c | 4 - nimble/transport/socket/src/ble_hci_socket.c | 20 +- nimble/transport/src/monitor.c | 39 +- nimble/transport/src/rx_task.c | 5 + nimble/transport/src/transport.c | 4 + nimble/transport/uart_ll/src/hci_uart.c | 2 +- .../include/console/console_port.h | 14 +- .../linux_blemesh_shell/shell/shell_port.c | 1 + .../examples/linux_blemesh_shell/src/ble.c | 7 +- .../examples/linux_blemesh_shell/src/main.c | 2 +- porting/nimble/include/log/log.h | 36 + porting/nimble/include/modlog/modlog.h | 1 - porting/nimble/src/mem.c | 3 +- porting/nimble/src/nimble_port.c | 73 +- porting/nimble/src/os_mbuf.c | 13 +- porting/nimble/src/os_mempool.c | 156 +++- porting/nimble/src/os_msys.c | 14 +- porting/nimble/src/os_msys_init.c | 13 +- .../dummy/include/nimble/nimble_npl_os_log.h | 4 +- .../esp-idf/include/nimble/nimble_npl_os.h | 4 + porting/npl/freertos/src/npl_os_freertos.c | 60 +- .../linux/include/nimble/nimble_npl_os_log.h | 3 +- porting/npl/linux/src/os_atomic.c | 12 +- porting/npl/linux/src/os_sem.c | 8 +- porting/npl/linux/src/os_task.c | 13 +- .../riot/include/nimble/nimble_npl_os_log.h | 3 +- porting/update_generated_files.sh | 2 +- 111 files changed, 3493 insertions(+), 1810 deletions(-) create mode 100644 porting/nimble/include/log/log.h diff --git a/.rat-excludes b/.rat-excludes index 5e06820fa..390ef0944 100644 --- a/.rat-excludes +++ b/.rat-excludes @@ -4,12 +4,24 @@ repos # Non-source files RELEASE_NOTES.md +.gitignore **/README.md Doxyfile .clang-format .mailmap .clang-format-ignore **/requirements.txt +README.md +pts-gap.txt +pts-gatt.txt +pts-l2cap.txt +pts-sm.txt +94654-20170317-085122560.tpg +94654-20170317-085441153.pts +uncrustify.cfg +.style_ignored_dirs +.mailmap +requirements.txt # Ignore documentation folder docs diff --git a/docs/ble_hs/ble_att.rst b/docs/ble_hs/ble_att.rst index 20cb94ac4..20be92795 100644 --- a/docs/ble_hs/ble_att.rst +++ b/docs/ble_hs/ble_att.rst @@ -124,6 +124,6 @@ Finds an available (idle) EATT channel for the given connection. If an EATT chan .. code-block:: c - void ble_eatt_release_chan(uint16_t conn_handle, uint8_t op) + void ble_eatt_release_chan(uint16_t conn_handle, uint16_t cid) -Releases a previously claimed EATT channel, marking it as idle and available for new transactions. +Releases a previously claimed EATT channel identified by ``cid``, marking it as idle and available for new transactions. diff --git a/nimble/drivers/dialog_cmac/src/ble_phy.c b/nimble/drivers/dialog_cmac/src/ble_phy.c index de7024b46..70414c663 100644 --- a/nimble/drivers/dialog_cmac/src/ble_phy.c +++ b/nimble/drivers/dialog_cmac/src/ble_phy.c @@ -1365,6 +1365,7 @@ ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs) ble_phy_mode_apply(g_ble_phy_data.phy_mode_rx); #endif + assert(ble_rf_is_enabled()); ble_phy_rx(); /* Store LLT value at RX start, we'll need this to set wfr */ @@ -1658,7 +1659,7 @@ ble_phy_encrypt_disable(void) #endif static int8_t -phy_txpower_round(int8_t dbm) +phy_txpower_round(int dbm) { static const int8_t supported_pwr_levels[] = {-18, -12, -8, -6, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6 }; diff --git a/nimble/host/include/host/ble_att.h b/nimble/host/include/host/ble_att.h index 44a336baa..b682b6e7c 100644 --- a/nimble/host/include/host/ble_att.h +++ b/nimble/host/include/host/ble_att.h @@ -112,14 +112,12 @@ struct os_mbuf; #define BLE_ATT_ERR_DB_OUT_OF_SYNC 0x12 #define BLE_ATT_ERR_VALUE_NOT_ALLOWED 0x13 -/**Requested value is not allowed. */ -#define BLE_ATT_ERR_VALUE_NOT_ALLOWED 0x13 - /**Write Request Rejected. */ #define BLE_ATT_ERR_WRITE_REQ_REJECTED 0xFC /**Client Characteristic Configuration Descriptor Improperly Configured. */ -#define BLE_ATT_ERR_CCCD_IMPORER_CONF 0xFD +#define BLE_ATT_ERR_CCCD_IMPROPER_CONF 0xFD + /**Procedure Already in Progress. */ #define BLE_ATT_ERR_PROC_IN_PROGRESS 0xFE diff --git a/nimble/host/include/host/ble_esp_gap.h b/nimble/host/include/host/ble_esp_gap.h index a738d5e73..156bfa985 100644 --- a/nimble/host/include/host/ble_esp_gap.h +++ b/nimble/host/include/host/ble_esp_gap.h @@ -338,6 +338,7 @@ int ble_gap_config_ext_scan_adi_filter(uint8_t enable, uint8_t did_filter, uint8 int ble_gap_dev_authorization(uint16_t conn_handle, bool authorized); +#if MYNEWT_VAL(BLE_HCI_VS) /** * Sets the vendor-specific event mask for BLE host. * @@ -351,6 +352,7 @@ ble_gap_dev_authorization(uint16_t conn_handle, bool authorized); */ int ble_hs_send_vs_event_mask(uint32_t event_mask); +#endif void ble_gap_rx_test_evt(const void *buf, uint8_t len); void ble_gap_tx_test_evt(const void *buf, uint8_t len); diff --git a/nimble/host/include/host/ble_esp_hs.h b/nimble/host/include/host/ble_esp_hs.h index f381816d0..a764b9653 100644 --- a/nimble/host/include/host/ble_esp_hs.h +++ b/nimble/host/include/host/ble_esp_hs.h @@ -21,7 +21,7 @@ void ble_hs_deinit(void); /** * Initializes the Bluetooth advertising list and associated mutex lock. */ -void ble_adv_list_init(void); +int ble_adv_list_init(void); /** * Deinitializes the Bluetooth advertising list, releasing allocated memory and resources. @@ -31,7 +31,7 @@ void ble_adv_list_deinit(void); /** * Adds a Bluetooth advertising packet to the list. */ -void ble_adv_list_add_packet(void *data); +bool ble_adv_list_add_packet(void *data); /** * Returns the count of Bluetooth advertising packets in the list. diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h index 583218e09..d83ef671c 100644 --- a/nimble/host/include/host/ble_gap.h +++ b/nimble/host/include/host/ble_gap.h @@ -1653,6 +1653,14 @@ struct ble_gap_event { uint8_t length; } unhandled_hci; #endif +#else + /* Legacy 1.6.0-idf field name; layout intentionally identical. */ + struct { + bool is_le_meta; + bool is_vs; + const void *ev; + uint8_t length; + } vs_hci; #endif /** @@ -4081,7 +4089,7 @@ int ble_gap_read_le_phy(uint16_t conn_handle, uint8_t *tx_phy, uint8_t *rx_phy); #define BLE_GAP_LE_PHY_CODED_MASK 0x04 /** Bitmask for any PHY. */ -#define BLE_GAP_LE_PHY_ANY_MASK 0x0F +#define BLE_GAP_LE_PHY_ANY_MASK 0x07 /** @} */ @@ -4595,6 +4603,8 @@ int ble_gap_rd_local_resolv_addr(uint8_t peer_addr_type, const ble_addr_t *peer_ uint8_t *out_addr); #endif +#if MYNEWT_VAL(BLE_HS_PVCY) + /** * Read local IRK command * @@ -4604,6 +4614,7 @@ int ble_gap_rd_local_resolv_addr(uint8_t peer_addr_type, const ble_addr_t *peer_ * */ int ble_gap_read_local_irk(uint8_t * out_irk); +#endif #if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) /** diff --git a/nimble/host/include/host/ble_gatt.h b/nimble/host/include/host/ble_gatt.h index e73fa75fe..487f24e88 100644 --- a/nimble/host/include/host/ble_gatt.h +++ b/nimble/host/include/host/ble_gatt.h @@ -415,16 +415,6 @@ struct ble_hs_cfg; /** @} */ -/*** @server. */ -/** Represents one notification tuple in a multi notification PDU */ -struct ble_gatt_notif_entry { - /** The attribute handle on which to notify. */ - uint16_t handle; - - /** The notification value. */ - struct os_mbuf *value; -}; - /*** @client. */ /** Represents a GATT error. */ struct ble_gatt_error { @@ -784,10 +774,6 @@ int ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles, uint8_t num_handles, ble_gatt_attr_fn *cb, void *cb_arg); -int ble_gattc_read_mult_var(uint16_t conn_handle, const uint16_t *handles, - uint8_t num_handles, ble_gatt_attr_mult_fn *cb, - void *cb_arg); - /** * Initiates GATT procedure: Read Multiple Variable Length Characteristic Values. * @@ -980,22 +966,6 @@ int ble_gatts_notify_multiple_custom(uint16_t conn_handle, size_t chr_count, struct ble_gatt_notif *tuples); -/** - * Sends multiple characteristic notifications on the specified - * attribute handles. This function consumes the mbuf of the - * notification value after sending notification. - * - * @param conn_handle The connection over which to execute the - * procedure. - * @param tuples The list of attribute handles and notification - * value tuples. - * @param num_tuples The number of notifications to send. - * - * @return 0 on success; nonzero on failure. - */ -int ble_gatts_multi_notify_custom(uint16_t conn_handle, - struct ble_gatt_notif_entry *tuples, uint16_t num_tuples); - /** * @deprecated Should not be used. Use ble_gatts_notify_custom instead. * @@ -1006,9 +976,8 @@ int ble_gatts_multi_notify_custom(uint16_t conn_handle, * @param om The value to write to the characteristic. * * @return 0 on success; nonzero on failure. + * @deprecated Use ble_gatts_notify_custom() instead. */ -int ble_gattc_notify_custom(uint16_t conn_handle, uint16_t att_handle, - struct os_mbuf *om); /** * Sends a characteristic notification. The content of the message is read diff --git a/nimble/host/include/host/ble_hs.h b/nimble/host/include/host/ble_hs.h index 6b884767a..6d83daed9 100644 --- a/nimble/host/include/host/ble_hs.h +++ b/nimble/host/include/host/ble_hs.h @@ -43,7 +43,9 @@ #include "host/ble_sm.h" #include "host/ble_store.h" #include "host/ble_uuid.h" +#if MYNEWT_VAL(BLE_ISO) #include "host/ble_iso.h" +#endif #include "nimble/nimble_npl.h" #include "host/ble_esp_hs.h" diff --git a/nimble/host/include/host/ble_hs_iso.h b/nimble/host/include/host/ble_hs_iso.h index 6bc521e29..3c0aa1093 100644 --- a/nimble/host/include/host/ble_hs_iso.h +++ b/nimble/host/include/host/ble_hs_iso.h @@ -16,8 +16,6 @@ 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/include/host/ble_hs_log.h b/nimble/host/include/host/ble_hs_log.h index 25896850d..66e109785 100644 --- a/nimble/host/include/host/ble_hs_log.h +++ b/nimble/host/include/host/ble_hs_log.h @@ -48,13 +48,13 @@ #include "logcfg/logcfg.h" #endif -#ifndef ESP_PLATFORM +#if !defined(ESP_PLATFORM) && !MYNEWT_VAL(NEWT_FEATURE_LOGCFG) #ifndef BLE_NPL_LOG_MODULE /** Defines the logging module for NimBLE Porting Layer (NPL). */ #define BLE_NPL_LOG_MODULE BLE_HS_LOG #endif #include -#endif /* !ESP_PLATFORM */ +#endif /* !ESP_PLATFORM && !NEWT_FEATURE_LOGCFG */ #ifdef __cplusplus extern "C" { @@ -71,8 +71,7 @@ struct os_mbuf; * @param lvl The log level of the message. * @param ... The format string and additional arguments for the log message. */ -#ifdef ESP_PLATFORM -/* Use logcfg BLE_HS_LOG_ macros — avoids conflict with BLE_NPL_LOG_IMPL. */ +#if defined(ESP_PLATFORM) || MYNEWT_VAL(NEWT_FEATURE_LOGCFG) #define BLE_HS_LOG(lvl, ...) \ BLE_HS_LOG_ ## lvl(__VA_ARGS__) #else @@ -89,7 +88,7 @@ struct os_mbuf; * @param lvl The log level of the message. * @param addr The Bluetooth address to be logged. */ -#ifdef ESP_PLATFORM +#if defined(ESP_PLATFORM) || MYNEWT_VAL(NEWT_FEATURE_LOGCFG) #define BLE_HS_LOG_ADDR(lvl, addr) \ BLE_HS_LOG_ ## lvl("%02x:%02x:%02x:%02x:%02x:%02x", \ (addr)[5], (addr)[4], (addr)[3], \ diff --git a/nimble/host/include/host/ble_iso.h b/nimble/host/include/host/ble_iso.h index ad1d0ed9a..75fc4ca59 100644 --- a/nimble/host/include/host/ble_iso.h +++ b/nimble/host/include/host/ble_iso.h @@ -177,7 +177,7 @@ struct ble_iso_event { * o BLE_ISO_EVENT_BIG_SYNC_TERMINATED */ struct { - uint16_t big_handle; + uint8_t big_handle; uint8_t reason; } big_terminated; diff --git a/nimble/host/mesh/include/mesh/glue.h b/nimble/host/mesh/include/mesh/glue.h index a991fe4c7..0796c070a 100644 --- a/nimble/host/mesh/include/mesh/glue.h +++ b/nimble/host/mesh/include/mesh/glue.h @@ -174,6 +174,18 @@ extern "C" { #define BT_GAP_ADV_SLOW_INT_MIN 0x0640 /* 1 s */ #define BT_GAP_ADV_SLOW_INT_MAX 0x0780 /* 1.2 s */ +#if defined(ESP_PLATFORM) && !defined(BLE_NPL_LOG) +#ifndef BLE_NPL_LOG_MODULE +#define BLE_NPL_LOG_MODULE BLE_MESH_LOG +#endif +#ifndef _BLE_NPL_LOG_CAT +#define _BLE_NPL_LOG_CAT(a, ...) _BLE_NPL_LOG_PRIMITIVE_CAT(a, __VA_ARGS__) +#define _BLE_NPL_LOG_PRIMITIVE_CAT(a, ...) a ## __VA_ARGS__ +#endif +#define BLE_NPL_LOG(lvl, ...) \ + _BLE_NPL_LOG_CAT(BLE_NPL_LOG_MODULE, _BLE_NPL_LOG_CAT(_, lvl))(__VA_ARGS__) +#endif + #define BT_DBG(fmt, ...) BLE_NPL_LOG(DEBUG, "%s: " fmt "\n", __func__, ## __VA_ARGS__); #define BT_INFO(fmt, ...) BLE_NPL_LOG(INFO, "%s: " fmt "\n", __func__, ## __VA_ARGS__); #define BT_WARN(fmt, ...) BLE_NPL_LOG(WARN, "%s: " fmt "\n", __func__, ## __VA_ARGS__); diff --git a/nimble/host/mesh/src/msg.c b/nimble/host/mesh/src/msg.c index b788ffe1e..33fbcda72 100644 --- a/nimble/host/mesh/src/msg.c +++ b/nimble/host/mesh/src/msg.c @@ -6,6 +6,9 @@ * SPDX-License-Identifier: Apache-2.0 */ +#define BLE_NPL_LOG_MODULE BLE_MESH_MODEL_LOG +#include + #include "mesh/mesh.h" void bt_mesh_model_msg_init(struct os_mbuf *msg, uint32_t opcode) diff --git a/nimble/host/services/ans/src/ble_svc_ans.c b/nimble/host/services/ans/src/ble_svc_ans.c index 4584580d6..ffbf14562 100644 --- a/nimble/host/services/ans/src/ble_svc_ans.c +++ b/nimble/host/services/ans/src/ble_svc_ans.c @@ -51,6 +51,8 @@ typedef struct { uint8_t _ble_svc_ans_alert_not_ctrl_pt[2]; uint8_t _ble_svc_ans_new_alert_cnt[BLE_SVC_ANS_CAT_NUM]; uint8_t _ble_svc_ans_unr_alert_cnt[BLE_SVC_ANS_CAT_NUM]; + uint8_t _ble_svc_ans_new_alert_notif_state; + uint8_t _ble_svc_ans_unr_alert_notif_state; uint16_t _ble_svc_ans_conn_handle; } ble_svc_ans_ctx_t; @@ -64,6 +66,8 @@ static ble_svc_ans_ctx_t * ble_svc_ans_ctx; #define ble_svc_ans_alert_not_ctrl_pt (ble_svc_ans_ctx->_ble_svc_ans_alert_not_ctrl_pt) #define ble_svc_ans_new_alert_cnt (ble_svc_ans_ctx->_ble_svc_ans_new_alert_cnt) #define ble_svc_ans_unr_alert_cnt (ble_svc_ans_ctx->_ble_svc_ans_unr_alert_cnt) +#define ble_svc_ans_new_alert_notif_state (ble_svc_ans_ctx->_ble_svc_ans_new_alert_notif_state) +#define ble_svc_ans_unr_alert_notif_state (ble_svc_ans_ctx->_ble_svc_ans_unr_alert_notif_state) #define ble_svc_ans_conn_handle (ble_svc_ans_ctx->_ble_svc_ans_conn_handle) #else /* MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) */ @@ -306,18 +310,21 @@ ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_SVC_ANS_CHR_UUID16_ALERT_NOT_CTRL_PT: if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + ble_hs_lock(); rc = ble_svc_ans_chr_write(ctxt->om, sizeof ble_svc_ans_alert_not_ctrl_pt, sizeof ble_svc_ans_alert_not_ctrl_pt, &ble_svc_ans_alert_not_ctrl_pt, NULL); if (rc != 0) { + ble_hs_unlock(); return rc; } - /* Get command ID and category ID */ + /* Get command ID and category ID while still holding lock */ cmd_id = ble_svc_ans_alert_not_ctrl_pt[0]; cat_id = ble_svc_ans_alert_not_ctrl_pt[1]; + ble_hs_unlock(); /* Set cat_bit_mask to the appropriate bitmask based on cat_id */ @@ -428,6 +435,8 @@ ble_svc_ans_on_gap_connect(uint16_t conn_handle) ble_hs_lock(); ble_svc_ans_conn_handle = conn_handle; + ble_svc_ans_new_alert_notif_state = 0; + ble_svc_ans_unr_alert_notif_state = 0; ble_hs_unlock(); } diff --git a/nimble/host/services/bas/src/ble_svc_bas.c b/nimble/host/services/bas/src/ble_svc_bas.c index d49c1d4c6..a079a82d0 100644 --- a/nimble/host/services/bas/src/ble_svc_bas.c +++ b/nimble/host/services/bas/src/ble_svc_bas.c @@ -37,6 +37,7 @@ static uint16_t ble_svc_bas_battery_handle; /* Battery level */ static uint8_t ble_svc_bas_battery_level; static bool ble_svc_bas_initialized; +static bool ble_svc_bas_counted; /* Access function */ static int @@ -138,6 +139,7 @@ ble_svc_bas_deinit(void) { ble_gatts_free_svcs(); ble_svc_bas_initialized = false; + ble_svc_bas_counted = false; } /** @@ -154,10 +156,13 @@ ble_svc_bas_init(void) ble_svc_bas_battery_level = 0; - rc = ble_gatts_count_cfg(ble_svc_bas_defs); - if (rc != 0) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); - return; + if (!ble_svc_bas_counted) { + rc = ble_gatts_count_cfg(ble_svc_bas_defs); + if (rc != 0) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + return; + } + ble_svc_bas_counted = true; } rc = ble_gatts_add_svcs(ble_svc_bas_defs); @@ -166,7 +171,6 @@ ble_svc_bas_init(void) return; } - ble_svc_bas_battery_level = 0; ble_svc_bas_initialized = true; } #endif diff --git a/nimble/host/services/cte/include/services/cte/ble_svc_cte.h b/nimble/host/services/cte/include/services/cte/ble_svc_cte.h index b32e650e0..23a234cdc 100644 --- a/nimble/host/services/cte/include/services/cte/ble_svc_cte.h +++ b/nimble/host/services/cte/include/services/cte/ble_svc_cte.h @@ -36,6 +36,7 @@ struct ble_hs_cfg; void ble_svc_cte_init(void); +void ble_svc_cte_deinit(void); #ifdef __cplusplus } diff --git a/nimble/host/services/cte/src/ble_svc_cte.c b/nimble/host/services/cte/src/ble_svc_cte.c index 1c64948bf..e1cef0b82 100644 --- a/nimble/host/services/cte/src/ble_svc_cte.c +++ b/nimble/host/services/cte/src/ble_svc_cte.c @@ -18,6 +18,7 @@ */ #include +#include #include #include "sysinit/sysinit.h" #include "syscfg/syscfg.h" @@ -335,6 +336,21 @@ ble_svc_cte_two_octet_chr_write(struct os_mbuf *om, } +static void +ble_svc_cte_rollback_aoa_state(uint16_t conn_handle, bool aoa_enabled_now, + bool aoa_disabled_now, uint8_t *cte_enable) +{ + if (aoa_enabled_now) { + if (ble_gap_conn_cte_rsp_enable(conn_handle, false) != 0) { + *cte_enable |= CTE_ENABLE_AOA_CONNECTION; + } + } else if (aoa_disabled_now) { + if (ble_gap_conn_cte_rsp_enable(conn_handle, true) != 0) { + *cte_enable &= ~CTE_ENABLE_AOA_CONNECTION; + } + } +} + /** * @brief Access callback for Constant Tone Extension Service cte enable * @@ -377,6 +393,8 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle, &config->cte_enable, NULL); if (rc == 0) { + bool aoa_enabled_now = false; + bool aoa_disabled_now = false; if(((config->cte_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) && ((old_enable & CTE_ENABLE_AOA_CONNECTION) != CTE_ENABLE_AOA_CONNECTION)) { /* 0->1 transition: enable AoA CTE response on this connection */ @@ -391,6 +409,7 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle, rc = 0xFC; break; } + aoa_enabled_now = true; } else if (((old_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) && ((config->cte_enable & CTE_ENABLE_AOA_CONNECTION) != CTE_ENABLE_AOA_CONNECTION)) { /* 1->0 transition only: disable AoA CTE response. @@ -402,26 +421,40 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle, rc = 0xFC; break; } + aoa_disabled_now = true; } if(((config->cte_enable & CTE_ENABLE_AOD_ADVERTISING) == CTE_ENABLE_AOD_ADVERTISING) && ((old_enable & CTE_ENABLE_AOD_ADVERTISING) != CTE_ENABLE_AOD_ADVERTISING)) { - if (adv_cte_config.cte_phy != CTE_PHY_MIN_VALUE) { + /* CTE PHY range check: valid values are CTE_PHY_MIN_VALUE (LE 1M) + * through CTE_PHY_MAX_VALUE (LE 2M). Both are valid per CTE spec. */ + if (adv_cte_config.cte_phy > CTE_PHY_MAX_VALUE) { + ble_svc_cte_rollback_aoa_state(conn_handle, + aoa_enabled_now, + aoa_disabled_now, + &old_enable); config->cte_enable = old_enable; rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED; break; } - /* Apply the current advertising CTE parameters to the controller - * using advertising instance 0 (default periodic advertising set). */ + /* Map GATT PHY characteristic to HCI AoD CTE_Type: + * LE 1M PHY (0) -> AoD 1us slots (0x01) + * LE 2M PHY (1) -> AoD 2us slots (0x02) + * The +1 offset converts the PHY value to the AoD type. */ + uint8_t ant_ids[] = {0, 1}; struct ble_gap_periodic_adv_cte_params cte_params = { .cte_length = adv_cte_config.cte_min_length, - .cte_type = adv_cte_config.cte_phy, + .cte_type = (uint8_t)(adv_cte_config.cte_phy + 1), .cte_count = adv_cte_config.cte_min_transmit_count, - .switching_pattern_length = 0, - .antenna_ids = NULL, + .switching_pattern_length = 2, + .antenna_ids = ant_ids, }; if (ble_gap_set_connless_cte_transmit_params(0, &cte_params) != 0 || ble_gap_set_connless_cte_transmit_enable(0, 1) != 0) { + ble_svc_cte_rollback_aoa_state(conn_handle, + aoa_enabled_now, + aoa_disabled_now, + &old_enable); config->cte_enable = old_enable; rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED; break; @@ -429,7 +462,16 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle, } else if (((old_enable & CTE_ENABLE_AOD_ADVERTISING) == CTE_ENABLE_AOD_ADVERTISING) && ((config->cte_enable & CTE_ENABLE_AOD_ADVERTISING) != CTE_ENABLE_AOD_ADVERTISING)) { /* 1->0 transition: disable connless CTE transmission */ - ble_gap_set_connless_cte_transmit_enable(0, 0); + rc = ble_gap_set_connless_cte_transmit_enable(0, 0); + if (rc != 0) { + ble_svc_cte_rollback_aoa_state(conn_handle, + aoa_enabled_now, + aoa_disabled_now, + &old_enable); + config->cte_enable = old_enable; + rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED; + break; + } } } } @@ -624,18 +666,30 @@ static void cte_init_config(void) { /** * @brief Initialize the Constant Tone Extension Service */ +static int ble_svc_cte_initialized = 0; + +void +ble_svc_cte_deinit(void) +{ + if (ble_svc_cte_initialized) { + ble_gap_event_listener_unregister(&cte_gap_listener); + } + ble_svc_cte_initialized = 0; +} + void ble_svc_cte_init(void) { int rc; - /* Guard against repeated calls: ble_gatts_count_cfg and ble_gatts_add_svcs - * are additive, so calling them more than once inflates resource counters - * and inserts duplicate GATT service entries. */ - static int initialized = 0; - if (initialized) { + /* Guard against repeated calls within the same init cycle: + * ble_gatts_count_cfg and ble_gatts_add_svcs are additive, so calling + * them more than once inflates resource counters and inserts duplicate + * GATT service entries. ble_svc_cte_deinit() resets this on stack deinit + * so that a subsequent nimble_port_init() cycle re-registers correctly. */ + if (ble_svc_cte_initialized) { return; } - initialized = 1; + ble_svc_cte_initialized = 1; cte_init_config(); ble_gap_event_listener_register(&cte_gap_listener, cte_gap_event, NULL); diff --git a/nimble/host/services/cts/src/ble_svc_cts.c b/nimble/host/services/cts/src/ble_svc_cts.c index d65c7b186..699fc4f71 100644 --- a/nimble/host/services/cts/src/ble_svc_cts.c +++ b/nimble/host/services/cts/src/ble_svc_cts.c @@ -169,9 +169,14 @@ ble_svc_cts_access(uint16_t conn_handle, uint16_t attr_handle, rc = os_mbuf_append(ctxt->om, ¤t_local_time_val, sizeof current_local_time_val); if (rc == 0) { - current_local_time_val.adjust_reason = 0; + /* Clear adjust_reason only on direct client reads, not when + * the stack reads the value to build notifications. */ + if (conn_handle != BLE_HS_CONN_HANDLE_NONE) { + current_local_time_val.adjust_reason = 0; + } + return 0; } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return BLE_ATT_ERR_INSUFFICIENT_RES; case BLE_GATT_ACCESS_OP_WRITE_CHR: rc = ble_svc_cts_chr_write(ctxt->om, sizeof(curr_time), sizeof(curr_time), &curr_time, NULL); diff --git a/nimble/host/services/dis/src/ble_svc_dis.c b/nimble/host/services/dis/src/ble_svc_dis.c index f49995b02..4ec4fb9ee 100644 --- a/nimble/host/services/dis/src/ble_svc_dis.c +++ b/nimble/host/services/dis/src/ble_svc_dis.c @@ -293,8 +293,12 @@ ble_svc_dis_access(uint16_t conn_handle, uint16_t attr_handle, { uint8_t sysid[8] = {0}; if (info != NULL) { - /* System ID is 8 bytes binary */ - memcpy(sysid, info, 8); + /* System ID is 8 bytes binary; cap copy length defensively */ + size_t len = strlen(info); + if (len > 8) { + len = 8; + } + memcpy(sysid, info, len); } int rc = os_mbuf_append(ctxt->om, sysid, 8); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; @@ -570,6 +574,7 @@ ble_svc_dis_init_dynamic(void) void ble_svc_dis_deinit(void) { + ble_gatts_free_svcs(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_svc_dis_data_ptr != NULL) { nimble_platform_mem_free(ble_svc_dis_data_ptr); diff --git a/nimble/host/services/gap/src/ble_svc_gap.c b/nimble/host/services/gap/src/ble_svc_gap.c index 364cfa9c7..5d01dadd5 100644 --- a/nimble/host/services/gap/src/ble_svc_gap.c +++ b/nimble/host/services/gap/src/ble_svc_gap.c @@ -237,19 +237,26 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt) ble_hs_unlock(); return BLE_ATT_ERR_UNLIKELY; } - // Free old name if already allocated - if (ble_hs_gap_svc_ctx->svc_gap_name) { - nimble_platform_mem_free(ble_hs_gap_svc_ctx->svc_gap_name); - ble_hs_gap_svc_ctx->svc_gap_name = NULL; + + char *new_name = nimble_platform_mem_calloc(1, om_len + 1); + if (!new_name) { + ble_hs_unlock(); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); + return BLE_ATT_ERR_INSUFFICIENT_RES; } - ble_hs_gap_svc_ctx->svc_gap_name = nimble_platform_mem_calloc(1, om_len + 1); - if (!ble_hs_gap_svc_ctx->svc_gap_name) { + rc = ble_hs_mbuf_to_flat(ctxt->om, new_name, om_len, NULL); + if (rc != 0) { + nimble_platform_mem_free(new_name); ble_hs_unlock(); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + return BLE_ATT_ERR_UNLIKELY; } -#endif + + if (ble_hs_gap_svc_ctx->svc_gap_name) { + nimble_platform_mem_free(ble_hs_gap_svc_ctx->svc_gap_name); + } + ble_hs_gap_svc_ctx->svc_gap_name = new_name; +#else rc = ble_hs_mbuf_to_flat(ctxt->om, ble_svc_gap_name, om_len, NULL); @@ -257,6 +264,7 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt) ble_hs_unlock(); return BLE_ATT_ERR_UNLIKELY; } +#endif ble_svc_gap_name[om_len] = '\0'; @@ -276,6 +284,12 @@ ble_svc_gap_appearance_read_access(struct ble_gatt_access_ctxt *ctxt) int rc; ble_hs_lock(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_gap_svc_ctx) { + ble_hs_unlock(); + return BLE_ATT_ERR_UNLIKELY; + } +#endif appearance = htole16(ble_svc_gap_appearance); ble_hs_unlock(); @@ -300,6 +314,12 @@ ble_svc_gap_appearance_write_access(struct ble_gatt_access_ctxt *ctxt) } ble_hs_lock(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_gap_svc_ctx) { + ble_hs_unlock(); + return BLE_ATT_ERR_UNLIKELY; + } +#endif rc = ble_hs_mbuf_to_flat(ctxt->om, &ble_svc_gap_appearance, om_len, NULL); if (rc != 0) { ble_hs_unlock(); @@ -411,15 +431,28 @@ ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle, #endif #if MYNEWT_VAL(ENC_ADV_DATA) - case BLE_SVC_GAP_CHR_UUID16_KEY_MATERIAL: + case BLE_SVC_GAP_CHR_UUID16_KEY_MATERIAL: { + uint8_t local_session_key[sizeof(ble_svc_gap_km.session_key)]; + uint8_t local_iv[sizeof(ble_svc_gap_km.iv)]; + assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); - rc = os_mbuf_append(ctxt->om, &(ble_svc_gap_km.session_key), sizeof(ble_svc_gap_km.session_key)); + /* Snapshot key material under the host lock to avoid a torn read + * concurrent with ble_svc_gap_device_key_material_set(). + */ + ble_hs_lock(); + memcpy(local_session_key, ble_svc_gap_km.session_key, + sizeof(local_session_key)); + memcpy(local_iv, ble_svc_gap_km.iv, sizeof(local_iv)); + ble_hs_unlock(); + + rc = os_mbuf_append(ctxt->om, local_session_key, sizeof(local_session_key)); if (rc != 0) { return BLE_ATT_ERR_INSUFFICIENT_RES; } - rc = os_mbuf_append(ctxt->om, &(ble_svc_gap_km.iv), sizeof(ble_svc_gap_km.iv)); + rc = os_mbuf_append(ctxt->om, local_iv, sizeof(local_iv)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } #endif #if MYNEWT_VAL(BLE_SVC_GAP_GATT_SECURITY_LEVEL) @@ -569,8 +602,8 @@ ble_svc_gap_device_key_material_set(uint8_t *session_key, uint8_t *iv) memcpy(&ble_svc_gap_km.session_key, session_key, BLE_EAD_KEY_SIZE); memcpy(&ble_svc_gap_km.iv, iv, BLE_EAD_IV_SIZE); - ble_gatts_chr_updated(ble_svc_gap_enc_adv_data_handle); ble_hs_unlock(); + ble_gatts_chr_updated(ble_svc_gap_enc_adv_data_handle); return 0; } #endif diff --git a/nimble/host/services/hid/src/ble_svc_hid.c b/nimble/host/services/hid/src/ble_svc_hid.c index 1fea0357d..5b7b2cddd 100644 --- a/nimble/host/services/hid/src/ble_svc_hid.c +++ b/nimble/host/services/hid/src/ble_svc_hid.c @@ -32,6 +32,7 @@ #include "host/ble_gap.h" #include "host/ble_gatt.h" #include "services/hid/ble_svc_hid.h" +#include "../../../src/ble_hs_priv.h" #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) #include "esp_nimble_mem.h" #endif @@ -335,6 +336,7 @@ fill_boot_mouse_inp(uint16_t instance) chr = ble_svc_hid_get_chr_block(); if (!chr) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); @@ -915,9 +917,12 @@ ble_svc_hid_init(void) /* Ensure this function only gets called by sysinit. */ SYSINIT_ASSERT_ACTIVE(); + ble_hs_lock(); if (!ble_gatts_mutable()) { + ble_hs_unlock(); return; } + ble_hs_unlock(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_svc_hid_static_vars == NULL) { diff --git a/nimble/host/services/htp/src/ble_svc_htp.c b/nimble/host/services/htp/src/ble_svc_htp.c index b805a1611..5aa73c681 100644 --- a/nimble/host/services/htp/src/ble_svc_htp.c +++ b/nimble/host/services/htp/src/ble_svc_htp.c @@ -50,10 +50,12 @@ static uint16_t ble_svc_htp_temp_type_val_handle; static uint16_t ble_svc_htp_intr_temp_val_handle; static uint16_t ble_svc_htp_msr_itvl_val_handle; +#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) static struct { uint16_t conn_handle; struct chr_subscribe chr_subs; } conn_chr_subs[MYNEWT_VAL(BLE_MAX_CONNECTIONS) + 1]; +#endif #define BLE_SVC_HTP_TEMP_MSR_MIN_ITVL 0x0001 #define BLE_SVC_HTP_TEMP_MSR_MAX_ITVL 0xffff @@ -82,7 +84,7 @@ ble_svc_htp_conn_slot(uint16_t conn_handle, int allocate) sizeof(conn_chr_subs[free_slot].chr_subs)); } - return free_slot; + return allocate ? free_slot : -1; } static uint32_t @@ -98,10 +100,10 @@ ble_svc_htp_temp_to_ieee11073(float temp) mantissa = (int32_t)(temp * 100.0f - 0.5f); } - if (mantissa > 0x7fffff) { - mantissa = 0x7fffff; - } else if (mantissa < -0x800000) { - mantissa = -0x800000; + if (mantissa > 0x7ffffb) { + mantissa = 0x7ffffb; + } else if (mantissa < -0x7ffffd) { + mantissa = -0x7ffffd; } return ((uint32_t)exponent << 24) | ((uint32_t)mantissa & 0x00ffffff); diff --git a/nimble/host/services/lls/src/ble_svc_lls.c b/nimble/host/services/lls/src/ble_svc_lls.c index a69c04eee..b92c2a235 100644 --- a/nimble/host/services/lls/src/ble_svc_lls.c +++ b/nimble/host/services/lls/src/ble_svc_lls.c @@ -141,8 +141,9 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, * function when a BLE_GAP_EVENT_DISCONNECT event is received and * pass the disconnect reason into this function. * - * Here, we then check if the disconnect reason is due to a timout, and if - * so, we call the ble_svc_lls_event_fn callback with the current + * Here, we check if the disconnect reason indicates an unintentional link + * loss (not an explicit termination by the local host or remote user), + * and if so, call the ble_svc_lls_event_fn callback with the current * alert level. The actual alert implementation is left up to the * developer. * @@ -152,10 +153,10 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, void ble_svc_lls_on_gap_disconnect(int reason) { - if (reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_SPVN_TMO) || - reason == BLE_HS_HCI_ERR(BLE_ERR_LMP_LL_RSP_TMO) || - reason == BLE_HS_HCI_ERR(BLE_ERR_INSTANT_PASSED) || - reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_TERM_MIC)) { + if (reason != BLE_HS_HCI_ERR(BLE_ERR_REM_USER_CONN_TERM) && + reason != BLE_HS_HCI_ERR(BLE_ERR_RD_CONN_TERM_RESRCS) && + reason != BLE_HS_HCI_ERR(BLE_ERR_RD_CONN_TERM_PWROFF) && + reason != BLE_HS_HCI_ERR(BLE_ERR_CONN_TERM_LOCAL)) { if (ble_svc_lls_cb_fn != NULL) { ble_svc_lls_cb_fn(ble_svc_lls_alert_level); } diff --git a/nimble/host/services/prox/src/ble_svc_prox.c b/nimble/host/services/prox/src/ble_svc_prox.c index 98fe84279..7be504b7a 100644 --- a/nimble/host/services/prox/src/ble_svc_prox.c +++ b/nimble/host/services/prox/src/ble_svc_prox.c @@ -19,6 +19,11 @@ #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_PROX_SERVICE +#define BLE_SVC_PROX_ALERT_NONE 0 +#define BLE_SVC_PROX_ALERT_MILD 1 +#define BLE_SVC_PROX_ALERT_HIGH 2 +#define BLE_SVC_PROX_CONN_HANDLE_NONE 0xffff + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) typedef struct { @@ -43,11 +48,6 @@ static ble_svc_prox_ctx_t * ble_svc_prox_ctx = NULL; static uint8_t ble_svc_prox_alert; static int8_t ble_svc_prox_tx_pwr_lvl; -#define BLE_SVC_PROX_ALERT_NONE 0 -#define BLE_SVC_PROX_ALERT_MILD 1 -#define BLE_SVC_PROX_ALERT_HIGH 2 -#define BLE_SVC_PROX_CONN_HANDLE_NONE 0xffff - #endif /* MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) */ /* Characteristic value handles */ @@ -140,7 +140,7 @@ ble_svc_prox_conn_slot(uint16_t conn_handle, int allocate) ble_svc_prox_conn[free_slot].immediate_alert = false; } - return free_slot; + return allocate ? free_slot : -1; } static const struct ble_gatt_svc_def ble_svc_prox_defs[] = { @@ -457,14 +457,20 @@ ble_svc_prox_init(void) ble_svc_prox_conn[i].immediate_alert = false; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_prox_ctx->_ble_prox_task_handle == NULL) { + BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", + 4096, NULL, 10, + &ble_svc_prox_ctx->_ble_prox_task_handle); + SYSINIT_PANIC_ASSERT(ret == pdPASS); + } +#else static TaskHandle_t ble_prox_task_handle; if (ble_prox_task_handle == NULL) { BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", 4096, NULL, 10, &ble_prox_task_handle); SYSINIT_PANIC_ASSERT(ret == pdPASS); } - - BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", 4096, NULL, 10, &ble_prox_task_handle); - SYSINIT_PANIC_ASSERT(ret == pdPASS); +#endif } #endif diff --git a/nimble/host/services/ras/src/ble_svc_ras.c b/nimble/host/services/ras/src/ble_svc_ras.c index 7d7fa6dab..3c4384c8b 100644 --- a/nimble/host/services/ras/src/ble_svc_ras.c +++ b/nimble/host/services/ras/src/ble_svc_ras.c @@ -425,22 +425,21 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl ble_svc_ras_od_rd_seg_len = 0; } /* Reset the ranging buffers */ - ble_svc_ras_cp_val[0]= 0x02; + ble_svc_ras_cp_val[0] = RASCP_RSP_OPCODE_RSP_CODE; /*Table 3.12. Response Code Values associated with Op Code 0x02*/ - ble_svc_ras_cp_val[1]=0x01; // Success - ble_svc_ras_cp_val[2]=0x00; /* Clear stale 3rd byte */ + ble_svc_ras_cp_val[1]=0x01; /* RASCP_OPCODE_ACK_RD (Request Op Code) */ + ble_svc_ras_cp_val[2]=0x01; /* Success */ ble_gatts_chr_updated(ble_svc_ras_cp_val_handle); MODLOG_DFLT(INFO, "Successfully completed the Ranging procedure\n"); for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) { - if (ranging_buffers[i].conn == conn_handle && ranging_buffers[i].isacked == false) { + if (ranging_buffers[i].conn == conn_handle) { ranging_buffers[i].conn = -1; /* Release buffer */ ranging_buffers[i].isacked = false; ranging_buffers[i].isbusy = false; ranging_buffers[i].isready = false; ranging_buffers[i].ranging_counter = 0; ranging_buffers[i].subevent_cursor = 0; - break; } } @@ -523,7 +522,11 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) { buf->ranging_data.ranging_header.ranging_counter = ranging_subevent.subev_result.procedure_counter; // buf->ranging_data.ranging_header.selected_tx_power = ranging_subevent.subev_result.selected_tx_power; uint8_t num_paths = ranging_subevent.subev_result.num_antenna_paths; - buf->ranging_data.ranging_header.antenna_paths_mask = (num_paths > 0) ? ((1 << num_paths) - 1) : 0; + if (num_paths > 4) { + MODLOG_DFLT(WARN, "Unexpected num_antenna_paths: %d, capping to 4\n", num_paths); + num_paths = 4; + } + buf->ranging_data.ranging_header.antenna_paths_mask = (num_paths > 0) ? ((1u << num_paths) - 1) : 0; uint16_t max_subevent_data = BLE_RAS_PROCEDURE_MEM - sizeof(struct ranging_header); diff --git a/nimble/host/services/tps/src/ble_svc_tps.c b/nimble/host/services/tps/src/ble_svc_tps.c index 59f0661c4..d2bd4b1aa 100644 --- a/nimble/host/services/tps/src/ble_svc_tps.c +++ b/nimble/host/services/tps/src/ble_svc_tps.c @@ -70,6 +70,8 @@ static int ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { + struct ble_hci_cb_read_tx_pwr_cp cmd; + struct ble_hci_cb_read_tx_pwr_rp rsp; int8_t tx_power_level; int rc; @@ -77,14 +79,15 @@ ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle, switch (ctxt->op) { case BLE_GATT_ACCESS_OP_READ_CHR: - /* Returns advertising TX power as an approximation of the connection - * TX power level. A spec-compliant implementation would use - * HCI_Read_Transmit_Power_Level with conn_handle, which requires an - * async HCI exchange not possible inside a synchronous GATT callback. */ - rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_power_level); + cmd.conn_handle = htole16(conn_handle); + cmd.type = 0x01; + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_CTLR_BASEBAND, + BLE_HCI_OCF_CB_READ_TX_PWR), + &cmd, sizeof(cmd), &rsp, sizeof(rsp)); if (rc != 0) { return BLE_ATT_ERR_UNLIKELY; } + tx_power_level = rsp.tx_level; rc = os_mbuf_append(ctxt->om, &tx_power_level, sizeof tx_power_level); diff --git a/nimble/host/src/ble_aes_ccm.c b/nimble/host/src/ble_aes_ccm.c index 93d3ccb2f..808ddd8d3 100644 --- a/nimble/host/src/ble_aes_ccm.c +++ b/nimble/host/src/ble_aes_ccm.c @@ -138,6 +138,17 @@ ble_aes_ccm_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *en #else #include #include + +static void +ble_aes_ccm_secure_zero(void *buf, size_t len) +{ + volatile uint8_t *p = buf; + + while (len--) { + *p++ = 0; + } +} + int ble_aes_ccm_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data) { @@ -158,7 +169,7 @@ ble_aes_ccm_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *en done: /* Securely erase sensitive key material from stack */ - memset(&s, 0, sizeof(s)); + ble_aes_ccm_secure_zero(&s, sizeof(s)); return rc; } #endif @@ -375,9 +386,12 @@ int ble_aes_ccm_decrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t return rc; } - /* Compare calculated MIC with received MIC */ - if (memcmp(mic, enc_msg + msg_len, mic_size) != 0) { - /* Zero out the decrypted plaintext on authentication failure */ + /* Compare calculated MIC with received MIC in constant time. */ + uint8_t diff = 0; + for (size_t i = 0; i < mic_size; i++) { + diff |= mic[i] ^ enc_msg[msg_len + i]; + } + if (diff != 0) { memset(out_msg, 0, msg_len); return BLE_HS_EAUTHEN; } diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c index af582bba6..30b0be19b 100644 --- a/nimble/host/src/ble_att.c +++ b/nimble/host/src/ble_att.c @@ -37,6 +37,9 @@ #define max(a, b) ((a) > (b) ? (a) : (b)) #endif +/* Core Spec Vol 3 Part G 5.3.1: minimum ATT_MTU for Enhanced ATT bearers */ +#define BLE_ATT_EATT_MIN_MTU 64 + static uint16_t ble_att_preferred_mtu_val; /** Dispatch table for incoming ATT requests. */ @@ -551,7 +554,7 @@ ble_att_chan_mtu(const struct ble_l2cap_chan *chan) * MTU field values of the two devices. Reference: * Core v5.4 Vol 3 Part G 5.3.1 ATT_MTU */ - return max(min(chan->coc_tx.mtu, chan->coc_rx.mtu), 64); + return max(min(chan->coc_tx.mtu, chan->coc_rx.mtu), BLE_ATT_EATT_MIN_MTU); } #endif @@ -592,8 +595,24 @@ ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle, return; } - if (op == BLE_ATT_OP_NOTIFY_REQ || op == BLE_ATT_OP_INDICATE_REQ || - op == BLE_ATT_OP_NOTIFY_MULTI_REQ) { + if (op == BLE_ATT_OP_INDICATE_REQ) { + struct os_mbuf *txom; + + os_mbuf_free_chain(*om); + *om = NULL; + + txom = ble_hs_mbuf_l2cap_pkt(); + if (txom == NULL) { + return; + } + if (ble_att_cmd_prepare(BLE_ATT_OP_INDICATE_RSP, 0, txom) == NULL) { + return; + } + ble_att_tx(conn_handle, cid, txom); + return; + } + + if (op == BLE_ATT_OP_NOTIFY_REQ || op == BLE_ATT_OP_NOTIFY_MULTI_REQ) { return; } diff --git a/nimble/host/src/ble_att_clt.c b/nimble/host/src/ble_att_clt.c index 244946720..ae076d72c 100644 --- a/nimble/host/src/ble_att_clt.c +++ b/nimble/host/src/ble_att_clt.c @@ -83,6 +83,7 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu) } else if (chan->flags & BLE_L2CAP_CHAN_F_TXED_MTU) { rc = BLE_HS_EALREADY; } else { + chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU; rc = 0; } ble_hs_unlock(); @@ -105,13 +106,6 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu) req->bamc_mtu = htole16(mtu); - ble_hs_lock(); - rc = ble_att_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT, &conn, &chan); - if (rc == 0) { - chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU; - } - ble_hs_unlock(); - rc = ble_att_tx(conn_handle, BLE_L2CAP_CID_ATT, txom); if (rc != 0) { /* Rollback the flag so future MTU exchanges are not blocked */ @@ -620,10 +614,10 @@ ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid, req->handles[i] = htole16(handles[i]); } - if (OS_MBUF_PKTLEN(txom) > ble_att_mtu_by_cid(conn_handle, cid)) { - os_mbuf_free_chain(txom); - return BLE_HS_EMSGSIZE; - } + /* MTU was already validated before allocation above; a second check here + * would only race with a concurrent MTU exchange and reject a packet that + * has already passed validation. + */ return ble_att_tx(conn_handle, cid, txom); } @@ -905,6 +899,11 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han goto err; } + if (counter == UINT32_MAX) { + rc = BLE_HS_EINVAL; + goto err; + } + /** Copying sign counter */ counter_le = htole32(counter); memcpy(&message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET + payload_len], diff --git a/nimble/host/src/ble_att_cmd.c b/nimble/host/src/ble_att_cmd.c index d68632a05..857d66286 100644 --- a/nimble/host/src/ble_att_cmd.c +++ b/nimble/host/src/ble_att_cmd.c @@ -27,6 +27,10 @@ #include "host/ble_hs_log.h" #if NIMBLE_BLE_CONNECT +/** + * Prepares an ATT command for transmission. On failure, the txom mbuf is + * freed and NULL is returned. + */ void * ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom) { @@ -246,7 +250,10 @@ ble_att_mtu_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_MTU_REQ, payload, BLE_ATT_MTU_CMD_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bamc_mtu = le16toh(src->bamc_mtu); } @@ -258,7 +265,10 @@ ble_att_mtu_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_MTU_RSP, payload, BLE_ATT_MTU_CMD_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bamc_mtu = le16toh(src->bamc_mtu); } @@ -294,7 +304,10 @@ ble_att_find_info_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_REQ, payload, BLE_ATT_FIND_INFO_REQ_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bafq_start_handle = le16toh(src->bafq_start_handle); dst->bafq_end_handle = le16toh(src->bafq_end_handle); } @@ -320,7 +333,10 @@ ble_att_find_info_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_RSP, payload, BLE_ATT_FIND_INFO_RSP_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bafp_format = src->bafp_format; } @@ -344,7 +360,10 @@ ble_att_find_type_value_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, payload, BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bavq_start_handle = le16toh(src->bavq_start_handle); dst->bavq_end_handle = le16toh(src->bavq_end_handle); dst->bavq_attr_type = le16toh(src->bavq_attr_type); @@ -372,7 +391,10 @@ ble_att_read_type_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_REQ, payload, BLE_ATT_READ_TYPE_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->batq_start_handle = le16toh(src->batq_start_handle); dst->batq_end_handle = le16toh(src->batq_end_handle); } @@ -398,7 +420,10 @@ ble_att_read_type_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_RSP, payload, BLE_ATT_READ_TYPE_RSP_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->batp_length = src->batp_length; } @@ -422,7 +447,10 @@ ble_att_read_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_REQ, payload, BLE_ATT_READ_REQ_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->barq_handle = le16toh(src->barq_handle); } @@ -446,7 +474,10 @@ ble_att_read_blob_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_BLOB_REQ, payload, BLE_ATT_READ_BLOB_REQ_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->babq_handle = le16toh(src->babq_handle); dst->babq_offset = le16toh(src->babq_offset); } @@ -500,7 +531,10 @@ ble_att_read_group_type_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload, BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bagq_start_handle = le16toh(src->bagq_start_handle); dst->bagq_end_handle = le16toh(src->bagq_end_handle); } @@ -526,7 +560,10 @@ ble_att_read_group_type_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload, BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bagp_length = src->bagp_length; } @@ -550,7 +587,10 @@ ble_att_write_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_WRITE_REQ, payload, BLE_ATT_WRITE_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bawq_handle = le16toh(src->bawq_handle); } @@ -562,7 +602,10 @@ ble_att_write_cmd_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_WRITE_CMD, payload, BLE_ATT_WRITE_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bawq_handle = le16toh(src->bawq_handle); } @@ -598,7 +641,10 @@ ble_att_prep_write_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_REQ, payload, BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bapc_handle = le16toh(src->bapc_handle); dst->bapc_offset = le16toh(src->bapc_offset); } @@ -624,7 +670,10 @@ ble_att_prep_write_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_RSP, payload, BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->bapc_handle = le16toh(src->bapc_handle); dst->bapc_offset = le16toh(src->bapc_offset); } @@ -650,7 +699,10 @@ ble_att_exec_write_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_REQ, payload, BLE_ATT_EXEC_WRITE_REQ_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->baeq_flags = src->baeq_flags; } @@ -688,7 +740,10 @@ ble_att_notify_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_NOTIFY_REQ, payload, BLE_ATT_NOTIFY_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->banq_handle = le16toh(src->banq_handle); } @@ -712,7 +767,10 @@ ble_att_indicate_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_INDICATE_REQ, payload, BLE_ATT_INDICATE_REQ_BASE_SZ, len); - if (src == NULL) { return; } + if (src == NULL) { + memset(dst, 0, sizeof *dst); + return; + } dst->baiq_handle = le16toh(src->baiq_handle); } diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c index 0e624dbad..07fd3955f 100644 --- a/nimble/host/src/ble_att_svr.c +++ b/nimble/host/src/ble_att_svr.c @@ -1205,7 +1205,6 @@ ble_att_svr_fill_type_value(uint16_t conn_handle, struct ble_att_svr_entry *ha; uint8_t buf[16]; uint16_t attr_len; - uint16_t req_val_len; uint16_t first; uint16_t prev; int any_entries; @@ -1214,11 +1213,6 @@ ble_att_svr_fill_type_value(uint16_t conn_handle, first = 0; prev = 0; rc = 0; - attr_om = NULL; - - /* Length of the attribute value in the request. */ - req_val_len = OS_MBUF_PKTLEN(rxom) - - sizeof(struct ble_att_find_type_value_req); /* Iterate through the attribute list, keeping track of the current * matching group. For each attribute entry, determine if data needs to be @@ -1290,8 +1284,6 @@ ble_att_svr_fill_type_value(uint16_t conn_handle, } done: - os_mbuf_free_chain(attr_om); - any_entries = OS_MBUF_PKTHDR(txom)->omp_len > BLE_ATT_FIND_TYPE_VALUE_RSP_BASE_SZ; if (rc == 0 && !any_entries) { @@ -1413,6 +1405,9 @@ static void ble_att_svr_make_conn_aware(uint16_t conn_handle) { conn->bhc_gatt_svr.half_aware = 1; ble_hs_conn_addrs(conn, &addrs); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gatts_conn_aware_states != NULL) +#endif for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { @@ -1435,6 +1430,9 @@ static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) { conn->bhc_gatt_svr.aware_state = true; ble_hs_conn_addrs(conn, &addrs); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gatts_conn_aware_states != NULL) +#endif for(int i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { @@ -1467,10 +1465,9 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid, struct ble_att_read_type_rsp *rsp; struct ble_att_svr_entry *entry; struct os_mbuf *txom; - struct os_mbuf *attr_om; uint16_t attr_len; - uint16_t max_attr_len; uint16_t mtu; + uint8_t buf[19]; int entry_written; int txomlen; int prev_attr_len; @@ -1501,12 +1498,6 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid, mtu = ble_att_mtu_by_cid(conn_handle, cid); - /* Per Core Spec: max attribute value length is min(ATT_MTU - 4, 253). */ - max_attr_len = mtu - 4; - if (max_attr_len > 253) { - max_attr_len = 253; - } - /* Find all matching attributes, writing a record for each. */ entry = NULL; while (1) { @@ -1524,9 +1515,8 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid, goto done; } - attr_len = OS_MBUF_PKTLEN(attr_om); - if (attr_len > max_attr_len) { - attr_len = max_attr_len; + if (attr_len > mtu - 4) { + attr_len = mtu - 4; } if (prev_attr_len == 0) { @@ -1549,14 +1539,12 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid, } data->handle = htole16(entry->ha_handle_id); - os_mbuf_copydata(attr_om, 0, attr_len, data->value); + memcpy(data->value, buf, attr_len); entry_written = 1; } } done: - os_mbuf_free_chain(attr_om); - if (!entry_written) { /* No matching attributes. */ if (*att_err == 0) { @@ -1649,6 +1637,9 @@ ble_att_svr_rx_read_type(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx conn->bhc_gatt_svr.half_aware = 0; ble_hs_conn_addrs(conn, &addrs); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gatts_conn_aware_states != NULL) +#endif for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { @@ -1734,6 +1725,7 @@ ble_att_svr_rx_read(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) if (ble_att_cmd_prepare(BLE_ATT_OP_READ_RSP, 0, txom) == NULL) { att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1799,6 +1791,7 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx if (ble_att_cmd_prepare(BLE_ATT_OP_READ_BLOB_RSP, 0, txom) == NULL) { att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1840,6 +1833,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; } @@ -1946,6 +1940,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; } @@ -1989,13 +1984,17 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid, uint16_t le_len = htole16(tuple_len); rc = os_mbuf_append(txom, &le_len, sizeof(le_len)); if (rc != 0) { + *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; *err_handle = handle; + rc = BLE_HS_ENOMEM; goto done; } if (tuple_len != 0) { rc = os_mbuf_appendfrom(txom, tmp, 0, tuple_len); if (rc != 0) { + *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; *err_handle = handle; + rc = BLE_HS_ENOMEM; goto done; } if (tuple_len != 0) { @@ -2598,8 +2597,12 @@ ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf * /* Strip the signature from the end of the mbuf. */ os_mbuf_adj(*rxom, -(BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ - BLE_ATT_SIGNED_WRITE_DATA_OFFSET)); + /* Extract received sign counter from the signature bytes */ + uint32_t received_sign_counter; + memcpy(&received_sign_counter, sign, sizeof(received_sign_counter)); + /* Authentication procedure */ - len = OS_MBUF_PKTLEN(*rxom) + sizeof(value_sec.sign_counter) + 1; + len = OS_MBUF_PKTLEN(*rxom) + sizeof(received_sign_counter) + 1; message = nimble_platform_mem_calloc(1,len); if (message == NULL) { rc = BLE_HS_ENOMEM; @@ -2608,7 +2611,7 @@ ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf * message[0] = BLE_ATT_OP_SIGNED_WRITE_CMD; os_mbuf_copydata(*rxom, 0, OS_MBUF_PKTLEN(*rxom), &message[1]); - memcpy(&message[1 + OS_MBUF_PKTLEN(*rxom)], &value_sec.sign_counter, sizeof(value_sec.sign_counter)); + memcpy(&message[1 + OS_MBUF_PKTLEN(*rxom)], &received_sign_counter, sizeof(received_sign_counter)); /* Converting message into little endian format */ swap_in_place(message, len); @@ -2626,12 +2629,10 @@ ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf * /* Converting cmac to little endian */ swap_in_place(cmac, sizeof cmac); - /* Extract received sign counter from the signature bytes */ - uint32_t received_sign_counter; - memcpy(&received_sign_counter, sign, sizeof(received_sign_counter)); - - /* Comparing sign counter — received must be > stored to prevent replay attacks */ - if (received_sign_counter <= value_sec.sign_counter) { + /* Comparing sign counter — received must be > stored to prevent replay + * attacks. UINT32_MAX means no signed message received yet. */ + if (value_sec.sign_counter != UINT32_MAX && + received_sign_counter <= value_sec.sign_counter) { rc = BLE_HS_EAUTHEN; goto err; } @@ -3089,6 +3090,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; } diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c index 85951352b..e8fe12a63 100644 --- a/nimble/host/src/ble_cs.c +++ b/nimble/host/src/ble_cs.c @@ -370,7 +370,9 @@ ble_cs_wr_cached_rem_supp_cap(const struct ble_cs_wr_cached_rem_supp_cap_cp *cmd BLE_HCI_OCF_LE_CS_WR_CACHED_REM_SUPP_CAP), &cp, sizeof(cp), &rp, sizeof(rp)); - rsp->conn_handle = le16toh(rp.conn_handle); + if (rc == 0) { + rsp->conn_handle = le16toh(rp.conn_handle); + } return rc; } @@ -571,7 +573,8 @@ ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data if (ev->status) { BLE_HS_LOG(ERROR, "%s status error=%d\n", __func__, ev->status); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), + BLE_HS_HCI_ERR(ev->status)); return BLE_HS_ECONTROLLER; } @@ -714,7 +717,8 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, BLE_HS_LOG(INFO, "Remote has No_FAE, continuing to config"); } else if (ev->status) { BLE_HS_LOG(ERROR, "FAE read failed with status 0x%02x, aborting", ev->status); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), BLE_HS_ECONTROLLER); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), + BLE_HS_HCI_ERR(ev->status)); return BLE_HS_ECONTROLLER; } @@ -749,6 +753,7 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, rc = ble_gap_conn_find(cmd.conn_handle, &desc); if (rc != 0) { + ble_cs_call_procedure_complete_cb(cmd.conn_handle, rc); return rc; } @@ -780,7 +785,8 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, if (desc.role == BLE_GAP_ROLE_MASTER) { rc = ble_cs_create_config(&cmd); if (rc) { - BLE_HS_LOG(INFO, "Failed to create CS config"); + BLE_HS_LOG(ERROR, "Failed to create CS config, rc=%d", rc); + ble_cs_call_procedure_complete_cb(cmd.conn_handle, rc); } } return rc; @@ -804,7 +810,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, if (ev->status) { BLE_HS_LOG(INFO, "Failed to enable CS security status=%d", ev->status); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), BLE_HS_HCI_ERR(ev->status)); return BLE_HS_ECONTROLLER; } @@ -895,10 +901,15 @@ ble_hs_hci_evt_le_cs_config_complete(uint8_t subevent, const void *data, if (ev->status) { BLE_HS_LOG(ERROR, "%s status error=%d\n", __func__, ev->status); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), BLE_HS_HCI_ERR(ev->status)); return BLE_HS_ECONTROLLER; } + if (ev->action != 0x01) { + BLE_HS_LOG(DEBUG, "CS config removed or invalid action, skipping security enable"); + return 0; + } + cmd.conn_handle = le16toh(ev->conn_handle); /* Exchange CS security keys */ @@ -909,17 +920,13 @@ ble_hs_hci_evt_le_cs_config_complete(uint8_t subevent, const void *data, return rc; } - if (ev->action != 0x01) { - BLE_HS_LOG(DEBUG, "CS config removed or invalid action, skipping security enable"); - return 0; - } - if (desc.role == BLE_GAP_ROLE_MASTER) { rc = ble_cs_sec_enable(&cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to enable CS security"); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), BLE_ERR_UNSPECIFIED); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), + BLE_HS_HCI_ERR(BLE_ERR_UNSPECIFIED)); return rc; } } @@ -940,7 +947,7 @@ ble_hs_hci_evt_le_cs_proc_enable_complete(uint8_t subevent, const void *data, if (ev->status) { BLE_HS_LOG(ERROR, "%s status error=%d\n", __func__, ev->status); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), BLE_HS_HCI_ERR(ev->status)); return BLE_HS_ECONTROLLER; } diff --git a/nimble/host/src/ble_ead.c b/nimble/host/src/ble_ead.c index c55015016..0227cda55 100644 --- a/nimble/host/src/ble_ead.c +++ b/nimble/host/src/ble_ead.c @@ -119,8 +119,8 @@ int ble_ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i } if (payload_size == 0) { - BLE_HS_LOG(DEBUG, "payload_size is set to 0. The encrypted result will only contain the " - "Randomizer and the MIC."); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; } /* Ensure payload_size isn't too large to wrap around when adding overhead */ @@ -140,7 +140,7 @@ static int ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_ uint8_t nonce[BLE_EAD_NONCE_SIZE]; /* Defense-in-depth: Validate size to prevent underflow (size_t is unsigned) */ - if (encrypted_payload_size < BLE_EAD_RANDOMIZER_SIZE + BLE_EAD_MIC_SIZE) { + if (encrypted_payload_size < BLE_EAD_RANDOMIZER_SIZE + BLE_EAD_MIC_SIZE + 1) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -196,8 +196,8 @@ int ble_ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i return BLE_HS_EINVAL; } - /* The underlying ead_decrypt already validates minimum size (Randomizer + MIC). - * BT spec allows empty AD structure sequences, so we don't enforce +2 bytes. */ + /* The underlying ead_decrypt already validates minimum size (Randomizer + MIC + 1). + * 1 is added because BT spec mandates that the plaintext shall consist of one or more AD structures. (CSS Part A Section 1.23.2) */ return ead_decrypt(session_key, iv, encrypted_payload, encrypted_payload_size, payload); } diff --git a/nimble/host/src/ble_eatt.c b/nimble/host/src/ble_eatt.c index b7ef9c82b..e60514cab 100644 --- a/nimble/host/src/ble_eatt.c +++ b/nimble/host/src/ble_eatt.c @@ -195,7 +195,10 @@ ble_eatt_wakeup_cb(struct ble_npl_event *ev) struct os_mbuf *txom; struct os_mbuf_pkthdr *omp; struct ble_l2cap_chan_info info; - int rc; + struct ble_l2cap_chan *chan; + uint16_t conn_handle; + uint16_t cid; + int rc = 0; eatt = ble_npl_event_get_arg(ev); assert(eatt); @@ -211,17 +214,36 @@ ble_eatt_wakeup_cb(struct ble_npl_event *ev) txom = OS_MBUF_PKTHDR_TO_MBUF(omp); ble_l2cap_get_chan_info(eatt->chan, &info); - - /* We need to unlock before calling ble_eatt_tx because it might lock again or trigger callbacks */ + chan = eatt->chan; + conn_handle = eatt->conn_handle; + cid = info.scid; + ble_hs_unlock(); - rc = ble_eatt_tx(eatt->conn_handle, info.scid, txom); + rc = ble_l2cap_send(chan, txom); ble_hs_lock(); - /* Break if channel is stalled or busy - both will reschedule wakeup */ - if (rc == BLE_HS_ESTALLED || rc == BLE_HS_EBUSY) { + /* Re-validate channel after lock drop: disconnect handler may have freed eatt */ + eatt = ble_eatt_find(conn_handle, cid); + if (eatt == NULL) { + if (rc == BLE_HS_EBUSY || (rc != 0 && rc != BLE_HS_ESTALLED)) { + os_mbuf_free_chain(txom); + } break; } - /* On success (0) or other errors: continue to next packet */ + + if (rc == BLE_HS_EBUSY) { + STAILQ_INSERT_HEAD(&eatt->eatt_tx_q, OS_MBUF_PKTHDR(txom), omp_next); + ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); + break; + } + + if (rc == BLE_HS_ESTALLED) { + break; + } + + if (rc != 0) { + os_mbuf_free_chain(txom); + } } ble_hs_unlock(); } @@ -258,7 +280,9 @@ ble_eatt_free_nolock(struct ble_eatt *eatt) struct os_mbuf_pkthdr *omp; ble_npl_eventq_remove(ble_hs_evq_get(), &eatt->setup_ev); + ble_npl_event_deinit(&eatt->setup_ev); ble_npl_eventq_remove(ble_hs_evq_get(), &eatt->wakeup_ev); + ble_npl_event_deinit(&eatt->wakeup_ev); while ((omp = STAILQ_FIRST(&eatt->eatt_tx_q)) != NULL) { STAILQ_REMOVE_HEAD(&eatt->eatt_tx_q, omp_next); @@ -272,9 +296,9 @@ ble_eatt_free_nolock(struct ble_eatt *eatt) static void ble_eatt_free(struct ble_eatt *eatt) { - ble_hs_lock(); + ble_hs_lock_nested(); ble_eatt_free_nolock(eatt); - ble_hs_unlock(); + ble_hs_unlock_nested(); } static int @@ -410,14 +434,6 @@ ble_eatt_setup_cb(struct ble_npl_event *ev) if (rc) { BLE_EATT_LOG_ERROR("eatt: Failed to connect EATT on conn_handle 0x%04x (status=%d)\n", eatt->conn_handle, rc); - /* ble_l2cap_enhanced_connect only frees the mbuf if it was consumed. - * If rc is non-zero, it means it failed before consuming it in most cases, - * but we need to be careful about double-free. - * The bug report says it double-frees because ble_l2cap_enhanced_connect - * already frees it on many error paths. */ - if (om) { - os_mbuf_free_chain(om); - } ble_eatt_free(eatt); } } @@ -593,10 +609,13 @@ ble_eatt_get_available_chan_cid(uint16_t conn_handle, uint8_t op) default_cid = ble_att_get_default_bearer_cid(conn_handle); if (default_cid) { eatt = ble_eatt_find(conn_handle, default_cid); + if (eatt && eatt->client_op) { + eatt = ble_eatt_find_not_busy(conn_handle); + } } else { eatt = ble_eatt_find_not_busy(conn_handle); } - if (!eatt || eatt->client_op != 0) { + if (!eatt) { cid = BLE_L2CAP_CID_ATT; goto done; } @@ -646,28 +665,32 @@ ble_eatt_tx(uint16_t conn_handle, uint16_t cid, struct os_mbuf *txom) } ble_att_truncate_to_mtu(eatt->chan, txom); - rc = ble_l2cap_send(eatt->chan, txom); + struct ble_l2cap_chan *chan = eatt->chan; + ble_hs_unlock(); + rc = ble_l2cap_send(chan, txom); if (rc == 0) { - ble_hs_unlock(); goto done; } if (rc == BLE_HS_ESTALLED) { BLE_EATT_LOG_DEBUG("ble_eatt_tx: Eatt stalled"); - /* L2CAP owns the mbuf; COC_TX_UNSTALLED event will fire when ready */ - /* Do NOT re-queue - this would cause use-after-free */ - ble_hs_unlock(); return rc; } else if (rc == BLE_HS_EBUSY) { BLE_EATT_LOG_DEBUG("ble_eatt_tx: Message queued"); - STAILQ_INSERT_HEAD(&eatt->eatt_tx_q, OS_MBUF_PKTHDR(txom), omp_next); - ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); - ble_hs_unlock(); - return 0; + ble_hs_lock(); + eatt = ble_eatt_find(conn_handle, cid); + if (eatt) { + STAILQ_INSERT_HEAD(&eatt->eatt_tx_q, OS_MBUF_PKTHDR(txom), omp_next); + ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); + ble_hs_unlock(); + return 0; + } else { + os_mbuf_free_chain(txom); + ble_hs_unlock(); + return BLE_HS_ENOENT; + } } else { BLE_EATT_LOG_ERROR("eatt: %s, ERROR %d ", __func__, rc); - /* Free mbuf and return error gracefully instead of crashing */ - ble_hs_unlock(); os_mbuf_free_chain(txom); return rc; } @@ -836,6 +859,8 @@ err: nimble_platform_mem_free(ble_eatt_conn_mem); ble_eatt_conn_mem = NULL; #endif + os_mempool_unregister(&ble_eatt_sdu_mbuf_mempool); + os_mempool_unregister(&ble_eatt_conn_pool); nimble_platform_mem_free(ble_eatt_ctx); ble_eatt_ctx = NULL; #endif diff --git a/nimble/host/src/ble_eddystone.c b/nimble/host/src/ble_eddystone.c index 49fa62b55..3d0350247 100644 --- a/nimble/host/src/ble_eddystone.c +++ b/nimble/host/src/ble_eddystone.c @@ -116,11 +116,16 @@ ble_eddystone_set_svc_data_base(uint8_t frame_type) */ static int ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields, + const uint8_t *svc_data, uint8_t svc_data_len) { + struct ble_hs_adv_fields fields; + ble_uuid16_t uuids16[BLE_EDDYSTONE_MAX_UUIDS16 + 1]; int rc; - if (adv_fields->num_uuids16 > BLE_EDDYSTONE_MAX_UUIDS16) { + fields = *adv_fields; + + if (fields.num_uuids16 > BLE_EDDYSTONE_MAX_UUIDS16) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -128,36 +133,35 @@ ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields, BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } - if (adv_fields->num_uuids16 > 0 && !adv_fields->uuids16_is_complete) { + if (fields.num_uuids16 > 0 && !fields.uuids16_is_complete) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } - if (adv_fields->svc_data_uuid16_len != 0) { + if (fields.svc_data_uuid16_len != 0) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } - ble_eddystone_uuids16[0] = - (ble_uuid16_t) BLE_UUID16_INIT(BLE_EDDYSTONE_SERVICE_UUID); + uuids16[0] = (ble_uuid16_t) BLE_UUID16_INIT(BLE_EDDYSTONE_SERVICE_UUID); /* Only copy if there are UUIDs and the pointer is valid */ - if (adv_fields->num_uuids16 > 0) { - if (adv_fields->uuids16 == NULL) { + if (fields.num_uuids16 > 0) { + if (fields.uuids16 == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } - memmove(ble_eddystone_uuids16 + 1, adv_fields->uuids16, - adv_fields->num_uuids16 * sizeof(ble_uuid16_t)); + memmove(uuids16 + 1, fields.uuids16, + fields.num_uuids16 * sizeof(ble_uuid16_t)); } - adv_fields->uuids16 = ble_eddystone_uuids16; - adv_fields->num_uuids16++; - adv_fields->uuids16_is_complete = 1; + fields.uuids16 = uuids16; + fields.num_uuids16++; + fields.uuids16_is_complete = 1; - adv_fields->svc_data_uuid16 = ble_eddystone_svc_data; - adv_fields->svc_data_uuid16_len = svc_data_len + + fields.svc_data_uuid16 = svc_data; + fields.svc_data_uuid16_len = svc_data_len + BLE_EDDYSTONE_SVC_DATA_BASE_SZ; - rc = ble_gap_adv_set_fields(adv_fields); + rc = ble_gap_adv_set_fields(&fields); if (rc != 0) { return rc; } @@ -169,6 +173,7 @@ int ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, const void *uid, int8_t measured_power) { + uint8_t svc_data_copy[BLE_EDDYSTONE_MAX_SVC_DATA_LEN]; uint8_t *svc_data; int rc; @@ -208,9 +213,13 @@ ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, svc_data[1 + BLE_EDDYSTONE_UID_LEN] = 0x00; svc_data[1 + BLE_EDDYSTONE_UID_LEN + 1] = 0x00; + memcpy(svc_data_copy, ble_eddystone_svc_data, + BLE_EDDYSTONE_UID_SVC_DATA_LEN + BLE_EDDYSTONE_SVC_DATA_BASE_SZ); + ble_hs_unlock(); - rc = ble_eddystone_set_adv_data_gen(adv_fields, BLE_EDDYSTONE_UID_SVC_DATA_LEN); + rc = ble_eddystone_set_adv_data_gen(adv_fields, svc_data_copy, + BLE_EDDYSTONE_UID_SVC_DATA_LEN); return rc; } @@ -221,6 +230,7 @@ ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields, uint8_t url_body_len, uint8_t url_suffix, int8_t measured_power) { + uint8_t svc_data_copy[BLE_EDDYSTONE_MAX_SVC_DATA_LEN]; uint8_t *svc_data; int url_len; int rc; @@ -278,9 +288,13 @@ ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields, svc_data[2 + url_body_len] = url_suffix; } + memcpy(svc_data_copy, ble_eddystone_svc_data, + url_len + 2 + BLE_EDDYSTONE_SVC_DATA_BASE_SZ); + ble_hs_unlock(); - rc = ble_eddystone_set_adv_data_gen(adv_fields, url_len + 2); + rc = ble_eddystone_set_adv_data_gen(adv_fields, svc_data_copy, + url_len + 2); return rc; } diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 04e82ea49..f510f0071 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -28,8 +28,8 @@ #include "ble_hs_periodic_sync_priv.h" #include "ble_gap_priv.h" #include "ble_hs_resolv_priv.h" -#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) #include "ble_hs_pvcy_priv.h" +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) #include "ble_att_priv.h" #endif #include "host/ble_hs_log.h" @@ -44,7 +44,7 @@ #include "host/ble_hs_iso_hci.h" #endif /* MYNEWT_VAL(BLE_ISO) */ -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) || MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) #include "esp_nimble_mem.h" #endif @@ -217,6 +217,10 @@ struct ble_gap_master_state { ble_gap_event_fn *cb; void *cb_arg; +#if NIMBLE_BLE_SCAN + /* Tracks controller scan state when op is overwritten by M_CONN. */ + uint8_t scan_active:1; +#endif /** * Indicates the type of master procedure that was preempted, or * BLE_GAP_OP_NULL if no procedure was preempted. @@ -332,11 +336,9 @@ static bssnz_t struct ble_gap_big_snc_state ble_gap_big_snc[MYNEWT_VAL(BLE_ISO_B #endif #endif /* MYNEWT_VAL(BLE_ISO) */ -#if NIMBLE_BLE_ADVERTISE #if MYNEWT_VAL(BLE_EXT_ADV) static bool ext_adv_legacy_configured = false; #endif -#endif struct ble_gap_update_entry { SLIST_ENTRY(ble_gap_update_entry) next; @@ -870,8 +872,9 @@ ble_gap_conn_find_handle_by_addr(const ble_addr_t *addr, uint16_t *out_conn_hand } struct foreach_handle_cb_arg { - ble_gap_conn_foreach_handle_fn *cb; - void *arg; + uint16_t *conn_handles; + int count; + int max_count; }; static int @@ -879,20 +882,34 @@ ble_gap_conn_foreach_handle_callback(struct ble_hs_conn *conn, void *arg) { struct foreach_handle_cb_arg *cb_arg = (struct foreach_handle_cb_arg *)arg; - return cb_arg->cb(conn->bhc_handle, cb_arg->arg); + if (cb_arg->count >= cb_arg->max_count) { + return BLE_HS_EDONE; + } + + cb_arg->conn_handles[cb_arg->count++] = conn->bhc_handle; + return 0; } void ble_gap_conn_foreach_handle(ble_gap_conn_foreach_handle_fn *cb, void *arg) { + uint16_t conn_handles[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; struct foreach_handle_cb_arg cb_arg = { - .cb = cb, - .arg = arg, + .conn_handles = conn_handles, + .count = 0, + .max_count = MYNEWT_VAL(BLE_MAX_CONNECTIONS), }; + int i; ble_hs_lock(); ble_hs_conn_foreach(ble_gap_conn_foreach_handle_callback, &cb_arg); ble_hs_unlock(); + + for (i = 0; i < cb_arg.count; i++) { + if (cb(conn_handles[i], arg) != 0) { + break; + } + } } #if NIMBLE_BLE_CONNECT @@ -902,6 +919,9 @@ ble_gap_extract_conn_cb(uint16_t conn_handle, { const struct ble_hs_conn *conn; + *out_cb = NULL; + *out_cb_arg = NULL; + if (conn_handle > BLE_HCI_LE_CONN_HANDLE_MAX) { return BLE_HS_EINVAL; } @@ -927,7 +947,8 @@ ble_gap_extract_conn_cb(uint16_t conn_handle, } #endif -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_ROLE_CENTRAL) && \ + MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) static void ble_gap_ensure_peer_rl_entry(const ble_addr_t *peer_addr); #endif @@ -944,8 +965,13 @@ ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode) return BLE_HS_EINVAL; } +#if MYNEWT_VAL(BLE_ROLE_CENTRAL) && MYNEWT_VAL(BLE_HS_PVCY) && \ + !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_ensure_peer_rl_entry(peer_addr); +#endif + #if MYNEWT_VAL(BLE_HS_PVCY) -#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_ROLE_CENTRAL) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) ble_gap_ensure_peer_rl_entry(peer_addr); #endif return ble_hs_pvcy_set_mode(peer_addr, priv_mode); @@ -1012,15 +1038,8 @@ ble_gap_set_prefered_default_le_phy(uint8_t tx_phys_mask, uint8_t rx_phys_mask) #if NIMBLE_BLE_CONNECT struct ble_hci_le_set_default_phy_cp cmd; - if (tx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK | - BLE_HCI_LE_PHY_2M_PREF_MASK | - BLE_HCI_LE_PHY_CODED_PREF_MASK)) { - return BLE_HS_EINVAL; - } - - if (rx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK | - BLE_HCI_LE_PHY_2M_PREF_MASK | - BLE_HCI_LE_PHY_CODED_PREF_MASK)) { + if ((tx_phys_mask & ~BLE_GAP_LE_PHY_ANY_MASK) || + (rx_phys_mask & ~BLE_GAP_LE_PHY_ANY_MASK)) { return BLE_HS_EINVAL; } @@ -1033,13 +1052,13 @@ ble_gap_set_prefered_default_le_phy(uint8_t tx_phys_mask, uint8_t rx_phys_mask) if (tx_phys_mask == 0) { cmd.all_phys |= BLE_HCI_LE_PHY_NO_TX_PREF_MASK; } else { - cmd.tx_phys = tx_phys_mask; + cmd.tx_phys = tx_phys_mask & BLE_GAP_LE_PHY_ANY_MASK; } if (rx_phys_mask == 0) { cmd.all_phys |= BLE_HCI_LE_PHY_NO_RX_PREF_MASK; } else { - cmd.rx_phys = rx_phys_mask; + cmd.rx_phys = rx_phys_mask & BLE_GAP_LE_PHY_ANY_MASK; } return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -1070,17 +1089,8 @@ ble_gap_set_prefered_le_phy(uint16_t conn_handle, uint8_t tx_phys_mask, return BLE_HS_ENOTCONN; } - if (tx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK | - BLE_HCI_LE_PHY_2M_PREF_MASK | - BLE_HCI_LE_PHY_CODED_PREF_MASK | - BLE_GAP_LE_PHY_ANY_MASK)) { - return BLE_HS_EINVAL; - } - - if (rx_phys_mask > (BLE_HCI_LE_PHY_1M_PREF_MASK | - BLE_HCI_LE_PHY_2M_PREF_MASK | - BLE_HCI_LE_PHY_CODED_PREF_MASK | - BLE_GAP_LE_PHY_ANY_MASK)) { + if ((tx_phys_mask & ~BLE_GAP_LE_PHY_ANY_MASK) || + (rx_phys_mask & ~BLE_GAP_LE_PHY_ANY_MASK)) { return BLE_HS_EINVAL; } @@ -1094,23 +1104,13 @@ ble_gap_set_prefered_le_phy(uint16_t conn_handle, uint8_t tx_phys_mask, if (tx_phys_mask == 0) { cmd.all_phys |= BLE_HCI_LE_PHY_NO_TX_PREF_MASK; } else { - cmd.tx_phys = tx_phys_mask & (BLE_HCI_LE_PHY_1M_PREF_MASK | - BLE_HCI_LE_PHY_2M_PREF_MASK | - BLE_HCI_LE_PHY_CODED_PREF_MASK); - if (cmd.tx_phys == 0) { - return BLE_HS_EINVAL; - } + cmd.tx_phys = tx_phys_mask; } if (rx_phys_mask == 0) { cmd.all_phys |= BLE_HCI_LE_PHY_NO_RX_PREF_MASK; } else { - cmd.rx_phys = rx_phys_mask & (BLE_HCI_LE_PHY_1M_PREF_MASK | - BLE_HCI_LE_PHY_2M_PREF_MASK | - BLE_HCI_LE_PHY_CODED_PREF_MASK); - if (cmd.rx_phys == 0) { - return BLE_HS_EINVAL; - } + cmd.rx_phys = rx_phys_mask; } cmd.phy_options = htole16(phy_opts); @@ -1168,6 +1168,8 @@ uint8_t* ble_resolve_adv_data(const uint8_t *adv_data, uint8_t adv_type, uint8_t * $misc * *****************************************************************************/ +#define BLE_GAP_EVENT_LISTENER_MAX 16 + static int ble_gap_event_listener_call(struct ble_gap_event *event); @@ -1766,7 +1768,6 @@ ble_gap_slave_reset_state(uint8_t instance) * All callers must ensure ble_hs_lock() is held to prevent data races * on concurrent bitfield access from different contexts. */ ble_gap_slave[instance].op = BLE_GAP_OP_NULL; - ble_gap_slave[instance].preempted = 0; #if !MYNEWT_VAL(BLE_EXT_ADV) ble_gap_slave[instance].exp_set = 0; @@ -1938,6 +1939,7 @@ ble_gap_master_connect_cancelled(void) event.link_estab.sync_handle = pawr_sync_handle; event.link_estab.adv_handle = pawr_adv_handle; #endif + ble_gap_event_listener_call(&event); state.cb(&event, state.cb_arg); } } @@ -2642,18 +2644,27 @@ int ble_gap_ext_adv_active(uint8_t instance) int ble_gap_adv_get_free_instance(uint8_t *out_adv_instance) { - uint8_t i; + int i; if (out_adv_instance == NULL) { return BLE_HS_EINVAL; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gap_vars == NULL) { + return BLE_HS_ENOENT; + } +#endif + + ble_hs_lock(); for (i = 0; i < BLE_ADV_INSTANCES; i++) { if (!ble_gap_slave[i].configured) { *out_adv_instance = i; + ble_hs_unlock(); return 0; } } + ble_hs_unlock(); return BLE_HS_ENOENT; } @@ -2699,6 +2710,9 @@ ble_gap_reset_state(int reason) #endif #if (NIMBLE_BLE_SCAN || NIMBLE_BLE_CONNECT) +#if NIMBLE_BLE_SCAN + ble_gap_master.scan_active = 0; +#endif if (ble_gap_master_in_progress()) { ble_gap_master_failed(reason); } @@ -2724,7 +2738,6 @@ ble_gap_reset_state(int reason) cb_arg = psync->cb_arg; sync_handle = psync->sync_handle; ble_hs_periodic_sync_remove(psync); - ble_npl_eventq_remove(ble_hs_evq_get(), &psync->lost_ev); ble_hs_periodic_sync_free(psync); ble_hs_unlock(); @@ -2773,30 +2786,22 @@ ble_gap_accept_master_conn(void) static int ble_gap_accept_slave_conn(uint8_t instance) { - int rc = 0; -#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) - if (instance >= BLE_ADV_INSTANCES) { - rc = BLE_HS_ENOENT; -} -#else + int rc; + if (instance >= BLE_ADV_INSTANCES) { rc = BLE_HS_ENOENT; } else if (!ble_gap_adv_active_instance(instance)) { - + rc = BLE_HS_ENOENT; + } else if (!ble_gap_slave[instance].connectable) { rc = BLE_HS_ENOENT; } else { - if (ble_gap_slave[instance].connectable) { - rc = 0; - } else { - rc = BLE_HS_ENOENT; - } + rc = 0; } if (rc == 0) { STATS_INC(ble_gap_stats, connect_slv); } -#endif return rc; } #endif @@ -2865,6 +2870,13 @@ static void ble_gap_slave_get_cb(uint8_t instance, ble_gap_event_fn **out_cb, void **out_cb_arg) { + *out_cb = NULL; + *out_cb_arg = NULL; + + if (instance >= BLE_ADV_INSTANCES) { + return; + } + ble_hs_lock(); *out_cb = ble_gap_slave[instance].cb; @@ -2900,14 +2912,20 @@ ble_gap_rx_cis_disconn(const struct ble_hci_ev_disconn_cmp *ev) } ble_gap_event_listener_call(&event); + ble_hs_lock(); cis = ble_gap_cis_find(handle); if (cis) { - if (cis->cb) { - cis->cb(&event, cis->cb_arg); - } + ble_gap_event_fn *cb = cis->cb; + void *cb_arg = cis->cb_arg; if (ev->status == 0) { cis->active = false; } + ble_hs_unlock(); + if (cb) { + cb(&event, cb_arg); + } + } else { + ble_hs_unlock(); } } @@ -3043,6 +3061,8 @@ ble_gap_rx_term_big_comp(const struct ble_hci_ev_le_subev_terminate_big_complete ble_hs_lock(); cb = ble_gap_big_brd[ev->big_handle].cb; cb_arg = ble_gap_big_brd[ev->big_handle].cb_arg; + ble_gap_big_brd[ev->big_handle].cb = NULL; + ble_gap_big_brd[ev->big_handle].cb_arg = NULL; ble_hs_unlock(); if (cb) { cb(&event, cb_arg); @@ -3106,6 +3126,8 @@ ble_gap_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev) ble_hs_lock(); cb = ble_gap_big_snc[ev->big_handle].cb; cb_arg = ble_gap_big_snc[ev->big_handle].cb_arg; + ble_gap_big_snc[ev->big_handle].cb = NULL; + ble_gap_big_snc[ev->big_handle].cb_arg = NULL; ble_hs_unlock(); if (cb) { cb(&event, cb_arg); @@ -3129,10 +3151,6 @@ ble_gap_rx_biginfo_adv_rpt(const struct ble_hci_ev_le_subev_biginfo_adv_report * } ble_hs_unlock(); - if (!psync) { - return; - } - memset(&event, 0, sizeof(event)); event.type = BLE_GAP_EVENT_BIGINFO_ADV_RPT; @@ -3151,11 +3169,11 @@ ble_gap_rx_biginfo_adv_rpt(const struct ble_hci_ev_le_subev_biginfo_adv_report * event.biginfo_report.framing = ev->framing; event.biginfo_report.encryption = ev->encryption; + ble_gap_event_listener_call(&event); + if (cb) { cb(&event, cb_arg); } - - ble_gap_event_listener_call(&event); } #if MYNEWT_VAL(BLE_ISO_CIS_ESTAB_V2) @@ -3332,6 +3350,8 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) ext_desc.props |= BLE_HCI_ADV_SCAN_MASK; break; case BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP: + /* Legacy HCI does not distinguish SCAN_RSP to ADV_IND vs ADV_SCAN_IND; + * omit connectable (conservative ADV_SCAN_IND mapping per Core Spec). */ ext_desc.props |= BLE_HCI_ADV_SCAN_MASK | BLE_HCI_ADV_SCAN_RSP_MASK; break; default: @@ -3347,6 +3367,9 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) ext_desc.length_data = desc->length_data; ext_desc.data = desc->data; ext_desc.direct_addr = desc->direct_addr; +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ext_desc.ota_addr = desc->ota_addr; +#endif ble_gap_disc_report(&ext_desc); #else ble_gap_disc_report(desc); @@ -3359,6 +3382,8 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) void ble_gap_rx_le_scan_timeout(void) { + ble_gap_master.scan_active = 0; + if (ble_gap_master.op == BLE_GAP_OP_M_DISC) { ble_gap_disc_complete(); } @@ -3437,7 +3462,7 @@ ble_gap_periodic_sync_reattempt_clear(bool clear_sync_reattempt) #endif void -ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev) +ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev, uint8_t subevent) { uint16_t sync_handle; struct ble_gap_event event; @@ -3446,6 +3471,8 @@ ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT int rc; bool reattempt_triggered = false; + ble_gap_event_fn *saved_sync_cb; + void *saved_sync_cb_arg; #endif memset(&event, 0, sizeof event); @@ -3482,10 +3509,12 @@ ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv event.periodic_sync.per_adv_ival = ev->interval; event.periodic_sync.adv_clk_accuracy = ev->aca; #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) - event.periodic_sync.num_subevents = ev->num_subevents; - event.periodic_sync.subevent_interval = ev->subevent_interval; - event.periodic_sync.response_slot_delay = ev->response_slot_delay; - event.periodic_sync.response_slot_spacing = ev->response_slot_spacing; + if (subevent == BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB_V2) { + event.periodic_sync.num_subevents = ev->num_subevents; + event.periodic_sync.subevent_interval = ev->subevent_interval; + event.periodic_sync.response_slot_delay = ev->response_slot_delay; + event.periodic_sync.response_slot_spacing = ev->response_slot_spacing; + } #endif ble_hs_periodic_sync_insert(ble_gap_sync.psync); @@ -3500,27 +3529,40 @@ ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv ble_conn_reattempt.sync_reattempt = 1; reattempt_triggered = true; + saved_sync_cb = ble_gap_sync.cb; + saved_sync_cb_arg = ble_gap_sync.cb_arg; ble_gap_sync.op = BLE_GAP_OP_NULL; ble_hs_unlock(); /* Make application aware of the reattempt */ - ble_gap_reattempt_count(ev->sync_handle, ble_conn_reattempt.count); + ble_gap_reattempt_count(le16toh(ev->sync_handle), + ble_conn_reattempt.count); - rc = ble_gap_periodic_adv_sync_create(&ble_conn_reattempt.periodic_addr, - ble_conn_reattempt.adv_sid, - &ble_conn_reattempt.periodic_params, - ble_conn_reattempt.cb, ble_conn_reattempt.cb_arg); + ble_addr_t reattempt_addr; + struct ble_gap_periodic_sync_params reattempt_params; + + reattempt_addr = ble_conn_reattempt.periodic_addr; + reattempt_params = ble_conn_reattempt.periodic_params; + rc = ble_gap_periodic_adv_sync_create(&reattempt_addr, + ble_conn_reattempt.adv_sid, + &reattempt_params, + ble_conn_reattempt.cb, + ble_conn_reattempt.cb_arg); if (rc != 0) { /* Create sync failed - must clean up reattempt state and notify app */ ble_hs_lock(); ble_gap_periodic_sync_reattempt_clear(true); - cb = ble_gap_sync.cb; - cb_arg = ble_gap_sync.cb_arg; - ble_gap_sync.cb = NULL; - ble_gap_sync.cb_arg = NULL; + if (ble_gap_sync.cb == saved_sync_cb && + ble_gap_sync.cb_arg == saved_sync_cb_arg) { + ble_gap_sync.cb = NULL; + ble_gap_sync.cb_arg = NULL; + } + + cb = saved_sync_cb; + cb_arg = saved_sync_cb_arg; ble_hs_unlock(); /* Notify application of final failure */ @@ -3558,14 +3600,16 @@ ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv ble_gap_sync.op = BLE_GAP_OP_NULL; ble_gap_sync.psync = NULL; + ble_gap_sync.cb = NULL; + ble_gap_sync.cb_arg = NULL; + ble_hs_unlock(); + /* Application events require the host to be in the unlcoked state. */ ble_gap_event_listener_call(&event); if (cb) { cb(&event, cb_arg); } - - ble_gap_sync.cb = NULL; - ble_gap_sync.cb_arg = NULL; + ble_hs_lock(); #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT } @@ -3715,10 +3759,12 @@ ble_gap_rx_connless_iq_report(const struct ble_hci_ev_le_subev_connless_iq_rpt * ble_hs_lock(); psync = ble_hs_periodic_sync_find_by_handle(sync_handle); - if (psync) { - cb = psync->cb; - cb_arg = psync->cb_arg; + if (psync == NULL) { + ble_hs_unlock(); + return; } + cb = psync->cb; + cb_arg = psync->cb_arg; ble_hs_unlock(); memset(&event, 0, sizeof event); @@ -3737,16 +3783,11 @@ ble_gap_rx_connless_iq_report(const struct ble_hci_ev_le_subev_connless_iq_rpt * } event.connless_iq_report.sample_count = ev->sample_count; - /* HCI interleaves I and Q: I[0],Q[0],I[1],Q[1],... (BT spec vol4 part E sec 5.2). - * De-interleave into separate arrays before handing to the application. */ - int8_t connless_i_buf[BLE_HCI_LE_IQ_SAMPLE_MAX_COUNT]; - int8_t connless_q_buf[BLE_HCI_LE_IQ_SAMPLE_MAX_COUNT]; - for (int k = 0; k < ev->sample_count; k++) { - connless_i_buf[k] = ev->iq_samples[2 * k]; - connless_q_buf[k] = ev->iq_samples[2 * k + 1]; - } - event.connless_iq_report.i_samples = connless_i_buf; - event.connless_iq_report.q_samples = connless_q_buf; + /* The ESP controller delivers I and Q samples as two contiguous blocks in + * the HCI event payload (I[0..n-1] followed by Q[0..n-1]), not interleaved + * per Core Spec Vol 4 Part E Sec 5.2 default. */ + event.connless_iq_report.i_samples = (int8_t *)(ev->iq_samples); + event.connless_iq_report.q_samples = (int8_t *)(ev->iq_samples + ev->sample_count); ble_gap_event_listener_call(&event); @@ -3777,16 +3818,9 @@ ble_gap_rx_conn_iq_report(const struct ble_hci_ev_le_subev_conn_iq_rpt *ev) } event.conn_iq_report.sample_count = ev->sample_count; - /* HCI interleaves I and Q: I[0],Q[0],I[1],Q[1],... (BT spec vol4 part E sec 5.2). - * De-interleave into separate arrays before handing to the application. */ - int8_t conn_i_buf[BLE_HCI_LE_IQ_SAMPLE_MAX_COUNT]; - int8_t conn_q_buf[BLE_HCI_LE_IQ_SAMPLE_MAX_COUNT]; - for (int k = 0; k < ev->sample_count; k++) { - conn_i_buf[k] = ev->iq_samples[2 * k]; - conn_q_buf[k] = ev->iq_samples[2 * k + 1]; - } - event.conn_iq_report.i_samples = conn_i_buf; - event.conn_iq_report.q_samples = conn_q_buf; + /* See note in ble_gap_rx_connless_iq_report(): ESP controller layout. */ + event.conn_iq_report.i_samples = (int8_t *)(ev->iq_samples); + event.conn_iq_report.q_samples = (int8_t *)(ev->iq_samples + ev->sample_count); ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, event.conn_iq_report.conn_handle); @@ -3838,7 +3872,9 @@ periodic_adv_transfer_disable(uint16_t conn_handle) } void -ble_gap_rx_periodic_adv_sync_transfer(const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev) +ble_gap_rx_periodic_adv_sync_transfer( + const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev, + uint8_t subevent) { struct ble_hci_le_periodic_adv_term_sync_cp cmd_term; struct ble_gap_event event; @@ -3878,7 +3914,11 @@ ble_gap_rx_periodic_adv_sync_transfer(const struct ble_hci_ev_le_subev_periodic_ memset(&event, 0, sizeof event); #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) - event.type = BLE_GAP_EVENT_PERIODIC_TRANSFER_V2; + if (subevent == BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER_V2) { + event.type = BLE_GAP_EVENT_PERIODIC_TRANSFER_V2; + } else { + event.type = BLE_GAP_EVENT_PERIODIC_TRANSFER; + } #else event.type = BLE_GAP_EVENT_PERIODIC_TRANSFER; #endif // MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) @@ -3911,10 +3951,12 @@ ble_gap_rx_periodic_adv_sync_transfer(const struct ble_hci_ev_le_subev_periodic_ event.periodic_transfer.per_adv_itvl = le16toh(ev->interval); event.periodic_transfer.adv_clk_accuracy = ev->aca; #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) - event.periodic_transfer.num_subevents = ev->num_subevents; - event.periodic_transfer.subevent_interval = ev->subevent_interval; - event.periodic_transfer.response_slot_delay = ev->response_slot_delay; - event.periodic_transfer.response_slot_spacing = ev->response_slot_spacing; + if (subevent == BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER_V2) { + event.periodic_transfer.num_subevents = ev->num_subevents; + event.periodic_transfer.subevent_interval = ev->subevent_interval; + event.periodic_transfer.response_slot_delay = ev->response_slot_delay; + event.periodic_transfer.response_slot_spacing = ev->response_slot_spacing; + } #endif ble_hs_unlock(); @@ -4175,13 +4217,20 @@ ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance) } #endif break; -#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) case BLE_ERR_CONN_ESTABLISHMENT: +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) if (!v1_evt) { ble_gap_rx_conn_comp_failed(evt); + break; } +#endif +#if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_OBSERVER) + if (ble_gap_master_in_progress()) { + ble_gap_master_connect_failure( + BLE_HS_HCI_ERR(evt->status)); + } +#endif break; -#endif // MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) default: /* this should never happen, unless controller is broken */ BLE_HS_LOG(INFO, "controller reported invalid error code in conn" @@ -4248,12 +4297,20 @@ ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance) conn->bhc_cb = ble_gap_master.cb; conn->bhc_cb_arg = ble_gap_master.cb_arg; conn->bhc_our_addr_type = ble_gap_master.conn.our_addr_type; + ble_gap_master_reset_state(); } else { + ble_hs_lock(); conn->bhc_cb = ble_gap_slave[instance].cb; conn->bhc_cb_arg = ble_gap_slave[instance].cb_arg; conn->bhc_our_addr_type = ble_gap_slave[instance].our_addr_type; +#if MYNEWT_VAL(BLE_EXT_ADV) + memcpy(conn->bhc_our_rnd_addr, ble_gap_slave[instance].rnd_addr, 6); +#endif + ble_gap_slave_reset_state(instance); + ble_hs_unlock(); } #else + ble_hs_lock(); conn->bhc_cb = ble_gap_slave[instance].cb; conn->bhc_cb_arg = ble_gap_slave[instance].cb_arg; conn->bhc_our_addr_type = ble_gap_slave[instance].our_addr_type; @@ -4261,6 +4318,7 @@ ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance) memcpy(conn->bhc_our_rnd_addr, ble_gap_slave[instance].rnd_addr, 6); #endif ble_gap_slave_reset_state(instance); + ble_hs_unlock(); #endif if (master_match) { @@ -4306,7 +4364,11 @@ ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance) /* This is also done when bonding is restored, so `conn` and `ble_gatts_conn_aware_states` need to be kept in sync */ ble_hs_conn_addrs(conn, &addrs); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_gatts_conn_aware_states != NULL) { + if (ble_gatts_conn_aware_states == NULL) { + /* GATTS was stopped; GATT database may have changed, + * so treat all reconnecting bonded peers as unaware */ + conn->bhc_gatt_svr.aware_state = false; + } else { #endif for (int i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if (memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, @@ -4326,11 +4388,16 @@ ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance) #endif if (evt->role == BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE) { - ble_gap_rd_rem_ver_tx(evt->connection_handle); + rc = ble_gap_rd_rem_ver_tx(evt->connection_handle); + if (rc != 0) { + conn->slave_conn = 1; + ble_gap_event_connect_call(htole16(evt->connection_handle), 0); + } } else { int rc_feat = ble_gap_rd_rem_sup_feat_tx(evt->connection_handle); if (rc_feat != 0) { - ble_gap_event_connect_call(evt->connection_handle, rc_feat); + /* Connection is established even if the follow-up HCI command fails. */ + ble_gap_event_connect_call(htole16(evt->connection_handle), 0); } } @@ -4409,6 +4476,7 @@ ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used { #if NIMBLE_BLE_CONNECT struct ble_hs_conn *conn; + int rc; ble_hs_lock(); @@ -4432,7 +4500,10 @@ ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used conn->supported_feat = get_le32(ev->features); #endif } - ble_gap_rd_rem_ver_tx(le16toh(ev->conn_handle)); + rc = ble_gap_rd_rem_ver_tx(le16toh(ev->conn_handle)); + if (rc != 0) { + ble_gap_event_connect_call(ev->conn_handle, 0); + } } else { if (conn != NULL) { if (ev->status == 0) { @@ -4444,8 +4515,8 @@ ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used } /* Connection is established regardless of feature read result; * pass 0 to avoid falsely reporting a connection failure. */ - ble_gap_event_connect_call(ev->conn_handle, 0); conn->slave_conn = 1; + ble_gap_event_connect_call(ev->conn_handle, 0); } } #endif @@ -4485,7 +4556,8 @@ ble_gap_rx_rd_rem_ver_info_complete(const struct ble_hci_ev_rd_rem_ver_info_cmp if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { int rc_feat = ble_gap_rd_rem_sup_feat_tx(le16toh(ev->conn_handle)); if (rc_feat != 0) { - ble_gap_event_connect_call(ev->conn_handle, rc_feat); + conn->slave_conn = 1; + ble_gap_event_connect_call(ev->conn_handle, 0); } } else { ble_gap_event_connect_call(ev->conn_handle, 0); @@ -4643,6 +4715,7 @@ ble_gap_master_timer(void) return ble_npl_time_ms_to_ticks32(BLE_GAP_CANCEL_RETRY_TIMEOUT_MS); } + ble_gap_master.scan_active = 0; ble_gap_disc_complete(); #else assert(0); @@ -4713,7 +4786,9 @@ ble_gap_update_timer(void) if (entry != NULL) { ble_gap_update_entry_free(entry); +#if NIMBLE_BLE_CONNECT ble_gap_update_notify(conn_handle, BLE_HS_ETIMEOUT); +#endif } } while (entry != NULL); @@ -4766,7 +4841,7 @@ ble_gap_timer(void) #if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV) { - uint32_t slave_ticks = ble_gap_slave_timer(); + int32_t slave_ticks = ble_gap_slave_timer(); min_ticks = min(min_ticks, slave_ticks); } #endif @@ -4782,33 +4857,12 @@ ble_gap_timer(void) static int ble_gap_wl_busy(void) { -#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - if (ble_host_rpa_enabled()) { - return BLE_HS_ENOTSUP; - } -#endif - - /* Check if a connection initiation procedure is using the whitelist. */ - if (ble_gap_master.op == BLE_GAP_OP_M_CONN && ble_gap_master.conn.using_wl) { - return 1; - } - - /* Check if a scanning procedure is using the whitelist. */ - if (ble_gap_master.op == BLE_GAP_OP_M_DISC && ble_gap_master.disc.using_wl) { - return 1; - } - -#if NIMBLE_BLE_ADVERTISE - /* Check if any advertising instance is using the whitelist. */ - int i; - for (i = 0; i < BLE_ADV_INSTANCES; i++) { - if (ble_gap_slave[i].op == BLE_GAP_OP_S_ADV && ble_gap_slave[i].using_wl) { - return 1; - } - } -#endif - - return 0; + /* Check if an auto or selective connection establishment procedure is in + * progress. Advertising/scanning FAL use is enforced by the controller + * (HCI Command Disallowed / BLE_HS_HCI_ERR(BLE_ERR_CMD_DISALLOWED)). + */ + return ble_gap_master.op == BLE_GAP_OP_M_CONN && + ble_gap_master.conn.using_wl; } int @@ -4920,11 +4974,9 @@ ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count) } } - if (ble_gap_wl_busy()) { - /* The filter accept list is currently in use by the controller. - * Mirror the CMD_DISALLOWED error the controller would return if the - * HCI command were sent. */ - rc = BLE_HS_HCI_ERR(BLE_ERR_CMD_DISALLOWED); + if (ble_gap_wl_busy() != 0) { + /* Connection establishment is using the whitelist. */ + rc = BLE_HS_EBUSY; goto done; } @@ -5015,17 +5067,16 @@ ble_gap_adv_stop(void) #if MYNEWT_VAL(BLE_EXT_ADV) int rc; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + + STATS_INC(ble_gap_stats, adv_stop); rc = ble_gap_ext_adv_stop(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE)); - if (rc) { - return rc; + if (rc != 0) { + STATS_INC(ble_gap_stats, adv_stop_fail); } - - rc = ble_gap_ext_adv_remove(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE)); - if (rc) { - return rc; - } - - return 0; + return rc; #else int rc; @@ -5182,9 +5233,15 @@ ble_gap_adv_validate(uint8_t own_addr_type, const ble_addr_t *peer_addr, return BLE_HS_EINVAL; } - /* Both must be zero or both must be non-zero; if only one is zero, apply default. */ + /* Both must be zero or both must be non-zero. */ + if ((adv_params->itvl_min == 0) != (adv_params->itvl_max == 0)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + if (adv_params->itvl_min && adv_params->itvl_max && adv_params->itvl_min > adv_params->itvl_max) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -5312,8 +5369,10 @@ ble_gap_apply_deferred_pvcy_add(void) rc); } } +#endif -#if NIMBLE_BLE_CONNECT +#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_ROLE_CENTRAL) && \ + MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) static void ble_gap_ensure_peer_rl_entry(const ble_addr_t *peer_addr) { @@ -5321,7 +5380,9 @@ ble_gap_ensure_peer_rl_entry(const ble_addr_t *peer_addr) struct ble_store_value_sec value_sec; int rc; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) ble_gap_apply_deferred_pvcy_add(); +#endif memset(&key_sec, 0, sizeof key_sec); key_sec.peer_addr = *peer_addr; @@ -5337,12 +5398,12 @@ ble_gap_ensure_peer_rl_entry(const ble_addr_t *peer_addr) value_sec.irk); } if (rc != 0) { - BLE_HS_LOG(DEBUG, - "resolving-list sync before set privacy mode; rc=%d\n", rc); + BLE_HS_LOG(WARN, + "failed to sync peer resolving-list entry before connect; rc=%d\n", + rc); } } #endif /* NIMBLE_BLE_CONNECT */ -#endif int ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr, @@ -5357,11 +5418,31 @@ ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr, int rc; STATS_INC(ble_gap_stats, adv_start); + rc = 0; if (!ble_hs_is_enabled()) { - return BLE_HS_EDISABLED; + rc = BLE_HS_EDISABLED; + goto done; } + if (adv_params == NULL) { + rc = BLE_HS_EINVAL; + goto done; + } + + if (adv_params->disc_mode >= BLE_GAP_DISC_MODE_MAX) { + rc = BLE_HS_EINVAL; + goto done; + } + + ble_hs_lock(); + if (ble_gap_is_preempted()) { + ble_hs_unlock(); + rc = BLE_HS_EPREEMPTED; + goto done; + } + ble_hs_unlock(); + memset(&ext_params, 0, sizeof(ext_params)); ext_params.own_addr_type = own_addr_type; @@ -5373,51 +5454,77 @@ ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr, switch (adv_params->conn_mode) { case BLE_GAP_CONN_MODE_NON: + if (adv_params->high_duty_cycle) { + rc = BLE_HS_EINVAL; + goto done; + } if (adv_params->disc_mode != BLE_GAP_DISC_MODE_NON) { ext_params.scannable = 1; } break; case BLE_GAP_CONN_MODE_DIR: if (!direct_addr) { - return BLE_HS_EINVAL; + rc = BLE_HS_EINVAL; + goto done; } ext_params.peer = *direct_addr; + ext_params.peer.type = + ble_hs_misc_peer_addr_type_to_id(direct_addr->type); ext_params.directed = 1; + ext_params.connectable = 1; if (adv_params->high_duty_cycle) { ext_params.high_duty_directed = 1; } break; case BLE_GAP_CONN_MODE_UND: + if (adv_params->high_duty_cycle) { + rc = BLE_HS_EINVAL; + goto done; + } ext_params.connectable = 1; ext_params.scannable = 1; break; default: - return BLE_HS_EINVAL; + rc = BLE_HS_EINVAL; + goto done; } ext_params.itvl_min = adv_params->itvl_min; ext_params.itvl_max = adv_params->itvl_max; ext_params.channel_map = adv_params->channel_map; ext_params.filter_policy = adv_params->filter_policy; - ext_params.high_duty_directed = adv_params->high_duty_cycle; /* configure legacy instance */ rc = ble_gap_ext_adv_configure(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE), &ext_params, NULL, cb, cb_arg); if (rc) { - return rc; + goto done; } ext_adv_legacy_configured = true; if (duration_ms == BLE_HS_FOREVER) { duration = 0; + } else if (duration_ms < 0 || (duration_ms > 0 && duration_ms < 10)) { + rc = BLE_HS_EINVAL; + goto done; + } else if (duration_ms == 0) { + /* HCI Duration=0 means infinite; legacy adv uses zero ticks for + * immediate stop, so use the minimum non-zero duration (10 ms). */ + duration = 1; } else { duration = duration_ms / 10; } - return ble_gap_ext_adv_start(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE), duration, 0); + rc = ble_gap_ext_adv_start(MYNEWT_VAL(BLE_HS_EXT_ADV_LEGACY_INSTANCE), + duration, 0); + +done: + if (rc != 0) { + STATS_INC(ble_gap_stats, adv_start_fail); + } + return rc; #else uint32_t duration_ticks; int rc; @@ -5645,7 +5752,6 @@ ble_gap_adv_set_data(const uint8_t *data, int data_len) return BLE_HS_EINVAL; } - memset(&cmd, 0, sizeof(cmd)); if (data_len > 0) { memcpy(cmd.adv_data, data, data_len); } @@ -5776,6 +5882,14 @@ ble_gap_adv_rsp_set_fields(const struct ble_hs_adv_fields *rsp_fields) uint8_t buf_sz; int rc; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + + if (rsp_fields == NULL) { + return BLE_HS_EINVAL; + } + rc = ble_hs_adv_set_fields(rsp_fields, buf, &buf_sz, sizeof buf); if (rc != 0) { return rc; @@ -5798,7 +5912,7 @@ ble_gap_adv_active(void) #if NIMBLE_BLE_ADVERTISE #if MYNEWT_VAL(BLE_EXT_ADV) /* Check all instances since extended advertising supports multiple */ - uint8_t i; + int i; for (i = 0; i < BLE_ADV_INSTANCES; i++) { if (ble_gap_adv_active_instance(i)) { return 1; @@ -6072,10 +6186,11 @@ ble_gap_ext_adv_params_validate(const struct ble_gap_ext_adv_params *params) return BLE_HS_EINVAL; } - /* BT spec Vol 4, Part E, 7.8.53: anonymous advertising shall not be connectable, - * scannable, or directed. */ + /* Anonymous advertising shall not be connectable or scannable. + * Directed anonymous advertising is valid when non-connectable and + * non-scannable (Core Spec Vol 6 Part B Sec 2.3.1.6). */ if (params->anonymous && - (params->connectable || params->scannable || params->directed)) { + (params->connectable || params->scannable)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -6108,6 +6223,11 @@ ble_gap_ext_adv_configure(uint8_t instance, ble_hs_lock(); + if (ble_gap_is_preempted()) { + ble_hs_unlock(); + return BLE_HS_EPREEMPTED; + } + #if NIMBLE_BLE_ADVERTISE || NIMBLE_BLE_CONNECT if (ble_gap_adv_active_instance(instance)) { ble_hs_unlock(); @@ -6258,12 +6378,10 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) ble_hs_lock(); -#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT - ble_adv_reattempt.type = 1; - ble_adv_reattempt.instance = instance; - ble_adv_reattempt.duration = duration; - ble_adv_reattempt.max_events = max_events; -#endif + if (ble_gap_is_preempted()) { + ble_hs_unlock(); + return BLE_HS_EPREEMPTED; + } #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) ble_gap_apply_deferred_pvcy_add(); @@ -6364,6 +6482,13 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) ble_gap_slave[instance].op = BLE_GAP_OP_S_ADV; +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT + ble_adv_reattempt.type = 1; + ble_adv_reattempt.instance = instance; + ble_adv_reattempt.duration = duration; + ble_adv_reattempt.max_events = max_events; +#endif + ble_hs_unlock(); return 0; } @@ -6405,7 +6530,6 @@ ble_gap_ext_adv_stop_no_lock(uint8_t instance) } ble_gap_slave[instance].op = BLE_GAP_OP_NULL; - ble_gap_slave[instance].preempted = 0; #if NIMBLE_BLE_ADVERTISE || NIMBLE_BLE_CONNECT if (!active) { @@ -6675,6 +6799,7 @@ ble_gap_ext_adv_rsp_set_validate(uint8_t instance, struct os_mbuf *data) /* total data must never exceed the configured max regardless of adv state */ if (len > MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -7150,6 +7275,7 @@ ble_gap_periodic_adv_set_data_validate(uint8_t instance, } if (data && OS_MBUF_PKTLEN(data) > MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -7472,9 +7598,8 @@ ble_gap_periodic_adv_sync_terminate(uint16_t sync_handle) * object to prevent it becoming a ghost entry. */ ble_hs_periodic_sync_remove(psync); - ble_npl_eventq_remove(ble_hs_evq_get(), &psync->lost_ev); - ble_hs_periodic_sync_free(psync); - psync = NULL; + ble_npl_eventq_put(ble_hs_evq_get(), &psync->lost_ev); + rc = 0; } ble_hs_unlock(); @@ -8195,20 +8320,21 @@ ble_gap_set_connless_cte_transmit_params(uint8_t instance, const struct ble_gap_ /* BT spec (§7.8.80): switching_pattern_length valid range is [0x02, 0x4B] * for AoD types. For AoA (cte_type==0) the switching pattern parameters * are ignored. */ + uint8_t sp_len = params->cte_type == 0 ? 0 : params->switching_pattern_length; + if (params->cte_type != 0) { - if (params->switching_pattern_length < 2 || - params->switching_pattern_length > 75 || - params->antenna_ids == NULL) { + if (sp_len < 2 || sp_len > 75 || params->antenna_ids == NULL) { return BLE_HS_EINVAL; } } - uint8_t buf[sizeof(struct ble_hci_le_set_connless_cte_tx_params_cp) + - params->switching_pattern_length]; + uint8_t buf[sizeof(struct ble_hci_le_set_connless_cte_tx_params_cp) + sp_len]; struct ble_hci_le_set_connless_cte_tx_params_cp *cmd = (void *)buf; uint8_t len = sizeof(buf); uint16_t opcode; + memset(buf, 0, len); + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -8219,9 +8345,9 @@ ble_gap_set_connless_cte_transmit_params(uint8_t instance, const struct ble_gap_ cmd->cte_length = params->cte_length; cmd->cte_type = params->cte_type; cmd->cte_count = params->cte_count; - cmd->switching_pattern_len = params->switching_pattern_length; - if (params->antenna_ids && params->switching_pattern_length > 0) { - memcpy(cmd->switching_pattern, params->antenna_ids, params->switching_pattern_length); + cmd->switching_pattern_len = sp_len; + if (sp_len > 0) { + memcpy(cmd->switching_pattern, params->antenna_ids, sp_len); } return ble_hs_hci_cmd_tx(opcode, cmd, len, NULL, 0); @@ -8274,7 +8400,9 @@ ble_gap_set_connless_iq_sampling_enable(uint16_t sync_handle, uint8_t sampling_e cmd->max_sampled_ctes = max_sampled_ctes; cmd->slot_durations = cte_sampling_params->slot_durations; cmd->switching_pattern_len = cte_sampling_params->switching_pattern_length; - memcpy(cmd->antenna_ids, cte_sampling_params->antenna_ids, cte_sampling_params->switching_pattern_length); + if (cte_sampling_params->switching_pattern_length > 0) { + memcpy(cmd->antenna_ids, cte_sampling_params->antenna_ids, cte_sampling_params->switching_pattern_length); + } return ble_hs_hci_cmd_tx(opcode, cmd, len, &rsp, sizeof(rsp)); } @@ -8306,7 +8434,9 @@ ble_gap_set_conn_cte_recv_param(uint16_t conn_handle, uint8_t sampling_enable, c cmd->sampling_enable = sampling_enable; cmd->slot_durations = cte_sampling_params->slot_durations; cmd->switching_pattern_len = cte_sampling_params->switching_pattern_length; - memcpy(cmd->antenna_ids, cte_sampling_params->antenna_ids, cte_sampling_params->switching_pattern_length); + if (cte_sampling_params->switching_pattern_length > 0) { + memcpy(cmd->antenna_ids, cte_sampling_params->antenna_ids, cte_sampling_params->switching_pattern_length); + } return ble_hs_hci_cmd_tx(opcode, cmd, len, &rsp, sizeof(rsp)); } @@ -8449,11 +8579,7 @@ ble_gap_disc_tx_params(uint8_t own_addr_type, cmd.scan_itvl = htole16(disc_params->itvl); cmd.scan_window = htole16(disc_params->window); cmd.own_addr_type = own_addr_type; - /* LE_Set_Scan_Parameters only supports filter policies 0 and 1. - * Policies 2 and 3 are only valid for extended scanning. Mask to - * bit 0 so policy 2 (NO_WL_INITA) maps to 0 (accept all) and - * policy 3 (USE_WL_INITA) maps to 1 (use FAL). */ - cmd.filter_policy = disc_params->filter_policy & 1; + cmd.filter_policy = disc_params->filter_policy; opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_PARAMS); @@ -8478,7 +8604,13 @@ ble_gap_disc_cancel_no_lock(void) STATS_INC(ble_gap_stats, discover_cancel); -#if !MYNEWT_VAL(BLE_HOST_ALLOW_CONNECT_WITH_SCAN) +#if MYNEWT_VAL(BLE_HOST_ALLOW_CONNECT_WITH_SCAN) + /* op can be M_CONN while discovery remains active in the controller. */ + if (!ble_gap_master.scan_active) { + rc = BLE_HS_EALREADY; + goto done; + } +#else /* Without connect-with-scan, disc_active reliably tracks controller scan * state. Skip the HCI command if the host already knows scan is idle. */ if (!ble_gap_disc_active()) { @@ -8488,22 +8620,12 @@ ble_gap_disc_cancel_no_lock(void) #endif rc = ble_gap_disc_disable_tx(); -#if MYNEWT_VAL(BLE_HOST_ALLOW_CONNECT_WITH_SCAN) - /* With connect-with-scan enabled, ble_gap_master.op may have been - * overwritten to M_CONN while the controller was still scanning, making - * disc_active() return false even though scan is active on the - * controller. We therefore skip the disc_active guard above and always - * attempt the HCI scan-disable. If the controller rejects it with - * CMD_DISALLOWED the scan was already inactive — treat that as success - * so callers observe the same EALREADY-equivalent behavior. */ - if (rc == BLE_HS_HCI_ERR(BLE_ERR_CMD_DISALLOWED)) { - rc = 0; - } -#endif if (rc != 0) { goto done; } + ble_gap_master.scan_active = 0; + if (ble_gap_master.op == BLE_GAP_OP_M_DISC) { ble_gap_master_reset_state(); } @@ -8616,13 +8738,14 @@ ble_gap_ext_scan_params_to_hci(const struct ble_gap_ext_disc_params *params, } #endif -int -ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, - uint8_t filter_duplicates, uint8_t filter_policy, - uint8_t limited, - const struct ble_gap_ext_disc_params *uncoded_params, - const struct ble_gap_ext_disc_params *coded_params, - ble_gap_event_fn *cb, void *cb_arg) +#if NIMBLE_BLE_SCAN && MYNEWT_VAL(BLE_EXT_ADV) +static int +ble_gap_ext_disc_internal(uint8_t own_addr_type, uint16_t duration, uint16_t period, + uint8_t filter_duplicates, uint8_t filter_policy, + uint8_t limited, + const struct ble_gap_ext_disc_params *uncoded_params, + const struct ble_gap_ext_disc_params *coded_params, + ble_gap_event_fn *cb, void *cb_arg, bool legacy_discovery) { #if NIMBLE_BLE_SCAN && MYNEWT_VAL(BLE_EXT_ADV) struct ble_hs_hci_ext_scan_param ucp; @@ -8650,6 +8773,8 @@ ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, * defaults. */ + ble_gap_master.disc.observer = 0; + if (uncoded_params) { ble_gap_ext_scan_params_to_hci(uncoded_params, &ucp); ble_gap_ext_disc_fill_dflts(limited, &ucp); @@ -8696,7 +8821,7 @@ ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, } ble_gap_master.op = BLE_GAP_OP_M_DISC; - ble_gap_master.legacy_discovery = 0; + ble_gap_master.legacy_discovery = legacy_discovery; rc = ble_gap_ext_disc_enable_tx(1, filter_duplicates, duration, period); if (rc != 0) { @@ -8704,6 +8829,8 @@ ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, goto done; } + ble_gap_master.scan_active = 1; + rc = 0; done: @@ -8718,6 +8845,21 @@ done: #endif } +int +ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, + uint8_t filter_duplicates, uint8_t filter_policy, + uint8_t limited, + const struct ble_gap_ext_disc_params *uncoded_params, + const struct ble_gap_ext_disc_params *coded_params, + ble_gap_event_fn *cb, void *cb_arg) +{ + return ble_gap_ext_disc_internal(own_addr_type, duration, period, + filter_duplicates, filter_policy, limited, + uncoded_params, coded_params, cb, cb_arg, + false); +} +#endif + #if NIMBLE_BLE_SCAN && !MYNEWT_VAL(BLE_EXT_ADV) static void ble_gap_disc_fill_dflts(struct ble_gap_disc_params *disc_params) @@ -8813,12 +8955,11 @@ ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms, } } - rc = ble_gap_ext_disc(own_addr_type, duration_ms/10, 0, - disc_params->filter_duplicates, - disc_params->filter_policy, disc_params->limited, - &p, NULL, cb, cb_arg); - - ble_gap_master.legacy_discovery = 1; + rc = ble_gap_ext_disc_internal(own_addr_type, duration_ms/10, 0, + disc_params->filter_duplicates, + disc_params->filter_policy, + disc_params->limited, + &p, NULL, cb, cb_arg, true); return rc; #else @@ -8894,6 +9035,8 @@ ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms, goto done; } + ble_gap_master.scan_active = 1; + if (duration_ms != BLE_HS_FOREVER) { ble_gap_master_set_timer(duration_ticks); } @@ -9083,6 +9226,10 @@ ble_gap_ext_conn_create_conn_synced_tx(uint8_t own_addr_type, return BLE_HS_EINVAL; } + if (phy_mask & ~BLE_GAP_LE_PHY_ANY_MASK) { + return BLE_HS_EINVAL; + } + if (!(phy_mask & (BLE_HCI_LE_PHY_1M_PREF_MASK | BLE_HCI_LE_PHY_2M_PREF_MASK | BLE_HCI_LE_PHY_CODED_PREF_MASK))) { @@ -9111,7 +9258,7 @@ ble_gap_ext_conn_create_conn_synced_tx(uint8_t own_addr_type, } cmd->own_addr_type = own_addr_type; - cmd->init_phy_mask = phy_mask; + cmd->init_phy_mask = phy_mask & BLE_GAP_LE_PHY_ANY_MASK; cmd->adv_handle = advertising_handle; cmd->subevent = subevent; @@ -9334,6 +9481,9 @@ ble_gap_ext_conn_create_tx( BLE_HCI_LE_PHY_CODED_PREF_MASK))) { return BLE_HS_EINVAL; } + if (phy_mask & ~BLE_GAP_LE_PHY_ANY_MASK) { + return BLE_HS_EINVAL; + } cmd = (void *) buf; params = cmd->conn_params; @@ -9358,8 +9508,7 @@ ble_gap_ext_conn_create_tx( } cmd->own_addr_type = own_addr_type; - /* Mask to the three valid PHY bits so BLE_GAP_LE_PHY_ANY_MASK (0x0F) works. */ - cmd->init_phy_mask = phy_mask & 0x07; + cmd->init_phy_mask = phy_mask & BLE_GAP_LE_PHY_ANY_MASK; if (phy_mask & BLE_GAP_LE_PHY_1M_MASK) { rc = ble_gap_check_conn_params(BLE_HCI_LE_PHY_1M, phy_1m_conn_params); @@ -9542,12 +9691,13 @@ ble_gap_ext_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, STATS_INC(ble_gap_stats, initiate); - if (phy_mask == 0) { - return BLE_HS_EINVAL; - } - ble_hs_lock(); + if (phy_mask == 0) { + rc = BLE_HS_EINVAL; + goto done; + } + #if MYNEWT_VAL(OPTIMIZE_MULTI_CONN) /* If the optimization is enabled, we disallow to invoke this API directly. * See @ble_gap_multi_connect() @@ -9815,11 +9965,9 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, goto done; } - if (conn_params != NULL) { - rc = ble_gap_check_conn_params(BLE_HCI_LE_PHY_1M, conn_params); - if (rc != 0) { - goto done; - } + rc = ble_gap_check_conn_params(BLE_HCI_LE_PHY_1M, conn_params); + if (rc != 0) { + goto done; } rc = ble_hs_id_use_addr(own_addr_type); @@ -10473,7 +10621,7 @@ ble_gap_rx_param_req(const struct ble_hci_ev_le_subev_rem_conn_param_req *ev) if (rc != 0) { ble_gap_update_failed(conn_handle, rc); } - } else if (rc != BLE_HS_ENOTCONN) { + } else { ble_gap_tx_param_neg_reply(conn_handle, BLE_ERR_CONN_PARMS); } #endif @@ -10825,8 +10973,8 @@ ble_gap_dev_authorization(uint16_t conn_handle, bool authorized) if (!(conn->bhc_sec_state.authenticated)) { /* Authorization requires prior MITM-protected authentication. */ ble_hs_unlock(); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EAUTHEN); - return BLE_HS_EAUTHEN; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EAUTHOR); + return BLE_HS_EAUTHOR; } // Update connection security flags @@ -10897,25 +11045,32 @@ ble_gap_encryption_initiate(uint16_t conn_handle, } #if NIMBLE_BLE_SM && MYNEWT_VAL(BLE_SMP_ID_RESET) +static int +ble_gap_reset_irk_bond_iter(int obj_type, union ble_store_value *val, void *arg) +{ + *(int *)arg = 1; + return 1; +} + static void ble_gap_reset_irk(void) { - ble_addr_t oldest_peer_id_addr; struct ble_store_value_local_irk value_local_irk; struct ble_store_key_local_irk key_local_irk; uint8_t local_id[BLE_DEV_ADDR_LEN]; int have_local_id; - int rc, num_peers = 0; + int rc; + int have_bonds = 0; uint8_t tmp_addr[6]; - /* Only need to know whether any bonds exist; use a single-element buffer. */ - rc = ble_store_util_bonded_peers(&oldest_peer_id_addr, &num_peers, 1); + rc = ble_store_iterate(BLE_STORE_OBJ_TYPE_OUR_SEC, + ble_gap_reset_irk_bond_iter, &have_bonds); if (rc != 0) { return; } - if (num_peers != 0) { - return ; + if (have_bonds) { + return; } memset(&key_local_irk, 0, sizeof key_local_irk); @@ -11023,8 +11178,7 @@ ble_gap_unpair(const ble_addr_t *peer_addr) #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) rc = ble_hs_pvcy_remove_entry(new_addr->type, new_addr->val); if (rc != 0) { - BLE_HS_LOG(ERROR, "Error removing privacy entry, rc = %x\n", rc); - err = rc; + BLE_HS_LOG(WARN, "Privacy entry not found or removal failed (best-effort), rc = %x\n", rc); } #endif } else { @@ -11032,8 +11186,7 @@ ble_gap_unpair(const ble_addr_t *peer_addr) /* If no rpa_rec, remove original addr directly */ rc = ble_hs_pvcy_remove_entry(peer_addr->type, peer_addr->val); if (rc != 0) { - BLE_HS_LOG(ERROR, "Error removing privacy entry, rc = %x\n", rc); - err = rc; + BLE_HS_LOG(WARN, "Privacy entry not found or removal failed (best-effort), rc = %x\n", rc); } #endif } @@ -11322,7 +11475,6 @@ ble_gap_enc_event(uint16_t conn_handle, int status, } #endif -#endif /* MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) */ #endif } @@ -11363,10 +11515,7 @@ ble_gap_repeat_pairing_event(const struct ble_gap_repeat_pairing *rp) /* Connection was closed between the SM unlock and this call. */ return rc; } - rc = ble_store_util_delete_peer(&desc.peer_id_addr); - if (rc != 0) { - return rc; - } + ble_store_util_delete_peer(&desc.peer_id_addr); return BLE_GAP_REPEAT_PAIRING_RETRY; #else struct ble_gap_event event; @@ -11731,7 +11880,10 @@ ble_gap_end_test_evt(const void *buf, uint8_t len) status = cmd_complete->status; ptr = (uint8_t *)cmd_complete->return_params; - num_pkt = get_le16(ptr); + num_pkt = 0; + if (len >= sizeof(*cmd_complete) + sizeof(num_pkt)) { + num_pkt = get_le16(ptr); + } memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_TEST_UPDATE; @@ -11794,15 +11946,21 @@ ble_gap_preempt_no_lock(void) #endif #if NIMBLE_BLE_CONNECT - rc = ble_gap_conn_cancel_no_lock(); - if (rc == 0) { - ble_gap_master.preempted_op = BLE_GAP_OP_M_CONN; + { + int was_cancelling = ble_gap_master.conn.cancel; + + rc = ble_gap_conn_cancel_no_lock(); + if ((rc == 0 || + (rc == BLE_HS_EALREADY && ble_gap_conn_active())) && + !was_cancelling) { + ble_gap_master.preempted_op = BLE_GAP_OP_M_CONN; + } } #endif #if NIMBLE_BLE_SCAN rc = ble_gap_disc_cancel_no_lock(); - if (rc == 0) { + if (rc == 0 && ble_gap_master.preempted_op != BLE_GAP_OP_M_CONN) { ble_gap_master.preempted_op = BLE_GAP_OP_M_DISC; } #endif @@ -11825,7 +11983,9 @@ ble_gap_preempt(void) { ble_hs_lock(); - ble_gap_preempted++; + if (ble_gap_preempted < UINT8_MAX) { + ble_gap_preempted++; + } if (ble_gap_preempted == 1) { ble_gap_preempt_no_lock(); } @@ -11926,6 +12086,7 @@ ble_gap_event_listener_register(struct ble_gap_event_listener *listener, { struct ble_gap_event_listener *evl = NULL; int rc; + int count = 0; ble_hs_lock(); @@ -11935,6 +12096,17 @@ ble_gap_event_listener_register(struct ble_gap_event_listener *listener, } } + if (!evl && fn) { + SLIST_FOREACH(evl, &ble_gap_event_listener_list, link) { + count++; + } + if (count >= BLE_GAP_EVENT_LISTENER_MAX) { + rc = BLE_HS_ENOMEM; + ble_hs_unlock(); + return rc; + } + } + if (!evl) { if (fn) { memset(listener, 0, sizeof(*listener)); @@ -11960,6 +12132,12 @@ ble_gap_event_listener_unregister(struct ble_gap_event_listener *listener) struct ble_gap_event_listener *evl = NULL; int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gap_vars == NULL) { + return BLE_HS_ENOENT; + } +#endif + /* * We check if element exists on the list only for sanity to let caller * know whether it registered its listener before. @@ -11990,8 +12168,8 @@ static int ble_gap_event_listener_call(struct ble_gap_event *event) { struct ble_gap_event_listener *evl; - ble_gap_event_fn *fns[16]; - void *args[16]; + ble_gap_event_fn *fns[BLE_GAP_EVENT_LISTENER_MAX]; + void *args[BLE_GAP_EVENT_LISTENER_MAX]; int count = 0; int i; @@ -12001,6 +12179,11 @@ ble_gap_event_listener_call(struct ble_gap_event *event) fns[count] = evl->fn; args[count] = evl->arg; count++; + } else { + BLE_HS_LOG(ERROR, "GAP event listener limit reached (%d). Truncating.\n", + (int)(sizeof(fns) / sizeof(fns[0]))); + assert(0); + break; } } ble_hs_unlock(); @@ -12045,12 +12228,36 @@ ble_gap_create_cis(uint8_t cis_cnt, const struct ble_hci_le_create_cis_params *p ble_gap_event_fn *cb, void *cb_arg) { int rc; + int i; + + if (params == NULL || cis_cnt == 0) { + return BLE_HS_EINVAL; + } ble_hs_lock(); + /* Pre-validate that enough free CIS state slots exist for handles that are + * not already tracked. Checking before the HCI command ensures the host + * state stays consistent with the controller. */ + int new_needed = 0; + int free_slots = 0; + for (i = 0; i < cis_cnt; i++) { + if (!ble_gap_cis_find(params[i].cis_handle)) { + new_needed++; + } + } + for (i = 0; i < MYNEWT_VAL(BLE_ISO_CIS); i++) { + if (!ble_gap_cis[i].active) { + free_slots++; + } + } + if (free_slots < new_needed) { + ble_hs_unlock(); + return BLE_HS_ENOMEM; + } + rc = ble_hs_hci_create_cis(cis_cnt, params); if (!rc) { - int i; for (i = 0; i < cis_cnt; i++) { struct ble_gap_cis_state *cis = ble_gap_cis_find(params[i].cis_handle); if (!cis) { @@ -12076,6 +12283,24 @@ ble_gap_accept_cis_request(uint16_t cis_handle, ble_gap_event_fn *cb, void *cb_a ble_hs_lock(); + /* Pre-validate that a CIS state slot is available before sending the HCI + * command. This prevents a silent failure where the controller creates the + * CIS but no callback is ever invoked because the host has no state entry. */ + if (!ble_gap_cis_find(cis_handle)) { + int i; + bool has_free = false; + for (i = 0; i < MYNEWT_VAL(BLE_ISO_CIS); i++) { + if (!ble_gap_cis[i].active) { + has_free = true; + break; + } + } + if (!has_free) { + ble_hs_unlock(); + return BLE_HS_ENOMEM; + } + } + rc = ble_hs_hci_accept_cis_request(cis_handle); if (!rc) { cis = ble_gap_cis_find(cis_handle); @@ -12373,6 +12598,7 @@ err: ble_gap_vars = NULL; } #else + os_mempool_unregister(&ble_gap_update_entry_pool); if (mutex_initialized) { ble_npl_mutex_deinit(&preempt_done_mutex); } @@ -12405,7 +12631,8 @@ ble_gap_enh_read_transmit_power_level(uint16_t conn_handle, uint8_t phy, uint8_t return rc; } - *out_status = rc; + /* HCI status is always 0 here; non-zero returns early above */ + *out_status = 0; *out_phy = rsp.phy; *out_curr_tx_power_level = rsp.curr_tx_power_level; @@ -12999,8 +13226,10 @@ static inline bool is_rpa(const uint8_t *addr) bool ble_gap_rpa_resolve(uint8_t *rpa, uint8_t *ida, uint8_t *addr_type) { +#if MYNEWT_VAL(BLE_HS_PVCY) struct ble_store_value_local_irk value = {0}; struct ble_store_key_local_irk local_irk_key = {0}; +#endif #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) > 0 int rc; int count = 0; @@ -13038,6 +13267,7 @@ bool ble_gap_rpa_resolve(uint8_t *rpa, uint8_t *ida, uint8_t *addr_type) } #endif +#if MYNEWT_VAL(BLE_HS_PVCY) /* ---- Step 2: Try our (local) IRKs ---- */ if (ble_store_read_local_irk(&local_irk_key, &value) == 0) { if (ble_hs_pvcy_resolve_with_irk(rpa, value.irk)) { @@ -13047,6 +13277,7 @@ bool ble_gap_rpa_resolve(uint8_t *rpa, uint8_t *ida, uint8_t *addr_type) return true; } } +#endif return false; /* No match */ } @@ -13079,7 +13310,7 @@ ble_gap_frame_space_update(uint16_t conn_handle, return BLE_HS_EINVAL; } - if ((phys & ~0x07) || (phys == 0)) { + if ((phys & ~BLE_GAP_LE_PHY_ANY_MASK) || (phys == 0)) { return BLE_HS_EINVAL; } diff --git a/nimble/host/src/ble_gap_priv.h b/nimble/host/src/ble_gap_priv.h index 515b52d9d..89381d0d7 100644 --- a/nimble/host/src/ble_gap_priv.h +++ b/nimble/host/src/ble_gap_priv.h @@ -84,10 +84,12 @@ void ble_gap_rx_le_scan_timeout(void); void ble_gap_rx_ext_adv_report(struct ble_gap_ext_disc_desc *desc); void ble_gap_rx_adv_set_terminated(const struct ble_hci_ev_le_subev_adv_set_terminated *ev); #if MYNEWT_VAL(BLE_PERIODIC_ADV) -void ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev); +void ble_gap_rx_peroidic_adv_sync_estab(const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev, uint8_t subevent); void ble_gap_rx_periodic_adv_rpt(const struct ble_hci_ev_le_subev_periodic_adv_rpt *ev); void ble_gap_rx_periodic_adv_sync_lost(const struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev); -void ble_gap_rx_periodic_adv_sync_transfer(const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev); +void ble_gap_rx_periodic_adv_sync_transfer( + const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev, + uint8_t subevent); #endif #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_BIGINFO_REPORTS) && !MYNEWT_VAL(BLE_ISO) void ble_gap_rx_biginfo_adv_rpt(const struct ble_hci_ev_le_subev_biginfo_adv_report *ev); diff --git a/nimble/host/src/ble_gatt_priv.h b/nimble/host/src/ble_gatt_priv.h index eaa08f52f..ccc0493d0 100644 --- a/nimble/host/src/ble_gatt_priv.h +++ b/nimble/host/src/ble_gatt_priv.h @@ -205,12 +205,13 @@ struct ble_gatt_resources { */ uint16_t cccds; -#if MYNEWT_VAL(BLE_CPFD_CAFD) /** * Number of characteristic extended properties descriptors. Each of * these also contributes to the total descriptor count. */ uint16_t ceps; + +#if MYNEWT_VAL(BLE_CPFD_CAFD) /** * Number of client presentation format descriptors. Each of * these also contributes to the total descriptor count. diff --git a/nimble/host/src/ble_gattc.c b/nimble/host/src/ble_gattc.c index 7307a858b..373327d11 100644 --- a/nimble/host/src/ble_gattc.c +++ b/nimble/host/src/ble_gattc.c @@ -244,10 +244,11 @@ struct ble_gattc_proc { }; +STAILQ_HEAD(ble_gattc_proc_list, ble_gattc_proc); + #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) static struct ble_gattc_proc_list temp_proc_list; #endif -STAILQ_HEAD(ble_gattc_proc_list, ble_gattc_proc); /** * Error functions - these handle an incoming ATT error response and apply it @@ -925,7 +926,9 @@ ble_gattc_proc_free(struct ble_gattc_proc *proc) static void ble_gattc_proc_insert(struct ble_gattc_proc *proc, bool insert_head) { +#if !MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) ble_gattc_dbg_assert_proc_not_inserted(proc); +#endif ble_hs_lock(); { @@ -947,6 +950,7 @@ ble_gattc_proc_insert(struct ble_gattc_proc *proc, bool insert_head) } #endif } + if (insert_head) { STAILQ_INSERT_HEAD(&ble_gattc_procs, proc, next); } else { @@ -955,6 +959,37 @@ ble_gattc_proc_insert(struct ble_gattc_proc *proc, bool insert_head) ble_hs_unlock(); } +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) +static bool +ble_gattc_proc_remove_preempted(struct ble_gattc_proc *proc) +{ + struct ble_gattc_proc *cur; + bool removed = false; + + ble_hs_lock(); + STAILQ_FOREACH(cur, &temp_proc_list, next) { + if (cur == proc) { + STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); + removed = true; + break; + } + } + + if (!removed) { + STAILQ_FOREACH(cur, &ble_gattc_procs, next) { + if (cur == proc) { + STAILQ_REMOVE(&ble_gattc_procs, proc, ble_gattc_proc, next); + removed = true; + break; + } + } + } + ble_hs_unlock(); + + return removed; +} +#endif + static void ble_gattc_proc_set_exp_timer(struct ble_gattc_proc *proc) { @@ -1040,6 +1075,11 @@ static ble_gattc_err_fn * ble_gattc_err_dispatch_get(uint8_t op) { BLE_HS_DBG_ASSERT(op < BLE_GATT_OP_CNT); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_ctx == NULL || ble_gattc_err_dispatch == NULL) { + return NULL; + } +#endif return ble_gattc_err_dispatch[op]; } @@ -1471,7 +1511,6 @@ ble_gattc_resume_procs(void) { struct ble_gattc_proc_list stall_list; struct ble_gattc_proc *proc; - struct ble_gattc_proc *proc_next; ble_gattc_resume_fn *resume_cb; int rc; @@ -1489,9 +1528,8 @@ ble_gattc_resume_procs(void) proc->flags &= ~BLE_GATTC_PROC_F_STALLED; rc = resume_cb(proc); ble_gattc_process_status(proc, rc, false); - - proc = proc_next; } + } static int32_t @@ -2055,9 +2093,9 @@ done: STATS_INC(ble_gattc_stats, disc_all_svcs_fail); #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } } #endif } @@ -2471,9 +2509,16 @@ ble_gattc_find_inc_svcs_rx_read_rsp(struct ble_gattc_proc *proc, int status, /* Proceed to the next service. */ proc->find_inc_svcs.cur_start = 0; proc->find_inc_svcs.cur_end = 0; + + /* Check if we've exhausted the handle range before resuming */ + if (proc->find_inc_svcs.prev_handle >= proc->find_inc_svcs.end_handle) { + ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL); + return BLE_HS_EDONE; + } + rc = ble_gattc_find_inc_svcs_resume(proc); if (rc != 0) { - goto err; + return BLE_HS_EDONE; } return 0; @@ -2650,9 +2695,9 @@ done: STATS_INC(ble_gattc_stats, find_inc_svcs_fail); #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); - ble_hs_unlock(); + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } } #endif } @@ -2841,10 +2886,12 @@ ble_gattc_disc_all_chrs_rx_complete(struct ble_gattc_proc *proc, int status) return BLE_HS_EDONE; } - /* We can stop discovery if there are less than 2 attributes left since - * complete characteristic requires at least 2 handles. + /* Per Core Spec, discovery should continue until Attribute Not Found + * or until the next request would start beyond end_handle. The previous + * `+ 1 >= end_handle` short-circuit skipped the final spec-mandated + * READ_BY_TYPE when prev_handle == end_handle - 1. */ - if (proc->disc_all_chrs.prev_handle + 1 >= proc->disc_all_chrs.end_handle) { + if (proc->disc_all_chrs.prev_handle >= proc->disc_all_chrs.end_handle) { /* Characteristic discovery complete. */ ble_gattc_disc_all_chrs_cb(proc, BLE_HS_EDONE, 0, NULL); return BLE_HS_EDONE; @@ -2904,15 +2951,14 @@ ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle, } done: -#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) - if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); - } -#endif - if (rc != 0) { +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) + if (proc != NULL) { + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } + } +#endif STATS_INC(ble_gattc_stats, disc_all_chrs_fail); } @@ -3134,6 +3180,9 @@ ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle, struct ble_gattc_proc *proc; int rc; +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) + bool in_temp_list = false; +#endif #if MYNEWT_VAL(BLE_GATT_CACHING) rc = ble_gattc_cache_conn_search_chrs_by_uuid(conn_handle, start_handle, end_handle, uuid, cb, cb_arg); @@ -3163,6 +3212,7 @@ ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle, #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) ble_hs_lock(); STAILQ_INSERT_TAIL(&temp_proc_list, proc, next); + in_temp_list = true; ble_hs_unlock(); #endif ble_gattc_log_disc_chr_uuid(proc); @@ -3176,10 +3226,10 @@ done: if (rc != 0) { STATS_INC(ble_gattc_stats, disc_chrs_uuid_fail); #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) - if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); - ble_hs_unlock(); + if (proc != NULL && in_temp_list) { + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } } #endif } @@ -3303,8 +3353,8 @@ ble_gattc_disc_all_dscs_rx_idata(struct ble_gattc_proc *proc, ble_gattc_dbg_assert_proc_not_inserted(proc); if (idata->attr_handle <= proc->disc_all_dscs.prev_handle || - idata->attr_handle > proc->disc_all_dscs.end_handle || - idata->attr_handle < proc->disc_all_dscs.chr_val_handle) { + idata->attr_handle > proc->disc_all_dscs.end_handle) { + /* Peer sent descriptors out of order or out of range; terminate. */ rc = BLE_HS_EBADDATA; goto done; @@ -3405,18 +3455,17 @@ ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t start_handle, } done: - /* One ble_gattc_proc cannot be part of multiple linked lists. - * Hence it needs to be removed before going into ble_gattc_process_status. - */ -#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) - if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); - } -#endif - if (rc != 0) { + /* One ble_gattc_proc cannot be part of multiple linked lists. + * Hence it needs to be removed before going into ble_gattc_process_status. + */ +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) + if (proc != NULL) { + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } + } +#endif STATS_INC(ble_gattc_stats, disc_all_dscs_fail); } @@ -3430,7 +3479,8 @@ int ble_gattc_check_valid_param(uint16_t num, uint16_t offset) if (num == 0) { return BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND); } else if (offset >= num) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INVALID_OFFSET); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, + BLE_HS_ATT_ERR(BLE_ATT_ERR_INVALID_OFFSET)); return BLE_HS_ATT_ERR(BLE_ATT_ERR_INVALID_OFFSET); } @@ -3475,7 +3525,7 @@ static void ble_gattc_fill_gatt_db_conversion(uint16_t *count, uint16_t num, esp } break; } - case BLE_GATT_DB_INCLUDED_SERVICE: { + case ESP_BLE_GATT_DB_INCLUDED_SERVICE: { ble_gattc_included_svc_elem_t *incl_result = (ble_gattc_included_svc_elem_t *)result; for (int i = 0; i < db_size; i++) { incl_result->handle = db[offset + i].handle; @@ -3825,10 +3875,6 @@ int ble_gattc_get_attr_count(uint16_t conn_handle, esp_ble_gatt_db_attr_type_t t if (count == NULL) { return BLE_HS_EINVAL; } - if (ble_gattc_cache_conn_find(conn_handle) == NULL) { - *count = 0; - return BLE_HS_ENOTCONN; - } if (type == ESP_BLE_GATT_DB_ALL) { ble_gattc_get_db_size(conn_handle, start_handle, end_handle, count); @@ -3845,13 +3891,12 @@ int ble_gattc_get_db(uint16_t conn_handle, uint16_t num = 0; ble_gattc_db_elem_t *db = NULL; + /* Not valid (issue-137): null check here is correct; downstream BLE_ATT_ERR_ATTR_NOT_FOUND conflation is pre-existing, not introduced by this MR */ if (count == NULL || result == NULL) { return BLE_HS_EINVAL; } - ble_hs_lock(); ble_gattc_get_cached_gatt_db(conn_handle, start_handle, end_handle, &db, &num, count); - ble_hs_unlock(); if (num == 0) { return BLE_ATT_ERR_ATTR_NOT_FOUND; @@ -4086,7 +4131,7 @@ ble_gattc_read_uuid_rx_adata(struct ble_gattc_proc *proc, attr.om = ble_hs_mbuf_from_flat(adata->value, adata->value_len); if (attr.om == NULL) { rc = BLE_HS_ENOMEM; - ble_gattc_read_uuid_cb(proc, rc, adata->att_handle, NULL); + rc = ble_gattc_read_uuid_cb(proc, rc, adata->att_handle, NULL); } else { rc = 0; rc = ble_gattc_read_uuid_cb(proc, rc, adata->att_handle, &attr); @@ -4143,6 +4188,9 @@ ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle, struct ble_gattc_proc *proc; int rc; +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) + bool in_temp_list = false; +#endif STATS_INC(ble_gattc_stats, read_uuid); @@ -4166,6 +4214,7 @@ ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle, #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) ble_hs_lock(); STAILQ_INSERT_TAIL(&temp_proc_list, proc, next); + in_temp_list = true; ble_hs_unlock(); #endif ble_gattc_log_read_uuid(start_handle, end_handle, uuid); @@ -4178,10 +4227,10 @@ done: if (rc != 0) { STATS_INC(ble_gattc_stats, read_uuid_fail); #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) - if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); - ble_hs_unlock(); + if (proc != NULL && in_temp_list) { + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } } #endif } @@ -4392,9 +4441,9 @@ done: STATS_INC(ble_gattc_stats, read_long_fail); #if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) if (proc != NULL) { - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); - ble_hs_unlock(); + if (!ble_gattc_proc_remove_preempted(proc)) { + proc = NULL; + } } #endif } @@ -4448,32 +4497,39 @@ 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 || attr_len > OS_MBUF_PKTLEN(*om)) { + if (attr_len > BLE_ATT_ATTR_MAX_LEN) { status = BLE_HS_EBADDATA; break; } - attr[i].om = os_msys_get_pkthdr(attr_len, 0); - if (!attr[i].om) { - status = BLE_HS_ENOMEM; - break; - } + { + uint16_t copy_len = 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, - * cleanup any partial data and set status if not set already - */ - if (i < proc->read_mult.num_handles || OS_MBUF_PKTLEN(*om) != 0) { - for (i = 0; i < proc->read_mult.num_handles; i++) { - os_mbuf_free_chain(attr[i].om); - attr[i].om = NULL; + if (copy_len > OS_MBUF_PKTLEN(*om)) { + copy_len = OS_MBUF_PKTLEN(*om); } + attr[i].om = os_msys_get_pkthdr(copy_len, 0); + if (!attr[i].om) { + status = BLE_HS_ENOMEM; + break; + } + + rc = os_mbuf_appendfrom(attr[i].om, *om, 0, copy_len); + if (rc) { + status = BLE_HS_ENOMEM; + break; + } + + os_mbuf_adj(*om, copy_len); + + if (copy_len < attr_len) { + i++; + break; + } + } + } + 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); @@ -4716,7 +4772,7 @@ ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle, cid = ble_eatt_get_available_chan_cid(conn_handle, BLE_GATT_OP_DUMMY); rc = ble_att_clt_tx_write_cmd(conn_handle, cid, attr_handle, txom); if (rc != 0) { - STATS_INC(ble_gattc_stats, write); + STATS_INC(ble_gattc_stats, write_no_rsp_fail); } ble_eatt_release_chan(conn_handle, cid); @@ -4762,7 +4818,6 @@ ble_gattc_signed_write(uint16_t conn_handle, uint16_t attr_handle, } int rc; - uint16_t cid; struct ble_store_value_sec value_sec; struct ble_store_key_sec key_sec; struct ble_gap_conn_desc desc; @@ -4797,11 +4852,11 @@ ble_gattc_signed_write(uint16_t conn_handle, uint16_t attr_handle, /* Converting the csrk to little endian */ swap_buf(csrk, value_sec.csrk, 16); - cid = ble_eatt_get_available_chan_cid(conn_handle, BLE_GATT_OP_DUMMY); - rc = ble_att_clt_tx_signed_write_cmd(conn_handle, cid, attr_handle, + rc = ble_att_clt_tx_signed_write_cmd(conn_handle, BLE_L2CAP_CID_ATT, attr_handle, csrk, value_sec.sign_counter, txom); - txom = NULL; - ble_eatt_release_chan(conn_handle, cid); + if (rc == 0) { + txom = NULL; + } if (rc != 0) { goto err; } @@ -5201,7 +5256,7 @@ ble_gattc_write_long_rx_exec(struct ble_gattc_proc *proc, int status) * response. */ ble_gattc_write_long_cb(proc, BLE_HS_EBADDATA, 0); - return BLE_HS_EBADDATA; + return BLE_HS_EDONE; } ble_gattc_write_long_cb(proc, status, 0); @@ -5328,7 +5383,8 @@ ble_gattc_write_reliable_tx(struct ble_gattc_proc *proc) { struct ble_gatt_attr *attr; struct os_mbuf *om; - int16_t max_sz; + uint16_t max_sz; + uint16_t mtu; int attr_idx; int rc; @@ -5346,12 +5402,12 @@ ble_gattc_write_reliable_tx(struct ble_gattc_proc *proc) attr = proc->write_reliable.attrs + attr_idx; - max_sz = ble_att_mtu_by_cid(proc->conn_handle, proc->cid) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ; - if (max_sz <= 0) { - /* Not connected. */ + mtu = ble_att_mtu_by_cid(proc->conn_handle, proc->cid); + if (mtu <= BLE_ATT_PREP_WRITE_CMD_BASE_SZ) { rc = BLE_HS_ENOTCONN; goto done; } + max_sz = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ; proc->write_reliable.length = min(max_sz, OS_MBUF_PKTLEN(attr->om) - attr->offset); @@ -5390,7 +5446,10 @@ ble_gattc_write_reliable_resume(struct ble_gattc_proc *proc) status = ble_gattc_write_reliable_tx(proc); rc = ble_gattc_process_resume_status(proc, status); if (rc != 0) { - ble_gattc_write_reliable_cb(proc, rc, 0); + /* Fire error callback here so ble_gattc_resume_procs (which calls + * ble_gattc_process_status with call_cb=false) does not silently + * drop the proc without notifying the application. */ + ble_gattc_write_reliable_err(proc, rc, 0); return rc; } @@ -5410,8 +5469,14 @@ ble_gattc_write_reliable_err(struct ble_gattc_proc *proc, int status, /* If we have successfully queued any data, and the failure occurred before * we could send the execute write command, then erase all queued data * BEFORE invoking the application callback to keep peer state consistent. + * cur_attr is incremented only after an attribute is fully queued; offset + * is advanced only after a verified Prepare Write Response. BLE_HS_EBADDATA + * means the peer queued data but local verification failed — cancel anyway. */ - if (proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) { + if ((proc->write_reliable.cur_attr > 0 || + proc->write_reliable.attrs[proc->write_reliable.cur_attr].offset > 0 || + status == BLE_HS_EBADDATA) && + proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) { ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid, BLE_ATT_EXEC_WRITE_F_CANCEL); @@ -5477,6 +5542,8 @@ ble_gattc_write_reliable_rx_prep(struct ble_gattc_proc *proc, } rc = ble_gattc_write_reliable_resume(proc); if (rc != 0) { + /* resume() already called ble_gattc_write_reliable_err(); skip err: + * label to avoid firing the application callback a second time. */ return BLE_HS_EDONE; } @@ -5556,9 +5623,11 @@ done: } /* Free supplied mbufs in case something failed. */ - for (i = 0; i < num_attrs; i++) { - os_mbuf_free_chain(attrs[i].om); - attrs[i].om = NULL; + if (attrs != NULL) { + for (i = 0; i < num_attrs; i++) { + os_mbuf_free_chain(attrs[i].om); + attrs[i].om = NULL; + } } ble_gattc_process_status(proc, rc, false); @@ -5626,10 +5695,8 @@ ble_gatts_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle, rc = BLE_HS_ENOMEM; goto done; } - ble_hs_lock(); rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, chr_val_handle, 0, &txom, NULL); - ble_hs_unlock(); if (rc != 0) { /* Fatal error; application disallowed attribute read. */ rc = BLE_HS_EAPP; @@ -5656,7 +5723,6 @@ done: return rc; } - int ble_gatts_notify_multiple_custom(uint16_t conn_handle, size_t chr_count, @@ -5674,7 +5740,6 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, uint16_t mtu = ble_att_mtu(conn_handle) - 1; struct os_mbuf *txom; struct ble_hs_conn *conn; - bool free_unused_tuples = false; txom = ble_hs_mbuf_att_pkt(); if (txom == NULL) { @@ -5710,7 +5775,6 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, rc = ble_att_svr_read_local(tuples[i].handle, &tuples[i].value); if (rc != 0) { BLE_HS_LOG(ERROR, "Attribute read failed (err=0x%02x)", rc); - free_unused_tuples = true; goto done; } } @@ -5733,7 +5797,13 @@ 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) { /* Flush any previously buffered characteristic first to maintain ordering */ if (cur_chr_cnt == 1) { - rc = ble_att_clt_tx_multi_notify(conn_handle, txom); + /* Strip the 4-byte handle+length header and send as a standard + * single-value notification: the spec requires at least two + * handle-value pairs in ATT_MULTIPLE_HANDLE_VALUE_NTF. */ + os_mbuf_adj(txom, 4); + rc = ble_att_clt_tx_notify(conn_handle, + tuples[last_appended_idx].handle, + txom); txom = NULL; if (rc != 0) { goto done; @@ -5767,11 +5837,19 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, } /* Handle */ - os_mbuf_append(txom, &tuples[i].handle, sizeof(uint16_t)); + rc = os_mbuf_append(txom, &tuples[i].handle, sizeof(uint16_t)); + if (rc != 0) { + rc = BLE_HS_ENOMEM; + goto done; + } /* Length */ - os_mbuf_append(txom, &OS_MBUF_PKTLEN(tuples[i].value), - sizeof(uint16_t)); + rc = os_mbuf_append(txom, &OS_MBUF_PKTLEN(tuples[i].value), + sizeof(uint16_t)); + if (rc != 0) { + rc = BLE_HS_ENOMEM; + goto done; + } /* Value */ os_mbuf_concat(txom, tuples[i].value); @@ -5786,7 +5864,7 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, rc = ble_att_clt_tx_notify(conn_handle, tuples[last_appended_idx].handle, txom); txom = NULL; - } else { + } else if (cur_chr_cnt >= 2) { rc = ble_att_clt_tx_multi_notify(conn_handle, txom); txom = NULL; } @@ -5799,13 +5877,6 @@ done: tuples[i].value = NULL; } STATS_INC(ble_gattc_stats, multi_notify_fail); - if (free_unused_tuples) { - for (i = 0; i < chr_count; i++) { - if (tuples[i].value != NULL) { - os_mbuf_free_chain(tuples[i].value); - } - } - } } os_mbuf_free_chain(txom); @@ -5848,6 +5919,13 @@ ble_gattc_cccd_write_complete_cb(uint16_t conn_handle, "status=%d conn_handle=%d attr_handle=%d", error->status, conn_handle, attr->handle); + ble_hs_lock(); + gatt_proc_active = false; + ble_hs_unlock(); + if (arg != NULL) { + nimble_platform_mem_free(arg); + } + return 0; } @@ -5860,7 +5938,9 @@ static int ble_gattc_cccd_register_cb(uint16_t conn_handle, const struct ble_gat if (error->status == BLE_HS_EDONE) { /* GATT procedure completed reset active flag */ + ble_hs_lock(); gatt_proc_active = false; + ble_hs_unlock(); if (cccd_reg_flag) { nimble_platform_mem_free(cccd_reg_flag); } @@ -5876,15 +5956,22 @@ static int ble_gattc_cccd_register_cb(uint16_t conn_handle, const struct ble_gat rc = ble_gattc_write_flat(conn_handle, dsc->handle, cccd_val, sizeof(cccd_val), - ble_gattc_cccd_write_complete_cb, NULL); + ble_gattc_cccd_write_complete_cb, cccd_reg_flag); if (rc != 0) { BLE_HS_LOG(WARN, "Failed to Register for Notification, status = %d", rc); + ble_hs_lock(); + gatt_proc_active = false; + ble_hs_unlock(); + nimble_platform_mem_free(cccd_reg_flag); + return rc; } return BLE_HS_EDONE; } } else { BLE_HS_LOG(WARN, "GATT descriptor discovery failed with status = %d", error->status); + ble_hs_lock(); gatt_proc_active = false; + ble_hs_unlock(); if (cccd_reg_flag) { nimble_platform_mem_free(cccd_reg_flag); } @@ -5920,7 +6007,9 @@ int ble_gattc_register_for_notification(uint16_t conn_handle, uint16_t char_val_ int rc = ble_gattc_disc_all_dscs(conn_handle, char_val_handle, 0xffff, ble_gattc_cccd_register_cb, cccd_reg_flag); if (rc != 0) { + ble_hs_lock(); gatt_proc_active = false; + ble_hs_unlock(); nimble_platform_mem_free(cccd_reg_flag); } @@ -5936,7 +6025,9 @@ static int ble_gattc_cccd_unregister_cb(uint16_t conn_handle, const struct ble_g if (error->status == BLE_HS_EDONE) { /* GATT procedure completed reset active flag */ + ble_hs_lock(); gatt_proc_active = false; + ble_hs_unlock(); if (cccd_unreg_flag) { nimble_platform_mem_free(cccd_unreg_flag); } @@ -5952,15 +6043,22 @@ static int ble_gattc_cccd_unregister_cb(uint16_t conn_handle, const struct ble_g rc = ble_gattc_write_flat(conn_handle, dsc->handle, cccd_val, sizeof(cccd_val), - ble_gattc_cccd_write_complete_cb, NULL); + ble_gattc_cccd_write_complete_cb, cccd_unreg_flag); if (rc != 0) { BLE_HS_LOG(WARN, "Failed to Unregister for Notification, status = %d", rc); + ble_hs_lock(); + gatt_proc_active = false; + ble_hs_unlock(); + nimble_platform_mem_free(cccd_unreg_flag); + return rc; } return BLE_HS_EDONE; } } else { BLE_HS_LOG(WARN, "GATT descriptor discovery failed with status = %d", error->status); + ble_hs_lock(); gatt_proc_active = false; + ble_hs_unlock(); if (cccd_unreg_flag) { nimble_platform_mem_free(cccd_unreg_flag); } @@ -6009,252 +6107,6 @@ int ble_gattc_unregister_for_notification(uint16_t conn_handle, uint16_t char_va #endif //MYNEWT_VAL(BLE_STORE_MAX_CCCDS) #endif //MYNEWT_VAL(BLE_GATTC) -#if MYNEWT_VAL(BLE_GATTS) -int -ble_gatts_notify_multiple_custom(uint16_t conn_handle, - size_t chr_count, - struct ble_gatt_notif *tuples) -{ -#if !MYNEWT_VAL(BLE_GATT_NOTIFY) - return BLE_HS_ENOTSUP; -#endif - - int rc; - size_t i = 0; - uint16_t cur_chr_cnt = 0; - /* mtu = MTU - 1 octet (OP code) */ - uint16_t mtu = ble_att_mtu(conn_handle) - 1; - struct os_mbuf *txom; - struct ble_hs_conn *conn; - - txom = ble_hs_mbuf_att_pkt(); - if (txom == NULL) { - return BLE_HS_ENOMEM; - } - - conn = ble_hs_conn_find(conn_handle); - if (conn == NULL) { - return BLE_HS_ENOTCONN; - } - - /* Read missing values */ - for (i = 0; i < chr_count; i++) { - if (tuples->handle == 0) { - rc = BLE_HS_EINVAL; - goto done; - } - if (tuples[i].value == NULL) { - rc = ble_att_svr_read_local(tuples[i].handle, &tuples[i].value); - if (rc != 0) { - goto done; - } - } - } - - /* If peer does not support fall back to multiple single value - * Notifications */ - if ((conn->bhc_gatt_svr.peer_cl_sup_feat[0] & 0x04) == 0) { - for (i = 0; i < chr_count; i++) { - rc = ble_att_clt_tx_notify(conn_handle, tuples[chr_count].handle, - tuples[chr_count].value); - if (rc != 0) { - goto done; - } - } - } - - for (i = 0; i < chr_count; i++) { - if (txom->om_len + tuples[i].value->om_len > mtu && cur_chr_cnt < 2) { - rc = ble_att_clt_tx_notify(conn_handle, tuples[i].handle, - tuples[i].value); - if (rc != 0) { - goto done; - } - continue; - } else if (txom->om_len + tuples[i].value->om_len > mtu) { - rc = ble_att_clt_tx_notify_mult(conn_handle, txom); - if (rc != 0) { - goto done; - } - cur_chr_cnt = 0; - /* buffer was consumed, allocate new one */ - txom = ble_hs_mbuf_att_pkt(); - if (txom == NULL) { - return BLE_HS_ENOMEM; - } - } - - os_mbuf_append(txom, &tuples[i].handle, sizeof(uint16_t)); - os_mbuf_append(txom, &tuples[i].value->om_len, - sizeof(uint16_t)); - os_mbuf_concat(txom, tuples[i].value); - cur_chr_cnt++; - } - - if (cur_chr_cnt == 1) { - rc = ble_att_clt_tx_notify(conn_handle, tuples[chr_count].handle, - tuples[chr_count].value); - } else { - rc = ble_att_clt_tx_notify_mult(conn_handle, txom); - } - -done: - return rc; -} - -int -ble_gatts_notify_multiple(uint16_t conn_handle, - size_t num_handles, - const uint16_t *chr_val_handles) -{ -#if !MYNEWT_VAL(BLE_GATT_NOTIFY_MULTIPLE) - return BLE_HS_ENOTSUP; -#endif - int rc; - size_t i; - struct ble_gatt_notif tuples[num_handles]; - struct ble_hs_conn *conn; - - BLE_HS_LOG_DEBUG("conn_handle %d\n", conn_handle); - conn = ble_hs_conn_find(conn_handle); - if (conn == NULL) { - return BLE_HS_ENOTCONN; - } - - /** Skip sending to client that doesn't support this feature */ - BLE_HS_LOG_DEBUG("ble_gatts_notify_multiple: peer_cl_sup_feat %d\n", - conn->bhc_gatt_svr.peer_cl_sup_feat[0]); - if ((conn->bhc_gatt_svr.peer_cl_sup_feat[0] & 0x04) == 0) { - for (i = 0; i < num_handles; i++) { - rc = ble_gatts_notify(conn_handle, chr_val_handles[i]); - if (rc != 0) { - return rc; - } - } - return 0; - } - - for (i = 0; i < num_handles; i++) { - tuples[i].handle = chr_val_handles[i]; - tuples[i].value = NULL; - BLE_HS_LOG(DEBUG, "handle 0x%02x\n", tuples[i].handle); - } - - rc = ble_gatts_notify_multiple_custom(conn_handle, num_handles, tuples); - return rc; -} - -int -ble_gatts_multi_notify_custom(uint16_t conn_handle, - struct ble_gatt_notif_entry * tuples, uint16_t num_tuples) -{ -#if !MYNEWT_VAL(BLE_GATT_MULTI_NOTIFY) - return BLE_HS_ENOTSUP; -#endif - - int i; - int rc; - uint16_t mtu; - uint16_t pdu_size; - int split_at; - struct os_mbuf *om = NULL; - - STATS_INC(ble_gattc_stats, multi_notify); - ble_gattc_log_multi_notify(tuples, num_tuples); - - for (i = 0; i < num_tuples; i++) { - if (tuples[i].value == NULL) { - tuples[i].value = ble_hs_mbuf_att_pkt(); - if (tuples[i].value == NULL) { - rc = BLE_HS_ENOMEM; - goto done; - } - rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, - tuples[i].handle, 0, tuples[i].value, NULL); - if (rc != 0) { - BLE_HS_LOG(ERROR, "Attribute read failed (err=0x%02x), rc"); - goto done; - } - } - } - - mtu = ble_att_mtu(conn_handle); - pdu_size = sizeof(uint8_t); - split_at = 0; - - for (i = 0; i < num_tuples; i++) { - pdu_size += (2 * sizeof(uint16_t)) + OS_MBUF_PKTLEN(tuples[i].value); - if (pdu_size > mtu) { - if (i == split_at) { - rc = BLE_HS_ENOMEM; - goto done; - } - split_at = i; - i--; - pdu_size = sizeof(uint8_t); - } - } - - pdu_size = sizeof(uint8_t); - split_at = 0; - om = ble_hs_mbuf_att_pkt(); - if (om == NULL) { - rc = BLE_HS_ENOMEM; - goto done; - } - for (i = 0; i < num_tuples; i++) { - pdu_size += (2 * sizeof(uint16_t)) + OS_MBUF_PKTLEN(tuples[i].value); - if (pdu_size > mtu) { - rc = ble_att_clt_tx_multi_notify(conn_handle, om); - if (rc != 0) { - goto done; - } - split_at = i; - i--; - pdu_size = sizeof(uint8_t); - om = ble_hs_mbuf_att_pkt(); - if (om == NULL) { - rc = BLE_HS_ENOMEM; - goto done; - } - continue; - } - rc = os_mbuf_copyinto(om, OS_MBUF_PKTLEN(om), - &tuples[i].handle, sizeof tuples[i].handle); - if (rc != 0) { - rc = BLE_HS_ENOMEM; - os_mbuf_free_chain(om); - goto done; - } - rc = os_mbuf_copyinto(om, OS_MBUF_PKTLEN(om), - &(OS_MBUF_PKTLEN(tuples[i].value)), - sizeof(OS_MBUF_PKTLEN(tuples[i].value))); - if (rc != 0) { - rc = BLE_HS_ENOMEM; - os_mbuf_free_chain(om); - goto done; - } - rc = os_mbuf_appendfrom(om, tuples[i].value, - 0, OS_MBUF_PKTLEN(tuples[i].value)); - if (rc != 0) { - rc = BLE_HS_ENOMEM; - os_mbuf_free_chain(om); - goto done; - } - } - rc = ble_att_clt_tx_multi_notify(conn_handle, om); - -done: - if (rc != 0) { - STATS_INC(ble_gattc_stats, multi_notify_fail); - } - for (i = 0; i < num_tuples; i++) { - ble_gap_notify_tx_event(rc, conn_handle, tuples[i].handle, 0); - os_mbuf_free_chain(tuples[i].value); - } - return rc; -} - int ble_gatts_notify_multiple(uint16_t conn_handle, size_t num_handles, @@ -6678,7 +6530,7 @@ ble_gattc_rx_find_type_value_complete(uint16_t conn_handle, uint16_t cid, int st BLE_GATT_OP_DISC_SVC_UUID); if (proc != NULL) { rc = ble_gattc_disc_svc_uuid_rx_complete(proc, status); - ble_gattc_process_status(proc, rc, true); + ble_gattc_process_status(proc, rc, false); } } @@ -6708,7 +6560,7 @@ ble_gattc_rx_read_type_adata(uint16_t conn_handle, uint16_t cid, */ ble_gattc_process_status(proc, rc, true); } else { - rc = -1; + rc = BLE_HS_ENOENT; } return rc; @@ -7305,5 +7157,3 @@ ble_gattc_deinit(void) #endif } #endif - -#endif diff --git a/nimble/host/src/ble_gattc_cache.c b/nimble/host/src/ble_gattc_cache.c index c06d4fa4e..fe0f34863 100644 --- a/nimble/host/src/ble_gattc_cache.c +++ b/nimble/host/src/ble_gattc_cache.c @@ -39,7 +39,7 @@ #define MAX_ADDR_LIST_CACHE_BUF 4096 #if MYNEWT_VAL(BLE_GATT_CACHING) -static const char *cache_key = "key"; +static const char *cache_key = "k"; static const char *cache_addr = "cache_addr_tab"; static uint8_t ble_gattc_cache_find_addr(ble_addr_t addr); static uint8_t ble_gattc_cache_find_hash(uint8_t * hash_key); @@ -101,7 +101,7 @@ print_addr(ble_addr_t addr) } } -/* Increased buffer size to safely hold "key" + 12 hex chars + null + margin */ +/* Buffer size to safely hold "k" + 14 hex chars + null */ #define GATT_CACHE_KEY_NAME_MAX_LEN 20 /* Thread-local storage for key name to avoid static buffer thread safety issues */ @@ -360,7 +360,11 @@ void ble_gattc_cache_get_addr_list(ble_addr_t *addr_list, uint8_t *out_num) return; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_static_vars == NULL || cache_env == NULL) { +#else if (cache_env == NULL) { +#endif *out_num = 0; return; } @@ -549,6 +553,7 @@ ble_gattc_cache_addr_save(uint8_t *out_index, ble_addr_t addr, uint8_t * hash_ke } else { if (cache_env->num_addr >= MAX_DEVICE_IN_CACHE) { nimble_platform_mem_free(p_buf); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } insert_ind = cache_env->num_addr; @@ -849,6 +854,12 @@ ble_gattc_cache_find_source(struct ble_gattc_cache_conn *cache_conn, uint8_t *da uint8_t num; cache_addr_info_t *addr_info; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_static_vars == NULL) { + ble_gap_assoc_event(cache_conn->conn_handle, ESP_FAIL, cache_conn->cache_state); + return ESP_FAIL; + } +#endif if (cache_env == NULL) { ble_gap_assoc_event(cache_conn->conn_handle, ESP_FAIL, cache_conn->cache_state); return ESP_FAIL; diff --git a/nimble/host/src/ble_gattc_cache_conn.c b/nimble/host/src/ble_gattc_cache_conn.c index b4179af58..2360d80ca 100644 --- a/nimble/host/src/ble_gattc_cache_conn.c +++ b/nimble/host/src/ble_gattc_cache_conn.c @@ -223,13 +223,7 @@ ble_gattc_cache_conn_find(uint16_t conn_handle) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_gattc_cache_conn_static_vars == NULL) { - if (ble_gattc_cache_conn_static_vars_init() == NULL) { - return NULL; - } - SLIST_INIT(&ble_gattc_cache_conn_static_vars->_ble_gattc_cache_conns); - } - else if (SLIST_FIRST(&ble_gattc_cache_conns) == NULL) { - SLIST_INIT(&ble_gattc_cache_conns); + return NULL; } #endif SLIST_FOREACH(ble_gattc_cache_conn, &ble_gattc_cache_conns, next) { @@ -245,10 +239,11 @@ struct ble_gattc_cache_conn * ble_gattc_cache_conn_find_by_addr(ble_addr_t peer_addr) { struct ble_gattc_cache_conn *ble_gattc_cache_conn; - /* TODO(issue-2): When BLE_STATIC_TO_DYNAMIC is enabled this function - * accesses ble_gattc_cache_conns via macro without first checking/ - * initialising ble_gattc_cache_conn_static_vars, unlike find(). Add the - * same NULL-guard and lazy-init call that find() performs. */ +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_conn_static_vars == NULL) { + return NULL; + } +#endif SLIST_FOREACH(ble_gattc_cache_conn, &ble_gattc_cache_conns, next) { if (memcmp(&ble_gattc_cache_conn->ble_gattc_cache_conn_addr, &peer_addr, sizeof(peer_addr)) == 0) { return ble_gattc_cache_conn; @@ -704,6 +699,24 @@ ble_gattc_cache_conn_incl_svc_find(const struct ble_gattc_cache_conn_svc *svc, u #endif #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) +static void +ble_gattc_cache_conn_undo_new_svc(struct ble_gattc_cache_conn *peer, + struct ble_gattc_cache_conn_svc *svc, + struct ble_gattc_cache_conn_svc *prev, + bool svc_new) +{ + if (!svc_new) { + return; + } + + if (prev == NULL) { + SLIST_REMOVE_HEAD(&peer->svcs, next); + } else { + SLIST_REMOVE_AFTER(prev, next); + } + os_memblock_put(&ble_gattc_cache_conn_svc_pool, svc); +} + int ble_gattc_cache_conn_inc_add(ble_addr_t peer_addr, const struct ble_gatt_incl_svc *gatt_svc) #else @@ -716,6 +729,7 @@ ble_gattc_cache_conn_inc_add(ble_addr_t peer_addr, const struct ble_gatt_svc *ga struct ble_gattc_cache_conn *peer; #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + bool svc_new; struct ble_gattc_cache_conn_incl_svc *incl_svc; struct ble_gattc_cache_conn_incl_svc *incl_svc_prev; struct ble_gattc_cache_conn_svc *cur_svc; @@ -727,6 +741,9 @@ ble_gattc_cache_conn_inc_add(ble_addr_t peer_addr, const struct ble_gatt_svc *ga return BLE_HS_EUNKNOWN; } +#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + svc_new = false; +#endif svc = ble_gattc_cache_conn_svc_find(peer, gatt_svc->start_handle, &prev); if (!svc) { @@ -754,6 +771,9 @@ ble_gattc_cache_conn_inc_add(ble_addr_t peer_addr, const struct ble_gatt_svc *ga } else { SLIST_INSERT_AFTER(prev, svc, next); } +#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + svc_new = true; +#endif BLE_HS_LOG(DEBUG, "Secondary Svc added with start_handle = %d , end_handle = %d", gatt_svc->start_handle, gatt_svc->end_handle); @@ -767,18 +787,21 @@ ble_gattc_cache_conn_inc_add(ble_addr_t peer_addr, const struct ble_gatt_svc *ga /* Handle provided by remote device falls outside known service ranges; * return error gracefully to avoid remote-triggered crash. */ BLE_HS_LOG(ERROR, "No service found for inc_svc handle=%d, %s", gatt_svc->handle, __func__); + ble_gattc_cache_conn_undo_new_svc(peer, svc, prev, svc_new); return BLE_HS_EUNKNOWN; } incl_svc = ble_gattc_cache_conn_incl_svc_find(cur_svc, gatt_svc->handle, &incl_svc_prev); if (incl_svc != NULL) { /* Already discovered */ + ble_gattc_cache_conn_undo_new_svc(peer, svc, prev, svc_new); return 0; } incl_svc = os_memblock_get(&ble_gattc_cache_conn_incl_svc_pool); if (incl_svc == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + ble_gattc_cache_conn_undo_new_svc(peer, svc, prev, svc_new); return BLE_HS_ENOMEM; } @@ -941,16 +964,18 @@ ble_gattc_get_db_size_with_handle(struct ble_gattc_cache_conn *peer, uint16_t st } #endif SLIST_FOREACH(chr, &svc->chrs, next) { - /* Use def_handle (declaration) for range check so descriptors - * beyond a characteristic whose val_handle is below start_handle - * are still evaluated. */ - if (chr->chr.def_handle < start_handle) { - continue; - } + /* Stop when the characteristic declaration is beyond the range; + * its value handle and descriptors cannot be in range either. */ if (chr->chr.def_handle > end_handle) { return db_size; } - db_size++; + /* Count the characteristic only when its value handle is within + * [start_handle, end_handle]. Always enter the descriptor loop so + * descriptors of a characteristic whose val_handle is just below + * start_handle are still evaluated. */ + if (chr->chr.val_handle >= start_handle && chr->chr.val_handle <= end_handle) { + db_size++; + } SLIST_FOREACH(dsc, &chr->dscs, next) { if (dsc->dsc.handle < start_handle) { @@ -993,7 +1018,7 @@ ble_gattc_get_db_size_with_type(struct ble_gattc_cache_conn *peer, uint8_t type, switch (type) { case BLE_GATT_DB_PRIMARY_SERVICE: /* Count if the service declaration handle falls in range; - * requiring end_handle <= end_handle wrongly excludes + * requiring svc->svc.end_handle <= end_handle wrongly excludes * services that extend beyond the queried range. */ if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY && svc->svc.start_handle >= start_handle && @@ -1064,9 +1089,6 @@ void ble_gattc_fill_gatt_db_el(ble_gattc_db_elem_t *attr, attr->uuid = uuid; } -/* TODO(issue-5/6): wrap entire body of this function with ble_hs_lock() / - * ble_hs_unlock() to prevent race conditions with the host task modifying the - * service list between the count and fill passes (heap overflow risk). */ void ble_gattc_get_service_with_uuid(uint16_t conn_handle, ble_uuid_t *svc_uuid, ble_gattc_db_elem_t **svc_db, @@ -1076,12 +1098,14 @@ void ble_gattc_get_service_with_uuid(uint16_t conn_handle, struct ble_gattc_cache_conn *cache_conn; struct ble_gattc_cache_conn_svc *svc; + ble_hs_lock(); // Find the connection in the cache cache_conn = ble_gattc_cache_conn_find(conn_handle); if (cache_conn == NULL || SLIST_EMPTY(&cache_conn->svcs)) { BLE_HS_LOG(WARN, "%s(), no service.", __func__); *count = 0; *svc_db = NULL; + ble_hs_unlock(); return; } @@ -1096,15 +1120,18 @@ void ble_gattc_get_service_with_uuid(uint16_t conn_handle, BLE_HS_LOG(DEBUG, "the db size is 0."); *count = 0; *svc_db = NULL; + ble_hs_unlock(); return; } // Allocate memory for the GATT database + size_t allocated_size = db_size; void *buffer = nimble_platform_mem_malloc(db_size * sizeof(ble_gattc_db_elem_t)); if (!buffer) { BLE_HS_LOG(WARN, "%s(), no resource.", __func__); *count = 0; *svc_db = NULL; + ble_hs_unlock(); return; } ble_gattc_db_elem_t *curr_db_attr = buffer; @@ -1113,6 +1140,9 @@ void ble_gattc_get_service_with_uuid(uint16_t conn_handle, // Iterate through services in the cache SLIST_FOREACH(svc, &cache_conn->svcs, next) { if (svc_uuid == NULL || ble_uuid_cmp(&svc->svc.uuid.u, svc_uuid) == 0) { + if (db_size >= allocated_size) { + break; + } ble_gattc_fill_gatt_db_el(curr_db_attr, svc->type == BLE_GATT_SVC_TYPE_PRIMARY ? /* attr type */ BLE_GATT_DB_PRIMARY_SERVICE:BLE_GATT_DB_SECONDARY_SERVICE, @@ -1128,11 +1158,9 @@ void ble_gattc_get_service_with_uuid(uint16_t conn_handle, *svc_db = buffer; *count = db_size; + ble_hs_unlock(); } -/* TODO(issue-5/6): wrap entire body with ble_hs_lock() / ble_hs_unlock(); the - * two-pass size-then-fill pattern is not safe against concurrent host task - * modifications (heap overflow / use-after-free). */ void ble_gattc_get_db_with_operation(uint16_t conn_handle, ble_gatt_get_db_op_t op, uint16_t char_handle, @@ -1152,12 +1180,15 @@ void ble_gattc_get_db_with_operation(uint16_t conn_handle, struct ble_gattc_cache_conn_chr *chr; struct ble_gattc_cache_conn_dsc *dsc; + ble_hs_lock(); + // Find the connection in the cache cache_conn = ble_gattc_cache_conn_find(conn_handle); if (cache_conn == NULL || SLIST_EMPTY(&cache_conn->svcs)) { BLE_HS_LOG(WARN, "the service cache is empty."); *count = 0; *char_db = NULL; + ble_hs_unlock(); return; } size_t db_size = ble_gattc_get_db_size_with_handle(cache_conn, start_handle, end_handle); @@ -1166,6 +1197,7 @@ void ble_gattc_get_db_with_operation(uint16_t conn_handle, BLE_HS_LOG(DEBUG, "the db size is 0."); *count = 0; *char_db = NULL; + ble_hs_unlock(); return; } @@ -1175,6 +1207,7 @@ void ble_gattc_get_db_with_operation(uint16_t conn_handle, BLE_HS_LOG(WARN, "%s(), no resource.", __func__); *count = 0; *char_db = NULL; + ble_hs_unlock(); return; } size_t allocated_size = db_size; @@ -1225,19 +1258,17 @@ void ble_gattc_get_db_with_operation(uint16_t conn_handle, #endif // Check for characteristics SLIST_FOREACH(chr, &svc->chrs, next) { - if (chr->chr.def_handle < start_handle) { - continue; - } if (chr->chr.def_handle > end_handle) { goto fill_done; } - if (db_size >= allocated_size) { - goto fill_done; - } - if ((op == BLE_GATT_OP_GET_ALL_CHAR || op == BLE_GATT_OP_GET_CHAR_BY_UUID) && (char_uuid == NULL || ble_uuid_cmp(&chr->chr.uuid.u, char_uuid) == 0)) { + if (chr->chr.val_handle >= start_handle && + chr->chr.val_handle <= end_handle) { + if (db_size >= allocated_size) { + goto fill_done; + } ble_gattc_fill_gatt_db_el(curr_db_attr, BLE_GATT_DB_CHARACTERISTIC, /* attr type*/ chr->chr.val_handle, /* handle */ @@ -1247,7 +1278,8 @@ void ble_gattc_get_db_with_operation(uint16_t conn_handle, chr->chr.uuid); /* uuid */ curr_db_attr++; db_size++; - continue; + } + continue; } if ((op == BLE_GATT_OP_GET_DESC_BY_HANDLE || op == BLE_GATT_OP_GET_ALL_DESC) && @@ -1295,8 +1327,9 @@ void ble_gattc_get_db_with_operation(uint16_t conn_handle, fill_done: *char_db = buffer; *count = db_size; + ble_hs_unlock(); return; - } +} void ble_gattc_get_db_size_with_type_handle(uint16_t conn_handle, ble_gattc_db_attr_type type, uint16_t start_handle, @@ -1305,13 +1338,16 @@ void ble_gattc_get_db_size_with_type_handle(uint16_t conn_handle, { struct ble_gattc_cache_conn *cache_conn; + ble_hs_lock(); cache_conn = ble_gattc_cache_conn_find(conn_handle); if (cache_conn == NULL) { *count = 0; - return ; + ble_hs_unlock(); + return; } - *count = ble_gattc_get_db_size_with_type(cache_conn, type, start_handle, end_handle, char_handle); + *count = ble_gattc_get_db_size_with_type(cache_conn, type, start_handle, end_handle, char_handle); + ble_hs_unlock(); } void ble_gattc_get_db_size_handle(uint16_t conn_handle, @@ -1321,12 +1357,15 @@ void ble_gattc_get_db_size_handle(uint16_t conn_handle, { struct ble_gattc_cache_conn *cache_conn; + ble_hs_lock(); cache_conn = ble_gattc_cache_conn_find(conn_handle); if (cache_conn == NULL) { *count = 0; + ble_hs_unlock(); return; } *count = ble_gattc_get_db_size_with_handle(cache_conn, start_handle, end_handle); + ble_hs_unlock(); } // Copy the server GATT database into db parameter. @@ -1380,7 +1419,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, } if (filled >= db_size) { *db = buffer; - *count = db_size; + *count = filled; return; } ble_gattc_fill_gatt_db_el(curr_db_attr, @@ -1403,13 +1442,13 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, if (included_svc->svc.handle > end_handle) { *db = buffer; - *count = db_size; + *count = filled; return; } if (filled >= db_size) { *db = buffer; - *count = db_size; + *count = filled; return; } ble_gattc_fill_gatt_db_el(curr_db_attr, @@ -1425,29 +1464,30 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, #endif // Iterate over characterstic SLIST_FOREACH(chr, &svc->chrs, next) { - if (chr->chr.def_handle < start_handle) { - continue; - } - if (chr->chr.def_handle > end_handle) { *db = buffer; - *count = db_size; + *count = filled; return; } - if (filled >= db_size) { - *db = buffer; - *count = db_size; - return; + /* Fill characteristic entry only when its value handle is within + * [start_handle, end_handle]. Always iterate descriptors below so + * in-range descriptors of boundary characteristics are not missed. */ + if (chr->chr.val_handle >= start_handle && chr->chr.val_handle <= end_handle) { + if (filled >= db_size) { + *db = buffer; + *count = filled; + return; + } + ble_gattc_fill_gatt_db_el(curr_db_attr, + BLE_GATT_DB_CHARACTERISTIC, /* attr type */ + chr->chr.val_handle, /* attr handle */ + 0, /* s_handle */ + 0, /* e_handle */ + chr->chr.properties, /* property */ + chr->chr.uuid); /* uuid */ + curr_db_attr++; + filled++; } - ble_gattc_fill_gatt_db_el(curr_db_attr, - BLE_GATT_DB_CHARACTERISTIC, /* attr type */ - chr->chr.val_handle, /* attr handle */ - 0, /* s_handle */ - 0, /* e_handle */ - chr->chr.properties, /* property */ - chr->chr.uuid); /* uuid */ - curr_db_attr++; - filled++; // Iterate over descriptors SLIST_FOREACH(dsc, &chr->dscs, next) { if (dsc->dsc.handle < start_handle) { @@ -1456,12 +1496,12 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, if (dsc->dsc.handle > end_handle) { *db = buffer; - *count = db_size; + *count = filled; return; } if (filled >= db_size) { *db = buffer; - *count = db_size; + *count = filled; return; } ble_gattc_fill_gatt_db_el(curr_db_attr, @@ -1546,8 +1586,12 @@ ble_gattc_cache_conn_broken(uint16_t conn_handle) /* Remove any pending disc_ev from the queue before freeing conn to * avoid use-after-free when the event fires after the connection struct - * has been returned to the pool. */ - ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); + * has been returned to the pool. Guard ev->event != NULL: conn struct is + * zeroed on creation so disc_ev may never have been initialized. */ + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { + ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(), + &conn->disc_ev); + } ble_npl_event_deinit(&conn->disc_ev); while ((svc = SLIST_FIRST(&conn->svcs)) != NULL) { @@ -1563,8 +1607,9 @@ ble_gattc_cache_conn_bonding_established(uint16_t conn_handle) { /* update the address of the peer */ struct ble_hs_conn *conn; - struct ble_hs_conn_addrs addrs; + struct ble_hs_conn_addrs addrs = {0}; struct ble_gattc_cache_conn *peer; + static const uint8_t zero_addr[BLE_DEV_ADDR_LEN]; peer = ble_gattc_cache_conn_find(conn_handle); if (peer == NULL) { @@ -1576,10 +1621,11 @@ ble_gattc_cache_conn_bonding_established(uint16_t conn_handle) conn = ble_hs_conn_find(conn_handle); BLE_HS_DBG_ASSERT(conn != NULL); ble_hs_conn_addrs(conn, &addrs); - peer->ble_gattc_cache_conn_addr = conn->bhc_peer_addr; - - peer->ble_gattc_cache_conn_addr.type = - ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type); + if (memcmp(addrs.peer_id_addr.val, zero_addr, sizeof zero_addr) == 0) { + addrs.peer_id_addr = conn->bhc_peer_addr; + addrs.peer_id_addr.type = ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type); + } + peer->ble_gattc_cache_conn_addr = addrs.peer_id_addr; ble_hs_unlock(); } @@ -1588,9 +1634,10 @@ void ble_gattc_cache_conn_bonding_restored(uint16_t conn_handle) { struct ble_hs_conn *conn; - struct ble_hs_conn_addrs addrs; + struct ble_hs_conn_addrs addrs = {0}; struct ble_gattc_cache_conn *peer; int rc; + static const uint8_t zero_addr[BLE_DEV_ADDR_LEN]; peer = ble_gattc_cache_conn_find(conn_handle); if (peer == NULL) { @@ -1602,6 +1649,10 @@ ble_gattc_cache_conn_bonding_restored(uint16_t conn_handle) conn = ble_hs_conn_find(conn_handle); BLE_HS_DBG_ASSERT(conn != NULL); ble_hs_conn_addrs(conn, &addrs); + if (memcmp(addrs.peer_id_addr.val, zero_addr, sizeof zero_addr) == 0) { + addrs.peer_id_addr = conn->bhc_peer_addr; + addrs.peer_id_addr.type = ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type); + } /* Use the resolved Identity Address so the cache entry remains valid * across reconnections where the peer rotates its RPA. */ peer->ble_gattc_cache_conn_addr = addrs.peer_id_addr; @@ -1710,7 +1761,7 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) } break; case BLE_GATT_OP_DISC_SVC_UUID : - rc = ble_gattc_cache_conn_search_svc_by_uuid(peer->conn_handle, op->uuid, op->cb, op->cb_arg); + rc = ble_gattc_cache_conn_search_svc_by_uuid(peer->conn_handle, &op->uuid.u, op->cb, op->cb_arg); if (rc != 0) { BLE_HS_LOG(ERROR, "search service by uuid failed"); } @@ -1728,7 +1779,7 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) } break; case BLE_GATT_OP_DISC_CHR_UUID : - rc = ble_gattc_cache_conn_search_chrs_by_uuid(peer->conn_handle, op->start_handle, op->end_handle, op->uuid, op->cb, op->cb_arg); + rc = ble_gattc_cache_conn_search_chrs_by_uuid(peer->conn_handle, op->start_handle, op->end_handle, &op->uuid.u, op->cb, op->cb_arg); if (rc != 0) { BLE_HS_LOG(ERROR, "search chars by uuid failed"); } @@ -1851,7 +1902,8 @@ ble_gattc_cache_conn_disc(struct ble_gattc_cache_conn *peer) { int rc; - if (peer->cache_state == SVC_DISC_IN_PROGRESS) { + if (peer->cache_state >= SVC_DISC_IN_PROGRESS && + peer->cache_state <= DSC_DISC_IN_PROGRESS) { return BLE_HS_EALREADY; } ble_gattc_cache_conn_undisc_all(peer->ble_gattc_cache_conn_addr); @@ -1861,6 +1913,9 @@ ble_gattc_cache_conn_disc(struct ble_gattc_cache_conn *peer) BLE_HS_LOG(INFO, "Initiating Remote Service Discovery"); peer->cache_state = SVC_DISC_IN_PROGRESS; rc = ble_gattc_disc_all_svcs(peer->conn_handle, ble_gattc_cache_conn_svc_disced, peer); + if (rc != 0) { + peer->cache_state = CACHE_INVALID; + } return rc; } @@ -1873,9 +1928,8 @@ ble_gattc_cache_conn_on_read(uint16_t conn_handle, uint16_t res; if (error->status == BLE_HS_EDONE) { - /* Hash characteristic not found; fall back to full discovery. */ - res = ble_gattc_cache_conn_disc((struct ble_gattc_cache_conn *)arg); - return res; + /* Ignore Read by UUID follow-up callback. */ + return 0; } if (error->status != 0) { @@ -2066,6 +2120,9 @@ ble_gattc_cache_conn_chr_disced(uint16_t conn_handle, const struct ble_gatt_erro /* All characteristics in this service discovered; start discovering * characteristics in the next service. */ + if (peer->cur_svc != NULL) { + peer->cur_svc->chrs_discovered = 1; + } if (peer->disc_prev_chr_val > 0) { ble_gattc_cache_conn_disc_chrs(peer); } @@ -2094,14 +2151,8 @@ ble_gattc_cache_conn_disc_chrs(struct ble_gattc_cache_conn *peer) * contains undiscovered characteristics. Then, discover all * characteristics belonging to that service. */ - /* TODO(issue-17): Using SLIST_EMPTY(&svc->chrs) to decide whether - * discovery is needed conflates "not yet discovered" with "has no - * characteristics". A service with zero characteristics keeps an empty - * list after discovery completes, causing this loop to restart discovery - * for it on every BLE_HS_EDONE callback (infinite loop). Track - * per-service chr-discovery state with an explicit flag instead. */ SLIST_FOREACH(svc, &peer->svcs, next) { - if (!ble_gattc_cache_conn_svc_is_empty(svc) && SLIST_EMPTY(&svc->chrs)) { + if (!ble_gattc_cache_conn_svc_is_empty(svc) && !svc->chrs_discovered) { peer->cur_svc = svc; peer->cache_state = CHR_DISC_IN_PROGRESS; rc = ble_gattc_disc_all_chrs(peer->conn_handle, @@ -2213,6 +2264,11 @@ int ble_gattc_cache_refresh(ble_addr_t peer_addr) return BLE_HS_EUNKNOWN; } + if (conn->cache_state >= SVC_DISC_IN_PROGRESS && + conn->cache_state <= DSC_DISC_IN_PROGRESS) { + return BLE_HS_EALREADY; + } + ble_gattc_cacheReset(&peer_addr); /* Invalidate and restart discovery*/ rc = ble_gattc_cache_conn_disc(conn); @@ -2268,7 +2324,8 @@ int ble_gattc_cache_assoc(ble_addr_t peer_addr) BLE_HS_LOG(INFO, "Cache already loaded for conn_handle=%d; " "cache state=%d. Skipping association.", cache_conn->conn_handle, cache_conn->cache_state); - ble_gap_assoc_event(cache_conn->conn_handle, 0, cache_conn->cache_state); + ble_gap_assoc_event(cache_conn->conn_handle, 0, CACHE_LOADED); + return 0; } if (cache_conn->cache_state == CACHE_INVALID) { @@ -2301,9 +2358,10 @@ int ble_gattc_cache_clean(ble_addr_t peer_addr) " %02x:%02x:%02x:%02x:%02x:%02x, Attempting cache clean anyway.", peer_addr.val[0], peer_addr.val[1], peer_addr.val[2], peer_addr.val[3], peer_addr.val[4], peer_addr.val[5]); - - ble_gattc_cacheReset(&peer_addr); ble_hs_unlock(); + /* Call cacheReset outside lock: it performs blocking NVS flash I/O + * which is inappropriate to hold ble_hs_lock during. */ + ble_gattc_cacheReset(&peer_addr); return 0; } @@ -2315,16 +2373,19 @@ int ble_gattc_cache_clean(ble_addr_t peer_addr) return BLE_HS_EUNKNOWN; } - // Reset any existing discovery flags/cache markers - ble_gattc_cacheReset(&peer_addr); - - /* TODO(issue-13): Before invalidating, iterate conn->svcs and call - * ble_gattc_cache_conn_svc_delete() on each entry to free service, - * characteristic, and descriptor pool blocks. Without this, repeated - * clean+rediscovery cycles exhaust the fixed mempools. */ + /* Free in-memory service list under lock, then reset NVS cache + * outside lock to avoid holding ble_hs_lock during blocking flash I/O. */ + struct ble_gattc_cache_conn_svc *svc; + while ((svc = SLIST_FIRST(&conn->svcs)) != NULL) { + SLIST_REMOVE_HEAD(&conn->svcs, next); + ble_gattc_cache_conn_svc_delete(svc); + } + conn->cur_svc = NULL; conn->cache_state = CACHE_INVALID; ble_hs_unlock(); + ble_gattc_cacheReset(&peer_addr); + BLE_HS_LOG(INFO, "GATT cache cleaned for peer: %02x:%02x:%02x:%02x:%02x:%02x", peer_addr.val[0], peer_addr.val[1], peer_addr.val[2], peer_addr.val[3], peer_addr.val[4], peer_addr.val[5]); @@ -2551,6 +2612,9 @@ ble_gattc_cache_conn_init(void) storage_cb = NULL; rc = ble_gattc_cache_init(storage_cb); + if (rc != 0) { + goto err; + } return 0; err: @@ -2679,11 +2743,11 @@ static void ble_gattc_cache_conn_fill_op(struct ble_gattc_cache_conn_op *op, op->cb_type = cb_type; op->start_handle = start_handle; op->end_handle = end_handle; - /* TODO(issue-4): storing the uuid pointer directly is unsafe when the - * caller passes a stack-allocated UUID and the operation is deferred. - * The pending_op structure should embed a ble_uuid_any_t and copy the - * UUID here with ble_uuid_copy() to avoid a use-after-return. */ - op->uuid = uuid; + if (uuid) { + ble_uuid_copy(&op->uuid, uuid); + } else { + memset(&op->uuid, 0, sizeof(op->uuid)); + } } int @@ -2692,7 +2756,6 @@ ble_gattc_cache_conn_search_all_svcs(uint16_t conn_handle, { struct ble_gattc_cache_conn *conn; struct ble_gattc_cache_conn_op *op; - ble_uuid_t uuid = {0}; int rc; conn = ble_gattc_cache_conn_find(conn_handle); if (conn == NULL) { @@ -2708,13 +2771,17 @@ ble_gattc_cache_conn_search_all_svcs(uint16_t conn_handle, return rc; } - CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_ALL_SVCS, - 0, 0, &uuid); /* Guard against re-init of a disc_ev that is already in the queue; - * re-init would memset the internal event struct and corrupt the queued flag. */ - if (ble_npl_event_is_queued(&conn->disc_ev)) { + * re-init would memset the internal event struct and corrupt the queued flag. + * Check ev->event != NULL first: it is NULL when disc_ev was never initialized + * (conn struct is zeroed on creation), and npl_freertos_event_is_queued would + * dereference NULL causing a crash. */ + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { return BLE_HS_EBUSY; } + + CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_ALL_SVCS, + 0, 0, NULL); ble_npl_event_init(&conn->disc_ev, ble_gattc_cache_search_all_svcs_cb, &conn->conn_handle); ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); return 0; @@ -2743,7 +2810,8 @@ ble_gattc_cache_conn_search_svc_by_uuid_cb(struct ble_npl_event *ev) dcb = op->cb; /* Capture before loop: a re-entrant call from dcb may overwrite pending_op. */ void *cb_arg = op->cb_arg; - const ble_uuid_t *uuid = op->uuid; + ble_uuid_any_t uuid_local = op->uuid; /* copy by value to protect against re-entrant overwrite */ + const ble_uuid_t *uuid = &uuid_local.u; SLIST_FOREACH(svc, &conn->svcs, next) { if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY && ble_uuid_cmp(&svc->svc.uuid.u, uuid) == 0) { dcb(conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, cb_arg); @@ -2777,11 +2845,12 @@ ble_gattc_cache_conn_search_svc_by_uuid(uint16_t conn_handle, const ble_uuid_t * return rc; } - CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_SVC_UUID, - 0, 0, uuid); - if (ble_npl_event_is_queued(&conn->disc_ev)) { + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { return BLE_HS_EBUSY; } + + CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_SVC_UUID, + 0, 0, uuid); ble_npl_event_init(&conn->disc_ev, ble_gattc_cache_conn_search_svc_by_uuid_cb, &conn->conn_handle); ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); return 0; @@ -2853,7 +2922,6 @@ ble_gattc_cache_conn_search_inc_svcs(uint16_t conn_handle, uint16_t start_handle { struct ble_gattc_cache_conn *conn; struct ble_gattc_cache_conn_op *op; - ble_uuid_t uuid = {0}; int rc; conn = ble_gattc_cache_conn_find(conn_handle); @@ -2870,8 +2938,17 @@ ble_gattc_cache_conn_search_inc_svcs(uint16_t conn_handle, uint16_t start_handle return rc; } + /* Guard against re-init of a disc_ev that is already in the queue; + * re-init would memset the internal event struct and corrupt the queued flag. + * Check ev->event != NULL first: it is NULL when disc_ev was never initialized + * (conn struct is zeroed on creation), and npl_freertos_event_is_queued would + * dereference NULL causing a crash. */ + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { + return BLE_HS_EBUSY; + } + CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_FIND_INC_SVCS, - start_handle, end_handle, &uuid); + start_handle, end_handle, NULL); /* put the event in the queue to mimic the gattc behaviour */ ble_npl_event_init(&conn->disc_ev, ble_gattc_cache_conn_search_inc_svcs_cb, &conn->conn_handle); ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); @@ -2903,19 +2980,25 @@ ble_gattc_cache_conn_search_all_chrs_cb(struct ble_npl_event *ev) void *cb_arg = op->cb_arg; uint16_t start_handle = op->start_handle; uint16_t end_handle = op->end_handle; - svc = ble_gattc_cache_conn_svc_find_range(conn, start_handle); - - if (svc == NULL) { - dcb(conn_handle, ble_gattc_cache_error(BLE_HS_EDONE, 0), NULL, cb_arg); - return; - } /* return all chrs within the requested handle range */ - SLIST_FOREACH(chr, &svc->chrs, next) { - if (chr->chr.def_handle < start_handle || chr->chr.def_handle > end_handle) { + SLIST_FOREACH(svc, &conn->svcs, next) { + if (svc->svc.end_handle < start_handle || svc->svc.start_handle > end_handle) { continue; } - dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, cb_arg); + + SLIST_FOREACH(chr, &svc->chrs, next) { + int dcb_rc; + + if (chr->chr.def_handle < start_handle || chr->chr.def_handle > end_handle) { + continue; + } + dcb_rc = dcb(conn_handle, ble_gattc_cache_error(status, 0), + &chr->chr, cb_arg); + if (dcb_rc != 0) { + return; + } + } } status = BLE_HS_EDONE; dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, cb_arg); @@ -2930,7 +3013,6 @@ ble_gattc_cache_conn_search_all_chrs(uint16_t conn_handle, uint16_t start_handle { struct ble_gattc_cache_conn *conn; struct ble_gattc_cache_conn_op *op; - ble_uuid_t uuid = {0}; int rc; conn = ble_gattc_cache_conn_find(conn_handle); @@ -2947,11 +3029,12 @@ ble_gattc_cache_conn_search_all_chrs(uint16_t conn_handle, uint16_t start_handle return rc; } - CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_ALL_CHRS, - start_handle, end_handle, &uuid); - if (ble_npl_event_is_queued(&conn->disc_ev)) { + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { return BLE_HS_EBUSY; } + + CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_ALL_CHRS, + start_handle, end_handle, NULL); ble_npl_event_init(&conn->disc_ev, ble_gattc_cache_conn_search_all_chrs_cb, &conn->conn_handle); ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); return 0; @@ -2980,22 +3063,30 @@ ble_gattc_cache_conn_search_chrs_by_uuid_cb(struct ble_npl_event *ev) dcb = op->cb; /* Capture before loop: a re-entrant call from dcb may overwrite pending_op. */ void *cb_arg = op->cb_arg; - const ble_uuid_t *uuid = op->uuid; + ble_uuid_any_t uuid_local = op->uuid; /* copy by value to protect against re-entrant overwrite */ + const ble_uuid_t *uuid = &uuid_local.u; uint16_t start_handle = op->start_handle; uint16_t end_handle = op->end_handle; - svc = ble_gattc_cache_conn_svc_find_range(conn, start_handle); - if (svc == NULL) { - status = BLE_HS_ENOENT; - dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, cb_arg); - return; - } + /* return chrs matching UUID within the requested handle range */ - SLIST_FOREACH(chr, &svc->chrs, next) { - if (chr->chr.def_handle < start_handle || chr->chr.def_handle > end_handle) { + SLIST_FOREACH(svc, &conn->svcs, next) { + if (svc->svc.end_handle < start_handle || svc->svc.start_handle > end_handle) { continue; } - if (ble_uuid_cmp(&chr->chr.uuid.u, uuid) == 0) { - dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, cb_arg); + + SLIST_FOREACH(chr, &svc->chrs, next) { + int dcb_rc; + + if (chr->chr.def_handle < start_handle || chr->chr.def_handle > end_handle) { + continue; + } + if (ble_uuid_cmp(&chr->chr.uuid.u, uuid) == 0) { + dcb_rc = dcb(conn_handle, ble_gattc_cache_error(status, 0), + &chr->chr, cb_arg); + if (dcb_rc != 0) { + return; + } + } } } status = BLE_HS_EDONE; @@ -3027,11 +3118,12 @@ ble_gattc_cache_conn_search_chrs_by_uuid(uint16_t conn_handle, uint16_t start_ha return rc; } - CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_CHR_UUID, - start_handle, end_handle, uuid); - if (ble_npl_event_is_queued(&conn->disc_ev)) { + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { return BLE_HS_EBUSY; } + + CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_CHR_UUID, + start_handle, end_handle, uuid); ble_npl_event_init(&conn->disc_ev, ble_gattc_cache_conn_search_chrs_by_uuid_cb, &conn->conn_handle); ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); return 0; @@ -3077,10 +3169,16 @@ ble_gattc_cache_conn_search_all_dscs_cb(struct ble_npl_event *ev) /* return only descriptors within the requested handle range */ SLIST_FOREACH(dsc, &chr->dscs, next) { + int dcb_rc; + if (dsc->dsc.handle < start_handle || dsc->dsc.handle > end_handle) { continue; } - dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, cb_arg); + dcb_rc = dcb(conn_handle, ble_gattc_cache_error(status, 0), + chr->chr.val_handle, &dsc->dsc, cb_arg); + if (dcb_rc != 0) { + return; + } } status = BLE_HS_EDONE; dcb(conn_handle, ble_gattc_cache_error(status, 0), 0, NULL, cb_arg); @@ -3096,7 +3194,6 @@ ble_gattc_cache_conn_search_all_dscs(uint16_t conn_handle, uint16_t start_handle struct ble_gattc_cache_conn *conn; struct ble_gattc_cache_conn_op *op; int rc; - ble_uuid_t uuid = {0}; conn = ble_gattc_cache_conn_find(conn_handle); if (conn == NULL) { @@ -3112,11 +3209,12 @@ ble_gattc_cache_conn_search_all_dscs(uint16_t conn_handle, uint16_t start_handle return rc; } - CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_ALL_DSCS, - start_handle, end_handle, &uuid); - if (ble_npl_event_is_queued(&conn->disc_ev)) { + if (conn->disc_ev.event && ble_npl_event_is_queued(&conn->disc_ev)) { return BLE_HS_EBUSY; } + + CHECK_CACHE_CONN_STATE(conn->cache_state, cb, cb_arg, BLE_GATT_OP_DISC_ALL_DSCS, + start_handle, end_handle, NULL); ble_npl_event_init(&conn->disc_ev, ble_gattc_cache_conn_search_all_dscs_cb, &conn->conn_handle); ble_npl_eventq_put((struct ble_npl_eventq *)ble_hs_evq_get(), &conn->disc_ev); return 0; diff --git a/nimble/host/src/ble_gattc_cache_priv.h b/nimble/host/src/ble_gattc_cache_priv.h index 6330629de..b729e2129 100644 --- a/nimble/host/src/ble_gattc_cache_priv.h +++ b/nimble/host/src/ble_gattc_cache_priv.h @@ -89,6 +89,7 @@ struct ble_gattc_cache_conn_svc { struct ble_gattc_cache_conn_incl_list incl_svc; #endif struct ble_gattc_cache_conn_chr_list chrs; + uint8_t chrs_discovered; }; SLIST_HEAD(ble_gattc_cache_conn_svc_list, ble_gattc_cache_conn_svc); @@ -109,7 +110,7 @@ struct ble_gattc_cache_conn_op { request comes while the cache is building */ uint16_t start_handle; uint16_t end_handle; - const ble_uuid_t *uuid; + ble_uuid_any_t uuid; void *cb; void *cb_arg; uint8_t cb_type; diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index 8a31cfab5..d764aa02f 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -483,8 +483,8 @@ ble_gatts_chr_def_access(uint16_t conn_handle, uint16_t attr_handle, { const struct ble_gatt_chr_def *chr; uint8_t *buf; - uint16_t uuid_len; uint16_t total_len; + int uuid_len; STATS_INC(ble_gatts_stats, chr_def_reads); @@ -492,6 +492,9 @@ ble_gatts_chr_def_access(uint16_t conn_handle, uint16_t attr_handle, chr = arg; uuid_len = ble_uuid_length(chr->uuid); + if (uuid_len < 0) { + return BLE_ATT_ERR_UNLIKELY; + } total_len = 3 + uuid_len; /* properties(1) + handle(2) + uuid */ /* Validate offset */ @@ -599,20 +602,23 @@ ble_gatts_chr_inc_val_stat(uint8_t gatt_op) bool ble_gatts_mutable(void) { + bool mutable; + + ble_hs_lock_nested(); /* Ensure no active GAP procedures. */ if (ble_gap_adv_active() || ble_gap_disc_active() || ble_gap_conn_active()) { + ble_hs_unlock_nested(); return false; } /* Ensure no established connections. */ - if (ble_hs_conn_first() != NULL) { - return false; - } + mutable = ble_hs_conn_first() == NULL; + ble_hs_unlock_nested(); - return true; + return mutable; } static int @@ -1154,6 +1160,10 @@ ble_gatts_read_cccd(uint16_t conn_handle, uint16_t chr_val_handle, uint8_t *cccd struct ble_gatts_clt_cfg *clt_cfg; struct ble_hs_conn *conn; + if (cccd_value == NULL) { + return BLE_HS_EINVAL; + } + ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); @@ -1432,7 +1442,9 @@ ble_gatts_register_chr(const struct ble_gatt_svc_def *svc, uint16_t def_handle; uint16_t val_handle; uint16_t dsc_handle; +#if MYNEWT_VAL(BLE_CPFD_CAFD) uint16_t cep_handle; +#endif uint16_t expected_handle; uint8_t att_flags; int rc; @@ -1462,7 +1474,6 @@ ble_gatts_register_chr(const struct ble_gatt_svc_def *svc, } expected_handle = def_handle; - (void)expected_handle; /* Register characteristic value attribute (cast away const on callback * arg). @@ -1474,7 +1485,8 @@ ble_gatts_register_chr(const struct ble_gatt_svc_def *svc, if (rc != 0) { return rc; } - BLE_HS_DBG_ASSERT(val_handle == ++expected_handle); + expected_handle++; + BLE_HS_DBG_ASSERT_EVAL(val_handle == expected_handle); if (chr->val_handle != NULL) { *chr->val_handle = val_handle; @@ -1494,7 +1506,8 @@ ble_gatts_register_chr(const struct ble_gatt_svc_def *svc, if (rc != 0) { return rc; } - BLE_HS_DBG_ASSERT(dsc_handle == ++expected_handle); + expected_handle++; + BLE_HS_DBG_ASSERT_EVAL(dsc_handle == expected_handle); } #if MYNEWT_VAL(BLE_CPFD_CAFD) @@ -1503,8 +1516,12 @@ ble_gatts_register_chr(const struct ble_gatt_svc_def *svc, if (rc != 0) { return rc; } - BLE_HS_DBG_ASSERT(cep_handle == ++expected_handle); + expected_handle++; + BLE_HS_DBG_ASSERT_EVAL(cep_handle == expected_handle); } +#endif + +#if MYNEWT_VAL(BLE_CPFD_CAFD) /* Register each Client Presentation Format Descriptor. */ rc = ble_gatts_register_cpfds(chr->cpfd); if (rc != 0) { @@ -3299,22 +3316,37 @@ static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) { clt_cfg_arg = arg; switch (clt_cfg_arg->action) { - case CONN_CLT_CFG_ADD: + case CONN_CLT_CFG_ADD: { /* added */ + int add_rc; 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; + add_rc = ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + clt_cfg_arg->chr_val_handle, allowed_flags, 0); + if (add_rc != 0 && clt_cfg_arg->rc == 0) { + /* Only record the first failure; return 0 so iteration continues + * for remaining connections. Caller checks arg.rc after the loop. + */ + clt_cfg_arg->rc = add_rc; + } + if (add_rc != 0) { + return 0; } (conn->bhc_gatt_svr.num_clt_cfgs)++; return 0; + } case CONN_CLT_CFG_REMOVE: /* removed */ - 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)--; + { + int remove_rc; + + remove_rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + clt_cfg_arg->chr_val_handle); + if (remove_rc != 0 && clt_cfg_arg->rc == 0) { + clt_cfg_arg->rc = remove_rc; + } + if (remove_rc == 0) { + (conn->bhc_gatt_svr.num_clt_cfgs)--; + } } return 0; @@ -3465,6 +3497,7 @@ ble_gatts_remove_svc_entry(const ble_uuid_t *uuid) int ble_gatts_delete_svc(const ble_uuid_t *uuid) { int rc; + int deregister_rc = BLE_HS_EUNKNOWN; struct ble_gatts_svc_entry * entry; int chr_val_handle; struct ble_gatt_chr_def *chr; @@ -3502,6 +3535,10 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) { arg.allowed_flags = 0; arg.rc = 0; ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &arg); + if (arg.rc != 0) { + BLE_HS_LOG(ERROR, "Failed to remove clt cfg from all connections; rc=%d\n", + arg.rc); + } } } /* keep the start handle and end handle before deleting the service */ @@ -3509,11 +3546,15 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) { start_handle = entry->handle; end_handle = entry->end_group_handle; /* deregister service now */ - rc = ble_gatts_deregister_svc(uuid); + deregister_rc = ble_gatts_deregister_svc(uuid); + rc = deregister_rc; done: - if (rc == 0) { - rc = ble_gatts_remove_svc_entry(uuid); + if (deregister_rc == 0) { + int remove_rc; + + remove_rc = ble_gatts_remove_svc_entry(uuid); + (void)remove_rc; #if MYNEWT_VAL(BLE_GATT_CACHING) /* make all bonded connections them unaware */ #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -3529,17 +3570,15 @@ done: ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL); #endif - /* send service change indication */ - // ble_svc_gatt_changed(start_handle, end_handle); } ble_hs_unlock(); - if (rc == 0) { + if (deregister_rc == 0) { /* send service change indication */ ble_svc_gatt_changed(start_handle, end_handle); } - return rc; + return deregister_rc == BLE_HS_EUNKNOWN ? rc : deregister_rc; } #endif @@ -3589,7 +3628,13 @@ ble_gatts_free_svcs(void) if (ble_gatts_svc_defs != NULL) { nimble_platform_mem_free(ble_gatts_svc_defs); ble_gatts_svc_defs = NULL; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gatts_static_vars != NULL) { +#endif ble_gatts_num_svc_defs = 0; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + } +#endif } ble_hs_unlock(); @@ -3714,6 +3759,7 @@ ble_gatts_count_resources(const struct ble_gatt_svc_def *svcs, res->attrs++; } +#if MYNEWT_VAL(BLE_CPFD_CAFD) /* If the characteristic permits reliable writes or auxiliary * writes, it has an Extended Properties descriptor. */ @@ -3725,11 +3771,10 @@ ble_gatts_count_resources(const struct ble_gatt_svc_def *svcs, * o 1 attribute */ res->dscs++; -#if MYNEWT_VAL(BLE_CPFD_CAFD) - res->ceps++; -#endif res->attrs++; + res->ceps++; } +#endif if (chr->descriptors != NULL) { for (d = 0; chr->descriptors[d].uuid != NULL; d++) { diff --git a/nimble/host/src/ble_gatts_lcl.c b/nimble/host/src/ble_gatts_lcl.c index 7e46a21f0..edaeb77b7 100644 --- a/nimble/host/src/ble_gatts_lcl.c +++ b/nimble/host/src/ble_gatts_lcl.c @@ -29,10 +29,10 @@ #if NIMBLE_BLE_CONNECT static const ble_uuid_t *uuid_ccc = BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16); -static const ble_uuid_t *uuid_cep = - BLE_UUID16_DECLARE(BLE_GATT_DSC_EXT_PROP_UUID16); #if MYNEWT_VAL(BLE_CPFD_CAFD) +static const ble_uuid_t *uuid_cep = + BLE_UUID16_DECLARE(BLE_GATT_DSC_EXT_PROP_UUID16); static const ble_uuid_t *uuid_cpfd = BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_PRE_FMT16); diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index aec2335e4..5db1ea654 100644 --- a/nimble/host/src/ble_hs.c +++ b/nimble/host/src/ble_hs.c @@ -45,6 +45,7 @@ #if MYNEWT_VAL(BLE_ISO) #include "host/ble_hs_iso.h" +#include "host/ble_iso.h" #endif /* MYNEWT_VAL(BLE_ISO) */ #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_GAP_SERVICE @@ -187,8 +188,8 @@ extern void ble_hs_flow_init(void); extern void ble_hs_flow_deinit(void); extern void ble_monitor_deinit(void); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) void ble_store_config_deinit(void); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) extern void ble_hs_hci_ctx_free(void); #endif @@ -485,7 +486,7 @@ ble_hs_sync(void) ble_hs_timer_sched(retry_tmo_ticks); if (rc == 0) { -#if NIMBLE_BLE_CONNECT +#if NIMBLE_BLE_CONNECT && NIMBLE_BLE_SM rc = ble_hs_misc_restore_irks(); if (rc != 0) { BLE_HS_LOG(INFO, "Failed to restore IRKs from store; status=%d\n", @@ -716,12 +717,14 @@ 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); } else { - /* Either ev is NULL or queue doesn't exist */ + if (ev) { + os_memblock_put(&ble_hs_hci_ev_pool, ev); + } #if MYNEWT_VAL(MP_RUNTIME_ALLOC) ble_transport_free(BLE_HCI_EVT, hci_evt); #else @@ -750,9 +753,15 @@ ble_hs_notifications_sched(void) void ble_hs_sched_reset(int reason) { - BLE_HS_DBG_ASSERT(ble_hs_reset_reason == 0); + ble_hs_lock_nested(); + if (ble_hs_reset_reason != 0) { + ble_hs_unlock_nested(); + return; + } ble_hs_reset_reason = reason; + ble_hs_unlock_nested(); + ble_npl_eventq_put(ble_hs_evq, &ble_hs_ev_reset); } @@ -952,7 +961,15 @@ 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_npl_mutex_init(&ble_hs_mutex); + SYSINIT_PANIC_ASSERT(rc == 0); + +#if MYNEWT_VAL(BLE_HS_DEBUG) + ble_hs_dbg_mutex_locked = 0; +#endif + + rc = ble_hs_hci_init(); + SYSINIT_PANIC_ASSERT(rc == 0); #if NIMBLE_BLE_CONNECT rc = ble_hs_conn_init(); @@ -964,7 +981,6 @@ ble_hs_init(void) SYSINIT_PANIC_ASSERT(rc == 0); #endif - #if NIMBLE_BLE_CONNECT rc = ble_l2cap_init(); SYSINIT_PANIC_ASSERT(rc == 0); @@ -1009,13 +1025,6 @@ ble_hs_init(void) STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_hs_stats), "ble_hs"); SYSINIT_PANIC_ASSERT(rc == 0); - rc = ble_npl_mutex_init(&ble_hs_mutex); - SYSINIT_PANIC_ASSERT(rc == 0); - -#if MYNEWT_VAL(BLE_HS_DEBUG) - ble_hs_dbg_mutex_locked = 0; -#endif - #ifdef MYNEWT ble_hs_evq_set((struct ble_npl_eventq *)os_eventq_dflt_get()); #else @@ -1079,18 +1088,26 @@ ble_transport_hs_init(void) void ble_hs_deinit(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_ctx) { + return; + } +#endif ble_hs_flow_deinit(); #if BLE_MONITOR ble_monitor_deinit(); #endif - ble_npl_mutex_deinit(&ble_hs_mutex); - ble_mqueue_deinit(&ble_hs_rx_q); ble_hs_stop_deinit(); +#if CONFIG_BT_NIMBLE_AOA_AOD + extern void ble_svc_cte_deinit(void); + ble_svc_cte_deinit(); +#endif + ble_gap_deinit(); ble_hs_hci_deinit(); @@ -1111,9 +1128,9 @@ ble_hs_deinit(void) ble_gattc_cache_conn_free_all_mem(); #endif + ble_att_deinit(); ble_l2cap_deinit(); - ble_att_deinit(); #endif #if MYNEWT_VAL(BLE_GATTS) @@ -1136,29 +1153,18 @@ ble_hs_deinit(void) #if (MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) ble_hs_resolv_deinit(); #endif -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_ctx) { - ble_hs_ctx->parent_task = NULL; - if (ble_hs_ctx->hci_os_event_buf) { - nimble_platform_mem_free(ble_hs_ctx->hci_os_event_buf); - ble_hs_ctx->hci_os_event_buf = NULL; - } - os_mempool_unregister(&ble_hs_hci_ev_pool); - - nimble_platform_mem_free(ble_hs_ctx); - ble_hs_ctx = NULL; - } - - if (ble_hs_state_ctx) { - nimble_platform_mem_free(ble_hs_state_ctx); - ble_hs_state_ctx = NULL; - } + ble_store_config_deinit(); #if MYNEWT_VAL(BLE_PERIODIC_ADV) ble_hs_periodic_sync_deinit(); #endif - ble_store_config_deinit(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + /* Must be called before ble_hs_ctx is freed — uses ble_hs_lock() which + * expands to ble_hs_ctx->mutex under BLE_STATIC_TO_DYNAMIC. */ +#if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_GAP_SERVICE + ble_svc_gap_deinit(); +#endif ble_hs_adv_parse_free(); @@ -1172,8 +1178,33 @@ ble_hs_deinit(void) ble_uuid_deinit(); -#if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_GAP_SERVICE - ble_svc_gap_deinit(); + if (ble_hs_ctx) { + ble_hs_ctx->parent_task = NULL; + if (ble_hs_ctx->hci_os_event_buf) { + nimble_platform_mem_free(ble_hs_ctx->hci_os_event_buf); + ble_hs_ctx->hci_os_event_buf = NULL; + } + os_mempool_unregister(&ble_hs_hci_ev_pool); + + /* ble_hs_mutex expands to ble_hs_ctx->mutex here; deinit before + * freeing the context that contains it. */ + ble_npl_mutex_deinit(&ble_hs_mutex); + + nimble_platform_mem_free(ble_hs_ctx); + ble_hs_ctx = NULL; + } + + if (ble_hs_state_ctx) { + nimble_platform_mem_free(ble_hs_state_ctx); + ble_hs_state_ctx = NULL; + } + +#else + /* Non-dynamic build: ble_hs_mutex is a static variable; destroy it last + * once all subsystems that call ble_hs_lock() have been torn down. */ +#if MYNEWT_VAL(BLE_HS_PVCY) + ble_hs_pvcy_irk_deinit(); #endif + ble_npl_mutex_deinit(&ble_hs_mutex); #endif } diff --git a/nimble/host/src/ble_hs_adv.c b/nimble/host/src/ble_hs_adv.c index 6ec8e5c34..5c8700ede 100644 --- a/nimble/host/src/ble_hs_adv.c +++ b/nimble/host/src/ble_hs_adv.c @@ -65,7 +65,7 @@ ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len, int rc; if (om ) { - if (data_len > 254) { + if (data_len == 255) { /* uint8_t max; +1 below would wrap to 0 */ return BLE_HS_EMSGSIZE; } data_len++; @@ -97,8 +97,7 @@ ble_hs_adv_set_flat_mbuf(uint8_t type, int data_len, const void *data, { int rc; - BLE_HS_DBG_ASSERT(data_len > 0); - if (data_len > 254) { + if (data_len <= 0 || data_len > 254) { return BLE_HS_EMSGSIZE; } @@ -158,7 +157,10 @@ ble_hs_adv_set_array_uuid16(uint8_t type, uint8_t num_elems, } else { /* elems is const ble_uuid16_t*; u.type is always BLE_UUID_TYPE_16 * by construction; ble_uuid_flat writes exactly 2 bytes for 16-bit UUIDs */ - ble_uuid_flat(&elems[i].u, dst + *dst_len); + rc = ble_uuid_flat(&elems[i].u, dst + *dst_len); + if (rc != 0) { + return rc; + } *dst_len += 2; } } @@ -233,8 +235,9 @@ ble_hs_adv_set_array_uuid128(uint8_t type, uint8_t num_elems, return rc; } } else { - if (ble_uuid_flat(&elems[i].u, dst + *dst_len) != 0) { - return BLE_HS_EINVAL; + rc = ble_uuid_flat(&elems[i].u, dst + *dst_len); + if (rc != 0) { + return rc; } *dst_len += 16; } @@ -641,6 +644,17 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields, #endif #endif + /*** 0x30 - Broadcast name. */ + if (adv_fields->broadcast_name != NULL && adv_fields->broadcast_name_len > 0) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_BROADCAST_NAME, + adv_fields->broadcast_name_len, + adv_fields->broadcast_name, + dst, &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } + } + /*** 0xff - Manufacturer specific data. */ if ((adv_fields->mfg_data != NULL) && (adv_fields->mfg_data_len >= 2)) { rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_MFG_DATA, @@ -802,7 +816,7 @@ ble_hs_adv_parse_uuids128(struct ble_hs_adv_fields *adv_fields, * type are present in advertising data. */ - free_slots = (BLE_HS_ADV_MAX_FIELD_SZ / sizeof(ble_uuid128_t)) - adv_fields->num_uuids128; + free_slots = (BLE_HS_ADV_MAX_FIELD_SZ / 16) - adv_fields->num_uuids128; if (uuid_cnt > free_slots) { /* not enough space to append */ @@ -1089,10 +1103,11 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, break; case BLE_HS_ADV_TYPE_LE_SUPP_FEAT: - if (data_len != BLE_HS_ADV_LE_SUPP_FEAT_LEN) { + if (data_len < 1 || data_len > BLE_HS_ADV_LE_SUPP_FEAT_LEN) { return BLE_HS_EBADDATA; } - memcpy(adv_fields->le_supp_feat, data, BLE_HS_ADV_LE_SUPP_FEAT_LEN); + memset(adv_fields->le_supp_feat, 0, BLE_HS_ADV_LE_SUPP_FEAT_LEN); + memcpy(adv_fields->le_supp_feat, data, data_len); adv_fields->le_supp_feat_is_present = 1; break; @@ -1111,6 +1126,11 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, adv_fields->enc_adv_data_len = data_len; break; #endif + case BLE_HS_ADV_TYPE_BROADCAST_NAME: + adv_fields->broadcast_name = data; + adv_fields->broadcast_name_len = data_len; + break; + case BLE_HS_ADV_TYPE_MFG_DATA: adv_fields->mfg_data = data; adv_fields->mfg_data_len = data_len; diff --git a/nimble/host/src/ble_hs_conn.c b/nimble/host/src/ble_hs_conn.c index 722269f00..51502c96b 100644 --- a/nimble/host/src/ble_hs_conn.c +++ b/nimble/host/src/ble_hs_conn.c @@ -78,7 +78,7 @@ ble_hs_conn_can_alloc(void) int result; - ble_hs_lock(); + ble_hs_lock_nested(); #if MYNEWT_VAL(BLE_GATTS) result = ble_hs_conn_pool.mp_num_free >= 1 && @@ -89,7 +89,7 @@ ble_hs_conn_can_alloc(void) ble_l2cap_chan_pool.mp_num_free >= BLE_HS_CONN_MIN_CHANS; #endif - ble_hs_unlock(); + ble_hs_unlock_nested(); return result; } @@ -511,13 +511,17 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, addrs->our_id_addr.type == BLE_ADDR_RANDOM) { our_id_addr_val = conn->bhc_our_rnd_addr; } else { + ble_hs_lock_nested(); rc = ble_hs_id_addr(addrs->our_id_addr.type, &our_id_addr_val, NULL); + ble_hs_unlock_nested(); if (rc != 0) { return; } } #else + ble_hs_lock_nested(); rc = ble_hs_id_addr(addrs->our_id_addr.type, &our_id_addr_val, NULL); + ble_hs_unlock_nested(); if (rc != 0) { return; } @@ -542,14 +546,15 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, memcpy(bhc_peer_addr.val, conn->bhc_peer_addr.val, BLE_DEV_ADDR_LEN); struct ble_hs_resolv_entry *rl = NULL; + ble_hs_lock_nested(); rl = ble_hs_resolv_list_find(bhc_peer_addr.val); if (rl != NULL) { memcpy(addrs->peer_id_addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN); addrs->peer_id_addr.type = rl->rl_addr_type; if (ble_host_rpa_enabled()) { - uint8_t *local_id = NULL; - rc = ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL); + const uint8_t *local_id = NULL; + rc = ble_hs_id_addr(BLE_ADDR_PUBLIC, &local_id, NULL); /* RL is present: populate our id addr with public ID */ if (rc == 0 && local_id != NULL) { @@ -561,6 +566,7 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, } } } + ble_hs_unlock_nested(); #endif switch (conn->bhc_peer_addr.type) { case BLE_ADDR_PUBLIC: @@ -614,7 +620,7 @@ ble_hs_conn_timer(void) * passes after a partial packet is received, the connection is * terminated. */ - if (conn->rx_len) { + if (conn->rx_frags) { time_diff = conn->rx_frag_tmo - now; if (time_diff <= 0) { @@ -692,11 +698,12 @@ ble_hs_conn_init(void) ble_hs_conn_elem_mem = NULL; #endif os_mempool_unregister(&ble_hs_conn_pool); + memset(&ble_hs_conn_pool, 0, sizeof(ble_hs_conn_pool)); if (ble_hs_conn_ctx) { nimble_platform_mem_free(ble_hs_conn_ctx); ble_hs_conn_ctx = NULL; } - memset(&ble_hs_conn_pool, 0, sizeof(ble_hs_conn_pool)); + #endif return BLE_HS_EOS; } diff --git a/nimble/host/src/ble_hs_flow.c b/nimble/host/src/ble_hs_flow.c index 2780d0315..7467579fe 100644 --- a/nimble/host/src/ble_hs_flow.c +++ b/nimble/host/src/ble_hs_flow.c @@ -211,6 +211,8 @@ ble_hs_flow_acl_free(struct os_mempool_ext *mpe, void *data, void *arg) #else int idx = ble_hs_flow_mbuf_index(om); if (idx < 0) { + BLE_HS_LOG(INFO, "ACL free: mbuf index not found; " + "flow credit may be lost\n"); return os_memblock_put_from_cb(&mpe->mpe_mp, data); } conn_handle = ble_hs_flow_mbuf_conn_handle[idx]; @@ -252,11 +254,11 @@ ble_hs_flow_connection_broken(uint16_t conn_handle) { uint16_t snapshot = ble_hs_flow_num_completed_pkts; int rc = ble_hs_flow_tx_num_comp_pkts(); + /* Decrement unconditionally to prevent counter overflow during async reset window */ + ble_hs_flow_num_completed_pkts = (ble_hs_flow_num_completed_pkts >= snapshot) ? + ble_hs_flow_num_completed_pkts - snapshot : 0; if (rc != 0) { ble_hs_sched_reset(rc); - } else { - ble_hs_flow_num_completed_pkts = (ble_hs_flow_num_completed_pkts >= snapshot) ? - ble_hs_flow_num_completed_pkts - snapshot : 0; } } #else @@ -391,6 +393,13 @@ ble_hs_flow_deinit(void) nimble_platform_mem_free(ble_hs_flow_ctx); ble_hs_flow_ctx = NULL; #else + ble_npl_eventq_remove(ble_hs_evq_get(), &ble_hs_flow_ev); + ble_npl_callout_stop(&ble_hs_flow_timer); +#if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED + ble_hci_trans_set_acl_free_cb(NULL, NULL); +#else + ble_transport_register_put_acl_from_ll_cb(NULL); +#endif ble_npl_event_deinit(&ble_hs_flow_ev); ble_npl_callout_deinit(&ble_hs_flow_timer); #endif /* MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) */ diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c index 8ab0b5606..9748cc6df 100644 --- a/nimble/host/src/ble_hs_hci.c +++ b/nimble/host/src/ble_hs_hci.c @@ -246,11 +246,6 @@ static void esp_hci_err_to_name(int error_code, uint16_t *opcode) esp_core_err_to_name(error_code, opcode); return; } - else if (error_code - 0x200 < 0) { - /* Converts error code to HCI base */ - error_code = BLE_HS_HCI_ERR(error_code); - } - for (int i = 0; i Opcode group field & Opcode command field (2) - * -> Parameter Length (1) - * Length of all the parameters (does not include any part of the hci - * command header - */ + +/* Length of the HCI command header (opcode 2 bytes + length 1 byte) */ #define BLE_HCI_CMD_HDR_LEN (3) static int @@ -64,10 +57,6 @@ ble_hs_hci_cmd_send(uint16_t opcode, uint8_t len, const void *cmddata) int rc; cmd = (struct ble_hci_cmd *)ble_transport_alloc_cmd(); - if (cmd == NULL) { - return BLE_HS_ENOMEM_EVT; - } - BLE_HS_DBG_ASSERT(cmd != NULL); if (cmd == NULL) { return BLE_HS_ENOMEM; } diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index e58aaca90..c69fbb733 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -17,6 +17,7 @@ * under the License. */ #include +#include #include #include #include @@ -112,6 +113,8 @@ static uint16_t ble_adv_list_count; #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) #define BLE_ADV_LIST_MAX_LENGTH 50 #define BLE_ADV_LIST_MAX_COUNT 200 + +static bool ble_adv_list_ready(void); #endif #if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) @@ -462,16 +465,15 @@ ble_hs_hci_evt_resolve_rpa(ble_addr_t *addr) } 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(); + if (rl == NULL && ble_hs_is_rpa(addr->val, ota_addr_type) && + ble_gap_rpa_resolve(addr->val, id_addr, &id_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 = id_addr_type; @@ -529,6 +531,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) if (conn_found) { int rc; + uint16_t conn_handle = le16toh(ev->conn_handle); #if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) should_reattempt = (!conn_slave && ev->reason == BLE_ERR_CONN_SPVN_TMO); @@ -542,7 +545,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_hci_ensure_ctx()) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + goto skip_reattempt; } #endif @@ -555,11 +558,9 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, reattempt_conn.count); reattempt_conn.count += 1; - /* Post event to interested application; pass raw LE value since - * ble_gap_reattempt_count applies le16toh internally. */ - ble_gap_reattempt_count(ev->conn_handle, reattempt_conn.count); + ble_gap_reattempt_count(conn_handle, reattempt_conn.count); #if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_OBSERVER) - rc = ble_gap_master_connect_reattempt(ev->conn_handle); + rc = ble_gap_master_connect_reattempt(conn_handle); if (rc != 0) { BLE_HS_LOG(INFO, "Master reconnect attempt failed; rc = %d", rc); } @@ -579,19 +580,19 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, #endif BLE_HS_LOG(INFO, "Reattempt advertising; reason: 0x%x, status = %x", ev->reason, ev->status); - ble_l2cap_sig_conn_broken(ev->conn_handle, BLE_ERR_CONN_ESTABLISHMENT); - ble_sm_connection_broken(ev->conn_handle); + ble_l2cap_sig_conn_broken(conn_handle, BLE_ERR_CONN_ESTABLISHMENT); + ble_sm_connection_broken(conn_handle); #if MYNEWT_VAL(BLE_GATTS) - ble_gatts_connection_broken(ev->conn_handle); + ble_gatts_connection_broken(conn_handle); #endif #if MYNEWT_VAL(BLE_GATTC) - ble_gattc_connection_broken(ev->conn_handle); + ble_gattc_connection_broken(conn_handle); #endif - ble_hs_flow_connection_broken(ev->conn_handle);; + ble_hs_flow_connection_broken(conn_handle);; #if MYNEWT_VAL(BLE_GATT_CACHING) - ble_gattc_cache_conn_broken(ev->conn_handle); + ble_gattc_cache_conn_broken(conn_handle); #endif - rc = ble_hs_atomic_conn_delete(ev->conn_handle); + rc = ble_hs_atomic_conn_delete(conn_handle); if (rc != 0) { return rc; } @@ -608,6 +609,11 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, /* Normal disconnect. Reset the structure */ memset(&reattempt_conn, 0x0, sizeof (struct ble_gap_reattempt_ctxt)); } + +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +skip_reattempt: + ; +#endif } #endif @@ -885,17 +891,29 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, if (evt.status == BLE_ERR_DIR_ADV_TMO || evt.role == BLE_HCI_LE_CONN_COMPLETE_ROLE_SLAVE) { -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_hci_ensure_ctx()) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; - } + /* PAwR slave connections use the V2 subevent (not ENH_CONN_COMPLETE). + * The controller does NOT send LE Advertising Set Terminated for PAwR, + * so we must NOT store them as pending — they would hang indefinitely. + * Fall through to the BLE_PERIODIC_ADV_WITH_RESPONSES handler instead. */ +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + if (subevent == BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE) #endif - - /* store this until we get set terminated event with adv handle */ - memcpy(&pend_conn_complete, &evt, sizeof(evt)); - pend_conn_complete_valid = true; - return 0; + { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_hci_ensure_ctx()) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; + } +#endif + /* store this until we get set terminated event with adv handle */ +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + evt.adv_handle = 0xFF; + evt.sync_handle = 0xFFFF; +#endif + memcpy(&pend_conn_complete, &evt, sizeof(evt)); + pend_conn_complete_valid = true; + return 0; + } } #endif @@ -907,7 +925,7 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, evt.sync_handle = 0xFFFF; } else { evt.adv_handle = ev->adv_handle; - evt.sync_handle = ev->sync_handle; + evt.sync_handle = le16toh(ev->sync_handle); if (evt.adv_handle < BLE_ADV_INSTANCES) { instance = evt.adv_handle; } @@ -1059,12 +1077,15 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) /* BLE Queue Congestion check*/ #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) - if (ble_get_adv_list_length() > BLE_ADV_LIST_MAX_LENGTH || ble_adv_list_count > BLE_ADV_LIST_MAX_COUNT) { - ble_adv_list_refresh(); - } - if (ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) == BLE_NPL_OK) { - ble_adv_list_count++; - ble_npl_mutex_release(&adv_list_lock); + if (ble_adv_list_ready()) { + if (ble_get_adv_list_length() > BLE_ADV_LIST_MAX_LENGTH || + ble_adv_list_count > BLE_ADV_LIST_MAX_COUNT) { + ble_adv_list_refresh(); + } + if (ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) == BLE_NPL_OK) { + ble_adv_list_count++; + ble_npl_mutex_release(&adv_list_lock); + } } #endif @@ -1133,7 +1154,6 @@ ble_hs_hci_evt_le_dir_adv_rpt(uint8_t subevent, const void *data, unsigned int l #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) desc.ota_addr = desc.addr; ble_hs_hci_evt_resolve_rpa(&desc.addr); - ble_hs_hci_evt_resolve_rpa(&desc.direct_addr); #endif ble_gap_rx_adv_report(&desc); } @@ -1537,8 +1557,8 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data, desc.periodic_adv_itvl = le16toh(report->periodic_itvl); #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + desc.ota_addr = desc.addr; ble_hs_hci_evt_resolve_rpa(&desc.addr); - ble_hs_hci_evt_resolve_rpa(&desc.direct_addr); #endif ble_gap_rx_ext_adv_report(&desc); @@ -1556,13 +1576,25 @@ ble_hs_hci_evt_le_periodic_adv_sync_estab(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev = data; + unsigned int min_len; - if (len < sizeof(*ev)) { +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + if (subevent == BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB) { + min_len = offsetof(struct ble_hci_ev_le_subev_periodic_adv_sync_estab, + num_subevents); + } else { + min_len = sizeof(*ev); + } +#else + min_len = sizeof(*ev); +#endif + + if (len < min_len) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - ble_gap_rx_peroidic_adv_sync_estab(ev); + ble_gap_rx_peroidic_adv_sync_estab(ev, subevent); return 0; } @@ -1643,13 +1675,25 @@ ble_hs_hci_evt_le_periodic_adv_sync_transfer(uint8_t subevent, const void *data, { #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER) const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev = data; + unsigned int min_len; - if (len < sizeof(*ev)) { +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + if (subevent == BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER) { + min_len = offsetof(struct ble_hci_ev_le_subev_periodic_adv_sync_transfer, + num_subevents); + } else { + min_len = sizeof(*ev); + } +#else + min_len = sizeof(*ev); +#endif + + if (len < min_len) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBADDATA); return BLE_HS_EBADDATA; } - ble_gap_rx_periodic_adv_sync_transfer(ev); + ble_gap_rx_periodic_adv_sync_transfer(ev, subevent); #endif return 0; @@ -1864,6 +1908,7 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data struct ble_gap_periodic_adv_response resp; const uint8_t *payload; unsigned int payload_len; + uint8_t i; if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); @@ -1877,7 +1922,7 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data payload = (const uint8_t *)(ev + 1); payload_len = len - sizeof(*ev); - for (uint8_t i = 0; i < ev->num_responses; i++) { + for (i = 0; i < ev->num_responses; i++) { if (payload_len < sizeof(*response)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; @@ -1892,9 +1937,9 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data resp.rssi = response->rssi; resp.cte_type = response->cte_type; resp.response_slot = response->response_slot; + resp.data_status = response->data_status; resp.data_length = response->data_length; resp.data = response->data; - resp.data_status = response->data_status; ble_gap_rx_periodic_adv_response(resp); @@ -2125,6 +2170,14 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev) rc = BLE_HS_ECONTROLLER; goto done; } + /* NOP Command Complete events are sent by the controller for flow + * control and have opcode 0; they have no dispatch entry and should + * be silently consumed. + */ + if (opcode == BLE_HCI_OPCODE_NOP) { + rc = 0; + goto done; + } entry = ble_hs_hci_evt_dispatch_find(opcode); } else { @@ -2236,21 +2289,25 @@ ble_adv_list_ready(void) return true; } -void ble_adv_list_init(void) +int ble_adv_list_init(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_hci_ensure_ctx()) { - return; + return BLE_HS_ENOMEM; } #endif if (ble_npl_mutex_init(&adv_list_lock) != BLE_NPL_OK) { BLE_HS_LOG(ERROR, "%s mutex init failed", __func__); - return; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_hs_hci_ctx_free(); +#endif + return BLE_HS_EOS; } SLIST_INIT(&ble_adv_list); ble_adv_list_count = 0; + return 0; } void ble_adv_list_deinit(void) @@ -2258,8 +2315,16 @@ void ble_adv_list_deinit(void) struct ble_addr_list_entry *device; struct ble_addr_list_entry *temp; - if (!ble_adv_list_ready() || - ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + if (!ble_adv_list_ready()) { + return; + } + + if (ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) { + SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next); + nimble_platform_mem_free(device); + } + ble_npl_mutex_deinit(&adv_list_lock); return; } @@ -2273,32 +2338,34 @@ void ble_adv_list_deinit(void) ble_npl_mutex_deinit(&adv_list_lock); } -void ble_adv_list_add_packet(void *data) +bool ble_adv_list_add_packet(void *data) { struct ble_addr_list_entry *device; struct ble_addr_list_entry *entry; if (!data) { BLE_HS_LOG(ERROR, "%s data is NULL", __func__); - return; + return false; } if (!ble_adv_list_ready() || ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { - return; + return false; } device = (struct ble_addr_list_entry *)data; SLIST_FOREACH(entry, &ble_adv_list, next) { - if (entry == device) { + if (entry->addr.type == device->addr.type && + memcmp(entry->addr.val, device->addr.val, BLE_DEV_ADDR_LEN) == 0) { ble_npl_mutex_release(&adv_list_lock); - return; + return false; } } SLIST_INSERT_HEAD(&ble_adv_list, device, next); ble_npl_mutex_release(&adv_list_lock); + return true; } uint32_t ble_get_adv_list_length(void) @@ -2373,7 +2440,9 @@ bool ble_check_adv_list(const uint8_t *addr, uint8_t addr_type) if (adv_packet) { adv_packet->addr.type = addr_type; memcpy(adv_packet->addr.val, addr, BLE_DEV_ADDR_LEN); - ble_adv_list_add_packet(adv_packet); + if (!ble_adv_list_add_packet(adv_packet)) { + nimble_platform_mem_free(adv_packet); + } } else { BLE_HS_LOG(ERROR, "%s adv_packet malloc failed", __func__); } diff --git a/nimble/host/src/ble_hs_id.c b/nimble/host/src/ble_hs_id.c index 72576aa12..9eb200398 100644 --- a/nimble/host/src/ble_hs_id.c +++ b/nimble/host/src/ble_hs_id.c @@ -317,6 +317,8 @@ ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr, } #endif + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + switch (id_addr_type) { case BLE_ADDR_PUBLIC: case BLE_ADDR_PUBLIC_ID: @@ -372,7 +374,6 @@ static int ble_hs_id_addr_type_usable(uint8_t own_addr_type) { uint8_t id_addr_type; - const uint8_t *id_addr_ptr; int nrpa; int rc; @@ -387,18 +388,17 @@ ble_hs_id_addr_type_usable(uint8_t own_addr_type) case BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT: case BLE_OWN_ADDR_RPA_RANDOM_DEFAULT: +#if !MYNEWT_VAL(BLE_HS_PVCY) + return BLE_HS_ENOTSUP; +#endif id_addr_type = ble_hs_misc_own_addr_type_to_id(own_addr_type); - rc = ble_hs_id_addr(id_addr_type, &id_addr_ptr, &nrpa); + rc = ble_hs_id_addr(id_addr_type, NULL, &nrpa); if (rc != 0) { return rc; } if (nrpa) { return BLE_HS_ENOADDR; } - -#if !MYNEWT_VAL(BLE_HS_PVCY) - return BLE_HS_ENOTSUP; -#endif break; default: diff --git a/nimble/host/src/ble_hs_iso.c b/nimble/host/src/ble_hs_iso.c index 4e5026be4..0a9facfdb 100644 --- a/nimble/host/src/ble_hs_iso.c +++ b/nimble/host/src/ble_hs_iso.c @@ -50,18 +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) { BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - if (pktlen < BLE_HCI_ISO_DATA_LOAD_HDR_SZ || max_pkts == 0) { + if (pktlen == 0 || max_pkts == 0) { return BLE_HS_EINVAL; } @@ -193,9 +187,25 @@ ble_hs_hci_iso_tx_now(uint16_t conn_handle, const uint8_t *sdu, uint16_t sdu_len uint8_t dlh_len; uint8_t *frag; int rc; +#if MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL) + uint16_t count; +#endif #if MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL) - uint16_t count = ble_hs_hci_iso_buf_needed(sdu_len, ts_flag); + dlh_len = (ts_flag ? BLE_HCI_ISO_DATA_LOAD_TS_SZ : 0) + + BLE_HCI_ISO_DATA_LOAD_HDR_SZ; + count = ble_hs_hci_iso_buf_needed(sdu_len, ts_flag); + + if (count == 0) { + return BLE_HS_EINVAL; + } + + /* Host does not support fragmentation yet; reject SDUs that don't fit in one buffer. */ + if (count > 1) { + BLE_HS_LOG(ERROR, "ISO SDU+hdr too large (%u > %u)", + (unsigned)(sdu_len + dlh_len), (unsigned)ble_hs_iso_buf_sz); + return BLE_HS_EMSGSIZE; + } /* Make sure the Controller ISO buffer can accommodate the SDU completely */ if (count > ble_hs_iso_avail_pkts) { @@ -207,10 +217,13 @@ ble_hs_hci_iso_tx_now(uint16_t conn_handle, const uint8_t *sdu, uint16_t sdu_len BLE_HS_LOG(WARN, "ISO flow control!"); return BLE_HS_EAGAIN; } + dlh_len = (ts_flag ? BLE_HCI_ISO_DATA_LOAD_TS_SZ : 0) + + BLE_HCI_ISO_DATA_LOAD_HDR_SZ; +#else + dlh_len = (ts_flag ? BLE_HCI_ISO_DATA_LOAD_TS_SZ : 0) + + BLE_HCI_ISO_DATA_LOAD_HDR_SZ; #endif - dlh_len = (ts_flag ? BLE_HCI_ISO_DATA_LOAD_TS_SZ : 0) + BLE_HCI_ISO_DATA_LOAD_HDR_SZ; - /* Note: * Here we allocate memory to hold the whole SDU, and in the BLE Controller, * the SDU will be checked. If it's too large, error will be returned. diff --git a/nimble/host/src/ble_hs_iso_hci.c b/nimble/host/src/ble_hs_iso_hci.c index 5b7b541b8..3f6d7d9d6 100644 --- a/nimble/host/src/ble_hs_iso_hci.c +++ b/nimble/host/src/ble_hs_iso_hci.c @@ -271,7 +271,7 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin } if (rsp_buf == NULL|| - rsp_len < sizeof(struct ble_hci_le_set_cig_params_rp) + cis_cnt * 2) { + rsp_len < sizeof(struct ble_hci_le_set_cig_params_test_rp) + cis_cnt * 2) { return BLE_HS_EINVAL; } @@ -311,7 +311,7 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_CIG_PARAMS_TEST), cmd_buf, cmd_len, rsp_buf, - sizeof(struct ble_hci_le_set_cig_params_rp) + + sizeof(struct ble_hci_le_set_cig_params_test_rp) + cis_cnt * 2); } #endif /* MYNEWT_VAL(BLE_ISO_TEST) */ diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index 816b02af7..d51089a8a 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/nimble/host/src/ble_hs_pvcy.c @@ -81,8 +81,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) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_HCI_ERR(BLE_ERR_INV_HCI_CMD_PARMS)); - return BLE_HS_HCI_ERR(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); @@ -209,17 +209,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_hs_lock(); + rc = ble_hs_resolv_list_rmv(addr_type, &cmd.peer_id_addr[0]); + ble_hs_unlock(); +#else 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]); -#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(); +#endif return rc; } @@ -260,7 +262,7 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, #endif if (addr_type > BLE_ADDR_RANDOM) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.peer_addr_type = addr_type; @@ -397,10 +399,10 @@ ble_hs_pvcy_ensure_started(void) } #endif - ble_hs_lock(); + ble_hs_lock_nested(); if (ble_hs_pvcy_started) { - ble_hs_unlock(); + ble_hs_unlock_nested(); return 0; } @@ -420,13 +422,13 @@ ble_hs_pvcy_ensure_started(void) } if (rc != 0) { - ble_hs_unlock(); + ble_hs_unlock_nested(); return rc; } ble_hs_pvcy_started = 1; - ble_hs_unlock(); + ble_hs_unlock_nested(); return 0; } @@ -527,7 +529,7 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) { uint8_t tmp_addr[6]; uint8_t new_irk[16]; - int rc; + int rc = 0; #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_pvcy_ctx == NULL) { @@ -555,7 +557,7 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) ble_hs_resolv_enable(0); } - ble_hs_resolv_list_clear_all(false); + ble_hs_resolv_list_clear_all(); if (rpa_state) { ble_hs_resolv_enable(1); @@ -580,7 +582,7 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) done: ble_gap_preempt_done(); if (rc != 0) { - return rc; + goto pvcy_done; } #endif @@ -597,11 +599,20 @@ done: memset(tmp_addr, 0, 6); rc = ble_hs_pvcy_add_entry(tmp_addr, 0, zero_irk); if (rc != 0) { +#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + goto pvcy_done; +#else return rc; +#endif } #endif - return 0; +#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +pvcy_done: + ble_gap_preempt_done(); +#endif + + return rc; } int @@ -670,17 +681,16 @@ ble_hs_pvcy_rpa_config(uint8_t enable) { int rc = 0; - ble_hs_lock(); -#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - ble_gap_preempt(); -#endif - if (enable != NIMBLE_HOST_DISABLE_PRIVACY) { rc = ble_hs_pvcy_ensure_started(); if (rc != 0) { - goto done; + return rc; } + } + ble_hs_lock(); + + if (enable != NIMBLE_HOST_DISABLE_PRIVACY) { ble_hs_resolv_enable(true); /* Configure NRPA address related flags according to input parameter */ @@ -689,12 +699,10 @@ ble_hs_pvcy_rpa_config(uint8_t enable) } else { ble_hs_resolv_nrpa_disable(); } - } else { ble_hs_resolv_enable(false); } -done: ble_hs_unlock(); if (rc == 0 && enable != NIMBLE_HOST_DISABLE_PRIVACY) { @@ -704,9 +712,6 @@ done: rc = ble_hs_gen_own_private_rnd(); ble_gap_preempt_done(); } -#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - ble_gap_preempt_done(); -#endif return rc; } diff --git a/nimble/host/src/ble_hs_resolv.c b/nimble/host/src/ble_hs_resolv.c index ba9327fe8..387290429 100644 --- a/nimble/host/src/ble_hs_resolv.c +++ b/nimble/host/src/ble_hs_resolv.c @@ -801,6 +801,8 @@ ble_hs_resolv_list_add(uint8_t *cmdbuf) const uint8_t *ident_addr; struct ble_hs_resolv_entry *rl; struct ble_hs_dev_records *p_dev_rec = NULL; + uint8_t p_dev_idaddr_backup[6]; + bool p_dev_idaddr_modified = false; BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); @@ -841,6 +843,8 @@ ble_hs_resolv_list_add(uint8_t *cmdbuf) p_dev_rec = ble_rpa_find_peer_dev_by_irk(rl->rl_peer_irk); if (p_dev_rec != NULL) { + p_dev_idaddr_modified = true; + memcpy(p_dev_idaddr_backup, p_dev_rec->identity_addr, 6); memcpy(p_dev_rec->identity_addr, ident_addr, 6); memcpy(rl->rl_pseudo_id, p_dev_rec->pseudo_addr, 6); } @@ -852,8 +856,12 @@ ble_hs_resolv_list_add(uint8_t *cmdbuf) if (ble_hs_resolv_gen_priv_addr(rl, 1) != 0 || ble_hs_resolv_gen_priv_addr(rl, 0) != 0) { memset(rl, 0, sizeof(*rl)); + if (p_dev_idaddr_modified) { + memcpy(p_dev_rec->identity_addr, p_dev_idaddr_backup, 6); + } return BLE_HS_EUNKNOWN; } + ++(g_ble_hs_resolv_data.rl_cnt); BLE_HS_LOG(DEBUG, "Device added to RL, Resolving list count = %d\n", g_ble_hs_resolv_data.rl_cnt); @@ -912,7 +920,7 @@ ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr) * Clear the resolving list */ void -ble_hs_resolv_list_clear_all(bool preserve_local) +ble_hs_resolv_list_clear_all(void) { struct ble_hs_resolv_entry local_entry; bool restore_local = false; @@ -930,10 +938,8 @@ ble_hs_resolv_list_clear_all(bool preserve_local) } #endif - /* Index 0 is the local identity. Some callers clear only peer state, - * while local IRK replacement must clear it too. - */ - if (preserve_local && g_ble_hs_resolv_data.rl_cnt > 0) { + /* Preserve local device entry at index 0 if it exists */ + if (g_ble_hs_resolv_data.rl_cnt > 0) { memcpy(&local_entry, &g_ble_hs_resolv_list[0], sizeof(local_entry)); restore_local = true; } diff --git a/nimble/host/src/ble_hs_resolv_priv.h b/nimble/host/src/ble_hs_resolv_priv.h index 0a373105f..9d8a8fdc7 100644 --- a/nimble/host/src/ble_hs_resolv_priv.h +++ b/nimble/host/src/ble_hs_resolv_priv.h @@ -69,7 +69,7 @@ uint8_t *ble_hs_get_rpa_local(void); /* Remove a device from the resolving list */ int ble_hs_resolv_list_rmv(uint8_t, uint8_t *); /* Clear the resolving list and peer dev record */ -void ble_hs_resolv_list_clear_all(bool preserve_local); +void ble_hs_resolv_list_clear_all(void); /* Update the local IRK stored at index 0 (must hold host lock) */ void ble_hs_resolv_update_local_irk(const uint8_t *irk); diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index 8c594da45..299029acb 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -502,8 +502,6 @@ ble_hs_startup_reset_tx(void) int ble_hs_startup_go(void) { - struct ble_store_gen_key gen_key; - int key_rc; int rc; rc = ble_hs_startup_reset_tx(); @@ -562,31 +560,44 @@ ble_hs_startup_go(void) return rc; } +#if MYNEWT_VAL(BLE_HS_PVCY) && NIMBLE_BLE_SM + struct ble_store_gen_key gen_key; + int key_rc; + if (ble_hs_cfg.store_gen_key_cb) { memset(&gen_key, 0, sizeof(gen_key)); key_rc = ble_hs_cfg.store_gen_key_cb(BLE_STORE_GEN_KEY_IRK, &gen_key, BLE_HS_CONN_HANDLE_NONE); -#if MYNEWT_VAL(BLE_HS_PVCY) if (key_rc == 0) { rc = ble_hs_pvcy_set_our_irk(gen_key.irk); if (rc != 0) { BLE_HS_LOG(WARN, "ble_hs_pvcy_set_our_irk (stored) rc=%d\n", rc); + return rc; } } -#endif } else { key_rc = -1; } -#if MYNEWT_VAL(BLE_HS_PVCY) if (key_rc != 0) { ble_hs_pvcy_set_default_irk(); rc = ble_hs_pvcy_set_our_irk(NULL); if (rc != 0) { BLE_HS_LOG(WARN, "ble_hs_pvcy_set_our_irk (default) rc=%d\n", rc); + return rc; } } +#elif MYNEWT_VAL(BLE_HS_PVCY) + /* SM disabled: no bond store or AES for IRK/RPA; skip privacy init. */ +#else + if (ble_hs_cfg.store_gen_key_cb) { + struct ble_store_gen_key gen_key; + + memset(&gen_key, 0, sizeof(gen_key)); + (void)ble_hs_cfg.store_gen_key_cb(BLE_STORE_GEN_KEY_IRK, &gen_key, + BLE_HS_CONN_HANDLE_NONE); + } #endif /* If flow control is enabled, configure the controller to use it. diff --git a/nimble/host/src/ble_hs_stop.c b/nimble/host/src/ble_hs_stop.c index f336a71b8..ebae19374 100644 --- a/nimble/host/src/ble_hs_stop.c +++ b/nimble/host/src/ble_hs_stop.c @@ -68,6 +68,10 @@ static uint8_t ble_hs_stop_conn_cnt; static struct ble_npl_callout ble_hs_stop_terminate_tmo; #endif +/** List of listeners being notified; guarded by ble_hs_stop_notifying. */ +static struct ble_hs_stop_listener_slist ble_hs_stop_notify_list; +static bool ble_hs_stop_notifying; + static int ble_hs_stop_hci_reset(void) { @@ -81,8 +85,9 @@ ble_hs_stop_hci_reset(void) static void ble_hs_stop_done(int status) { - struct ble_hs_stop_listener_slist slist; + struct ble_hs_stop_listener_slist notify_list; struct ble_hs_stop_listener *listener; + struct ble_hs_stop_listener *l; int rc; ble_npl_callout_stop(&ble_hs_stop_terminate_tmo); @@ -94,28 +99,60 @@ ble_hs_stop_done(int status) return; } - slist = ble_hs_stop_listeners; + ble_hs_stop_notify_list = ble_hs_stop_listeners; SLIST_INIT(&ble_hs_stop_listeners); - - ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF; + ble_hs_stop_notifying = true; ble_hs_unlock(); ble_gap_event_listener_unregister(&ble_hs_stop_gap_listener); - /* Reset the controller. */ + /* Reset the controller while still in STOPPING state so the HCI ACK is + * not dropped by host_rcv_pkt (which drops packets when state == OFF). */ rc = ble_hs_stop_hci_reset(); if (rc != 0) { BLE_HS_LOG(ERROR, "ble_hs_stop: failed to reset controller; rc=%d\n", rc); } + /* Mark host as OFF and absorb any listeners registered during the HCI + * reset before notifying waiters. */ + ble_hs_lock(); + ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF; + while ((listener = SLIST_FIRST(&ble_hs_stop_listeners)) != NULL) { + bool already; + + SLIST_REMOVE_HEAD(&ble_hs_stop_listeners, link); + already = false; + SLIST_FOREACH(l, &ble_hs_stop_notify_list, link) { + if (l == listener) { + already = true; + break; + } + } + if (!already) { + SLIST_INSERT_HEAD(&ble_hs_stop_notify_list, listener, link); + } + } + ble_hs_stop_notifying = false; + notify_list = ble_hs_stop_notify_list; + SLIST_INIT(&ble_hs_stop_notify_list); + ble_hs_unlock(); + /* Clear advertising, scanning and connection states. */ - ble_gap_reset_state(0); + if (status != 0) { + if (status == BLE_HS_ETIMEOUT) { + ble_gap_reset_state(BLE_ERR_CONN_SPVN_TMO); + } else { + ble_gap_reset_state(BLE_ERR_REM_USER_CONN_TERM); + } + } else { + ble_gap_reset_state(0); + } /* After LL reset the controller loses its random address */ ble_hs_id_reset(); - SLIST_FOREACH(listener, &slist, link) { + SLIST_FOREACH(listener, ¬ify_list, link) { listener->fn(status, listener->arg); } } @@ -160,13 +197,14 @@ ble_hs_stop_terminate_all_periodic_sync(void) * * Also, once the sync is terminated, the psync will be freed and * removed from the list such that the next iteration yields the next - * psync handle. Continue on errors so all syncs are cleaned up. + * psync handle. Return on errors; the caller (ble_hs_stop) will + * report the failure via ble_hs_stop_done. */ rc = ble_gap_periodic_adv_sync_terminate(sync_handle); if (rc != 0 && rc != BLE_HS_ENOTCONN) { BLE_HS_LOG(ERROR, "failed to terminate periodic sync=0x%04x, rc=%d\n", sync_handle, rc); - /* Continue attempting to terminate remaining syncs. */ + return rc; } } @@ -249,15 +287,22 @@ ble_hs_stop_register_listener(struct ble_hs_stop_listener *listener, BLE_HS_DBG_ASSERT(fn != NULL); + listener->fn = fn; + listener->arg = arg; + /* Guard against duplicate registration which would corrupt the list. */ SLIST_FOREACH(l, &ble_hs_stop_listeners, link) { if (l == listener) { return; } } - - listener->fn = fn; - listener->arg = arg; + if (ble_hs_stop_notifying) { + SLIST_FOREACH(l, &ble_hs_stop_notify_list, link) { + if (l == listener) { + return; + } + } + } SLIST_INSERT_HEAD(&ble_hs_stop_listeners, listener, link); } @@ -362,8 +407,6 @@ ble_hs_stop(struct ble_hs_stop_listener *listener, void ble_hs_stop_init(void) { - int rc; - #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (!ble_hs_stop_ctx) { ble_hs_stop_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_stop_ctx)); @@ -375,13 +418,15 @@ ble_hs_stop_init(void) #endif #ifdef MYNEWT - rc = ble_npl_callout_init(&ble_hs_stop_terminate_tmo, ble_npl_eventq_dflt_get(), - ble_hs_stop_terminate_timeout_cb, NULL); + ble_npl_callout_init(&ble_hs_stop_terminate_tmo, ble_npl_eventq_dflt_get(), + ble_hs_stop_terminate_timeout_cb, NULL); #else + int rc; + rc = ble_npl_callout_init(&ble_hs_stop_terminate_tmo, nimble_port_get_dflt_eventq(), ble_hs_stop_terminate_timeout_cb, NULL); -#endif SYSINIT_PANIC_ASSERT(rc == 0); +#endif } void diff --git a/nimble/host/src/ble_iso.c b/nimble/host/src/ble_iso.c index 600e7ed5b..42ccc85d3 100644 --- a/nimble/host/src/ble_iso.c +++ b/nimble/host/src/ble_iso.c @@ -17,9 +17,15 @@ * under the License. */ +#include +#include + #include "syscfg/syscfg.h" #if MYNEWT_VAL(BLE_ISO) +_Static_assert(MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE) > 4, + "BLE_TRANSPORT_ISO_SIZE must be > 4 for ISO SDU header"); + #include "os/os_mbuf.h" #include "host/ble_hs_log.h" #include "host/ble_hs.h" @@ -62,7 +68,7 @@ enum ble_iso_conn_type { struct ble_iso_big { SLIST_ENTRY(ble_iso_big) next; - uint8_t handle; + uint16_t handle; uint16_t max_pdu; uint8_t bis_cnt; @@ -73,7 +79,7 @@ struct ble_iso_big { struct ble_iso_conn { SLIST_ENTRY(ble_iso_conn) next; enum ble_iso_conn_type type; - uint8_t handle; + uint16_t handle; struct ble_iso_rx_data_info rx_info; struct os_mbuf *rx_buf; @@ -96,6 +102,18 @@ static struct os_mempool ble_iso_bis_pool; static os_membuf_t ble_iso_bis_mem[ OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_ISO_MAX_BISES), sizeof (struct ble_iso_bis))]; +static void +ble_iso_broadcast_code_set(uint8_t *dst, const char *src) +{ + size_t code_len; + + memset(dst, 0, 16); + if (src != NULL) { + code_len = strnlen(src, 16); + memcpy(dst, src, code_len); + } +} + static void ble_iso_conn_append(struct ble_iso_conn *conn) { @@ -119,7 +137,7 @@ ble_iso_big_handle_set(struct ble_iso_big *big) uint8_t i; /* Set next free handle */ - for (i = BLE_HCI_ISO_BIG_HANDLE_MIN; i < BLE_HCI_ISO_BIG_HANDLE_MAX; i++) { + for (i = BLE_HCI_ISO_BIG_HANDLE_MIN; i <= BLE_HCI_ISO_BIG_HANDLE_MAX; i++) { struct ble_iso_big *node = NULL; if (free_handle > BLE_HCI_ISO_BIG_HANDLE_MAX) { @@ -134,7 +152,7 @@ ble_iso_big_handle_set(struct ble_iso_big *big) } } - if (node == NULL || node->handle != big->handle) { + if (node == NULL) { return 0; } } @@ -209,12 +227,13 @@ static int ble_iso_big_free(struct ble_iso_big *big) { struct ble_iso_conn *conn; + struct ble_iso_conn *conn_tmp; struct ble_iso_bis *rem_bis[MYNEWT_VAL(BLE_ISO_MAX_BISES)] = { [0 ... MYNEWT_VAL(BLE_ISO_MAX_BISES) - 1] = NULL }; uint8_t i = 0; - SLIST_FOREACH(conn, &ble_iso_conns, next) { + SLIST_FOREACH_SAFE(conn, &ble_iso_conns, next, conn_tmp) { struct ble_iso_bis *bis; if (conn->type != BLE_ISO_CONN_BIS) { @@ -223,6 +242,11 @@ ble_iso_big_free(struct ble_iso_big *big) bis = CONTAINER_OF(conn, struct ble_iso_bis, conn); if (bis->big == big) { + if (conn->rx_buf != NULL) { + os_mbuf_free_chain(conn->rx_buf); + conn->rx_buf = NULL; + memset(&conn->rx_info, 0, sizeof(conn->rx_info)); + } SLIST_REMOVE(&ble_iso_conns, conn, ble_iso_conn, next); rem_bis[i++] = bis; } @@ -261,8 +285,7 @@ ble_iso_create_big(const struct ble_iso_create_big_params *create_params, struct ble_iso_big *big; int rc; - cp.adv_handle = create_params->adv_handle; - if (create_params->bis_cnt > MYNEWT_VAL(BLE_ISO_MAX_BISES)) { + if (create_params->bis_cnt == 0 || create_params->bis_cnt > MYNEWT_VAL(BLE_ISO_MAX_BISES)) { return BLE_HS_EINVAL; } @@ -285,18 +308,23 @@ ble_iso_create_big(const struct ble_iso_create_big_params *create_params, } } + cp.big_handle = big->handle; cp.adv_handle = create_params->adv_handle; cp.num_bis = create_params->bis_cnt; put_le24(cp.sdu_interval, big_params->sdu_interval); - cp.max_sdu = big_params->max_sdu; - cp.max_transport_latency = big_params->max_transport_latency; + cp.max_sdu = htole16(big_params->max_sdu); + cp.max_transport_latency = htole16(big_params->max_transport_latency); cp.rtn = big_params->rtn; cp.phy = big_params->phy; cp.packing = big_params->packing; cp.framing = big_params->framing; cp.encryption = big_params->encryption; if (big_params->encryption) { - memcpy(cp.broadcast_code, big_params->broadcast_code, 16); + if (big_params->broadcast_code == NULL) { + ble_iso_big_free(big); + return BLE_HS_EINVAL; + } + ble_iso_broadcast_code_set(cp.broadcast_code, big_params->broadcast_code); } rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -339,6 +367,8 @@ ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_compl { struct ble_iso_event event; struct ble_iso_big *big; + ble_iso_event_fn *cb; + void *cb_arg; big = ble_iso_big_find_by_handle(ev->big_handle); if (big == NULL) { @@ -346,9 +376,13 @@ ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_compl return; } + cb = big->cb; + cb_arg = big->cb_arg; + memset(&event, 0, sizeof(event)); event.type = BLE_ISO_EVENT_BIG_CREATE_COMPLETE; event.big_created.status = ev->status; + event.big_created.desc.big_handle = ev->big_handle; if (event.big_created.status != 0) { ble_iso_big_free(big); @@ -356,14 +390,13 @@ ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_compl if (big->bis_cnt != ev->num_bis) { BLE_HS_LOG_ERROR("Unexpected num_bis=%d != bis_cnt=%d\n", ev->num_bis, big->bis_cnt); - /* XXX: Should we destroy the group? */ } - ble_iso_big_conn_handles_init(big, ev->conn_handle, ev->num_bis); + ble_iso_big_conn_handles_init(big, ev->conn_handle, + min(big->bis_cnt, ev->num_bis)); - big->max_pdu = ev->max_pdu; + big->max_pdu = le16toh(ev->max_pdu); - event.big_created.desc.big_handle = ev->big_handle; event.big_created.desc.big_sync_delay = get_le24(ev->big_sync_delay); event.big_created.desc.transport_latency_big = get_le24(ev->transport_latency_big); @@ -371,16 +404,18 @@ ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_compl event.big_created.desc.bn = ev->bn; event.big_created.desc.pto = ev->pto; event.big_created.desc.irc = ev->irc; - event.big_created.desc.max_pdu = ev->max_pdu; - event.big_created.desc.iso_interval = ev->iso_interval; - event.big_created.desc.num_bis = ev->num_bis; - memcpy(event.big_created.desc.conn_handle, ev->conn_handle, - ev->num_bis * sizeof(uint16_t)); + event.big_created.desc.max_pdu = le16toh(ev->max_pdu); + event.big_created.desc.iso_interval = le16toh(ev->iso_interval); + event.big_created.desc.num_bis = min(big->bis_cnt, ev->num_bis); + for (uint8_t i = 0; i < event.big_created.desc.num_bis; i++) { + event.big_created.desc.conn_handle[i] = + le16toh(ev->conn_handle[i]); + } event.big_created.phy = ev->phy; } - if (big->cb != NULL) { - big->cb(&event, big->cb_arg); + if (cb != NULL) { + cb(&event, cb_arg); } } @@ -396,15 +431,18 @@ ble_iso_rx_terminate_big_complete(const struct ble_hci_ev_le_subev_terminate_big return; } + ble_iso_event_fn *cb = big->cb; + void *cb_arg = big->cb_arg; + event.type = BLE_ISO_EVENT_BIG_TERMINATE_COMPLETE; event.big_terminated.big_handle = ev->big_handle; event.big_terminated.reason = ev->reason; - if (big->cb != NULL) { - big->cb(&event, big->cb_arg); - } - ble_iso_big_free(big); + + if (cb != NULL) { + cb(&event, cb_arg); + } } static int @@ -419,7 +457,10 @@ ble_iso_tx_complete(uint16_t conn_handle, const uint8_t *data, return BLE_HS_ENOMEM; } - os_mbuf_extend(om, 8); + if (os_mbuf_extend(om, 8) == NULL) { + os_mbuf_free_chain(om); + return BLE_HS_ENOMEM; + } /* Connection_Handle, PB_Flag, TS_Flag */ put_le16(&om->om_data[0], BLE_HCI_ISO_HANDLE(conn_handle, BLE_HCI_ISO_PB_COMPLETE, 0)); @@ -428,11 +469,12 @@ ble_iso_tx_complete(uint16_t conn_handle, const uint8_t *data, /* Packet_Sequence_Number placeholder */ put_le16(&om->om_data[4], 0); /* ISO_SDU_Length */ - put_le16(&om->om_data[6], data_len); + put_le16(&om->om_data[6], data_len & BLE_HCI_ISO_SDU_LENGTH_MASK); rc = os_mbuf_append(om, data, data_len); if (rc) { - return rc; + os_mbuf_free_chain(om); + return BLE_HS_ENOMEM; } return ble_transport_to_ll_iso(om); @@ -450,13 +492,16 @@ ble_iso_tx_segmented(uint16_t conn_handle, const uint8_t *data, int rc; while (data_left) { - packet_len = min(MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE), data_left); if (data_left == data_len) { pb = BLE_HCI_ISO_PB_FIRST; - } else if (packet_len == data_left) { - pb = BLE_HCI_ISO_PB_LAST; + packet_len = min(MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE) - 4, data_left); } else { - pb = BLE_HCI_ISO_PB_CONTINUATION; + packet_len = min(MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE), data_left); + if (packet_len == data_left) { + pb = BLE_HCI_ISO_PB_LAST; + } else { + pb = BLE_HCI_ISO_PB_CONTINUATION; + } } om = ble_hs_mbuf_bare_pkt(); @@ -464,7 +509,10 @@ ble_iso_tx_segmented(uint16_t conn_handle, const uint8_t *data, return BLE_HS_ENOMEM; } - os_mbuf_extend(om, pb == BLE_HCI_ISO_PB_FIRST ? 8: 4); + if (os_mbuf_extend(om, pb == BLE_HCI_ISO_PB_FIRST ? 8 : 4) == NULL) { + os_mbuf_free_chain(om); + return BLE_HS_ENOMEM; + } /* Connection_Handle, PB_Flag, TS_Flag */ put_le16(&om->om_data[0], @@ -476,20 +524,24 @@ ble_iso_tx_segmented(uint16_t conn_handle, const uint8_t *data, put_le16(&om->om_data[2], packet_len + 4); /* Packet_Sequence_Number placeholder */ - put_le16(&om->om_data[8], 0); + put_le16(&om->om_data[4], 0); - /* ISO_SDU_Length */ - put_le16(&om->om_data[10], packet_len); + /* ISO_SDU_Length: must be total SDU length, not fragment length (BT Spec 5.4.5) */ + put_le16(&om->om_data[6], data_len & BLE_HCI_ISO_SDU_LENGTH_MASK); } else { put_le16(&om->om_data[2], packet_len); } rc = os_mbuf_append(om, data + offset, packet_len); if (rc) { - return rc; + os_mbuf_free_chain(om); + return BLE_HS_ENOMEM; } - ble_transport_to_ll_iso(om); + rc = ble_transport_to_ll_iso(om); + if (rc != 0) { + return rc; + } offset += packet_len; data_left -= packet_len; @@ -503,7 +555,11 @@ ble_iso_tx(uint16_t conn_handle, void *data, uint16_t data_len) { int rc; - if (data_len <= MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE)) { + if (data_len > BLE_HCI_ISO_SDU_LENGTH_MASK) { + return BLE_HS_EINVAL; + } + + if (data_len <= MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE) - 4) { rc = ble_iso_tx_complete(conn_handle, data, data_len); } else { rc = ble_iso_tx_segmented(conn_handle, data, data_len); @@ -523,6 +579,10 @@ ble_iso_big_sync_create(const struct ble_iso_big_sync_create_params *param, struct ble_iso_big *big; int rc; + if (param->bis_cnt == 0 || param->bis_cnt > MYNEWT_VAL(BLE_ISO_MAX_BISES)) { + return BLE_HS_EINVAL; + } + big = ble_iso_big_alloc(); if (big == NULL) { return BLE_HS_ENOMEM; @@ -538,7 +598,7 @@ ble_iso_big_sync_create(const struct ble_iso_big_sync_create_params *param, if (param->broadcast_code != NULL) { cp->encryption = BLE_HCI_ISO_BIG_ENCRYPTION_ENCRYPTED; - memcpy(cp->broadcast_code, param->broadcast_code, sizeof(cp->broadcast_code)); + ble_iso_broadcast_code_set(cp->broadcast_code, param->broadcast_code); } else { cp->encryption = BLE_HCI_ISO_BIG_ENCRYPTION_UNENCRYPTED; memset(cp->broadcast_code, 0, sizeof(cp->broadcast_code)); @@ -632,6 +692,7 @@ ble_iso_rx_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_establ memset(&event, 0, sizeof(event)); event.type = BLE_ISO_EVENT_BIG_SYNC_ESTABLISHED; event.big_sync_established.status = ev->status; + event.big_sync_established.desc.big_handle = ev->big_handle; if (event.big_sync_established.status != 0) { ble_iso_big_free(big); @@ -639,12 +700,13 @@ ble_iso_rx_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_establ if (big->bis_cnt != ev->num_bis) { BLE_HS_LOG_ERROR("Unexpected num_bis=%d != bis_cnt=%d\n", ev->num_bis, big->bis_cnt); - /* XXX: Should we destroy the group? */ } - ble_iso_big_conn_handles_init(big, ev->conn_handle, ev->num_bis); + ble_iso_big_conn_handles_init(big, ev->conn_handle, + min(big->bis_cnt, ev->num_bis)); + + big->max_pdu = le16toh(ev->max_pdu); - event.big_sync_established.desc.big_handle = ev->big_handle; event.big_sync_established.desc.transport_latency_big = get_le24(ev->transport_latency_big); event.big_sync_established.desc.nse = ev->nse; @@ -653,9 +715,11 @@ ble_iso_rx_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_establ event.big_sync_established.desc.irc = ev->irc; event.big_sync_established.desc.max_pdu = le16toh(ev->max_pdu); event.big_sync_established.desc.iso_interval = le16toh(ev->iso_interval); - event.big_sync_established.desc.num_bis = ev->num_bis; - memcpy(event.big_sync_established.desc.conn_handle, ev->conn_handle, - ev->num_bis * sizeof(uint16_t)); + event.big_sync_established.desc.num_bis = min(big->bis_cnt, ev->num_bis); + for (uint8_t i = 0; i < event.big_sync_established.desc.num_bis; i++) { + event.big_sync_established.desc.conn_handle[i] = + le16toh(ev->conn_handle[i]); + } } if (cb != NULL) { @@ -754,6 +818,9 @@ ble_iso_event_iso_rx_emit(struct ble_iso_conn *conn) if (conn->cb != NULL) { conn->cb(&event, conn->cb_arg); + } else { + os_mbuf_free_chain(conn->rx_buf); + conn->rx_buf = NULL; } } @@ -849,7 +916,7 @@ ble_iso_rx_data(struct os_mbuf *om, void *arg) if (conn == NULL) { BLE_HS_LOG_DEBUG("Unknown handle=%d\n", conn_handle); os_mbuf_free_chain(om); - return BLE_HS_EMSGSIZE; + return BLE_HS_ENOTCONN; } rc = ble_iso_conn_rx_data_load(conn, om, pb_flag, ts_flag > 0, arg); @@ -882,42 +949,52 @@ ble_iso_data_path_setup(const struct ble_iso_data_path_setup_params *param) return BLE_HS_EINVAL; } - cp = (void *)buf; - put_le16(&cp->conn_handle, param->conn_handle); - cp->data_path_dir = 0; - - if (param->data_path_dir & BLE_ISO_DATA_DIR_TX) { - /* Input (Host to Controller) */ - cp->data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_INPUT; + if (param->codec_config_len > UINT8_MAX - sizeof(*cp)) { + return BLE_HS_EINVAL; } - if (param->data_path_dir & BLE_ISO_DATA_DIR_RX) { + cp = (void *)buf; + put_le16(&cp->conn_handle, param->conn_handle); + cp->data_path_id = param->data_path_id; + + if (param->data_path_dir == BLE_ISO_DATA_DIR_TX) { + /* Input (Host to Controller) */ + cp->data_path_dir = BLE_HCI_ISO_DATA_PATH_DIR_INPUT; + } else if (param->data_path_dir == BLE_ISO_DATA_DIR_RX) { /* Output (Controller to Host) */ - cp->data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT; + cp->data_path_dir = BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT; if (cp->data_path_id == BLE_HCI_ISO_DATA_PATH_ID_HCI && param->cb == NULL) { BLE_HS_LOG_ERROR("param->cb is NULL\n"); return BLE_HS_EINVAL; } - - conn->cb = param->cb; - conn->cb_arg = param->cb_arg; + } else { + return BLE_HS_EINVAL; } - cp->data_path_id = param->data_path_id; cp->codec_id[0] = param->codec_id.format; put_le16(&cp->codec_id[1], param->codec_id.company_id); put_le16(&cp->codec_id[3], param->codec_id.vendor_specific); + if (param->ctrl_delay > 0xFFFFFFUL) { + return BLE_HS_EINVAL; + } put_le24(cp->controller_delay, param->ctrl_delay); cp->codec_config_len = param->codec_config_len; - memcpy(cp->codec_config, param->codec_config, cp->codec_config_len); + if (cp->codec_config_len > 0) { + memcpy(cp->codec_config, param->codec_config, cp->codec_config_len); + } rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SETUP_ISO_DATA_PATH), cp, sizeof(*cp) + cp->codec_config_len, &rp, sizeof(rp)); + if (rc == 0 && param->data_path_dir == BLE_ISO_DATA_DIR_RX) { + conn->cb = param->cb; + conn->cb_arg = param->cb_arg; + } + return rc; } @@ -937,23 +1014,34 @@ ble_iso_data_path_remove(const struct ble_iso_data_path_remove_params *param) put_le16(&cp.conn_handle, param->conn_handle); - if (param->data_path_dir & BLE_ISO_DATA_DIR_TX) { - /* Input (Host to Controller) */ - cp.data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_INPUT; + if (param->data_path_dir == BLE_ISO_DATA_DIR_TX) { + /* Remove Input path (Host to Controller) */ + cp.data_path_dir |= BLE_HCI_ISO_REMOVE_DATA_PATH_INPUT; } - if (param->data_path_dir & BLE_ISO_DATA_DIR_RX) { - /* Output (Controller to Host) */ - cp.data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT; + if (param->data_path_dir == BLE_ISO_DATA_DIR_RX) { + /* Remove Output path (Controller to Host) */ + cp.data_path_dir |= BLE_HCI_ISO_REMOVE_DATA_PATH_OUTPUT; + } - conn->cb = NULL; - conn->cb_arg = NULL; + if (cp.data_path_dir == 0) { + return BLE_HS_EINVAL; } rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REMOVE_ISO_DATA_PATH), &cp, sizeof(cp), &rp, sizeof(rp)); + if (rc == 0 && (param->data_path_dir == BLE_ISO_DATA_DIR_RX)) { + if (conn->rx_buf != NULL) { + os_mbuf_free_chain(conn->rx_buf); + conn->rx_buf = NULL; + memset(&conn->rx_info, 0, sizeof(conn->rx_info)); + } + conn->cb = NULL; + conn->cb_arg = NULL; + } + return rc; } @@ -963,6 +1051,7 @@ ble_iso_init(void) int rc; SLIST_INIT(&ble_iso_bigs); + SLIST_INIT(&ble_iso_conns); rc = os_mempool_init(&ble_iso_big_pool, MYNEWT_VAL(BLE_ISO_MAX_BIGS), diff --git a/nimble/host/src/ble_iso_priv.h b/nimble/host/src/ble_iso_priv.h index 9005b5067..4d280e2ad 100644 --- a/nimble/host/src/ble_iso_priv.h +++ b/nimble/host/src/ble_iso_priv.h @@ -20,11 +20,13 @@ #ifndef H_BLE_ISO_PRIV_ #define H_BLE_ISO_PRIV_ +#include "syscfg/syscfg.h" #include "nimble/hci_common.h" #ifdef __cplusplus extern "C" { #endif +#if MYNEWT_VAL(BLE_ISO) void ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_complete *ev); @@ -39,6 +41,7 @@ ble_iso_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev); int ble_iso_rx_data(struct os_mbuf *om, void *arg); +#endif /* MYNEWT_VAL(BLE_ISO) */ #ifdef __cplusplus } diff --git a/nimble/host/src/ble_l2cap.c b/nimble/host/src/ble_l2cap.c index 0d0673f2b..8e3389dad 100644 --- a/nimble/host/src/ble_l2cap.c +++ b/nimble/host/src/ble_l2cap.c @@ -258,8 +258,8 @@ ble_l2cap_enhanced_connect(uint16_t conn_handle, ble_hs_unlock(); if (rc != 0) { - /* Ownership of sdu_rx mbufs is not transferred to the stack on failure. - * The caller is responsible for freeing them. + /* Ownership of sdu_rx mbufs is transferred to the stack. + * The stack is responsible for freeing them even on failure. */ } @@ -405,7 +405,6 @@ ble_l2cap_rx(uint16_t conn_handle, uint8_t pb, struct os_mbuf *om) struct ble_l2cap_chan *chan; struct ble_l2cap_hdr hdr; struct os_mbuf *rx_frags; - uint16_t rx_len; uint16_t rx_cid; int rc; @@ -474,8 +473,8 @@ ble_l2cap_rx(uint16_t conn_handle, uint8_t pb, struct os_mbuf *om) chan = ble_hs_conn_chan_find_by_scid(conn, hdr.cid); if (chan) { - if (chan->dcid >= BLE_L2CAP_COC_CID_START && - chan->dcid <= BLE_L2CAP_COC_CID_END && hdr.len > chan->my_coc_mps) { + if (chan->scid >= BLE_L2CAP_COC_CID_START && + chan->scid <= BLE_L2CAP_COC_CID_END && hdr.len > chan->my_coc_mps) { ble_l2cap_sig_disconnect_nolock(chan); ble_l2cap_rx_free(conn); rc = BLE_HS_EBADDATA; @@ -483,7 +482,10 @@ ble_l2cap_rx(uint16_t conn_handle, uint8_t pb, struct os_mbuf *om) } if (hdr.len > ble_l2cap_get_mtu(chan)) { - ble_l2cap_sig_disconnect_nolock(chan); + if (chan->scid >= BLE_L2CAP_COC_CID_START && + chan->scid <= BLE_L2CAP_COC_CID_END) { + ble_l2cap_sig_disconnect_nolock(chan); + } ble_l2cap_rx_free(conn); rc = BLE_HS_EBADDATA; goto done; @@ -501,7 +503,6 @@ ble_l2cap_rx(uint16_t conn_handle, uint8_t pb, struct os_mbuf *om) } rx_frags = conn->rx_frags; - rx_len = conn->rx_len; rx_cid = conn->rx_cid; conn->rx_frags = NULL; @@ -523,18 +524,15 @@ ble_l2cap_rx(uint16_t conn_handle, uint8_t pb, struct os_mbuf *om) return 0; } - if (chan->dcid >= BLE_L2CAP_COC_CID_START && - chan->dcid <= BLE_L2CAP_COC_CID_END && rx_len > chan->my_coc_mps) { +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 + if (chan->scid >= BLE_L2CAP_COC_CID_START && + chan->scid <= BLE_L2CAP_COC_CID_END && + chan->coc_rx.sdus[chan->coc_rx.current_sdu_idx] == NULL) { ble_l2cap_disconnect(chan); os_mbuf_free_chain(rx_frags); - return BLE_HS_EBADDATA; - } - - if (rx_len > ble_l2cap_get_mtu(chan)) { - ble_l2cap_disconnect(chan); - os_mbuf_free_chain(rx_frags); - return BLE_HS_EBADDATA; + return BLE_HS_EINVAL; } +#endif rc = chan->rx_fn(chan, &rx_frags); os_mbuf_free_chain(rx_frags); @@ -600,14 +598,14 @@ ble_l2cap_init(void) size_t chan_mem_bytes = OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_MAX_CHANS) + MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM), sizeof (struct ble_l2cap_chan)) * sizeof(os_membuf_t); - if (ble_l2cap_chan_mem == NULL) { + if (ble_l2cap_chan_mem == NULL && chan_mem_bytes != 0) { ble_l2cap_chan_mem = (os_membuf_t *)nimble_platform_mem_calloc(1, chan_mem_bytes); - if (ble_l2cap_chan_mem == NULL) { - nimble_platform_mem_free(ble_l2cap_ctx); - ble_l2cap_ctx = NULL; - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; - } + } + if (ble_l2cap_chan_mem == NULL && chan_mem_bytes != 0) { + nimble_platform_mem_free(ble_l2cap_ctx); + ble_l2cap_ctx = NULL; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; } #endif #endif diff --git a/nimble/host/src/ble_l2cap_coc.c b/nimble/host/src/ble_l2cap_coc.c index 37266c2b8..82fe634de 100644 --- a/nimble/host/src/ble_l2cap_coc.c +++ b/nimble/host/src/ble_l2cap_coc.c @@ -66,15 +66,6 @@ static struct os_mempool ble_l2cap_coc_srv_pool; static struct ble_l2cap_coc_srv_list ble_l2cap_coc_srvs; #endif - -#ifndef min -#define min(a, b) ((a) < (b) ? (a) : (b)) -#endif - -#ifndef max -#define max(a, b) ((a) > (b) ? (a) : (b)) -#endif - static struct ble_l2cap_coc_srv * ble_l2cap_coc_srv_find(uint16_t psm); @@ -151,6 +142,7 @@ int ble_l2cap_coc_remove_server_nolock(uint16_t psm) { struct ble_l2cap_coc_srv *srv; + int rc; srv = ble_l2cap_coc_srv_find(psm); if (!srv) { @@ -159,7 +151,8 @@ ble_l2cap_coc_remove_server_nolock(uint16_t psm) STAILQ_REMOVE(&ble_l2cap_coc_srvs, srv, ble_l2cap_coc_srv, next); - os_memblock_put(&ble_l2cap_coc_srv_pool, srv); + rc = os_memblock_put(&ble_l2cap_coc_srv_pool, srv); + BLE_HS_DBG_ASSERT_EVAL(rc == 0); return 0; } @@ -215,6 +208,19 @@ ble_l2cap_coc_get_cid(uint32_t *cid_mask) return BLE_L2CAP_COC_CID_START + bit; } +static uint16_t +ble_l2cap_coc_calc_initial_credits(uint16_t mtu, uint16_t mps) +{ + uint16_t credits; + + credits = mtu / mps; + if (mtu % mps) { + credits++; + } + + return credits; +} + static void ble_l2cap_event_coc_received_data(struct ble_l2cap_chan *chan, struct os_mbuf *om) @@ -243,6 +249,11 @@ ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan, struct os_mbuf **om) int rc; sdu_idx = chan->coc_rx.current_sdu_idx; + if (rx->sdus[sdu_idx] == NULL) { + BLE_HS_LOG(ERROR, "CoC RX with no buffer armed (sdu_idx=%u)\n", sdu_idx); + ble_l2cap_disconnect(chan); + return BLE_HS_EINVAL; + } BLE_HS_DBG_ASSERT(rx->sdus[sdu_idx] != NULL); om_total = OS_MBUF_PKTLEN(*om); @@ -378,10 +389,7 @@ ble_l2cap_coc_set_new_mtu_mps(struct ble_l2cap_chan *chan, uint16_t mtu, } chan->my_coc_mps = mps; chan->coc_rx.mtu = mtu; - chan->initial_credits = mtu / chan->my_coc_mps; - if (mtu % chan->my_coc_mps) { - chan->initial_credits++; - } + chan->initial_credits = ble_l2cap_coc_calc_initial_credits(mtu, mps); } struct ble_l2cap_chan * @@ -425,14 +433,12 @@ ble_l2cap_coc_chan_alloc(struct ble_hs_conn *conn, uint16_t psm, uint16_t mtu, } /* Number of credits should allow to send full SDU with on given - * L2CAP MTU + * L2CAP MTU. Keep local credits at initial_credits so that + * recv_ready() during the accept callback does not send a duplicate + * Flow Control Credit indication before the connection response. */ - chan->coc_rx.credits = mtu / chan->my_coc_mps; - if (mtu % chan->my_coc_mps) { - chan->coc_rx.credits++; - } - - chan->initial_credits = chan->coc_rx.credits; + chan->initial_credits = ble_l2cap_coc_calc_initial_credits(mtu, chan->my_coc_mps); + chan->coc_rx.credits = chan->initial_credits; return chan; } @@ -637,7 +643,7 @@ failed: os_mbuf_free_chain(tx->sdus[0]); } tx->sdus[0] = NULL; - if (tx->data_offset > 0) { + if (tx->data_offset > 0 && rc != BLE_HS_ENOTCONN) { ble_l2cap_sig_disconnect_nolock(chan); } tx->data_offset = 0; @@ -738,18 +744,10 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx) ble_hs_unlock(); ble_l2cap_sig_le_credits(cached_conn_handle, cached_scid, credits_to_send); - ble_hs_lock(); - /* Re-validate connection and channel after relock -- they could have - * been freed while the lock was released. Credits were already updated - * before unlocking; no further update needed here. */ - conn = ble_hs_conn_find(cached_conn_handle); - if (conn != NULL) { - c = ble_hs_conn_chan_find_by_scid(conn, cached_scid); - } + } else { + ble_hs_unlock(); } - ble_hs_unlock(); - return 0; } diff --git a/nimble/host/src/ble_l2cap_coc_priv.h b/nimble/host/src/ble_l2cap_coc_priv.h index 2a394d13d..79c3a2ac2 100644 --- a/nimble/host/src/ble_l2cap_coc_priv.h +++ b/nimble/host/src/ble_l2cap_coc_priv.h @@ -62,8 +62,6 @@ struct ble_l2cap_coc_srv { #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 int ble_l2cap_coc_init(void); void ble_l2cap_coc_deinit(void); -int ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, - ble_l2cap_event_fn *cb, void *cb_arg); int ble_l2cap_coc_create_server_nolock(uint16_t psm, uint16_t mtu, ble_l2cap_event_fn *cb, void *cb_arg); int ble_l2cap_coc_remove_server_nolock(uint16_t psm); @@ -89,11 +87,6 @@ ble_l2cap_coc_init(void) { inline void ble_l2cap_coc_deinit (void){ -} -static inline int -ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, - ble_l2cap_event_fn *cb, void *cb_arg) { - return BLE_HS_ENOTSUP; } static inline int ble_l2cap_coc_create_server_nolock(uint16_t psm, uint16_t mtu, diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c index f6e50152b..6f29163c9 100644 --- a/nimble/host/src/ble_l2cap_sig.c +++ b/nimble/host/src/ble_l2cap_sig.c @@ -285,9 +285,9 @@ ble_l2cap_sig_proc_insert(struct ble_l2cap_sig_proc *proc) { ble_l2cap_sig_dbg_assert_proc_not_inserted(proc); - ble_hs_lock(); + ble_hs_lock_nested(); STAILQ_INSERT_HEAD(&ble_l2cap_sig_procs, proc, next); - ble_hs_unlock(); + ble_hs_unlock_nested(); } /** @@ -376,13 +376,6 @@ ble_l2cap_sig_proc_set_timer(struct ble_l2cap_sig_proc *proc) ble_hs_timer_resched(); } -static void -ble_l2cap_sig_proc_start(struct ble_l2cap_sig_proc *proc) -{ - ble_l2cap_sig_proc_set_timer(proc); - ble_l2cap_sig_proc_insert(proc); -} - static void ble_l2cap_sig_process_status(struct ble_l2cap_sig_proc *proc, int status) { @@ -654,6 +647,13 @@ done: #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 +static void +ble_l2cap_sig_proc_start(struct ble_l2cap_sig_proc *proc) +{ + ble_l2cap_sig_proc_set_timer(proc); + ble_l2cap_sig_proc_insert(proc); +} + static int ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err) { @@ -1101,6 +1101,11 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle, ble_hs_unlock(); rc = ble_l2cap_event_coc_accept(chans[i], le16toh(req->mtu)); + if (rc == 0 && + chans[i]->coc_rx.sdus[chans[i]->coc_rx.current_sdu_idx] == NULL) { + /* ECFC requires a valid receive buffer during accept. */ + rc = BLE_HS_ENOMEM; + } if (rc == 0) { rsp->dcids[i] = htole16(chans[i]->scid); chan_created++; @@ -1544,12 +1549,14 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m int i; if (!sdu_rx || !cb) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + /* Callers transfer ownership of sdu_rx[] mbufs on entry. When cb is + * NULL but sdu_rx is provided, free the mbufs to avoid leaking. */ if (sdu_rx) { for (i = 0; i < num; i++) { os_mbuf_free_chain(sdu_rx[i]); } } - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -1570,11 +1577,8 @@ 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) { - for (i = 0; i < num; i++) { - os_mbuf_free_chain(sdu_rx[i]); - } - ble_l2cap_sig_proc_free(proc); - return BLE_HS_EINVAL; + rc = BLE_HS_EINVAL; + goto failed; } proc->op = BLE_L2CAP_SIG_PROC_OP_CONNECT; @@ -1584,13 +1588,14 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ, proc->id, sizeof(*req) + num * sizeof(uint16_t), &txom); if (!req) { - ble_l2cap_sig_proc_free(proc); - return BLE_HS_ENOMEM; + rc = BLE_HS_ENOMEM; + goto failed; } for (i = 0; i < num; i++) { proc->connect.chan[i] = ble_l2cap_coc_chan_alloc(conn, psm, mtu, sdu_rx[i], cb, cb_arg); + sdu_rx[i] = NULL; /* ownership transferred to chan_alloc (freed on failure) */ if (!proc->connect.chan[i]) { os_mbuf_free_chain(txom); rc = BLE_HS_ENOMEM; @@ -1619,11 +1624,12 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m failed: for (i = 0; i < num; i++) { - if (proc->connect.chan[i]) { + if (i < BLE_L2CAP_MAX_COC_CONN_REQ && proc->connect.chan[i]) { proc->connect.chan[i]->cb = NULL; - proc->connect.chan[i]->coc_rx.sdus[0] = NULL; ble_l2cap_chan_free(conn, proc->connect.chan[i]); proc->connect.chan[i] = NULL; + } else { + os_mbuf_free_chain(sdu_rx[i]); } } @@ -1804,13 +1810,10 @@ ble_l2cap_sig_coc_disconnect_cb(struct ble_l2cap_sig_proc *proc, int status) chan = ble_hs_conn_chan_find_by_scid(conn, proc->disconnect.scid); if (chan) { SLIST_REMOVE(&conn->bhc_channels, chan, ble_l2cap_chan, next); + ble_l2cap_chan_free(conn, chan); } } ble_hs_unlock(); - - if (chan) { - ble_l2cap_chan_free(conn, chan); - } } static int @@ -1963,7 +1966,7 @@ ble_l2cap_sig_le_credits_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, } int -ble_l2cap_sig_le_credits(uint16_t conn_handle, uint16_t scid, uint16_t credits) +ble_l2cap_sig_le_credits_nolock(uint16_t conn_handle, uint16_t scid, uint16_t credits) { struct ble_l2cap_sig_le_credits *cmd; struct os_mbuf *txom; @@ -1978,7 +1981,19 @@ ble_l2cap_sig_le_credits(uint16_t conn_handle, uint16_t scid, uint16_t credits) cmd->scid = htole16(scid); cmd->credits = htole16(credits); - return ble_l2cap_sig_tx(conn_handle, txom); + return ble_l2cap_sig_tx_nolock(conn_handle, txom); +} + +int +ble_l2cap_sig_le_credits(uint16_t conn_handle, uint16_t scid, uint16_t credits) +{ + int rc; + + ble_hs_lock(); + rc = ble_l2cap_sig_le_credits_nolock(conn_handle, scid, credits); + ble_hs_unlock(); + + return rc; } #endif diff --git a/nimble/host/src/ble_l2cap_sig_priv.h b/nimble/host/src/ble_l2cap_sig_priv.h index ffae6058f..049b4c8cb 100644 --- a/nimble/host/src/ble_l2cap_sig_priv.h +++ b/nimble/host/src/ble_l2cap_sig_priv.h @@ -145,6 +145,10 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb, void *cb_arg) { + /* Real implementation takes ownership of sdu_rx on entry; honor the + * same contract in the stub so callers never leak the mbuf chain. + */ + os_mbuf_free_chain(sdu_rx); return BLE_HS_ENOTSUP; } @@ -159,6 +163,7 @@ ble_l2cap_sig_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t mtu, struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb, void *cb_arg) { + os_mbuf_free_chain(sdu_rx); return BLE_HS_ENOTSUP; } @@ -204,6 +209,13 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, uint8_t num, struct os_mbuf *sdu_rx[], ble_l2cap_event_fn *cb, void *cb_arg) { + uint8_t i; + + if (sdu_rx != NULL) { + for (i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } + } return BLE_HS_ENOTSUP; } @@ -220,6 +232,11 @@ ble_l2cap_sig_ecoc_connect_nolock(uint16_t conn_handle, uint16_t psm, uint16_t m uint8_t num, struct os_mbuf *sdu_rx[], ble_l2cap_event_fn *cb, void *cb_arg) { + if (sdu_rx != NULL) { + for (uint8_t i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } + } return BLE_HS_ENOTSUP; } diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index 09799bcee..3b5051295 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -56,8 +56,7 @@ #ifndef min #define min(a, b) ((a) < (b) ? (a) : (b)) -#endif /* NIMBLE_BLE_SM */ -#endif /* NIMBLE_BLE_CONNECT */ +#endif /* min */ #ifndef max #define max(a, b) ((a) > (b) ? (a) : (b)) @@ -172,7 +171,7 @@ static struct os_mempool ble_sm_proc_pool; static struct ble_sm_proc_list ble_sm_procs; #endif // BLE_STATIC_TO_DYNAMIC -static void ble_sm_pair_cfg(struct ble_sm_proc *proc); +static int ble_sm_pair_cfg(struct ble_sm_proc *proc); /***************************************************************************** @@ -869,16 +868,10 @@ ble_sm_build_authreq(void) { 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; - } + authreq = (ble_hs_cfg.sm_bonding << 0) | + (ble_hs_cfg.sm_mitm << 2) | + (ble_hs_cfg.sm_sc << 3) | + (ble_hs_cfg.sm_keypress << 4); return authreq; } @@ -1205,15 +1198,11 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res, #endif if (res->enc_cb && res->app_status != BLE_HS_ENOTCONN) { - /* Do not send this event on broken connection */ - ble_gap_pairing_complete_event(conn_handle, res->app_status); - } - /* Persist keys if bonding has successfully completed. */ - if (res->app_status == 0 && - rm && - (proc->flags & BLE_SM_PROC_F_BONDING)) { - - ble_sm_persist_keys(proc); + /* Do not send this event on broken connection. + * Use sm_err as fallback when app_status was not explicitly set. */ + int status = res->app_status != 0 ? res->app_status + : BLE_HS_SM_US_ERR(res->sm_err); + ble_gap_pairing_complete_event(conn_handle, status); } #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) @@ -1232,8 +1221,6 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res, ble_gap_passkey_event(conn_handle, &res->passkey_params); } - } - if (rm) { ble_sm_proc_free(proc); break; @@ -1435,6 +1422,7 @@ ble_sm_enc_event_rx(uint16_t conn_handle, uint8_t evt_status, int encrypted) BLE_HS_DBG_ASSERT(proc->rx_key_flags == 0); proc->state = BLE_SM_PROC_STATE_NONE; authenticated = !!(proc->flags & BLE_SM_PROC_F_AUTHENTICATED); + sc = !!(proc->flags & BLE_SM_PROC_F_SC); bonded = 1; res.restore = 1; @@ -1508,8 +1496,8 @@ ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type, key_sec.peer_addr.type = peer_addr_type; memcpy(key_sec.peer_addr.val, peer_addr, 6); - for (i = 0; ; i++) { - key_sec.idx = i; + for (i = 0; i <= UINT8_MAX; i++) { + key_sec.idx = (uint8_t)i; rc = ble_store_read_our_sec(&key_sec, value_sec); if (rc != 0) { return rc; @@ -1518,6 +1506,7 @@ ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type, return 0; } } + return BLE_HS_ENOENT; } static int @@ -1603,9 +1592,11 @@ ble_sm_ltk_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, if (value_sec->authenticated) { proc->flags |= BLE_SM_PROC_F_AUTHENTICATED; } + if (value_sec->sc) { + proc->flags |= BLE_SM_PROC_F_SC; + } } 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 { @@ -1616,11 +1607,11 @@ 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 = BLE_HS_ENOENT; + res->app_status = 0; } + } if (res->app_status == 0) { @@ -1697,6 +1688,7 @@ ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev) if (conn == NULL) { ble_sm_ltk_req_neg_reply_tx(conn_handle); res.app_status = BLE_HS_ENOTCONN; + res.execute = 0; if (proc != NULL) { proc->state = BLE_SM_PROC_STATE_NONE; } @@ -1889,7 +1881,7 @@ ble_sm_state_after_pair(struct ble_sm_proc *proc) } } -static void +static int ble_sm_pair_cfg(struct ble_sm_proc *proc) { struct ble_sm_pair_cmd *pair_req, *pair_rsp; @@ -1949,6 +1941,7 @@ ble_sm_pair_cfg(struct ble_sm_proc *proc) rc = ble_sm_io_action(proc, &ioact); BLE_HS_DBG_ASSERT_EVAL(rc == 0); + return rc; } static void @@ -2033,7 +2026,13 @@ ble_sm_pair_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, proc->state = ble_sm_state_after_pair(proc); rc = ble_sm_io_action(proc, &ioact); - BLE_HS_DBG_ASSERT(rc == 0); + if (rc != 0) { + os_mbuf_free_chain(txom); + res->sm_err = BLE_SM_ERR_INVAL; + res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); + res->enc_cb = 1; + return; + } if (ble_sm_ioact_state(ioact) == proc->state) { res->passkey_params.action = ioact; @@ -2087,9 +2086,8 @@ ble_sm_verify_auth_requirements(uint8_t cmd) /* Fail if security level forces MITM protection and remote does not * support it. SC-Only mode also requires MITM. */ - if ((ble_hs_cfg.sm_sc_only || ble_hs_cfg.sm_sec_lvl >= 3) && + if ((ble_hs_cfg.sm_sec_lvl >= 3 || ble_hs_cfg.sm_sc_only) && !(cmd & BLE_SM_PAIR_AUTHREQ_MITM)) { - return false; } @@ -2198,11 +2196,10 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, } else if (req->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) { res->sm_err = BLE_SM_ERR_INVAL; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); - } else if (!(req->authreq & BLE_SM_PAIR_AUTHREQ_BOND) && - (req->init_key_dist != 0 || req->resp_key_dist != 0)) { - /* If the Bonding_Flags is set to No Bonding, the Initiator Key - * Distribution and Responder Key Distribution fields shall be set - * to zero. + } else if (req->authreq & 0x02) { + /* Bonding_Flags field uses only the two least significant bits; + * values 10b and 11b are reserved for future use and must be + * rejected (Core Spec Vol 3 Part H Sec 3.5.1). */ res->sm_err = BLE_SM_ERR_INVAL; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); @@ -2218,6 +2215,7 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, } else if (!ble_sm_verify_auth_requirements(req->authreq)) { res->sm_err = BLE_SM_ERR_AUTHREQ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ); + res->enc_cb = 1; } else { /* The request looks good. Precalculate our pairing response and * determine some properties of the imminent link. We need this @@ -2226,11 +2224,16 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, * information in a notification to the app. */ ble_sm_pair_rsp_fill(proc); - ble_sm_pair_cfg(proc); - - proc_flags = proc->flags; - key_size = proc->key_size; - res->execute = 1; + rc = ble_sm_pair_cfg(proc); + 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_flags = proc->flags; + key_size = proc->key_size; + res->execute = 1; + } } } @@ -2282,17 +2285,16 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om, if (rsp->max_enc_key_size < BLE_SM_PAIR_KEY_SZ_MIN) { res->sm_err = BLE_SM_ERR_ENC_KEY_SZ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ); + res->enc_cb = 1; } else if (rsp->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) { res->sm_err = BLE_SM_ERR_INVAL; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); - } 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->enc_cb = 1; + } else if (rsp->authreq & 0x02) { + /* Reserved Bonding_Flags values 10b/11b must be rejected. */ 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 || 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 @@ -2300,9 +2302,11 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om, */ res->sm_err = BLE_SM_ERR_ENC_KEY_SZ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ); + res->enc_cb = 1; } else if (!ble_sm_verify_auth_requirements(rsp->authreq)) { res->sm_err = BLE_SM_ERR_AUTHREQ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ); + res->enc_cb = 1; } else { 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 || @@ -2310,10 +2314,12 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om, res->sm_err = BLE_SM_ERR_INVAL; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); + res->enc_cb = 1; } else { - ble_sm_pair_cfg(proc); - - rc = ble_sm_io_action(proc, &ioact); + rc = ble_sm_pair_cfg(proc); + if (rc == 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); @@ -2363,6 +2369,12 @@ ble_sm_sec_req_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, } cmd->authreq = ble_sm_build_authreq(); + 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) { + + cmd->authreq &= ~BLE_SM_PAIR_AUTHREQ_MITM; + } rc = ble_sm_tx(proc->conn_handle, txom); if (rc != 0) { res->app_status = rc; @@ -2781,6 +2793,7 @@ ble_sm_enc_info_rx(uint16_t conn_handle, struct os_mbuf **om, res->sm_err = BLE_SM_ERR_KEY_REJ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_KEY_REJ); + res->enc_cb = 1; } else { proc->rx_key_flags &= ~BLE_SM_KE_F_ENC_INFO; proc->peer_keys.ltk_valid = 1; @@ -2962,7 +2975,7 @@ ble_sm_sign_info_rx(uint16_t conn_handle, struct os_mbuf **om, memcpy(proc->peer_keys.csrk, cmd->sig_key, 16); proc->peer_keys.csrk_valid = 1; - proc->peer_keys.sign_counter = 0; + proc->peer_keys.sign_counter = UINT32_MAX; ble_sm_key_rxed(proc, res); } @@ -3068,6 +3081,7 @@ ble_sm_update_peer_sign_counter(uint16_t conn_handle, uint32_t sign_counter) struct ble_store_key_sec key_sec; struct ble_store_value_sec value_sec; struct ble_gap_conn_desc desc; + uint32_t orig_sign_counter; int rc; rc = ble_gap_conn_find(conn_handle, &desc); @@ -3085,13 +3099,33 @@ ble_sm_update_peer_sign_counter(uint16_t conn_handle, uint32_t sign_counter) if (value_sec.csrk_present != 1) { return BLE_HS_ENOENT; } - - rc = ble_store_delete_peer_sec(&key_sec); - if (rc != 0) { - return rc; + if (sign_counter == UINT32_MAX) { + return BLE_HS_EINVAL; } + +#if MYNEWT_VAL(BLE_HS_PVCY) + if (value_sec.irk_present == 1) { + ble_hs_pvcy_remove_entry(value_sec.peer_addr.type, + value_sec.peer_addr.val); + } +#endif + + orig_sign_counter = value_sec.sign_counter; value_sec.sign_counter = sign_counter; rc = ble_store_write_peer_sec(&value_sec); + if (rc != 0) { + value_sec.sign_counter = orig_sign_counter; + int recovery_rc = ble_store_write_peer_sec(&value_sec); + (void)recovery_rc; +#if MYNEWT_VAL(BLE_HS_PVCY) + if (recovery_rc != 0 && value_sec.irk_present == 1) { + ble_hs_pvcy_add_entry(value_sec.peer_addr.val, + value_sec.peer_addr.type, + value_sec.irk); + } +#endif + } + return rc; } @@ -3101,6 +3135,7 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle, uint32_t new_counter) struct ble_store_key_sec key_sec; struct ble_store_value_sec value_sec; struct ble_gap_conn_desc desc; + uint32_t orig_sign_counter; int rc; rc = ble_gap_conn_find(conn_handle, &desc); @@ -3118,14 +3153,8 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle, uint32_t new_counter) if (value_sec.csrk_present != 1) { return BLE_HS_ENOENT; } - if (value_sec.sign_counter == (uint32_t)0xffffffff) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; - } - - rc = ble_store_delete_peer_sec(&key_sec); - if (rc != 0) { - return rc; + if (new_counter == UINT32_MAX) { + return BLE_HS_EINVAL; } #if MYNEWT_VAL(BLE_HS_PVCY) @@ -3147,9 +3176,20 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle, uint32_t new_counter) #endif #endif + orig_sign_counter = value_sec.sign_counter; value_sec.sign_counter = new_counter; rc = ble_store_write_peer_sec(&value_sec); if (rc != 0) { + value_sec.sign_counter = orig_sign_counter; + int recovery_rc = ble_store_write_peer_sec(&value_sec); + (void)recovery_rc; +#if MYNEWT_VAL(BLE_HS_PVCY) + if (recovery_rc != 0 && value_sec.irk_present == 1) { + ble_hs_pvcy_add_entry(value_sec.peer_addr.val, + value_sec.peer_addr.type, + value_sec.irk); + } +#endif return rc; } @@ -3771,16 +3811,13 @@ ble_sm_get_static_passkey_config(uint32_t *passkey, bool *enabled) } #endif -#if MYNEWT_VAL(BLE_SM_CSIS_SIRK) +#if MYNEWT_VAL(BLE_SM_CSIS_SIRK) && NIMBLE_BLE_SM int ble_sm_csis_decrypt_sirk(const uint8_t *ltk, const uint8_t *enc_sirk, uint8_t *out) { 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; @@ -3807,14 +3844,15 @@ ble_sm_csis_resolve_rsi(const uint8_t *rsi, const uint8_t *sirk, rc = ble_store_read_peer_sec(&key_sec, &value_sec); if (rc != 0) { - return rc; + goto done; } else if (!value_sec.ltk_present) { - return BLE_HS_ENOENT; + rc = BLE_HS_ENOENT; + goto done; } rc = ble_sm_csis_decrypt_sirk(value_sec.ltk, sirk, plaintext_sirk); if (rc != 0) { - return rc; + goto done; } } else { memcpy(plaintext_sirk, sirk, 16); @@ -3822,14 +3860,22 @@ ble_sm_csis_resolve_rsi(const uint8_t *rsi, const uint8_t *sirk, rc = ble_sm_alg_csis_sih(plaintext_sirk, prand, local_hash); if (rc != 0) { - return rc; + goto done; } if (memcmp(local_hash, hash, 3)) { - return BLE_HS_EAUTHEN; + rc = BLE_HS_EAUTHEN; + goto done; } - return 0; + rc = 0; + +done: + memset(plaintext_sirk, 0, sizeof(plaintext_sirk)); + memset(&value_sec, 0, sizeof(value_sec)); + __asm__ volatile("" : : "r"(plaintext_sirk), "r"(&value_sec) : "memory"); + + return rc; } int @@ -3875,4 +3921,5 @@ ble_sm_csis_generate_rsi(const uint8_t *sirk, uint8_t *out) return 0; } -#endif +#endif /* BLE_SM_CSIS_SIRK */ +#endif /* NIMBLE_BLE_CONNECT */ diff --git a/nimble/host/src/ble_sm_alg.c b/nimble/host/src/ble_sm_alg.c index 25b3ac549..70d35bc15 100644 --- a/nimble/host/src/ble_sm_alg.c +++ b/nimble/host/src/ble_sm_alg.c @@ -32,21 +32,71 @@ #include "nimble/ble.h" #include "ble_hs_priv.h" - -#include +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT +#include "psa/crypto.h" +#else +#include "mbedtls/aes.h" +#include "mbedtls/cipher.h" +#include "mbedtls/entropy.h" +#include "mbedtls/cmac.h" +#include "mbedtls/ecdh.h" +#include "mbedtls/ecp.h" +#include "mbedtls/esp_mbedtls_random.h" +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT +#else +#include "tinycrypt/aes.h" +#include "tinycrypt/constants.h" +#include "tinycrypt/utils.h" #if MYNEWT_VAL(BLE_SM_SC) -#include -#include -#include +#include "tinycrypt/cmac_mode.h" +#include "tinycrypt/ecc_dh.h" #if MYNEWT_VAL(TRNG) #include "trng/trng.h" #endif #endif +#endif + +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +#if !CONFIG_MBEDTLS_VER_4_X_SUPPORT +#if MYNEWT_VAL(BLE_SM_SC) +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#include "esp_nimble_mem.h" +static mbedtls_ecp_keypair *keypair_ptr __attribute__((unused)) = NULL; +#define keypair (*keypair_ptr) +#else +static mbedtls_ecp_keypair keypair; +#endif /* MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) */ +#endif +#endif /* !CONFIG_MBEDTLS_VER_4_X_SUPPORT */ +#else #if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(TRNG) static struct trng_dev *g_trng; #endif +#endif + +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT +#define BLE_PUB_KEY_LEN 65 +static const char * const TAG = "ble_sm_alg"; +#else +#define BLE_PUB_KEY_LEN 64 +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT + +/** + * Keep forward-compatibility with Mbed TLS 3.x. + * + * Direct access to fields of structures declared in public headers is no longer + * supported. In Mbed TLS 3, the layout of structures is not considered part of + * the stable API, and minor versions (3.1, 3.2, etc.) may add, remove, rename, + * reorder or change the type of structure fields. + */ +#if (MBEDTLS_VERSION_NUMBER < 0x03000000) +#ifndef MBEDTLS_PRIVATE +#define MBEDTLS_PRIVATE(member) member +#endif +#endif static void ble_sm_alg_secure_zero(void *buf, size_t len) @@ -72,24 +122,90 @@ int ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data) { - int ret = 0; - struct mbedtls_aes_context ctx; uint8_t tmp[16]; int rc = 0; swap_buf(tmp, key, 16); - mbedtls_aes_init(&ctx); - mbedtls_aes_setkey_enc(&ctx, tmp, 128); - swap_buf(tmp, plaintext, 16); - if (mbedtls_aes_crypt_ecb(&ctx, MBEDTLS_AES_ENCRYPT, tmp, enc_data)) { - ret = BLE_HS_EUNKNOWN; - } else { - swap_in_place(enc_data, 16); +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT + psa_status_t status; + psa_key_attributes_t key_attributes = PSA_KEY_ATTRIBUTES_INIT; + psa_key_id_t key_id = 0; + psa_set_key_usage_flags(&key_attributes, PSA_KEY_USAGE_ENCRYPT); + psa_set_key_algorithm(&key_attributes, PSA_ALG_ECB_NO_PADDING); + psa_set_key_type(&key_attributes, PSA_KEY_TYPE_AES); + psa_set_key_bits(&key_attributes, 128); + status = psa_import_key(&key_attributes, tmp, 16, &key_id); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to import AES key: %d", status); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + rc = BLE_HS_EUNKNOWN; + goto done; } - mbedtls_aes_free(&ctx); + psa_reset_key_attributes(&key_attributes); - return ret; + swap_buf(tmp, plaintext, 16); + + size_t output_len = 0; + status = psa_cipher_encrypt(key_id, PSA_ALG_ECB_NO_PADDING, tmp, + 16, enc_data, 16, &output_len); + if (status != PSA_SUCCESS || output_len != 16) { + ESP_LOGE(TAG, "Encryption failed: %d", status); + psa_destroy_key(key_id); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + rc = BLE_HS_EUNKNOWN; + goto done; + } + psa_destroy_key(key_id); +#else + mbedtls_aes_context s = {0}; + + mbedtls_aes_init(&s); + if (mbedtls_aes_setkey_enc(&s, tmp, 128) != 0) { + mbedtls_aes_free(&s); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + rc = BLE_HS_EUNKNOWN; + goto done; + } + + swap_buf(tmp, plaintext, 16); + + if (mbedtls_aes_crypt_ecb(&s, MBEDTLS_AES_ENCRYPT, tmp, enc_data) != 0) { + mbedtls_aes_free(&s); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + rc = BLE_HS_EUNKNOWN; + goto done; + } + + mbedtls_aes_free(&s); +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT +#else + struct tc_aes_key_sched_struct s; + + if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + ble_sm_alg_secure_zero(&s, sizeof(s)); + rc = BLE_HS_EUNKNOWN; + goto done; + } + + swap_buf(tmp, plaintext, 16); + + if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + ble_sm_alg_secure_zero(&s, sizeof(s)); + rc = BLE_HS_EUNKNOWN; + goto done; + } + ble_sm_alg_secure_zero(&s, sizeof(s)); +#endif + + swap_in_place(enc_data, 16); + +done: + ble_sm_alg_secure_zero(tmp, sizeof(tmp)); + return rc; } int @@ -206,14 +322,120 @@ done: * @param len Length of the message in octets. * @param out Output; message authentication code. */ -static int + +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +int ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len, uint8_t *out) { - if (mbedtls_cipher_cmac(mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_AES_128_ECB), - key, 128, in, len, out)) { +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT + psa_status_t status; + psa_key_attributes_t key_attributes = PSA_KEY_ATTRIBUTES_INIT; + psa_key_id_t key_id = 0; + psa_algorithm_t alg = PSA_ALG_CMAC; + psa_set_key_usage_flags(&key_attributes, PSA_KEY_USAGE_SIGN_MESSAGE); + psa_set_key_algorithm(&key_attributes, PSA_ALG_CMAC); + psa_set_key_type(&key_attributes, PSA_KEY_TYPE_AES); + psa_set_key_bits(&key_attributes, 128); + status = psa_import_key(&key_attributes, key, 16, &key_id); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to import key: %d", status); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; } + psa_reset_key_attributes(&key_attributes); + + psa_mac_operation_t operation = PSA_MAC_OPERATION_INIT; + status = psa_mac_sign_setup(&operation, key_id, alg); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to setup MAC sign operation: %d", status); + psa_destroy_key(key_id); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; + } + + size_t output_len = 0; + status = psa_mac_update(&operation, in, len); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to update MAC operation: %d", status); + psa_mac_abort(&operation); + psa_destroy_key(key_id); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; + } + + status = psa_mac_sign_finish(&operation, out, 16, &output_len); + if (status != PSA_SUCCESS || output_len != 16) { + ESP_LOGE(TAG, "Failed to finish MAC sign operation: %d", status); + psa_mac_abort(&operation); + psa_destroy_key(key_id); + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; + } + + psa_destroy_key(key_id); + return 0; +#else + int rc = BLE_HS_EUNKNOWN; + mbedtls_cipher_context_t ctx = {0}; + const mbedtls_cipher_info_t *cipher_info; + + mbedtls_cipher_init(&ctx); + + cipher_info = mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_AES_128_ECB); + + if (cipher_info == NULL) { + goto exit; + } + + if (mbedtls_cipher_setup(&ctx, cipher_info) != 0) { + goto exit; + } + + rc = mbedtls_cipher_cmac_starts(&ctx, key, 128); + if (rc != 0) { + goto exit; + } + + rc = mbedtls_cipher_cmac_update(&ctx, in, len); + if (rc != 0) { + goto exit; + } + + rc = mbedtls_cipher_cmac_finish(&ctx, out); + +exit: + mbedtls_cipher_free(&ctx); + if (rc != 0) { + rc = BLE_HS_EUNKNOWN; + } + return rc; +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT +} + +#else +int +ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len, + uint8_t *out) +{ + struct tc_aes_key_sched_struct sched; + struct tc_cmac_struct state; + int rc = BLE_HS_EUNKNOWN; + + if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + goto done; + } + + if (tc_cmac_update(&state, in, len) == TC_CRYPTO_FAIL) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + goto done; + } + + if (tc_cmac_final(out, &state) == TC_CRYPTO_FAIL) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + goto done; + } rc = 0; @@ -222,6 +444,7 @@ done: ble_sm_alg_secure_zero(&sched, sizeof(sched)); return rc; } +#endif #if MYNEWT_VAL(BLE_SM_SC) @@ -480,18 +703,24 @@ ble_sm_alg_csis_k1(const uint8_t *n, size_t n_len, const uint8_t *salt, /* T = AES-CMAC_SALT (N) */ rc = ble_sm_alg_aes_cmac(salt_be, n_be, n_len, t); if (rc != 0) { - return rc; + goto done; } /* AES-CMAC_T (P) */ rc = ble_sm_alg_aes_cmac(t, p, p_len, out); if (rc != 0) { - return rc; + goto done; } swap_in_place(out, 16); - return 0; +done: + memset(t, 0, sizeof(t)); + memset(salt_be, 0, sizeof(salt_be)); + memset(n_be, 0, sizeof(n_be)); + __asm__ volatile("" : : "r"(t), "r"(salt_be), "r"(n_be) : "memory"); + + return rc; } int @@ -583,97 +812,170 @@ ble_sm_alg_csis_sih(const uint8_t *k, const uint8_t *r, uint8_t *out) rc = ble_sm_alg_encrypt(k, r1, r1); if (rc != 0) { + ble_sm_alg_secure_zero(r1, sizeof(r1)); return rc; } memcpy(out, r1, 3); + ble_sm_alg_secure_zero(r1, sizeof(r1)); return 0; } -int -ble_sm_alg_rng(void *arg, unsigned char *buf, size_t size) -{ - (void)arg; - -#if MYNEWT_VAL(SELFTEST) - while (size--) { - buf[size] = 9; - } - - return 0; -#elif MYNEWT_VAL(TRNG) - size_t num; - - if (!g_trng) { - g_trng = (struct trng_dev *)os_dev_open("trng", OS_WAIT_FOREVER, NULL); - assert(g_trng); - } - - while (size) { - num = trng_read(g_trng, buf, size); - buf += num; - size -= num; - } - - return 0; -#else - return ble_hs_hci_util_rand(buf, size); -#endif -} - int ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y, const uint8_t *our_priv_key, uint8_t *out_dhkey) { - int err; - mbedtls_mpi priv; - mbedtls_mpi shared; - mbedtls_ecp_group group; - mbedtls_ecp_point point; - uint8_t pub_key[65]; + uint8_t dh[32]; + uint8_t pk[BLE_PUB_KEY_LEN]; + uint8_t priv[32]; + int rc = BLE_HS_EUNKNOWN; - mbedtls_mpi_init(&priv); - mbedtls_mpi_init(&shared); - mbedtls_ecp_point_init(&point); - mbedtls_ecp_group_init(&group); + swap_buf(priv, our_priv_key, 32); - pub_key[0] = 0x04; /* Uncompressed point identifier */ - swap_buf(&pub_key[1], peer_pub_key_x, 32); - swap_buf(&pub_key[33], peer_pub_key_y, 32); +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT + // PSA/mbedTLS expects 65 bytes: 0x04 prefix + X (32 bytes) + Y (32 bytes) + pk[0] = 0x04; // Uncompressed format for public key + swap_buf(&pk[1], peer_pub_key_x, 32); + swap_buf(&pk[33], peer_pub_key_y, 32); - err = mbedtls_ecp_group_load(&group, MBEDTLS_ECP_DP_SECP256R1); - - if (err == 0) { - err = mbedtls_ecp_point_read_binary(&group, &point, pub_key, sizeof(pub_key)); + psa_key_id_t key_id = 0; + psa_status_t status; + psa_key_attributes_t key_attributes = PSA_KEY_ATTRIBUTES_INIT; + psa_set_key_type(&key_attributes, PSA_KEY_TYPE_ECC_KEY_PAIR(PSA_ECC_FAMILY_SECP_R1)); + psa_set_key_bits(&key_attributes, 256); + psa_set_key_algorithm(&key_attributes, PSA_ALG_ECDH); + psa_set_key_usage_flags(&key_attributes, PSA_KEY_USAGE_DERIVE); + status = psa_import_key(&key_attributes, priv, 32, &key_id); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to import key: %d", status); + goto exit; + } + psa_reset_key_attributes(&key_attributes); + size_t output_len = 0; + status = psa_raw_key_agreement(PSA_ALG_ECDH, key_id, pk, BLE_PUB_KEY_LEN, dh, sizeof(dh), &output_len); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to perform raw key agreement: %d", status); + goto exit; } - if (err == 0) { - err = mbedtls_ecp_check_pubkey(&group, &point); + if (output_len != 32) { + ESP_LOGE(TAG, "Unexpected output length: %zu", output_len); + goto exit; + } + rc = 0; + +exit: + if (key_id != 0) { + psa_destroy_key(key_id); + } +#else + swap_buf(pk, peer_pub_key_x, 32); + swap_buf(&pk[32], peer_pub_key_y, 32); + +#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!keypair_ptr) { + keypair_ptr = nimble_platform_mem_calloc(1, sizeof(mbedtls_ecp_keypair)); + if (!keypair_ptr) { + rc = BLE_HS_ENOMEM; + goto exit_cleanup; + } + } +#endif + + struct mbedtls_ecp_point pt = {0}, Q = {0}; + mbedtls_mpi z = {0}, d = {0}; + + uint8_t pub[65] = {0}; + /* Hardcoded first byte of pub key for MBEDTLS_ECP_PF_UNCOMPRESSED */ + pub[0] = 0x04; + memcpy(&pub[1], pk, 64); + + /* Initialize the required structures here */ + mbedtls_ecp_point_init(&pt); + mbedtls_ecp_point_init(&Q); + mbedtls_mpi_init(&d); + mbedtls_mpi_init(&z); + + /* Below 3 steps are to validate public key on curve secp256r1 */ + if (mbedtls_ecp_group_load(&keypair.MBEDTLS_PRIVATE(grp), MBEDTLS_ECP_DP_SECP256R1) != 0) { + goto exit; } - if (err == 0) { - err = mbedtls_mpi_read_binary_le(&priv, our_priv_key, 32); + if (mbedtls_ecp_point_read_binary(&keypair.MBEDTLS_PRIVATE(grp), &pt, pub, 65) != 0) { + goto exit; } - if (err == 0) { - err = mbedtls_ecdh_compute_shared(&group, &shared, &point, &priv, - ble_sm_alg_rng, NULL); + if (mbedtls_ecp_check_pubkey(&keypair.MBEDTLS_PRIVATE(grp), &pt) != 0) { + goto exit; } - if (err == 0) { - err = mbedtls_mpi_write_binary_le(&shared, out_dhkey, 32); + /* Prepare point Q from pub key */ + if (mbedtls_ecp_point_read_binary(&keypair.MBEDTLS_PRIVATE(grp), &Q, pub, 65) != 0) { + goto exit; } - mbedtls_ecp_group_free(&group); - mbedtls_ecp_point_free(&point); - mbedtls_mpi_free(&priv); - mbedtls_mpi_free(&shared); - if (err) { - return BLE_HS_EUNKNOWN; + if (mbedtls_mpi_read_binary(&d, priv, 32) != 0) { + goto exit; } - return 0; + rc = mbedtls_ecdh_compute_shared(&keypair.MBEDTLS_PRIVATE(grp), &z, &Q, &d, + mbedtls_esp_random, NULL); + if (rc != 0) { + goto exit; + } + + rc = mbedtls_mpi_write_binary(&z, dh, 32); + if (rc != 0) { + goto exit; + } + +exit: + mbedtls_ecp_point_free(&pt); + mbedtls_mpi_free(&z); + mbedtls_mpi_free(&d); + mbedtls_ecp_point_free(&Q); +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT + if (rc != 0) { +#if !CONFIG_MBEDTLS_VER_4_X_SUPPORT && MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (keypair_ptr) { + mbedtls_ecp_keypair_free(&keypair); + nimble_platform_mem_free(keypair_ptr); + keypair_ptr = NULL; + } +#endif + rc = BLE_HS_EUNKNOWN; + goto exit_cleanup; + } +#else + // TinyCrypt/uECC expects 64 bytes: X (32 bytes) + Y (32 bytes), no prefix + swap_buf(pk, peer_pub_key_x, 32); + swap_buf(&pk[32], peer_pub_key_y, 32); + + if (uECC_valid_public_key(pk, uECC_secp256r1()) < 0) { + rc = BLE_HS_EUNKNOWN; + goto exit_cleanup; + } + + rc = uECC_shared_secret(pk, priv, dh, uECC_secp256r1()); + if (rc == TC_CRYPTO_FAIL) { + rc = BLE_HS_EUNKNOWN; + goto exit_cleanup; + } +#endif + + swap_buf(out_dhkey, dh, 32); + rc = 0; + +exit_cleanup: + /* Zero sensitive key material from stack */ + memset(dh, 0, sizeof(dh)); + memset(priv, 0, sizeof(priv)); + /* Use a memory barrier to prevent compiler from optimizing out the memsets */ + __asm__ __volatile__("" : : "r"(dh), "r"(priv) : "memory"); + + return rc; } /* based on Core Specification 4.2 Vol 3. Part H 2.3.5.6.1 */ @@ -696,6 +998,121 @@ static const uint8_t ble_sm_alg_dbg_pub_key[64] = { }; #endif +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +static int +mbedtls_gen_keypair(uint8_t *public_key, uint8_t *private_key) +{ + int rc = BLE_HS_EUNKNOWN; +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT + psa_status_t status = PSA_SUCCESS; + psa_key_id_t key_id = 0; + psa_key_attributes_t key_attributes = PSA_KEY_ATTRIBUTES_INIT; + psa_algorithm_t alg = PSA_ALG_ECDH; + psa_key_type_t key_type = PSA_KEY_TYPE_ECC_KEY_PAIR(PSA_ECC_FAMILY_SECP_R1); + psa_key_usage_t key_usage = PSA_KEY_USAGE_DERIVE | PSA_KEY_USAGE_EXPORT; + + psa_set_key_type(&key_attributes, key_type); + psa_set_key_bits(&key_attributes, 256); + psa_set_key_algorithm(&key_attributes, alg); + psa_set_key_usage_flags(&key_attributes, key_usage); + status = psa_generate_key(&key_attributes, &key_id); + if (status != PSA_SUCCESS) { + ESP_LOGE(TAG, "Failed to generate key: %d", status); + goto exit; + } + psa_reset_key_attributes(&key_attributes); + + size_t olen = 0; + status = psa_export_public_key(key_id, public_key, BLE_PUB_KEY_LEN, &olen); + if (status != PSA_SUCCESS || olen != BLE_PUB_KEY_LEN) { + ESP_LOGE(TAG, "Failed to export public key: %d", status); + goto exit; + } + + status = psa_export_key(key_id, private_key, 32, &olen); + if (status != PSA_SUCCESS || olen != 32) { + ESP_LOGE(TAG, "Failed to export private key: %d", status); + goto exit; + } + + rc = 0; + +exit: + if (key_id != 0) { + psa_destroy_key(key_id); + } + if (rc != 0) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; + } +#else + +#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!keypair_ptr) { + keypair_ptr = nimble_platform_mem_calloc(1, sizeof(mbedtls_ecp_keypair)); + if (!keypair_ptr) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; + } + } +#endif + + /* Free the previously allocate keypair */ + mbedtls_ecp_keypair_free(&keypair); + + mbedtls_ecp_keypair_init(&keypair); + + if ((rc = mbedtls_ecp_gen_key(MBEDTLS_ECP_DP_SECP256R1, &keypair, + mbedtls_esp_random, NULL)) != 0) { + goto exit; + } + + if (( rc = mbedtls_mpi_write_binary(&keypair.MBEDTLS_PRIVATE(d), private_key, 32)) != 0) { + goto exit; + } + + size_t olen = 0; + uint8_t pub[65] = {0}; + + if ((rc = mbedtls_ecp_point_write_binary(&keypair.MBEDTLS_PRIVATE(grp), &keypair.MBEDTLS_PRIVATE(Q), MBEDTLS_ECP_PF_UNCOMPRESSED, + &olen, pub, 65)) != 0) { + goto exit; + } + + memcpy(public_key, &pub[1], 64); + +exit: + if (rc != 0) { + mbedtls_ecp_keypair_free(&keypair); + +#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (keypair_ptr) { + nimble_platform_mem_free(keypair_ptr); + keypair_ptr = NULL; + } +#endif + rc = BLE_HS_EUNKNOWN; + } +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT + return rc; +} + +void mbedtls_free_keypair(void) +{ +#ifndef CONFIG_MBEDTLS_VER_4_X_SUPPORT +#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (keypair_ptr) { + mbedtls_ecp_keypair_free(&keypair); + nimble_platform_mem_free(keypair_ptr); + keypair_ptr = NULL; + } +#else + mbedtls_ecp_keypair_free(&keypair); +#endif +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT +} +#endif + /** * pub: 64 bytes * priv: 32 bytes @@ -709,66 +1126,85 @@ ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv) swap_buf(&pub[32], &ble_sm_alg_dbg_pub_key[32], 32); swap_buf(priv, ble_sm_alg_dbg_priv_key, 32); #else - uint8_t pub_buf[65]; - size_t pub_len = 0; - mbedtls_ecp_keypair keypair; - mbedtls_ecp_group group; - mbedtls_ecp_point point; - mbedtls_mpi d; - int err; - - mbedtls_ecp_keypair_init(&keypair); - mbedtls_ecp_group_init(&group); - mbedtls_ecp_point_init(&point); - mbedtls_mpi_init(&d); + uint8_t pk[BLE_PUB_KEY_LEN]; do { - err = mbedtls_ecp_gen_key(MBEDTLS_ECP_DP_SECP256R1, &keypair, - ble_sm_alg_rng, NULL); - if (err) { - break; - } - err = mbedtls_ecp_export(&keypair, &group, &d, &point); - if (err) { - break; +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) + if (mbedtls_gen_keypair(pk, priv) != 0) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; } +#if CONFIG_MBEDTLS_VER_4_X_SUPPORT + // PSA/mbedTLS: pk[0]=0x04, pk[1..32]=X, pk[33..64]=Y + swap_buf(pub, &pk[1], 32); // Extract X (skip 0x04 prefix) + swap_buf(&pub[32], &pk[33], 32); // Extract Y +#else + swap_buf(pub, pk, 32); + swap_buf(&pub[32], &pk[32], 32); +#endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT +#else + if (uECC_make_key(pk, priv, uECC_secp256r1()) != TC_CRYPTO_SUCCESS) { + BLE_HS_LOG(ERROR, "%s:%d rc=%d\n", __func__, __LINE__, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; + } + // TinyCrypt/uECC: pk[0..31]=X, pk[32..63]=Y (no prefix) + swap_buf(pub, pk, 32); // Extract X (from start) + swap_buf(&pub[32], &pk[32], 32); // Extract Y +#endif - err = mbedtls_mpi_write_binary_le(&d, priv, 32); - if (err) { - break; - } /* Make sure generated key isn't debug key. */ } while (memcmp(priv, ble_sm_alg_dbg_priv_key, 32) == 0); - - if (err == 0) { - err = mbedtls_ecp_point_write_binary(&group, &point, MBEDTLS_ECP_PF_UNCOMPRESSED, - &pub_len, pub_buf, sizeof(pub_buf)); - } - - if (err == 0) { - if (pub_len == sizeof(pub_buf)) { - swap_buf(pub, &pub_buf[1], 32); - swap_buf(pub + 32, &pub_buf[33], 32); - } else { - err = BLE_HS_EUNKNOWN; - } - } - - mbedtls_ecp_keypair_free(&keypair); - mbedtls_ecp_group_free(&group); - mbedtls_ecp_point_free(&point); - mbedtls_mpi_free(&d); - - if (err) { - return BLE_HS_EUNKNOWN; - } - + swap_in_place(priv, 32); #endif return 0; } +#if MYNEWT_VAL(SELFTEST) +/* Unit tests rely on custom RNG function not being set */ +#define ble_sm_alg_rand NULL +#elif !(MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS)) +/* used by uECC to get random data */ +static int +ble_sm_alg_rand(uint8_t *dst, unsigned int size) +{ +#if MYNEWT_VAL(TRNG) + size_t num; + + if (!g_trng) { + g_trng = (struct trng_dev *)os_dev_open("trng", OS_WAIT_FOREVER, NULL); + assert(g_trng); + } + + while (size) { + num = trng_read(g_trng, dst, size); + dst += num; + size -= num; + } +#else + if (!ble_hs_is_enabled()) { + return 0; + } + + if (ble_hs_hci_util_rand(dst, size)) { + return 0; + } +#endif + + return 1; +} +#endif + +void +ble_sm_alg_ecc_init(void) +{ +#if (!MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS)) + uECC_set_rng(ble_sm_alg_rand); +#endif + return; +} + #endif #endif #endif diff --git a/nimble/host/src/ble_sm_priv.h b/nimble/host/src/ble_sm_priv.h index 94fc1a3db..e69b2704a 100644 --- a/nimble/host/src/ble_sm_priv.h +++ b/nimble/host/src/ble_sm_priv.h @@ -333,11 +333,14 @@ int ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y, const uint8_t *our_priv_key, uint8_t *out_dhkey); int ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv); +void ble_sm_alg_ecc_init(void); int ble_sm_csis_generate_rsi(const uint8_t *sirk, uint8_t *out); int ble_sm_csis_encrypt_sirk(const uint8_t *ltk, const uint8_t *plaintext_sirk, uint8_t *out); int ble_sm_csis_decrypt_sirk(const uint8_t *ltk, const uint8_t *enc_sirk, uint8_t *out); +int ble_sm_csis_resolve_rsi(const uint8_t *rsi, const uint8_t *sirk, + const ble_addr_t *ltk_peer_addr); void ble_sm_enc_change_rx(const struct ble_hci_ev_enrypt_chg *ev); void ble_sm_enc_key_refresh_rx(const struct ble_hci_ev_enc_key_refresh *ev); @@ -388,10 +391,13 @@ void ble_sm_sc_deinit(void); #define ble_sm_sc_random_exec(proc, res) #define ble_sm_sc_random_rx(proc, res) #define ble_sm_sc_public_key_exec(proc, res, arg) -#define ble_sm_sc_public_key_rx(conn_handle, op, om, res) +#define ble_sm_sc_public_key_rx(conn_handle, om, res) #define ble_sm_sc_dhkey_check_exec(proc, res, arg) -#define ble_sm_sc_dhkey_check_rx(conn_handle, op, om, res) +#define ble_sm_sc_dhkey_check_rx(conn_handle, om, res) #define ble_sm_sc_init() +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#define ble_sm_sc_deinit() +#endif #endif diff --git a/nimble/host/src/ble_sm_sc.c b/nimble/host/src/ble_sm_sc.c index ee247c5f3..c88eeef4a 100644 --- a/nimble/host/src/ble_sm_sc.c +++ b/nimble/host/src/ble_sm_sc.c @@ -172,13 +172,6 @@ ble_sm_sc_io_action(struct ble_sm_proc *proc, uint8_t *action) } else if (pair_req->oob_data_flag == BLE_SM_PAIR_OOB_YES || pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES) { *action = BLE_SM_IOACT_OOB_SC; -#if MYNEWT_VAL(STATIC_PASSKEY) - } else if (ble_hs_cfg.sm_static_passkey) { - *action = BLE_SM_IOACT_STATIC; - proc->pair_alg = BLE_SM_PAIR_ALG_PASSKEY; - proc->flags |= BLE_SM_PROC_F_AUTHENTICATED; - return 0; -#endif } else if (!(pair_req->authreq & BLE_SM_PAIR_AUTHREQ_MITM) && !(pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_MITM)) { @@ -193,6 +186,13 @@ ble_sm_sc_io_action(struct ble_sm_proc *proc, uint8_t *action) *action = ble_sm_sc_resp_ioa[pair_rsp->io_cap][pair_req->io_cap]; } +#if MYNEWT_VAL(STATIC_PASSKEY) + if ((*action == BLE_SM_IOACT_NONE || *action == BLE_SM_IOACT_INPUT || + *action == BLE_SM_IOACT_DISP) && ble_hs_cfg.sm_static_passkey) { + *action = BLE_SM_IOACT_STATIC; + } +#endif + switch (*action) { case BLE_SM_IOACT_NONE: proc->pair_alg = BLE_SM_PAIR_ALG_JW; @@ -1065,6 +1065,7 @@ ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data) void ble_sm_sc_init(void) { + ble_sm_alg_ecc_init(); ble_sm_sc_keys_generated = 0; #if MYNEWT_VAL(BLE_HS_DEBUG) ble_sm_dbg_sc_keys_set = 0; diff --git a/nimble/host/src/ble_store.c b/nimble/host/src/ble_store.c index 4241e7b91..fcd37432f 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -617,6 +617,7 @@ int ble_store_read_local_irk(const struct ble_store_key_local_irk *key, struct ble_store_value_local_irk *out_value) { +#if NIMBLE_BLE_CONNECT union ble_store_value local_store_value = {0}; union ble_store_key local_store_key = {0}; int rc; @@ -632,6 +633,9 @@ ble_store_read_local_irk(const struct ble_store_key_local_irk *key, } return rc; +#else + return BLE_HS_ENOTSUP; +#endif } #endif diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index b5540622f..5a065ef12 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -106,6 +106,9 @@ int ble_restore_our_sec_nvs(void) qsort(temp_our_secs, temp_count, sizeof(struct ble_store_value_sec), ble_store_config_compare_bond_count); + /* Copy sorted order back to global array before assigning sequential bond counts */ + memcpy(ble_store_config_our_secs, temp_our_secs, temp_count * sizeof(struct ble_store_value_sec)); + ble_store_config_our_bond_count = 0; for (int i = 0; i < temp_count; i++) { @@ -128,6 +131,9 @@ int ble_restore_peer_sec_nvs(void) qsort(temp_peer_secs, temp_count, sizeof(struct ble_store_value_sec), ble_store_config_compare_bond_count); + /* Copy sorted order back to global array before assigning sequential bond counts */ + memcpy(ble_store_config_peer_secs, temp_peer_secs, temp_count * sizeof(struct ble_store_value_sec)); + ble_store_config_peer_bond_count = 0; for (int i = 0; i < temp_count; i++) { @@ -1202,16 +1208,9 @@ ble_store_config_delete(int obj_type, const union ble_store_key *key) } } -/* Track initialization state for idempotency */ -static bool g_ble_store_config_initialized = false; - void ble_store_config_init(void) { - if (g_ble_store_config_initialized) { - return; - } - #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_store_config_vars == NULL) { ble_store_config_vars = nimble_platform_mem_calloc(1, sizeof(ble_store_config_vars_t)); @@ -1240,8 +1239,6 @@ ble_store_config_init(void) ble_store_config_num_rpa_recs = 0; ble_store_config_num_local_irks=0; ble_store_config_conf_init(); - - g_ble_store_config_initialized = true; } void @@ -1257,7 +1254,4 @@ ble_store_config_deinit(void) ble_hs_cfg.store_read_cb = NULL; ble_hs_cfg.store_write_cb = NULL; ble_hs_cfg.store_delete_cb = NULL; - - /* Reset initialization flag to allow re-initialization */ - g_ble_store_config_initialized = false; } 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 c5fb67fe8..741216c2e 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/nimble/host/store/config/src/ble_store_config_conf.c @@ -162,6 +162,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) MYNEWT_VAL(BLE_STORE_MAX_BONDS), &ble_store_config_num_peer_secs); return rc; +#if MYNEWT_VAL(BLE_STORE_MAX_CCCDS) } else if (strcmp(argv[0], "cccd") == 0) { rc = ble_store_config_deserialize_arr( val, @@ -170,6 +171,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) MYNEWT_VAL(BLE_STORE_MAX_CCCDS), &ble_store_config_num_cccds); return rc; +#endif } #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) else if (strcmp(argv[0], "csfc") == 0) { @@ -217,9 +219,11 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), /* Get the largest possible encoding size */ buf_sz = BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ; +#if MYNEWT_VAL(BLE_STORE_MAX_CCCDS) if (BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ > buf_sz) { buf_sz = BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ; } +#endif if (BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ > buf_sz) { buf_sz = BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ; } @@ -253,12 +257,14 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), buf_sz); func("ble_hs/peer_sec", buf); +#if MYNEWT_VAL(BLE_STORE_MAX_CCCDS) ble_store_config_serialize_arr(ble_store_config_cccds, sizeof *ble_store_config_cccds, ble_store_config_num_cccds, buf, buf_sz); func("ble_hs/cccd", buf); +#endif #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) ble_store_config_serialize_arr(ble_store_config_csfcs, sizeof *ble_store_config_csfcs, diff --git a/nimble/host/store/config/src/ble_store_config_priv.h b/nimble/host/store/config/src/ble_store_config_priv.h index 941bdb937..1b1515eb7 100644 --- a/nimble/host/store/config/src/ble_store_config_priv.h +++ b/nimble/host/store/config/src/ble_store_config_priv.h @@ -168,9 +168,6 @@ int ble_store_config_persist_eads(void); int ble_store_config_persist_rpa_recs(void); int ble_store_config_persist_local_irk(void); void ble_store_config_conf_init(void); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -void ble_store_config_deinit(void); -#endif #else static inline int ble_store_config_persist_our_secs(void) { return 0; } @@ -183,11 +180,11 @@ static inline int ble_store_config_persist_eads(void) { return 0; } static inline int ble_store_config_persist_rpa_recs(void) { return 0; } static inline int ble_store_config_persist_local_irk(void) { return 0; } static inline void ble_store_config_conf_init(void) { } -//static inline void ble_store_config_deinit(void) { } #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) static inline int ble_store_persist_peer_records(void) { return 0; } #endif #endif /* MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) */ +void ble_store_config_deinit(void); #ifdef __cplusplus } diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index 33081b556..c28e3bf55 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -942,15 +942,28 @@ int ble_store_config_persist_local_irk(void) ESP_LOGD(TAG, "Deleting Local IRK, nvs idx = %d", nvs_idx); return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_LOCAL_IRK, nvs_idx); } else { - /* Equal counts - this could be an update, sync all entries */ + /* Equal counts - this is a resync/resequence. The previous + * implementation deleted all entries first and then re-wrote them, + * which could leave NVS with fewer entries than RAM on a partial + * write failure. Instead, overwrite slots 1..N in place (newer + * values win), then delete any extras only after every write + * succeeded. + */ + char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; ESP_LOGD(TAG, "Syncing Local IRK values to NVS..."); - /* Delete existing entries first to avoid duplication/corruption */ - for (int i = 1; i <= nvs_count; i++) { - ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_LOCAL_IRK, i); - } for (int i = 0; i < ble_store_config_num_local_irks; i++) { val.local_irk = ble_store_config_local_irks[i]; - int rc = ble_store_nvs_write(BLE_STORE_OBJ_TYPE_LOCAL_IRK, &val); + get_nvs_key_string(BLE_STORE_OBJ_TYPE_LOCAL_IRK, i + 1, key_string); + int rc = ble_nvs_write_key_value(key_string, &val.local_irk, + sizeof(struct ble_store_value_local_irk)); + if (rc != 0) { + return rc; + } + } + /* No extras expected since counts are equal, but be defensive. */ + for (int i = ble_store_config_num_local_irks + 1; + i <= get_nvs_max_obj_value(BLE_STORE_OBJ_TYPE_LOCAL_IRK); i++) { + int rc = ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_LOCAL_IRK, i); if (rc != 0) { return rc; } @@ -985,6 +998,25 @@ int ble_store_config_persist_rpa_recs(void) } ESP_LOGD(TAG, "Deleting RPA_REC, nvs idx = %d", nvs_idx); return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_ADDR, nvs_idx); + } else { + char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; + ESP_LOGD(TAG, "Syncing RPA_RECS values to NVS..."); + for (int i = 0; i < ble_store_config_num_rpa_recs; i++) { + val.rpa_rec = ble_store_config_rpa_recs[i]; + get_nvs_key_string(BLE_STORE_OBJ_TYPE_PEER_ADDR, i + 1, key_string); + int rc = ble_nvs_write_key_value(key_string, &val.rpa_rec, + sizeof(struct ble_store_value_rpa_rec)); + if (rc != 0) { + return rc; + } + } + for (int i = ble_store_config_num_rpa_recs + 1; + i <= get_nvs_max_obj_value(BLE_STORE_OBJ_TYPE_PEER_ADDR); i++) { + int rc = ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_ADDR, i); + if (rc != 0) { + return rc; + } + } } return 0; @@ -1019,6 +1051,25 @@ int ble_store_config_persist_peer_secs(void) } ESP_LOGD(TAG, "Deleting peer sec, nvs idx = %d", nvs_idx); return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_SEC, nvs_idx); + } else { + char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; + ESP_LOGD(TAG, "Syncing peer sec values to NVS..."); + for (int i = 0; i < ble_store_config_num_peer_secs; i++) { + val.sec = ble_store_config_peer_secs[i]; + get_nvs_key_string(BLE_STORE_OBJ_TYPE_PEER_SEC, i + 1, key_string); + int rc = ble_nvs_write_key_value(key_string, &val.sec, + sizeof(struct ble_store_value_sec)); + if (rc != 0) { + return rc; + } + } + for (int i = ble_store_config_num_peer_secs + 1; + i <= get_nvs_max_obj_value(BLE_STORE_OBJ_TYPE_PEER_SEC); i++) { + int rc = ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_SEC, i); + if (rc != 0) { + return rc; + } + } } return 0; } @@ -1052,6 +1103,25 @@ int ble_store_config_persist_our_secs(void) } ESP_LOGD(TAG, "Deleting our sec, nvs idx = %d", nvs_idx); return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_OUR_SEC, nvs_idx); + } else { + char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; + ESP_LOGD(TAG, "Syncing our sec values to NVS..."); + for (int i = 0; i < ble_store_config_num_our_secs; i++) { + val.sec = ble_store_config_our_secs[i]; + get_nvs_key_string(BLE_STORE_OBJ_TYPE_OUR_SEC, i + 1, key_string); + int rc = ble_nvs_write_key_value(key_string, &val.sec, + sizeof(struct ble_store_value_sec)); + if (rc != 0) { + return rc; + } + } + for (int i = ble_store_config_num_our_secs + 1; + i <= get_nvs_max_obj_value(BLE_STORE_OBJ_TYPE_OUR_SEC); i++) { + int rc = ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_OUR_SEC, i); + if (rc != 0) { + return rc; + } + } } return 0; } diff --git a/nimble/host/test/src/ble_gatts_reg_test.c b/nimble/host/test/src/ble_gatts_reg_test.c index ddbfe8f24..7789c213f 100644 --- a/nimble/host/test/src/ble_gatts_reg_test.c +++ b/nimble/host/test/src/ble_gatts_reg_test.c @@ -265,8 +265,9 @@ ble_gatts_reg_test_misc_verify_entry(uint8_t op, const ble_uuid_t *uuid) int i; for (i = 0; i < ble_gatts_reg_test_num_entries; i++) { - entry = ble_gatts_reg_test_entries + i; - if (entry->op == op && ble_uuid_cmp(&entry->uuid.u, uuid) == 0) { + if (ble_gatts_reg_test_entries[i].op == op && + ble_uuid_cmp(&ble_gatts_reg_test_entries[i].uuid.u, uuid) == 0) { + entry = ble_gatts_reg_test_entries + i; break; } } diff --git a/nimble/host/test/src/ble_hs_test_util.c b/nimble/host/test/src/ble_hs_test_util.c index a31a4ef3e..d00c18ec1 100644 --- a/nimble/host/test/src/ble_hs_test_util.c +++ b/nimble/host/test/src/ble_hs_test_util.c @@ -1924,22 +1924,23 @@ ble_hs_test_util_store_read(int obj_type, const union ble_store_key *key, ble_sm_test_store_key.cccd = key->cccd; break; default: - return BLE_HS_ENOTSUP; + break; } rc = ble_store_config_read(obj_type, key, value); - - switch (ble_sm_test_store_obj_type) { - case BLE_STORE_OBJ_TYPE_PEER_SEC: - case BLE_STORE_OBJ_TYPE_OUR_SEC: - ble_sm_test_store_value.sec = value->sec; - break; - case BLE_STORE_OBJ_TYPE_CCCD: - ble_sm_test_store_value.cccd = value->cccd; - break; - default: - rc = BLE_HS_ENOTSUP; - break; + if (rc == 0) { + switch (ble_sm_test_store_obj_type) { + case BLE_STORE_OBJ_TYPE_PEER_SEC: + case BLE_STORE_OBJ_TYPE_OUR_SEC: + ble_sm_test_store_value.sec = value->sec; + break; + case BLE_STORE_OBJ_TYPE_CCCD: + ble_sm_test_store_value.cccd = value->cccd; + break; + default: + rc = BLE_HS_ENOTSUP; + break; + } } return rc; @@ -1963,7 +1964,7 @@ ble_hs_test_util_store_write(int obj_type, const union ble_store_value *value) ble_sm_test_store_value.cccd = value->cccd; break; default: - rc = BLE_HS_ENOTSUP; + /* For other types, preserve the return code from ble_store_config_write */ break; } diff --git a/nimble/host/test/src/ble_sm_test.c b/nimble/host/test/src/ble_sm_test.c index dcdbaf01e..c27c38aa4 100644 --- a/nimble/host/test/src/ble_sm_test.c +++ b/nimble/host/test/src/ble_sm_test.c @@ -430,7 +430,7 @@ TEST_CASE_SELF(ble_sm_test_case_peer_sec_req_inval) TEST_CASE_SELF(ble_sm_test_case_peer_sec_req_reject) { - struct ble_sm_sec_req sec_req; + struct ble_sm_sec_req sec_req = {0}; int rc; struct ble_sm_pair_cmd pair_rsp = {0x04, 0, 0x0D, 10, 0x02, 0x02}; diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h index 70e4ba7d9..0370f966c 100644 --- a/nimble/include/nimble/ble.h +++ b/nimble/include/nimble/ble.h @@ -358,7 +358,6 @@ enum ble_error_codes * (Corresponds to Resolved Private Address). */ #define BLE_ADDR_RANDOM_ID (0x03) -#define BLE_ADDR_ANONYMOUS (0xFF) /** * Bluetooth Device Address Type: Anonymous diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 0535c7a55..be1aabf62 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -2913,7 +2913,7 @@ struct hci_data_hdr #define BLE_HCI_PB_FIRST_FLUSH 2 #define BLE_HCI_PB_FULL 3 -#define BLE_HCI_ISO_CONN_HANDLE_MASK (0x07ff) +#define BLE_HCI_ISO_CONN_HANDLE_MASK (0x0fff) #define BLE_HCI_ISO_PB_FLAG_MASK (0x3000) #define BLE_HCI_ISO_TS_FLAG_MASK (0x4000) #define BLE_HCI_ISO_LENGTH_MASK (0x3fff) @@ -2938,7 +2938,7 @@ struct hci_data_hdr #define BLE_HCI_ISO_PKT_STATUS_VALID 0x00 #define BLE_HCI_ISO_PKT_STATUS_INVALID 0x01 -#define BLE_HCI_ISO_PKT_STATUS_LOST 0x10 +#define BLE_HCI_ISO_PKT_STATUS_LOST 0x02 #define BLE_HCI_ISO_BIG_HANDLE_MIN 0x00 #define BLE_HCI_ISO_BIG_HANDLE_MAX 0xEF @@ -2948,6 +2948,8 @@ struct hci_data_hdr #define BLE_HCI_ISO_DATA_PATH_DIR_INPUT 0x00 #define BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT 0x01 +#define BLE_HCI_ISO_REMOVE_DATA_PATH_INPUT 0x01 +#define BLE_HCI_ISO_REMOVE_DATA_PATH_OUTPUT 0x02 #define BLE_HCI_ISO_DATA_PATH_ID_HCI 0x00 @@ -2961,7 +2963,8 @@ struct ble_hci_iso { uint8_t data[0]; }; -#define BLE_HCI_ISO_HDR_SDU_LENGTH_MASK (0x07ff) +#define BLE_HCI_ISO_HDR_SDU_LENGTH_MASK (0x0fff) + struct ble_hci_iso_data { uint16_t packet_seq_num; diff --git a/nimble/transport/cdc/src/cdc_hci.c b/nimble/transport/cdc/src/cdc_hci.c index 08c8f984a..2fcaf3c20 100644 --- a/nimble/transport/cdc/src/cdc_hci.c +++ b/nimble/transport/cdc/src/cdc_hci.c @@ -302,6 +302,10 @@ cdc_hci_acl_packet_write(struct usb_in_packet *packet, size_t offset) if (new_offset == 0) { /* Write H4 ACL type */ new_offset += tud_cdc_n_write_char(cdc_num, HCI_H4_ACL); + if (new_offset == 0) { + /* USB FIFO full; cannot write H4 type byte yet */ + return (int)offset; + } } for (;;) { diff --git a/nimble/transport/common/hci_ipc/src/hci_ipc.c b/nimble/transport/common/hci_ipc/src/hci_ipc.c index b27f6a537..11b4d1e8f 100644 --- a/nimble/transport/common/hci_ipc/src/hci_ipc.c +++ b/nimble/transport/common/hci_ipc/src/hci_ipc.c @@ -72,6 +72,9 @@ hci_ipc_alloc(struct hci_ipc_sm *sm) sm->om = ble_transport_alloc_iso_from_ll(); #endif break; + default: + assert(0); + break; } sm->rem_len = sm->hdr.length; @@ -122,9 +125,11 @@ hci_ipc_frame(struct hci_ipc_sm *sm) break; default: assert(0); - break; + return; } + assert(rc == 0); + sm->hdr.type = 0; sm->hdr.length = 0; sm->hdr_len = 0; @@ -247,7 +252,7 @@ hci_ipc_init(volatile struct hci_ipc_shm *shm, struct hci_ipc_sm *sm) ble_npl_time_delay(1); /* 1 tick delay every 1000 iterations */ } } - if (timeout == 0) { + if (shm->n2a_num_evt_disc == 0) { abort(); /* Timeout waiting for host initialization */ } /* Memory barrier to ensure we see updated values from host */ diff --git a/nimble/transport/esp_ipc_btdm/src/hci_esp_ipc.c b/nimble/transport/esp_ipc_btdm/src/hci_esp_ipc.c index d414525d7..07b9c4013 100644 --- a/nimble/transport/esp_ipc_btdm/src/hci_esp_ipc.c +++ b/nimble/transport/esp_ipc_btdm/src/hci_esp_ipc.c @@ -23,7 +23,11 @@ ble_transport_dummy_host_recv_cb(hci_driver_data_type_t type, uint8_t *data, uin } else if (type == HCI_DRIVER_TYPE_ISO) { free(data); } else { +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + ble_transport_free(BLE_HCI_EVT, data); +#else ble_transport_free(data); +#endif } return 0; } @@ -51,7 +55,11 @@ ble_transport_host_recv_cb(hci_driver_data_type_t type, uint8_t *data, uint16_t else if (type == HCI_DRIVER_TYPE_EVT) { rc = ble_transport_to_hs_evt(data); } else { +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + ble_transport_free(BLE_HCI_EVT, data); +#else ble_transport_free(data); +#endif } return rc; } @@ -104,8 +112,16 @@ ble_transport_alloc_cmd(void) return pkt->data; } +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) +void +ble_transport_free(uint8_t type, void *buf) +{ + r_ble_hci_trans_buf_free(buf); +} +#else void ble_transport_free(void *buf) { r_ble_hci_trans_buf_free(buf); } +#endif diff --git a/nimble/transport/nrf5340/src/nrf5340_ble_hci.c b/nimble/transport/nrf5340/src/nrf5340_ble_hci.c index f73be0552..0988673e3 100644 --- a/nimble/transport/nrf5340/src/nrf5340_ble_hci.c +++ b/nimble/transport/nrf5340/src/nrf5340_ble_hci.c @@ -122,8 +122,6 @@ nrf5340_ble_hci_trans_rx(int channel, void *user_data) static void nrf5340_ble_hci_init(void) { - os_sr_t sr; - SYSINIT_ASSERT_ACTIVE(); #if MYNEWT_VAL(BLE_TRANSPORT_RX_TASK) @@ -131,9 +129,7 @@ nrf5340_ble_hci_init(void) #endif ipc_nrf5340_recv(IPC_RX_CHANNEL, nrf5340_ble_hci_trans_rx, NULL); - OS_ENTER_CRITICAL(sr); nrf5340_ble_hci_trans_rx(IPC_RX_CHANNEL, NULL); - OS_EXIT_CRITICAL(sr); } #if MYNEWT_VAL(BLE_CONTROLLER) diff --git a/nimble/transport/socket/src/ble_hci_socket.c b/nimble/transport/socket/src/ble_hci_socket.c index ab0b48206..4594c0d4b 100644 --- a/nimble/transport/socket/src/ble_hci_socket.c +++ b/nimble/transport/socket/src/ble_hci_socket.c @@ -193,6 +193,7 @@ ble_hci_sock_acl_tx(struct os_mbuf *om) int i; struct os_mbuf *m; uint8_t ch; + int pktlen; memset(&msg, 0, sizeof(msg)); memset(iov, 0, sizeof(iov)); @@ -204,6 +205,10 @@ ble_hci_sock_acl_tx(struct os_mbuf *om) iov[0].iov_base = &ch; i = 1; for (m = om; m; m = SLIST_NEXT(m, om_next)) { + if (i >= 8) { + os_mbuf_free_chain(om); + return BLE_ERR_MEM_CAPACITY; + } iov[i].iov_base = m->om_data; iov[i].iov_len = m->om_len; i++; @@ -213,9 +218,10 @@ ble_hci_sock_acl_tx(struct os_mbuf *om) STATS_INC(hci_sock_stats, omsg); STATS_INC(hci_sock_stats, oacl); STATS_INCN(hci_sock_stats, obytes, OS_MBUF_PKTLEN(om) + 1); + pktlen = OS_MBUF_PKTLEN(om); i = sendmsg(ble_hci_sock_state.sock, &msg, 0); os_mbuf_free_chain(om); - if (i != OS_MBUF_PKTLEN(om) + 1) { + if (i != pktlen + 1) { if (i < 0) { dprintf(1, "sendmsg() failed : %d\n", errno); } else { @@ -289,6 +295,7 @@ ble_hci_sock_iso_tx(struct os_mbuf *om) int i; struct os_mbuf *m; uint8_t ch; + int pktlen; memset(&msg, 0, sizeof(msg)); memset(iov, 0, sizeof(iov)); @@ -300,6 +307,10 @@ ble_hci_sock_iso_tx(struct os_mbuf *om) iov[0].iov_base = &ch; i = 1; for (m = om; m; m = SLIST_NEXT(m, om_next)) { + if (i >= 8) { + os_mbuf_free_chain(om); + return BLE_ERR_MEM_CAPACITY; + } iov[i].iov_base = m->om_data; iov[i].iov_len = m->om_len; i++; @@ -310,8 +321,9 @@ ble_hci_sock_iso_tx(struct os_mbuf *om) STATS_INC(hci_sock_stats, oiso); STATS_INCN(hci_sock_stats, obytes, OS_MBUF_PKTLEN(om) + 1); i = sendmsg(ble_hci_sock_state.sock, &msg, 0); + pktlen = OS_MBUF_PKTLEN(om); os_mbuf_free_chain(om); - if (i != OS_MBUF_PKTLEN(om) + 1) { + if (i != pktlen + 1) { if (i < 0) { dprintf(1, "sendmsg() failed : %d\n", errno); } else { @@ -606,8 +618,8 @@ ble_hci_sock_rx_msg(void) if (bhss->rx_off < BLE_HCI_DATA_HDR_SZ) { return -1; } - len = 1 + BLE_HCI_DATA_HDR_SZ + (bhss->rx_data[4] << 8) + - bhss->rx_data[3]; + len = 1 + BLE_HCI_DATA_HDR_SZ + + (((bhss->rx_data[4] << 8) | bhss->rx_data[3]) & BLE_HCI_ISO_LENGTH_MASK); if (bhss->rx_off < len) { return -1; } diff --git a/nimble/transport/src/monitor.c b/nimble/transport/src/monitor.c index 15f92f673..63d3c14c3 100644 --- a/nimble/transport/src/monitor.c +++ b/nimble/transport/src/monitor.c @@ -68,6 +68,25 @@ static struct { #endif #if MYNEWT_VAL(BLE_MONITOR_UART) +static int +monitor_uart_available_space(void) +{ + int head; + int tail; + int space; + + head = tx_ringbuf_head; + tail = tx_ringbuf_tail; + + if (head >= tail) { + space = sizeof(tx_ringbuf) - (head - tail) - 1; + } else { + space = tail - head - 1; + } + + return space; +} + static inline int inc_and_wrap(int i, int max) { @@ -345,14 +364,19 @@ ble_monitor_deinit(void) int ble_monitor_send(uint16_t opcode, const void *data, size_t len) { - os_sr_t sr; + ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER); - OS_ENTER_CRITICAL(sr); +#if MYNEWT_VAL(BLE_MONITOR_UART) + if (monitor_uart_available_space() < (int)(len + 32)) { + ble_npl_mutex_release(&lock); + return BLE_HS_ENOMEM; + } +#endif monitor_write_header(opcode, len); monitor_write(data, len); - OS_EXIT_CRITICAL(sr); + ble_npl_mutex_release(&lock); return 0; } @@ -405,11 +429,6 @@ ble_monitor_log(int level, const char *fmt, ...) static const char id[] = "nimble"; struct ble_monitor_user_logging ulog; va_list va; - int len; - - va_start(va, fmt); - len = vsnprintf(NULL, 0, fmt, va); - va_end(va); switch (level) { case LOG_LEVEL_ERROR: @@ -512,9 +531,7 @@ ble_transport_to_ll_cmd(void *buf) struct ble_hci_cmd *cmd; #if !(SOC_ESP_NIMBLE_CONTROLLER) && CONFIG_BT_CONTROLLER_ENABLED - if (cmd_buf[0] == 0x01) { - cmd_buf++; - } + cmd_buf++; #endif cmd = (void *)cmd_buf; diff --git a/nimble/transport/src/rx_task.c b/nimble/transport/src/rx_task.c index a729a86f6..cef3dc0a5 100644 --- a/nimble/transport/src/rx_task.c +++ b/nimble/transport/src/rx_task.c @@ -25,6 +25,7 @@ #include #include #include +#include #if !defined(MYNEWT) || MYNEWT_VAL(BLE_TRANSPORT_RX_TASK_STACK_SIZE) @@ -80,7 +81,11 @@ ble_transport_rx_register(ble_transport_rx_func_t *func, void *arg) void ble_transport_rx(void) { +#ifdef MYNEWT ble_npl_eventq_put(&rx_eventq, &rx_event); +#else + ble_npl_eventq_put(nimble_port_get_dflt_eventq(), &rx_event); +#endif } #endif diff --git a/nimble/transport/src/transport.c b/nimble/transport/src/transport.c index d1adbe471..78c8fe0df 100644 --- a/nimble/transport/src/transport.c +++ b/nimble/transport/src/transport.c @@ -448,6 +448,10 @@ void ble_buf_free(void) { os_msys_buf_free(); +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + return; +#endif + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_trans_ctx == NULL) { return; diff --git a/nimble/transport/uart_ll/src/hci_uart.c b/nimble/transport/uart_ll/src/hci_uart.c index 8cd2bbed0..5d9aefde0 100644 --- a/nimble/transport/uart_ll/src/hci_uart.c +++ b/nimble/transport/uart_ll/src/hci_uart.c @@ -253,7 +253,7 @@ ble_transport_ll_deinit(void) { memset(&hci_uart_h4sm, 0, sizeof(hci_uart_h4sm)); - hal_uart_close(MYNEWT_VAL(BLE_TRANSPORT_UART_PORT)); + hal_uart_close(MYNEWT_VAL(BLE_TRANSPORT_UART_LL_PORT)); hal_uart_deinit_cbs(); } diff --git a/porting/examples/linux_blemesh_shell/include/console/console_port.h b/porting/examples/linux_blemesh_shell/include/console/console_port.h index e5a05fc2c..9a443089c 100644 --- a/porting/examples/linux_blemesh_shell/include/console/console_port.h +++ b/porting/examples/linux_blemesh_shell/include/console/console_port.h @@ -17,18 +17,22 @@ * under the License. */ -#ifndef __LOG_H__ -#define __LOG_H__ +#ifndef __CONSOLE_PORT_H__ +#define __CONSOLE_PORT_H__ #ifdef __cplusplus extern "C" { #endif -struct log { -}; +#include "os/os_eventq.h" +#include "nimble/nimble_npl.h" + +void console_set_queues(struct os_eventq *avail, struct os_eventq *lines); +void console_set_event_cb(ble_npl_event_fn *cb); +int console_init(void); #ifdef __cplusplus } #endif -#endif /* __LOG_H__ */ +#endif /* __CONSOLE_PORT_H__ */ diff --git a/porting/examples/linux_blemesh_shell/shell/shell_port.c b/porting/examples/linux_blemesh_shell/shell/shell_port.c index bd9af6d3b..16705cd69 100644 --- a/porting/examples/linux_blemesh_shell/shell/shell_port.c +++ b/porting/examples/linux_blemesh_shell/shell/shell_port.c @@ -217,6 +217,7 @@ shell_nlip_input_register(shell_nlip_input_func_t nf, void *arg) int shell_nlip_output(struct os_mbuf *m) { + os_mbuf_free_chain(m); return 0; } #endif diff --git a/porting/examples/linux_blemesh_shell/src/ble.c b/porting/examples/linux_blemesh_shell/src/ble.c index c380ac88d..2efa4d5f8 100644 --- a/porting/examples/linux_blemesh_shell/src/ble.c +++ b/porting/examples/linux_blemesh_shell/src/ble.c @@ -352,7 +352,12 @@ vnd_model_recv(struct bt_mesh_model *model, buf->om_len); bt_mesh_model_msg_init(msg, BT_MESH_MODEL_OP_3(0x01, CID_VENDOR)); - os_mbuf_append(msg, buf->om_data, buf->om_len); + rc = os_mbuf_append(msg, buf->om_data, buf->om_len); + if (rc) { + console_printf("#vendor-model-recv: append failed\n"); + os_mbuf_free_chain(msg); + return rc; + } rc = bt_mesh_model_send(model, ctx, msg, NULL, NULL); if (rc) { diff --git a/porting/examples/linux_blemesh_shell/src/main.c b/porting/examples/linux_blemesh_shell/src/main.c index 16905d242..71806000b 100644 --- a/porting/examples/linux_blemesh_shell/src/main.c +++ b/porting/examples/linux_blemesh_shell/src/main.c @@ -102,5 +102,5 @@ main(int argc, char *argv[]) NULL, TASK_DEFAULT_PRIORITY, BLE_NPL_TIME_FOREVER, TASK_DEFAULT_STACK, TASK_DEFAULT_STACK_SIZE); - pthread_exit(&ret); + pthread_exit(NULL); } diff --git a/porting/nimble/include/log/log.h b/porting/nimble/include/log/log.h new file mode 100644 index 000000000..69556af6b --- /dev/null +++ b/porting/nimble/include/log/log.h @@ -0,0 +1,36 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#ifndef __LOG_H__ +#define __LOG_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef ESP_PLATFORM +struct log { +}; +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __LOG_H__ */ diff --git a/porting/nimble/include/modlog/modlog.h b/porting/nimble/include/modlog/modlog.h index 8bf2f750b..c3996928d 100644 --- a/porting/nimble/include/modlog/modlog.h +++ b/porting/nimble/include/modlog/modlog.h @@ -26,7 +26,6 @@ #ifndef ESP_PLATFORM #include "log/log.h" #endif -#include "log_common/log_common.h" #if CONFIG_BLE_HOST_NIMBLE_COMPRESSED_LOG_ENABLE #include "nimble_log_index.h" diff --git a/porting/nimble/src/mem.c b/porting/nimble/src/mem.c index 0547f60e6..1c12b2f60 100644 --- a/porting/nimble/src/mem.c +++ b/porting/nimble/src/mem.c @@ -239,7 +239,8 @@ mem_init_mbuf_pool(void *mem, struct os_mempool *mempool, if (num_blocks < 0 || num_blocks > UINT16_MAX) { return OS_EINVAL; } - if (block_size < 0 || block_size > UINT16_MAX) { + if (block_size <= 0 || block_size > UINT16_MAX || + block_size < (int)sizeof(struct os_mbuf)) { return OS_EINVAL; } diff --git a/porting/nimble/src/nimble_port.c b/porting/nimble/src/nimble_port.c index 8d63d55ef..a526b050f 100644 --- a/porting/nimble/src/nimble_port.c +++ b/porting/nimble/src/nimble_port.c @@ -56,6 +56,7 @@ #define NIMBLE_PORT_LOG_TAG "BLE_INIT" extern void os_msys_init(void); +extern void os_msys_buf_free(void); #if CONFIG_BT_NIMBLE_ENABLED extern void ble_hs_deinit(void); @@ -97,9 +98,10 @@ static struct ble_hs_stop_listener stop_listener; #endif extern void os_msys_init(void); +extern void os_msys_buf_free(void); extern void os_mempool_module_init(void); #if MYNEWT_VAL(MP_RUNTIME_ALLOC) -extern void os_mempool_deinit(bool is_controller); +extern void os_mempool_deinit(void); #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -129,6 +131,15 @@ ble_npl_reset_deinit_flag(void) { ble_npl_deiniting = false; } + +static void +ble_npl_free_ctx(void) +{ + if (ble_npl_ctx != NULL) { + nimble_platform_mem_free(ble_npl_ctx); + ble_npl_ctx = NULL; + } +} #endif /** @@ -170,17 +181,29 @@ esp_err_t esp_nimble_init(void) /* Initialize the function pointers for OS porting */ rc = npl_freertos_funcs_init(); if (rc != 0) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_npl_free_ctx(); +#endif return ESP_ERR_NO_MEM; } if (npl_freertos_mempool_init() != 0) { ESP_LOGE(NIMBLE_PORT_LOG_TAG, "mempool init failed\n"); + npl_freertos_funcs_deinit(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_npl_free_ctx(); +#endif return ESP_FAIL; } #if CONFIG_BT_CONTROLLER_ENABLED if(esp_nimble_hci_init() != ESP_OK) { ESP_LOGE(NIMBLE_PORT_LOG_TAG, "hci inits failed\n"); + npl_freertos_mempool_deinit(); + npl_freertos_funcs_deinit(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_npl_free_ctx(); +#endif return ESP_FAIL; } /* Initialize default event queue before ble_hs_init() posts start event. */ @@ -192,6 +215,11 @@ esp_err_t esp_nimble_init(void) ret = ble_buf_alloc(); if (ret != ESP_OK) { ble_buf_free(); + npl_freertos_mempool_deinit(); + npl_freertos_funcs_deinit(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_npl_free_ctx(); +#endif return ESP_FAIL; } ble_transport_init(); @@ -216,7 +244,26 @@ esp_err_t esp_nimble_init(void) #endif #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) - ble_adv_list_init(); + if (ble_adv_list_init() != 0) { +#if !SOC_ESP_NIMBLE_CONTROLLER || !CONFIG_BT_CONTROLLER_ENABLED +#if CONFIG_BT_CONTROLLER_ENABLED + esp_nimble_hci_deinit(); + os_msys_buf_free(); +#else + ble_transport_deinit(); + ble_buf_free(); +#endif + ble_npl_eventq_deinit(&g_eventq_dflt); + npl_freertos_mempool_deinit(); + npl_freertos_funcs_deinit(); +#else + ble_npl_eventq_deinit(&g_eventq_dflt); +#endif +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_npl_free_ctx(); +#endif + return ESP_FAIL; + } #endif /* Initialize the host */ @@ -251,6 +298,14 @@ esp_err_t esp_nimble_deinit(void) ble_transport_ll_deinit(); +#if (BT_HCI_LOG_INCLUDED == TRUE) + bt_hci_log_deinit(); +#endif + +#if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) + ble_adv_list_deinit(); +#endif + #if !SOC_ESP_NIMBLE_CONTROLLER || !CONFIG_BT_CONTROLLER_ENABLED #if CONFIG_BT_CONTROLLER_ENABLED if(esp_nimble_hci_deinit() != ESP_OK) { @@ -261,9 +316,6 @@ esp_err_t esp_nimble_deinit(void) return ESP_FAIL; } #else -#if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) - ble_adv_list_deinit(); -#endif ble_transport_deinit(); ble_buf_free(); #endif @@ -283,10 +335,7 @@ esp_err_t esp_nimble_deinit(void) #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_npl_ctx) { - nimble_platform_mem_free(ble_npl_ctx); - ble_npl_ctx = NULL; - } + ble_npl_free_ctx(); /* Keep ble_npl_deiniting = true until nimble_port_deinit completes. * This prevents controller deinit from re-allocating ble_npl_ctx when it * calls nimble_port_get_dflt_eventq(). The flag will be reset at the START @@ -304,7 +353,7 @@ esp_err_t esp_nimble_deinit(void) bt_osal_freertos_funcs_deinit(); #if MYNEWT_VAL(MP_RUNTIME_ALLOC) - os_mempool_deinit(0); + os_mempool_deinit(); #endif return ret; @@ -384,7 +433,7 @@ nimble_port_deinit(void) if(esp_nimble_deinit() != ESP_OK) { ESP_LOGE(NIMBLE_PORT_LOG_TAG, "nimble host deinit failed\n"); - ret = ESP_FAIL; + return ESP_FAIL; } #if CONFIG_BT_CONTROLLER_ENABLED @@ -492,7 +541,7 @@ nimble_port_get_dflt_eventq(void) /* Normal case: try to allocate context */ if (ble_npl_ensure_ctx()) { - return NULL; + return &g_eventq_shutdown_fallback; } #endif diff --git a/porting/nimble/src/os_mbuf.c b/porting/nimble/src/os_mbuf.c index 91cd80fcc..bd012f7d9 100644 --- a/porting/nimble/src/os_mbuf.c +++ b/porting/nimble/src/os_mbuf.c @@ -235,8 +235,8 @@ 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; + if (user_hdr_len > (255 - sizeof(struct os_mbuf_pkthdr))) { + goto err_hdr; } total_pkthdr_len = user_hdr_len + sizeof(struct os_mbuf_pkthdr); @@ -247,6 +247,13 @@ os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len) m = os_mbuf_get_pkthdr(pool, user_hdr_len); return (m); +err_hdr: + log_count++; + if ((log_count % 100) == 0) { + ESP_LOGI("ESP_LOG_INFO", "user_hdr_len too large (%u)\n", user_hdr_len); + log_count = 0; + } + return (NULL); err: log_count ++; if ((log_count % 100) == 0) { @@ -961,7 +968,7 @@ 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, (uint16_t)total_len); + max(OS_MBUF_PKTHDR(om)->omp_len, total_len); } return rc; diff --git a/porting/nimble/src/os_mempool.c b/porting/nimble/src/os_mempool.c index f575a72ad..e351be838 100644 --- a/porting/nimble/src/os_mempool.c +++ b/porting/nimble/src/os_mempool.c @@ -88,9 +88,32 @@ os_mempool_poison_check(const struct os_mempool *mp, void *start) p++; } } + +static bool +os_mempool_poison_valid(const struct os_mempool *mp, void *start) +{ + uint32_t *p; + uint32_t *end; + int sz; + + sz = OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size); + p = start; + end = p + sz / 4; + p += sizeof(struct os_memblock) / 4; + + while (p < end) { + if (*p != os_mem_poison) { + return false; + } + p++; + } + + return true; +} #else #define os_mempool_poison(mp, start) #define os_mempool_poison_check(mp, start) +#define os_mempool_poison_valid(mp, start) true #endif #if MYNEWT_VAL(OS_MEMPOOL_GUARD) #define OS_MEMPOOL_GUARD_PATTERN 0xBAFF1ED1 @@ -118,9 +141,26 @@ os_mempool_guard_check(const struct os_mempool *mp, void *start) assert(*tgt == OS_MEMPOOL_GUARD_PATTERN); } } + +static bool +os_mempool_guard_valid(const struct os_mempool *mp, void *start) +{ + uint32_t *tgt; + + if ((mp->mp_flags & OS_MEMPOOL_F_EXT) == 0) { + tgt = (uint32_t *)((uintptr_t)start + + OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)); + if (*tgt != OS_MEMPOOL_GUARD_PATTERN) { + return false; + } + } + + return true; +} #else #define os_mempool_guard(mp, start) #define os_mempool_guard_check(mp, start) +#define os_mempool_guard_valid(mp, start) true #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -143,6 +183,10 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks, int i; uint8_t *block_addr; struct os_memblock *block_ptr; +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) && MYNEWT_VAL(MP_BLOCK_REUSED) + uint8_t old_flags = 0; + struct os_memblock *old_first = NULL; +#endif /* Check for valid parameters */ if (!mp || (block_size == 0)) { @@ -165,6 +209,15 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks, } } +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) && MYNEWT_VAL(MP_BLOCK_REUSED) + if (membuf == NULL && mp != NULL && + (mp->mp_flags & (OS_MEMPOOL_F_REUSED | OS_MEMPOOL_F_RUNTIME)) == + (OS_MEMPOOL_F_REUSED | OS_MEMPOOL_F_RUNTIME)) { + old_flags = mp->mp_flags; + old_first = SLIST_FIRST(mp); + } +#endif + /* Initialize the memory pool structure */ mp->mp_block_size = block_size; mp->mp_num_free = blocks; @@ -173,11 +226,28 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks, mp->mp_num_blocks = blocks; mp->mp_membuf_addr = (uintptr_t)membuf; mp->name = name; - SLIST_FIRST(mp) = membuf; + if (blocks > 0) { + SLIST_FIRST(mp) = membuf; + } else { + SLIST_FIRST(mp) = NULL; + } #if MYNEWT_VAL(MP_RUNTIME_ALLOC) if (membuf == NULL) { /* Runtime allocation mode */ +#if MYNEWT_VAL(MP_BLOCK_REUSED) + if (old_flags & OS_MEMPOOL_F_REUSED) { + void *temp_ptr; + struct os_memblock *block_ptr; + + block_ptr = old_first; + while (block_ptr != NULL) { + temp_ptr = block_ptr; + block_ptr = SLIST_NEXT(block_ptr, mb_next); + nimble_platform_mem_free(temp_ptr); + } + } +#endif mp->mp_membuf_addr = 0; mp->mp_flags |= OS_MEMPOOL_F_RUNTIME; #if MYNEWT_VAL(MP_BLOCK_REUSED) @@ -259,6 +329,10 @@ os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks, { int rc; + if (mpe == NULL) { + return OS_INVALID_PARM; + } + rc = os_mempool_init_internal(&mpe->mpe_mp, blocks, block_size, membuf, name, OS_MEMPOOL_F_EXT); if (rc != 0) { @@ -388,15 +462,17 @@ os_mempool_ext_clear(struct os_mempool_ext *mpe) { int rc; + if (mpe == NULL) { + return OS_INVALID_PARM; + } + /* Clear the mempool first while EXT flag is intact */ rc = os_mempool_clear(&mpe->mpe_mp); + if (rc != OS_OK) { + return rc; + } - /* Then clear the extended flags using bitwise operation to preserve other flags */ -#if MYNEWT_VAL(MP_RUNTIME_ALLOC) - mpe->mpe_mp.mp_flags &= ~OS_MEMPOOL_F_EXT; -#else - mpe->mpe_mp.mp_flags &= ~OS_MEMPOOL_F_EXT; -#endif + /* Clear callback pointers; keep OS_MEMPOOL_F_EXT so block stride stays consistent. */ mpe->mpe_put_cb = NULL; mpe->mpe_put_arg = NULL; @@ -408,16 +484,21 @@ os_mempool_is_sane(const struct os_mempool *mp) { struct os_memblock *block; + if (mp == NULL) { + return false; + } + #if MYNEWT_VAL(MP_RUNTIME_ALLOC) /* Runtime mode cannot verify sanity */ if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) { assert(0); + return false; } #endif /* Verify that each block in the free list belongs to the mempool. */ /* Limit iterations to prevent infinite loops in case of corruption */ - uint16_t iterations = 0; + uint32_t iterations = 0; SLIST_FOREACH(block, mp, mb_next) { if (++iterations > mp->mp_num_blocks) { /* Potential cycle detected */ @@ -426,8 +507,12 @@ os_mempool_is_sane(const struct os_mempool *mp) if (!os_memblock_from(mp, block)) { return false; } - os_mempool_poison_check(mp, block); - os_mempool_guard_check(mp, block); + if (!os_mempool_poison_valid(mp, block)) { + return false; + } + if (!os_mempool_guard_valid(mp, block)) { + return false; + } } return true; @@ -444,6 +529,7 @@ os_memblock_from(const struct os_mempool *mp, const void *block_addr) /* Runtime allocation mode doesn't support from */ if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) { assert(0); + return false; } #endif @@ -485,6 +571,7 @@ os_memblock_get(struct os_mempool *mp) OS_ENTER_CRITICAL(sr); if (mp->mp_num_free) { +#if MYNEWT_VAL(MP_BLOCK_REUSED) if (mp->mp_flags & OS_MEMPOOL_F_REUSED) { if (SLIST_FIRST(mp) != NULL) { block = SLIST_FIRST(mp); @@ -493,13 +580,15 @@ os_memblock_get(struct os_mempool *mp) need_alloc = true; mp->mp_alloc_blocks++; } else { - /* apply the changes: pool budget exhausted and free list empty; - * exit without decrementing mp_num_free to prevent permanent - * pool exhaustion when mp_num_free > 0 but no block available */ + /* Pool budget exhausted and free list empty; exit without + * decrementing mp_num_free. */ OS_EXIT_CRITICAL(sr); + os_trace_api_ret_u32(OS_TRACE_ID_MEMBLOCK_GET, 0); return NULL; } - } else { + } else +#endif + { need_alloc = true; } /* Decrement number free by 1 */ @@ -541,6 +630,7 @@ os_memblock_get(struct os_mempool *mp) os_mempool_guard_check(mp, block); } + os_trace_api_ret_u32(OS_TRACE_ID_MEMBLOCK_GET, (uintptr_t)block); return block; } #endif @@ -580,6 +670,10 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr) os_sr_t sr; struct os_memblock *block; + if (mp == NULL || block_addr == NULL) { + return OS_INVALID_PARM; + } + os_trace_api_u32x2(OS_TRACE_ID_MEMBLOCK_PUT_FROM_CB, (uintptr_t)mp, (uintptr_t)block_addr); @@ -587,6 +681,7 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr) if (mp->mp_flags & OS_MEMPOOL_F_RUNTIME) { bool need_free = true; os_mempool_guard_check(mp, block_addr); + os_mempool_poison(mp, block_addr); /* Runtime allocation mode - free directly */ OS_ENTER_CRITICAL(sr); @@ -600,21 +695,28 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr) } } + if (mp->mp_num_free >= mp->mp_num_blocks) { + OS_EXIT_CRITICAL(sr); + return OS_INVALID_PARM; + } + block = (struct os_memblock *)block_addr; SLIST_NEXT(block, mb_next) = SLIST_FIRST(mp); SLIST_FIRST(mp) = block; need_free = false; } + if (mp->mp_num_free >= mp->mp_num_blocks) { + OS_EXIT_CRITICAL(sr); + return OS_INVALID_PARM; + } mp->mp_num_free++; - assert(mp->mp_num_blocks >= mp->mp_num_free); OS_EXIT_CRITICAL(sr); - /* Poison and free outside critical section to minimize lock hold time */ - if (!need_free) { - os_mempool_poison(mp, block_addr); - } else { + /* Free outside critical section to minimize lock hold time */ + if (need_free) { nimble_platform_mem_free(block_addr); } + os_trace_api_ret_u32(OS_TRACE_ID_MEMBLOCK_PUT_FROM_CB, (uint32_t)OS_OK); return OS_OK; } #endif @@ -723,6 +825,10 @@ os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi) { struct os_mempool *cur; + if (omi == NULL) { + return NULL; + } + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) os_mempool_list_ensure_init(); #endif @@ -741,8 +847,12 @@ os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi) omi->omi_num_blocks = cur->mp_num_blocks; omi->omi_num_free = cur->mp_num_free; omi->omi_min_free = cur->mp_min_free; - strncpy(omi->omi_name, cur->name, sizeof(omi->omi_name) - 1); - omi->omi_name[sizeof(omi->omi_name) - 1] = '\0'; + if (cur->name != NULL) { + strncpy(omi->omi_name, cur->name, sizeof(omi->omi_name) - 1); + omi->omi_name[sizeof(omi->omi_name) - 1] = '\0'; + } else { + omi->omi_name[0] = '\0'; + } return (cur); } @@ -806,5 +916,9 @@ os_mempool_deinit(void) } mp = STAILQ_NEXT(mp, mp_list); } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + STAILQ_INIT(&g_os_mempool_list); + g_os_mempool_list_inited = false; +#endif } #endif diff --git a/porting/nimble/src/os_msys.c b/porting/nimble/src/os_msys.c index da54aa26c..86a56f534 100644 --- a/porting/nimble/src/os_msys.c +++ b/porting/nimble/src/os_msys.c @@ -24,8 +24,7 @@ #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); +extern STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list; #if CONFIG_BT_NIMBLE_ENABLED #define OS_MSYS_1_BLOCK_COUNT MYNEWT_VAL(MSYS_1_BLOCK_COUNT) @@ -173,7 +172,7 @@ os_msys_sanity(struct os_sanity_check *sc, void *arg) idx++; } - return ESP_OK; + return OS_OK; } #endif @@ -273,6 +272,7 @@ os_msys_buf_free(void) os_msys_ctx->init_2_data = NULL; } #endif + os_msys_reset(); nimble_platform_mem_free(os_msys_ctx); os_msys_ctx = NULL; } @@ -288,16 +288,18 @@ os_msys_buf_free(void) os_mempool_unregister(&os_msys_init_2_mempool); nimble_platform_mem_free(os_msys_init_2_data); os_msys_init_2_data = NULL; -#endif #endif os_msys_reset(); +#endif } void os_msys_init(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - int rc_ctx = ble_os_msys_ensure_ctx(); - SYSINIT_PANIC_ASSERT(rc_ctx == 0); + if (ble_os_msys_ensure_ctx() != 0) { + SYSINIT_PANIC_ASSERT(0); + return; + } #endif #if OS_MSYS_SANITY_ENABLED diff --git a/porting/nimble/src/os_msys_init.c b/porting/nimble/src/os_msys_init.c index a8d0f457f..3391343f1 100644 --- a/porting/nimble/src/os_msys_init.c +++ b/porting/nimble/src/os_msys_init.c @@ -24,8 +24,7 @@ #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); +extern STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list; #if CONFIG_BT_NIMBLE_ENABLED #define OS_MSYS_1_BLOCK_COUNT MYNEWT_VAL(MSYS_1_BLOCK_COUNT) @@ -173,7 +172,7 @@ os_msys_sanity(struct os_sanity_check *sc, void *arg) idx++; } - return ESP_OK; + return OS_OK; } #endif @@ -265,7 +264,6 @@ os_msys_buf_free(void) 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 os_mempool_unregister(&os_msys_ctx->init_2_mempool); @@ -273,26 +271,23 @@ os_msys_buf_free(void) 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 + os_msys_reset(); nimble_platform_mem_free(os_msys_ctx); os_msys_ctx = NULL; } - os_msys_reset(); #else #if OS_MSYS_1_BLOCK_COUNT > 0 os_mempool_unregister(&os_msys_init_1_mempool); 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 os_mempool_unregister(&os_msys_init_2_mempool); nimble_platform_mem_free(os_msys_init_2_data); os_msys_init_2_data = NULL; - os_mempool_unregister(&os_msys_init_2_mempool); #endif os_msys_reset(); #endif @@ -307,7 +302,7 @@ void os_msys_init(void) #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); + SYSINIT_PANIC_ASSERT(0); return; } #endif diff --git a/porting/npl/dummy/include/nimble/nimble_npl_os_log.h b/porting/npl/dummy/include/nimble/nimble_npl_os_log.h index 7391c7435..de091df0e 100644 --- a/porting/npl/dummy/include/nimble/nimble_npl_os_log.h +++ b/porting/npl/dummy/include/nimble/nimble_npl_os_log.h @@ -20,8 +20,8 @@ #ifndef _NIMBLE_NPL_OS_LOG_H_ #define _NIMBLE_NPL_OS_LOG_H_ -#define BLE_NPL_LOG_IMPL(lvl) \ +#define BLE_NPL_LOG_IMPL(lvl, lvl_value) \ static inline void _BLE_NPL_LOG_CAT(BLE_NPL_LOG_MODULE, \ - _BLE_NPL_LOG_CAT(_, lvl))(const char *fmt, ...) { } + _BLE_NPL_LOG_CAT(_, lvl))(const char *fmt, ...) { (void)(lvl_value); } #endif /* _NIMBLE_NPL_OS_LOG_H_ */ diff --git a/porting/npl/esp-idf/include/nimble/nimble_npl_os.h b/porting/npl/esp-idf/include/nimble/nimble_npl_os.h index 0f9548ccc..ef61ec88c 100644 --- a/porting/npl/esp-idf/include/nimble/nimble_npl_os.h +++ b/porting/npl/esp-idf/include/nimble/nimble_npl_os.h @@ -96,6 +96,10 @@ ble_npl_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) static inline void ble_npl_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) { + if (evq == NULL || ev == NULL) { + return; + } + btdm_osal_eventq_put(&evq->eventq, &ev->event); } diff --git a/porting/npl/freertos/src/npl_os_freertos.c b/porting/npl/freertos/src/npl_os_freertos.c index e6f81094c..492fed4f4 100644 --- a/porting/npl/freertos/src/npl_os_freertos.c +++ b/porting/npl/freertos/src/npl_os_freertos.c @@ -55,6 +55,8 @@ portMUX_TYPE ble_port_mutex = portMUX_INITIALIZER_UNLOCKED; #define BLE_NPL_ENTER_CRITICAL() vPortEnterCritical(&ble_port_mutex) #define BLE_NPL_EXIT_CRITICAL() vPortExitCritical(&ble_port_mutex) #endif +#define BLE_NPL_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&ble_port_mutex) +#define BLE_NPL_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&ble_port_mutex) static SemaphoreHandle_t npl_eventq_sync; static uint8_t hw_critical_state_status[portNUM_PROCESSORS]; @@ -269,7 +271,7 @@ npl_freertos_get_current_task_id(void) return xTaskGetCurrentTaskHandle(); } -void IRAM_ATTR +void npl_freertos_event_init(struct ble_npl_event *ev, ble_npl_event_fn *fn, void *arg) { @@ -383,7 +385,7 @@ npl_freertos_callout_mem_reset(struct ble_npl_callout *co) ble_npl_event_reset(&callout->ev); } -static inline bool +static inline bool IRAM_ATTR in_isr(void) { return xPortInIsrContext() != 0; @@ -444,7 +446,7 @@ npl_eventq_queued_set_isr(struct ble_npl_event_freertos *event, bool queued) portEXIT_CRITICAL_ISR(&ble_port_mutex); } -static bool IRAM_ATTR +static bool IRAM_ATTR __attribute__((unused)) npl_eventq_queued_claim_isr(struct ble_npl_event_freertos *event) { bool already; @@ -552,9 +554,9 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event; if (event) { if (in_isr()) { - UBaseType_t sr = portSET_INTERRUPT_MASK_FROM_ISR(); + BLE_NPL_ENTER_CRITICAL_ISR(); event->queued = false; - portCLEAR_INTERRUPT_MASK_FROM_ISR(sr); + BLE_NPL_EXIT_CRITICAL_ISR(); } else { BLE_NPL_ENTER_CRITICAL(); event->queued = false; @@ -576,13 +578,13 @@ npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) /* Use critical section to make check-and-set of queued flag atomic */ if (in_isr()) { - UBaseType_t sr = portSET_INTERRUPT_MASK_FROM_ISR(); + BLE_NPL_ENTER_CRITICAL_ISR(); if (event->queued) { - portCLEAR_INTERRUPT_MASK_FROM_ISR(sr); + BLE_NPL_EXIT_CRITICAL_ISR(); return; } event->queued = true; - portCLEAR_INTERRUPT_MASK_FROM_ISR(sr); + BLE_NPL_EXIT_CRITICAL_ISR(); ret = xQueueSendToBackFromISR(eventq->q, &ev, &woken); if (woken == pdTRUE) { @@ -1064,10 +1066,9 @@ npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq memset(callout, 0, sizeof(*callout)); ble_npl_event_init(&callout->ev, ev_cb, ev_arg); + callout->evq = evq; #if CONFIG_BT_NIMBLE_USE_ESP_TIMER - callout->evq = evq; - esp_timer_create_args_t create_args = { .callback = ble_npl_event_fn_wrapper, .arg = callout, @@ -1177,7 +1178,7 @@ npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks) npl_freertos_eventq_remove(callout->evq, &callout->ev); } ret2 = xTimerChangePeriodFromISR(callout->handle, ticks, &woken2); - ret3 = xTimerResetFromISR(callout->handle, &woken3); + ret3 =xTimerResetFromISR(callout->handle, &woken3); portYIELD_FROM_ISR(woken1 || woken2 || woken3); @@ -1219,7 +1220,11 @@ npl_freertos_callout_stop(struct ble_npl_callout *co) } } +#if CONFIG_BT_NIMBLE_USE_ESP_TIMER +bool +#else bool IRAM_ATTR +#endif npl_freertos_callout_is_active(struct ble_npl_callout *co) { struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co; @@ -1282,6 +1287,10 @@ npl_freertos_callout_remaining_ticks(struct ble_npl_callout *co, uint64_t expiry = 0; esp_err_t err; + if (!esp_timer_is_active((esp_timer_handle_t)(callout->handle))) { + return 0; + } + //Fetch expiry time in microseconds err = esp_timer_get_expiry_time((esp_timer_handle_t)(callout->handle), &expiry); if (err != ESP_OK) { @@ -1293,8 +1302,7 @@ npl_freertos_callout_remaining_ticks(struct ble_npl_callout *co, npl_freertos_time_ms_to_ticks((uint32_t)(expiry / 1000), &exp); #else //esp_timer_get_expiry_time() is only available from IDF 5.0 onwards - //Set expiry to 0 - exp = 0; + return 0; #endif //ESP_IDF_VERSION #else /* Check if timer is active before getting expiry time */ @@ -1393,11 +1401,7 @@ void npl_freertos_time_delay(ble_npl_time_t ticks) { #if CONFIG_BT_NIMBLE_USE_ESP_TIMER - /* Use pdMS_TO_TICKS for proper ceiling division to ensure minimum 1 tick */ - ble_npl_time_t delay_ticks = pdMS_TO_TICKS(ticks); - if (delay_ticks == 0 && ticks > 0) { - delay_ticks = 1; /* Ensure minimum delay for non-zero input */ - } + ble_npl_time_t delay_ticks = (ble_npl_time_t)(((uint64_t)ticks + portTICK_PERIOD_MS - 1) / portTICK_PERIOD_MS); vTaskDelay(delay_ticks); #else vTaskDelay(ticks); @@ -1654,20 +1658,21 @@ _error: #endif #if (SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED) - if(ble_freertos_ev_buf) { - nimble_platform_mem_free(ble_freertos_ev_buf); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_freertos_ctx) { +#endif + if(ble_freertos_ev_buf) { + nimble_platform_mem_free(ble_freertos_ev_buf); ble_freertos_ev_buf = NULL; + } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) } +#endif rc = -1; #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_freertos_ctx) { - os_mempool_unregister(&ble_freertos_mutex_pool); - os_mempool_unregister(&ble_freertos_sem_pool); - os_mempool_unregister(&ble_freertos_co_pool); - os_mempool_unregister(&ble_freertos_evq_pool); - os_mempool_unregister(&ble_freertos_ev_pool); nimble_platform_mem_free(ble_freertos_ctx); ble_freertos_ctx = NULL; } @@ -1721,11 +1726,6 @@ void npl_freertos_mempool_deinit(void) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_freertos_ctx) { - os_mempool_unregister(&ble_freertos_mutex_pool); - os_mempool_unregister(&ble_freertos_sem_pool); - os_mempool_unregister(&ble_freertos_co_pool); - os_mempool_unregister(&ble_freertos_evq_pool); - os_mempool_unregister(&ble_freertos_ev_pool); nimble_platform_mem_free(ble_freertos_ctx); ble_freertos_ctx = NULL; } diff --git a/porting/npl/linux/include/nimble/nimble_npl_os_log.h b/porting/npl/linux/include/nimble/nimble_npl_os_log.h index 2e359f878..a0b506b7f 100644 --- a/porting/npl/linux/include/nimble/nimble_npl_os_log.h +++ b/porting/npl/linux/include/nimble/nimble_npl_os_log.h @@ -24,11 +24,12 @@ #include /* Example on how to use macro to generate module logging functions */ -#define BLE_NPL_LOG_IMPL(lvl) \ +#define BLE_NPL_LOG_IMPL(lvl, lvl_value) \ static inline void _BLE_NPL_LOG_CAT( \ BLE_NPL_LOG_MODULE, _BLE_NPL_LOG_CAT(_, lvl))(const char *fmt, ...) \ { \ va_list args; \ + (void)(lvl_value); \ va_start(args, fmt); \ vprintf(fmt, args); \ va_end(args); \ diff --git a/porting/npl/linux/src/os_atomic.c b/porting/npl/linux/src/os_atomic.c index 531c4a274..429459feb 100644 --- a/porting/npl/linux/src/os_atomic.c +++ b/porting/npl/linux/src/os_atomic.c @@ -24,21 +24,15 @@ #include "nimble/nimble_npl.h" -static struct ble_npl_mutex s_mutex; -static uint8_t s_mutex_inited = 0; +static pthread_mutex_t s_mutex = PTHREAD_MUTEX_INITIALIZER; uint32_t ble_npl_hw_enter_critical(void) { - if( !s_mutex_inited ) { - ble_npl_mutex_init(&s_mutex); - s_mutex_inited = 1; - } - - pthread_mutex_lock(&s_mutex.lock); + pthread_mutex_lock(&s_mutex); return 0; } void ble_npl_hw_exit_critical(uint32_t ctx) { - pthread_mutex_unlock(&s_mutex.lock); + pthread_mutex_unlock(&s_mutex); } diff --git a/porting/npl/linux/src/os_sem.c b/porting/npl/linux/src/os_sem.c index be43c6499..e5a56133c 100644 --- a/porting/npl/linux/src/os_sem.c +++ b/porting/npl/linux/src/os_sem.c @@ -61,7 +61,9 @@ ble_npl_sem_pend(struct ble_npl_sem *sem, uint32_t timeout) } if (timeout == BLE_NPL_TIME_FOREVER) { - err = sem_wait(&sem->lock); + do { + err = sem_wait(&sem->lock); + } while (err != 0 && errno == EINTR); } else { err = clock_gettime(CLOCK_REALTIME, &wait); if (err) { @@ -70,6 +72,10 @@ ble_npl_sem_pend(struct ble_npl_sem *sem, uint32_t timeout) wait.tv_sec += timeout / 1000; wait.tv_nsec += (timeout % 1000) * 1000000; + if (wait.tv_nsec >= 1000000000) { + wait.tv_sec += wait.tv_nsec / 1000000000; + wait.tv_nsec %= 1000000000; + } while ((err = sem_timedwait(&sem->lock, &wait)) != 0) { switch (errno) { diff --git a/porting/npl/linux/src/os_task.c b/porting/npl/linux/src/os_task.c index c34507f15..7ee251ec4 100644 --- a/porting/npl/linux/src/os_task.c +++ b/porting/npl/linux/src/os_task.c @@ -61,20 +61,27 @@ ble_npl_task_init(struct ble_npl_task *t, const char *name, ble_npl_task_func_t return err; err = pthread_attr_getschedparam(&t->attr, &t->param); - if (err) + if (err) { + pthread_attr_destroy(&t->attr); return err; + } err = pthread_attr_setschedpolicy(&t->attr, SCHED_RR); - if (err) + if (err) { + pthread_attr_destroy(&t->attr); return err; + } t->param.sched_priority = prio; err = pthread_attr_setschedparam(&t->attr, &t->param); - if (err) + if (err) { + pthread_attr_destroy(&t->attr); return err; + } t->name = name; err = pthread_create(&t->handle, &t->attr, func, arg); + pthread_attr_destroy(&t->attr); return err; } diff --git a/porting/npl/riot/include/nimble/nimble_npl_os_log.h b/porting/npl/riot/include/nimble/nimble_npl_os_log.h index f585070da..5e06f8676 100644 --- a/porting/npl/riot/include/nimble/nimble_npl_os_log.h +++ b/porting/npl/riot/include/nimble/nimble_npl_os_log.h @@ -24,12 +24,13 @@ #include /* Example on how to use macro to generate module logging functions */ -#define BLE_NPL_LOG_IMPL(lvl) \ +#define BLE_NPL_LOG_IMPL(lvl, lvl_value) \ __attribute__((__format__ (__printf__, 1, 0))) \ static inline void _BLE_NPL_LOG_CAT(BLE_NPL_LOG_MODULE, \ _BLE_NPL_LOG_CAT(_, lvl))(const char *fmt, ...)\ { \ va_list args; \ + (void)(lvl_value); \ va_start(args, fmt); \ vprintf(fmt, args); \ va_end(args); \ diff --git a/porting/update_generated_files.sh b/porting/update_generated_files.sh index da409511b..bbcd5f6df 100755 --- a/porting/update_generated_files.sh +++ b/porting/update_generated_files.sh @@ -43,5 +43,5 @@ for target in "${!targets[@]}"; do find "${targets[$target]}/include" -type f -name 'syscfg.h' -exec sed -i '/MYNEWT_VAL_REPO_*/,/#endif/d' {} \; find "${targets[$target]}/include" -type f -name 'syscfg.h' -exec sed -i -E ':a;N;$!ba;s:/\*([^*]|(\*+([^*/])))*\*+/::g' {} \; find "${targets[$target]}/include" -type f -name 'syscfg.h' -exec sed -i '$!N;/^\n$/{$q;D;};P;D;' {} \; - find "${targets[$target]}/include" -type f -name 'syscfg.h' -exec sh -c 'cat "$0" "$1" > "$1.tmp" && mv "$1.tmp" "$1"' "repos/apache-mynewt-nimble/.github/LICENSE_TEMPLATE" {} \; + find "${targets[$target]}/include" -type f -name 'syscfg.h' -exec sh -c 'if [ -f "$0" ]; then cat "$0" "$1" > "$1.tmp" && mv "$1.tmp" "$1"; fi' "repos/apache-mynewt-nimble/.github/LICENSE_TEMPLATE" {} \; done