From d750d6baeb578b1dcc295cd3a50df5258cdeffee Mon Sep 17 00:00:00 2001 From: Rahul Tank Date: Mon, 29 Jun 2026 11:26:45 +0530 Subject: [PATCH] fix(nimble): Defer events / requests from stack until connection event is sent to GAP layer --- nimble/host/src/ble_att.c | 108 ++- nimble/host/src/ble_att_priv.h | 3 + nimble/host/src/ble_gap.c | 825 ++++++++++++++++++++- nimble/host/src/ble_gap_priv.h | 5 + nimble/host/src/ble_gattc_cache_conn.c | 8 + nimble/host/src/ble_hs_conn.c | 13 + nimble/host/src/ble_hs_conn_priv.h | 48 ++ nimble/host/src/ble_hs_pvcy.c | 87 +++ nimble/host/src/ble_hs_pvcy_priv.h | 4 + nimble/host/src/ble_sm.c | 57 ++ nimble/host/src/ble_store.c | 63 +- nimble/host/test/src/ble_hs_pvcy_test.c | 22 +- porting/npl/freertos/src/npl_os_freertos.c | 84 ++- 13 files changed, 1270 insertions(+), 57 deletions(-) diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c index b9d033296..f2e491f55 100644 --- a/nimble/host/src/ble_att.c +++ b/nimble/host/src/ble_att.c @@ -22,6 +22,10 @@ #include "ble_hs_priv.h" #include "host/ble_att.h" #include "host/ble_hs_log.h" +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +#include "host/ble_gap.h" +#include "esp_nimble_mem.h" +#endif #if NIMBLE_BLE_CONNECT @@ -547,10 +551,9 @@ ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle, } #if MYNEWT_VAL(BLE_GATTS) os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om)); - if (ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, - BLE_ATT_ERR_REQ_NOT_SUPPORTED) == 0) { - *om = NULL; - } + ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, + BLE_ATT_ERR_REQ_NOT_SUPPORTED); + *om = NULL; #else os_mbuf_free_chain(*om); *om = NULL; @@ -576,7 +579,11 @@ ble_att_send_outstanding_after_response(uint16_t conn_handle) ble_hs_unlock(); } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +int +#else static int +#endif ble_att_rx_extended(uint16_t conn_handle, uint16_t cid, struct os_mbuf **om) { const struct ble_att_rx_dispatch_entry *entry; @@ -585,6 +592,99 @@ ble_att_rx_extended(uint16_t conn_handle, uint16_t cid, struct os_mbuf **om) BLE_HS_DBG_ASSERT(*om != NULL); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + { + struct ble_hs_conn *conn; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL && !conn->bhc_connect_delivered) { + struct ble_att_deferred_req *req; + bool defer_ok = true; + + if ((conn->bhc_deferred_event_cnt + conn->bhc_deferred_att_cnt) >= + BLE_HS_CONN_DEFERRED_EVENT_MAX) { + defer_ok = false; + BLE_HS_LOG(ERROR, + "ble_att_rx_extended: defer queue full; conn=%u cid=%u\n", + conn_handle, cid); + } + + ble_hs_unlock(); + + if (defer_ok) { + req = nimble_platform_mem_calloc(1, sizeof(*req)); + if (req == NULL) { + BLE_HS_LOG(ERROR, + "ble_att_rx_extended: defer alloc failed; conn=%u cid=%u\n", + conn_handle, cid); + } else { + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + nimble_platform_mem_free(req); + return BLE_HS_ENOTCONN; + } else if (conn->bhc_connect_delivered) { + ble_hs_unlock(); + nimble_platform_mem_free(req); + goto process_att_normally; + } else if ((conn->bhc_deferred_event_cnt + + conn->bhc_deferred_att_cnt) >= + BLE_HS_CONN_DEFERRED_EVENT_MAX) { + ble_hs_unlock(); + nimble_platform_mem_free(req); + BLE_HS_LOG(ERROR, + "ble_att_rx_extended: defer queue full post-alloc; " + "conn=%u cid=%u\n", + conn_handle, cid); + } else { + req->cid = cid; + req->om = *om; + req->seq = conn->bhc_deferred_seq++; + STAILQ_INSERT_TAIL(&conn->bhc_deferred_att_reqs, req, next); + conn->bhc_deferred_att_cnt++; + *om = NULL; + ble_hs_unlock(); + return 0; + } + } + } + + rc = os_mbuf_copydata(*om, 0, 1, &op); + if (rc != 0) { + return BLE_HS_EMSGSIZE; + } + + /* + * Defer queue is full or allocation failed. Respond on the wire + * for ATT requests; drop everything else. Either way the + * application is not invoked so CONNECT stays first. + */ + if (cid == BLE_L2CAP_CID_ATT && ble_att_is_request_op(op)) { +#if MYNEWT_VAL(BLE_GATTS) + os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om)); + ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, + BLE_ATT_ERR_INSUFFICIENT_RES); + *om = NULL; +#endif + if (*om != NULL) { + os_mbuf_free_chain(*om); + *om = NULL; + } + } else { + /* Notifications, write commands, EATT ops, etc. */ + os_mbuf_free_chain(*om); + *om = NULL; + } + return 0; + } else { + ble_hs_unlock(); + } + } + +process_att_normally: +#endif rc = os_mbuf_copydata(*om, 0, 1, &op); if (rc != 0) { return BLE_HS_EMSGSIZE; diff --git a/nimble/host/src/ble_att_priv.h b/nimble/host/src/ble_att_priv.h index bc3248cf2..deb20b199 100644 --- a/nimble/host/src/ble_att_priv.h +++ b/nimble/host/src/ble_att_priv.h @@ -182,6 +182,9 @@ void ble_att_truncate_to_mtu(const struct ble_l2cap_chan *att_chan, void ble_att_set_peer_mtu(struct ble_l2cap_chan *chan, uint16_t peer_mtu); uint16_t ble_att_chan_mtu(const struct ble_l2cap_chan *chan); uint16_t ble_att_mtu_by_cid(uint16_t conn_handle, uint16_t cid); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +int ble_att_rx_extended(uint16_t conn_handle, uint16_t cid, struct os_mbuf **om); +#endif int ble_att_init(void); void ble_att_deinit(void); /*** @svr */ diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index f4e8ac078..e21ed3354 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -27,6 +27,10 @@ #include "ble_hs_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" +#include "ble_att_priv.h" +#endif #include "host/ble_hs_log.h" #if MYNEWT_VAL(BLE_GATT_CACHING) #include "ble_gattc_cache_priv.h" @@ -47,6 +51,13 @@ #define min(a, b) ((a) < (b) ? (a) : (b)) #endif +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static ble_addr_t ble_gap_deferred_pvcy_addr; +static uint8_t ble_gap_deferred_pvcy_irk[16]; +static bool ble_gap_deferred_pvcy_add; +static bool ble_gap_deferred_pvcy_replace; +#endif + #ifndef max #define max(a, b) ((a) > (b) ? (a) : (b)) #endif @@ -887,6 +898,10 @@ 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) +static void ble_gap_ensure_peer_rl_entry(const ble_addr_t *peer_addr); +#endif + int ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode) { @@ -901,6 +916,9 @@ ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode) } #if MYNEWT_VAL(BLE_HS_PVCY) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && !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); #else return BLE_HS_ENOTSUP; @@ -1135,6 +1153,37 @@ ble_gap_call_event_cb(struct ble_gap_event *event, } #if NIMBLE_BLE_CONNECT +static void ble_gap_update_failed(uint16_t conn_handle, int status); + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +static void +ble_gap_enc_side_effects(uint16_t conn_handle, int status, + int security_restored, int bonded) +{ +#if NIMBLE_BLE_SM +#if MYNEWT_VAL(BLE_GATTC) && MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) + ble_gattc_recover_gatt_proc(conn_handle, status); +#endif + + if (status == 0) { +#if MYNEWT_VAL(BLE_GATTS) + if (security_restored) { + ble_gatts_bonding_restored(conn_handle); +#if MYNEWT_VAL(BLE_GATT_CACHING) + ble_gattc_cache_conn_bonding_restored(conn_handle); +#endif + } else if (bonded) { + ble_gatts_bonding_established(conn_handle); +#if MYNEWT_VAL(BLE_GATT_CACHING) + ble_gattc_cache_conn_bonding_established(conn_handle); +#endif + } +#endif + } +#endif +} +#endif /* MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) */ + static int ble_gap_call_conn_event_cb(struct ble_gap_event *event, uint16_t conn_handle) { @@ -1154,6 +1203,388 @@ ble_gap_call_conn_event_cb(struct ble_gap_event *event, uint16_t conn_handle) return 0; } + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +enum ble_gap_defer_result { + BLE_GAP_DEFER_NOT_NEEDED = 0, + BLE_GAP_DEFER_QUEUED, + BLE_GAP_DEFER_FAILED, +}; + +struct ble_gap_defer_meta { + uint8_t enc_security_restored; + uint8_t enc_bonded; + const struct ble_gap_upd_params *upd_peer; + const struct ble_gap_upd_params *upd_self; +}; + +static void ble_gap_deferred_drain_ev(struct ble_npl_event *ev); +static void ble_gap_deferred_drain_step(uint16_t conn_handle); +static void ble_gap_deferred_drain_start(uint16_t conn_handle); +static void ble_gap_conn_deferred_discard(struct ble_hs_conn *conn); +static void ble_gap_deliver_deferred_gap(struct ble_gap_deferred_event *dev, + uint16_t conn_handle); +static void ble_gap_notify_conn_event(uint16_t conn_handle, + struct ble_gap_event *event); +static enum ble_gap_defer_result +ble_gap_defer_event(uint16_t conn_handle, const struct ble_gap_event *event, + const struct ble_gap_defer_meta *meta); +static void ble_gap_mark_connect_delivered(uint16_t conn_handle); + +static bool +ble_gap_deferred_conn_cb_only(uint8_t type) +{ + switch (type) { + case BLE_GAP_EVENT_PASSKEY_ACTION: + case BLE_GAP_EVENT_PARING_COMPLETE: + case BLE_GAP_EVENT_CONN_UPDATE_REQ: + case BLE_GAP_EVENT_REPEAT_PAIRING: + case BLE_GAP_EVENT_CACHE_ASSOC: + return true; + default: + return false; + } +} + +/** + * Attempt to defer a GAP event until BLE_GAP_EVENT_CONNECT has been delivered. + * + * Must be called with the ble_hs lock NOT held. Event ownership, including + * notify_rx.om for BLE_GAP_EVENT_NOTIFY_RX, is transferred only when this + * returns BLE_GAP_DEFER_QUEUED. Otherwise the caller retains ownership. + * + * Memory for the queue node is allocated before acquiring the ble_hs lock so + * the critical section stays short and allocation-failure handling stays out + * of the hot path. + * + * @param conn_handle Connection handle. + * @param event Fully populated GAP event to defer. + */ +static enum ble_gap_defer_result +ble_gap_defer_event(uint16_t conn_handle, + const struct ble_gap_event *event, + const struct ble_gap_defer_meta *meta) +{ + struct ble_hs_conn *conn; + struct ble_gap_deferred_event *dev; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL || conn->bhc_connect_delivered) { + ble_hs_unlock(); + return BLE_GAP_DEFER_NOT_NEEDED; + } + + if ((conn->bhc_deferred_event_cnt + conn->bhc_deferred_att_cnt) >= + BLE_HS_CONN_DEFERRED_EVENT_MAX) { + ble_hs_unlock(); + BLE_HS_LOG(ERROR, + "ble_gap_defer_event: queue full; type=%d conn=%u\n", + event->type, conn_handle); + return BLE_GAP_DEFER_FAILED; + } + + ble_hs_unlock(); + + dev = nimble_platform_mem_calloc(1, sizeof(*dev)); + if (dev == NULL) { + BLE_HS_LOG(ERROR, + "ble_gap_defer_event: alloc failed; type=%d conn=%u\n", + event->type, conn_handle); + return BLE_GAP_DEFER_FAILED; + } + + dev->event = *event; + if (meta != NULL) { + dev->enc_security_restored = meta->enc_security_restored; + dev->enc_bonded = meta->enc_bonded; + if (meta->upd_peer != NULL) { + dev->upd_peer = *meta->upd_peer; + dev->event.conn_update_req.peer_params = &dev->upd_peer; + } + if (meta->upd_self != NULL) { + dev->upd_self = *meta->upd_self; + dev->event.conn_update_req.self_params = &dev->upd_self; + } + } + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + + if (conn == NULL || conn->bhc_connect_delivered) { + ble_hs_unlock(); + nimble_platform_mem_free(dev); + return BLE_GAP_DEFER_NOT_NEEDED; + } + + if ((conn->bhc_deferred_event_cnt + conn->bhc_deferred_att_cnt) >= + BLE_HS_CONN_DEFERRED_EVENT_MAX) { + ble_hs_unlock(); + nimble_platform_mem_free(dev); + BLE_HS_LOG(ERROR, + "ble_gap_defer_event: queue full post-alloc; type=%d conn=%u\n", + event->type, conn_handle); + return BLE_GAP_DEFER_FAILED; + } + + dev->seq = conn->bhc_deferred_seq++; + STAILQ_INSERT_TAIL(&conn->bhc_deferred_events, dev, next); + conn->bhc_deferred_event_cnt++; + ble_hs_unlock(); + return BLE_GAP_DEFER_QUEUED; +} + +static void +ble_gap_notify_conn_event(uint16_t conn_handle, struct ble_gap_event *event) +{ + switch (ble_gap_defer_event(conn_handle, event, NULL)) { + case BLE_GAP_DEFER_QUEUED: + return; + case BLE_GAP_DEFER_NOT_NEEDED: + ble_gap_event_listener_call(event); + ble_gap_call_conn_event_cb(event, conn_handle); + break; + case BLE_GAP_DEFER_FAILED: + BLE_HS_LOG(ERROR, + "ble_gap_notify_conn_event: defer failed; type=%d conn=%u\n", + event->type, conn_handle); + ble_gap_event_listener_call(event); + ble_gap_call_conn_event_cb(event, conn_handle); + break; + } +} + +static void +ble_gap_deliver_deferred_gap(struct ble_gap_deferred_event *dev, + uint16_t conn_handle) +{ + struct ble_gap_event *event; + + event = &dev->event; + + if (event->type == BLE_GAP_EVENT_CONN_UPDATE_REQ) { + /* + * HCI Reply/NRR was already sent in ble_gap_rx_param_req(); only + * deliver the deferred application notification here. + */ + event->conn_update_req.self_params = &dev->upd_self; + event->conn_update_req.peer_params = &dev->upd_peer; + ble_gap_call_conn_event_cb(event, conn_handle); + return; + } + + if (!ble_gap_deferred_conn_cb_only(event->type)) { + ble_gap_event_listener_call(event); + } + ble_gap_call_conn_event_cb(event, conn_handle); + + if (event->type == BLE_GAP_EVENT_ENC_CHANGE) { + ble_gap_enc_side_effects(conn_handle, event->enc_change.status, + dev->enc_security_restored, dev->enc_bonded); + } + + if (event->type == BLE_GAP_EVENT_NOTIFY_RX) { + os_mbuf_free_chain(event->notify_rx.om); + } +} + +static void +ble_gap_deferred_drain_step(uint16_t conn_handle) +{ + struct ble_hs_conn *conn; + struct ble_gap_deferred_event *dev; + struct ble_att_deferred_req *req; + bool more; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + return; + } + + if (!conn->bhc_deferred_draining) { + ble_hs_unlock(); + return; + } + + dev = STAILQ_FIRST(&conn->bhc_deferred_events); + req = STAILQ_FIRST(&conn->bhc_deferred_att_reqs); + + if (req == NULL || (dev != NULL && dev->seq <= req->seq)) { + if (dev != NULL) { + STAILQ_REMOVE_HEAD(&conn->bhc_deferred_events, next); + conn->bhc_deferred_event_cnt--; + } + } else { + dev = NULL; + STAILQ_REMOVE_HEAD(&conn->bhc_deferred_att_reqs, next); + conn->bhc_deferred_att_cnt--; + } + + more = !STAILQ_EMPTY(&conn->bhc_deferred_events) || + !STAILQ_EMPTY(&conn->bhc_deferred_att_reqs); + + if (!more) { + conn->bhc_deferred_draining = 0; + } + + ble_hs_unlock(); + + if (!more) { + ble_npl_eventq_remove(ble_hs_evq_get(), &conn->bhc_deferred_drain_ev); + } + + if (dev != NULL) { + ble_gap_deliver_deferred_gap(dev, conn_handle); + nimble_platform_mem_free(dev); + } else if (req != NULL) { + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL) { + /* + * Temporarily clear the draining flag so ble_att_rx_extended + * processes the replayed request normally instead of + * re-deferring it back onto the queue (which would livelock). + * The flag is restored under lock in the if (more) block below. + */ + conn->bhc_deferred_draining = 0; + } + ble_hs_unlock(); + + if (conn == NULL) { + os_mbuf_free_chain(req->om); + nimble_platform_mem_free(req); + return; + } + + struct os_mbuf *om = req->om; + ble_att_rx_extended(conn_handle, req->cid, &om); + if (om != NULL) { + os_mbuf_free_chain(om); + } + nimble_platform_mem_free(req); + } + + if (more) { + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL) { + conn->bhc_deferred_draining = 1; + ble_npl_eventq_put(ble_hs_evq_get(), &conn->bhc_deferred_drain_ev); + } + ble_hs_unlock(); + } +} + +static void +ble_gap_deferred_drain_ev(struct ble_npl_event *ev) +{ + uint16_t conn_handle; + + conn_handle = (uint16_t)(uintptr_t)ble_npl_event_get_arg(ev); + ble_gap_deferred_drain_step(conn_handle); +} + +static void +ble_gap_deferred_drain_start(uint16_t conn_handle) +{ + struct ble_hs_conn *conn; + bool schedule; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + return; + } + + if (conn->bhc_deferred_draining) { + ble_hs_unlock(); + return; + } + + schedule = !STAILQ_EMPTY(&conn->bhc_deferred_events) || + !STAILQ_EMPTY(&conn->bhc_deferred_att_reqs); + + if (schedule) { + conn->bhc_deferred_draining = 1; + ble_npl_eventq_put(ble_hs_evq_get(), &conn->bhc_deferred_drain_ev); + } + + ble_hs_unlock(); +} + +static void +ble_gap_mark_connect_delivered(uint16_t conn_handle) +{ + struct ble_hs_conn *conn; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL) { + conn->bhc_connect_delivered = 1; + } + ble_hs_unlock(); +} + +void +ble_gap_conn_deferred_init(struct ble_hs_conn *conn) +{ + BLE_HS_DBG_ASSERT(!conn->bhc_deferred_inited); + + ble_npl_event_init(&conn->bhc_deferred_drain_ev, ble_gap_deferred_drain_ev, + (void *)(uintptr_t)conn->bhc_handle); + conn->bhc_deferred_inited = 1; +} + +static void +ble_gap_conn_deferred_discard(struct ble_hs_conn *conn) +{ + struct ble_gap_deferred_event *dev; + struct ble_att_deferred_req *req; + + if (conn == NULL || !conn->bhc_deferred_inited) { + return; + } + + ble_npl_eventq_remove(ble_hs_evq_get(), &conn->bhc_deferred_drain_ev); + conn->bhc_deferred_draining = 0; + + while ((dev = STAILQ_FIRST(&conn->bhc_deferred_events)) != NULL) { + STAILQ_REMOVE_HEAD(&conn->bhc_deferred_events, next); + if (dev->event.type == BLE_GAP_EVENT_NOTIFY_RX) { + os_mbuf_free_chain(dev->event.notify_rx.om); + } + nimble_platform_mem_free(dev); + } + conn->bhc_deferred_event_cnt = 0; + + while ((req = STAILQ_FIRST(&conn->bhc_deferred_att_reqs)) != NULL) { + STAILQ_REMOVE_HEAD(&conn->bhc_deferred_att_reqs, next); + os_mbuf_free_chain(req->om); + nimble_platform_mem_free(req); + } + conn->bhc_deferred_att_cnt = 0; +} + +void +ble_gap_conn_deferred_cleanup(struct ble_hs_conn *conn) +{ + if (conn == NULL || !conn->bhc_deferred_inited) { + return; + } + + ble_gap_conn_deferred_discard(conn); + ble_npl_event_deinit(&conn->bhc_deferred_drain_ev); + conn->bhc_deferred_inited = 0; +} + +#endif /* MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) */ +#endif /* NIMBLE_BLE_CONNECT */ + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static void ble_gap_apply_deferred_pvcy_add(void); #endif static bool @@ -1649,8 +2080,12 @@ ble_gap_update_notify(uint16_t conn_handle, int status) event.conn_update.conn_handle = conn_handle; event.conn_update.status = status; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); +#endif /* Terminate the connection on procedure timeout. */ if (status == BLE_HS_ETIMEOUT) { @@ -1824,8 +2259,15 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) struct ble_gap_snapshot snap; struct ble_gap_event event; struct ble_hs_conn *conn; + bool post_connect_fail; bool send = 1; int rc; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_addr_t deferred_pvcy_addr; + uint8_t deferred_pvcy_irk[16]; + bool deferred_pvcy_add = false; + bool deferred_pvcy_replace = false; +#endif memset(&event, 0, sizeof event); snap.desc = &event.disconnect.conn; @@ -1880,20 +2322,22 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) if (conn->slave_conn) { conn->slave_conn = 0; #endif - } else { - send = 0; - } + } else { + send = 0; + } } + post_connect_fail = conn != NULL && !send && + !(conn->bhc_flags & BLE_HS_CONN_F_MASTER); + ble_hs_unlock(); /* If we never posted a connect event for a slave (send == 0), treat this as * a connection failure and post a connect-failed event instead of a * disconnect. This allows applications to restart advertising. */ - if (conn != NULL && !send && !(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) - { - ble_gap_event_connect_call(conn_handle, BLE_HS_EAGAIN); + if (post_connect_fail) { + ble_gap_event_connect_call(htole16(conn_handle), BLE_HS_EAGAIN); } ble_hs_lock(); @@ -1903,11 +2347,37 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) conn->bhc_max_rx_time = 0; conn->bhc_max_tx_octets = 0; conn->bhc_max_rx_octets = 0; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (conn->bhc_deferred_pvcy_add) { + deferred_pvcy_addr = conn->bhc_deferred_pvcy_addr; + memcpy(deferred_pvcy_irk, conn->bhc_deferred_pvcy_irk, + sizeof deferred_pvcy_irk); + deferred_pvcy_replace = conn->bhc_deferred_pvcy_replace; + conn->bhc_deferred_pvcy_add = 0; + conn->bhc_deferred_pvcy_replace = 0; + deferred_pvcy_add = true; + } +#endif } ble_hs_unlock(); ble_hs_atomic_conn_delete(conn_handle); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (deferred_pvcy_add) { + ble_gap_deferred_pvcy_addr = deferred_pvcy_addr; + memcpy(ble_gap_deferred_pvcy_irk, deferred_pvcy_irk, + sizeof ble_gap_deferred_pvcy_irk); + ble_gap_deferred_pvcy_replace = deferred_pvcy_replace; + ble_gap_deferred_pvcy_add = true; + /* + * Peer IRK was held while connected; push it to the controller + * resolving list now so scan/connect work without starting adv. + */ + ble_gap_apply_deferred_pvcy_add(); + } +#endif + event.type = BLE_GAP_EVENT_DISCONNECT; event.disconnect.reason = reason; @@ -3572,11 +4042,33 @@ ble_gap_event_connect_call(uint16_t conn_handle, int status) ble_gap_event_listener_call(&event); #if NIMBLE_BLE_CONNECT ble_gap_call_conn_event_cb(&event, handle); + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + /* + * Application callbacks for CONNECT/LINK_ESTAB are complete. + * Success: drain deferred GAP/ATT work in seq order. + * Failure: drop the deferred queue; connection is not established. + */ + ble_gap_mark_connect_delivered(handle); + if (status == 0) { + ble_gap_deferred_drain_start(handle); + } else { + struct ble_hs_conn *conn; + + ble_hs_lock(); + conn = ble_hs_conn_find(handle); + if (conn != NULL) { + ble_gap_conn_deferred_discard(conn); + } + ble_hs_unlock(); + } +#endif #endif - if (status == 0 ) { - ble_hs_hci_util_set_data_len(le16toh(conn_handle), BLE_HCI_SUGG_DEF_DATALEN_TX_OCTETS_MAX, - BLE_HCI_SUGG_DEF_DATALEN_TX_TIME_MAX); + if (status == 0) { + ble_hs_hci_util_set_data_len(le16toh(conn_handle), + BLE_HCI_SUGG_DEF_DATALEN_TX_OCTETS_MAX, + BLE_HCI_SUGG_DEF_DATALEN_TX_TIME_MAX); } } @@ -3665,15 +4157,30 @@ ble_gap_rx_l2cap_update_req(uint16_t conn_handle, { #if NIMBLE_BLE_CONNECT struct ble_gap_event event; - int rc; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + struct ble_hs_conn *conn; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL && !conn->bhc_connect_delivered) { + ble_hs_unlock(); + /* + * Reject until BLE_GAP_EVENT_CONNECT is delivered. Deferring and + * returning success would auto-accept per ble_l2cap_sig_update_req_rx(). + */ + return BLE_HS_EREJECT; + } + ble_hs_unlock(); +#endif memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_L2CAP_UPDATE_REQ; event.conn_update_req.conn_handle = conn_handle; event.conn_update_req.peer_params = params; - rc = ble_gap_call_conn_event_cb(&event, conn_handle); - return rc; + ble_gap_event_listener_call(&event); + return ble_gap_call_conn_event_cb(&event, conn_handle); #else return BLE_HS_ENOTSUP; #endif @@ -3693,9 +4200,13 @@ ble_gap_rx_phy_update_complete(const struct ble_hci_ev_le_subev_phy_update_compl event.phy_updated.tx_phy = ev->tx_phy; event.phy_updated.rx_phy = ev->rx_phy; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } void @@ -3708,6 +4219,7 @@ ble_gap_rx_data_len_change(const struct ble_hci_ev_le_subev_data_len_chg *ev) memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_DATA_LEN_CHG; + event.data_len_chg.conn_handle = conn_handle; event.data_len_chg.max_tx_octets = le16toh(ev->max_tx_octets); event.data_len_chg.max_rx_octets = le16toh(ev->max_rx_octets); event.data_len_chg.max_tx_time = le16toh(ev->max_tx_time); @@ -3723,9 +4235,13 @@ ble_gap_rx_data_len_change(const struct ble_hci_ev_le_subev_data_len_chg *ev) } ble_hs_unlock(); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } static int32_t @@ -4076,10 +4592,14 @@ ble_gap_adv_stop_no_lock(void) STATS_INC(ble_gap_stats, adv_stop); - active = ble_gap_adv_active(); - BLE_HS_LOG(INFO, "GAP procedure initiated: stop advertising.\n"); + active = ble_gap_adv_active(); + if (!active) { + rc = BLE_HS_EALREADY; + goto done; + } + rc = ble_gap_adv_enable_tx(0); if (rc != 0) { goto done; @@ -4087,12 +4607,6 @@ ble_gap_adv_stop_no_lock(void) ble_gap_slave_reset_state(0); - if (!active) { - rc = BLE_HS_EALREADY; - } else { - rc = 0; - } - done: if (rc != 0) { STATS_INC(ble_gap_stats, adv_stop_fail); @@ -4325,6 +4839,85 @@ ble_gap_adv_validate(uint8_t own_addr_type, const ble_addr_t *peer_addr, } #endif +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static void +ble_gap_apply_deferred_pvcy_add(void) +{ + ble_addr_t addr; + uint8_t irk[16]; + bool replace; + int rc; + + if (!ble_gap_deferred_pvcy_add) { + return; + } + + addr = ble_gap_deferred_pvcy_addr; + memcpy(irk, ble_gap_deferred_pvcy_irk, sizeof irk); + replace = ble_gap_deferred_pvcy_replace; + ble_gap_deferred_pvcy_add = false; + ble_gap_deferred_pvcy_replace = false; + + if (replace) { + /* Entry is expected to be present (e.g. already loaded by + * ble_hs_misc_restore_irks at startup); use replace_entry which + * removes then re-adds so it is idempotent. + */ + rc = ble_hs_pvcy_replace_entry(addr.val, addr.type, irk); + } else { + /* First bond: entry should not be in the resolving list yet. + * Attempt a plain add; if it fails because the entry is already + * present (e.g. a sign-counter bump skipped the remove step to avoid + * a resolution gap), fall back to replace_entry which is idempotent. + * Controllers report CMD_DISALLOWED (0x0C) for a duplicate add per + * Core Spec Vol 4 Part E §7.8.38; guard both that and the older + * INV_HCI_CMD_PARMS (0x12) code seen on some implementations. + */ + rc = ble_hs_pvcy_add_entry(addr.val, addr.type, irk); + if (rc == BLE_HS_HCI_ERR(BLE_ERR_CMD_DISALLOWED) || + rc == BLE_HS_HCI_ERR(BLE_ERR_INV_HCI_CMD_PARMS)) { + rc = ble_hs_pvcy_replace_entry(addr.val, addr.type, irk); + } + } + + if (rc != 0) { + BLE_HS_LOG(WARN, + "failed to add deferred resolving-list entry before advertising; rc=%d\n", + rc); + } +} + +#if NIMBLE_BLE_CONNECT +static void +ble_gap_ensure_peer_rl_entry(const ble_addr_t *peer_addr) +{ + struct ble_store_key_sec key_sec; + struct ble_store_value_sec value_sec; + int rc; + + ble_gap_apply_deferred_pvcy_add(); + + memset(&key_sec, 0, sizeof key_sec); + key_sec.peer_addr = *peer_addr; + rc = ble_store_read_peer_sec(&key_sec, &value_sec); + if (rc != 0 || !value_sec.irk_present) { + return; + } + + rc = ble_hs_pvcy_add_entry(peer_addr->val, peer_addr->type, value_sec.irk); + if (rc == BLE_HS_HCI_ERR(BLE_ERR_CMD_DISALLOWED) || + rc == BLE_HS_HCI_ERR(BLE_ERR_INV_HCI_CMD_PARMS)) { + rc = ble_hs_pvcy_replace_entry(peer_addr->val, peer_addr->type, + value_sec.irk); + } + if (rc != 0) { + BLE_HS_LOG(DEBUG, + "resolving-list sync before set privacy mode; 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, int32_t duration_ms, @@ -4344,6 +4937,10 @@ ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr, return BLE_HS_EDISABLED; } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT @@ -5006,6 +5603,10 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) ble_adv_reattempt.max_events = max_events; #endif +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); if (!ble_gap_slave[instance].configured) { ble_hs_unlock(); @@ -7236,6 +7837,10 @@ ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, return BLE_HS_EDISABLED; } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); rc = ble_gap_disc_ext_validate(own_addr_type); @@ -7409,6 +8014,10 @@ ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms, return BLE_HS_EDISABLED; } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); /* Make a copy of the parameter strcuture and fill unspecified values with @@ -7751,6 +8360,10 @@ ble_gap_connect_with_synced(uint8_t own_addr_type, uint8_t advertising_handle, STATS_INC(ble_gap_stats, initiate); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); if (ble_gap_conn_active()) { @@ -8104,6 +8717,10 @@ ble_gap_ext_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, } #endif // MYNEWT_VAL(OPTIMIZE_MULTI_CONN) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); if (ble_gap_conn_active()) { @@ -8291,6 +8908,10 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, } #endif // MYNEWT_VAL(OPTIMIZE_MULTI_CONN) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_gap_apply_deferred_pvcy_add(); +#endif + ble_hs_lock(); if (ble_gap_conn_active()) { @@ -8913,8 +9534,6 @@ ble_gap_rx_param_req(const struct ble_hci_ev_le_subev_rem_conn_param_req *ev) uint16_t conn_handle; int rc; - memset(&event, 0, sizeof event); - peer_params.itvl_min = le16toh(ev->min_interval); peer_params.itvl_max = le16toh(ev->max_interval); peer_params.latency = le16toh(ev->latency); @@ -8935,6 +9554,48 @@ ble_gap_rx_param_req(const struct ble_hci_ev_le_subev_rem_conn_param_req *ev) event.conn_update_req.conn_handle = conn_handle; event.conn_update_req.self_params = &self_params; event.conn_update_req.peer_params = &peer_params; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + { + struct ble_gap_defer_meta meta; + struct ble_hs_conn *conn; + enum ble_gap_defer_result defer_rc; + bool defer_app_cb; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + defer_app_cb = (conn != NULL && !conn->bhc_connect_delivered); + ble_hs_unlock(); + + if (defer_app_cb) { + memset(&meta, 0, sizeof meta); + meta.upd_peer = &peer_params; + meta.upd_self = &self_params; + + defer_rc = ble_gap_defer_event(conn_handle, &event, &meta); + if (defer_rc == BLE_GAP_DEFER_QUEUED) { + /* + * Vol 6 Part B §5.1.7.2 / HCI §7.7.65.6: reply to the Controller + * after queuing; deliver the application callback when the + * deferred events are drained. + */ + rc = ble_gap_tx_param_pos_reply(conn_handle, &self_params); + if (rc != 0) { + ble_gap_update_failed(conn_handle, rc); + } + return; + } + if (defer_rc == BLE_GAP_DEFER_FAILED) { + BLE_HS_LOG(ERROR, + "ble_gap_rx_param_req: defer failed; conn=%u\n", + conn_handle); + } + + /* NOT_NEEDED or FAILED: fall through to the normal path. */ + } + } +#endif + rc = ble_gap_call_conn_event_cb(&event, conn_handle); if (rc == 0) { rc = ble_gap_tx_param_pos_reply(conn_handle, &self_params); @@ -9597,8 +10258,25 @@ ble_gap_passkey_event(uint16_t conn_handle, event.type = BLE_GAP_EVENT_PASSKEY_ACTION; event.passkey.conn_handle = conn_handle; event.passkey.params = *passkey_params; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + switch (ble_gap_defer_event(conn_handle, &event, NULL)) { + case BLE_GAP_DEFER_QUEUED: + return; + case BLE_GAP_DEFER_NOT_NEEDED: + ble_gap_call_conn_event_cb(&event, conn_handle); + break; + case BLE_GAP_DEFER_FAILED: + BLE_HS_LOG(ERROR, + "ble_gap_passkey_event: defer failed; conn=%u\n", + conn_handle); + ble_gap_call_conn_event_cb(&event, conn_handle); + break; + } +#else ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } void @@ -9613,6 +10291,33 @@ ble_gap_enc_event(uint16_t conn_handle, int status, event.enc_change.conn_handle = conn_handle; event.enc_change.status = status; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + { + struct ble_gap_defer_meta meta; + + memset(&meta, 0, sizeof meta); + meta.enc_security_restored = security_restored; + meta.enc_bonded = bonded; + + switch (ble_gap_defer_event(conn_handle, &event, &meta)) { + case BLE_GAP_DEFER_QUEUED: + return; + case BLE_GAP_DEFER_NOT_NEEDED: + break; + case BLE_GAP_DEFER_FAILED: + BLE_HS_LOG(ERROR, + "ble_gap_enc_event: defer failed; conn=%u status=%d\n", + conn_handle, status); + break; + } + } +#endif + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_event_listener_call(&event); + ble_gap_call_conn_event_cb(&event, conn_handle); + ble_gap_enc_side_effects(conn_handle, status, security_restored, bonded); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); @@ -9650,6 +10355,7 @@ ble_gap_enc_event(uint16_t conn_handle, int status, } #endif +#endif /* MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) */ #endif } @@ -9666,8 +10372,14 @@ ble_gap_identity_event(uint16_t conn_handle, const ble_addr_t *peer_id_addr) event.type = BLE_GAP_EVENT_IDENTITY_RESOLVED; event.identity_resolved.conn_handle = conn_handle; event.identity_resolved.peer_id_addr = *peer_id_addr; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else + ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } int @@ -9706,8 +10418,25 @@ ble_gap_pairing_complete_event(uint16_t conn_handle, int status) event.type = BLE_GAP_EVENT_PARING_COMPLETE; event.pairing_complete.conn_handle = conn_handle; event.pairing_complete.status = status; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + switch (ble_gap_defer_event(conn_handle, &event, NULL)) { + case BLE_GAP_DEFER_QUEUED: + return; + case BLE_GAP_DEFER_NOT_NEEDED: + ble_gap_call_conn_event_cb(&event, conn_handle); + break; + case BLE_GAP_DEFER_FAILED: + BLE_HS_LOG(ERROR, + "ble_gap_pairing_complete_event: defer failed; conn=%u\n", + conn_handle); + ble_gap_call_conn_event_cb(&event, conn_handle); + break; + } +#else ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } /***************************************************************************** * $cache * @@ -9725,8 +10454,24 @@ ble_gap_assoc_event(uint16_t conn_handle, int status, uint8_t cache_state) event.cache_assoc.status = status; event.cache_assoc.cache_state = cache_state; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + switch (ble_gap_defer_event(conn_handle, &event, NULL)) { + case BLE_GAP_DEFER_QUEUED: + return; + case BLE_GAP_DEFER_NOT_NEEDED: + ble_gap_call_conn_event_cb(&event, conn_handle); + break; + case BLE_GAP_DEFER_FAILED: + BLE_HS_LOG(ERROR, + "ble_gap_assoc_event: defer failed; conn=%u\n", + conn_handle); + ble_gap_call_conn_event_cb(&event, conn_handle); + break; + } +#else ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } #endif @@ -9758,12 +10503,17 @@ ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle, #if (MYNEWT_VAL(BLE_GATT_NOTIFY) || MYNEWT_VAL(BLE_GATT_INDICATE)) && NIMBLE_BLE_CONNECT struct ble_gap_event event; + #if MYNEWT_VAL(BLE_GATT_CACHING) && MYNEWT_VAL(BLE_GATTC) uint16_t start_handle; uint16_t end_handle; uint16_t svc_changed_handle = ble_gattc_cache_conn_get_svc_changed_handle(conn_handle); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + if (svc_changed_handle != 0 && attr_handle == svc_changed_handle && OS_MBUF_PKTLEN(om) >= 4) { +#else if (svc_changed_handle != 0xFFFF && attr_handle == svc_changed_handle && OS_MBUF_PKTLEN(om) >= 4) { +#endif uint8_t buf[4]; os_mbuf_copydata(om, 0, 4, buf); start_handle = get_le16(buf); @@ -9771,12 +10521,30 @@ ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle, ble_gattc_cache_conn_update(conn_handle, start_handle, end_handle); } #endif + memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_NOTIFY_RX; event.notify_rx.conn_handle = conn_handle; event.notify_rx.attr_handle = attr_handle; event.notify_rx.om = om; event.notify_rx.indication = is_indication; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + { + enum ble_gap_defer_result defer_rc; + + defer_rc = ble_gap_defer_event(conn_handle, &event, NULL); + if (defer_rc == BLE_GAP_DEFER_QUEUED) { + return; + } + if (defer_rc == BLE_GAP_DEFER_FAILED) { + BLE_HS_LOG(ERROR, + "ble_gap_notify_rx_event: defer failed; conn=%u\n", + conn_handle); + } + } +#endif + ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); @@ -9797,9 +10565,14 @@ ble_gap_notify_tx_event(int status, uint16_t conn_handle, uint16_t attr_handle, event.notify_tx.status = status; event.notify_tx.attr_handle = attr_handle; event.notify_tx.indication = is_indication; + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } /***************************************************************************** @@ -9831,9 +10604,13 @@ ble_gap_subscribe_event(uint16_t conn_handle, uint16_t attr_handle, event.subscribe.prev_indicate = !!prev_indicate; event.subscribe.cur_indicate = !!cur_indicate; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } /***************************************************************************** @@ -9852,9 +10629,13 @@ ble_gap_mtu_event(uint16_t conn_handle, uint16_t cid, uint16_t mtu) event.mtu.channel_id = cid; event.mtu.value = mtu; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_gap_notify_conn_event(conn_handle, &event); +#else ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif +#endif } #if MYNEWT_VAL(BLE_HCI_VS) diff --git a/nimble/host/src/ble_gap_priv.h b/nimble/host/src/ble_gap_priv.h index bf5bef24a..8afdc1699 100644 --- a/nimble/host/src/ble_gap_priv.h +++ b/nimble/host/src/ble_gap_priv.h @@ -35,6 +35,7 @@ struct hci_disconn_complete; struct hci_encrypt_change; struct ble_hs_hci_ack; struct ble_hs_adv; +struct ble_hs_conn; STATS_SECT_START(ble_gap_stats) STATS_SECT_ENTRY(wl_set) @@ -97,6 +98,10 @@ void ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc); void ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used_feat *ev); void ble_gap_rx_rd_rem_ver_info_complete(const struct ble_hci_ev_rd_rem_ver_info_cmp *ev); void ble_gap_event_connect_call(uint16_t conn_handle, int status); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && NIMBLE_BLE_CONNECT +void ble_gap_conn_deferred_init(struct ble_hs_conn *conn); +void ble_gap_conn_deferred_cleanup(struct ble_hs_conn *conn); +#endif #if MYNEWT_VAL(BLE_CONN_SUBRATING) void ble_gap_rx_subrate_change(const struct ble_hci_ev_le_subev_subrate_change *ev); #endif diff --git a/nimble/host/src/ble_gattc_cache_conn.c b/nimble/host/src/ble_gattc_cache_conn.c index 8b760f93a..5c2fb10b6 100644 --- a/nimble/host/src/ble_gattc_cache_conn.c +++ b/nimble/host/src/ble_gattc_cache_conn.c @@ -2222,7 +2222,11 @@ ble_gattc_cache_conn_get_svc_changed_handle(uint16_t conn_handle) peer = ble_gattc_cache_conn_find(conn_handle); if (peer == NULL) { BLE_HS_LOG(ERROR, "Cannot find connection with conn_handle %d", conn_handle); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + return 0; +#else return -1; +#endif } /* Check if attr_handle is of service change char */ @@ -2231,7 +2235,11 @@ ble_gattc_cache_conn_get_svc_changed_handle(uint16_t conn_handle) if (chr == NULL) { BLE_HS_LOG(DEBUG, "Cannot find service change characteristic"); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + return 0; +#else return -1; +#endif } return chr->chr.val_handle; } diff --git a/nimble/host/src/ble_hs_conn.c b/nimble/host/src/ble_hs_conn.c index ffbe9be64..52785790c 100644 --- a/nimble/host/src/ble_hs_conn.c +++ b/nimble/host/src/ble_hs_conn.c @@ -207,6 +207,15 @@ ble_hs_conn_alloc(uint16_t conn_handle) memset(conn, 0, sizeof *conn); conn->bhc_handle = conn_handle; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + STAILQ_INIT(&conn->bhc_deferred_events); + conn->bhc_deferred_event_cnt = 0; + conn->bhc_deferred_att_cnt = 0; + conn->bhc_deferred_seq = 0; + STAILQ_INIT(&conn->bhc_deferred_att_reqs); + ble_gap_conn_deferred_init(conn); +#endif + SLIST_INIT(&conn->bhc_channels); chan = ble_att_create_chan(conn_handle); @@ -323,6 +332,10 @@ ble_hs_conn_free(struct ble_hs_conn *conn) os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && NIMBLE_BLE_CONNECT + ble_gap_conn_deferred_cleanup(conn); +#endif + #if MYNEWT_VAL(BLE_HS_DEBUG) memset(conn, 0xff, sizeof *conn); #endif diff --git a/nimble/host/src/ble_hs_conn_priv.h b/nimble/host/src/ble_hs_conn_priv.h index 28dfb952a..f5e7c3c28 100644 --- a/nimble/host/src/ble_hs_conn_priv.h +++ b/nimble/host/src/ble_hs_conn_priv.h @@ -38,6 +38,37 @@ typedef uint8_t ble_hs_conn_flags_t; #define BLE_HS_CONN_F_TERMINATING 0x02 #define BLE_HS_CONN_F_TX_FRAG 0x04 /* Cur ACL packet partially txed. */ +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +#include "host/ble_gap.h" + +/** + * GAP event deferred until BLE_GAP_EVENT_CONNECT has been delivered. + * Queued on ble_hs_conn.bhc_deferred_events. + */ +struct ble_gap_deferred_event { + STAILQ_ENTRY(ble_gap_deferred_event) next; + uint16_t seq; + struct ble_gap_event event; + uint8_t enc_security_restored; + uint8_t enc_bonded; + struct ble_gap_upd_params upd_peer; + struct ble_gap_upd_params upd_self; +}; + +/** + * L2CAP ATT payload deferred until BLE_GAP_EVENT_CONNECT has been delivered. + * Queued on ble_hs_conn.bhc_deferred_att_reqs. + */ +struct ble_att_deferred_req { + STAILQ_ENTRY(ble_att_deferred_req) next; + struct os_mbuf *om; + uint16_t cid; + uint16_t seq; +}; + +#define BLE_HS_CONN_DEFERRED_EVENT_MAX 64 +#endif + #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) #define BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN_REM \ ((MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) % (8 * sizeof(uint32_t))) ? 1 : 0) @@ -51,6 +82,23 @@ struct ble_hs_conn { SLIST_ENTRY(ble_hs_conn) bhc_next; uint8_t bhc_our_addr_type; uint8_t slave_conn : 1; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + uint8_t bhc_connect_delivered : 1; + uint8_t bhc_deferred_draining : 1; + uint8_t bhc_deferred_inited : 1; + uint8_t bhc_deferred_event_cnt; + uint8_t bhc_deferred_att_cnt; + uint16_t bhc_deferred_seq; + STAILQ_HEAD(, ble_gap_deferred_event) bhc_deferred_events; + STAILQ_HEAD(, ble_att_deferred_req) bhc_deferred_att_reqs; + struct ble_npl_event bhc_deferred_drain_ev; +#if MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + uint8_t bhc_deferred_pvcy_add; + uint8_t bhc_deferred_pvcy_replace; + ble_addr_t bhc_deferred_pvcy_addr; + uint8_t bhc_deferred_pvcy_irk[16]; +#endif +#endif #if MYNEWT_VAL(BLE_EXT_ADV) uint8_t bhc_our_rnd_addr[6]; #endif diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index 35fdbae7b..05edcb722 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/nimble/host/src/ble_hs_pvcy.c @@ -125,17 +125,57 @@ void ble_hs_reset_rpa_timeout(void) } #if (!MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +static uint8_t ble_hs_pvcy_resolve_en; +#endif + int ble_hs_pvcy_set_resolve_enabled(int enable) { struct ble_hci_le_set_addr_res_en_cp cmd; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + int rc; + cmd.enable = !!enable; + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SET_ADDR_RES_EN), + &cmd, sizeof(cmd), NULL, 0); + if (rc == 0) { + ble_hs_pvcy_resolve_en = cmd.enable; + } + + return rc; +#else cmd.enable = enable; return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN), &cmd, sizeof(cmd), NULL, 0); +#endif } + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +static void +ble_hs_pvcy_restore_resolve_if_needed(uint8_t was_enabled) +{ + int rc_en; + + if (!was_enabled) { + return; + } + + rc_en = BLE_HS_EUNKNOWN; + for (int i = 0; i < 3 && rc_en != 0; i++) { + rc_en = ble_hs_pvcy_set_resolve_enabled(1); + } + if (rc_en != 0) { + BLE_HS_LOG(ERROR, + "ble_hs_pvcy: address resolution restore failed after " + "retries; privacy broken\n"); + } +} +#endif #endif int @@ -227,6 +267,41 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, return 0; } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +int +ble_hs_pvcy_replace_entry(const uint8_t *addr, uint8_t addr_type, + const uint8_t *irk) +{ +#if (MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) + (void)ble_hs_pvcy_remove_entry(addr_type, addr); + return ble_hs_pvcy_add_entry(addr, addr_type, irk); +#else + int rc; + uint8_t resolve_was_en; + + STATS_INC(ble_hs_stats, pvcy_add_entry); + + ble_gap_preempt(); + resolve_was_en = ble_hs_pvcy_resolve_en; + + rc = ble_hs_pvcy_set_resolve_enabled(0); + if (rc == 0) { + (void)ble_hs_pvcy_remove_entry(addr_type, addr); + rc = ble_hs_pvcy_add_entry_hci(addr, addr_type, irk); + ble_hs_pvcy_restore_resolve_if_needed(resolve_was_en); + } + + ble_gap_preempt_done(); + + if (rc != 0) { + STATS_INC(ble_hs_stats, pvcy_add_entry_fail); + } + + return rc; +#endif +} +#endif /* MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) */ + int ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type, const uint8_t *irk) @@ -244,8 +319,20 @@ ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type, #else ble_gap_preempt(); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + uint8_t resolve_was_en; + + resolve_was_en = ble_hs_pvcy_resolve_en; + rc = ble_hs_pvcy_set_resolve_enabled(0); + if (rc == 0) { + /* Try to add the entry now that GAP is halted and address resolution is disabled. */ + rc = ble_hs_pvcy_add_entry_hci(addr, addr_type, irk); + ble_hs_pvcy_restore_resolve_if_needed(resolve_was_en); + } +#else /* Try to add the entry now that GAP is halted. */ rc = ble_hs_pvcy_add_entry_hci(addr, addr_type, irk); +#endif /* Allow GAP procedures to be started again. */ ble_gap_preempt_done(); diff --git a/nimble/host/src/ble_hs_pvcy_priv.h b/nimble/host/src/ble_hs_pvcy_priv.h index 3167fdc9b..7c7d16777 100644 --- a/nimble/host/src/ble_hs_pvcy_priv.h +++ b/nimble/host/src/ble_hs_pvcy_priv.h @@ -39,6 +39,10 @@ int ble_hs_pvcy_our_irk(const uint8_t **out_irk); int ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr); int ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addrtype, const uint8_t *irk); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) +int ble_hs_pvcy_replace_entry(const uint8_t *addr, uint8_t addr_type, + const uint8_t *irk); +#endif int ble_hs_pvcy_ensure_started(void); int ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode); #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index 4f10d3687..eb2e916d7 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -996,6 +996,22 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle, int rc; do { +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + int defer_repeat_chk; + + defer_repeat_chk = 0; + ble_hs_lock(); + { + struct ble_hs_conn *conn; + + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL && !conn->bhc_connect_delivered) { + defer_repeat_chk = 1; + } + } + ble_hs_unlock(); +#endif + /* If the peer isn't bonded, indicate that the pairing procedure should * continue. */ @@ -1009,6 +1025,25 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle, return rc; } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + /* + * Repeat pairing needs a synchronous app decision and cannot be + * deferred. Reject pairing from bonded peers until CONNECT is + * delivered so the bond is not overwritten without consent. + */ + if (defer_repeat_chk) { + struct ble_hs_conn *conn; + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL && !conn->bhc_connect_delivered) { + ble_hs_unlock(); + return BLE_HS_EBUSY; + } + ble_hs_unlock(); + } +#endif + /* Peer is already bonded. Ask the application what to do about it. */ rp.conn_handle = conn_handle; rp.cur_key_size = bond.key_size; @@ -1122,7 +1157,12 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res, ble_sm_persist_keys(proc); } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + if (res->enc_cb && + res->app_status != BLE_HS_ENOTCONN) { +#else if (res->enc_cb) { +#endif BLE_HS_DBG_ASSERT(proc == NULL || rm); ble_gap_enc_event(conn_handle, res->app_status, res->restore, res->bonded); } @@ -2081,6 +2121,13 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, /* The app indicated that the pairing request should be ignored. */ res->app_status = rc; res->execute = 0; + if (rc == BLE_HS_EBUSY) { + /* + * CONNECT not yet delivered; send Pairing Failed so the peer + * does not sit in SMP until the 30 s timer expires. + */ + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + } } } } @@ -2885,12 +2932,22 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle) } #if MYNEWT_VAL(BLE_HS_PVCY) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + /* + * Only the sign counter is being bumped; the IRK is unchanged. + * The controller resolving list stores the IRK, not the sign counter, + * so there is no reason to remove and re-add the entry here. + * ble_store_write_peer_sec will update the resolving list entry at + * disconnect (deferred path) or immediately if the peer is not connected. + */ +#else if (value_sec.irk_present == 1) { ble_hs_pvcy_remove_entry(value_sec.peer_addr.type, value_sec.peer_addr.val); /* No need to check if the above command fails or passes. * Proceed with trying to write the new sign counter. */ } +#endif #endif value_sec.sign_counter += 1; diff --git a/nimble/host/src/ble_store.c b/nimble/host/src/ble_store.c index 2e286aa1d..bd1ea2b3d 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -283,6 +283,20 @@ ble_store_write_peer_sec(const struct ble_store_value_sec *value_sec) #if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) int rc; +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) + struct ble_store_key_sec key_sec; + struct ble_store_value_sec old_sec; + int replace_entry; + + replace_entry = 0; + if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) && + value_sec->irk_present) { + memset(&key_sec, 0, sizeof key_sec); + key_sec.peer_addr = value_sec->peer_addr; + rc = ble_store_read_peer_sec(&key_sec, &old_sec); + replace_entry = rc == 0 && old_sec.irk_present; + } +#endif /* BLE_DEFER_CONN_EVENTS && BLE_HS_PVCY */ rc = ble_store_persist_sec(BLE_STORE_OBJ_TYPE_PEER_SEC, value_sec); if (rc != 0) { @@ -292,11 +306,56 @@ ble_store_write_peer_sec(const struct ble_store_value_sec *value_sec) if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) && value_sec->irk_present) { #if MYNEWT_VAL(BLE_HS_PVCY) +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + /* Do not update the controller resolving list while this peer is still + * connected. The bond is already persisted for the current link, and + * some controllers reject LE Add Device To Resolving List in this + * state even when GAP is preempted. + */ +#if !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_hs_lock(); + struct ble_hs_conn *conn = ble_hs_conn_find_by_addr(&value_sec->peer_addr); + if (conn != NULL) { + conn->bhc_deferred_pvcy_add = 1; + conn->bhc_deferred_pvcy_replace = replace_entry; + conn->bhc_deferred_pvcy_addr = value_sec->peer_addr; + memcpy(conn->bhc_deferred_pvcy_irk, value_sec->irk, + sizeof conn->bhc_deferred_pvcy_irk); + ble_hs_unlock(); + return 0; + } + ble_hs_unlock(); +#endif + + /* Write the peer IRK to the controller keycache + * There is not much to do here if it fails */ + if (replace_entry) { + rc = ble_hs_pvcy_replace_entry(value_sec->peer_addr.val, + value_sec->peer_addr.type, + value_sec->irk); + } else { + rc = ble_hs_pvcy_add_entry(value_sec->peer_addr.val, + value_sec->peer_addr.type, + value_sec->irk); + if (rc == BLE_HS_EINVAL || + rc == BLE_HS_HCI_ERR(BLE_ERR_CMD_DISALLOWED) || + rc == BLE_HS_HCI_ERR(BLE_ERR_INV_HCI_CMD_PARMS)) { + /* Entry already present in resolving list; replace it. + * Occurs when the store entry was deleted and re-written + * without first removing the resolving list entry (e.g. + * sign-counter increment with BLE_HOST_BASED_PRIVACY=1). */ + rc = ble_hs_pvcy_replace_entry(value_sec->peer_addr.val, + value_sec->peer_addr.type, + value_sec->irk); + } + } +#else /* Write the peer IRK to the controller keycache * There is not much to do here if it fails */ rc = ble_hs_pvcy_add_entry(value_sec->peer_addr.val, - value_sec->peer_addr.type, - value_sec->irk); + value_sec->peer_addr.type, + value_sec->irk); +#endif /* MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) */ if (rc != 0) { return rc; } diff --git a/nimble/host/test/src/ble_hs_pvcy_test.c b/nimble/host/test/src/ble_hs_pvcy_test.c index 7fd357d05..2c6650714 100644 --- a/nimble/host/test/src/ble_hs_pvcy_test.c +++ b/nimble/host/test/src/ble_hs_pvcy_test.c @@ -110,10 +110,18 @@ ble_hs_pvcy_test_util_add_irk_set_acks(bool scanning, bool connecting) 0); } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_hs_test_util_hci_ack_append( + BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN), 0); +#endif ble_hs_test_util_hci_ack_append( BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST), 0); ble_hs_test_util_hci_ack_append( BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PRIVACY_MODE), 0); +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_hs_test_util_hci_ack_append( + BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN), 0); +#endif } static void @@ -167,6 +175,12 @@ ble_hs_pvcy_test_util_add_irk_verify_tx(const ble_addr_t *peer_addr, NULL); } +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SET_ADDR_RES_EN, + NULL); +#endif + ble_hs_test_util_hci_verify_tx_add_irk(peer_addr->type, peer_addr->val, peer_irk, @@ -175,6 +189,12 @@ ble_hs_pvcy_test_util_add_irk_verify_tx(const ble_addr_t *peer_addr, ble_hs_test_util_hci_verify_tx_set_priv_mode(peer_addr->type, peer_addr->val, BLE_GAP_PRIVATE_MODE_DEVICE); + +#if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) + ble_hs_test_util_hci_verify_tx(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SET_ADDR_RES_EN, + NULL); +#endif } static void @@ -192,7 +212,7 @@ ble_hs_pvcy_test_util_add_irk(const ble_addr_t *peer_addr, rc = ble_hs_pvcy_add_entry(peer_addr->val, peer_addr->type, peer_irk); TEST_ASSERT_FATAL(rc == 0); - num_acks = 3; + num_acks = MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) ? 5 : 3; if (scanning) { num_acks++; } diff --git a/porting/npl/freertos/src/npl_os_freertos.c b/porting/npl/freertos/src/npl_os_freertos.c index ad402f095..d665c87ad 100644 --- a/porting/npl/freertos/src/npl_os_freertos.c +++ b/porting/npl/freertos/src/npl_os_freertos.c @@ -42,6 +42,7 @@ portMUX_TYPE ble_port_mutex = portMUX_INITIALIZER_UNLOCKED; static SemaphoreHandle_t npl_eventq_sync; +static uint8_t hw_critical_state_status[portNUM_PROCESSORS]; #if CONFIG_BT_NIMBLE_USE_ESP_TIMER static const char *TAG = "Timer"; @@ -357,25 +358,35 @@ static void npl_eventq_sync_init(void) { if (npl_eventq_sync == NULL) { - npl_eventq_sync = xSemaphoreCreateMutex(); + npl_eventq_sync = xSemaphoreCreateRecursiveMutex(); BLE_LL_ASSERT(npl_eventq_sync); } } -static void +static bool npl_eventq_lock(void) { - if (!in_isr()) { - BLE_LL_ASSERT(npl_eventq_sync); - xSemaphoreTake(npl_eventq_sync, portMAX_DELAY); + BaseType_t core; + + if (in_isr()) { + return false; } + + core = xPortGetCoreID(); + if (core >= portNUM_PROCESSORS || hw_critical_state_status[core] != 0) { + return false; + } + + BLE_LL_ASSERT(npl_eventq_sync); + xSemaphoreTakeRecursive(npl_eventq_sync, portMAX_DELAY); + return true; } static void -npl_eventq_unlock(void) +npl_eventq_unlock(bool locked) { - if (!in_isr()) { - xSemaphoreGive(npl_eventq_sync); + if (locked) { + xSemaphoreGiveRecursive(npl_eventq_sync); } } @@ -485,7 +496,8 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) } } } else if (tmo == 0) { - npl_eventq_lock(); + bool locked = npl_eventq_lock(); + portENTER_CRITICAL(&ble_port_mutex); ret = xQueueReceive(eventq->q, &ev, 0); if (ret == pdPASS && ev != NULL) { @@ -495,7 +507,7 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) } } portEXIT_CRITICAL(&ble_port_mutex); - npl_eventq_unlock(); + npl_eventq_unlock(locked); } else { TickType_t deadline = 0; TickType_t remaining; @@ -518,7 +530,8 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) return NULL; } - npl_eventq_lock(); + bool locked = npl_eventq_lock(); + portENTER_CRITICAL(&ble_port_mutex); ret = xQueueReceive(eventq->q, &ev, 0); if (ret == pdPASS && ev != NULL) { @@ -526,12 +539,13 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) if (event) { event->queued = false; } - portEXIT_CRITICAL(&ble_port_mutex); - npl_eventq_unlock(); - break; } portEXIT_CRITICAL(&ble_port_mutex); - npl_eventq_unlock(); + if (ret == pdPASS && ev != NULL) { + npl_eventq_unlock(locked); + break; + } + npl_eventq_unlock(locked); } } @@ -561,10 +575,10 @@ npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) } return; } else { - npl_eventq_lock(); + bool locked = npl_eventq_lock(); if (npl_eventq_queued_claim(event)) { - npl_eventq_unlock(); + npl_eventq_unlock(locked); return; } @@ -573,7 +587,7 @@ npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) ESP_LOGW("NimBLE", "eventq put: queue full, event dropped"); npl_eventq_queued_set_task(event, false); } - npl_eventq_unlock(); + npl_eventq_unlock(locked); } } @@ -637,9 +651,10 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq, } else { removed = false; - npl_eventq_lock(); + bool locked = npl_eventq_lock(); + if (!npl_eventq_queued_get_task(event)) { - npl_eventq_unlock(); + npl_eventq_unlock(locked); return; } @@ -663,11 +678,10 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq, } } if (removed) { - event->queued = 0; + event->queued = false; } portEXIT_CRITICAL(&ble_port_mutex); - - npl_eventq_unlock(); + npl_eventq_unlock(locked); } } @@ -1341,26 +1355,40 @@ npl_freertos_hw_set_isr(int irqn, uint32_t addr) #endif -uint8_t hw_critical_state_status = 0; - uint32_t npl_freertos_hw_enter_critical(void) { - ++hw_critical_state_status; + BaseType_t core; + portENTER_CRITICAL(&ble_port_mutex); + core = xPortGetCoreID(); + if (core < portNUM_PROCESSORS) { + ++hw_critical_state_status[core]; + } return 0; } uint8_t npl_freertos_hw_is_in_critical(void) { - return hw_critical_state_status; + BaseType_t core; + + core = xPortGetCoreID(); + if (core >= portNUM_PROCESSORS) { + return 0; + } + return hw_critical_state_status[core]; } void npl_freertos_hw_exit_critical(uint32_t ctx) { - --hw_critical_state_status; + BaseType_t core; + + core = xPortGetCoreID(); + if (core < portNUM_PROCESSORS && hw_critical_state_status[core] > 0) { + --hw_critical_state_status[core]; + } portEXIT_CRITICAL(&ble_port_mutex); }