diff --git a/apps/btshell/src/cmd.c b/apps/btshell/src/cmd.c index 4148b8e8a..7796bff7f 100644 --- a/apps/btshell/src/cmd.c +++ b/apps/btshell/src/cmd.c @@ -1845,7 +1845,11 @@ cmd_set_adv_data_or_scan_rsp(int argc, char **argv, bool scan_rsp, rc = ble_gap_ext_adv_rsp_set_data(instance, adv_data); #if MYNEWT_VAL(BLE_PERIODIC_ADV) } else if (periodic) { +#if MYNEWT_VAL(BLE_PERIODIC_ADV_ENH) + rc = ble_gap_periodic_adv_set_data(instance, adv_data, NULL); +#else rc = ble_gap_periodic_adv_set_data(instance, adv_data); +#endif #endif } else { rc = ble_gap_ext_adv_set_data(instance, adv_data); diff --git a/nimble/controller/src/ble_ll_sync.c b/nimble/controller/src/ble_ll_sync.c index 8aef8e2c8..82e84d4eb 100644 --- a/nimble/controller/src/ble_ll_sync.c +++ b/nimble/controller/src/ble_ll_sync.c @@ -1898,7 +1898,7 @@ ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len) return BLE_ERR_UNK_ADV_INDENT; } - if (cmd->enable) { + if (cmd->enable & 0x01) { sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DISABLED; } else { sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED; diff --git a/nimble/host/include/host/ble_ead.h b/nimble/host/include/host/ble_ead.h index 8d6b60821..4a1c9ba3c 100644 --- a/nimble/host/include/host/ble_ead.h +++ b/nimble/host/include/host/ble_ead.h @@ -88,9 +88,10 @@ struct key_material { * * @return 0 on success; * BLE_HS_EINVAL if the specified value is not - * within the allowed range. - * BLE_HS_ECANCEL if error occurred during the random number - * generation + * within the allowed range; + * other nonzero BLE host error codes on internal + * failure (e.g., HCI error during randomizer + * generation). */ int ble_ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t iv[BLE_EAD_IV_SIZE], const uint8_t *payload, diff --git a/nimble/host/include/host/ble_esp_gap.h b/nimble/host/include/host/ble_esp_gap.h index e4a95c922..0b5df37b2 100644 --- a/nimble/host/include/host/ble_esp_gap.h +++ b/nimble/host/include/host/ble_esp_gap.h @@ -347,8 +347,10 @@ ble_gap_dev_authorization(uint16_t conn_handle, bool authorized); * @return 0 on success; * A nonzero value indicating an error if the command fails. */ +#if MYNEWT_VAL(BLE_HCI_VS) int ble_hs_send_vs_event_mask(uint32_t event_mask); +#endif void ble_gap_rx_test_evt(const void *buf, uint8_t len); void ble_gap_tx_test_evt(const void *buf, uint8_t len); diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h index 66ab3704a..34a833dd3 100644 --- a/nimble/host/include/host/ble_gap.h +++ b/nimble/host/include/host/ble_gap.h @@ -2749,14 +2749,6 @@ struct ble_gap_set_periodic_adv_subev_data_params { }; #endif -#if MYNEWT_VAL(BLE_ADV_SEND_CONSTANT_DID) -struct ble_gap_adv_const_did_cmd_params { - uint16_t handle; - uint8_t enable; - uint16_t did; -}; -#endif // MYNEWT_VAL(BLE_ADV_SEND_CONSTANT_DID) - /** * Configure periodic advertising for specified advertising instance * @@ -3553,37 +3545,6 @@ int ble_gap_read_sugg_def_data_len(uint16_t *out_sugg_max_tx_octets, int ble_gap_write_sugg_def_data_len(uint16_t sugg_max_tx_octets, uint16_t sugg_max_tx_time); -/** - * Read LE Suggested Default Data Length in controller (OGF = 0x08, OCF = 0x0024). - * - * @param out_sugg_max_tx_octets The Host's suggested value for the Controller's maximum transmitted - * number of payload octets in LL Data PDUs to be used for new - * connections. (Range 0x001B-0x00FB). - * @param out_sugg_max_tx_time The Host's suggested value for the Controller's maximum packet - * transmission time for packets containing LL Data PDUs to be used - * for new connections. (Range 0x0148-0x4290). - * - * @return 0 on success, - * other error code on failure. - */ -int ble_gap_read_sugg_def_data_len(uint16_t *out_sugg_max_tx_octets, - uint16_t *out_sugg_max_tx_time); - -/** - * Configure LE Suggested Default Data Length in controller (OGF = 0x08, OCF = 0x0024). - * - * @param sugg_max_tx_octets The Host's suggested value for the Controller's maximum transmitted - * number of payload octets in LL Data PDUs to be used for new - * connections. (Range 0x001B-0x00FB). - * @param sugg_max_tx_time The Host's suggested value for the Controller's maximum packet - * transmission time for packets containing LL Data PDUs to be used - * for new connections. (Range 0x0148-0x4290). - * - * @return 0 on success, - * other error code on failure. - */ -int ble_gap_write_sugg_def_data_len(uint16_t sugg_max_tx_octets, uint16_t sugg_max_tx_time); - /** * Initiates the GAP security procedure. * @@ -4224,7 +4185,6 @@ int ble_gap_rd_local_resolv_addr(uint8_t peer_addr_type, const ble_addr_t *peer_ */ int ble_gap_read_local_irk(uint8_t * out_irk); -#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) /** * Set or clear a bit controlled by the host in the link layer featureSet * stored in the Controller @@ -4236,7 +4196,9 @@ int ble_gap_read_local_irk(uint8_t * out_irk); * return 0 on success; nonzero on failure */ int ble_gap_set_host_feat(uint8_t bit_num,uint8_t bit_val); -#endif + +int ble_gap_rd_all_local_supp_features(uint8_t *status, uint8_t *max_page, uint8_t *le_features); +int ble_gap_rd_all_remote_features(uint16_t conn_handle, uint8_t page_requested); #if MYNEWT_VAL(BLE_FRAME_SPACE_UPDATE) /* * Request a Frame Space Update. diff --git a/nimble/host/include/host/ble_hs_hci.h b/nimble/host/include/host/ble_hs_hci.h index 0be6a2ee7..25ac6040b 100644 --- a/nimble/host/include/host/ble_hs_hci.h +++ b/nimble/host/include/host/ble_hs_hci.h @@ -102,6 +102,17 @@ int ble_hs_hci_set_chan_class(const uint8_t *chan_map); */ int ble_hs_hci_util_rand(void *dst, int len); +/** + * Returns the number of available ACL transmit buffers on the controller. + * + * Applications can use this to implement notification throttling — only + * enqueue new notifications when available buffers are above a waterline. + * + * @return The number of ACL packet buffers the controller can currently + * accept from the host. + */ +uint16_t ble_hs_hci_get_avail_pkts(void); + #if MYNEWT_VAL(BLE_HCI_VS) /** * Send an arbitrary HCI command to the controller. diff --git a/nimble/host/include/host/ble_hs_pvcy.h b/nimble/host/include/host/ble_hs_pvcy.h index 1769b8f71..be9c2b169 100644 --- a/nimble/host/include/host/ble_hs_pvcy.h +++ b/nimble/host/include/host/ble_hs_pvcy.h @@ -68,7 +68,9 @@ int ble_hs_pvcy_rpa_config(uint8_t enable); void ble_hs_resolv_deinit(void); #endif +#if (!MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) int ble_hs_pvcy_set_resolve_enabled(int enable); +#endif int ble_hs_set_rpa_timeout(uint16_t timeout); #ifdef __cplusplus diff --git a/nimble/host/services/bas/include/services/bas/ble_svc_bas.h b/nimble/host/services/bas/include/services/bas/ble_svc_bas.h index e941fb5ed..f4a8b7ff7 100644 --- a/nimble/host/services/bas/include/services/bas/ble_svc_bas.h +++ b/nimble/host/services/bas/include/services/bas/ble_svc_bas.h @@ -31,6 +31,7 @@ extern "C" { #define BLE_SVC_BAS_CHR_UUID16_BATTERY_LEVEL 0x2A19 int ble_svc_bas_battery_level_set(uint8_t level); +uint8_t ble_svc_bas_battery_level_get(void); void ble_svc_bas_init(void); void ble_svc_bas_deinit(void); diff --git a/nimble/host/services/bas/src/ble_svc_bas.c b/nimble/host/services/bas/src/ble_svc_bas.c index 8710cf9b0..c42f13f58 100644 --- a/nimble/host/services/bas/src/ble_svc_bas.c +++ b/nimble/host/services/bas/src/ble_svc_bas.c @@ -134,6 +134,10 @@ ble_svc_bas_battery_level_set(uint8_t level) void ble_svc_bas_deinit(void) { + ble_svc_bas_battery_level = 0; +#if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0 + ble_svc_bas_battery_handle = 0; +#endif ble_gatts_free_svcs(); } diff --git a/nimble/host/services/bleuart/src/bleuart.c b/nimble/host/services/bleuart/src/bleuart.c index 3e9f21764..2ff38411f 100644 --- a/nimble/host/services/bleuart/src/bleuart.c +++ b/nimble/host/services/bleuart/src/bleuart.c @@ -147,13 +147,21 @@ bleuart_uart_read(void) off = 0; while (1) { - rc = console_read(console_buf + off, - MYNEWT_VAL(BLEUART_MAX_INPUT) - off, &full_line); - if (rc <= 0 && !full_line) { - vTaskDelay(pdMS_TO_TICKS(10)); - continue; + if (off >= MYNEWT_VAL(BLEUART_MAX_INPUT)) { + full_line = 1; + } else { + rc = console_read(console_buf + off, + MYNEWT_VAL(BLEUART_MAX_INPUT) - off, &full_line); + if (rc < 0) { + off = 0; + break; + } + if (rc <= 0 && !full_line) { + vTaskDelay(pdMS_TO_TICKS(10)); + continue; + } + off += rc; } - off += rc; if (!full_line) { continue; } @@ -192,10 +200,10 @@ bleuart_init(void) /* Ensure this function only gets called by sysinit. */ SYSINIT_ASSERT_ACTIVE(); - rc = console_init(bleuart_uart_read); - SYSINIT_PANIC_ASSERT(rc == 0); - console_buf = nimble_platform_mem_calloc(1, MYNEWT_VAL(BLEUART_MAX_INPUT)); SYSINIT_PANIC_ASSERT(console_buf != NULL); + + rc = console_init(bleuart_uart_read); + SYSINIT_PANIC_ASSERT(rc == 0); } #endif diff --git a/nimble/host/services/cte/src/ble_svc_cte.c b/nimble/host/services/cte/src/ble_svc_cte.c index 235ad42e0..26c9ef9f4 100644 --- a/nimble/host/services/cte/src/ble_svc_cte.c +++ b/nimble/host/services/cte/src/ble_svc_cte.c @@ -190,6 +190,10 @@ static const struct ble_gatt_svc_def ble_svc_cte_defs[] = { * @return A pointer to the matching CTE configuration instance, or NULL if not found. */ static cte_instance_config_t* cte_find_config_by_conn_handle(uint16_t conn_handle) { + if (conn_handle == 0xffff) { + return NULL; + } + for (int i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { if (cte_config[i].conn_handle == conn_handle) { return &cte_config[i]; @@ -303,6 +307,7 @@ ble_svc_cte_two_octet_chr_write(struct os_mbuf *om, if (rc != 0) { return BLE_ATT_ERR_UNLIKELY; } + value = le16toh(value); // Check if the value is within the allowed range if (value < min_value || value > max_value) { @@ -366,18 +371,19 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle, if(ble_gap_set_conn_cte_transmit_param(conn_handle, BLE_GAP_CTE_RSP_ALLOW_AOA_MASK, 0, NULL) != 0) { config->cte_enable = old_enable; - rc = 0xFC; + rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED; break; } if(ble_gap_conn_cte_rsp_enable(conn_handle, true) != 0) { config->cte_enable = old_enable; - rc = 0xFC; + rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED; break; } - } else if ((old_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) { + } else if ((old_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION && + (config->cte_enable & CTE_ENABLE_AOA_CONNECTION) == 0) { if(ble_gap_conn_cte_rsp_enable(conn_handle, false) != 0) { config->cte_enable = old_enable; - rc = 0xFC; + rc = SERVICE_ERROR_WRITE_REQUEST_REJECTED; break; } } @@ -509,11 +515,13 @@ static int ble_svc_cte_adv_cte_interval_access(uint16_t conn_handle, uint16_t at } switch (ctxt->op) { - case BLE_GATT_ACCESS_OP_READ_CHR: + case BLE_GATT_ACCESS_OP_READ_CHR: { // Handle read characteristic request - rc = os_mbuf_append(ctxt->om, &config->cte_interval, sizeof(config->cte_interval)) == 0 ? + uint16_t interval_le = htole16(config->cte_interval); + rc = os_mbuf_append(ctxt->om, &interval_le, sizeof(interval_le)) == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; break; + } case BLE_GATT_ACCESS_OP_WRITE_CHR: // Handle write characteristic request diff --git a/nimble/host/services/cts/src/ble_svc_cts.c b/nimble/host/services/cts/src/ble_svc_cts.c index 5a1a03c63..dc6b8bbf7 100644 --- a/nimble/host/services/cts/src/ble_svc_cts.c +++ b/nimble/host/services/cts/src/ble_svc_cts.c @@ -114,7 +114,8 @@ ble_svc_cts_chr_write(struct os_mbuf *om, uint16_t min_len, int ble_svc_cts_curr_time_validate(struct ble_svc_cts_curr_time curr_time) { if(curr_time.et_256.d_d_t.day_of_week > 7 || - (curr_time.et_256.d_d_t.d_t.year < 1582)|| + (curr_time.et_256.d_d_t.d_t.year != 0 && + curr_time.et_256.d_d_t.d_t.year < 1582)|| curr_time.et_256.d_d_t.d_t.year > 9999 || curr_time.et_256.d_d_t.d_t.month > 12 || curr_time.et_256.d_d_t.d_t.day > 31 || diff --git a/nimble/host/services/dis/src/ble_svc_dis.c b/nimble/host/services/dis/src/ble_svc_dis.c index a2d0f3ae0..5a119a764 100644 --- a/nimble/host/services/dis/src/ble_svc_dis.c +++ b/nimble/host/services/dis/src/ble_svc_dis.c @@ -176,7 +176,8 @@ const struct ble_gatt_svc_def ble_svc_dis_include_def[] = { .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = &ble_svc_dis_include_uuid.u, .includes = included_services, - } + }, + { 0 } }; /** @@ -484,6 +485,17 @@ ble_svc_dis_deinit(void) nimble_platform_mem_free(ble_svc_dis_data_ptr); ble_svc_dis_data_ptr = NULL; } +#else + ble_svc_dis_data.model_number = MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_DEFAULT); + ble_svc_dis_data.serial_number = MYNEWT_VAL(BLE_SVC_DIS_SERIAL_NUMBER_DEFAULT); + ble_svc_dis_data.firmware_revision = MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_DEFAULT); + ble_svc_dis_data.hardware_revision = MYNEWT_VAL(BLE_SVC_DIS_HARDWARE_REVISION_DEFAULT); + ble_svc_dis_data.software_revision = MYNEWT_VAL(BLE_SVC_DIS_SOFTWARE_REVISION_DEFAULT); + ble_svc_dis_data.manufacturer_name = MYNEWT_VAL(BLE_SVC_DIS_MANUFACTURER_NAME_DEFAULT); + ble_svc_dis_data.system_id = MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_DEFAULT); + ble_svc_dis_data.pnp_id = MYNEWT_VAL(BLE_SVC_DIS_PNP_ID_DEFAULT); + ble_svc_dis_data.ieee = "dummy_data"; + ble_svc_dis_data.udi = NULL; #endif } diff --git a/nimble/host/services/gap/src/ble_svc_gap.c b/nimble/host/services/gap/src/ble_svc_gap.c index 597ba2542..dccadac93 100644 --- a/nimble/host/services/gap/src/ble_svc_gap.c +++ b/nimble/host/services/gap/src/ble_svc_gap.c @@ -238,8 +238,8 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt) ble_hs_gap_svc_ctx->svc_gap_name = nimble_platform_mem_calloc(1, om_len + 1); if (!ble_hs_gap_svc_ctx->svc_gap_name) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); + return BLE_ATT_ERR_INSUFFICIENT_RES; } #endif @@ -251,8 +251,11 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt) ble_svc_gap_name[om_len] = '\0'; - if (ble_svc_gap_chr_changed_cb_fn) { - ble_svc_gap_chr_changed_cb_fn(BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME); + { + ble_svc_gap_chr_changed_fn *cb = ble_svc_gap_chr_changed_cb_fn; + if (cb) { + cb(BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME); + } } return rc; @@ -292,8 +295,11 @@ ble_svc_gap_appearance_write_access(struct ble_gatt_access_ctxt *ctxt) ble_svc_gap_appearance = le16toh(ble_svc_gap_appearance); - if (ble_svc_gap_chr_changed_cb_fn) { - ble_svc_gap_chr_changed_cb_fn(BLE_SVC_GAP_CHR_UUID16_APPEARANCE); + { + ble_svc_gap_chr_changed_fn *cb = ble_svc_gap_chr_changed_cb_fn; + if (cb) { + cb(BLE_SVC_GAP_CHR_UUID16_APPEARANCE); + } } return rc; @@ -518,6 +524,14 @@ ble_svc_gap_device_key_material_set(uint8_t *session_key, uint8_t *iv) BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + int rc; + + rc = ble_svc_gap_appearance_init(); + if (rc != 0) { + return rc; + } +#endif memcpy(&ble_svc_gap_km.session_key, session_key, BLE_EAD_KEY_SIZE); memcpy(&ble_svc_gap_km.iv, iv, BLE_EAD_IV_SIZE); ble_gatts_chr_updated(ble_svc_gap_enc_adv_data_handle); @@ -588,7 +602,7 @@ ble_svc_gap_deinit_name(void) void ble_svc_gap_init(void) { -#if NIMBLE_BLE_CONNECT +#if NIMBLE_BLE_CONNECT || MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) int rc; #endif /* Ensure this function only gets called by sysinit. */ diff --git a/nimble/host/services/gatt/src/ble_svc_gatt.c b/nimble/host/services/gatt/src/ble_svc_gatt.c index 1fc33f77b..5821cc0b2 100644 --- a/nimble/host/services/gatt/src/ble_svc_gatt.c +++ b/nimble/host/services/gatt/src/ble_svc_gatt.c @@ -144,7 +144,16 @@ ble_svc_gatt_cl_sup_feat_access(uint16_t conn_handle, uint16_t attr_handle, return 0; } if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - return ble_gatts_peer_cl_sup_feat_update(conn_handle, ctxt->om); + rc = ble_gatts_peer_cl_sup_feat_update(conn_handle, ctxt->om); + if (rc == 0) { + return 0; + } + if (rc == BLE_ATT_ERR_INSUFFICIENT_RES || + rc == BLE_ATT_ERR_UNLIKELY || + rc == BLE_ATT_ERR_VALUE_NOT_ALLOWED) { + return rc; + } + return BLE_ATT_ERR_UNLIKELY; } return 0; @@ -270,6 +279,8 @@ ble_svc_gatt_init(void) void ble_svc_gatt_deinit(void) { + ble_svc_gatt_local_srv_sup_feat = 0; + ble_svc_gatt_local_cl_sup_feat = 0; ble_gatts_free_svcs(); } #endif diff --git a/nimble/host/services/hid/src/ble_svc_hid.c b/nimble/host/services/hid/src/ble_svc_hid.c index 31c523d72..4180b82ea 100644 --- a/nimble/host/services/hid/src/ble_svc_hid.c +++ b/nimble/host/services/hid/src/ble_svc_hid.c @@ -300,6 +300,10 @@ fill_boot_mouse_inp(uint8_t instance) return; } + if (hid_instances[instance].mouse_inp_rpt_len == 0) { + hid_instances[instance].mouse_inp_rpt_len = 3; + } + write_flags = BLE_GATT_CHR_F_WRITE | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_WRITE_ENC | @@ -538,7 +542,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, uint16_t out_rpt_len = 0; uint8_t instances = get_curr_svc_idx(); uint16_t handle; - uint8_t val; + uint8_t val = 0xFF; for (int instance = 0; instance < instances; instance++) { switch (uuid16) { @@ -597,7 +601,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, s_char_write_cb) { s_char_write_cb(attr_handle, BLE_SVC_HID_CHR_UUID16_HID_CTRL_PT, hid_instances[instance].ctrl_pt); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return BLE_ATT_ERR_UNLIKELY; @@ -619,7 +623,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, &hid_instances[instance].kbd_out_rpt, out_rpt_len); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return 0; case BLE_SVC_HID_CHR_UUID16_BOOT_KBD_INP: @@ -636,7 +640,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) { ble_gatts_chr_updated(*(ctxt->chr->val_handle)); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return 0; case BLE_SVC_HID_CHR_UUID16_BOOT_MOUSE_INP: @@ -681,7 +685,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, s_char_write_cb) { s_char_write_cb(attr_handle, BLE_SVC_HID_CHR_UUID16_PROTOCOL_MODE, hid_instances[instance].proto_mode); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return BLE_ATT_ERR_UNLIKELY; } @@ -709,7 +713,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) { ble_gatts_chr_updated(*(ctxt->chr->val_handle)); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return 0; @@ -861,6 +865,7 @@ ble_svc_hid_reset(void) void ble_svc_hid_deinit(void) { + ble_svc_hid_reset(); ble_gatts_free_svcs(); } diff --git a/nimble/host/services/hr/include/services/hr/ble_svc_hr.h b/nimble/host/services/hr/include/services/hr/ble_svc_hr.h index 2f8eaa50e..c303ce42b 100644 --- a/nimble/host/services/hr/include/services/hr/ble_svc_hr.h +++ b/nimble/host/services/hr/include/services/hr/ble_svc_hr.h @@ -30,6 +30,8 @@ void ble_svc_hr_on_gap_disconnect(uint16_t conn_handle); void ble_svc_hr_init(void); +int ble_svc_hr_notify_measurement(void); + #ifdef __cplusplus } #endif diff --git a/nimble/host/services/hr/src/ble_svc_hr.c b/nimble/host/services/hr/src/ble_svc_hr.c index 0b8126663..b3dc523eb 100644 --- a/nimble/host/services/hr/src/ble_svc_hr.c +++ b/nimble/host/services/hr/src/ble_svc_hr.c @@ -31,7 +31,7 @@ static int ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); -static int ble_svc_hr_notify_measurement(void); +int ble_svc_hr_notify_measurement(void); static int ble_svc_hr_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len, void *dst, @@ -107,7 +107,8 @@ ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle, switch (uuid16) { case BLE_SVC_HR_CHR_UUID16_MEASUREMENT: - rc = ble_svc_hr_notify_measurement(); + rc = os_mbuf_append(ctxt->om, &ble_svc_hr_measurement, + sizeof(ble_svc_hr_measurement)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; case BLE_SVC_HR_CHR_UUID16_BODY_SENSOR_LOC: @@ -122,7 +123,7 @@ ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle, &ble_svc_hr_ctrl_pt, NULL); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return BLE_SVC_HS_ERR_CMD_NOT_SUPPORTED; @@ -172,26 +173,34 @@ int ble_svc_hr_notify_measurement(void) { int rc; + int first_err = 0; struct os_mbuf *txom = NULL; - for (int i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { + for (int i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { if (ble_svc_hr_conn_handle[i] != -1) { txom = ble_hs_mbuf_from_flat(&ble_svc_hr_measurement, sizeof(ble_svc_hr_measurement)); if (!txom) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + if (first_err == 0) { + first_err = BLE_HS_ENOMEM; + } + continue; } rc = ble_gatts_notify_custom(ble_svc_hr_conn_handle[i], ble_svc_hr_measurement_val_handle, txom); if (rc != 0) { - return rc; + BLE_HS_LOG(ERROR, "%s conn=%d rc=%d\n", __func__, + ble_svc_hr_conn_handle[i], rc); + if (first_err == 0) { + first_err = rc; + } } } } - return 0; + return first_err; } /** diff --git a/nimble/host/services/htp/src/ble_svc_htp.c b/nimble/host/services/htp/src/ble_svc_htp.c index 989414fca..a7771d891 100644 --- a/nimble/host/services/htp/src/ble_svc_htp.c +++ b/nimble/host/services/htp/src/ble_svc_htp.c @@ -115,7 +115,7 @@ static const struct ble_gatt_svc_def ble_svc_htp_defs[] = { { { .uuid = BLE_UUID16_DECLARE(BLE_SVC_HTP_DSC_UUID16_VALID_RANGE), - .att_flags = BLE_ATT_F_READ | BLE_ATT_F_WRITE, + .att_flags = BLE_ATT_F_READ, .access_cb = ble_svc_htp_access, }, { 0, @@ -166,7 +166,7 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_SVC_HTP_CHR_UUID16_MEASUREMENT_ITVL: if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = ble_svc_htp_chr_write(ctxt->om, 0, sizeof(ble_svc_htp_temp_msr_itvl), + rc = ble_svc_htp_chr_write(ctxt->om, sizeof(ble_svc_htp_temp_msr_itvl), sizeof(ble_svc_htp_temp_msr_itvl), &ble_svc_htp_temp_msr_itvl, NULL); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; @@ -178,8 +178,16 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, return BLE_SVC_HS_ERR_OUT_OF_RANGE; } + case BLE_SVC_HTP_DSC_UUID16_VALID_RANGE: + /* Valid Range descriptor is read-only; minimum and maximum interval values */ + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + static const uint16_t valid_range[2] = {1, 65535}; + rc = os_mbuf_append(ctxt->om, valid_range, sizeof(valid_range)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + return BLE_ATT_ERR_WRITE_NOT_PERMITTED; + default: - assert(0); return BLE_ATT_ERR_UNLIKELY; } } @@ -216,6 +224,9 @@ ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr) if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { return false; } + if (chr < 0 || chr >= 3) { + return false; + } return conn_chr_subs[conn_handle].chr_subs[chr]; } @@ -276,7 +287,7 @@ ble_svc_htp_notify(uint16_t conn_handle, float temp, bool temp_unit) txom = ble_hs_mbuf_from_flat(&flags, sizeof(flags)); if (!txom) { - return ESP_FAIL; + return BLE_HS_ENOMEM; } rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp)); @@ -323,7 +334,7 @@ ble_svc_htp_indicate(uint16_t conn_handle, float temp, bool temp_unit) txom = ble_hs_mbuf_from_flat(&flags, sizeof(flags)); if (!txom) { - return ESP_FAIL; + return BLE_HS_ENOMEM; } rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp)); diff --git a/nimble/host/services/ipss/src/ble_svc_ipss.c b/nimble/host/services/ipss/src/ble_svc_ipss.c index 85bcaa08f..c3f32c123 100644 --- a/nimble/host/services/ipss/src/ble_svc_ipss.c +++ b/nimble/host/services/ipss/src/ble_svc_ipss.c @@ -52,5 +52,6 @@ ble_svc_ipss_init(void) rc = ble_gatts_add_svcs(ble_svc_ipss_defs); SYSINIT_PANIC_ASSERT(rc == 0); + (void)rc; } #endif diff --git a/nimble/host/services/lls/src/ble_svc_lls.c b/nimble/host/services/lls/src/ble_svc_lls.c index af3fe69f3..dda2d8e32 100644 --- a/nimble/host/services/lls/src/ble_svc_lls.c +++ b/nimble/host/services/lls/src/ble_svc_lls.c @@ -111,7 +111,7 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, sizeof ble_svc_lls_alert_level); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - case BLE_GATT_ACCESS_OP_WRITE_CHR: + case BLE_GATT_ACCESS_OP_WRITE_CHR: { uint8_t level; rc = ble_svc_lls_chr_write(ctxt->om, sizeof level, @@ -124,6 +124,7 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, ble_svc_lls_alert_level = level; } return rc; + } default: assert(0); @@ -150,9 +151,11 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, void ble_svc_lls_on_gap_disconnect(int reason) { + ble_svc_lls_event_fn *cb = ble_svc_lls_cb_fn; + if (reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_SPVN_TMO)) { - if (ble_svc_lls_cb_fn != NULL) { - ble_svc_lls_cb_fn(ble_svc_lls_alert_level); + if (cb != NULL) { + cb(ble_svc_lls_alert_level); } } } diff --git a/nimble/host/services/prox/include/services/prox/ble_svc_prox.h b/nimble/host/services/prox/include/services/prox/ble_svc_prox.h index 96fe258fb..14b7f51eb 100644 --- a/nimble/host/services/prox/include/services/prox/ble_svc_prox.h +++ b/nimble/host/services/prox/include/services/prox/ble_svc_prox.h @@ -36,6 +36,7 @@ void ble_svc_prox_ctx_deinit(void); * @brief Initializes proximity service. */ void ble_svc_prox_init(void); +void ble_svc_prox_deinit(void); #ifdef __cplusplus } diff --git a/nimble/host/services/prox/src/ble_svc_prox.c b/nimble/host/services/prox/src/ble_svc_prox.c index a65f13f4e..39fe2bb6f 100644 --- a/nimble/host/services/prox/src/ble_svc_prox.c +++ b/nimble/host/services/prox/src/ble_svc_prox.c @@ -222,15 +222,28 @@ ble_svc_prox_link_loss_access(uint16_t conn_handle, uint16_t attr_handle, uint16_t uuid16; int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_prox_ctx == NULL) { + return BLE_ATT_ERR_UNLIKELY; + } +#endif + uuid16 = ble_uuid_u16(ctxt->chr->uuid); assert(uuid16 != 0); switch (uuid16) { case BLE_SVC_PROX_CHR_UUID16_ALERT_LVL: if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = ble_svc_prox_chr_write(ctxt->om, 1, sizeof(ble_svc_prox_link_loss_alert), - &ble_svc_prox_link_loss_alert, NULL); - return rc; + uint8_t tmp_alert; + rc = ble_svc_prox_chr_write(ctxt->om, 1, sizeof(tmp_alert), &tmp_alert, NULL); + if (rc != 0) { + return rc; + } + if (tmp_alert > 2) { + return BLE_ATT_ERR_VALUE_NOT_ALLOWED; + } + ble_svc_prox_link_loss_alert = tmp_alert; + return 0; } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { rc = os_mbuf_append(ctxt->om, &ble_svc_prox_link_loss_alert, sizeof(ble_svc_prox_link_loss_alert)); @@ -251,6 +264,12 @@ ble_svc_prox_imm_alert_access(uint16_t conn_handle, uint16_t attr_handle, { uint16_t uuid16; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_prox_ctx == NULL) { + return BLE_ATT_ERR_UNLIKELY; + } +#endif + uuid16 = ble_uuid_u16(ctxt->chr->uuid); assert(uuid16 != 0); @@ -259,7 +278,7 @@ ble_svc_prox_imm_alert_access(uint16_t conn_handle, uint16_t attr_handle, if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { int rc = ble_svc_prox_chr_write(ctxt->om, 1, 1, &ble_svc_prox_alert, NULL); if (rc != 0) { - return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + return rc; } MODLOG_DFLT(INFO, "Path loss = %d", ble_svc_prox_alert); @@ -282,6 +301,12 @@ ble_svc_prox_tx_pwr_access(uint16_t conn_handle, uint16_t attr_handle, uint16_t uuid16; int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_prox_ctx == NULL) { + return BLE_ATT_ERR_UNLIKELY; + } +#endif + uuid16 = ble_uuid_u16(ctxt->chr->uuid); assert(uuid16 != 0); @@ -361,4 +386,18 @@ ble_svc_prox_init(void) BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", 4096, NULL, 10, &ble_prox_task_handle); SYSINIT_PANIC_ASSERT(ret == pdPASS); } + +void +ble_svc_prox_deinit(void) +{ +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + /* ble_svc_prox_ctx_deinit handles NULL check, task deletion, and ctx free */ + ble_svc_prox_ctx_deinit(); +#else + if (ble_prox_task_handle != NULL) { + vTaskDelete(ble_prox_task_handle); + ble_prox_task_handle = NULL; + } +#endif +} #endif diff --git a/nimble/host/services/ras/include/services/ras/ble_svc_ras.h b/nimble/host/services/ras/include/services/ras/ble_svc_ras.h index b8d80e222..d488004b9 100644 --- a/nimble/host/services/ras/include/services/ras/ble_svc_ras.h +++ b/nimble/host/services/ras/include/services/ras/ble_svc_ras.h @@ -173,6 +173,18 @@ struct ranging_buffer { /** Index for tracking the current write position in subevent data. */ uint16_t subevent_cursor; + /** Offset of the most recently written subevent_header within the + * subevents array. Used to update num_steps_reported when a + * RESULT_CONTINUE event appends additional steps to the same subevent. */ + uint16_t last_subevent_hdr_offset; + + /** True only when last_subevent_hdr_offset points to a successfully written + * subevent header for the current subevent. Reset to false at the start of + * every RESULT event (before the overflow check) so that a RESULT_CONTINUE + * arriving after a failed/overflow RESULT does not corrupt the previous + * subevent's header via a stale offset. */ + bool last_subevent_hdr_valid; + /** Reference count to ensure safe access and prevent premature reuse. */ uint8_t refcount; @@ -227,6 +239,7 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent); void ble_gatts_indicate_control_point_response(uint16_t attr_handle , uint16_t ranging_counter); struct ranging_buffer *ranging_buffer_alloc(uint16_t conn_handle , uint16_t ranging_counter); void ble_svc_ras_init(void); +void ble_svc_ras_deinit(void); #endif diff --git a/nimble/host/services/ras/src/ble_svc_ras.c b/nimble/host/services/ras/src/ble_svc_ras.c index 4bea74e21..c384be618 100644 --- a/nimble/host/services/ras/src/ble_svc_ras.c +++ b/nimble/host/services/ras/src/ble_svc_ras.c @@ -92,6 +92,9 @@ static void ranging_buffer_init(uint16_t conn_handle, struct ranging_buffer *buf buf->isbusy = true; buf->isacked = false; buf->subevent_cursor = 0; + buf->last_subevent_hdr_offset = 0; + buf->last_subevent_hdr_valid = false; + memset(&buf->ranging_data, 0, sizeof(buf->ranging_data)); } static void reset_ranging_buffer(void) @@ -104,6 +107,8 @@ static void reset_ranging_buffer(void) ranging_buffers[i].isbusy = false; ranging_buffers[i].isacked = false; ranging_buffers[i].subevent_cursor = 0; + ranging_buffers[i].last_subevent_hdr_offset = 0; + ranging_buffers[i].last_subevent_hdr_valid = false; } } @@ -113,9 +118,10 @@ ble_svc_ras_ensure_ctx_init() { if (ble_svc_ras_ctx == NULL) { ble_svc_ras_ctx = nimble_platform_mem_calloc(1, sizeof(ble_svc_ras_ctx_t)); + if (ble_svc_ras_ctx != NULL) { + reset_ranging_buffer(); + } } - - reset_ranging_buffer(); } void @@ -377,9 +383,11 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl sizeof(ble_svc_ras_rt_rd_val), sizeof(ble_svc_ras_rt_rd_val), &ble_svc_ras_rt_rd_val, NULL); - ble_gatts_chr_updated(attr_handle); - MODLOG_DFLT(INFO, "Notification/Indication scheduled for " - "all subscribed peers.\n"); + if (rc == 0) { + ble_gatts_chr_updated(attr_handle); + MODLOG_DFLT(INFO, "Notification/Indication scheduled for " + "all subscribed peers.\n"); + } return rc; } else if (attr_handle == ble_svc_ras_od_rd_val_handle) { /* Ensure the buffer is allocated before writing to it */ @@ -420,10 +428,23 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl ble_svc_ras_od_rd_val = NULL; ble_svc_ras_od_rd_seg_len = 0; } - /* Reset the ranging buffers */ + /* Reset the ranging buffer for this connection */ + for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) { + if (ranging_buffers[i].conn == (int)conn_handle) { + ranging_buffers[i].conn = -1; + ranging_buffers[i].ranging_counter = 0; + ranging_buffers[i].isready = false; + ranging_buffers[i].isbusy = false; + ranging_buffers[i].isacked = false; + ranging_buffers[i].subevent_cursor = 0; + ranging_buffers[i].last_subevent_hdr_offset = 0; + ranging_buffers[i].last_subevent_hdr_valid = false; + } + } ble_svc_ras_cp_val[0]= 0x02; /*Table 3.12. Response Code Values associated with Op Code 0x02*/ ble_svc_ras_cp_val[1]=0x01; // Success + ble_svc_ras_cp_val[2]=0x01; // Response value: Success ble_gatts_chr_updated(ble_svc_ras_cp_val_handle); MODLOG_DFLT(INFO, "Successfully completed the Ranging procedure\n"); @@ -440,18 +461,22 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl sizeof(ble_svc_ras_rd_val), sizeof(ble_svc_ras_rd_val), &ble_svc_ras_rd_val, NULL); - ble_gatts_chr_updated(attr_handle); - MODLOG_DFLT(INFO, "Notification/Indication scheduled for " - "all subscribed peers.\n"); + if (rc == 0) { + ble_gatts_chr_updated(attr_handle); + MODLOG_DFLT(INFO, "Notification/Indication scheduled for " + "all subscribed peers.\n"); + } return rc; } else if (attr_handle == ble_svc_ras_rd_ov_val_handle) { rc = gatt_svr_write(ctxt->om, sizeof(ble_svc_ras_rd_ov_val), sizeof(ble_svc_ras_rd_ov_val), &ble_svc_ras_rd_ov_val, NULL); - ble_gatts_chr_updated(attr_handle); - MODLOG_DFLT(INFO, "Notification/Indication scheduled for " - "all subscribed peers.\n"); + if (rc == 0) { + ble_gatts_chr_updated(attr_handle); + MODLOG_DFLT(INFO, "Notification/Indication scheduled for " + "all subscribed peers.\n"); + } return rc; } @@ -472,6 +497,48 @@ unknown: void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) { struct ranging_buffer *buf = NULL; + uint16_t conn_handle; + uint16_t procedure_counter; + uint8_t config_id; + uint8_t procedure_done_status; + uint8_t subevent_done_status; + uint8_t abort_reason; + uint8_t num_antenna_paths; + uint8_t num_steps_reported; + const struct cs_steps_data *steps; + uint16_t start_acl_conn_event_counter = 0; + uint16_t frequency_compensation = 0; + uint8_t reference_power_level = 0; + + /* Extract fields from the correct union member based on event type */ + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) { + conn_handle = ranging_subevent.subev_result.conn_handle; + procedure_counter = ranging_subevent.subev_result.procedure_counter; + config_id = ranging_subevent.subev_result.config_id; + procedure_done_status = ranging_subevent.subev_result.procedure_done_status; + subevent_done_status = ranging_subevent.subev_result.subevent_done_status; + abort_reason = ranging_subevent.subev_result.abort_reason; + num_antenna_paths = ranging_subevent.subev_result.num_antenna_paths; + num_steps_reported = ranging_subevent.subev_result.num_steps_reported; + steps = ranging_subevent.subev_result.steps; + start_acl_conn_event_counter = ranging_subevent.subev_result.start_acl_conn_event_counter; + frequency_compensation = ranging_subevent.subev_result.frequency_compensation; + reference_power_level = ranging_subevent.subev_result.reference_power_level; + } else if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE) { + conn_handle = ranging_subevent.subev_result_continue.conn_handle; + /* subev_result_continue has no procedure_counter; reuse conn to find buf */ + procedure_counter = 0; + config_id = ranging_subevent.subev_result_continue.config_id; + procedure_done_status = ranging_subevent.subev_result_continue.procedure_done_status; + subevent_done_status = ranging_subevent.subev_result_continue.subevent_done_status; + abort_reason = ranging_subevent.subev_result_continue.abort_reason; + num_antenna_paths = ranging_subevent.subev_result_continue.num_antenna_paths; + num_steps_reported = ranging_subevent.subev_result_continue.num_steps_reported; + steps = ranging_subevent.subev_result_continue.steps; + } else { + MODLOG_DFLT(ERROR, "Unknown CS event type %d\n", ranging_subevent.type); + return; + } #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) ble_svc_ras_ensure_ctx_init(); @@ -480,70 +547,162 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) { } #endif - /* Check if the subevent is already stored */ - for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) { - if (ranging_buffers[i].conn == ranging_subevent.subev_result.conn_handle && - ranging_buffers[i].ranging_counter == ranging_subevent.subev_result.procedure_counter) { - buf = &ranging_buffers[i]; - break; + /* Find the buffer for this event. + * RESULT: match on conn_handle + ranging_counter (unique per procedure). + * RESULT_CONTINUE: procedure_counter not available per BT Core Spec §7.7.65.45; + * match on conn_handle + config_id. config_id is only 4 bits + * (0-3), so successive procedures on the same connection share + * the same config_id. Among all matching buffers pick the one + * with the highest ranging_counter — that is the most recently + * started procedure and the correct target for RESULT_CONTINUE. */ + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE) { + uint16_t best_counter = 0; + bool found = false; + for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) { + if (ranging_buffers[i].conn != (int)conn_handle) { + continue; + } + if (ranging_buffers[i].ranging_data.ranging_header.config_id == config_id) { + if (!found || ranging_buffers[i].ranging_counter > best_counter) { + buf = &ranging_buffers[i]; + best_counter = ranging_buffers[i].ranging_counter; + found = true; + } + } + } + } else { + for (int i = 0; i < BLE_RAS_MAX_SUBEVENTS_PER_PROCEDURE; i++) { + if (ranging_buffers[i].conn != (int)conn_handle) { + continue; + } + if (ranging_buffers[i].ranging_counter == procedure_counter) { + buf = &ranging_buffers[i]; + break; + } } } if (buf == NULL) { - /* Allocate a new buffer if not found */ - buf = ranging_buffer_alloc(ranging_subevent.subev_result.conn_handle, ranging_subevent.subev_result.procedure_counter); + /* Per BT spec §7.7.65.45, RESULT_CONTINUE shall only follow a prior + * RESULT event. Receiving RESULT_CONTINUE without an existing buffer + * is a protocol error — reject to avoid producing corrupt data. */ + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE) { + MODLOG_DFLT(ERROR, "RESULT_CONTINUE without prior RESULT for conn 0x%04x\n", + conn_handle); + return; + } + buf = ranging_buffer_alloc(conn_handle, procedure_counter); if (buf == NULL) { MODLOG_DFLT(ERROR,"No available buffer for storing ranging data\n"); return; } } - buf->ranging_data.ranging_header.config_id = ranging_subevent.subev_result.config_id; - buf->ranging_data.ranging_header.ranging_counter = ranging_subevent.subev_result.procedure_counter; - // buf->ranging_data.ranging_header.selected_tx_power = ranging_subevent.subev_result.selected_tx_power; - /* convert antenna path mask using bitmask */ - buf->ranging_data.ranging_header.antenna_paths_mask = ranging_subevent.subev_result.num_antenna_paths; + /* Only set header fields for initial RESULT event; RESULT_CONTINUE appends + * to an existing buffer and must not overwrite the valid ranging_counter. */ + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) { + buf->ranging_data.ranging_header.config_id = config_id; + buf->ranging_data.ranging_header.ranging_counter = procedure_counter; + /* convert antenna path mask using bitmask */ + buf->ranging_data.ranging_header.antenna_paths_mask = num_antenna_paths; + } uint16_t max_subevent_data = BLE_RAS_PROCEDURE_MEM - sizeof(struct ranging_header); - if (buf->subevent_cursor + sizeof(struct subevent_header) > max_subevent_data) { - MODLOG_DFLT(ERROR, "Ranging buffer overflow on subevent header\n"); + /* RESULT_CONTINUE appends step data to the same logical subevent; no new + * subevent_header is written. Only RESULT events open a new subevent. */ + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) { + /* Invalidate the prior subevent's header pointer before the overflow + * check. If this RESULT overflows and returns early, any subsequent + * RESULT_CONTINUE must not corrupt the previous subevent's header. */ + buf->last_subevent_hdr_valid = false; + + if (buf->subevent_cursor + sizeof(struct subevent_header) > max_subevent_data) { + MODLOG_DFLT(ERROR, "Ranging buffer overflow on subevent header\n"); + return; + } + + /* Save offset so RESULT_CONTINUE can locate and update this header. */ + buf->last_subevent_hdr_offset = buf->subevent_cursor; + buf->last_subevent_hdr_valid = true; + + struct subevent_header *subevent_hdr = (struct subevent_header *)(buf->ranging_data.subevents + buf->subevent_cursor); + buf->subevent_cursor += sizeof(struct subevent_header); + subevent_hdr->start_acl_conn_event = start_acl_conn_event_counter; + subevent_hdr->freq_compensation = frequency_compensation; + subevent_hdr->ranging_done_status = procedure_done_status; + subevent_hdr->subevent_done_status = subevent_done_status; + /* Abort_Reason per BT Core Spec §7.7.65.44: bits 0-3 = procedure abort, + * bits 4-7 = subevent abort. Extract each nibble separately. */ + subevent_hdr->ranging_abort_reason = abort_reason & 0x0F; + subevent_hdr->subevent_abort_reason = (abort_reason >> 4) & 0x0F; + subevent_hdr->ref_power_level = reference_power_level; + /* num_steps_reported set after the step loop to reflect actual written count. */ + } + + /* For RESULT_CONTINUE: if the corresponding RESULT event failed (overflow), + * last_subevent_hdr_valid is false. Writing step data into the buffer with + * no subevent header to account for it produces a corrupt RAS segment. + * Reject early, before the loop advances subevent_cursor. */ + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT_CONTINUE && + !buf->last_subevent_hdr_valid) { + MODLOG_DFLT(ERROR, "RESULT_CONTINUE with no valid subevent header; dropping\n"); return; } - struct subevent_header *subevent_hdr = (struct subevent_header *)(buf->ranging_data.subevents + buf->subevent_cursor); - buf->subevent_cursor += sizeof(struct subevent_header); - subevent_hdr->start_acl_conn_event = ranging_subevent.subev_result.start_acl_conn_event_counter; - subevent_hdr->freq_compensation = ranging_subevent.subev_result.frequency_compensation; - subevent_hdr->ranging_done_status = ranging_subevent.subev_result.procedure_done_status; - subevent_hdr->subevent_done_status = ranging_subevent.subev_result.subevent_done_status; - subevent_hdr->ranging_abort_reason = ranging_subevent.subev_result.abort_reason; - subevent_hdr->subevent_abort_reason = ranging_subevent.subev_result.abort_reason; - subevent_hdr->ref_power_level = ranging_subevent.subev_result.reference_power_level; - subevent_hdr->num_steps_reported = ranging_subevent.subev_result.num_steps_reported; - - - /* Add step data to buf using manual pointer advancement for flexible array member */ - const uint8_t *step_ptr = (const uint8_t *)ranging_subevent.subev_result.steps; - for (int i = 0; i < ranging_subevent.subev_result.num_steps_reported; i++) { + /* Add step data; track actual steps written so the header reflects reality + * even if the buffer overflows mid-loop and we return early. */ + uint8_t steps_written = 0; + const uint8_t *step_ptr = (const uint8_t *)steps; + for (int i = 0; i < num_steps_reported; i++) { const struct cs_steps_data *step = (const struct cs_steps_data *)step_ptr; if (buf->subevent_cursor + BLE_RAS_STEP_MODE_LEN + step->data_len > max_subevent_data) { MODLOG_DFLT(ERROR, "Ranging buffer overflow on step data\n"); - return; + break; } buf->ranging_data.subevents[buf->subevent_cursor] = step->mode; buf->subevent_cursor += BLE_RAS_STEP_MODE_LEN; memcpy(&buf->ranging_data.subevents[buf->subevent_cursor], step->data, step->data_len); buf->subevent_cursor += step->data_len; - step_ptr += sizeof(struct cs_steps_data) + step->data_len; + steps_written++; + } + + /* Update the subevent header with the count of steps actually written. + * For RESULT: correct any pre-set count in case of overflow. + * For RESULT_CONTINUE: accumulate only the steps that fit, and overwrite + * the status/abort fields with the final values from this last event + * (BT Core Spec v6.2 §7.7.65.44 — the Continue event carries the terminal + * done/abort status that supersedes the partial status set by RESULT). + * Guard with last_subevent_hdr_valid: a RESULT_CONTINUE arriving after an + * overflowed RESULT must not corrupt the previous subevent's header. */ + if (buf->last_subevent_hdr_valid) { + struct subevent_header *subevent_hdr = + (struct subevent_header *)(buf->ranging_data.subevents + + buf->last_subevent_hdr_offset); + if (ranging_subevent.type == BLE_CS_EVENT_SUBEVET_RESULT) { + subevent_hdr->num_steps_reported = steps_written; + } else { + subevent_hdr->num_steps_reported += steps_written; + subevent_hdr->ranging_done_status = procedure_done_status; + subevent_hdr->subevent_done_status = subevent_done_status; + subevent_hdr->ranging_abort_reason = abort_reason & 0x0F; + subevent_hdr->subevent_abort_reason = (abort_reason >> 4) & 0x0F; + } } /* Create RAS segment*/ struct segment *ras_segment; - uint16_t max_data_len = ble_att_mtu(ranging_subevent.subev_result.conn_handle) - sizeof(struct segment_header) - 4; + uint16_t att_mtu = ble_att_mtu(conn_handle); + uint16_t overhead = (uint16_t)(sizeof(struct segment_header) + 4); + if (att_mtu == 0 || att_mtu <= overhead) { + MODLOG_DFLT(INFO, "MTU (%d) too small for RAS segment overhead (%d)\n", + att_mtu, overhead); + return; + } + uint16_t max_data_len = att_mtu - overhead; MODLOG_DFLT(INFO, "Max data len : %d\n", max_data_len); ras_segment= nimble_platform_mem_calloc(1,sizeof(struct segment)+ max_data_len); if (ras_segment == NULL) { @@ -621,4 +780,20 @@ ble_svc_ras_init(void) { reset_ranging_buffer(); } + +void +ble_svc_ras_deinit(void) +{ +#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_ras_od_rd_val != NULL) { + nimble_platform_mem_free(ble_svc_ras_od_rd_val); + ble_svc_ras_od_rd_val = NULL; + } + ble_svc_ras_od_rd_seg_len = 0; +#else + /* ble_svc_ras_ctx_deinit handles NULL check, frees inner buffer, + * frees ctx itself, and sets ble_svc_ras_ctx = NULL. */ + ble_svc_ras_ctx_deinit(); +#endif +} #endif diff --git a/nimble/host/services/ras/src/rrsp/ble_svc_ras_rrsp.c b/nimble/host/services/ras/src/rrsp/ble_svc_ras_rrsp.c index 531c75929..f500e6a3c 100644 --- a/nimble/host/services/ras/src/rrsp/ble_svc_ras_rrsp.c +++ b/nimble/host/services/ras/src/rrsp/ble_svc_ras_rrsp.c @@ -27,7 +27,7 @@ static uint32_t ble_svc_ras_feat_val; static uint16_t ble_svc_ras_rd_val; static uint16_t ble_svc_ras_rd_ov_val; -static uint16_t ble_svc_ras_cp_val; +static uint8_t ble_svc_ras_cp_val[RASCP_CMD_OPCODE_LEN + sizeof(uint16_t)]; static uint16_t ble_svc_ras_feat_val_handle; static uint16_t ble_svc_ras_od_val_handle; diff --git a/nimble/host/services/sps/src/ble_svc_sps.c b/nimble/host/services/sps/src/ble_svc_sps.c index b69d26ccd..084e41682 100644 --- a/nimble/host/services/sps/src/ble_svc_sps.c +++ b/nimble/host/services/sps/src/ble_svc_sps.c @@ -20,6 +20,7 @@ #include #include #include "sysinit/sysinit.h" +#include "host/ble_att.h" #include "host/ble_hs.h" #include "services/sps/ble_svc_sps.h" #include "host/ble_hs_log.h" @@ -113,6 +114,11 @@ ble_svc_sps_chr_write(struct os_mbuf *om, uint16_t min_len, } void ble_svc_sps_scan_refresh() { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_sps_static_vars == NULL) { + return; + } +#endif /* spec allows only value 0 to send */ ble_scan_refresh = 0; ble_gatts_chr_updated(ble_scan_refresh_handle); @@ -131,8 +137,16 @@ ble_svc_sps_access(uint16_t conn_handle, uint16_t attr_handle, assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR); rc = ble_svc_sps_chr_write(ctxt->om, 4, 4, buf, NULL); if(rc == 0) { - ble_scan_itvl = get_le16(buf); - ble_scan_window = get_le16(buf + 2); + uint16_t itvl = get_le16(buf); + uint16_t window = get_le16(buf + 2); + + if (itvl < 0x0004 || itvl > 0x4000 || + window < 0x0004 || window > 0x4000 || + window > itvl) { + return BLE_ATT_ERR_VALUE_NOT_ALLOWED; + } + ble_scan_itvl = itvl; + ble_scan_window = window; if (ble_svc_sps_cb_fn) { ble_svc_sps_cb_fn(ble_scan_itvl, ble_scan_window); } @@ -198,8 +212,17 @@ ble_svc_sps_set_cb(ble_svc_sps_event_fn *cb) void ble_svc_sps_deinit(void) { ble_gatts_free_svcs(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_sps_static_vars != NULL) { + ble_scan_itvl = 0; + ble_scan_window = 0; + nimble_platform_mem_free(ble_svc_sps_static_vars); + ble_svc_sps_static_vars = NULL; + } +#else ble_scan_itvl = 0; ble_scan_window = 0; +#endif } /** diff --git a/nimble/host/src/ble_aes_ccm.c b/nimble/host/src/ble_aes_ccm.c index 261d66580..cc4ee0dea 100644 --- a/nimble/host/src/ble_aes_ccm.c +++ b/nimble/host/src/ble_aes_ccm.c @@ -64,8 +64,7 @@ ble_aes_ccm_hex(const void *buf, size_t len) char *str; int i; - str = hexbufs[curbuf++]; - curbuf %= ARRAY_SIZE(hexbufs); + str = hexbufs[curbuf++ % ARRAY_SIZE(hexbufs)]; len = min(len, (sizeof(hexbufs[0]) - 1) / 2); @@ -142,18 +141,26 @@ int ble_aes_ccm_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data) { struct tc_aes_key_sched_struct s = {0}; + int rc = 0; if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto done; } if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; } - return 0; +done: + memset(&s, 0, sizeof(s)); + /* Compiler barrier: prevent the optimizer from eliminating the memset above + * as a dead store. The key schedule in s is sensitive cryptographic material + * and must actually be erased from the stack before the frame is released. */ + __asm__ volatile("" : : "r"(&s) : "memory"); + return rc; } #endif @@ -361,14 +368,36 @@ int ble_aes_ccm_decrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t rc = ble_aes_ccm_crypt(key_reversed, nonce, enc_msg, out_msg, msg_len); if (rc != 0) { + memset(out_msg, 0, msg_len); + memset(key_reversed, 0, sizeof(key_reversed)); + __asm__ volatile("" : : "r"(key_reversed) : "memory"); return rc; } rc = ble_aes_ccm_auth(key_reversed, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size); + memset(key_reversed, 0, sizeof(key_reversed)); + __asm__ volatile("" : : "r"(key_reversed) : "memory"); if (rc != 0) { + memset(out_msg, 0, msg_len); + memset(mic, 0, sizeof(mic)); + __asm__ volatile("" : : "r"(mic) : "memory"); return rc; } + /* Constant-time MIC comparison to prevent timing side-channel attacks */ + volatile uint8_t diff = 0; + for (size_t i = 0; i < mic_size; i++) { + diff |= mic[i] ^ enc_msg[msg_len + i]; + } + if (diff != 0) { + memset(out_msg, 0, msg_len); + memset(mic, 0, sizeof(mic)); + __asm__ volatile("" : : "r"(mic) : "memory"); + return BLE_HS_EAUTHEN; + } + + memset(mic, 0, sizeof(mic)); + __asm__ volatile("" : : "r"(mic) : "memory"); return 0; } @@ -400,11 +429,15 @@ int ble_aes_ccm_encrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t /** Calculating MIC */ int rc = ble_aes_ccm_auth(key_reversed, nonce, msg, msg_len, aad, aad_len, mic, mic_size); if (rc != 0) { + memset(key_reversed, 0, sizeof(key_reversed)); + __asm__ volatile("" : : "r"(key_reversed) : "memory"); return rc; } /** Encrypting advertisement */ rc = ble_aes_ccm_crypt(key_reversed, nonce, msg, out_msg, msg_len); + memset(key_reversed, 0, sizeof(key_reversed)); + __asm__ volatile("" : : "r"(key_reversed) : "memory"); if (rc != 0) { return rc; } diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c index f2e491f55..63863f955 100644 --- a/nimble/host/src/ble_att.c +++ b/nimble/host/src/ble_att.c @@ -550,6 +550,16 @@ ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle, return; } #if MYNEWT_VAL(BLE_GATTS) + /* + * Responses and notifications have bit 0 set (includes Handle Value + * Indication 0x1D). Handle Value Confirmation (0x1E) has bit 0 clear. + */ + if ((op & 0x01) || op == BLE_ATT_OP_INDICATE_RSP) { + os_mbuf_free_chain(*om); + *om = NULL; + return; + } + os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om)); ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, BLE_ATT_ERR_REQ_NOT_SUPPORTED); @@ -690,10 +700,6 @@ process_att_normally: return BLE_HS_EMSGSIZE; } - if (cid == BLE_L2CAP_CID_ATT && ble_att_is_response_op(op)) { - ble_att_send_outstanding_after_response(conn_handle); - } - entry = ble_att_rx_dispatch_entry_find(op); if (entry == NULL) { BLE_HS_LOG(INFO, "ATT handler not found; op=0x%02x conn_handle=0x%04x " @@ -708,6 +714,9 @@ process_att_normally: os_mbuf_adj(*om, 1); rc = entry->bde_fn(conn_handle, cid, om); + if (cid == BLE_L2CAP_CID_ATT && ble_att_is_response_op(op)) { + ble_att_send_outstanding_after_response(conn_handle); + } if (rc != 0) { if (rc == BLE_HS_ENOTSUP) { ble_att_rx_handle_unknown_request(op, conn_handle, cid, om); @@ -760,11 +769,11 @@ ble_att_set_preferred_mtu(uint16_t mtu) return BLE_HS_EINVAL; } - ble_att_preferred_mtu_val = mtu; - /* Set my_mtu for established connections that haven't exchanged. */ ble_hs_lock(); + ble_att_preferred_mtu_val = mtu; + i = 0; while ((conn = ble_hs_conn_find_by_idx(i)) != NULL) { chan = ble_hs_conn_chan_find_by_scid(conn, BLE_L2CAP_CID_ATT); diff --git a/nimble/host/src/ble_att_clt.c b/nimble/host/src/ble_att_clt.c index 12d1f8f2f..4a4e211c4 100644 --- a/nimble/host/src/ble_att_clt.c +++ b/nimble/host/src/ble_att_clt.c @@ -94,6 +94,12 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu) req = ble_att_cmd_get(BLE_ATT_OP_MTU_REQ, sizeof(*req), &txom); if (req == NULL) { + ble_hs_lock(); + if (ble_att_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT, + &conn, &chan) == 0) { + chan->flags &= ~BLE_L2CAP_CHAN_F_TXED_MTU; + } + ble_hs_unlock(); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } @@ -124,11 +130,9 @@ ble_att_clt_rx_mtu(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) mtu = 0; -#if MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0 - if (ble_hs_cfg.eatt && cid != BLE_L2CAP_CID_ATT) { + if (cid != BLE_L2CAP_CID_ATT) { return BLE_HS_ENOTSUP; } -#endif rc = ble_hs_mbuf_pullup_base(rxom, sizeof(*cmd)); if (rc == 0) { @@ -334,7 +338,7 @@ ble_att_clt_parse_find_type_value_hinfo( rc = ble_hs_mbuf_pullup_base(om, sizeof(*group)); if (rc != 0) { - return BLE_HS_EBADDATA; + return rc; } group = (struct ble_att_handle_group *)(*om)->om_data; @@ -371,7 +375,7 @@ ble_att_clt_rx_find_type_value(uint16_t conn_handle, uint16_t cid, struct os_mbu /* Notify GATT client that the full response has been parsed. */ ble_gattc_rx_find_type_value_complete(conn_handle, cid, rc); - return 0; + return rc; } /***************************************************************************** @@ -555,7 +559,7 @@ int ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid, const uint16_t *handles, int num_handles, bool variable) { -#if !NIMBLE_BLE_ATT_CLT_READ_MULT +#if !NIMBLE_BLE_ATT_CLT_READ_MULT && !NIMBLE_BLE_ATT_CLT_READ_MULT_VAR return BLE_HS_ENOTSUP; #endif @@ -564,7 +568,7 @@ ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid, int i; uint8_t op; - if (num_handles < 1) { + if (num_handles < 2) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -625,6 +629,7 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle, uint16_t cid, struct ble_att_read_group_type_req *req; struct os_mbuf *txom; + int rc; if (start_handle == 0 || start_handle > end_handle) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); @@ -640,7 +645,11 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle, uint16_t cid, req->bagq_start_handle = htole16(start_handle); req->bagq_end_handle = htole16(end_handle); - ble_uuid_flat(uuid, req->uuid); + rc = ble_uuid_flat(uuid, req->uuid); + if (rc != 0) { + os_mbuf_free_chain(txom); + return rc; + } return ble_att_tx(conn_handle, cid, txom); } @@ -898,14 +907,16 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han nimble_platform_mem_free(message); message = NULL; } + /* After concat, txom is owned by txom2 chain; caller must not free txom */ os_mbuf_concat(txom2, txom); + txom = NULL; return ble_att_tx(conn_handle, cid, txom2); err: if (message != NULL) { nimble_platform_mem_free(message); message = NULL; } - /* Do not free txom here; the caller ble_gattc_signed_write is responsible for it on error. */ + os_mbuf_free_chain(txom); os_mbuf_free_chain(txom2); return rc; } @@ -920,6 +931,7 @@ ble_att_clt_tx_prep_write(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t offset, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_PREP_WRITE + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -1072,7 +1084,9 @@ ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle, cid = ble_eatt_get_available_chan_cid(conn_handle, BLE_GATT_OP_DUMMY); rc = ble_att_tx(conn_handle, cid, txom2); - ble_eatt_release_chan(conn_handle, BLE_GATT_OP_DUMMY); + if (cid != BLE_L2CAP_CID_ATT) { + ble_eatt_release_chan(conn_handle, BLE_GATT_OP_DUMMY); + } return rc; err: @@ -1138,6 +1152,7 @@ int ble_att_clt_tx_notify_mult(uint16_t conn_handle, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_NOTIFY_MULT + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -1147,6 +1162,7 @@ ble_att_clt_tx_notify_mult(uint16_t conn_handle, struct os_mbuf *txom) if (ble_att_cmd_get(BLE_ATT_OP_NOTIFY_MULTI_REQ, 0, &txom2) == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + os_mbuf_free_chain(txom); return BLE_HS_ENOMEM; } diff --git a/nimble/host/src/ble_att_cmd.c b/nimble/host/src/ble_att_cmd.c index 44d081657..0ebb92a61 100644 --- a/nimble/host/src/ble_att_cmd.c +++ b/nimble/host/src/ble_att_cmd.c @@ -36,13 +36,12 @@ ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom) return NULL; } - if (os_mbuf_extend(txom, sizeof(*hdr) + len) == NULL) { + hdr = os_mbuf_extend(txom, sizeof(*hdr) + len); + if (hdr == NULL) { os_mbuf_free_chain(txom); return NULL; } - hdr = (struct ble_att_hdr *)(txom)->om_data; - hdr->opcode = opcode; return hdr->data; @@ -231,7 +230,10 @@ ble_att_mtu_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_MTU_REQ, payload, BLE_ATT_MTU_CMD_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bamc_mtu = le16toh(src->bamc_mtu); } @@ -243,7 +245,10 @@ ble_att_mtu_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_MTU_RSP, payload, BLE_ATT_MTU_CMD_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bamc_mtu = le16toh(src->bamc_mtu); } @@ -267,6 +272,9 @@ ble_att_mtu_rsp_write(void *payload, int len, dst = ble_att_init_write(BLE_ATT_OP_MTU_RSP, payload, BLE_ATT_MTU_CMD_SZ, len); + if (dst == NULL) { + return; + } dst->bamc_mtu = htole16(src->bamc_mtu); } @@ -278,7 +286,10 @@ ble_att_find_info_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_REQ, payload, BLE_ATT_FIND_INFO_REQ_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bafq_start_handle = le16toh(src->bafq_start_handle); dst->bafq_end_handle = le16toh(src->bafq_end_handle); } @@ -304,6 +315,10 @@ ble_att_find_info_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_RSP, payload, BLE_ATT_FIND_INFO_RSP_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bafp_format = src->bafp_format; } @@ -316,6 +331,9 @@ ble_att_find_info_rsp_write(void *payload, int len, dst = ble_att_init_write(BLE_ATT_OP_FIND_INFO_RSP, payload, BLE_ATT_FIND_INFO_RSP_BASE_SZ, len); + if (dst == NULL) { + return; + } dst->bafp_format = src->bafp_format; } @@ -328,7 +346,10 @@ ble_att_find_type_value_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, payload, BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bavq_start_handle = le16toh(src->bavq_start_handle); dst->bavq_end_handle = le16toh(src->bavq_end_handle); dst->bavq_attr_type = le16toh(src->bavq_attr_type); @@ -356,7 +377,10 @@ ble_att_read_type_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_REQ, payload, BLE_ATT_READ_TYPE_REQ_BASE_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->batq_start_handle = le16toh(src->batq_start_handle); dst->batq_end_handle = le16toh(src->batq_end_handle); } @@ -382,7 +406,10 @@ ble_att_read_type_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_RSP, payload, BLE_ATT_READ_TYPE_RSP_BASE_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->batp_length = src->batp_length; } @@ -394,7 +421,9 @@ ble_att_read_type_rsp_write(void *payload, int len, dst = ble_att_init_write(BLE_ATT_OP_READ_TYPE_RSP, payload, BLE_ATT_READ_TYPE_RSP_BASE_SZ, len); - + if (dst == NULL) { + return; + } dst->batp_length = src->batp_length; } @@ -406,6 +435,10 @@ ble_att_read_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_REQ, payload, BLE_ATT_READ_REQ_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->barq_handle = le16toh(src->barq_handle); } @@ -430,6 +463,10 @@ ble_att_read_blob_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_BLOB_REQ, payload, BLE_ATT_READ_BLOB_REQ_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->babq_handle = le16toh(src->babq_handle); dst->babq_offset = le16toh(src->babq_offset); @@ -484,7 +521,10 @@ ble_att_read_group_type_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload, BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len); - + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bagq_start_handle = le16toh(src->bagq_start_handle); dst->bagq_end_handle = le16toh(src->bagq_end_handle); } @@ -497,7 +537,10 @@ ble_att_read_group_type_req_write( dst = ble_att_init_write(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload, BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len); - + if (dst == NULL) { + BLE_HS_DBG_ASSERT(dst != NULL); + return; + } dst->bagq_start_handle = htole16(src->bagq_start_handle); dst->bagq_end_handle = htole16(src->bagq_end_handle); } @@ -510,6 +553,10 @@ ble_att_read_group_type_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload, BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bagp_length = src->bagp_length; } @@ -522,7 +569,9 @@ ble_att_read_group_type_rsp_write( dst = ble_att_init_write(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload, BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len); - + if (dst == NULL) { + return; + } dst->bagp_length = src->bagp_length; } @@ -534,6 +583,10 @@ ble_att_write_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_WRITE_REQ, payload, BLE_ATT_WRITE_REQ_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bawq_handle = le16toh(src->bawq_handle); } @@ -546,6 +599,10 @@ ble_att_write_cmd_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_WRITE_CMD, payload, BLE_ATT_WRITE_REQ_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bawq_handle = le16toh(src->bawq_handle); } @@ -568,6 +625,10 @@ ble_att_write_cmd_write(void *payload, int len, dst = ble_att_init_write(BLE_ATT_OP_WRITE_CMD, payload, BLE_ATT_WRITE_REQ_BASE_SZ, len); + if (dst == NULL) { + BLE_HS_DBG_ASSERT(dst != NULL); + return; + } dst->bawq_handle = htole16(src->bawq_handle); } @@ -579,6 +640,10 @@ ble_att_prep_write_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_REQ, payload, BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bapc_handle = le16toh(src->bapc_handle); dst->bapc_offset = le16toh(src->bapc_offset); @@ -605,6 +670,10 @@ ble_att_prep_write_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_RSP, payload, BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->bapc_handle = le16toh(src->bapc_handle); dst->bapc_offset = le16toh(src->bapc_offset); @@ -631,6 +700,10 @@ ble_att_exec_write_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_REQ, payload, BLE_ATT_EXEC_WRITE_REQ_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->baeq_flags = src->baeq_flags; } @@ -669,6 +742,10 @@ ble_att_notify_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_NOTIFY_REQ, payload, BLE_ATT_NOTIFY_REQ_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->banq_handle = le16toh(src->banq_handle); } @@ -693,6 +770,10 @@ ble_att_indicate_req_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_INDICATE_REQ, payload, BLE_ATT_INDICATE_REQ_BASE_SZ, len); + if (src == NULL) { + memset(dst, 0, sizeof(*dst)); + return; + } dst->baiq_handle = le16toh(src->baiq_handle); } diff --git a/nimble/host/src/ble_att_priv.h b/nimble/host/src/ble_att_priv.h index deb20b199..8ed55489c 100644 --- a/nimble/host/src/ble_att_priv.h +++ b/nimble/host/src/ble_att_priv.h @@ -148,9 +148,7 @@ int ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags, uint8_t min_key_size, uint16_t *handle_id, ble_att_svr_access_fn *cb, void *cb_arg); -#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle); -#endif #if MYNEWT_VAL(BLE_GATT_CACHING) int ble_att_get_database_size(int *out_size); int ble_att_fill_database_info(uint8_t *out_data); diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c index 26df0b4f4..f12dc0558 100644 --- a/nimble/host/src/ble_att_svr.c +++ b/nimble/host/src/ble_att_svr.c @@ -108,6 +108,9 @@ ble_att_svr_ensure_ctx(void) return BLE_HS_ENOMEM; } + STAILQ_INIT(&ble_att_svr_ctx->list); + STAILQ_INIT(&ble_att_svr_ctx->hidden_list); + return 0; } #endif @@ -156,6 +159,9 @@ ble_att_svr_next_id(void) { /* Rollover is fatal. */ BLE_HS_DBG_ASSERT(ble_att_svr_id != UINT16_MAX); + if (ble_att_svr_id == UINT16_MAX) { + return 0; /* Invalid handle; caller must check */ + } return ++ble_att_svr_id; } @@ -187,6 +193,11 @@ ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags, entry->ha_flags = flags; entry->ha_min_key_size = min_key_size; entry->ha_handle_id = ble_att_svr_next_id(); + if (entry->ha_handle_id == 0) { + ble_att_svr_entry_free(entry); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; + } entry->ha_cb = cb; entry->ha_cb_arg = cb_arg; @@ -199,7 +210,6 @@ ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags, return 0; } -#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) /** * Deregister a host attribute with the BLE stack. * @@ -229,7 +239,6 @@ int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle) { } return 0; } -#endif uint16_t ble_att_svr_prev_handle(void) @@ -972,7 +981,10 @@ ble_att_svr_fill_info(uint16_t start_handle, uint16_t end_handle, put_le16(buf + 0, ha->ha_handle_id); - ble_uuid_flat(ha->ha_uuid, buf + 2); + rc = ble_uuid_flat(ha->ha_uuid, buf + 2); + if (rc != 0) { + goto done; + } num_entries++; } @@ -1010,6 +1022,7 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle, uint16_t cid, if (rsp == NULL) { *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1020,8 +1033,12 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle, uint16_t cid, rc = ble_att_svr_fill_info(start_handle, end_handle, txom, mtu, &rsp->bafp_format); if (rc != 0) { - *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND; - rc = BLE_HS_ENOENT; + if (rc == BLE_HS_ENOMEM) { + *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + } else { + *att_err = BLE_ATT_ERR_ATTR_NOT_FOUND; + rc = BLE_HS_ENOENT; + } goto done; } @@ -1161,13 +1178,6 @@ ble_att_svr_is_valid_group_end(const ble_uuid_t *uuid_group, return 1; } - /* Grouping is defined only for 16-bit UUIDs, so non-16-bit UUID attribute - * cannot end group. - */ - if (uuid->type != BLE_UUID_TYPE_16) { - return 0; - } - switch (ble_uuid_u16(uuid_group)) { case BLE_ATT_UUID_PRIMARY_SERVICE: case BLE_ATT_UUID_SECONDARY_SERVICE: @@ -1263,12 +1273,22 @@ ble_att_svr_fill_type_value(uint16_t conn_handle, if (ble_uuid_cmp(ha->ha_uuid, &attr_type.u) == 0) { rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf, &attr_len, out_att_err); + if (rc == BLE_HS_EMSGSIZE) { + /* Attribute value longer than search buffer; treat as non-match. */ + *out_att_err = 0; + rc = 0; + continue; + } if (rc != 0) { goto done; } - /* value is at the end of req */ - rc = os_mbuf_cmpf(rxom, sizeof(struct ble_att_find_type_value_req), - buf, attr_len); + /* value is at the end of req; require exact length match */ + if ((uint16_t)(OS_MBUF_PKTLEN(rxom) - sizeof(struct ble_att_find_type_value_req)) != attr_len) { + rc = 1; /* length mismatch - treat as non-match */ + } else { + rc = os_mbuf_cmpf(rxom, sizeof(struct ble_att_find_type_value_req), + buf, attr_len); + } if (rc == 0) { first = ha->ha_handle_id; prev = ha->ha_handle_id; @@ -1324,6 +1344,7 @@ ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle, uint16_t cid, if (buf == NULL) { *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1429,7 +1450,7 @@ static void ble_att_svr_make_conn_aware(uint16_t conn_handle) { static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) { struct ble_hs_conn *conn; - struct ble_hs_conn_addrs addrs; + struct ble_hs_conn_addrs addrs = {0}; conn = ble_hs_conn_find_assert(conn_handle); BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); @@ -1502,6 +1523,7 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle, uint16_t cid, rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_TYPE_RSP, sizeof(*rsp), txom); if (rsp == NULL) { + txom = NULL; /* ble_att_cmd_prepare freed txom on failure */ *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; *err_handle = 0; rc = BLE_HS_ENOMEM; @@ -1721,7 +1743,7 @@ ble_att_svr_rx_read(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) #if MYNEWT_VAL(BLE_GATT_CACHING) ble_hs_lock(); if((ble_att_svr_get_csfs(conn_handle)[0] & 1) - && ble_svc_gatt_csf_handle() != err_handle ) { + && ble_svc_gatt_csf_handle() != err_handle) { if (!ble_att_svr_check_conn_aware(conn_handle)) { att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC; rc = BLE_HS_EREJECT; @@ -1740,6 +1762,7 @@ ble_att_svr_rx_read(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) if (ble_att_cmd_prepare(BLE_ATT_OP_READ_RSP, 0, txom) == NULL) { att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1785,7 +1808,7 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx #if MYNEWT_VAL(BLE_GATT_CACHING) ble_hs_lock(); if((ble_att_svr_get_csfs(conn_handle)[0] & 1) - && ble_svc_gatt_csf_handle() != err_handle ) { + && ble_svc_gatt_csf_handle() != err_handle) { if (!ble_att_svr_check_conn_aware(conn_handle)) { att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC; rc = BLE_HS_EREJECT; @@ -1805,6 +1828,7 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx if (ble_att_cmd_prepare(BLE_ATT_OP_READ_BLOB_RSP, 0, txom) == NULL) { att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1846,6 +1870,7 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle, uint16_t cid, *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; *err_handle = 0; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1952,6 +1977,7 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid, *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; *err_handle = 0; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1996,12 +2022,16 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid, uint16_t le_len = htole16(tuple_len); rc = os_mbuf_append(txom, &le_len, sizeof(le_len)); if (rc != 0) { + *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + rc = BLE_HS_ENOMEM; *err_handle = handle; goto done; } if (tuple_len != 0) { rc = os_mbuf_appendfrom(txom, tmp, 0, tuple_len); if (rc != 0) { + *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + rc = BLE_HS_ENOMEM; *err_handle = handle; goto done; } @@ -2102,6 +2132,9 @@ ble_att_svr_service_uuid(struct ble_att_svr_entry *entry, rc = ble_uuid_init_from_buf(uuid, val, attr_len); } + if (rc != 0 && out_att_err != NULL) { + *out_att_err = BLE_ATT_ERR_UNLIKELY; + } return rc; } @@ -2131,7 +2164,11 @@ ble_att_svr_read_group_type_entry_write(struct os_mbuf *om, uint16_t mtu, put_le16(buf + 0, start_group_handle); put_le16(buf + 2, end_group_handle); - ble_uuid_flat(service_uuid, buf + 4); + int rc2 = ble_uuid_flat(service_uuid, buf + 4); + if (rc2 != 0) { + os_mbuf_adj(om, -len); + return rc2; + } return 0; } @@ -2181,6 +2218,7 @@ ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle, uint16_t cid, rsp = ble_att_cmd_prepare(BLE_ATT_OP_READ_GROUP_TYPE_RSP, sizeof(*rsp), txom); if (rsp == NULL) { + txom = NULL; /* ble_att_cmd_prepare freed txom on failure */ *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; goto done; @@ -2192,7 +2230,6 @@ ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle, uint16_t cid, continue; } if (entry->ha_handle_id > end_handle) { - /* The full input range has been searched. */ rc = 0; goto done; } @@ -2407,8 +2444,6 @@ ble_att_svr_build_write_rsp(struct os_mbuf **rxom, struct os_mbuf **out_txom, */ rc = ble_att_svr_pkt(rxom, &txom, att_err); if (rc != 0) { - os_mbuf_free_chain(txom); - txom = NULL; goto done; } @@ -2497,6 +2532,7 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf * { #if !MYNEWT_VAL(BLE_ATT_SVR_WRITE_NO_RSP) os_mbuf_free_chain(*rxom); + *rxom = NULL; return BLE_HS_ENOTSUP; #endif @@ -2506,6 +2542,7 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf * !ble_att_svr_check_conn_aware(conn_handle)) { ble_hs_unlock(); os_mbuf_free_chain(*rxom); + *rxom = NULL; return BLE_HS_EREJECT; } ble_hs_unlock(); @@ -2575,6 +2612,12 @@ ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf * return rc; } + /* Ensure packet is large enough to hold the handle and 12-byte signature. */ + if (OS_MBUF_PKTLEN(*rxom) < (uint16_t)(sizeof(*req) + (BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ - BLE_ATT_SIGNED_WRITE_DATA_OFFSET))) { + rc = BLE_HS_EBADDATA; + goto err; + } + req = (struct ble_att_signed_write_cmd *)(*rxom)->om_data; handle = le16toh(req->handle); @@ -2656,6 +2699,10 @@ ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om) { int rc; + if (om == NULL) { + return BLE_HS_EINVAL; + } + rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, &om, NULL); @@ -2844,10 +2891,16 @@ ble_att_svr_prep_write(uint16_t conn_handle, ble_att_svr_prep_extract(prep_list, &attr_handle, &om); /* Attribute existence was verified during prepare-write request - * processing. + * processing. Guard against dynamic deletion between prepare and + * execute. */ attr = ble_att_svr_find_by_handle(attr_handle); BLE_HS_DBG_ASSERT(attr != NULL); + if (attr == NULL) { + os_mbuf_free_chain(om); + *err_handle = attr_handle; + return BLE_ATT_ERR_INVALID_HANDLE; + } rc = ble_att_svr_write(conn_handle, attr, 0, &om, &att_err); os_mbuf_free_chain(om); @@ -2949,7 +3002,7 @@ ble_att_svr_rx_prep_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r #if MYNEWT_VAL(BLE_GATT_CACHING) ble_hs_lock(); if((ble_att_svr_get_csfs(conn_handle)[0] & 1) - && ble_svc_gatt_csf_handle() != err_handle ) { + && ble_svc_gatt_csf_handle() != err_handle) { if (!ble_att_svr_check_conn_aware(conn_handle)) { att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC; rc = BLE_HS_EREJECT; @@ -3071,6 +3124,7 @@ ble_att_svr_rx_exec_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r if (ble_att_cmd_prepare(BLE_ATT_OP_EXEC_WRITE_RSP, 0, txom) == NULL) { att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -3124,8 +3178,8 @@ ble_att_svr_rx_notify(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), NULL); if (rc != 0) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + return rc; } req = (struct ble_att_notify_req *)(*rxom)->om_data; @@ -3175,7 +3229,6 @@ ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid, struct os_mbuf * while (pkt_len > 0) { rc = ble_att_svr_pullup_req_base(rxom, sizeof(struct ble_att_tuple_list), NULL); if (rc != 0) { - rc = BLE_HS_ENOMEM; goto done; } @@ -3193,6 +3246,13 @@ ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid, struct os_mbuf * goto done; } + if (attr_len > OS_MBUF_PKTLEN(*rxom)) { + BLE_HS_LOG_ERROR("attr_len (%d) > pkt_len (%d)", + attr_len, OS_MBUF_PKTLEN(*rxom)); + rc = BLE_HS_EBADDATA; + goto done; + } + tmp = os_msys_get_pkthdr(attr_len, 0); if (!tmp) { BLE_HS_LOG_ERROR("not enough resources, aborting"); @@ -3260,6 +3320,7 @@ ble_att_svr_build_indicate_rsp(struct os_mbuf **rxom, if (ble_att_cmd_prepare(BLE_ATT_OP_INDICATE_RSP, 0, txom) == NULL) { rc = BLE_HS_ENOMEM; *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + txom = NULL; goto done; } @@ -3480,9 +3541,7 @@ ble_att_svr_free_start_mem(void) ble_att_svr_entry_mem = NULL; } os_mempool_unregister(&ble_att_svr_entry_pool); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) memset(&ble_att_svr_entry_pool, 0, sizeof(ble_att_svr_entry_pool)); -#endif } int @@ -3536,6 +3595,8 @@ ble_att_svr_deinit(void) return; } + ble_att_svr_reset(); + if (ble_att_svr_prep_entry_mem) { nimble_platform_mem_free(ble_att_svr_prep_entry_mem); ble_att_svr_prep_entry_mem = NULL; @@ -3552,6 +3613,7 @@ ble_att_svr_deinit(void) void ble_att_svr_stop(void) { + ble_att_svr_reset(); ble_att_svr_free_start_mem(); } @@ -3652,8 +3714,17 @@ int ble_att_get_database_size(int *out_size) /* uuid is stored in the value attribute */ struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next); + uint8_t val[20]; + uint16_t attr_len; + if (next_entry == NULL) { - size += 4; // Updated + rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE, + entry, 0, sizeof(val), val, + &attr_len, &att_error); + if (rc != 0) { + return rc; + } + size += (4 + attr_len); continue; } /* handle(2 bytes) + type(2 bytes) + properties(1 byte) @@ -3708,7 +3779,10 @@ int ble_att_fill_database_info(uint8_t *out_data) put_le16(data, entry->ha_handle_id); uuid = BLE_UUID16(entry->ha_uuid); put_le16(data + 2, uuid->value); - ble_uuid_flat(&service_uuid.u, data + 4); + rc = ble_uuid_flat(&service_uuid.u, data + 4); + if (rc != 0) { + return rc; + } data += (4 + ble_uuid_length(&service_uuid.u)); } else if(uuid->value == BLE_ATT_UUID_INCLUDE) { @@ -3728,6 +3802,22 @@ int ble_att_fill_database_info(uint8_t *out_data) data += (4 + attr_len); } else if(uuid->value == BLE_ATT_UUID_CHARACTERISTIC) { + struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next); + if (next_entry == NULL) { + put_le16(data, entry->ha_handle_id); + uuid = BLE_UUID16(entry->ha_uuid); + put_le16(data + 2, uuid->value); + + rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE, + entry, 0, sizeof(val), val, + &attr_len, &att_error); + if (rc != 0) { + return rc; + } + memcpy(data + 4, val, attr_len); + data += (4 + attr_len); + continue; + } /* handle(2 bytes) + type(2 bytes) + properties(1 byte) + val_handle(2 bytes) + uuid */ @@ -3743,6 +3833,7 @@ int ble_att_fill_database_info(uint8_t *out_data) } memcpy(data + 4, val, attr_len); data += (4 + attr_len); + entry = next_entry; } else if(uuid->value == 0x2901 || uuid->value == 0x2902 || diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c index ad7fd3221..68386262a 100644 --- a/nimble/host/src/ble_cs.c +++ b/nimble/host/src/ble_cs.c @@ -693,7 +693,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, struct ble_gap_conn_desc desc; if (len != sizeof(*ev) || ev->status) { - BLE_HS_LOG(INFO, "Failed to enable CS security BLE_HS_ECNOTEROLLER"); + BLE_HS_LOG(INFO, "Failed to enable CS security"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; @@ -715,7 +715,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, cmd.min_procedure_interval = 10; cmd.max_procedure_interval = 10; /* Minimum/maximum suggested durations for each CS subevent in microseconds. - * 1250us and 5000us selected. + * 60000us selected for both min and max. */ cmd.min_subevent_len = 60000; cmd.max_subevent_len = 60000; @@ -825,6 +825,11 @@ ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data, int steps_remaining = 0; int step_size = 0; + if (len < sizeof(*event)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); + return BLE_HS_ECONTROLLER; + } + expected_len += sizeof(*event); steps_remaining = event->num_steps_reported; step_ptr = (void *)event->steps; @@ -869,6 +874,10 @@ ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data void *step_ptr = NULL; int step_size = 0; + if (len < sizeof(*event)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); + return BLE_HS_ECONTROLLER; + } expected_len = sizeof(*event); steps_remaining = event->num_steps_reported; step_ptr = (void *)event->steps; @@ -889,7 +898,7 @@ ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data } expected_len += step_size; - step_ptr += step_size; + step_ptr = (uint8_t *)step_ptr + step_size; steps_remaining--; } @@ -929,6 +938,10 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p return rc; } #endif + if (params == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } /* Channel Sounding setup phase: * 1. Set local default CS settings @@ -944,7 +957,7 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p cmd.conn_handle = params->conn_handle; rc = ble_cs_rd_rem_supp_cap(&cmd); if (rc) { - BLE_HS_LOG(DEBUG, "Failed to read local supported CS capabilities," + BLE_HS_LOG(DEBUG, "Failed to read remote supported CS capabilities, " "err %d", rc); } diff --git a/nimble/host/src/ble_ead.c b/nimble/host/src/ble_ead.c index a487a7eaf..9e3a153ac 100644 --- a/nimble/host/src/ble_ead.c +++ b/nimble/host/src/ble_ead.c @@ -123,6 +123,11 @@ int ble_ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i "Randomizer and the MIC."); } + if (payload_size > 0xffff) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + /* Ensure payload_size isn't too large to wrap around when adding overhead */ if (payload_size > SIZE_MAX - (BLE_EAD_RANDOMIZER_SIZE + BLE_EAD_MIC_SIZE)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); diff --git a/nimble/host/src/ble_eatt.c b/nimble/host/src/ble_eatt.c index 0ec6743c7..8bad0169b 100644 --- a/nimble/host/src/ble_eatt.c +++ b/nimble/host/src/ble_eatt.c @@ -235,9 +235,7 @@ ble_eatt_alloc(void) struct ble_eatt *eatt; eatt = os_memblock_get(&ble_eatt_conn_pool); - if (eatt) { - SLIST_INSERT_HEAD(&g_ble_eatt_list, eatt, next); - } else { + if (!eatt) { BLE_EATT_LOG_DEBUG("eatt: Failed to allocate new eatt context\n"); return NULL; } @@ -249,6 +247,8 @@ ble_eatt_alloc(void) STAILQ_INIT(&eatt->eatt_tx_q); ble_npl_event_init(&eatt->setup_ev, ble_eatt_setup_cb, eatt); ble_npl_event_init(&eatt->wakeup_ev, ble_eatt_wakeup_cb, eatt); + + SLIST_INSERT_HEAD(&g_ble_eatt_list, eatt, next); return eatt; } @@ -257,6 +257,15 @@ ble_eatt_free(struct ble_eatt *eatt) { struct os_mbuf_pkthdr *omp; + /* Remove pending events and release internal event resources. On FreeRTOS, + * ble_npl_event_init allocates memory that event_deinit must free. */ + ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(), + &eatt->setup_ev); + ble_npl_event_deinit(&eatt->setup_ev); + ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(), + &eatt->wakeup_ev); + ble_npl_event_deinit(&eatt->wakeup_ev); + while ((omp = STAILQ_FIRST(&eatt->eatt_tx_q)) != NULL) { STAILQ_REMOVE_HEAD(&eatt->eatt_tx_q, omp_next); os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); @@ -399,7 +408,6 @@ ble_eatt_setup_cb(struct ble_npl_event *ev) if (rc) { BLE_EATT_LOG_ERROR("eatt: Failed to connect EATT on conn_handle 0x%04x (status=%d)\n", eatt->conn_handle, rc); - os_mbuf_free_chain(om); ble_eatt_free(eatt); } } @@ -459,28 +467,30 @@ ble_gatt_eatt_read_uuid_cb(uint16_t conn_handle, if (error == NULL || (error->status != 0 && error->status != BLE_HS_EDONE)) { BLE_EATT_LOG_DEBUG("eatt: Cannot find Server Supported features on peer device\n"); - return BLE_HS_EDONE; + return 0; } - if (attr == NULL) { - BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n"); - return BLE_HS_EDONE; - } + if (error->status == 0) { + if (attr == NULL) { + BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n"); + return 0; + } - rc = os_mbuf_copydata(attr->om, 0, 1, &supported_features); - if (rc) { - BLE_EATT_LOG_ERROR("eatt: Cannot read srv supported features \n"); - return BLE_HS_EDONE; - } + rc = os_mbuf_copydata(attr->om, 0, 1, &supported_features); + if (rc) { + BLE_EATT_LOG_ERROR("eatt: Cannot read srv supported features \n"); + return 0; + } - if (supported_features & 0x01) { - struct ble_npl_event *ev = (struct ble_npl_event *)nimble_platform_mem_calloc(1, sizeof(struct ble_npl_event)); - if (ev) { - ble_npl_event_init(ev, ble_gatt_eatt_read_cl_uuid, (void *)((uintptr_t) conn_handle)); - ble_npl_eventq_put(ble_hs_evq_get(), ev); + if (supported_features & 0x01) { + struct ble_npl_event *ev = (struct ble_npl_event *)nimble_platform_mem_calloc(1, sizeof(struct ble_npl_event)); + if (ev) { + ble_npl_event_init(ev, ble_gatt_eatt_read_cl_uuid, (void *)((uintptr_t) conn_handle)); + ble_npl_eventq_put(ble_hs_evq_get(), ev); + } } } - return BLE_HS_EDONE; + return 0; } static void @@ -679,11 +689,18 @@ ble_eatt_start(uint16_t conn_handle) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) void ble_eatt_deinit(void) { + struct ble_eatt *eatt; + struct ble_eatt *next; + if (ble_eatt_ctx == NULL) { return; } + SLIST_FOREACH_SAFE(eatt, &g_ble_eatt_list, next, next) { + ble_eatt_free(eatt); + } + #if !MYNEWT_VAL(MP_RUNTIME_ALLOC) if (ble_eatt_sdu_coc_mem) { nimble_platform_mem_free(ble_eatt_sdu_coc_mem); @@ -735,9 +752,9 @@ ble_eatt_init(ble_eatt_att_rx_fn att_rx_cb) if (!ble_eatt_sdu_coc_mem) { // free the allocated memory nimble_platform_mem_free(ble_eatt_conn_mem); + ble_eatt_conn_mem = NULL; nimble_platform_mem_free(ble_eatt_ctx); - ble_eatt_conn_mem = NULL; - ble_eatt_ctx = NULL; + ble_eatt_ctx = NULL; BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } @@ -781,6 +798,8 @@ ble_eatt_init(ble_eatt_att_rx_fn att_rx_cb) err: #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + os_mempool_unregister(&ble_eatt_sdu_mbuf_mempool); + os_mempool_unregister(&ble_eatt_conn_pool); #if !MYNEWT_VAL(MP_RUNTIME_ALLOC) nimble_platform_mem_free(ble_eatt_sdu_coc_mem); ble_eatt_sdu_coc_mem = NULL; diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 1edee4770..ad0db163d 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -43,7 +43,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 @@ -673,6 +673,7 @@ ble_gap_fill_conn_desc(struct ble_hs_conn *conn, { struct ble_hs_conn_addrs addrs; + memset(&addrs, 0, sizeof(addrs)); ble_hs_conn_addrs(conn, &addrs); desc->our_id_addr = addrs.our_id_addr; @@ -783,8 +784,10 @@ ble_gap_read_rem_ver_info(uint16_t conn_handle, uint8_t *version, uint16_t *manu if (subversion != NULL) { *subversion = params.subversion; } -#endif return 0; +#else + return BLE_HS_ENOTSUP; +#endif } int @@ -826,10 +829,12 @@ ble_gap_conn_find_handle_by_addr(const ble_addr_t *addr, uint16_t *out_conn_hand ble_hs_lock(); conn = ble_hs_conn_find_by_addr(addr); - if (conn != NULL) { - *out_conn_handle = conn->bhc_handle; - } else { - *out_conn_handle = BLE_HS_CONN_HANDLE_NONE; + if (out_conn_handle != NULL) { + if (conn != NULL) { + *out_conn_handle = conn->bhc_handle; + } else { + *out_conn_handle = BLE_HS_CONN_HANDLE_NONE; + } } ble_hs_unlock(); @@ -865,7 +870,9 @@ ble_gap_conn_foreach_handle(ble_gap_conn_foreach_handle_fn *cb, void *arg) .arg = arg, }; + ble_hs_lock(); ble_hs_conn_foreach(ble_gap_conn_foreach_handle_callback, &cb_arg); + ble_hs_unlock(); } #if NIMBLE_BLE_CONNECT @@ -875,7 +882,12 @@ ble_gap_extract_conn_cb(uint16_t conn_handle, { const struct ble_hs_conn *conn; - BLE_HS_DBG_ASSERT(conn_handle <= BLE_HCI_LE_CONN_HANDLE_MAX); + *out_cb = NULL; + *out_cb_arg = NULL; + + if (conn_handle > BLE_HCI_LE_CONN_HANDLE_MAX) { + return BLE_HS_EINVAL; + } ble_hs_lock(); @@ -915,6 +927,12 @@ ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode) return BLE_HS_EINVAL; } + if (priv_mode != BLE_GAP_PRIVATE_MODE_NETWORK && + priv_mode != BLE_GAP_PRIVATE_MODE_DEVICE) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + #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); @@ -983,16 +1001,21 @@ 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_ERR_INV_HCI_CMD_PARMS; + /* BLE_GAP_LE_PHY_ANY_MASK (0x0F) means no preference; map to 0 so the + * no-preference HCI path is taken below. Strip other unknown bits. */ + if (tx_phys_mask == BLE_GAP_LE_PHY_ANY_MASK) { + tx_phys_mask = 0; + } else { + 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 (rx_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_ERR_INV_HCI_CMD_PARMS; + if (rx_phys_mask == BLE_GAP_LE_PHY_ANY_MASK) { + rx_phys_mask = 0; + } else { + 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 (!ble_hs_is_enabled()) { @@ -1041,16 +1064,21 @@ 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)) { - return BLE_ERR_INV_HCI_CMD_PARMS; + /* BLE_GAP_LE_PHY_ANY_MASK (0x0F) means no preference; map to 0 so the + * no-preference HCI path is taken below. Strip other unknown bits. */ + if (tx_phys_mask == BLE_GAP_LE_PHY_ANY_MASK) { + tx_phys_mask = 0; + } else { + 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 (rx_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_ERR_INV_HCI_CMD_PARMS; + if (rx_phys_mask == BLE_GAP_LE_PHY_ANY_MASK) { + rx_phys_mask = 0; + } else { + 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 (phy_opts > BLE_HCI_LE_PHY_CODED_S8_PREF) { @@ -1109,18 +1137,24 @@ uint8_t* ble_resolve_adv_data(const uint8_t *adv_data, uint8_t adv_type, uint8_t const struct ble_hs_adv_field *fields; const uint8_t *data; + if (length == NULL) { + return NULL; + } + rc = ble_hs_adv_find_field(adv_type, adv_data, adv_data_len, &fields); /*Fill adv field*/ if (rc == 0) { if (fields->length > 0) { *length = fields->length - 1; /* minus length of type*/ + data = fields->value; /* Type specific adv data*/ + return (uint8_t*)data; } else { *length = 0; + return NULL; } - data = fields->value; /* Type specific adv data*/ - return (uint8_t*)data; } + *length = 0; return NULL; } /***************************************************************************** @@ -1617,6 +1651,8 @@ ble_gap_master_reset_state(void) ble_gap_master.op = BLE_GAP_OP_NULL; ble_gap_master.exp_set = 0; ble_gap_master.conn.cancel = 0; + ble_gap_master.cb = NULL; + ble_gap_master.cb_arg = NULL; memset(&ble_gap_master.conn.peer_addr, 0, sizeof(ble_gap_master.conn.peer_addr)); @@ -1692,8 +1728,10 @@ ble_gap_master_conn_matches_slave_complete(const struct ble_gap_conn_complete *e if (ble_host_rpa_enabled()) { ble_addr_t master_addr = ble_gap_master.conn.peer_addr; + ble_hs_lock(); ble_rpa_replace_peer_params_with_rl(master_addr.val, &master_addr.type, NULL); + ble_hs_unlock(); if (ble_gap_addr_type_equivalent(master_addr.type, evt_addr.type) && memcmp(master_addr.val, evt_addr.val, BLE_DEV_ADDR_LEN) == 0) { return ble_gap_master_cancel_pending_conn(); @@ -1753,6 +1791,13 @@ ble_gap_slave_extract_cb(uint8_t instance, { ble_hs_lock(); + if (instance >= BLE_ADV_INSTANCES) { + *out_cb = NULL; + *out_cb_arg = NULL; + ble_hs_unlock(); + return; + } + *out_cb = ble_gap_slave[instance].cb; *out_cb_arg = ble_gap_slave[instance].cb_arg; ble_gap_slave_reset_state(instance); @@ -1779,9 +1824,10 @@ ble_gap_adv_finished(uint8_t instance, int reason, uint16_t conn_handle, event.adv_complete.num_ext_adv_events = num_events; #endif + ble_gap_slave_extract_cb(instance, &cb, &cb_arg); + ble_gap_event_listener_call(&event); - ble_gap_slave_extract_cb(instance, &cb, &cb_arg); if (cb != NULL) { cb(&event, cb_arg); } @@ -1802,6 +1848,7 @@ ble_gap_master_connect_failure(int status) memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_CONNECT; event.connect.status = status; + event.connect.conn_handle = BLE_HS_CONN_HANDLE_NONE; #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) event.connect.sync_handle = pawr_sync_handle; @@ -1812,6 +1859,7 @@ ble_gap_master_connect_failure(int status) //TODO Remove duplication of event fields event.type = BLE_GAP_EVENT_LINK_ESTAB; event.link_estab.status = status; + event.link_estab.conn_handle = BLE_HS_CONN_HANDLE_NONE; #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) event.link_estab.sync_handle = pawr_sync_handle; @@ -2056,11 +2104,21 @@ ble_gap_disc_complete(void) struct ble_gap_master_state state; struct ble_gap_event event; + /* Hold the lock from the guard check through the state copy-and-reset to + * close the TOCTOU window between checking op and extracting state. */ + ble_hs_lock(); + if (ble_gap_master.op != BLE_GAP_OP_M_DISC) { + ble_hs_unlock(); + return; + } + state = ble_gap_master; + ble_gap_master_reset_state(); + ble_gap_master.preempted_op = BLE_GAP_OP_NULL; + ble_hs_unlock(); + memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_DISC_COMPLETE; event.disc_complete.reason = 0; - - ble_gap_master_extract_state(&state, 1); if (ble_gap_has_client(&state)) { ble_gap_call_event_cb(&event, state.cb, state.cb_arg); } @@ -2221,8 +2279,11 @@ ble_gap_master_failed(int status) #if NIMBLE_BLE_SCAN case BLE_GAP_OP_M_DISC: STATS_INC(ble_gap_stats, initiate_fail); + /* ble_gap_disc_complete() resets state atomically under lock; + * do not call ble_gap_master_reset_state() again here — it would + * run without the lock and clobber any new operation started + * between disc_complete's unlock and this point. */ ble_gap_disc_complete(); - ble_gap_master_reset_state(); break; #endif @@ -2532,6 +2593,11 @@ int ble_gap_ext_adv_active(uint8_t instance) if (instance >= BLE_ADV_INSTANCES) { return 0; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_gap_vars) { + return 0; + } +#endif #if NIMBLE_BLE_ADVERTISE || NIMBLE_BLE_CONNECT return ble_gap_adv_active_instance(instance); #else @@ -2578,7 +2644,9 @@ ble_gap_reset_state(int reason) #endif #if (NIMBLE_BLE_SCAN || NIMBLE_BLE_CONNECT) - ble_gap_master_failed(reason); + if (ble_gap_master.op != BLE_GAP_OP_NULL) { + ble_gap_master_failed(reason); + } #endif } @@ -2645,7 +2713,7 @@ ble_gap_accept_slave_conn(uint8_t instance) #if NIMBLE_BLE_SCAN static int ble_gap_rx_adv_report_sanity_check(const uint8_t *adv_data, uint8_t adv_data_len, - int is_scan_rsp) + int is_scan_rsp, int skip_flags_check) { const struct ble_hs_adv_field *flags; int rc; @@ -2661,10 +2729,11 @@ ble_gap_rx_adv_report_sanity_check(const uint8_t *adv_data, uint8_t adv_data_len return -1; } - if (ble_gap_master.disc.observer || is_scan_rsp) { + if (ble_gap_master.disc.observer || is_scan_rsp || skip_flags_check) { /* Observer role is enabled; All adv reports regardless of * Flags AD Type need to be discovered. - * Also, ignore AD type checks for scan response data + * Also, ignore AD type checks for scan response data and for + * incomplete extended advertising reports. */ return 0; } @@ -2678,16 +2747,16 @@ ble_gap_rx_adv_report_sanity_check(const uint8_t *adv_data, uint8_t adv_data_len if (rc != 0) { /* The advertisement does not have Flags AD Type. Rejected */ return -1; - } else if ((flags->length == 2) && !(flags->value[0] & BLE_HS_ADV_F_DISC_LTD)) { - /* Limited flag is not set in Flags AD Type. Rejected */ + } else if (flags->length < 2 || !(flags->value[0] & BLE_HS_ADV_F_DISC_LTD)) { + /* Malformed flags or limited flag not set. Rejected */ return -1; } } else { if (rc != 0) { /* The advertisement does not have Flags AD Type. Rejected */ return -1; - } else if ((flags->length == 2) && !(flags->value[0] & (BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_DISC_GEN))) { - /* General or Limited flag is not set in Flags AD Type. Rejected */ + } else if (flags->length < 2 || !(flags->value[0] & (BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_DISC_GEN))) { + /* Malformed flags or general/limited flag not set. Rejected */ return -1; } } @@ -2704,8 +2773,8 @@ ble_gap_rx_cis_disconn(const struct ble_hci_ev_disconn_cmp *ev) memset(&event, 0, sizeof(event)); event.type = BLE_GAP_EVENT_DISCONNECT; - event.disconnect.reason = ev->reason; - event.disconnect.conn.conn_handle = ev->conn_handle; + event.disconnect.reason = BLE_HS_HCI_ERR(ev->reason); + event.disconnect.conn.conn_handle = le16toh(ev->conn_handle); ble_gap_event_listener_call(&event); if (ble_gap_cis.cb) { @@ -2723,7 +2792,7 @@ ble_gap_rx_cis_estab(const struct ble_hci_ev_le_subev_cis_established *ev) event.type = BLE_GAP_EVENT_CIS_ESTAB; event.cis_estab.status = ev->status; - event.cis_estab.cis_handle = ev->conn_handle; + event.cis_estab.cis_handle = le16toh(ev->conn_handle); event.cis_estab.cig_sync_delay = get_le24(ev->cig_sync_delay); event.cis_estab.cis_sync_delay = get_le24(ev->cis_sync_delay); event.cis_estab.transport_latency_c_to_p = get_le24(ev->transport_latency_c_to_p); @@ -2735,9 +2804,9 @@ ble_gap_rx_cis_estab(const struct ble_hci_ev_le_subev_cis_established *ev) event.cis_estab.bn_p_to_c = ev->bn_p_to_c; event.cis_estab.ft_c_to_p = ev->ft_c_to_p; event.cis_estab.ft_p_to_c = ev->ft_p_to_c; - event.cis_estab.max_pdu_c_to_p = ev->max_pdu_c_to_p; - event.cis_estab.max_pdu_p_to_c = ev->max_pdu_p_to_c; - event.cis_estab.iso_interval = ev->iso_interval; + event.cis_estab.max_pdu_c_to_p = le16toh(ev->max_pdu_c_to_p); + event.cis_estab.max_pdu_p_to_c = le16toh(ev->max_pdu_p_to_c); + event.cis_estab.iso_interval = le16toh(ev->iso_interval); ble_gap_event_listener_call(&event); if (ble_gap_cis.cb) { @@ -2749,8 +2818,6 @@ void ble_gap_rx_cis_request(const struct ble_hci_ev_le_subev_cis_request *ev) { struct ble_gap_event event; - ble_gap_event_fn *cb; - void *cb_arg; memset(&event, 0, sizeof(event)); @@ -2763,10 +2830,15 @@ ble_gap_rx_cis_request(const struct ble_hci_ev_le_subev_cis_request *ev) ble_gap_event_listener_call(&event); +#if NIMBLE_BLE_CONNECT + ble_gap_event_fn *cb; + void *cb_arg; + ble_gap_extract_conn_cb(le16toh(ev->acl_conn_handle), &cb, &cb_arg); if (cb) { cb(&event, cb_arg); } +#endif } void @@ -2787,15 +2859,17 @@ ble_gap_rx_create_big_comp(const struct ble_hci_ev_le_subev_create_big_complete event.create_big_comp.bn = ev->bn; event.create_big_comp.pto = ev->pto; event.create_big_comp.irc = ev->irc; - event.create_big_comp.max_pdu = ev->max_pdu; - event.create_big_comp.iso_interval = ev->iso_interval; + event.create_big_comp.max_pdu = le16toh(ev->max_pdu); + event.create_big_comp.iso_interval = le16toh(ev->iso_interval); event.create_big_comp.bis_cnt = ev->num_bis; if (ev->num_bis > MYNEWT_VAL(BLE_ISO_BIS_PER_BIG)) { BLE_HS_LOG(ERROR, "num_bis (%d) exceeds BLE_ISO_BIS_PER_BIG limit\n", ev->num_bis); return; } - memcpy(event.create_big_comp.bis_handle, ev->conn_handle, ev->num_bis * 2); + for (int i = 0; i < ev->num_bis; i++) { + event.create_big_comp.bis_handle[i] = le16toh(ev->conn_handle[i]); + } ble_gap_event_listener_call(&event); if (ble_gap_big_brd.cb) { @@ -2807,6 +2881,8 @@ void ble_gap_rx_term_big_comp(const struct ble_hci_ev_le_subev_terminate_big_complete *ev) { struct ble_gap_event event; + ble_gap_event_fn *cb; + void *cb_arg; memset(&event, 0, sizeof(event)); @@ -2815,9 +2891,18 @@ ble_gap_rx_term_big_comp(const struct ble_hci_ev_le_subev_terminate_big_complete event.term_big_comp.big_handle = ev->big_handle; event.term_big_comp.reason = ev->reason; + ble_hs_lock(); + + cb = ble_gap_big_brd.cb; + cb_arg = ble_gap_big_brd.cb_arg; + ble_gap_big_brd.cb = NULL; + ble_gap_big_brd.cb_arg = NULL; + + ble_hs_unlock(); + ble_gap_event_listener_call(&event); - if (ble_gap_big_brd.cb) { - ble_gap_big_brd.cb(&event, ble_gap_big_brd.cb_arg); + if (cb) { + cb(&event, cb_arg); } } @@ -2825,6 +2910,9 @@ void ble_gap_rx_big_sync_estab(const struct ble_hci_ev_le_subev_big_sync_established *ev) { struct ble_gap_event event; + ble_gap_event_fn *cb; + void *cb_arg; + bool invalid_bis; memset(&event, 0, sizeof(event)); @@ -2837,16 +2925,36 @@ ble_gap_rx_big_sync_estab(const struct ble_hci_ev_le_subev_big_sync_established event.big_sync_estab.bn = ev->bn; event.big_sync_estab.pto = ev->pto; event.big_sync_estab.irc = ev->irc; - event.big_sync_estab.max_pdu = ev->max_pdu; - event.big_sync_estab.iso_interval = ev->iso_interval; + event.big_sync_estab.max_pdu = le16toh(ev->max_pdu); + event.big_sync_estab.iso_interval = le16toh(ev->iso_interval); event.big_sync_estab.bis_cnt = ev->num_bis; - for(size_t i = 0; i < ev->num_bis; i++){ - event.big_sync_estab.bis_handle[i] = ev->conn_handle[i]; + + invalid_bis = ev->num_bis > MYNEWT_VAL(BLE_ISO_BIS_PER_BIG); + if (invalid_bis) { + BLE_HS_LOG(ERROR, "num_bis (%d) exceeds BLE_ISO_BIS_PER_BIG limit\n", ev->num_bis); + event.big_sync_estab.bis_cnt = 0; + if (ev->status == BLE_ERR_SUCCESS) { + event.big_sync_estab.status = BLE_ERR_UNSPECIFIED; + } + } else { + for (size_t i = 0; i < ev->num_bis; i++) { + event.big_sync_estab.bis_handle[i] = le16toh(ev->conn_handle[i]); + } } + /* Copy cb/cb_arg under lock before invoking the application callback. */ + ble_hs_lock(); + cb = ble_gap_big_snc.cb; + cb_arg = ble_gap_big_snc.cb_arg; + if (ev->status != 0 || invalid_bis) { + ble_gap_big_snc.cb = NULL; + ble_gap_big_snc.cb_arg = NULL; + } + ble_hs_unlock(); + ble_gap_event_listener_call(&event); - if (ble_gap_big_snc.cb) { - ble_gap_big_snc.cb(&event, ble_gap_big_snc.cb_arg); + if (cb) { + cb(&event, cb_arg); } } @@ -2854,6 +2962,8 @@ void ble_gap_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev) { struct ble_gap_event event; + ble_gap_event_fn *cb; + void *cb_arg; memset(&event, 0, sizeof(event)); @@ -2862,9 +2972,16 @@ ble_gap_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev) event.big_sync_lost.big_handle = ev->big_handle; event.big_sync_lost.reason = ev->reason; + ble_hs_lock(); + cb = ble_gap_big_snc.cb; + cb_arg = ble_gap_big_snc.cb_arg; + ble_gap_big_snc.cb = NULL; + ble_gap_big_snc.cb_arg = NULL; + ble_hs_unlock(); + ble_gap_event_listener_call(&event); - if (ble_gap_big_snc.cb) { - ble_gap_big_snc.cb(&event, ble_gap_big_snc.cb_arg); + if (cb) { + cb(&event, cb_arg); } } @@ -2878,16 +2995,16 @@ ble_gap_rx_biginfo_adv_rpt(const struct ble_hci_ev_le_subev_biginfo_adv_report * event.type = BLE_GAP_EVENT_BIGINFO_ADV_RPT; - event.biginfo_report.sync_handle = ev->sync_handle; + event.biginfo_report.sync_handle = le16toh(ev->sync_handle); event.biginfo_report.bis_cnt = ev->bis_cnt; event.biginfo_report.nse = ev->nse; - event.biginfo_report.iso_interval = ev->iso_interval; + event.biginfo_report.iso_interval = le16toh(ev->iso_interval); event.biginfo_report.bn = ev->bn; event.biginfo_report.pto = ev->pto; event.biginfo_report.irc = ev->irc; - event.biginfo_report.max_pdu = ev->max_pdu; + event.biginfo_report.max_pdu = le16toh(ev->max_pdu); event.biginfo_report.sdu_interval = get_le24(ev->sdu_interval); - event.biginfo_report.max_sdu = ev->max_sdu; + event.biginfo_report.max_sdu = le16toh(ev->max_sdu); event.biginfo_report.phy = ev->phy; event.biginfo_report.framing = ev->framing; event.biginfo_report.encryption = ev->encryption; @@ -2912,7 +3029,7 @@ ble_gap_rx_cis_estab_v2(const struct ble_hci_ev_le_subev_cis_established_v2 *ev) event.type = BLE_GAP_EVENT_CIS_ESTAB_V2; event.cis_estab_v2.status = ev->status; - event.cis_estab_v2.cis_handle = ev->conn_handle; + event.cis_estab_v2.cis_handle = le16toh(ev->conn_handle); event.cis_estab_v2.cig_sync_delay = get_le24(ev->cig_sync_delay); event.cis_estab_v2.cis_sync_delay = get_le24(ev->cis_sync_delay); event.cis_estab_v2.transport_latency_c_to_p = get_le24(ev->transport_latency_c_to_p); @@ -2924,12 +3041,12 @@ ble_gap_rx_cis_estab_v2(const struct ble_hci_ev_le_subev_cis_established_v2 *ev) event.cis_estab_v2.bn_p_to_c = ev->bn_p_to_c; event.cis_estab_v2.ft_c_to_p = ev->ft_c_to_p; event.cis_estab_v2.ft_p_to_c = ev->ft_p_to_c; - event.cis_estab_v2.max_pdu_c_to_p = ev->max_pdu_c_to_p; - event.cis_estab_v2.max_pdu_p_to_c = ev->max_pdu_p_to_c; - event.cis_estab_v2.iso_interval = ev->iso_interval; + event.cis_estab_v2.max_pdu_c_to_p = le16toh(ev->max_pdu_c_to_p); + event.cis_estab_v2.max_pdu_p_to_c = le16toh(ev->max_pdu_p_to_c); + event.cis_estab_v2.iso_interval = le16toh(ev->iso_interval); event.cis_estab_v2.sub_interval = get_le24(ev->sub_interval); - event.cis_estab_v2.max_sdu_c_to_p = ev->max_sdu_c_to_p; - event.cis_estab_v2.max_sdu_p_to_c = ev->max_sdu_p_to_c; + event.cis_estab_v2.max_sdu_c_to_p = le16toh(ev->max_sdu_c_to_p); + event.cis_estab_v2.max_sdu_p_to_c = le16toh(ev->max_sdu_p_to_c); event.cis_estab_v2.sdu_interval_c_to_p = get_le24(ev->sdu_interval_c_to_p); event.cis_estab_v2.sdu_interval_p_to_c = get_le24(ev->sdu_interval_p_to_c); event.cis_estab_v2.framing = ev->framing; @@ -3039,7 +3156,8 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) { #if NIMBLE_BLE_SCAN if (ble_gap_rx_adv_report_sanity_check(desc->data, desc->length_data, - desc->event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP)) { + desc->event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP, + 0)) { return; } @@ -3052,14 +3170,20 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) void ble_gap_rx_le_scan_timeout(void) { - ble_gap_disc_complete(); + if (ble_gap_disc_active()) { + ble_gap_disc_complete(); + } } void ble_gap_rx_ext_adv_report(struct ble_gap_ext_disc_desc *desc) { + int skip_flags; + + skip_flags = desc->data_status != BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE; if (ble_gap_rx_adv_report_sanity_check(desc->data, desc->length_data, - desc->props & BLE_HCI_ADV_SCAN_RSP_MASK)) { + desc->props & BLE_HCI_ADV_SCAN_RSP_MASK, + skip_flags)) { return; } @@ -3305,7 +3429,7 @@ ble_gap_rx_periodic_adv_rpt(const struct ble_hci_ev_le_subev_periodic_adv_rpt *e event.periodic_report.data_length = ev->data_len; event.periodic_report.data = ev->data; #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) - event.periodic_report.event_counter = ev->event_counter; + event.periodic_report.event_counter = le16toh(ev->event_counter); event.periodic_report.subevent = ev->subevent; #endif @@ -3467,7 +3591,9 @@ ble_gap_rx_conn_iq_report(const struct ble_hci_ev_le_subev_conn_iq_rpt *ev) event.conn_iq_report.q_samples = (int8_t *)(ev->iq_samples + ev->sample_count); ble_gap_event_listener_call(&event); +#if NIMBLE_BLE_CONNECT ble_gap_call_conn_event_cb(&event, event.conn_iq_report.conn_handle); +#endif } @@ -3485,7 +3611,9 @@ ble_gap_rx_cte_req_failed(const struct ble_hci_ev_le_subev_cte_req_failed *ev) event.cte_req_fail.conn_handle = conn_handle; ble_gap_event_listener_call(&event); +#if NIMBLE_BLE_CONNECT ble_gap_call_conn_event_cb(&event, conn_handle); +#endif } #endif @@ -3504,8 +3632,8 @@ periodic_adv_transfer_disable(uint16_t conn_handle) cmd.conn_handle = htole16(conn_handle); cmd.sync_cte_type = 0x00; cmd.mode = 0x00; - cmd.skip = 0x0000; - cmd.sync_timeout = 0x000a; + cmd.skip = htole16(0x0000); + cmd.sync_timeout = htole16(0x000a); rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp)); if (!rc) { @@ -3651,10 +3779,21 @@ void ble_gap_rx_subrate_change(const struct ble_hci_ev_le_subev_subrate_change *ev) { struct ble_gap_event event; + struct ble_hs_conn *conn; uint16_t conn_handle; conn_handle = le16toh(ev->conn_handle); + if (ev->status == 0) { + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn != NULL) { + conn->bhc_latency = le16toh(ev->periph_latency); + conn->bhc_supervision_timeout = le16toh(ev->supervision_tmo); + } + ble_hs_unlock(); + } + memset(&event, 0x0, sizeof event); event.type = BLE_GAP_EVENT_SUBRATE_CHANGE; @@ -3704,16 +3843,16 @@ ble_gap_rx_periodic_adv_response(const struct ble_gap_periodic_adv_response resp ble_gap_event_fn *cb; void *cb_arg; + if (resp.adv_handle >= BLE_ADV_INSTANCES) { + return; + } + memset(&event, 0x0, sizeof event); event.type = BLE_GAP_EVENT_PER_SUBEV_RESP; memcpy(&event.periodic_adv_response, &resp, sizeof(struct ble_gap_periodic_adv_response)); - if (resp.adv_handle >= BLE_ADV_INSTANCES) { - return; - } - ble_gap_slave_get_cb(resp.adv_handle, &cb, &cb_arg); if (cb != NULL) { cb(&event, cb_arg); @@ -4140,11 +4279,11 @@ ble_gap_rx_rd_rem_ver_info_complete(const struct ble_hci_ev_rd_rem_ver_info_cmp } conn->bhc_rd_rem_ver_params.version = ev->version; - conn->bhc_rd_rem_ver_params.manufacturer = ev->manufacturer; - conn->bhc_rd_rem_ver_params.subversion = ev->subversion; + conn->bhc_rd_rem_ver_params.manufacturer = le16toh(ev->manufacturer); + conn->bhc_rd_rem_ver_params.subversion = le16toh(ev->subversion); if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { - ble_gap_rd_rem_sup_feat_tx(ev->conn_handle); + ble_gap_rd_rem_sup_feat_tx(le16toh(ev->conn_handle)); } else { ble_gap_event_connect_call(ev->conn_handle, 0); } @@ -4157,6 +4296,7 @@ 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; @@ -4174,13 +4314,19 @@ ble_gap_rx_l2cap_update_req(uint16_t conn_handle, ble_hs_unlock(); #endif + struct ble_gap_upd_params self_params; + 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; + self_params = *params; + event.conn_update_req.self_params = &self_params; ble_gap_event_listener_call(&event); - return ble_gap_call_conn_event_cb(&event, conn_handle); + rc = ble_gap_call_conn_event_cb(&event, conn_handle); + *params = self_params; + return rc; #else return BLE_HS_ENOTSUP; #endif @@ -4195,7 +4341,7 @@ ble_gap_rx_phy_update_complete(const struct ble_hci_ev_le_subev_phy_update_compl memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_PHY_UPDATE_COMPLETE; - event.phy_updated.status = ev->status; + event.phy_updated.status = ev->status ? BLE_HS_HCI_ERR(ev->status) : 0; event.phy_updated.conn_handle = conn_handle; event.phy_updated.tx_phy = ev->tx_phy; event.phy_updated.rx_phy = ev->rx_phy; @@ -4326,9 +4472,6 @@ ble_gap_slave_timer(void) return 100; } - /* Clear the timer and cancel the current procedure. */ - ble_gap_slave_reset_state(0); - /* Indicate to application that advertising has stopped. */ ble_gap_adv_finished(0, BLE_HS_ETIMEOUT, 0, 0); @@ -4408,7 +4551,10 @@ ble_gap_timer(void) min_ticks = min(master_ticks, update_ticks); #if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV) - min_ticks = min(min_ticks, ble_gap_slave_timer()); + int32_t slave_ticks; + + slave_ticks = ble_gap_slave_timer(); + min_ticks = min(min_ticks, slave_ticks); #endif return min_ticks; @@ -4438,6 +4584,9 @@ ble_gap_wl_busy(void) int ble_gap_wl_tx_add(const ble_addr_t *addr) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } struct ble_hci_le_add_whte_list_cp cmd; if (addr->type > BLE_ADDR_RANDOM && @@ -4457,6 +4606,10 @@ ble_gap_wl_tx_add(const ble_addr_t *addr) int ble_gap_wl_tx_clear(void) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLEAR_WHITE_LIST), NULL, 0, NULL, 0 ); @@ -4466,6 +4619,10 @@ int ble_gap_wl_read_size(uint8_t *size) { struct ble_hci_le_rd_white_list_rp rsp; int rc; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_WHITE_LIST_SIZE), NULL, 0, &rsp, sizeof(rsp)); @@ -4476,7 +4633,6 @@ int ble_gap_wl_read_size(uint8_t *size) { return rc; } -#endif int ble_gap_wl_tx_rmv(const ble_addr_t *addr) @@ -4499,6 +4655,7 @@ ble_gap_wl_tx_rmv(const ble_addr_t *addr) BLE_HCI_OCF_LE_RMV_WHITE_LIST), &cmd, sizeof(cmd), NULL, 0); } +#endif int ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count) @@ -4519,6 +4676,10 @@ ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count) return BLE_HS_EDISABLED; } + if (white_list_count > 0 && addrs == NULL) { + return BLE_HS_EINVAL; + } + ble_hs_lock(); for (i = 0; i < white_list_count; i++) { @@ -4707,14 +4868,9 @@ ble_gap_adv_params_tx(uint8_t own_addr_type, const ble_addr_t *peer_addr, uint16_t max; /* Fill optional fields if application did not specify them. */ - if ((adv_params->itvl_min == 0) && (adv_params->itvl_max == 0)) { - ble_gap_adv_dflt_itvls(adv_params->conn_mode, &min, &max); - cmd.min_interval = htole16(min); - cmd.max_interval = htole16(max); - } else { - cmd.min_interval = htole16(adv_params->itvl_min); - cmd.max_interval = htole16(adv_params->itvl_max); - } + ble_gap_adv_dflt_itvls(adv_params->conn_mode, &min, &max); + cmd.min_interval = htole16(adv_params->itvl_min ? adv_params->itvl_min : min); + cmd.max_interval = htole16(adv_params->itvl_max ? adv_params->itvl_max : max); cmd.type = ble_gap_adv_type(adv_params); cmd.own_addr_type = own_addr_type; @@ -4779,6 +4935,12 @@ ble_gap_adv_validate(uint8_t own_addr_type, const ble_addr_t *peer_addr, 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; + } + switch (adv_params->conn_mode) { case BLE_GAP_CONN_MODE_NON: /* High duty cycle only allowed for directed advertising. */ @@ -5077,7 +5239,7 @@ int ble_gap_adv_set_data(const uint8_t *data, int data_len) { #if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV) - struct ble_hci_le_set_adv_data_cp cmd; + struct ble_hci_le_set_adv_data_cp cmd = {0}; uint16_t opcode; STATS_INC(ble_gap_stats, adv_set_data); @@ -5108,21 +5270,26 @@ int ble_gap_adv_rsp_set_data(const uint8_t *data, int data_len) { #if NIMBLE_BLE_ADVERTISE && !MYNEWT_VAL(BLE_EXT_ADV) - struct ble_hci_le_set_scan_rsp_data_cp cmd; + struct ble_hci_le_set_scan_rsp_data_cp cmd = {0}; uint16_t opcode; if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } - /* Check for valid parameters */ - if (((data == NULL) && (data_len != 0)) || - (data_len > BLE_HCI_MAX_SCAN_RSP_DATA_LEN)) { + /* Check for valid parameters. Reject negative data_len (consistent with + * sibling ble_gap_adv_set_data) before the data_len > 0 guard can mask it + * by allowing execution to reach cmd.scan_rsp_len = data_len, which would + * truncate a negative int to uint8_t and corrupt the HCI command. */ + if (data_len < 0 || data_len > BLE_HCI_MAX_SCAN_RSP_DATA_LEN || + ((data == NULL) && (data_len != 0))) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } - memcpy(cmd.scan_rsp, data, data_len); + if (data_len > 0) { + memcpy(cmd.scan_rsp, data, data_len); + } cmd.scan_rsp_len = data_len; opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA); @@ -5144,6 +5311,11 @@ ble_gap_adv_set_fields(const struct ble_hs_adv_fields *adv_fields) if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } + + if (adv_fields == NULL) { + return BLE_HS_EINVAL; + } + #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT memcpy(&ble_adv_reattempt.fields, adv_fields, sizeof( struct ble_hs_adv_fields)); #endif @@ -5172,6 +5344,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; @@ -5192,7 +5372,28 @@ int ble_gap_adv_active(void) { #if NIMBLE_BLE_ADVERTISE +#if MYNEWT_VAL(BLE_EXT_ADV) + int i; + +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_gap_vars) { + return 0; + } +#endif + for (i = 0; i < BLE_ADV_INSTANCES; i++) { + if (ble_gap_adv_active_instance(i)) { + return 1; + } + } + return 0; +#else +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_gap_vars) { + return 0; + } +#endif return ble_gap_adv_active_instance(0); +#endif #else return 0; #endif @@ -5409,6 +5610,28 @@ ble_gap_ext_adv_params_validate(const struct ble_gap_ext_adv_params *params) return BLE_HS_EINVAL; } + if ((params->itvl_min == 0) != (params->itvl_max == 0)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + + if (params->itvl_max && +#if MYNEWT_VAL(BLE_HIGH_DUTY_ADV_ITVL) + (params->itvl_max < 0x5) +#else + (params->itvl_max < 0x20) +#endif + ) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + + if (params->itvl_min && params->itvl_max && + params->itvl_min > params->itvl_max) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + /* Don't allow connectable advertising if we won't be able to allocate * a new connection. */ @@ -5471,14 +5694,6 @@ ble_gap_ext_adv_configure(uint8_t instance, return rc; } -#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT - ble_adv_reattempt.instance = instance; - - ble_adv_reattempt.cb = cb; - - ble_adv_reattempt.cb_arg = cb_arg; -#endif - ble_hs_lock(); #if NIMBLE_BLE_ADVERTISE || NIMBLE_BLE_CONNECT @@ -5498,6 +5713,11 @@ ble_gap_ext_adv_configure(uint8_t instance, ble_gap_slave[instance].configured = 1; ble_gap_slave[instance].cb = cb; ble_gap_slave[instance].cb_arg = cb_arg; +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT + ble_adv_reattempt.instance = instance; + ble_adv_reattempt.cb = cb; + ble_adv_reattempt.cb_arg = cb_arg; +#endif ble_gap_slave[instance].our_addr_type = params->own_addr_type; ble_gap_slave[instance].connectable = params->connectable; @@ -5613,13 +5833,6 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) return BLE_HS_EDISABLED; } -#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 MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) ble_gap_apply_deferred_pvcy_add(); #endif @@ -5701,6 +5914,13 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) } } +#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 + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE); cmd = (void *) buf; @@ -5794,7 +6014,14 @@ ble_gap_ext_adv_stop(uint8_t instance) static int ble_gap_ext_adv_set_data_validate(uint8_t instance, struct os_mbuf *data) { - uint16_t len = OS_MBUF_PKTLEN(data); + uint16_t len; + + if (data == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + + len = OS_MBUF_PKTLEN(data); if (!ble_gap_slave[instance].configured) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); @@ -5817,6 +6044,11 @@ ble_gap_ext_adv_set_data_validate(uint8_t instance, struct os_mbuf *data) return 0; } + 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; + } + /* if already advertising, data must fit in single HCI command * as per BT 5.0 Vol 2, Part E, 7.8.54. Don't bother Controller with such * a request. @@ -5948,14 +6180,6 @@ ble_gap_ext_adv_set_data(uint8_t instance, struct os_mbuf *data) rc = BLE_HS_EDISABLED; goto done; } -#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && NIMBLE_BLE_CONNECT - ble_adv_reattempt.type = 1; - ble_adv_reattempt.instance = instance; - - if (ble_adv_reattempt.retry) { - ble_adv_reattempt.retry = 0; - } -#endif ble_hs_lock(); rc = ble_gap_ext_adv_set_data_validate(instance, data); @@ -6087,6 +6311,14 @@ ble_gap_ext_adv_remove(uint8_t instance) return BLE_HS_EBUSY; } +#if MYNEWT_VAL(BLE_PERIODIC_ADV) + if (ble_gap_slave[instance].periodic_op == BLE_GAP_OP_S_PERIODIC_ADV) { + ble_hs_unlock(); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBUSY); + return BLE_HS_EBUSY; + } +#endif + cmd.adv_handle = instance; opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REMOVE_ADV_SET); @@ -6253,8 +6485,8 @@ ble_gap_periodic_adv_configure(uint8_t instance, /* The corresponding extended advertising instance should be configured */ if (!ble_gap_slave[instance].configured) { ble_hs_unlock(); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; } /* Periodic advertising shall not be configured while it is already @@ -6263,8 +6495,8 @@ ble_gap_periodic_adv_configure(uint8_t instance, */ if (ble_gap_slave[instance].periodic_op == BLE_GAP_OP_S_PERIODIC_ADV) { ble_hs_unlock(); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); - return BLE_HS_EINVAL; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBUSY); + return BLE_HS_EBUSY; } rc = ble_gap_periodic_adv_params_tx(instance, params); @@ -6369,6 +6601,11 @@ ble_gap_periodic_adv_set(uint8_t instance, struct os_mbuf **data) if (*data) len = OS_MBUF_PKTLEN(*data); + 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; + } + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA); cmd->adv_handle = instance; @@ -6550,6 +6787,14 @@ ble_gap_periodic_adv_stop_no_lock(uint8_t instance) uint16_t opcode; int rc; + if (!ble_gap_slave[instance].periodic_configured) { + return BLE_HS_EINVAL; + } + + if (ble_gap_slave[instance].periodic_op != BLE_GAP_OP_S_PERIODIC_ADV) { + return BLE_HS_EALREADY; + } + cmd.enable = 0x00; cmd.adv_handle = instance; @@ -6634,6 +6879,10 @@ ble_gap_periodic_adv_sync_create(const ble_addr_t *addr, uint8_t adv_sid, BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } + if (params == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } if ((params->skip > 0x1f3) || (params->sync_timeout > 0x4000) || (params->sync_timeout < 0x0A)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); @@ -6734,6 +6983,10 @@ ble_gap_periodic_adv_sync_create_cancel(void) uint16_t opcode; int rc = 0; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + ble_hs_lock(); if (ble_gap_sync.op != BLE_GAP_OP_SYNC) { @@ -6978,6 +7231,12 @@ periodic_adv_transfer_enable(uint16_t conn_handle, cmd.skip = htole16(params->skip); cmd.sync_timeout = htole16(params->sync_timeout); + } else { + /* params == NULL: enable with reporting (mode=2) and minimum valid + * sync_timeout per BT spec. Mode=0 would disable sync, contradicting + * this function's enable semantics. */ + cmd.mode = 0x02; + cmd.sync_timeout = htole16(0x000A); } rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp)); @@ -7035,6 +7294,13 @@ ble_gap_periodic_adv_sync_receive(uint16_t conn_handle, return BLE_HS_EDISABLED; } + if (params) { + if ((params->skip > 0x1f3) || (params->sync_timeout > 0x4000) || + (params->sync_timeout < 0x0A)) { + return BLE_HS_EINVAL; + } + } + ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); @@ -7092,20 +7358,26 @@ ble_gap_add_dev_to_periodic_adv_list(const ble_addr_t *peer_addr, struct ble_hci_le_add_dev_to_periodic_adv_list_cp cmd; uint16_t opcode; - if (peer_addr == NULL) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + if (peer_addr == NULL || (adv_sid > 0x0f)) { return BLE_HS_EINVAL; } - if ((peer_addr->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (peer_addr->type > BLE_ADDR_RANDOM_ID) { + return BLE_HS_EINVAL; } if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } - cmd.peer_addr_type = peer_addr->type; + /* Map identity address types to HCI-compliant values */ + if (peer_addr->type == BLE_ADDR_PUBLIC_ID) { + cmd.peer_addr_type = BLE_ADDR_PUBLIC; + } else if (peer_addr->type == BLE_ADDR_RANDOM_ID) { + cmd.peer_addr_type = BLE_ADDR_RANDOM; + } else { + cmd.peer_addr_type = peer_addr->type; + } memcpy(cmd.peer_addr, peer_addr->val, BLE_DEV_ADDR_LEN); cmd.sid = adv_sid; @@ -7121,15 +7393,26 @@ ble_gap_rem_dev_from_periodic_adv_list(const ble_addr_t *peer_addr, uint8_t adv_ struct ble_hci_le_rem_dev_from_periodic_adv_list_cp cmd; uint16_t opcode; - if (peer_addr == NULL || (peer_addr->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (peer_addr == NULL || (adv_sid > 0x0f)) { + return BLE_HS_EINVAL; + } + + if (peer_addr->type > BLE_ADDR_RANDOM_ID) { + return BLE_HS_EINVAL; } if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } - cmd.peer_addr_type = peer_addr->type; + /* Map identity address types to HCI-compliant values */ + if (peer_addr->type == BLE_ADDR_PUBLIC_ID) { + cmd.peer_addr_type = BLE_ADDR_PUBLIC; + } else if (peer_addr->type == BLE_ADDR_RANDOM_ID) { + cmd.peer_addr_type = BLE_ADDR_RANDOM; + } else { + cmd.peer_addr_type = peer_addr->type; + } memcpy(cmd.peer_addr, peer_addr->val, BLE_DEV_ADDR_LEN); cmd.sid = adv_sid; @@ -7205,9 +7488,14 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents, { struct ble_hci_le_set_periodic_adv_subev_data_cp *cmd; struct periodic_adv_subevents *subevents; - uint16_t cmd_len = ble_gap_calc_periodic_adv_data_size(num_subevents, params) + 2; + uint16_t cmd_len; + + if (num_subevents > 0 && params == NULL) { + return BLE_HS_EINVAL; + } + + cmd_len = ble_gap_calc_periodic_adv_data_size(num_subevents, params) + 2; uint16_t opcode; - uint16_t subev_data_len; uint8_t buf_size; uint8_t param_size; uint8_t buf_offset; @@ -7276,11 +7564,16 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents, subevents->response_slot_start = params[i].response_slot_start; subevents->response_slot_count = params[i].response_slot_count; cmd->num_subevents ++; - ble_hs_mbuf_to_flat(params[i].data, subevents->subevent_data, OS_MBUF_PKTLEN(params[i].data), - (void*)&subev_data_len); + rc = ble_hs_mbuf_to_flat(params[i].data, subevents->subevent_data, + OS_MBUF_PKTLEN(params[i].data), NULL); + if (rc != 0) { + ble_hs_unlock(); + goto done; + } subevents->subevent_data_length = OS_MBUF_PKTLEN(params[i].data); subevents = (struct periodic_adv_subevents *)( - (uintptr_t)subevents + sizeof(struct periodic_adv_subevents) + subev_data_len); + (uintptr_t)subevents + sizeof(struct periodic_adv_subevents) + + OS_MBUF_PKTLEN(params[i].data)); } rc = ble_hs_hci_cmd_tx(opcode, cmd, buf_offset + 2, NULL, 0); @@ -7316,6 +7609,12 @@ int ble_gap_periodic_adv_set_response_data(uint16_t sync_handle, if (data) { data_len = OS_MBUF_PKTLEN(data); } + + if (len + data_len > 255) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + uint8_t buf[len + data_len]; //!TODO: Check if we can set all of data in one hci command. @@ -7335,11 +7634,6 @@ int ble_gap_periodic_adv_set_response_data(uint16_t sync_handle, opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_RESPONSE_DATA); - if (len + data_len > 255) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); - return BLE_HS_EINVAL; - } - return ble_hs_hci_cmd_tx(opcode, cmd, len + data_len, NULL, 0); } @@ -7349,10 +7643,19 @@ int ble_gap_periodic_adv_sync_subev(uint16_t sync_handle, uint8_t include_tx_pow struct ble_hci_le_set_periodic_adv_sync_subevent *cmd; struct ble_hci_le_set_periodic_adv_sync_subevent_rp rsp; struct ble_hs_periodic_sync *psync; - uint8_t cmd_buf[sizeof(*cmd) + num_subevents]; uint16_t opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_SYNC_SUBEVENT); int rc; + if (num_subevents > 0 && subevents == NULL) { + return BLE_HS_EINVAL; + } + + if (num_subevents == 0 || num_subevents > 128) { + return BLE_HS_EINVAL; + } + + uint8_t cmd_buf[sizeof(struct ble_hci_le_set_periodic_adv_sync_subevent) + num_subevents]; + ble_hs_lock(); psync = ble_hs_periodic_sync_find_by_handle(sync_handle); ble_hs_unlock(); @@ -7466,8 +7769,23 @@ ble_gap_set_connless_cte_transmit_params(uint8_t instance, const struct ble_gap_ return BLE_HS_EINVAL; } + uint8_t switching_pattern_len; + + if (params->switching_pattern_length < 0x02 || + params->switching_pattern_length > 0x4B) { + return BLE_HS_EINVAL; + } + if (params->antenna_ids == NULL) { + return BLE_HS_EINVAL; + } + switching_pattern_len = params->switching_pattern_length; + + if (instance >= BLE_ADV_INSTANCES) { + return BLE_HS_EINVAL; + } + uint8_t buf[sizeof(struct ble_hci_le_set_connless_cte_tx_params_cp) + - params->switching_pattern_length]; + switching_pattern_len]; struct ble_hci_le_set_connless_cte_tx_params_cp *cmd = (void *)buf; uint8_t len = sizeof(buf); uint16_t opcode; @@ -7482,8 +7800,8 @@ 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; - memcpy(cmd->switching_pattern, params->antenna_ids, params->switching_pattern_length); + cmd->switching_pattern_len = switching_pattern_len; + memcpy(cmd->switching_pattern, params->antenna_ids, switching_pattern_len); return ble_hs_hci_cmd_tx(opcode, cmd, len, NULL, 0); } @@ -7494,6 +7812,10 @@ ble_gap_set_connless_cte_transmit_enable(uint8_t instance, uint8_t cte_enable) struct ble_hci_le_set_connless_cte_tx_enable_cp cmd; uint16_t opcode; + if (instance >= BLE_ADV_INSTANCES) { + return BLE_HS_EINVAL; + } + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -7515,8 +7837,18 @@ ble_gap_set_connless_iq_sampling_enable(uint16_t sync_handle, uint8_t sampling_e return BLE_HS_EINVAL; } + if (sampling_enable) { + if (cte_sampling_params->switching_pattern_length < 0x02 || + cte_sampling_params->switching_pattern_length > 0x4B) { + return BLE_HS_EINVAL; + } + if (cte_sampling_params->antenna_ids == NULL) { + return BLE_HS_EINVAL; + } + } + uint8_t buf[sizeof(struct ble_hci_le_set_connless_iq_sampling_enable_cp) + - cte_sampling_params->switching_pattern_length]; + (sampling_enable ? cte_sampling_params->switching_pattern_length : 0)]; struct ble_hci_le_set_connless_iq_sampling_enable_cp *cmd = (struct ble_hci_le_set_connless_iq_sampling_enable_cp*)buf; struct ble_hci_le_set_connless_iq_sampling_enable_rp rsp; uint8_t len = sizeof(buf); @@ -7530,8 +7862,13 @@ ble_gap_set_connless_iq_sampling_enable(uint16_t sync_handle, uint8_t sampling_e cmd->sampling_enable = sampling_enable; 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 (sampling_enable) { + 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); + } else { + cmd->switching_pattern_len = 0; + } return ble_hs_hci_cmd_tx(opcode, cmd, len, &rsp, sizeof(rsp)); } @@ -7539,11 +7876,26 @@ ble_gap_set_connless_iq_sampling_enable(uint16_t sync_handle, uint8_t sampling_e int ble_gap_set_conn_cte_recv_param(uint16_t conn_handle, uint8_t sampling_enable, const struct ble_gap_cte_sampling_params *cte_sampling_params) { - if (!ble_hs_is_enabled() || cte_sampling_params == NULL) { + if (cte_sampling_params == NULL) { + return BLE_HS_EINVAL; + } + + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } - uint8_t buf[sizeof(struct ble_hci_le_set_conn_cte_rx_params_cp) + cte_sampling_params->switching_pattern_length]; + if (sampling_enable) { + if (cte_sampling_params->switching_pattern_length < 0x02 || + cte_sampling_params->switching_pattern_length > 0x4B) { + return BLE_HS_EINVAL; + } + if (cte_sampling_params->antenna_ids == NULL) { + return BLE_HS_EINVAL; + } + } + + uint8_t buf[sizeof(struct ble_hci_le_set_conn_cte_rx_params_cp) + + (sampling_enable ? cte_sampling_params->switching_pattern_length : 0)]; struct ble_hci_le_set_conn_cte_rx_params_cp *cmd = (void *)buf; struct ble_hci_le_set_conn_cte_rx_params_rp rsp; uint8_t len = sizeof(buf); @@ -7554,8 +7906,13 @@ ble_gap_set_conn_cte_recv_param(uint16_t conn_handle, uint8_t sampling_enable, c cmd->conn_handle = htole16(conn_handle); 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 (sampling_enable) { + 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); + } else { + cmd->switching_pattern_len = 0; + } return ble_hs_hci_cmd_tx(opcode, cmd, len, &rsp, sizeof(rsp)); } @@ -7563,6 +7920,10 @@ ble_gap_set_conn_cte_recv_param(uint16_t conn_handle, uint8_t sampling_enable, c int ble_gap_set_conn_cte_transmit_param(uint16_t conn_handle, uint8_t cte_types, uint8_t switching_pattern_len, const uint8_t *antenna_ids) { + if (switching_pattern_len > 0x4B) { + return BLE_HS_EINVAL; + } + uint8_t buf[sizeof(struct ble_hci_le_set_conn_cte_tx_params_cp) + switching_pattern_len]; struct ble_hci_le_set_conn_cte_tx_params_cp *cmd = (void *)buf; struct ble_hci_le_set_conn_cte_tx_params_rp rsp; @@ -7577,12 +7938,18 @@ ble_gap_set_conn_cte_transmit_param(uint16_t conn_handle, uint8_t cte_types, uin cmd->conn_handle = htole16(conn_handle); cmd->cte_types = cte_types; - if (cmd->cte_types != BIT(0) && (switching_pattern_len == 0 || antenna_ids == NULL)) { + if (cmd->cte_types != BIT(0) && switching_pattern_len == 0) { + BLE_HS_LOG(ERROR, "Invalid antenna_ids!\n"); + return BLE_HS_EINVAL; + } + if (switching_pattern_len > 0 && antenna_ids == NULL) { BLE_HS_LOG(ERROR, "Invalid antenna_ids!\n"); return BLE_HS_EINVAL; } cmd->switching_pattern_len = switching_pattern_len; - memcpy(cmd->antenna_ids, antenna_ids, switching_pattern_len); + if (switching_pattern_len > 0) { + memcpy(cmd->antenna_ids, antenna_ids, switching_pattern_len); + } return ble_hs_hci_cmd_tx(opcode, cmd, len, &rsp, sizeof(rsp)); @@ -7641,6 +8008,11 @@ ble_gap_read_antenna_information(uint8_t *switch_sampling_rates, uint8_t *num_an return BLE_HS_EDISABLED; } + if (switch_sampling_rates == NULL || num_antennae == NULL || + max_switch_pattern_len == NULL || max_cte_len == NULL) { + return BLE_HS_EINVAL; + } + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ANTENNA_INFO); rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, &rsp, sizeof(rsp)); @@ -7869,10 +8241,18 @@ 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); - ble_gap_master.disc.observer = !uncoded_params->disable_observer_mode; + + if (ucp.scan_window > ucp.scan_itvl) { + rc = BLE_HS_EINVAL; + goto done; + } + + ble_gap_master.disc.observer |= !uncoded_params->disable_observer_mode; /* XXX: We should do it only once */ if (!uncoded_params->passive) { @@ -7886,7 +8266,13 @@ ble_gap_ext_disc(uint8_t own_addr_type, uint16_t duration, uint16_t period, if (coded_params) { ble_gap_ext_scan_params_to_hci(coded_params, &cp); ble_gap_ext_disc_fill_dflts(limited, &cp); - ble_gap_master.disc.observer = !coded_params->disable_observer_mode; + + if (cp.scan_window > cp.scan_itvl) { + rc = BLE_HS_EINVAL; + goto done; + } + + ble_gap_master.disc.observer |= !coded_params->disable_observer_mode; /* XXX: We should do it only once */ if (!coded_params->passive) { @@ -7993,6 +8379,10 @@ ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms, ble_adv_list_refresh(); #endif + if (duration_ms < 0) { + return BLE_HS_EINVAL; + } + #if MYNEWT_VAL(BLE_EXT_ADV) struct ble_gap_ext_disc_params p = {0}; @@ -8027,6 +8417,10 @@ ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms, STATS_INC(ble_gap_stats, discover); + if (disc_params == NULL) { + return BLE_HS_EINVAL; + } + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -8113,6 +8507,11 @@ int ble_gap_disc_active(void) { #if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_OBSERVER) +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gap_vars == NULL) { + return 0; + } +#endif /* Assume read is atomic; mutex not necessary. */ return ble_gap_master.op == BLE_GAP_OP_M_DISC; #else @@ -8186,6 +8585,9 @@ ble_gap_subrate_req(uint16_t conn_handle, uint16_t subrate_min, uint16_t subrate uint16_t max_latency, uint16_t cont_num, uint16_t supervision_tmo) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } struct ble_hci_le_subrate_req_cp cmd; uint16_t opcode; @@ -8239,6 +8641,13 @@ ble_gap_check_conn_params(uint8_t phy, const struct ble_gap_conn_params *params) return BLE_ERR_INV_HCI_CMD_PARMS; } + /* supervision_timeout (in 10ms units) must be > (1 + latency) * itvl_max * 1.25ms * 2 + * i.e. supervision_timeout * 4 > (1 + latency) * itvl_max */ + if ((uint32_t)params->supervision_timeout * 4 <= + (uint32_t)(1 + params->latency) * params->itvl_max) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + /* Check connection event length */ if (params->min_ce_len > params->max_ce_len) { return BLE_ERR_INV_HCI_CMD_PARMS; @@ -8777,6 +9186,11 @@ ble_gap_ext_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, goto done; } + if (phy_mask == 0) { + rc = BLE_HS_EINVAL; + goto done; + } + if ((phy_mask & BLE_GAP_LE_PHY_1M_MASK) && phy_1m_conn_params == NULL) { phy_1m_conn_params = &ble_gap_conn_params_dflt; } @@ -8791,6 +9205,11 @@ ble_gap_ext_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, phy_coded_conn_params = &ble_gap_conn_params_dflt; } + if (duration_ms < 0) { + rc = BLE_HS_EINVAL; + goto done; + } + if (duration_ms == 0) { duration_ms = BLE_GAP_CONN_DUR_DFLT; } @@ -9027,7 +9446,7 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, if (rl != NULL && rl->rl_isrpa) { memcpy(bhc_peer_addr.val, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); - bhc_peer_addr.type = rl->rl_addr_type; + bhc_peer_addr.type = BLE_ADDR_RANDOM; } } #endif @@ -9112,6 +9531,9 @@ ble_gap_common_factor_set(bool enable, uint32_t common_factor) } if (enable) { + if (common_factor == 0) { + return BLE_HS_EINVAL; + } vs_cmd[0] = common_factor & 0xfful; vs_cmd[1] = (common_factor >> 8) & 0xfful; vs_cmd[2] = (common_factor >> 16) & 0xfful; @@ -9614,13 +10036,17 @@ ble_gap_rx_param_req(const struct ble_hci_ev_le_subev_rem_conn_param_req *ev) #endif rc = ble_gap_call_conn_event_cb(&event, conn_handle); + if (rc == BLE_HS_ENOTCONN) { + ble_gap_tx_param_neg_reply(conn_handle, BLE_ERR_CONN_PARMS); + return; + } if (rc == 0) { rc = ble_gap_tx_param_pos_reply(conn_handle, &self_params); if (rc != 0) { ble_gap_update_failed(conn_handle, rc); } } else { - ble_gap_tx_param_neg_reply(conn_handle, rc); + ble_gap_tx_param_neg_reply(conn_handle, BLE_ERR_CONN_PARMS); } #endif } @@ -9677,6 +10103,10 @@ ble_gap_validate_conn_params(const struct ble_gap_upd_params *params) return false; } + if (params->min_ce_len > params->max_ce_len) { + return false; + } + return true; } #endif @@ -9695,6 +10125,10 @@ ble_gap_update_params(uint16_t conn_handle, l2cap_update = 0; + if (params == NULL) { + return BLE_HS_EINVAL; + } + /* Validate parameters with a spec */ if (!ble_gap_validate_conn_params(params)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); @@ -9835,6 +10269,9 @@ ble_gap_read_sugg_def_data_len(uint16_t *out_sugg_max_tx_octets, uint16_t *out_sugg_max_tx_time) { #if NIMBLE_BLE_CONNECT + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } return ble_hs_hci_util_read_sugg_def_data_len(out_sugg_max_tx_octets, out_sugg_max_tx_time); #else @@ -9934,8 +10371,13 @@ done: int ble_gap_dev_authorization(uint16_t conn_handle, bool authorized) { +#if NIMBLE_BLE_SM struct ble_hs_conn *conn; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); @@ -9965,6 +10407,9 @@ ble_gap_dev_authorization(uint16_t conn_handle, bool authorized) } return 0; +#else + return BLE_HS_ENOTSUP; +#endif } int @@ -9976,6 +10421,11 @@ ble_gap_pair_initiate(uint16_t conn_handle) return BLE_HS_EDISABLED; } + rc = ble_hs_atomic_conn_flags(conn_handle, NULL); + if (rc != 0) { + return rc; + } + rc = ble_sm_pair_initiate(conn_handle); return rc; @@ -9993,6 +10443,10 @@ ble_gap_encryption_initiate(uint16_t conn_handle, ble_hs_conn_flags_t conn_flags; int rc; + if (ltk == NULL) { + return BLE_HS_EINVAL; + } + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -10015,7 +10469,7 @@ ble_gap_encryption_initiate(uint16_t conn_handle, #endif } -#if NIMBLE_BLE_SM && MYNEWT_VAL(BLE_SMP_ID_RESET) +#if NIMBLE_BLE_SM && MYNEWT_VAL(BLE_SMP_ID_RESET) && (MYNEWT_VAL(BLE_STORE_MAX_BONDS) > 0) static void ble_gap_reset_irk(void) { @@ -10027,8 +10481,12 @@ ble_gap_reset_irk(void) int rc, num_peers; uint8_t tmp_addr[6]; - ble_store_util_bonded_peers(&oldest_peer_id_addr[0], &num_peers, - MYNEWT_VAL(BLE_STORE_MAX_BONDS)); + num_peers = 0; + rc = ble_store_util_bonded_peers(&oldest_peer_id_addr[0], &num_peers, + MYNEWT_VAL(BLE_STORE_MAX_BONDS)); + if (rc != 0) { + return; + } if (num_peers != 0) { return ; @@ -10079,6 +10537,11 @@ ble_gap_unpair(const ble_addr_t *peer_addr) #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) && !MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) int defer_add_pending = 0; #endif + + if (peer_addr == NULL) { + return BLE_HS_EINVAL; + } + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -10163,18 +10626,22 @@ ble_gap_unpair(const ble_addr_t *peer_addr) rc = ble_store_util_delete_peer(&key.sec.peer_addr); if (rc != 0) { BLE_HS_LOG(ERROR, "Error while removing LTK , rc = %x\n",rc); + err = rc; } } else { rc = ble_store_read(BLE_STORE_OBJ_TYPE_OUR_SEC, &key, &value); if (!rc) { - ble_store_util_delete_peer(&key.sec.peer_addr); + rc = ble_store_util_delete_peer(&key.sec.peer_addr); + if (rc != 0) { + err = rc; + } } else { BLE_HS_LOG(ERROR,"No record found for the given address in ble store , rc = %x\n",rc); err = rc ; } } -#if MYNEWT_VAL(BLE_SMP_ID_RESET) +#if MYNEWT_VAL(BLE_SMP_ID_RESET) && (MYNEWT_VAL(BLE_STORE_MAX_BONDS) > 0) /* There are tracking risks associated with using a fixed or static IRK. * A best-practices approach, when all pairing and bonding records are deleted, * assign a new randomly-generated IRK. @@ -10304,16 +10771,19 @@ ble_gap_passkey_event(uint16_t conn_handle, 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_passkey_event: defer failed; conn=%u\n", conn_handle); + ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); break; } #else + ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif #endif @@ -10433,7 +10903,10 @@ ble_gap_repeat_pairing_event(const struct ble_gap_repeat_pairing *rp) struct ble_gap_conn_desc desc; rc = ble_gap_conn_find(rp->conn_handle, &desc); assert(rc == 0); - ble_store_util_delete_peer(&desc.peer_id_addr); + rc = ble_store_util_delete_peer(&desc.peer_id_addr); + if (rc != 0) { + return BLE_GAP_REPEAT_PAIRING_IGNORE; + } return BLE_GAP_REPEAT_PAIRING_RETRY; #else struct ble_gap_event event; @@ -10464,16 +10937,19 @@ ble_gap_pairing_complete_event(uint16_t conn_handle, int status) 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_pairing_complete_event: defer failed; conn=%u\n", conn_handle); + ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); break; } #else + ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, conn_handle); #endif #endif @@ -10589,6 +11065,8 @@ ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle, ble_gap_call_conn_event_cb(&event, conn_handle); os_mbuf_free_chain(event.notify_rx.om); +#else + os_mbuf_free_chain(om); #endif } @@ -10746,6 +11224,10 @@ ble_gap_rx_test_evt(const void *buf, uint8_t len) struct ble_hci_ev_command_complete *cmd_complete = (void *) buf; uint8_t status; + if (buf == NULL || len < sizeof(*cmd_complete)) { + return; + } + status = cmd_complete->status; memset(&event, 0, sizeof event); @@ -10764,6 +11246,10 @@ ble_gap_tx_test_evt(const void *buf, uint8_t len) struct ble_hci_ev_command_complete *cmd_complete = (void *) buf; uint8_t status; + if (buf == NULL || len < sizeof(*cmd_complete)) { + return; + } + status = cmd_complete->status; memset(&event, 0, sizeof event); @@ -10784,6 +11270,10 @@ ble_gap_end_test_evt(const void *buf, uint8_t len) uint8_t status; uint16_t num_pkt; + if (buf == NULL || len < sizeof(*cmd_complete)) { + return; + } + status = cmd_complete->status; num_pkt = 0; if (len >= sizeof(*cmd_complete) + sizeof(num_pkt)) { @@ -10858,9 +11348,20 @@ ble_gap_preempt_no_lock(void) #endif #if NIMBLE_BLE_SCAN + ble_gap_event_fn *preempt_disc_cb; + void *preempt_disc_cb_arg; + + preempt_disc_cb = NULL; + preempt_disc_cb_arg = NULL; + if (ble_gap_master.op == BLE_GAP_OP_M_DISC) { + preempt_disc_cb = ble_gap_master.cb; + preempt_disc_cb_arg = ble_gap_master.cb_arg; + } rc = ble_gap_disc_cancel_no_lock(); if (rc == 0) { ble_gap_master.preempted_op = BLE_GAP_OP_M_DISC; + ble_gap_master.cb = preempt_disc_cb; + ble_gap_master.cb_arg = preempt_disc_cb_arg; } #endif } @@ -10899,6 +11400,8 @@ ble_gap_preempt(void) static struct ble_npl_mutex preempt_done_mutex; #endif +static bool gap_preempt_mutex_initialized; + void ble_gap_preempt_done(void) { @@ -10914,6 +11417,10 @@ ble_gap_preempt_done(void) } slaves[BLE_ADV_INSTANCES]; #endif + if (!gap_preempt_mutex_initialized) { + return; + } + master_cb = NULL; master_arg = NULL; @@ -10922,9 +11429,7 @@ ble_gap_preempt_done(void) /* Protects slaves from accessing by multiple threads */ ble_npl_mutex_pend(&preempt_done_mutex, 0xFFFFFFFF); -#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) memset(slaves, 0, sizeof(slaves)); -#endif ble_hs_lock(); @@ -10999,37 +11504,37 @@ ble_gap_event_listener_register(struct ble_gap_event_listener *listener, int ble_gap_event_listener_unregister(struct ble_gap_event_listener *listener) { - struct ble_gap_event_listener *evl = NULL; - int rc; + struct ble_gap_event_listener *evl; + struct ble_gap_event_listener *prev; /* * We check if element exists on the list only for sanity to let caller * know whether it registered its listener before. */ + prev = NULL; SLIST_FOREACH(evl, &ble_gap_event_listener_list, link) { if (evl == listener) { - break; + if (prev == NULL) { + SLIST_REMOVE_HEAD(&ble_gap_event_listener_list, link); + } else { + SLIST_NEXT(prev, link) = SLIST_NEXT(evl, link); + } + return 0; } + prev = evl; } - if (!evl) { - rc = BLE_HS_ENOENT; - } else { - SLIST_REMOVE(&ble_gap_event_listener_list, listener, - ble_gap_event_listener, link); - rc = 0; - } - - return rc; + return BLE_HS_ENOENT; } static int ble_gap_event_listener_call(struct ble_gap_event *event) { struct ble_gap_event_listener *evl = NULL; + struct ble_gap_event_listener *evl_next; - SLIST_FOREACH(evl, &ble_gap_event_listener_list, link) { + SLIST_FOREACH_SAFE(evl, &ble_gap_event_listener_list, link, evl_next) { evl->fn(event, evl->arg); } @@ -11077,6 +11582,9 @@ ble_gap_accept_cis_request(uint16_t cis_handle, ble_gap_event_fn *cb, void *cb_a int ble_gap_reject_cis_request(uint16_t cis_handle, uint8_t reason) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } int rc; ble_hs_lock(); @@ -11117,6 +11625,10 @@ ble_gap_create_big(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, { int rc; + if (num_bis == 0 || num_bis > MYNEWT_VAL(BLE_ISO_BIS_PER_BIG)) { + return BLE_HS_EINVAL; + } + ble_hs_lock(); rc = ble_hs_hci_create_big(big_handle, adv_handle, num_bis, sdu_interval, @@ -11143,6 +11655,10 @@ ble_gap_create_big_test(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, { int rc; + if (num_bis == 0 || num_bis > MYNEWT_VAL(BLE_ISO_BIS_PER_BIG)) { + return BLE_HS_EINVAL; + } + ble_hs_lock(); rc = ble_hs_hci_create_big_test(big_handle, adv_handle, num_bis, sdu_interval, @@ -11206,7 +11722,7 @@ ble_gap_big_terminate_sync(uint8_t big_handle) ble_hs_lock(); rc = ble_hs_hci_big_terminate_sync(big_handle); - if (!rc) { + if (rc == 0) { ble_gap_big_snc.cb = NULL; ble_gap_big_snc.cb_arg = NULL; } @@ -11226,6 +11742,10 @@ ble_gap_init(void) { int rc; + if (gap_preempt_mutex_initialized) { + return 0; + } + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_gap_vars == NULL) { ble_gap_vars = nimble_platform_mem_calloc(1, sizeof(ble_gap_vars_t)); @@ -11269,8 +11789,9 @@ ble_gap_init(void) if (rc) { BLE_HS_LOG(ERROR, "mutex init failed with reason %d \n", rc); - goto err; + goto err; } + gap_preempt_mutex_initialized = true; SLIST_INIT(&ble_gap_update_entries); SLIST_INIT(&ble_gap_event_listener_list); @@ -11281,17 +11802,14 @@ ble_gap_init(void) ble_gap_update_entry_mem, "ble_gap_update"); if (rc != 0) { - goto err; - } - - switch (rc) { - case 0: - break; - case OS_ENOMEM: - rc = BLE_HS_ENOMEM; - goto err; - default: - rc = BLE_HS_EOS; + switch (rc) { + case OS_ENOMEM: + rc = BLE_HS_ENOMEM; + break; + default: + rc = BLE_HS_EOS; + break; + } goto err; } @@ -11305,6 +11823,10 @@ ble_gap_init(void) return 0; err: + if (gap_preempt_mutex_initialized) { + gap_preempt_mutex_initialized = false; + ble_npl_mutex_deinit(&preempt_done_mutex); + } #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_gap_vars != NULL) { #if !MYNEWT_VAL(MP_RUNTIME_ALLOC) // Doubt @@ -11332,18 +11854,26 @@ ble_gap_enh_read_transmit_power_level(uint16_t conn_handle, uint8_t phy, uint8_t uint16_t opcode; int rc; + if (out_status == NULL || out_phy == NULL || + out_curr_tx_power_level == NULL || out_max_tx_power_level == NULL) { + return BLE_HS_EINVAL; + } + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ENH_READ_TRANSMIT_POWER_LEVEL); cmd.conn_handle = htole16(conn_handle); cmd.phy = phy; + *out_status = 0; rc = ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), &rsp, sizeof(rsp)); - - if (rc!=0) { + if (rc != 0) { + if (rc >= BLE_HS_ERR_HCI_BASE && rc < BLE_HS_ERR_HCI_BASE + 0x100) { + *out_status = rc - BLE_HS_ERR_HCI_BASE; + } return rc; } + *out_status = 0; - *out_status = rc; *out_phy = rsp.phy; *out_curr_tx_power_level = rsp.curr_tx_power_level; *out_max_tx_power_level = rsp.max_tx_power_level; @@ -11450,6 +11980,10 @@ ble_gap_rd_local_resolv_addr(uint8_t peer_addr_type, const ble_addr_t *peer_addr uint16_t opcode; int rc; + if (peer_addr == NULL || out_addr == NULL) { + return BLE_HS_EINVAL; + } + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR); cmd.peer_addr_type = peer_addr_type; @@ -11470,6 +12004,7 @@ ble_gap_rd_local_resolv_addr(uint8_t peer_addr_type, const ble_addr_t *peer_addr int ble_gap_read_local_irk(uint8_t * out_irk) { +#if MYNEWT_VAL(BLE_HS_PVCY) const uint8_t * local_irk; int rc; @@ -11485,6 +12020,9 @@ ble_gap_read_local_irk(uint8_t * out_irk) memcpy(out_irk, local_irk, 16); return 0; +#else + return BLE_HS_ENOTSUP; +#endif } #if MYNEWT_VAL(BLE_HCI_VS) @@ -11494,6 +12032,10 @@ ble_gap_pcl_param_validate(struct ble_gap_set_auto_pcl_params *params) { int8_t eff_upper, eff_lower; + if (params == NULL) { + return BLE_HS_EINVAL; + } + eff_lower = params->m1_lower_limit ? params->m1_lower_limit : ESP_1M_LOW; eff_upper = params->m1_upper_limit ? params->m1_upper_limit : ESP_1M_HIGH; if (eff_upper < eff_lower) { @@ -11592,7 +12134,10 @@ ble_gap_duplicate_exception_list(uint8_t subcode, uint32_t type, uint8_t *value, { uint8_t device_info_array[1 + 4 + BLE_DEV_ADDR_LEN] = {0}; - if (!value) { + /* value is only required for types that copy data from it */ + if (!value && + (type == BLE_DUPLICATE_SCAN_EXCEPTIONAL_INFO_ADV_ADDR || + type == BLE_DUPLICATE_SCAN_EXCEPTIONAL_INFO_MESH_LINK_ID)) { return BLE_HS_EINVAL; } @@ -11629,7 +12174,11 @@ ble_gap_duplicate_exception_list(uint8_t subcode, uint32_t type, uint8_t *value, int ble_gap_clear_legacy_adv(void) { - return ble_hs_hci_send_vs_cmd(BLE_HCI_OCF_VS_LEGACY_ADV_CLEAR, + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + + return ble_hs_hci_send_vs_cmd(BLE_HCI_OCF_VS_LEGACY_ADV_CLEAR, NULL, 0, NULL, 0); } @@ -11659,13 +12208,15 @@ int ble_gap_set_scan_chan(uint8_t state, uint8_t *bitmap) #if MYNEWT_VAL(BLE_ADV_SEND_CONSTANT_DID) int ble_gap_set_adv_constant_did(uint16_t handle, uint8_t enable, uint16_t did) { - struct ble_gap_adv_const_did_cmd_params vs_cmd; - - vs_cmd.handle = handle; - vs_cmd.enable = enable; - vs_cmd.did = did; + /* Manually serialize to avoid struct-padding issues (uint8_t enable followed + * by uint16_t did would get a padding byte without __packed). Consistent with + * the pattern used by other VS commands in this file. */ + uint8_t vs_cmd[5]; + put_le16(vs_cmd + 0, handle); + vs_cmd[2] = enable; + put_le16(vs_cmd + 3, did); return ble_hs_hci_send_vs_cmd(BLE_HCI_OCF_VS_SET_ADV_DID, - &vs_cmd, sizeof(vs_cmd), NULL, 0); + vs_cmd, sizeof(vs_cmd), NULL, 0); } #endif // MYNEWT_VAL(BLE_ADV_SEND_CONSTANT_DID) @@ -11720,18 +12271,30 @@ ble_gap_set_data_related_addr_change_param(uint8_t adv_handle, uint8_t change_re int ble_gap_dtm_tx_start(uint8_t tx_chan, uint8_t test_data_len, uint8_t payload) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + return ble_hs_hci_dtm_tx_start(tx_chan, test_data_len, payload); } int ble_gap_dtm_rx_start(uint8_t rx_chan) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + return ble_hs_hci_dtm_rx_start(rx_chan); } int ble_gap_dtm_stop(void) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + return ble_hs_hci_dtm_stop(); } @@ -11739,12 +12302,20 @@ int ble_gap_dtm_enh_tx_start(uint8_t tx_chan, uint8_t test_data_len, uint8_t payload, uint8_t phy) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + return ble_hs_hci_dtm_enh_tx_start(tx_chan, test_data_len, payload, phy); } int ble_gap_dtm_enh_rx_start(uint8_t rx_chan, uint8_t index, uint8_t phy) { + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + return ble_hs_hci_dtm_enh_rx_start(rx_chan, index, phy); } #endif @@ -11764,7 +12335,10 @@ ble_gap_rd_all_remote_features(uint16_t conn_handle, uint8_t page_requested) void ble_gap_deinit(void) { - ble_npl_mutex_deinit(&preempt_done_mutex); + if (gap_preempt_mutex_initialized) { + gap_preempt_mutex_initialized = false; + ble_npl_mutex_deinit(&preempt_done_mutex); + } #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_gap_vars != NULL) { @@ -11787,10 +12361,6 @@ ble_gap_host_check_status(void) { int status = 0; - uint16_t conn_handle = 0; -#if MYNEWT_VAL(BLE_PERIODIC_ADV) - uint16_t sync_handle = 0; -#endif struct ble_hs_conn *conn; #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) ble_addr_t oldest_peer_id_addr[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; @@ -11825,17 +12395,12 @@ ble_gap_host_check_status(void) } /* Terminate links */ - for (conn_handle=0; conn_handle<50; conn_handle++) { - ble_hs_lock(); - - conn = ble_hs_conn_find(conn_handle); - - ble_hs_unlock(); - if (conn != NULL) { - BLE_HS_LOG(ERROR, "Connection exists \n"); - status |= BIT(BLE_GAP_STATUS_CONN); - break; - } + ble_hs_lock(); + conn = ble_hs_conn_first(); + ble_hs_unlock(); + if (conn != NULL) { + BLE_HS_LOG(ERROR, "Connection exists \n"); + status |= BIT(BLE_GAP_STATUS_CONN); } #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) @@ -11857,8 +12422,6 @@ ble_gap_host_check_status(void) /* Check if privacy is disabled */ #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - extern int is_ble_hs_resolv_enabled(void); - if (is_ble_hs_resolv_enabled()) { BLE_HS_LOG(ERROR, "Host based Privacy not disabled \n"); status |= BIT(BLE_GAP_STATUS_HOST_PRIVACY); @@ -11869,18 +12432,11 @@ ble_gap_host_check_status(void) #if MYNEWT_VAL(BLE_PERIODIC_ADV) struct ble_hs_periodic_sync *psync; - for (sync_handle=0; sync_handle<50; sync_handle++) { - ble_hs_lock(); + psync = ble_hs_periodic_sync_first(); - psync = ble_hs_periodic_sync_find_by_handle(sync_handle); - - ble_hs_unlock(); - - if (psync) { - BLE_HS_LOG(ERROR, "Periodic adv not disabled \n"); - status |= BIT(BLE_GAP_STATUS_PERIODIC); - break; - } + if (psync != NULL) { + BLE_HS_LOG(ERROR, "Periodic adv not disabled \n"); + status |= BIT(BLE_GAP_STATUS_PERIODIC); } #endif @@ -11888,18 +12444,21 @@ ble_gap_host_check_status(void) } #endif -#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) int ble_gap_set_host_feat(uint8_t bit_num, uint8_t bit_val) { struct ble_hci_le_set_host_feature_cp cmd; + if (bit_val > 0x01) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + cmd.bit_num=bit_num; cmd.bit_val=bit_val; return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_HOST_FEATURE),&cmd, sizeof(cmd), NULL, 0); } -#endif /* Check if input is an RPA (Resolvable Private Address) */ static inline bool is_rpa(const uint8_t *addr) @@ -11949,8 +12508,8 @@ bool ble_gap_rpa_resolve(uint8_t *rpa, uint8_t *ida, uint8_t *addr_type) struct ble_store_value_local_irk value = {0}; struct ble_store_key_local_irk key = {0}; - (void)ble_store_read_local_irk(&key, &value); - if (ble_hs_pvcy_resolve_with_irk(rpa, value.irk)) { + if (ble_store_read_local_irk(&key, &value) == 0 && + ble_hs_pvcy_resolve_with_irk(rpa, value.irk)) { /* Match with local IRK -> return ida = 00:00:00:00:00:00 */ memset(ida, 0, 6); *addr_type = BLE_ADDR_PUBLIC; /* default type */ @@ -11969,6 +12528,9 @@ ble_gap_frame_space_update(uint16_t conn_handle, uint16_t spacing_types) { #if NIMBLE_BLE_CONNECT + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } struct ble_hci_le_frame_space_update_cp cmd; struct ble_hs_conn *conn; diff --git a/nimble/host/src/ble_gattc.c b/nimble/host/src/ble_gattc.c index a4f205025..fb5ba9a7a 100644 --- a/nimble/host/src/ble_gattc.c +++ b/nimble/host/src/ble_gattc.c @@ -741,12 +741,14 @@ ble_gattc_log_write(uint16_t att_handle, uint16_t len, int expecting_rsp) BLE_HS_LOG(INFO, "att_handle=%d len=%d\n", att_handle, len); } +#if NIMBLE_BLE_ATT_CLT_SIGNED_WRITE static void ble_gattc_log_signed_write(uint16_t att_handle, uint16_t len) { ble_gattc_log_proc_init("signed write; "); BLE_HS_LOG(INFO, "att_handle=%d len=%d\n", att_handle, len); } +#endif static void ble_gattc_log_write_long(struct ble_gattc_proc *proc) @@ -794,7 +796,7 @@ static void ble_gattc_log_indicate(uint16_t att_handle) { ble_gattc_log_proc_init("indicate; "); - BLE_HS_LOG(INFO, "att_handle=%d\n", att_handle); + BLE_HS_LOG(INFO, "att_handle=%u\n", att_handle); } #if MYNEWT_VAL(BLE_GATTC) @@ -1014,6 +1016,11 @@ static ble_gattc_err_fn * ble_gattc_err_dispatch_get(uint8_t op) { BLE_HS_DBG_ASSERT(op < BLE_GATT_OP_CNT); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_ctx == NULL || ble_gattc_err_dispatch == NULL) { + return NULL; + } +#endif return ble_gattc_err_dispatch[op]; } @@ -1350,24 +1357,11 @@ ble_gattc_extract_with_rx_entry(uint16_t conn_handle, uint16_t cid, * @return The matching proc entry on success; * null on failure. */ -#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) #define BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle, cid, rx_entries, out_rx_entry) \ ble_gattc_extract_with_rx_entry( \ (conn_handle), (cid), (rx_entries), \ sizeof (rx_entries) / sizeof (rx_entries)[0], \ (const void **)(out_rx_entry)) -#else -/* - * When memory optimization flag is enabled: - * RX entry table is dynamically allocated with 4 entries - * to optimize memory usage and reduce static allocation. - */ -#define BLE_GATTC_RX_EXTRACT_RX_ENTRY(conn_handle, cid, rx_entries, out_rx_entry) \ - ble_gattc_extract_with_rx_entry( \ - (conn_handle), (cid), (rx_entries), \ - 4, \ - (const void **)(out_rx_entry)) -#endif #endif /** @@ -1391,7 +1385,9 @@ ble_gattc_fail_procs(uint16_t conn_handle, uint8_t op, int status) */ while ((proc = STAILQ_FIRST(&temp_list)) != NULL) { err_cb = ble_gattc_err_dispatch_get(proc->op); - err_cb(proc, status, 0); + if (err_cb != NULL) { + err_cb(proc, status, 0); + } STAILQ_REMOVE_HEAD(&temp_list, next); ble_gattc_proc_free(proc); @@ -1448,7 +1444,9 @@ ble_gattc_resume_procs(void) ble_gattc_extract_stalled(&stall_list); - STAILQ_FOREACH(proc, &stall_list, next) { + while ((proc = STAILQ_FIRST(&stall_list)) != NULL) { + STAILQ_REMOVE_HEAD(&stall_list, next); + resume_cb = ble_gattc_resume_dispatch_get(proc->op); BLE_HS_DBG_ASSERT(resume_cb != NULL); @@ -1734,8 +1732,8 @@ ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *cb_arg) if (proc == NULL) { BLE_HS_LOG(INFO, "GATTC proc alloc failed; conn_handle=0x%04x op=mtu\n", conn_handle); - rc = BLE_HS_ENOMEM; - goto done; + STATS_INC(ble_gattc_stats, mtu_fail); + return BLE_HS_ENOMEM; } proc->op = BLE_GATT_OP_MTU; @@ -1747,15 +1745,9 @@ ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *cb_arg) ble_gattc_log_proc_init("exchange mtu\n"); rc = ble_gattc_mtu_tx(proc); - if (rc != 0) { - goto done; - } - -done: if (rc != 0) { STATS_INC(ble_gattc_stats, mtu_fail); } - ble_gattc_process_status(proc, rc); return rc; } @@ -1891,7 +1883,8 @@ ble_gattc_disc_all_svcs_rx_adata(struct ble_gattc_proc *proc, goto done; } - if (adata->end_group_handle <= proc->disc_all_svcs.prev_handle) { + if (adata->att_handle > adata->end_group_handle || + adata->end_group_handle <= proc->disc_all_svcs.prev_handle) { /* Peer sent services out of order; terminate procedure. */ rc = BLE_HS_EBADDATA; goto done; @@ -1905,7 +1898,7 @@ ble_gattc_disc_all_svcs_rx_adata(struct ble_gattc_proc *proc, rc = 0; done: - cbrc = ble_gattc_disc_all_svcs_cb(proc, rc, 0, &service); + cbrc = ble_gattc_disc_all_svcs_cb(proc, rc, 0, rc == 0 ? &service : NULL); if (rc != 0 || cbrc != 0) { return BLE_HS_EDONE; } else { @@ -1991,13 +1984,19 @@ ble_gattc_disc_all_svcs(uint16_t conn_handle, ble_gatt_disc_svc_fn *cb, } done: + /* One ble_gattc_proc cannot be part of multiple linked lists. + * Hence it needs to be removed before going into ble_gattc_process_status. + */ +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) + if (proc != NULL) { + ble_hs_lock(); + STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); + ble_hs_unlock(); + } +#endif + if (rc != 0) { STATS_INC(ble_gattc_stats, disc_all_svcs_fail); -#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); -#endif } ble_gattc_process_status(proc, rc); @@ -2061,7 +2060,10 @@ ble_gattc_disc_svc_uuid_tx(struct ble_gattc_proc *proc) ble_gattc_dbg_assert_proc_not_inserted(proc); - ble_uuid_flat(&proc->disc_svc_uuid.service_uuid.u, val); + rc = ble_uuid_flat(&proc->disc_svc_uuid.service_uuid.u, val); + if (rc != 0) { + return rc; + } rc = ble_att_clt_tx_find_type_value(proc->conn_handle, proc->cid, proc->disc_svc_uuid.prev_handle + 1, 0xffff, BLE_ATT_UUID_PRIMARY_SERVICE, @@ -2137,7 +2139,7 @@ ble_gattc_disc_svc_uuid_rx_hinfo(struct ble_gattc_proc *proc, rc = 0; done: - cbrc = ble_gattc_disc_svc_uuid_cb(proc, rc, 0, &service); + cbrc = ble_gattc_disc_svc_uuid_cb(proc, rc, 0, rc == 0 ? &service : NULL); if (rc != 0 || cbrc != 0) { return BLE_HS_EDONE; } else { @@ -2203,7 +2205,10 @@ ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, const ble_uuid_t *uuid, ble_gattc_proc_prepare(proc, conn_handle, BLE_GATT_OP_DISC_SVC_UUID); - ble_uuid_to_any(uuid, &proc->disc_svc_uuid.service_uuid); + rc = ble_uuid_to_any(uuid, &proc->disc_svc_uuid.service_uuid); + if (rc != 0) { + goto done; + } proc->disc_svc_uuid.prev_handle = 0x0000; proc->disc_svc_uuid.cb = cb; proc->disc_svc_uuid.cb_arg = cb_arg; @@ -2398,6 +2403,10 @@ ble_gattc_find_inc_svcs_rx_read_rsp(struct ble_gattc_proc *proc, int status, /* Proceed to the next service. */ proc->find_inc_svcs.cur_start = 0; proc->find_inc_svcs.cur_end = 0; + if (proc->find_inc_svcs.prev_handle >= proc->find_inc_svcs.end_handle) { + ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL); + return BLE_HS_EDONE; + } rc = ble_gattc_find_inc_svcs_resume(proc); if (rc != 0) { goto err; @@ -2474,8 +2483,8 @@ ble_gattc_find_inc_svcs_rx_adata(struct ble_gattc_proc *proc, done: if (call_cb) { - cbrc = ble_gattc_find_inc_svcs_cb(proc, 0, 0, &service); - if (rc != 0) { + cbrc = ble_gattc_find_inc_svcs_cb(proc, rc, 0, rc == 0 ? &service : NULL); + if (rc == 0) { rc = cbrc; } } else { @@ -2505,7 +2514,8 @@ ble_gattc_find_inc_svcs_rx_complete(struct ble_gattc_proc *proc, int status) return BLE_HS_EDONE; } - if (proc->find_inc_svcs.prev_handle == 0xffff) { + if (proc->find_inc_svcs.prev_handle >= proc->find_inc_svcs.end_handle && + proc->find_inc_svcs.cur_start == 0) { /* Procedure complete. */ ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL); return BLE_HS_EDONE; @@ -2712,7 +2722,8 @@ ble_gattc_disc_all_chrs_rx_adata(struct ble_gattc_proc *proc, chr.properties = adata->value[0]; chr.val_handle = get_le16(adata->value + 1); - if (adata->att_handle <= proc->disc_all_chrs.prev_handle) { + if (adata->att_handle <= proc->disc_all_chrs.prev_handle || + adata->att_handle > proc->disc_all_chrs.end_handle) { /* Peer sent characteristics out of order; terminate procedure. */ rc = BLE_HS_EBADDATA; goto done; @@ -2806,13 +2817,19 @@ ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle, } done: + /* One ble_gattc_proc cannot be part of multiple linked lists. + * Hence it needs to be removed before going into ble_gattc_process_status. + */ +#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) + if (proc != NULL) { + ble_hs_lock(); + STAILQ_REMOVE(&temp_proc_list, proc, ble_gattc_proc, next); + ble_hs_unlock(); + } +#endif + if (rc != 0) { STATS_INC(ble_gattc_stats, disc_all_chrs_fail); -#if MYNEWT_VAL(BLE_GATTC_PROC_PREEMPTION_PROTECT) - ble_hs_lock(); - STAILQ_REMOVE(&temp_proc_list,proc,ble_gattc_proc, next); - ble_hs_unlock(); -#endif } ble_gattc_process_status(proc, rc); @@ -3001,7 +3018,7 @@ ble_gattc_disc_chr_uuid_rx_complete(struct ble_gattc_proc *proc, int status) return BLE_HS_EDONE; } - if (proc->disc_chr_uuid.prev_handle == proc->disc_chr_uuid.end_handle) { + if (proc->disc_chr_uuid.prev_handle >= proc->disc_chr_uuid.end_handle) { /* Characteristic discovery complete. */ ble_gattc_disc_chr_uuid_cb(proc, BLE_HS_EDONE, 0, NULL); return BLE_HS_EDONE; @@ -3027,6 +3044,14 @@ ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle, struct ble_gattc_proc *proc; int rc; + if (uuid == NULL) { + return BLE_HS_EINVAL; + } + + if (start_handle == 0 || start_handle > end_handle) { + return BLE_HS_EINVAL; + } + #if MYNEWT_VAL(BLE_GATT_CACHING) rc = ble_gattc_cache_conn_search_chrs_by_uuid(conn_handle, start_handle, end_handle, uuid, cb, cb_arg); if(rc == 0) { @@ -3243,6 +3268,16 @@ ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t start_handle, struct ble_gattc_proc *proc; int rc; + if (start_handle >= end_handle) { + /* No descriptors to discover. Use a local error struct — ble_gattc_error() + * uses a static variable and is non-reentrant (ble_hs task only). */ + if (cb != NULL) { + struct ble_gatt_error err = { .status = BLE_HS_EDONE, .att_handle = 0 }; + cb(conn_handle, &err, start_handle, NULL, cb_arg); + } + return 0; + } + #if MYNEWT_VAL(BLE_GATT_CACHING) rc = ble_gattc_cache_conn_search_all_dscs(conn_handle, start_handle, end_handle, cb, cb_arg); if(rc == 0) { @@ -3350,7 +3385,7 @@ static void ble_gattc_fill_gatt_db_conversion(uint16_t *count, uint16_t num, esp } break; } - case BLE_GATT_DB_INCLUDED_SERVICE: { + case ESP_BLE_GATT_DB_INCLUDED_SERVICE: { ble_gattc_included_svc_elem_t *incl_result = (ble_gattc_included_svc_elem_t *)result; for (int i = 0; i < db_size; i++) { incl_result->handle = db[offset + i].handle; @@ -3460,6 +3495,10 @@ int ble_gattc_get_service(uint16_t conn_handle, ble_gattc_service_elem_t *result, uint16_t *count, uint16_t offset) { + if (result == NULL || count == NULL) { + return BLE_HS_EINVAL; + } + int rc; ble_gattc_db_elem_t *db = NULL; uint16_t svc_num = 0; @@ -3489,6 +3528,10 @@ int ble_gattc_get_all_char(uint16_t conn_handle, ble_gattc_char_elem_t *result, uint16_t *count, uint16_t offset) { + if (result == NULL || count == NULL) { + return BLE_HS_EINVAL; + } + int rc; ble_gattc_db_elem_t *db = NULL; uint16_t char_num = 0; @@ -3517,6 +3560,10 @@ int ble_gattc_get_all_descr(uint16_t conn_handle, ble_gattc_descr_elem_t *result, uint16_t *count, uint16_t offset) { + if (result == NULL || count == NULL) { + return BLE_HS_EINVAL; + } + int rc; ble_gattc_db_elem_t *db = NULL; uint16_t descr_num = 0; @@ -3546,9 +3593,13 @@ int ble_gattc_get_char_by_uuid(uint16_t conn_handle, ble_uuid_t *char_uuid, ble_gattc_char_elem_t *result, uint16_t *count) { - int rc; - ble_gattc_db_elem_t *db = NULL; - uint16_t char_num = 0; + if (result == NULL || count == NULL || char_uuid == NULL) { + return BLE_HS_EINVAL; + } + + int rc; + ble_gattc_db_elem_t *db = NULL; + uint16_t char_num = 0; ble_gattc_get_cached_char_by_uuid_db(conn_handle, start_handle, end_handle, char_uuid, &db, &char_num); @@ -3578,6 +3629,10 @@ int ble_gattc_get_descr_by_uuid(uint16_t conn_handle, ble_uuid_t *descr_uuid, ble_gattc_descr_elem_t *result, uint16_t *count) { + if (result == NULL || count == NULL || char_uuid == NULL) { + return BLE_HS_EINVAL; + } + int rc; ble_gattc_db_elem_t *db = NULL; uint16_t descr_num = 0; @@ -3669,6 +3724,11 @@ int ble_gattc_get_attr_count(uint16_t conn_handle, esp_ble_gatt_db_attr_type_t t uint16_t start_handle, uint16_t end_handle, uint16_t char_handle, uint16_t *count) { + if (count == NULL || type < ESP_BLE_GATT_DB_PRIMARY_SERVICE || + type > ESP_BLE_GATT_DB_ALL) { + return BLE_HS_EINVAL; + } + if (type == ESP_BLE_GATT_DB_ALL) { ble_gattc_get_db_size(conn_handle, start_handle, end_handle, count); } else { @@ -3681,12 +3741,23 @@ int ble_gattc_get_db(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle, ble_gattc_db_elem_t *result, uint16_t *count) { + if (result == NULL || count == NULL) { + if (count != NULL) { + *count = 0; + } + return BLE_HS_EINVAL; + } + uint16_t num = 0; ble_gattc_db_elem_t *db = NULL; ble_gattc_get_cached_gatt_db(conn_handle, start_handle, end_handle, &db, &num, count); if (num == 0) { + if (db != NULL) { + nimble_platform_mem_free(db); + } + *count = 0; return BLE_ATT_ERR_ATTR_NOT_FOUND; } if (db) { @@ -3917,11 +3988,10 @@ ble_gattc_read_uuid_rx_adata(struct ble_gattc_proc *proc, attr.offset = 0; attr.om = ble_hs_mbuf_from_flat(adata->value, adata->value_len); if (attr.om == NULL) { - rc = BLE_HS_ENOMEM; + rc = ble_gattc_read_uuid_cb(proc, BLE_HS_ENOMEM, 0, NULL); } else { - rc = 0; + rc = ble_gattc_read_uuid_cb(proc, 0, 0, &attr); } - rc = ble_gattc_read_uuid_cb(proc, rc, 0, &attr); /* Free the attribute mbuf if the application has not consumed it. */ os_mbuf_free_chain(attr.om); @@ -3957,6 +4027,8 @@ ble_gattc_read_uuid_rx_complete(struct ble_gattc_proc *proc, int status) static int ble_gattc_read_uuid_tx(struct ble_gattc_proc *proc) { + ble_gattc_dbg_assert_proc_not_inserted(proc); + return ble_att_clt_tx_read_type(proc->conn_handle, proc->cid, proc->read_uuid.start_handle, proc->read_uuid.end_handle, @@ -3985,7 +4057,10 @@ ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle, ble_gattc_proc_prepare(proc, conn_handle, BLE_GATT_OP_READ_UUID); - ble_uuid_to_any(uuid, &proc->read_uuid.chr_uuid); + rc = ble_uuid_to_any(uuid, &proc->read_uuid.chr_uuid); + if (rc != 0) { + goto done; + } proc->read_uuid.start_handle = start_handle; proc->read_uuid.end_handle = end_handle; proc->read_uuid.cb = cb; @@ -4146,6 +4221,7 @@ ble_gattc_read_long_rx_read_rsp(struct ble_gattc_proc *proc, int status, mtu = ble_att_mtu_by_cid(proc->conn_handle, proc->cid); if (mtu == 0) { /* No longer connected. */ + ble_gattc_read_long_cb(proc, BLE_HS_ENOTCONN, 0, NULL); return BLE_HS_EDONE; } @@ -4229,12 +4305,14 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status, for (i = 0; i < proc->read_mult.num_handles; i++) { attr[i].handle = proc->read_mult.handles[i]; attr[i].offset = 0; - if (om == NULL || OS_MBUF_PKTLEN(*om) == 0) { + if (om == NULL || OS_MBUF_PKTLEN(*om) < 2) { continue; } *om = os_mbuf_pullup(*om, 2); - assert(*om); + if (!*om) { + break; + } attr_len = get_le16((*om)->om_data); @@ -4260,9 +4338,6 @@ ble_gattc_read_mult_cb_var(struct ble_gattc_proc *proc, int status, os_mbuf_adj(*om, attr_len); } - /*FIXME Testing assert */ - assert(i == proc->read_mult.num_handles); - proc->read_mult.cb_mult(proc->conn_handle, ble_gattc_error(status, att_handle), &attr[0], i, @@ -4396,7 +4471,7 @@ ble_gattc_read_mult_internal(uint16_t conn_handle, const uint16_t *handles, ble_gatt_attr_mult_fn *cb_mult, void *cb_arg) { -#if !MYNEWT_VAL(BLE_GATT_READ_MULT) +#if !MYNEWT_VAL(BLE_GATT_READ_MULT) && !MYNEWT_VAL(BLE_GATT_READ_MULT_VAR) return BLE_HS_ENOTSUP; #endif @@ -4407,7 +4482,8 @@ ble_gattc_read_mult_internal(uint16_t conn_handle, const uint16_t *handles, STATS_INC(ble_gattc_stats, read_mult); - if (num_handles > MYNEWT_VAL(BLE_GATT_READ_MAX_ATTRS)) { + if (num_handles < 2 || + num_handles > MYNEWT_VAL(BLE_GATT_READ_MAX_ATTRS)) { rc = BLE_HS_EINVAL; goto done; } @@ -4448,8 +4524,12 @@ ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles, uint8_t num_handles, ble_gatt_attr_fn *cb, void *cb_arg) { +#if MYNEWT_VAL(BLE_GATT_READ_MULT) return ble_gattc_read_mult_internal(conn_handle, handles, num_handles, false, cb, NULL, cb_arg); +#else + return BLE_HS_ENOTSUP; +#endif } int @@ -4474,6 +4554,7 @@ ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle, struct os_mbuf *txom) { #if !MYNEWT_VAL(BLE_GATT_WRITE_NO_RSP) + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -4562,8 +4643,11 @@ ble_gattc_signed_write(uint16_t conn_handle, uint16_t attr_handle, /* Converting the csrk to little endian */ swap_buf(csrk, value_sec.csrk, 16); - rc = ble_att_clt_tx_signed_write_cmd(conn_handle, BLE_L2CAP_CID_ATT, attr_handle, - csrk, value_sec.sign_counter, txom); + rc = ble_att_clt_tx_signed_write_cmd(conn_handle, BLE_L2CAP_CID_ATT, + attr_handle, csrk, + value_sec.sign_counter, txom); + /* callee frees txom on both success and error paths */ + txom = NULL; if (rc != 0) { goto err; } @@ -4645,6 +4729,7 @@ ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle, struct os_mbuf *txom, ble_gatt_attr_fn *cb, void *cb_arg) { #if !MYNEWT_VAL(BLE_GATT_WRITE) + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -4669,6 +4754,10 @@ ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle, proc->write.cb_arg = cb_arg; #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) proc->write.om = os_mbuf_dup(txom); + if (proc->write.om == NULL) { + rc = BLE_HS_ENOMEM; + goto done; + } #endif ble_gattc_log_write(attr_handle, OS_MBUF_PKTLEN(txom), 1); @@ -4832,6 +4921,10 @@ ble_gattc_write_long_resume(struct ble_gattc_proc *proc) status = ble_gattc_write_long_tx(proc); rc = ble_gattc_process_resume_status(proc, status); if (rc != 0) { + if (proc->write_long.attr.offset > 0) { + ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid, + BLE_ATT_EXEC_WRITE_F_CANCEL); + } ble_gattc_write_long_cb(proc, rc, 0); return rc; } @@ -4876,6 +4969,8 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc, { struct os_mbuf *om; int rc; + bool exec_canceled = false; + bool prepare_acked = false; ble_gattc_dbg_assert_proc_not_inserted(proc); @@ -4886,6 +4981,7 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc, rc = status; goto err; } + prepare_acked = true; /* Verify the response. */ if (proc->write_long.attr.offset >= @@ -4924,20 +5020,24 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc, /* if data doesn't match up send cancel write */ ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid, BLE_ATT_EXEC_WRITE_F_CANCEL); + exec_canceled = true; goto err; } else { /* Send follow-up request. */ proc->write_long.attr.offset += OS_MBUF_PKTLEN(om); rc = ble_gattc_write_long_resume(proc); if (rc != 0) { - goto err; + return BLE_HS_EDONE; } return 0; } err: - /* XXX: Might need to cancel pending writes. */ + if (!exec_canceled && (prepare_acked || proc->write_long.attr.offset > 0)) { + ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid, + BLE_ATT_EXEC_WRITE_F_CANCEL); + } ble_gattc_write_long_cb(proc, rc, 0); return BLE_HS_EDONE; } @@ -4957,7 +5057,8 @@ ble_gattc_write_long_rx_exec(struct ble_gattc_proc *proc, int status) /* Expecting an execute write response, not a prepare write * response. */ - return BLE_HS_EBADDATA; + ble_gattc_write_long_cb(proc, BLE_HS_EBADDATA, 0); + return BLE_HS_EDONE; } ble_gattc_write_long_cb(proc, status, 0); @@ -4970,12 +5071,17 @@ ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle, ble_gatt_attr_fn *cb, void *cb_arg) { #if !MYNEWT_VAL(BLE_GATT_WRITE_LONG) + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif struct ble_gattc_proc *proc; int rc; + if (txom == NULL) { + return BLE_HS_EINVAL; + } + STATS_INC(ble_gattc_stats, write_long); proc = ble_gattc_proc_alloc(); @@ -5079,7 +5185,7 @@ ble_gattc_write_reliable_tx(struct ble_gattc_proc *proc) { struct ble_gatt_attr *attr; struct os_mbuf *om; - uint16_t max_sz; + int max_sz; int attr_idx; int rc; @@ -5157,16 +5263,20 @@ ble_gattc_write_reliable_err(struct ble_gattc_proc *proc, int status, uint16_t att_handle) { ble_gattc_dbg_assert_proc_not_inserted(proc); - ble_gattc_write_reliable_cb(proc, status, att_handle); /* If we have successfully queued any data, and the failure occurred before - * we could send the execute write command, then erase all queued data. + * we could send the execute write command, send cancel before notifying + * the application, matching the pattern of ble_gattc_write_long_err. */ - if (proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) { - + if (proc->write_reliable.cur_attr < proc->write_reliable.num_attrs && + (proc->write_reliable.cur_attr > 0 || + proc->write_reliable.attrs[proc->write_reliable.cur_attr].offset > 0 || + status == BLE_HS_EBADDATA)) { ble_att_clt_tx_exec_write(proc->conn_handle, proc->cid, BLE_ATT_EXEC_WRITE_F_CANCEL); } + + ble_gattc_write_reliable_cb(proc, status, att_handle); } /** @@ -5244,7 +5354,13 @@ static int ble_gattc_write_reliable_rx_exec(struct ble_gattc_proc *proc, int status) { ble_gattc_dbg_assert_proc_not_inserted(proc); - ble_gattc_write_reliable_cb(proc, status, 0); + + if (status == 0 && + proc->write_reliable.cur_attr < proc->write_reliable.num_attrs) { + ble_gattc_write_reliable_cb(proc, BLE_HS_EBADDATA, 0); + } else { + ble_gattc_write_reliable_cb(proc, status, 0); + } return BLE_HS_EDONE; } @@ -5266,7 +5382,7 @@ ble_gattc_write_reliable(uint16_t conn_handle, STATS_INC(ble_gattc_stats, write_reliable); - if (num_attrs > MYNEWT_VAL(BLE_GATT_WRITE_MAX_ATTRS)) { + if (num_attrs <= 0 || num_attrs > MYNEWT_VAL(BLE_GATT_WRITE_MAX_ATTRS)) { rc = BLE_HS_EINVAL; goto done; } @@ -5336,6 +5452,9 @@ ble_gatts_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle, struct os_mbuf *txom) { #if !MYNEWT_VAL(BLE_GATT_NOTIFY) + if (txom != NULL) { + os_mbuf_free_chain(txom); + } return BLE_HS_ENOTSUP; #endif #if MYNEWT_VAL(BLE_GATT_CACHING) @@ -5372,8 +5491,6 @@ ble_gatts_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle, rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, chr_val_handle, 0, txom, NULL); if (rc != 0) { - /* Fatal error; application disallowed attribute read. */ - rc = BLE_HS_EAPP; goto done; } } @@ -5462,6 +5579,7 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, if (peer_supports_multi_notify == 0) { for (i = 0; i < chr_count; i++) { rc = ble_att_clt_tx_notify(conn_handle, tuples[i].handle, tuples[i].value); + tuples[i].value = NULL; /* callee always consumes mbuf */ if (rc != 0) { goto done; } @@ -5471,14 +5589,32 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, for (i = 0; i < chr_count; i++) { if (OS_MBUF_PKTLEN(txom) + OS_MBUF_PKTLEN(tuples[i].value) > mtu && cur_chr_cnt < 2) { + if (cur_chr_cnt == 1) { + os_mbuf_adj(txom, sizeof(uint16_t) + sizeof(uint16_t)); + rc = ble_att_clt_tx_notify(conn_handle, + tuples[last_appended_idx].handle, + txom); + txom = NULL; + if (rc != 0) { + goto done; + } + cur_chr_cnt = 0; + txom = ble_hs_mbuf_att_pkt(); + if (txom == NULL) { + rc = BLE_HS_ENOMEM; + goto done; + } + } rc = ble_att_clt_tx_notify(conn_handle, tuples[i].handle, tuples[i].value); + tuples[i].value = NULL; /* callee always consumes mbuf */ if (rc != 0) { goto done; } continue; } else if (OS_MBUF_PKTLEN(txom) + OS_MBUF_PKTLEN(tuples[i].value) > mtu) { rc = ble_att_clt_tx_notify_mult(conn_handle, txom); + txom = NULL; /* callee always consumes txom */ if (rc != 0) { goto done; } @@ -5500,16 +5636,24 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, /* Value */ os_mbuf_concat(txom, tuples[i].value); + tuples[i].value = NULL; /* txom now owns this mbuf */ cur_chr_cnt++; last_appended_idx = i; /* Track the last appended index */ } if (cur_chr_cnt == 1) { - /* Use the last appended index, not chr_count which may be out of bounds */ + /* Strip the handle+length header prepended in the loop; the + * remaining data is the attribute value, which ble_att_clt_tx_notify + * expects. tuples[last_appended_idx].value is NULL (cleared in the + * loop after os_mbuf_concat) so we cannot reuse it here. + */ + os_mbuf_adj(txom, sizeof(uint16_t) + sizeof(uint16_t)); rc = ble_att_clt_tx_notify(conn_handle, tuples[last_appended_idx].handle, - tuples[last_appended_idx].value); - } else { + txom); + txom = NULL; + } else if (cur_chr_cnt > 1) { rc = ble_att_clt_tx_notify_mult(conn_handle, txom); + txom = NULL; } done: @@ -5529,7 +5673,9 @@ done: } } - os_mbuf_free_chain(txom); + if (txom != NULL) { + os_mbuf_free_chain(txom); + } /* Tell the application that multiple notification transmissions were attempted. */ for (i = 0; i < chr_count; i++) { ble_gap_notify_tx_event(rc, conn_handle, tuples[i].handle, 0); @@ -5546,7 +5692,12 @@ int ble_gattc_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle, struct os_mbuf *txom) { +#if MYNEWT_VAL(BLE_GATTS) return ble_gatts_notify_custom(conn_handle, chr_val_handle, txom); +#else + os_mbuf_free_chain(txom); + return BLE_HS_ENOTSUP; +#endif } #if MYNEWT_VAL(BLE_GATTC) @@ -5593,6 +5744,7 @@ static int ble_gattc_cccd_register_cb(uint16_t conn_handle, const struct ble_gat ble_gattc_cccd_write_complete_cb, NULL); if (rc != 0) { BLE_HS_LOG(WARN, "Failed to Register for Notification, status = %d", rc); + rc = 0; } } } else { @@ -5663,6 +5815,7 @@ static int ble_gattc_cccd_unregister_cb(uint16_t conn_handle, const struct ble_g ble_gattc_cccd_write_complete_cb, NULL); if (rc != 0) { BLE_HS_LOG(WARN, "Failed to Unregister for Notification, status = %d", rc); + rc = 0; } } } else { @@ -5820,6 +5973,7 @@ ble_gatts_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle, struct os_mbuf *txom) { #if !MYNEWT_VAL(BLE_GATT_INDICATE) + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -5858,6 +6012,20 @@ ble_gatts_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle, } } #endif + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + rc = BLE_HS_ENOTCONN; + goto done; + } + if (conn->bhc_gatt_svr.indicate_val_handle != 0) { + ble_hs_unlock(); + rc = BLE_HS_EALREADY; + goto done; + } + ble_hs_unlock(); + if (txom == NULL) { /* No custom attribute data; read the value from the specified * attribute. @@ -5871,9 +6039,6 @@ ble_gatts_indicate_custom(uint16_t conn_handle, uint16_t chr_val_handle, rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, chr_val_handle, 0, txom, NULL); if (rc != 0) { - /* Fatal error; application disallowed attribute read. */ - BLE_HS_DBG_ASSERT(0); - rc = BLE_HS_EAPP; goto done; } } @@ -5948,14 +6113,14 @@ ble_gattc_rx_err(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t s struct ble_gattc_proc *proc; ble_gattc_err_fn *err_cb; #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) - struct ble_gap_conn_desc desc; + struct ble_gap_conn_desc desc = {0}; int rc; #endif proc = ble_gattc_extract_first_by_conn_cid_op(conn_handle, cid, BLE_GATT_OP_NONE); #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) if (proc != NULL) { - ble_gap_conn_find(conn_handle, &desc); + rc = ble_gap_conn_find(conn_handle, &desc); proc->error.att_handle = handle; proc->error.status = status; } @@ -5963,7 +6128,7 @@ ble_gattc_rx_err(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t s if (proc != NULL) { #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) - if (proc != NULL && desc.sec_state.encrypted == 0 && + if (proc != NULL && rc == 0 && desc.sec_state.encrypted == 0 && (status == BLE_ATT_ERR_INSUFFICIENT_ENC || status == BLE_ATT_ERR_INSUFFICIENT_AUTHEN)) { rc = ble_gap_security_initiate(conn_handle); @@ -6238,7 +6403,7 @@ void ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, uint16_t cid, int status, struct os_mbuf **om, bool variable) { -#if !NIMBLE_BLE_ATT_CLT_READ_MULT +#if !NIMBLE_BLE_ATT_CLT_READ_MULT && !NIMBLE_BLE_ATT_CLT_READ_MULT_VAR return; #endif @@ -6404,6 +6569,10 @@ ble_gattc_err_dispatch_init(void) return BLE_HS_EINVAL; } + if (ble_gattc_err_dispatch != NULL) { + return 0; + } + ble_gattc_err_dispatch = nimble_platform_mem_calloc(1, BLE_GATT_OP_CNT * sizeof(ble_gattc_err_fn *)); if (!ble_gattc_err_dispatch) { @@ -6456,6 +6625,10 @@ ble_gattc_resume_dispatch_init(void) return BLE_HS_EINVAL; } + if (ble_gattc_resume_dispatch != NULL) { + return 0; + } + ble_gattc_resume_dispatch = nimble_platform_mem_calloc(1, BLE_GATT_OP_CNT * sizeof(ble_gattc_resume_fn *)); if (!ble_gattc_resume_dispatch) { diff --git a/nimble/host/src/ble_gattc_cache.c b/nimble/host/src/ble_gattc_cache.c index 999b84f5d..153332278 100644 --- a/nimble/host/src/ble_gattc_cache.c +++ b/nimble/host/src/ble_gattc_cache.c @@ -28,7 +28,7 @@ #include "host/ble_gatt.h" #include "esp_nimble_mem.h" #include "host/ble_hs_log.h" -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#if MYNEWT_VAL(BLE_GATT_CACHING) #include "nvs.h" #endif @@ -138,8 +138,21 @@ cacheEraseItem(cache_handle_t handle, const char *key) static int cacheErase(cache_handle_t handle) { + int rc; + if (cache_fn.erase_all) { - return cache_fn.erase_all(handle); + rc = cache_fn.erase_all(handle); + if (rc != 0) { + return rc; + } + + if (cache_fn.commit) { + rc = cache_fn.commit(handle); + if (rc != 0) { + return rc; + } + } + return 0; } return -1; } @@ -147,8 +160,21 @@ cacheErase(cache_handle_t handle) static int cacheWrite(cache_handle_t handle, const char * key, const void* value, size_t length) { + int rc; + if (cache_fn.write) { - return cache_fn.write(handle, key, value, length); + rc = cache_fn.write(handle, key, value, length); + if (rc != 0) { + return rc; + } + + if (cache_fn.commit) { + rc = cache_fn.commit(handle); + if (rc != 0) { + return rc; + } + } + return 0; } return -1; } @@ -247,6 +273,10 @@ ble_gattc_cacheReset(ble_addr_t *addr) /* Reduced the number address counter also */ cache_env->num_addr--; + /* Clear the stale trailing element to prevent is_open/cache_fp reuse */ + memset(&cache_env->cache_addr[cache_env->num_addr], 0, + sizeof(cache_addr_info_t)); + /* Update addr list to storage flash */ if (cache_env->num_addr > 0) { uint8_t *p_buf = nimble_platform_mem_calloc(1,MAX_ADDR_LIST_CACHE_BUF); @@ -320,10 +350,7 @@ ble_gattc_cache_find_addr(ble_addr_t addr) #endif if (cache_env == NULL) { - cache_env = nimble_platform_mem_calloc(1, sizeof(cache_env_t)); - if (cache_env == NULL) { - return INVALID_ADDR_NUM; - } + return INVALID_ADDR_NUM; } uint8_t num = cache_env->num_addr; @@ -340,13 +367,25 @@ ble_gattc_cache_find_addr(ble_addr_t addr) void ble_gattc_cache_get_addr_list(ble_addr_t *addr_list, uint8_t *out_num) { - uint8_t num = cache_env->num_addr; + uint8_t num; if (addr_list == NULL || out_num == NULL) { BLE_HS_LOG(WARN, "Invalid input to ble_gattc_cache_get_addr_list."); return; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_static_vars == NULL || cache_env == NULL) { +#else + if (cache_env == NULL) { +#endif + BLE_HS_LOG(WARN, "ble_gattc_cache_get_addr_list: cache not initialized."); + *out_num = 0; + return; + } + + num = cache_env->num_addr; + for (uint8_t i = 0; i < num; i++) { memcpy(&addr_list[i], &cache_env->cache_addr[i].addr, sizeof(ble_addr_t)); } @@ -578,7 +617,8 @@ ble_gattc_cache_addr_save(uint8_t *out_index, ble_addr_t addr, uint8_t * hash_ke static int handle_compare(const void *s1, const void *s2) { - return ((struct ble_gatt_nv_attr *)s1)->s_handle - ((struct ble_gatt_nv_attr *)s2)->s_handle; + return ((const struct ble_gatt_nv_attr *)s1)->s_handle - + ((const struct ble_gatt_nv_attr *)s2)->s_handle; } static void @@ -646,16 +686,31 @@ ble_gattc_cache_load_nv_attr(uint8_t index, int *num_attr) return NULL; } + if (length % sizeof(ble_gatt_nv_attr) != 0) { + BLE_HS_LOG(ERROR, "Cache data length %zu not aligned to attr size %zu", + length, sizeof(ble_gatt_nv_attr)); + return NULL; + } + *num_attr = length / (sizeof(ble_gatt_nv_attr)); + if (*num_attr == 0) { + return NULL; + } nv_attr = (struct ble_gatt_nv_attr *) nimble_platform_mem_calloc(1,(*num_attr) * sizeof(struct ble_gatt_nv_attr)); if (nv_attr == NULL) { return NULL; } + /* Pass the actual allocated size to prevent heap overflow */ + length = (size_t)(*num_attr) * sizeof(struct ble_gatt_nv_attr); rc = cacheRead(cache_env->cache_addr[index].cache_fp, getKeyname(&cache_env->cache_addr[index].addr), nv_attr, &length); BLE_HS_LOG(INFO, "%s, rc = %d, length = %d index = %d", __func__, rc, length, index); + if (rc != 0) { + nimble_platform_mem_free(nv_attr); + return NULL; + } return nv_attr; } @@ -761,6 +816,7 @@ ble_gattc_cache_assoc_load(ble_addr_t src_addr, uint8_t src_index, ble_addr_t as nv_attr = ble_gattc_cache_load_nv_attr(src_index, &num_attr); if (nv_attr == NULL) { BLE_HS_LOG(ERROR, "Failed to load nv_attr from source index %d", src_index); + cacheClose(src_addr); return BLE_HS_EINVAL; } @@ -798,21 +854,30 @@ ble_gattc_cache_assoc_load(ble_addr_t src_addr, uint8_t src_index, ble_addr_t as BLE_HS_LOG(DEBUG, "Successfully associated cache from src_addr to assoc_addr."); - if (rc != 0) { - return rc; - } - - return 0; + cacheClose(src_addr); + return rc; } int ble_gattc_cache_find_source(struct ble_gattc_cache_conn *cache_conn, uint8_t *database_hash) { uint8_t addr_index = 0; - uint8_t num = cache_env->num_addr; - cache_addr_info_t *addr_info = &cache_env->cache_addr[0]; + uint8_t num; + cache_addr_info_t *addr_info; int rc = ESP_FAIL; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_static_vars == NULL || cache_env == NULL) { +#else + if (cache_env == NULL) { +#endif + ble_gap_assoc_event(cache_conn->conn_handle, rc, cache_conn->cache_state); + return rc; + } + + num = cache_env->num_addr; + addr_info = &cache_env->cache_addr[0]; + /* Iterate through all cached addresses to find a matching database hash */ for (addr_index = 0; addr_index < num; addr_index++, addr_info++) { /* Compare stored hash with the provided database_hash */ @@ -844,14 +909,12 @@ ble_gattc_cache_load(ble_addr_t peer_addr) if (!cacheOpen(peer_addr, true, &index)) { BLE_HS_LOG(INFO, "gattc cache open fail"); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } if ((nv_attr = ble_gattc_cache_load_nv_attr(index, &num_attr)) == NULL) { BLE_HS_LOG(INFO, "%s, gattc cache nv_attr load fail", __func__); cacheClose(peer_addr); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -948,7 +1011,6 @@ ble_gattc_cache_init(void *storage_cb) uint8_t *p_buf = NULL; #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - int no_cached_blob = 0; rc = ble_gattc_cache_static_vars_init(); if (rc != 0) { return rc; @@ -985,18 +1047,15 @@ ble_gattc_cache_init(void *storage_cb) if ((rc = cacheRead(fp, cache_key, p_buf, &length)) != 0) { BLE_HS_LOG(DEBUG, "%s, Line = %d, storage flash get blob data fail, err_code = 0x%x", __func__, __LINE__, rc); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) /* * This is expected scenario during first boot or * when no GATT cache exists yet and should not * be treated as a fatal error. */ - if (rc == ESP_ERR_NVS_NOT_FOUND){ - no_cached_blob = 1; + if (rc == ESP_ERR_NVS_NOT_FOUND) { nimble_platform_mem_free(p_buf); return 0; } -#endif goto error; } @@ -1038,9 +1097,6 @@ error: nimble_platform_mem_free(p_buf); p_buf = NULL; } - if (cache_env && cache_env->is_open && cache_fn.close) { - cache_fn.close(cache_env->addr_fp); - } if (cache_env) { /* Close NVS handle if it was opened before freeing cache_env */ if (cache_env->is_open && cache_fn.close) { @@ -1052,7 +1108,7 @@ error: } #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_gattc_cache_static_vars && !no_cached_blob) { + if (ble_gattc_cache_static_vars) { nimble_platform_mem_free(ble_gattc_cache_static_vars); ble_gattc_cache_static_vars = NULL; } diff --git a/nimble/host/src/ble_gattc_cache_conn.c b/nimble/host/src/ble_gattc_cache_conn.c index 5c2fb10b6..00adb6525 100644 --- a/nimble/host/src/ble_gattc_cache_conn.c +++ b/nimble/host/src/ble_gattc_cache_conn.c @@ -44,7 +44,6 @@ switch(cache_state) { \ case SVC_DISC_IN_PROGRESS: \ if((void*)ble_gattc_cache_conn_svc_disced == cb) { \ - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \ return BLE_HS_EINVAL; \ } \ ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \ @@ -52,7 +51,6 @@ return 0; \ case CHR_DISC_IN_PROGRESS: \ if((void*)ble_gattc_cache_conn_chr_disced == cb) { \ - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \ return BLE_HS_EINVAL; \ } \ ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \ @@ -60,7 +58,6 @@ return 0; \ case INC_DISC_IN_PROGRESS: \ if((void *)ble_gattc_cache_conn_inc_disced == cb) { \ - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \ return BLE_HS_EINVAL; \ } \ ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \ @@ -68,7 +65,6 @@ return 0; \ case DSC_DISC_IN_PROGRESS: \ if((void*)ble_gattc_cache_conn_dsc_disced == cb) { \ - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); \ return BLE_HS_EINVAL; \ } \ ble_gattc_cache_conn_fill_op(op, s_handle, e_handle, p_uuid, cb, \ @@ -175,10 +171,13 @@ int ble_gattc_cache_conn_chr_is_empty(const struct ble_gattc_cache_conn_svc *svc, const struct ble_gattc_cache_conn_chr *chr); static struct ble_gattc_cache_conn_chr * -ble_gattc_cache_conn_chr_find(const struct ble_gattc_cache_conn_svc *svc, uint16_t chr_def_handle, +ble_gattc_cache_conn_chr_find(const struct ble_gattc_cache_conn_svc *svc, uint16_t chr_val_handle, struct ble_gattc_cache_conn_chr **out_prev); #if MYNEWT_VAL(BLE_GATTC) +static int +ble_gattc_cache_conn_disc(struct ble_gattc_cache_conn *peer); + static void ble_gattc_cache_conn_disc_chrs(struct ble_gattc_cache_conn *ble_gattc_cache_conn); @@ -187,6 +186,12 @@ ble_gattc_cache_conn_disc_incs(struct ble_gattc_cache_conn *ble_gattc_cache_conn static void ble_gattc_cache_conn_disc_dscs(struct ble_gattc_cache_conn *peer); + +static void +ble_gattc_cache_conn_pending_op_fail(struct ble_gattc_cache_conn *peer, int status); + +static struct ble_gatt_error * +ble_gattc_cache_error(int status, uint16_t att_handle); #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -234,6 +239,11 @@ struct ble_gattc_cache_conn * ble_gattc_cache_conn_find_by_addr(ble_addr_t peer_addr) { struct ble_gattc_cache_conn *ble_gattc_cache_conn; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_conn_static_vars == NULL) { + return NULL; + } +#endif SLIST_FOREACH(ble_gattc_cache_conn, &ble_gattc_cache_conns, next) { if (memcmp(&ble_gattc_cache_conn->ble_gattc_cache_conn_addr, &peer_addr, sizeof(peer_addr)) == 0) { return ble_gattc_cache_conn; @@ -377,7 +387,11 @@ ble_gattc_cache_conn_chr_end_handle(const struct ble_gattc_cache_conn_svc *svc, next_chr = SLIST_NEXT(chr, next); if (next_chr != NULL) { - return next_chr->chr.def_handle - 1; + uint16_t end = next_chr->chr.def_handle - 1; + if (end < chr->chr.val_handle) { + end = chr->chr.val_handle; + } + return end; } else { return svc->svc.end_handle; } @@ -471,7 +485,7 @@ ble_gattc_cache_conn_db_hash_read(uint16_t conn_handle, } #endif -const struct ble_gattc_cache_conn_svc * +static const struct ble_gattc_cache_conn_svc * ble_gattc_cache_conn_svc_find_uuid(const struct ble_gattc_cache_conn *ble_gattc_cache_conn, const ble_uuid_t *uuid) { const struct ble_gattc_cache_conn_svc *svc; @@ -536,7 +550,7 @@ ble_gattc_cache_conn_chr_add(ble_addr_t peer_addr, uint16_t svc_start_handle, return BLE_HS_EUNKNOWN; } - chr = ble_gattc_cache_conn_chr_find(svc, gatt_chr->def_handle, &prev); + chr = ble_gattc_cache_conn_chr_find(svc, gatt_chr->val_handle, &prev); if (chr != NULL) { /* Characteristic already discovered. */ return 0; @@ -922,7 +936,9 @@ ble_gattc_get_db_size_with_handle(struct ble_gattc_cache_conn *peer, uint16_t st break; } - db_size++; + if (svc->svc.start_handle >= start_handle) { + db_size++; + } #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) SLIST_FOREACH(included_svc, &svc->incl_svc, next) { if (included_svc->svc.handle < start_handle) { @@ -985,16 +1001,14 @@ ble_gattc_get_db_size_with_type(struct ble_gattc_cache_conn *peer, uint8_t type, switch (type) { case BLE_GATT_DB_PRIMARY_SERVICE: if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY && - svc->svc.start_handle >= start_handle && - svc->svc.end_handle <= end_handle) { + svc->svc.start_handle >= start_handle) { db_size++; } break; case BLE_GATT_DB_SECONDARY_SERVICE: if (svc->type == BLE_GATT_SVC_TYPE_SECONDARY && - svc->svc.start_handle >= start_handle && - svc->svc.end_handle <= end_handle) { + svc->svc.start_handle >= start_handle) { db_size++; } break; @@ -1011,7 +1025,7 @@ ble_gattc_get_db_size_with_type(struct ble_gattc_cache_conn *peer, uint8_t type, case BLE_GATT_DB_CHARACTERISTIC: // Iterate through characteristics. SLIST_FOREACH(chr, &svc->chrs, next) { - if (chr->chr.def_handle >= start_handle && chr->chr.def_handle <= end_handle) { //Updated + if (chr->chr.val_handle >= start_handle && chr->chr.val_handle <= end_handle) { db_size++; } } @@ -1266,6 +1280,10 @@ void ble_gattc_get_db_size_with_type_handle(uint16_t conn_handle, *count = 0; return ; } + if (cache_conn->cache_state != CACHE_VERIFIED) { + *count = 0; + return; + } *count = ble_gattc_get_db_size_with_type(cache_conn, type, start_handle, end_handle, char_handle); } @@ -1282,6 +1300,10 @@ void ble_gattc_get_db_size_handle(uint16_t conn_handle, *count = 0; return; } + if (cache_conn->cache_state != CACHE_VERIFIED) { + *count = 0; + return; + } *count = ble_gattc_get_db_size_with_handle(cache_conn, start_handle, end_handle); } @@ -1317,6 +1339,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, } ble_gattc_db_elem_t *curr_db_attr = buffer; + size_t written = 0; struct ble_gattc_cache_conn_svc *svc; #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) struct ble_gattc_cache_conn_incl_svc *included_svc; @@ -1333,16 +1356,22 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, if (svc->svc.start_handle > end_handle) { break; } - ble_gattc_fill_gatt_db_el(curr_db_attr, - svc->type == BLE_GATT_SVC_TYPE_PRIMARY ? /* attr type */ - BLE_GATT_DB_PRIMARY_SERVICE: - BLE_GATT_DB_SECONDARY_SERVICE, - 0, /* attr handle */ - svc->svc.start_handle, /* start handle*/ - svc->svc.end_handle, /* end handle */ - 0, /* property */ - svc->svc.uuid); /* uuid */ - curr_db_attr++; + if (written >= db_size) { + break; + } + if (svc->svc.start_handle >= start_handle) { + ble_gattc_fill_gatt_db_el(curr_db_attr, + svc->type == BLE_GATT_SVC_TYPE_PRIMARY ? /* attr type */ + BLE_GATT_DB_PRIMARY_SERVICE: + BLE_GATT_DB_SECONDARY_SERVICE, + 0, /* attr handle */ + svc->svc.start_handle, /* start handle*/ + svc->svc.end_handle, /* end handle */ + 0, /* property */ + svc->svc.uuid); /* uuid */ + curr_db_attr++; + written++; + } #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) // Iterate over included services SLIST_FOREACH(included_svc, &svc->incl_svc, next) { @@ -1352,10 +1381,13 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, if (included_svc->svc.handle > end_handle) { *db = buffer; - *count = db_size; + *count = (uint16_t)written; return; } + if (written >= db_size) { + break; + } ble_gattc_fill_gatt_db_el(curr_db_attr, BLE_GATT_DB_INCLUDED_SERVICE, /* attr type */ included_svc->svc.handle, /* attr handle */ @@ -1364,6 +1396,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, 0, /* property */ included_svc->svc.uuid); /* uuid */ curr_db_attr++; + written++; } #endif // Iterate over characterstic @@ -1374,9 +1407,12 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, if (chr->chr.val_handle > end_handle) { *db = buffer; - *count = db_size; + *count = (uint16_t)written; return; } + if (written >= db_size) { + break; + } ble_gattc_fill_gatt_db_el(curr_db_attr, BLE_GATT_DB_CHARACTERISTIC, /* attr type */ chr->chr.val_handle, /* attr handle */ @@ -1385,6 +1421,7 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, chr->chr.properties, /* property */ chr->chr.uuid); /* uuid */ curr_db_attr++; + written++; // Iterate over descriptors SLIST_FOREACH(dsc, &chr->dscs, next) { if (dsc->dsc.handle < start_handle) { @@ -1393,9 +1430,12 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, if (dsc->dsc.handle > end_handle) { *db = buffer; - *count = db_size; + *count = (uint16_t)written; return; } + if (written >= db_size) { + break; + } ble_gattc_fill_gatt_db_el(curr_db_attr, BLE_GATT_DB_DESCRIPTOR, /* attr type */ dsc->dsc.handle, /* attr handle */ @@ -1404,12 +1444,13 @@ static void ble_gattc_get_gatt_db_impl(struct ble_gattc_cache_conn *peer, 0, /* property */ dsc->dsc.uuid); /* uuid */ curr_db_attr++; + written++; } } } *db = buffer; - *count = db_size; + *count = (uint16_t)written; return; } @@ -1465,6 +1506,7 @@ ble_gattc_cache_conn_broken(uint16_t conn_handle) { struct ble_gattc_cache_conn_svc *svc; struct ble_gattc_cache_conn *conn; + bool was_queued; conn = ble_gattc_cache_conn_find(conn_handle); if (conn == NULL) { @@ -1475,6 +1517,25 @@ ble_gattc_cache_conn_broken(uint16_t conn_handle) /* clean the cache_conn */ SLIST_REMOVE(&ble_gattc_cache_conns, conn, ble_gattc_cache_conn, next); + /* Remove any pending discovery event to prevent UAF after conn is freed. + * Use portable NPL API — disc_ev.event is a struct on esp-idf port, not a + * pointer, so direct != NULL comparison does not compile there. */ + was_queued = ble_npl_event_is_queued(&conn->disc_ev); + if (was_queued) { + ble_npl_eventq_remove((struct ble_npl_eventq *)ble_hs_evq_get(), + &conn->disc_ev); + } + if (conn->pending_op.cb != NULL && + (was_queued || conn->cache_state != CACHE_VERIFIED)) { +#if MYNEWT_VAL(BLE_GATTC) + ble_gattc_cache_conn_pending_op_fail(conn, BLE_HS_ENOTCONN); +#endif + } + /* Always deinit to free the internal FreeRTOS event allocation even when + * the event was already dispatched (no longer queued). deinit is a no-op + * if disc_ev was never initialized (ev->event == NULL). */ + ble_npl_event_deinit(&conn->disc_ev); + while ((svc = SLIST_FIRST(&conn->svcs)) != NULL) { SLIST_REMOVE_HEAD(&conn->svcs, next); ble_gattc_cache_conn_svc_delete(svc); @@ -1575,6 +1636,71 @@ static void service_sanity_check(struct ble_gattc_cache_conn_svc_list *svcs) } } +static void +ble_gattc_cache_conn_pending_op_fail(struct ble_gattc_cache_conn *peer, int status) +{ + struct ble_gattc_cache_conn_op *op; + uint16_t conn_handle; + ble_gatt_disc_svc_fn *disc_svc_cb; +#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + ble_gatt_disc_incl_svc_fn *disc_incl_svc_cb; +#endif + ble_gatt_chr_fn *chr_cb; + ble_gatt_dsc_fn *dsc_cb; + void *cb_arg; + uint8_t cb_type; + + op = &peer->pending_op; + if (op->cb == NULL) { + return; + } + + conn_handle = peer->conn_handle; + cb_type = op->cb_type; + cb_arg = op->cb_arg; + disc_svc_cb = (ble_gatt_disc_svc_fn *)op->cb; +#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + disc_incl_svc_cb = (ble_gatt_disc_incl_svc_fn *)op->cb; +#endif + chr_cb = (ble_gatt_chr_fn *)op->cb; + dsc_cb = (ble_gatt_dsc_fn *)op->cb; + memset(op, 0, sizeof(*op)); + + switch (cb_type) { + case BLE_GATT_OP_DISC_ALL_SVCS: + disc_svc_cb(conn_handle, + ble_gattc_cache_error(status, 0), NULL, cb_arg); + break; + case BLE_GATT_OP_DISC_SVC_UUID: + disc_svc_cb(conn_handle, + ble_gattc_cache_error(status, 0), NULL, cb_arg); + break; + case BLE_GATT_OP_FIND_INC_SVCS: +#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + disc_incl_svc_cb(conn_handle, + ble_gattc_cache_error(status, 0), NULL, cb_arg); +#else + disc_svc_cb(conn_handle, + ble_gattc_cache_error(status, 0), NULL, cb_arg); +#endif + break; + case BLE_GATT_OP_DISC_ALL_CHRS: + chr_cb(conn_handle, + ble_gattc_cache_error(status, 0), NULL, cb_arg); + break; + case BLE_GATT_OP_DISC_CHR_UUID: + chr_cb(conn_handle, + ble_gattc_cache_error(status, 0), NULL, cb_arg); + break; + case BLE_GATT_OP_DISC_ALL_DSCS: + dsc_cb(conn_handle, + ble_gattc_cache_error(status, 0), 0, NULL, cb_arg); + break; + default: + break; + } +} + static void ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) { @@ -1583,8 +1709,26 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) const struct ble_gattc_cache_conn_chr *chr; bool bonded; + int rediscover; + peer->disc_prev_chr_val = 0; + rediscover = peer->needs_rediscovery; + peer->needs_rediscovery = 0; if (rc == 0) { + if (rediscover) { + peer->cache_state = CACHE_INVALID; + if (!MYNEWT_VAL(BLE_GATT_CACHING_DISABLE_AUTO)) { + rc = ble_gattc_cache_conn_disc(peer); + if (rc == 0) { + return; + } + peer->cache_state = CACHE_INVALID; + } else { + rc = BLE_HS_EAGAIN; + } + ble_gattc_cache_conn_pending_op_fail(peer, rc); + return; + } /* discovery complete */ peer->cache_state = CACHE_VERIFIED; service_sanity_check(&peer->svcs); @@ -1594,18 +1738,21 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) ble_addr_t peer_addr; ble_hs_lock(); hs_conn = ble_hs_conn_find(peer->conn_handle); - BLE_HS_DBG_ASSERT(hs_conn != NULL); - bonded = hs_conn->bhc_sec_state.bonded; - peer_addr = hs_conn->bhc_peer_addr; /* Copy address while holding lock */ - ble_hs_unlock(); + if (hs_conn != NULL) { + bonded = hs_conn->bhc_sec_state.bonded; + peer_addr = hs_conn->bhc_peer_addr; /* Copy address while holding lock */ + ble_hs_unlock(); - chr = ble_gattc_cache_conn_chr_find_uuid(peer, - BLE_UUID16_DECLARE(BLE_GATT_SVC_UUID16), - BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_DATABASE_HASH_UUID16)); - if (bonded || chr != NULL) { - /* persist the cache */ - ble_gattc_cacheReset(&peer_addr); - ble_gattc_cache_conn_cache_peer(peer); /* TODO */ + chr = ble_gattc_cache_conn_chr_find_uuid(peer, + BLE_UUID16_DECLARE(BLE_GATT_SVC_UUID16), + BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_DATABASE_HASH_UUID16)); + if (bonded || chr != NULL) { + /* persist the cache */ + ble_gattc_cacheReset(&peer_addr); + ble_gattc_cache_conn_cache_peer(peer); /* TODO */ + } + } else { + ble_hs_unlock(); } } else { peer->cache_state = CACHE_INVALID; @@ -1613,6 +1760,10 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) /* respond to the pending gatt op */ op = &peer->pending_op; if (op->cb) { + if (rc != 0) { + ble_gattc_cache_conn_pending_op_fail(peer, rc); + return; + } switch (op->cb_type) { case BLE_GATT_OP_DISC_ALL_SVCS : rc = ble_gattc_cache_conn_search_all_svcs(peer->conn_handle, op->cb, op->cb_arg); @@ -1672,6 +1823,7 @@ ble_gattc_cache_conn_undisc_all(ble_addr_t peer_addr) SLIST_REMOVE_HEAD(&peer->svcs, next); ble_gattc_cache_conn_svc_delete(svc); } + peer->cur_svc = NULL; } #if MYNEWT_VAL(BLE_GATTC) @@ -1765,6 +1917,10 @@ ble_gattc_cache_conn_disc(struct ble_gattc_cache_conn *peer) BLE_HS_LOG(INFO, "Initiating Remote Service Discovery"); peer->cache_state = SVC_DISC_IN_PROGRESS; rc = ble_gattc_disc_all_svcs(peer->conn_handle, ble_gattc_cache_conn_svc_disced, peer); + if (rc != 0) { + peer->cache_state = CACHE_INVALID; + peer->disc_prev_chr_val = 0; + } return rc; } @@ -1793,7 +1949,13 @@ ble_gattc_cache_conn_on_read(uint16_t conn_handle, ble_gattc_cache_conn_disc_complete((struct ble_gattc_cache_conn *)arg, res); return 0; } else { - res = ble_gattc_cache_conn_disc((struct ble_gattc_cache_conn *)arg); + struct ble_gattc_cache_conn *peer = arg; + + peer->needs_rediscovery = 0; + res = ble_gattc_cache_conn_disc(peer); + if (res != 0) { + ble_gattc_cache_conn_disc_complete((struct ble_gattc_cache_conn *)arg, res); + } return res; } } @@ -1813,6 +1975,11 @@ ble_gattc_cache_conn_create(uint16_t conn_handle, ble_addr_t ble_gattc_cache_con return 0; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gattc_cache_conn_static_vars == NULL) { + return BLE_HS_ENOMEM; + } +#endif cache_conn = os_memblock_get(&ble_gattc_cache_conn_pool); if (cache_conn == NULL) { /* Out of memory. */ @@ -1919,6 +2086,7 @@ ble_gattc_cache_conn_disc_dscs(struct ble_gattc_cache_conn *peer) ble_gattc_cache_conn_dsc_disced, peer); if (rc != 0) { ble_gattc_cache_conn_disc_complete(peer, rc); + return; } peer->disc_prev_chr_val = chr->chr.val_handle; @@ -1945,11 +2113,11 @@ ble_gattc_cache_conn_chr_disced(uint16_t conn_handle, const struct ble_gatt_erro case 0: rc = ble_gattc_cache_conn_chr_add(peer->ble_gattc_cache_conn_addr, peer->cur_svc->svc.start_handle, chr); - if (chr->uuid.u16.value == BLE_GATTC_DATABASE_HASH_UUID128) { - rc = ble_gattc_read(peer->conn_handle, chr->val_handle, - ble_gattc_cache_conn_db_hash_read, peer); - if (rc != 0) { - BLE_HS_LOG(ERROR, "Failed to read Database Hash %d", rc); + if (rc == 0 && chr->uuid.u.type == BLE_UUID_TYPE_16 && chr->uuid.u16.value == BLE_GATTC_DATABASE_HASH_UUID128) { + int read_rc = ble_gattc_read(peer->conn_handle, chr->val_handle, + ble_gattc_cache_conn_db_hash_read, peer); + if (read_rc != 0) { + BLE_HS_LOG(ERROR, "Failed to read Database Hash %d", read_rc); } } break; @@ -2019,8 +2187,8 @@ ble_gattc_cache_conn_disc_incs(struct ble_gattc_cache_conn *peer) if (peer->cur_svc == NULL) { if (peer->disc_prev_chr_val > 0) { ble_gattc_cache_conn_disc_chrs(peer); - return; } + return; } } svc = peer->cur_svc; @@ -2057,6 +2225,14 @@ ble_gattc_cache_conn_update(uint16_t conn_handle, uint16_t start_handle, uint16_ return; //Update } + if (peer->cache_state == SVC_DISC_IN_PROGRESS || + peer->cache_state == CHR_DISC_IN_PROGRESS || + peer->cache_state == INC_DISC_IN_PROGRESS || + peer->cache_state == DSC_DISC_IN_PROGRESS || + peer->cache_state == VERIFY_IN_PROGRESS) { + peer->needs_rediscovery = 1; + return; + } peer->cache_state = CACHE_INVALID; if (MYNEWT_VAL(BLE_GATT_CACHING_DISABLE_AUTO)) { /* Do not automatically re-discover and correct cache */ @@ -2067,7 +2243,6 @@ ble_gattc_cache_conn_update(uint16_t conn_handle, uint16_t start_handle, uint16_ peer->cache_state = CACHE_INVALID; } } -#endif int ble_gattc_cache_refresh(ble_addr_t peer_addr) { @@ -2130,8 +2305,10 @@ ble_gattc_cache_conn_assoc_on_read(uint16_t conn_handle, BLE_HS_LOG(WARN, "Failed to copy database hash from attr->om (rc=%d)", rc); return rc; } - rc = ble_gattc_cache_find_source((struct ble_gattc_cache_conn *)arg, database_hash); - + rc = ble_gattc_cache_find_source((struct ble_gattc_cache_conn *)arg, database_hash); + if (rc == 0) { + return BLE_HS_EDONE; + } return rc; } @@ -2147,12 +2324,14 @@ int ble_gattc_cache_assoc(ble_addr_t peer_addr) return BLE_HS_EUNKNOWN; } - if (cache_conn->cache_state == CACHE_LOADED) { + if (cache_conn->cache_state == CACHE_LOADED || + cache_conn->cache_state == CACHE_VERIFIED) { BLE_HS_LOG(INFO, "Cache already loaded for conn_handle=%d; " "cache state=%d. Skipping association.", cache_conn->conn_handle, cache_conn->cache_state); ble_gap_assoc_event(cache_conn->conn_handle, 0, cache_conn->cache_state); + return 0; } if (cache_conn->cache_state == CACHE_INVALID) { @@ -2243,6 +2422,7 @@ ble_gattc_cache_conn_get_svc_changed_handle(uint16_t conn_handle) } return chr->chr.val_handle; } +#endif void ble_gattc_cache_conn_free_mem(void) @@ -2428,6 +2608,9 @@ ble_gattc_cache_conn_init(void) storage_cb = NULL; rc = ble_gattc_cache_init(storage_cb); + if (rc != 0) { + goto err; + } return 0; err: @@ -2459,14 +2642,6 @@ static int ble_gattc_cache_conn_verify(struct ble_gattc_cache_conn *conn) { struct ble_hs_conn *gap_conn; int rc; -#if MYNEWT_VAL(BLE_GATT_CACHING_ASSOC_ENABLE) - if (conn->assoc_success && conn->cache_state == CACHE_LOADED) { - conn->cache_state = CACHE_VERIFIED; - BLE_HS_LOG(INFO, "Associate complete, skipping Discovery"); - ble_gattc_cache_conn_disc_complete(conn, 0); - return 0; - } -#endif if (conn->cache_state == CACHE_VERIFIED) { return 0; @@ -2484,6 +2659,14 @@ static int ble_gattc_cache_conn_verify(struct ble_gattc_cache_conn *conn) BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOTCONN); return BLE_HS_ENOTCONN; } +#if MYNEWT_VAL(BLE_GATT_CACHING_ASSOC_ENABLE) + if (conn->assoc_success && conn->cache_state == CACHE_LOADED) { + conn->cache_state = CACHE_VERIFIED; + BLE_HS_LOG(INFO, "Associate complete, skipping Discovery"); + ble_gattc_cache_conn_disc_complete(conn, 0); + return 0; + } +#endif if (conn->cache_state == CACHE_LOADED) { if (bonded) { conn->cache_state = CACHE_VERIFIED; @@ -2513,6 +2696,7 @@ static void ble_gattc_cache_search_all_svcs_cb(struct ble_npl_event *ev) int status = 0; uint16_t conn_handle; ble_gatt_disc_svc_fn *dcb; + bool aborted; conn_handle = *(uint16_t*)ble_npl_event_get_arg(ev); conn = ble_gattc_cache_conn_find(conn_handle); @@ -2530,13 +2714,19 @@ static void ble_gattc_cache_search_all_svcs_cb(struct ble_npl_event *ev) return; } + aborted = false; SLIST_FOREACH(svc, &conn->svcs, next) { if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY) { - dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg); + if (dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg) != 0) { + aborted = true; + break; + } } } - status = BLE_HS_EDONE; - dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); + if (!aborted) { + status = BLE_HS_EDONE; + dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); + } return; } @@ -2764,6 +2954,9 @@ ble_gattc_cache_conn_search_all_chrs_cb(struct ble_npl_event *ev) op = &conn->pending_op; dcb = op->cb; + if (dcb == NULL) { + return; + } svc = ble_gattc_cache_conn_svc_find_range(conn, op->start_handle); if (svc == NULL) { @@ -2773,6 +2966,10 @@ ble_gattc_cache_conn_search_all_chrs_cb(struct ble_npl_event *ev) /* return all chrs */ SLIST_FOREACH(chr, &svc->chrs, next) { + if (chr->chr.def_handle < op->start_handle || + chr->chr.def_handle > op->end_handle) { + continue; + } dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, op->cb_arg); } status = BLE_HS_EDONE; @@ -2894,6 +3091,7 @@ ble_gattc_cache_conn_search_all_dscs_cb(struct ble_npl_event *ev) int status = 0; uint16_t conn_handle; ble_gatt_dsc_fn *dcb; + bool aborted; conn_handle = *(uint16_t*)ble_npl_event_get_arg(ev); conn = ble_gattc_cache_conn_find(conn_handle); @@ -2917,11 +3115,20 @@ ble_gattc_cache_conn_search_all_dscs_cb(struct ble_npl_event *ev) return; } + aborted = false; SLIST_FOREACH(dsc, &chr->dscs, next) { - dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, op->cb_arg); + if (dsc->dsc.handle < op->start_handle || dsc->dsc.handle > op->end_handle) { + continue; + } + if (dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, op->cb_arg) != 0) { + aborted = true; + break; + } + } + if (!aborted) { + status = BLE_HS_EDONE; + dcb(conn_handle, ble_gattc_cache_error(status, 0), 0, NULL, op->cb_arg); } - status = BLE_HS_EDONE; - dcb(conn_handle, ble_gattc_cache_error(status, 0), 0, NULL, op->cb_arg); return; } diff --git a/nimble/host/src/ble_gattc_cache_priv.h b/nimble/host/src/ble_gattc_cache_priv.h index 6330629de..cd4af6ea3 100644 --- a/nimble/host/src/ble_gattc_cache_priv.h +++ b/nimble/host/src/ble_gattc_cache_priv.h @@ -126,6 +126,8 @@ struct ble_gattc_cache_conn { struct ble_gattc_cache_conn_svc_list svcs; uint8_t cache_state; + /** Set when a DB change arrives during in-progress discovery. */ + uint8_t needs_rediscovery; /** Keeps track of where we are in the service discovery process. */ uint16_t disc_prev_chr_val; struct ble_gattc_cache_conn_svc *cur_svc; @@ -308,7 +310,7 @@ int ble_gattc_cache_conn_search_all_svcs(uint16_t conn_handle, ble_gatt_disc_svc_fn *cb, void *cb_arg); int ble_gattc_cache_conn_search_svc_by_uuid(uint16_t conn_handle, const ble_uuid_t *uuid, ble_gatt_disc_svc_fn *cb, void *cb_arg); -#if (MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)) +#if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) int ble_gattc_cache_conn_search_inc_svcs(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle, ble_gatt_disc_incl_svc_fn *cb, void *cb_arg); diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index 599c14983..8d13dbba9 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -49,6 +49,13 @@ enum { CONN_CLT_CFG_ADD = 1, CONN_CLT_CFG_REMOVE = 2, }; + +struct ble_gatts_conn_clt_cfg_upd_arg { + uint16_t action; + uint16_t chr_val_handle; + uint16_t allowed_flags; + int rc; +}; #endif #endif @@ -298,15 +305,26 @@ ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle, BLE_HS_DBG_ASSERT(op == BLE_ATT_ACCESS_OP_READ); svc = arg; + { + uint8_t uuid_buf[16]; + int uuid_len; - buf = os_mbuf_extend(*om, ble_uuid_length(svc->uuid)); - if (buf == NULL) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); - return BLE_ATT_ERR_INSUFFICIENT_RES; + uuid_len = ble_uuid_length(svc->uuid); + if (offset > uuid_len) { + return BLE_ATT_ERR_INVALID_OFFSET; + } + + ble_uuid_flat(svc->uuid, uuid_buf); + + buf = os_mbuf_extend(*om, uuid_len - offset); + if (buf == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + + memcpy(buf, uuid_buf + offset, uuid_len - offset); } - ble_uuid_flat(svc->uuid, buf); - return 0; } @@ -609,7 +627,9 @@ ble_gatts_val_access(uint16_t conn_handle, uint16_t attr_handle, attr_len = OS_MBUF_PKTLEN(gatt_ctxt->om) - initial_len - offset; if (attr_len >= 0) { if (new_om) { - os_mbuf_appendfrom(*om, gatt_ctxt->om, offset, attr_len); + if (os_mbuf_appendfrom(*om, gatt_ctxt->om, offset, attr_len) != 0) { + rc = BLE_ATT_ERR_INSUFFICIENT_RES; + } } } else { rc = BLE_ATT_ERR_INVALID_OFFSET; @@ -845,6 +865,17 @@ ble_gatts_dsc_is_sane(const struct ble_gatt_dsc_def *dsc) return 0; } + if ((dsc->att_flags & (BLE_ATT_F_READ | BLE_ATT_F_WRITE | + BLE_ATT_F_READ_ENC | BLE_ATT_F_WRITE_ENC | + BLE_ATT_F_READ_AUTHEN | BLE_ATT_F_WRITE_AUTHEN | + BLE_ATT_F_READ_AUTHOR | BLE_ATT_F_WRITE_AUTHOR)) == 0) { + return 0; + } + + if (dsc->min_key_size > 16) { + return 0; + } + return 1; } @@ -939,6 +970,10 @@ ble_gatts_clt_cfg_find_idx(struct ble_gatts_clt_cfg *cfgs, struct ble_gatts_clt_cfg *cfg; int i; + if (cfgs == NULL) { + return -1; + } + for (i = 0; i < ble_gatts_num_cfgable_chrs; i++) { cfg = cfgs + i; if (cfg->chr_val_handle == chr_val_handle) { @@ -1003,7 +1038,7 @@ ble_gatts_subscribe_event(uint16_t conn_handle, uint16_t attr_handle, static int ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle, uint8_t att_op, uint16_t offset, - struct os_mbuf *om, + struct os_mbuf **om, struct ble_store_value_cccd *out_cccd, uint8_t *out_prev_clt_cfg_flags, uint8_t *out_cur_clt_cfg_flags) @@ -1048,7 +1083,7 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle, switch (gatt_op) { case BLE_GATT_ACCESS_OP_READ_DSC: STATS_INC(ble_gatts_stats, dsc_reads); - buf = os_mbuf_extend(om, 2); + buf = os_mbuf_extend(*om, 2); if (buf == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); return BLE_ATT_ERR_INSUFFICIENT_RES; @@ -1058,15 +1093,18 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle, case BLE_GATT_ACCESS_OP_WRITE_DSC: STATS_INC(ble_gatts_stats, dsc_writes); - if (OS_MBUF_PKTLEN(om) != 2) { + if (OS_MBUF_PKTLEN(*om) != 2) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN); return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; } - om = os_mbuf_pullup(om, 2); - BLE_HS_DBG_ASSERT(om != NULL); + *om = os_mbuf_pullup(*om, 2); + if (*om == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_INSUFFICIENT_RES); + return BLE_ATT_ERR_INSUFFICIENT_RES; + } - flags = get_le16(om->om_data); + flags = get_le16((*om)->om_data); if ((flags & ~clt_cfg->allowed) != 0) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_ATT_ERR_REQ_NOT_SUPPORTED); return BLE_ATT_ERR_REQ_NOT_SUPPORTED; @@ -1117,7 +1155,7 @@ ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle, rc = BLE_ATT_ERR_UNLIKELY; } else { rc = ble_gatts_clt_cfg_access_locked(conn, attr_handle, op, offset, - *om, &cccd_value, &prev_flags, + om, &cccd_value, &prev_flags, &cur_flags); } @@ -1205,13 +1243,22 @@ ble_gatts_cpfd_access(uint16_t conn_handle, uint16_t attr_handle, STATS_INC(ble_gatts_stats, dsc_reads); cpfd = arg; - - rc = 0; - rc += os_mbuf_append(*om, &(cpfd->format), sizeof(cpfd->format)); - rc += os_mbuf_append(*om, &(cpfd->exponent), sizeof(cpfd->exponent)); - rc += os_mbuf_append(*om, &(cpfd->unit), sizeof(cpfd->unit)); - rc += os_mbuf_append(*om, &(cpfd->name_space), sizeof(cpfd->name_space)); - rc += os_mbuf_append(*om, &(cpfd->description), sizeof(cpfd->description)); + { + uint8_t cpfd_bytes[7]; + + cpfd_bytes[0] = cpfd->format; + cpfd_bytes[1] = (uint8_t)cpfd->exponent; + put_le16(&cpfd_bytes[2], cpfd->unit); + cpfd_bytes[4] = cpfd->name_space; + put_le16(&cpfd_bytes[5], cpfd->description); + + if (offset > sizeof(cpfd_bytes)) { + return BLE_ATT_ERR_INVALID_OFFSET; + } + + rc = os_mbuf_append(*om, cpfd_bytes + offset, + sizeof(cpfd_bytes) - offset); + } return ((rc == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES); } @@ -1546,6 +1593,59 @@ ble_gatts_register_round(int *out_num_registered, ble_gatt_register_fn *cb, * BLE_HS_EINVAL if the service definition table * contains an invalid element. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static void +ble_gatts_unregister_svcs(const struct ble_gatt_svc_def *svcs, int num_svcs) +{ + struct ble_gatts_svc_entry *entry; + int i; + int rc; + + if (svcs == NULL || num_svcs <= 0) { + return; + } + + for (i = num_svcs - 1; i >= 0; i--) { + entry = ble_gatts_find_svc_entry(&svcs[i]); + if (entry == NULL) { + continue; + } + if (entry->handle != 0) { + rc = ble_att_svr_deregister(entry->handle, entry->end_group_handle); + BLE_HS_DBG_ASSERT_EVAL(rc == 0); + } + STAILQ_REMOVE(&ble_gatts_svc_entries, entry, ble_gatts_svc_entry, next); + ble_gatts_svc_entry_free(entry); + } +} +#else +static void +ble_gatts_unregister_svcs_entries(int start_idx, int num_svcs) +{ + int i; + int rc; + + if (num_svcs <= 0) { + return; + } + + for (i = start_idx + num_svcs - 1; i >= start_idx; i--) { + if (ble_gatts_svc_entries[i].handle != 0) { + rc = ble_att_svr_deregister( + ble_gatts_svc_entries[i].handle, + ble_gatts_svc_entries[i].end_group_handle); + BLE_HS_DBG_ASSERT_EVAL(rc == 0); + } + memset(&ble_gatts_svc_entries[i], 0, + sizeof ble_gatts_svc_entries[i]); + } + + if (ble_gatts_num_svc_entries == start_idx + num_svcs) { + ble_gatts_num_svc_entries = start_idx; + } +} +#endif + int ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, ble_gatt_register_fn *cb, void *cb_arg) @@ -1563,6 +1663,11 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, } #else int idx; + int start_idx; +#endif + +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + start_idx = ble_gatts_num_svc_entries; #endif for (i = 0; svcs[i].type != BLE_GATT_SVC_TYPE_END; i++) { @@ -1570,6 +1675,7 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, entry = ble_gatts_svc_entry_alloc(); if (entry == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + ble_gatts_unregister_svcs(svcs, i); return BLE_HS_ENOMEM; } entry -> svc = svcs + i; @@ -1577,9 +1683,10 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, entry -> end_group_handle = 0xffff; STAILQ_INSERT_TAIL(&ble_gatts_svc_entries, entry, next); #else - idx = ble_gatts_num_svc_entries + i; + idx = start_idx + i; if (idx >= ble_hs_max_services) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + ble_gatts_unregister_svcs_entries(start_idx, i); return BLE_HS_ENOMEM; } @@ -1597,6 +1704,11 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, while (total_registered < num_svcs) { rc = ble_gatts_register_round(&cur_registered, cb, cb_arg); if (rc != 0) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + ble_gatts_unregister_svcs(svcs, num_svcs); +#else + ble_gatts_unregister_svcs_entries(start_idx, num_svcs); +#endif return rc; } total_registered += cur_registered; @@ -1750,6 +1862,12 @@ ble_gatts_free_svc_defs(void) static void ble_gatts_free_mem(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gatts_static_vars == NULL) { + return; + } +#endif + #if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int rc; #endif @@ -1826,9 +1944,6 @@ ble_gatts_stop(void) ble_hs_max_client_configs = 0; ble_gatts_free_svc_defs(); -#if MYNEWT_VAL(MP_RUNTIME_ALLOC) - ble_att_svr_reset(); -#endif ble_att_svr_stop(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -1855,6 +1970,7 @@ ble_gatts_start(void) uint16_t allowed_flags; ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC); int num_elems; + #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) if (STAILQ_EMPTY(&ble_gatts_clt_cfgs)) { STAILQ_INIT(&ble_gatts_clt_cfgs); @@ -1877,11 +1993,6 @@ ble_gatts_start(void) ble_gatts_free_mem(); - rc = ble_att_svr_start(); - if (rc != 0) { - goto done; - } - #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_gatts_ensure_ctx()) { rc = BLE_HS_ENOMEM; @@ -1889,8 +2000,14 @@ ble_gatts_start(void) } #endif + rc = ble_att_svr_start(); + if (rc != 0) { + goto done; + } + #if MYNEWT_VAL(BLE_GATT_CACHING) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) if (ble_gatts_conn_aware_states == NULL) { ble_gatts_conn_aware_states = nimble_platform_mem_calloc(1, sizeof(struct ble_gatts_aware_state) * MYNEWT_VAL(BLE_STORE_MAX_BONDS)); if (ble_gatts_conn_aware_states == NULL) { @@ -1898,6 +2015,7 @@ ble_gatts_start(void) goto done; } } +#endif #else memset(ble_gatts_conn_aware_states, 0, sizeof ble_gatts_conn_aware_states); #endif @@ -2017,7 +2135,10 @@ ble_gatts_start(void) if (allowed_flags != 0) { #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) clt_cfg = ble_gatts_clt_cfg_alloc(); - BLE_HS_DBG_ASSERT_EVAL(clt_cfg != NULL); + if (clt_cfg == NULL) { + rc = BLE_HS_ENOMEM; + goto done; + } clt_cfg->chr_val_handle = ha->ha_handle_id + 1; clt_cfg->allowed = allowed_flags; @@ -2053,8 +2174,20 @@ ble_gatts_conn_can_alloc(void) } #endif - return ble_gatts_num_cfgable_chrs == 0 || + if (ble_gatts_num_cfgable_chrs == 0) { + return 1; + } + +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + return ble_gatts_clt_cfg_pool.mp_num_free >= ble_gatts_num_cfgable_chrs; +#else + return 1; +#endif +#else + return ble_gatts_clt_cfg_pool.mp_num_blocks == 0 || ble_gatts_clt_cfg_pool.mp_num_free > 0; +#endif } int @@ -2372,11 +2505,11 @@ ble_gatts_chr_updated(uint16_t chr_val_handle) chr_val_handle); if (clt_cfg == NULL) { - break; + continue; } #else if (conn->bhc_gatt_svr.clt_cfgs == NULL ) { - break; + continue; } BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs > clt_cfg_idx); @@ -2462,12 +2595,16 @@ ble_gatts_peer_cl_sup_feat_get(uint16_t conn_handle, uint8_t *out_supported_feat goto done; } + uint8_t orig_len = len; if (MYNEWT_VAL(BLE_GATT_CSFC_SIZE) < len) { len = MYNEWT_VAL(BLE_GATT_CSFC_SIZE); } memcpy(out_supported_feat, conn->bhc_gatt_svr.peer_cl_sup_feat, sizeof(uint8_t) * len); + if (len < orig_len) { + memset(out_supported_feat + len, 0, orig_len - len); + } done: ble_hs_unlock(); @@ -2480,8 +2617,10 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) struct ble_hs_conn *conn; struct ble_store_value_csfc value_csfc; struct ble_store_key_csfc key_csfc; + ble_addr_t peer_addr; uint8_t feat[MYNEWT_VAL(BLE_GATT_CSFC_SIZE)] = {}; uint16_t len; + int bonded = 0; int rc = 0; int i; @@ -2503,9 +2642,10 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) return BLE_ATT_ERR_UNLIKELY; } - /* clear RFU bits */ - for (i = 0; i < MYNEWT_VAL(BLE_GATT_CSFC_SIZE); i++) { - feat[i] &= (BLE_GATT_CHR_CLI_SUP_FEAT_MASK >> (8 * i)); + /* clear RFU bits - only first byte has defined bits, rest are RFU */ + feat[0] &= (uint8_t)BLE_GATT_CHR_CLI_SUP_FEAT_MASK; + for (i = 1; i < MYNEWT_VAL(BLE_GATT_CSFC_SIZE); i++) { + feat[i] = 0; } ble_hs_lock(); @@ -2529,27 +2669,30 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) memcpy(conn->bhc_gatt_svr.peer_cl_sup_feat, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); - if (conn->bhc_sec_state.bonded) { - memset(&key_csfc, 0, sizeof key_csfc); - key_csfc.peer_addr = conn->bhc_peer_addr; - - rc = ble_store_delete_csfc(&key_csfc); - if (rc != 0) { - goto done; - } - - memset(&value_csfc, 0, sizeof value_csfc); - value_csfc.peer_addr = conn->bhc_peer_addr; - memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); - - rc = ble_store_write_csfc(&value_csfc); - if (rc != 0) { - goto done; - } + bonded = conn->bhc_sec_state.bonded; + if (bonded) { + peer_addr = conn->bhc_peer_addr; } done: ble_hs_unlock(); + + if (rc == 0 && bonded) { + memset(&key_csfc, 0, sizeof key_csfc); + key_csfc.peer_addr = peer_addr; + + rc = ble_store_delete_csfc(&key_csfc); + if (rc != 0 && rc != BLE_HS_ENOENT) { + return rc; + } + + memset(&value_csfc, 0, sizeof value_csfc); + value_csfc.peer_addr = peer_addr; + memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); + + rc = ble_store_write_csfc(&value_csfc); + } + return rc; } @@ -2594,6 +2737,10 @@ ble_gatts_tx_notifications_one_chr(uint16_t chr_val_handle) #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle); + if (clt_cfg == NULL) { + ble_hs_unlock(); + continue; + } #else BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs > clt_cfg_idx); @@ -2966,7 +3113,7 @@ ble_gatts_find_svc_chr_attr(const ble_uuid_t *svc_uuid, } next = STAILQ_NEXT(cur, ha_next); - if (cur->ha_handle_id == svc_entry->end_group_handle) { + if (cur->ha_handle_id >= svc_entry->end_group_handle) { /* Reached end of service without a match. */ return BLE_HS_ENOENT; } @@ -3074,15 +3221,18 @@ ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid, } #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) -static void ble_gatts_add_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle, uint16_t allowed_flags, uint8_t flags) { +static int ble_gatts_add_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle, uint16_t allowed_flags, uint8_t flags) { struct ble_gatts_clt_cfg *cfg; cfg = ble_gatts_clt_cfg_alloc(); - BLE_HS_DBG_ASSERT_EVAL(cfg != NULL); + if (cfg == NULL) { + return BLE_HS_ENOMEM; + } cfg->chr_val_handle = chr_val_handle; cfg->allowed = allowed_flags; cfg->flags = flags; STAILQ_INSERT_TAIL(clt_cfgs, cfg, next); + return 0; } static int ble_gatts_remove_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle) { @@ -3112,28 +3262,37 @@ ble_gatts_conn_unaware(struct ble_hs_conn *conn, void *arg) { } #endif -/* takes two arguments -arg[0] : added/removed -arg[1] : affected chr_val_handle -arg[2] : allowed_flags -*/ +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) +static int +ble_gatts_conn_foreach_count(struct ble_hs_conn *conn, void *arg) +{ + (void)conn; + (*(int *)arg)++; + return 0; +} +#endif + static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) { + struct ble_gatts_conn_clt_cfg_upd_arg *upd_arg = arg; int rc; - uint16_t action = ((uint16_t *) arg)[0]; - uint16_t chr_val_handle = ((uint16_t *) arg)[1]; - uint16_t allowed_flags; - switch(action) { - case 1: - /* added */ - allowed_flags = ((uint16_t *) arg)[2]; - ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle, - allowed_flags, 0); - (conn->bhc_gatt_svr.num_clt_cfgs)++; + + switch (upd_arg->action) { + case CONN_CLT_CFG_ADD: + rc = ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + upd_arg->chr_val_handle, + upd_arg->allowed_flags, 0); + if (rc == 0) { + (conn->bhc_gatt_svr.num_clt_cfgs)++; + } else { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + if (upd_arg->rc == 0) { + upd_arg->rc = rc; + } + } return 0; - case 2: - /* removed */ + case CONN_CLT_CFG_REMOVE: rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, - chr_val_handle); + upd_arg->chr_val_handle); if (rc == 0) { (conn->bhc_gatt_svr.num_clt_cfgs)--; } @@ -3153,35 +3312,86 @@ static struct ble_gatts_clt_cfg * ble_gatts_get_last_cfg(struct ble_gatts_clt_cf } return prev; } + +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static void +ble_gatts_rollback_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs, + int num_svcs, + struct ble_gatts_clt_cfg *clt_cfg_start, + int cfgable_chrs_start) +#else +static void +ble_gatts_rollback_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs, + int num_svcs, + int svc_start_idx, + struct ble_gatts_clt_cfg *clt_cfg_start, + int cfgable_chrs_start) +#endif +{ + struct ble_gatts_clt_cfg *cfg; + struct ble_gatts_conn_clt_cfg_upd_arg upd_arg; + + while ((cfg = ble_gatts_get_last_cfg(&ble_gatts_clt_cfgs)) != clt_cfg_start) { + upd_arg.action = CONN_CLT_CFG_REMOVE; + upd_arg.chr_val_handle = cfg->chr_val_handle; + upd_arg.rc = 0; + ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &upd_arg); + ble_gatts_remove_clt_cfg(&ble_gatts_clt_cfgs, cfg->chr_val_handle); + } + +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + ble_gatts_unregister_svcs(svcs, num_svcs); +#else + ble_gatts_unregister_svcs_entries(svc_start_idx, num_svcs); +#endif + ble_gatts_num_cfgable_chrs = cfgable_chrs_start; + ble_gatts_free_svc_defs(); +} + int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { void *p; int i; int rc = 0; + int num_svcs; + int cfgable_chrs_start; struct ble_att_svr_entry *ha; struct ble_gatt_chr_def *chr; struct ble_gatts_svc_entry *entry; ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC); uint16_t allowed_flags; struct ble_gatts_clt_cfg *cfg; - uint16_t arg[3]; + struct ble_gatts_clt_cfg *clt_cfg_start; + struct ble_gatts_conn_clt_cfg_upd_arg upd_arg; uint16_t start_handle, end_handle; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + int svc_start_idx; +#endif + + num_svcs = 0; + while (svcs[num_svcs].type != BLE_GATT_SVC_TYPE_END) { + num_svcs++; + } p = nimble_platform_mem_calloc(1,sizeof *ble_gatts_svc_defs); if (p == NULL) { - rc = BLE_HS_ENOMEM; - goto done; + return BLE_HS_ENOMEM; } ble_hs_lock(); + clt_cfg_start = ble_gatts_get_last_cfg(&ble_gatts_clt_cfgs); +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + svc_start_idx = ble_gatts_num_svc_entries; +#endif + ble_gatts_svc_defs = p; ble_gatts_svc_defs[0] = svcs; + cfgable_chrs_start = ble_gatts_num_cfgable_chrs; rc = ble_gatts_register_svcs(ble_gatts_svc_defs[0], ble_hs_cfg.gatts_register_cb, ble_hs_cfg.gatts_register_arg); if (rc != 0) { -#if BLE_HS_DEBUG - BLE_HS_DBG_ASSERT(0); /* memory leak expected */ -#endif + ble_gatts_num_cfgable_chrs = cfgable_chrs_start; + ble_gatts_free_svc_defs(); goto done; } ble_gatts_free_svc_defs(); @@ -3191,24 +3401,45 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { ha = NULL; } else { ha = ble_att_svr_find_by_handle(cfg->chr_val_handle - 1); + if (ha == NULL) { + rc = BLE_HS_EUNKNOWN; + goto rollback; + } } while ((ha = ble_att_svr_find_by_uuid(ha, &uuid.u, 0xffff)) != NULL) { chr = ha->ha_cb_arg; allowed_flags = ble_gatts_chr_clt_cfg_allowed(chr); if (allowed_flags != 0) { - ble_gatts_add_clt_cfg(&ble_gatts_clt_cfgs, ha->ha_handle_id + 1, allowed_flags, 0); +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + int conn_cnt = 0; + + ble_hs_conn_foreach(ble_gatts_conn_foreach_count, &conn_cnt); + if (ble_gatts_clt_cfg_pool.mp_num_free < 1 + conn_cnt) { + rc = BLE_HS_ENOMEM; + goto rollback; + } +#endif + rc = ble_gatts_add_clt_cfg(&ble_gatts_clt_cfgs, ha->ha_handle_id + 1, allowed_flags, 0); + if (rc != 0) { + goto rollback; + } /* update connections */ - arg[0] = CONN_CLT_CFG_ADD; - arg[1] = ha->ha_handle_id + 1; - arg[2] = allowed_flags; - ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg); + upd_arg.action = CONN_CLT_CFG_ADD; + upd_arg.chr_val_handle = ha->ha_handle_id + 1; + upd_arg.allowed_flags = allowed_flags; + upd_arg.rc = 0; + ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &upd_arg); + if (upd_arg.rc != 0) { + rc = upd_arg.rc; + goto rollback; + } } } i = 0; entry = ble_gatts_find_svc_entry(&svcs[i]); if (entry == NULL) { rc = BLE_HS_ENOENT; - goto done; + goto rollback; } start_handle = entry->handle; while(svcs[i].type != BLE_GATT_SVC_TYPE_END) { @@ -3217,7 +3448,7 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { entry = ble_gatts_find_svc_entry(&svcs[i - 1]); if (entry == NULL) { rc = BLE_HS_ENOENT; - goto done; + goto rollback; } end_handle = entry->end_group_handle; #if MYNEWT_VAL(BLE_GATT_CACHING) @@ -3229,14 +3460,24 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { ble_gatts_conn_aware_states[i].half_aware = 0; ble_gatts_conn_aware_states[i].aware = false; } - ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) } #endif + ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL); #endif /* send service change indication */ ble_svc_gatt_changed(start_handle, end_handle); +rollback: + if (rc != 0) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + ble_gatts_rollback_add_dynamic_svcs(svcs, num_svcs, clt_cfg_start, + cfgable_chrs_start); +#else + ble_gatts_rollback_add_dynamic_svcs(svcs, num_svcs, svc_start_idx, + clt_cfg_start, cfgable_chrs_start); +#endif + } done: ble_hs_unlock(); return rc; @@ -3284,10 +3525,10 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) { int chr_val_handle; struct ble_gatt_chr_def *chr; uint16_t allowed_flags; - uint16_t arg[2]; + struct ble_gatts_conn_clt_cfg_upd_arg upd_arg; ble_uuid16_t uuid_chr = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC); struct ble_att_svr_entry *ha; - uint16_t start_handle, end_handle; + uint16_t start_handle = 0, end_handle = 0; #if MYNEWT_VAL(BLE_GATT_CACHING) int i; #endif @@ -3301,7 +3542,8 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) { } ha = ble_att_svr_find_by_handle(entry->handle); if (ha == NULL) { - rc = BLE_HS_ENOENT; + /* Service not yet registered; remove entry without ATT cleanup */ + rc = 0; goto done; } while ((ha = ble_att_svr_find_by_uuid(ha, &uuid_chr.u, entry->end_group_handle)) != NULL) { @@ -3312,9 +3554,10 @@ int ble_gatts_delete_svc(const ble_uuid_t *uuid) { ble_gatts_remove_clt_cfg(&ble_gatts_clt_cfgs, chr_val_handle); /* update connections */ - arg[0] = CONN_CLT_CFG_REMOVE; - arg[1] = chr_val_handle; - ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, arg); + upd_arg.action = CONN_CLT_CFG_REMOVE; + upd_arg.chr_val_handle = chr_val_handle; + upd_arg.rc = 0; + ble_hs_conn_foreach(ble_gatts_update_conn_clt_cfg, &upd_arg); } } /* keep the start handle and end handle before deleting the service */ @@ -3328,22 +3571,24 @@ done: if (rc == 0) { rc = ble_gatts_remove_svc_entry(uuid); #if MYNEWT_VAL(BLE_GATT_CACHING) - /* make all bonded connections them unaware */ + if (start_handle != 0 || end_handle != 0) { + /* make all bonded connections them unaware */ #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_gatts_conn_aware_states != NULL) { + if (ble_gatts_conn_aware_states != NULL) { #endif - for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { - ble_gatts_conn_aware_states[i].aware = false; - ble_gatts_conn_aware_states[i].half_aware = 0; - } - ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL); + for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { + ble_gatts_conn_aware_states[i].aware = false; + ble_gatts_conn_aware_states[i].half_aware = 0; + } #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + } +#endif + ble_hs_conn_foreach(ble_gatts_conn_unaware, NULL); } #endif -#endif - - /* send service change indication */ - ble_svc_gatt_changed(start_handle, end_handle); + if (start_handle != 0 || end_handle != 0) { + ble_svc_gatt_changed(start_handle, end_handle); + } } ble_hs_unlock(); return rc; @@ -3362,6 +3607,11 @@ ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs) goto done; } + if (svcs == NULL) { + rc = BLE_HS_EINVAL; + goto done; + } + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_gatts_ensure_ctx()) { rc = BLE_HS_ENOMEM; @@ -3594,17 +3844,28 @@ int ble_gatts_get_cfgable_chrs(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_gatts_static_vars == NULL) { + ble_gatts_static_vars_t *svars = ble_gatts_static_vars; + if (svars == NULL) { return 0; } -#endif + return svars->_ble_gatts_num_cfgable_chrs; +#else return ble_gatts_num_cfgable_chrs; +#endif } #endif void ble_gatts_lcl_svc_foreach(ble_gatt_svc_foreach_fn cb, void *arg) { + if (cb == NULL) { + return; + } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_gatts_static_vars == NULL) { + return; + } +#endif #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) struct ble_gatts_svc_entry *entry; @@ -3644,9 +3905,8 @@ ble_gatts_reset(void) #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_gatts_ensure_ctx()) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; + if (ble_gatts_static_vars == NULL) { + return 0; } #endif @@ -3669,15 +3929,16 @@ ble_gatts_reset(void) #endif /* Note: gatts memory gets freed on next call to ble_gatts_start(). */ - } #if MYNEWT_VAL(BLE_SVC_HID_SERVICE) - ble_svc_hid_reset(); + ble_svc_hid_reset(); #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) && CONFIG_BT_NIMBLE_SPS_SERVICE - ble_svc_sps_reset(); + ble_svc_sps_reset(); #endif + } + ble_hs_unlock(); return rc; diff --git a/nimble/host/src/ble_gatts_lcl.c b/nimble/host/src/ble_gatts_lcl.c index 609c534bb..4c00a43c1 100644 --- a/nimble/host/src/ble_gatts_lcl.c +++ b/nimble/host/src/ble_gatts_lcl.c @@ -54,6 +54,9 @@ static const char * const ble_gatt_chr_f_names[] = { "WRITE_ENC", "WRITE_AUTHEN", "WRITE_AUTHOR", + "NOTIFY_INDICATE_ENC", + "NOTIFY_INDICATE_AUTHEN", + "NOTIFY_INDICATE_AUTHOR", NULL }; @@ -69,7 +72,7 @@ static const char * const ble_gatt_dsc_f_names[] = { NULL }; -#define BLE_CHR_FLAGS_STR_LEN 180 +#define BLE_CHR_FLAGS_STR_LEN 256 static char * ble_gatts_flags_to_str(uint32_t flags, char *buf, @@ -269,7 +272,9 @@ ble_gatt_show_local_svc(const struct ble_gatt_svc_def *svc, void ble_gatts_show_local(void) { + ble_hs_lock(); ble_gatts_lcl_svc_foreach(ble_gatt_show_local_svc, NULL); + ble_hs_unlock(); } #endif diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index 37be3793d..f014cb6bb 100644 --- a/nimble/host/src/ble_hs.c +++ b/nimble/host/src/ble_hs.c @@ -238,6 +238,11 @@ ble_hs_evq_get(void) void ble_hs_evq_set(struct ble_npl_eventq *evq) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_ctx == NULL) { + return; + } +#endif ble_hs_evq = evq; } @@ -288,11 +293,14 @@ ble_hs_lock_nested(void) rc = ble_npl_mutex_pend(&ble_hs_mutex, 0xffffffff); #if MYNEWT_VAL(BLE_HS_DEBUG) + BLE_HS_DBG_ASSERT(ble_hs_task_handle_index < MAX_NESTED_LOCKS); counter_lock++; ble_hs_mutex_locked = 1; ble_hs_task_handle = xTaskGetCurrentTaskHandle(); - ble_hs_task_handles[ble_hs_task_handle_index] = xTaskGetCurrentTaskHandle(); - ble_hs_task_handle_index++; + if (ble_hs_task_handle_index < MAX_NESTED_LOCKS) { + ble_hs_task_handles[ble_hs_task_handle_index] = ble_hs_task_handle; + ble_hs_task_handle_index++; + } #endif BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED); } @@ -315,10 +323,13 @@ ble_hs_unlock_nested(void) if (counter_lock == 0) { ble_hs_mutex_locked = 0; } - if (ble_hs_task_handles[ble_hs_task_handle_index - 1] == xTaskGetCurrentTaskHandle()) { + if (counter_lock < MAX_NESTED_LOCKS && + ble_hs_task_handle_index > 0 && + ble_hs_task_handles[ble_hs_task_handle_index - 1] == xTaskGetCurrentTaskHandle()) { ble_hs_task_handle_index--; ble_hs_task_handles[ble_hs_task_handle_index] = NULL; - ble_hs_task_handle = ble_hs_task_handles[ble_hs_task_handle_index -1]; + ble_hs_task_handle = (ble_hs_task_handle_index > 0) ? + ble_hs_task_handles[ble_hs_task_handle_index - 1] : NULL; } } #endif @@ -388,6 +399,8 @@ ble_hs_wakeup_tx_conn(struct ble_hs_conn *conn) */ STAILQ_INSERT_HEAD(&conn->bhc_tx_q, OS_MBUF_PKTHDR(om), omp_next); return BLE_HS_EAGAIN; + } else if (rc != 0) { + return rc; } } @@ -453,12 +466,22 @@ ble_hs_clear_rx_queue(void) int ble_hs_is_enabled(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_state_ctx) { + return 0; + } +#endif return ble_hs_enabled_state == BLE_HS_ENABLED_STATE_ON; } int ble_hs_synced(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_state_ctx) { + return 0; + } +#endif return ble_hs_sync_state == BLE_HS_SYNC_STATE_GOOD; } @@ -486,10 +509,12 @@ ble_hs_sync(void) if (rc == 0) { #if NIMBLE_BLE_CONNECT - rc = ble_hs_misc_restore_irks(); - if (rc != 0) { + int irk_rc; + + irk_rc = ble_hs_misc_restore_irks(); + if (irk_rc != 0) { BLE_HS_LOG(INFO, "Failed to restore IRKs from store; status=%d\n", - rc); + irk_rc); } #endif if (ble_hs_cfg.sync_cb != NULL) { @@ -506,6 +531,7 @@ static int ble_hs_reset(void) { int rc; + int reset_reason; STATS_INC(ble_hs_stats, reset); @@ -513,16 +539,23 @@ ble_hs_reset(void) ble_hs_clear_rx_queue(); + /* Atomically retrieve and clear reset reason under lock to avoid race + * with concurrent ble_hs_sched_reset() calls. + */ + ble_hs_lock_nested(); + reset_reason = ble_hs_reset_reason; + ble_hs_reset_reason = 0; + ble_hs_unlock_nested(); + /* Clear adverising and scanning states. */ - ble_gap_reset_state(ble_hs_reset_reason); + ble_gap_reset_state(reset_reason); /* Clear configured addresses. */ ble_hs_id_reset(); - if (ble_hs_cfg.reset_cb != NULL && ble_hs_reset_reason != 0) { - ble_hs_cfg.reset_cb(ble_hs_reset_reason); + if (ble_hs_cfg.reset_cb != NULL && reset_reason != 0) { + ble_hs_cfg.reset_cb(reset_reason); } - ble_hs_reset_reason = 0; rc = ble_hs_sync(); return rc; @@ -580,7 +613,6 @@ ble_hs_timer_reset(uint32_t ticks) if (!ble_hs_is_enabled()) { ble_npl_callout_stop(&ble_hs_timer); - ble_npl_callout_deinit(&ble_hs_timer); } else { rc = ble_npl_callout_reset(&ble_hs_timer, ticks); BLE_HS_DBG_ASSERT_EVAL(rc == 0); @@ -596,6 +628,10 @@ ble_hs_timer_sched(int32_t ticks_from_now) return; } + if (!ble_hs_is_enabled()) { + return; + } + /* Reset timer if it is not currently scheduled or if the specified time is * sooner than the previous expiration time. */ @@ -605,8 +641,8 @@ ble_hs_timer_sched(int32_t ticks_from_now) ble_npl_callout_get_ticks(&ble_hs_timer))) < 0) { ble_hs_timer_reset(ticks_from_now); } - else if (ble_npl_callout_get_ticks(&ble_hs_timer) <= ble_npl_time_get()) { - /* Reset timer if currect time is later than expiration time. */ + else if ((ble_npl_stime_t)(ble_npl_time_get() - ble_npl_callout_get_ticks(&ble_hs_timer)) >= 0) { + /* Reset timer if current time is later than expiration time. */ BLE_HS_LOG(DEBUG,"exp_time:%d.now:%d.ticks:%d.active:%d.Need reset.",ble_npl_callout_get_ticks(&ble_hs_timer),ble_npl_time_get(),ticks_from_now,ble_npl_callout_is_active(&ble_hs_timer)); ble_hs_timer_reset(ticks_from_now); } @@ -716,12 +752,15 @@ ble_hs_enqueue_hci_event(uint8_t *hci_evt) ev = os_memblock_get(&ble_hs_hci_ev_pool); - if (ev) { + if (ev && ble_hs_evq) { memset (ev, 0, sizeof *ev); ble_npl_event_init(ev, ble_hs_event_rx_hci_ev, hci_evt); ble_npl_eventq_put(ble_hs_evq, ev); } else { /* Either ev is NULL or queue doesn't exist */ + if (ev) { + os_memblock_put(&ble_hs_hci_ev_pool, ev); + } #if MYNEWT_VAL(MP_RUNTIME_ALLOC) ble_transport_free(BLE_HCI_EVT, hci_evt); #else @@ -883,11 +922,12 @@ ble_hs_tx_data(struct os_mbuf *om) if (pkt_len + 1 <= BLE_HS_HCI_LOG_BUF_SIZE) { uint8_t data[BLE_HS_HCI_LOG_BUF_SIZE]; data[0] = 0x02; - os_mbuf_copydata(om, 0, pkt_len, &data[1]); - bt_hci_log_record_hci_data(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len); + if (os_mbuf_copydata(om, 0, pkt_len, &data[1]) == 0) { + bt_hci_log_record_hci_data(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len); #if BT_HCI_INSIGHTS_INCLUDED - bt_hci_log_record_insights(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len); + bt_hci_log_record_insights(HCI_LOG_DATA_TYPE_H2C_ACL, &data[1], pkt_len); #endif + } } #endif @@ -925,6 +965,10 @@ ble_hs_init(void) ble_hs_state_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_state_ctx)); if (!ble_hs_state_ctx) { MODLOG_DFLT(ERROR, "Failed to allocate ble_hs_state_ctx (%zu bytes)\n", sizeof(*ble_hs_state_ctx)); + nimble_platform_mem_free(ble_hs_ctx->hci_os_event_buf); + ble_hs_ctx->hci_os_event_buf = NULL; + nimble_platform_mem_free(ble_hs_ctx); + ble_hs_ctx = NULL; return; } } @@ -1086,6 +1130,12 @@ ble_transport_hs_init(void) void ble_hs_deinit(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_ctx == NULL) { + return; + } +#endif + ble_hs_flow_deinit(); #if BLE_MONITOR @@ -1143,6 +1193,12 @@ ble_hs_deinit(void) #if (MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) ble_hs_resolv_deinit(); #endif +#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) && MYNEWT_VAL(BLE_PERIODIC_ADV) + ble_hs_periodic_sync_deinit(); +#endif +#if MYNEWT_VAL(BLE_HS_PVCY) + ble_hs_pvcy_irk_deinit(); +#endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_ctx) { ble_hs_ctx->parent_task = NULL; @@ -1169,10 +1225,6 @@ ble_hs_deinit(void) ble_hs_adv_parse_free(); -#if MYNEWT_VAL(BLE_HS_PVCY) - ble_hs_pvcy_irk_deinit(); -#endif - ble_hs_id_ctx_free(); ble_hs_hci_ctx_free(); diff --git a/nimble/host/src/ble_hs_adv.c b/nimble/host/src/ble_hs_adv.c index f3af45429..e214a7af2 100644 --- a/nimble/host/src/ble_hs_adv.c +++ b/nimble/host/src/ble_hs_adv.c @@ -42,6 +42,11 @@ typedef struct{ ble_uuid16_t _ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2]; ble_uuid32_t _ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4]; ble_uuid128_t _ble_hs_adv_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16]; +#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) + ble_uuid16_t _ble_hs_adv_sol_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2]; + ble_uuid32_t _ble_hs_adv_sol_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4]; + ble_uuid128_t _ble_hs_adv_sol_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16]; +#endif }ble_hs_adv_uuids_ctx; static ble_hs_adv_uuids_ctx *ble_hs_adv_uuids; @@ -49,11 +54,21 @@ static ble_hs_adv_uuids_ctx *ble_hs_adv_uuids; #define ble_hs_adv_uuids16 (ble_hs_adv_uuids->_ble_hs_adv_uuids16) #define ble_hs_adv_uuids32 (ble_hs_adv_uuids->_ble_hs_adv_uuids32) #define ble_hs_adv_uuids128 (ble_hs_adv_uuids->_ble_hs_adv_uuids128) +#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) +#define ble_hs_adv_sol_uuids16 (ble_hs_adv_uuids->_ble_hs_adv_sol_uuids16) +#define ble_hs_adv_sol_uuids32 (ble_hs_adv_uuids->_ble_hs_adv_sol_uuids32) +#define ble_hs_adv_sol_uuids128 (ble_hs_adv_uuids->_ble_hs_adv_sol_uuids128) +#endif #else static ble_uuid16_t ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2]; static ble_uuid32_t ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4]; static ble_uuid128_t ble_hs_adv_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16]; +#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) +static ble_uuid16_t ble_hs_adv_sol_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2]; +static ble_uuid32_t ble_hs_adv_sol_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4]; +static ble_uuid128_t ble_hs_adv_sol_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16]; +#endif #endif static int @@ -62,6 +77,10 @@ ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len, { int rc; + if (data_len > 254) { + return BLE_HS_EMSGSIZE; + } + if (om ) { data_len++; rc = os_mbuf_append(om, &data_len, sizeof(data_len)); @@ -430,7 +449,7 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields, #if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) /*** 0x14 - 16 bit service solicitaion */ - if (adv_fields->sol_uuids16 != NULL) { + if (adv_fields->sol_uuids16 != NULL && adv_fields->sol_num_uuids16 > 0) { rc = ble_hs_adv_set_array_uuid16(BLE_HS_ADV_TYPE_SOL_UUIDS16, adv_fields->sol_num_uuids16, adv_fields->sol_uuids16, dst, &dst_len_local, max_len, om); @@ -440,7 +459,7 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields, } /*** 0x15 - 128-bit service solicitation. */ - if (adv_fields->sol_uuids128 != NULL) { + if (adv_fields->sol_uuids128 != NULL && adv_fields->sol_num_uuids128 > 0) { rc = ble_hs_adv_set_array_uuid128(BLE_HS_ADV_TYPE_SOL_UUIDS128, adv_fields->sol_num_uuids128, adv_fields->sol_uuids128, dst, &dst_len_local, max_len, om); @@ -664,6 +683,9 @@ ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields, #if !NIMBLE_BLE_ADVERTISE return BLE_HS_ENOTSUP; #endif + if (om == NULL) { + return BLE_HS_EINVAL; + } return adv_set_fields(adv_fields, NULL, NULL, 0, om); } @@ -714,6 +736,92 @@ ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields *adv_fields, return 0; } +#if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) +static int +ble_hs_adv_parse_sol_uuids16(struct ble_hs_adv_fields *adv_fields, + const uint8_t *data, uint8_t data_len) +{ + ble_uuid_any_t uuid; + int uuid_cnt; + int i; + + if (data_len % 2 != 0) { + return BLE_HS_EBADDATA; + } + + uuid_cnt = data_len / 2; + if (uuid_cnt > (BLE_HS_ADV_MAX_FIELD_SZ / 2)) { + return BLE_HS_EMSGSIZE; + } + + adv_fields->sol_uuids16 = ble_hs_adv_sol_uuids16; + adv_fields->sol_num_uuids16 = uuid_cnt; + + for (i = 0; i < uuid_cnt; i++) { + ble_uuid_init_from_buf(&uuid, data + i * 2, 2); + ble_hs_adv_sol_uuids16[i] = uuid.u16; + } + + return 0; +} + +static int +ble_hs_adv_parse_sol_uuids32(struct ble_hs_adv_fields *adv_fields, + const uint8_t *data, uint8_t data_len) +{ + ble_uuid_any_t uuid; + int uuid_cnt; + int i; + + if (data_len % 4 != 0) { + return BLE_HS_EBADDATA; + } + + uuid_cnt = data_len / 4; + if (uuid_cnt > (BLE_HS_ADV_MAX_FIELD_SZ / 4)) { + return BLE_HS_EMSGSIZE; + } + + adv_fields->sol_uuids32 = ble_hs_adv_sol_uuids32; + adv_fields->sol_num_uuids32 = uuid_cnt; + + for (i = 0; i < uuid_cnt; i++) { + ble_uuid_init_from_buf(&uuid, data + i * 4, 4); + ble_hs_adv_sol_uuids32[i] = uuid.u32; + } + + return 0; +} + +static int +ble_hs_adv_parse_sol_uuids128(struct ble_hs_adv_fields *adv_fields, + const uint8_t *data, uint8_t data_len) +{ + ble_uuid_any_t uuid; + int uuid_cnt; + int i; + + if (data_len % 16 != 0) { + return BLE_HS_EBADDATA; + } + + uuid_cnt = data_len / 16; + if (uuid_cnt > (BLE_HS_ADV_MAX_FIELD_SZ / 16)) { + return BLE_HS_EMSGSIZE; + } + + adv_fields->sol_uuids128 = ble_hs_adv_sol_uuids128; + adv_fields->sol_num_uuids128 = uuid_cnt; + + for (i = 0; i < uuid_cnt; i++) { + ble_uuid_init_from_buf(&uuid, data + i * 16, 16); + ble_hs_adv_sol_uuids128[i] = uuid.u128; + } + + return 0; +} +#endif + static int ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields *adv_fields, const uint8_t *data, uint8_t data_len) @@ -844,6 +952,9 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, *total_len = src[0] + 1; if (*total_len < 2) { + if (*total_len == 1 && src[0] == 0) { + return 0; + } return BLE_HS_EBADDATA; } @@ -962,18 +1073,10 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, #if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) case BLE_HS_ADV_TYPE_SOL_UUIDS16: - rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len); - if (rc != 0) { - return rc; - } - break; + return ble_hs_adv_parse_sol_uuids16(adv_fields, data, data_len); case BLE_HS_ADV_TYPE_SOL_UUIDS128: - rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len); - if (rc != 0) { - return rc; - } - break; + return ble_hs_adv_parse_sol_uuids128(adv_fields, data, data_len); #endif case BLE_HS_ADV_TYPE_SVC_DATA_UUID16: @@ -1019,6 +1122,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, } adv_fields->device_addr = data; adv_fields->device_addr_type = data[6]; + adv_fields->device_addr_is_present = 1; break; case BLE_HS_ADV_TYPE_LE_ROLE: @@ -1061,11 +1165,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, #if MYNEWT_VAL(BLE_EXTRA_ADV_FIELDS) case BLE_HS_ADV_TYPE_SOL_UUIDS32: - rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len); - if (rc != 0) { - return rc; - } - break; + return ble_hs_adv_parse_sol_uuids32(adv_fields, data, data_len); #endif #if MYNEWT_VAL(ENC_ADV_DATA) @@ -1125,6 +1225,10 @@ ble_hs_adv_parse(const uint8_t *data, uint8_t length, while (length > 1) { field = (const void *) data; + if (field->length == 0) { + return 0; + } + if (field->length >= length) { return BLE_HS_EBADDATA; } diff --git a/nimble/host/src/ble_hs_conn.c b/nimble/host/src/ble_hs_conn.c index 52785790c..0f773fda5 100644 --- a/nimble/host/src/ble_hs_conn.c +++ b/nimble/host/src/ble_hs_conn.c @@ -361,7 +361,7 @@ ble_hs_conn_insert(struct ble_hs_conn *conn) BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - BLE_HS_DBG_ASSERT_EVAL(ble_hs_conn_find(conn->bhc_handle) == NULL); + BLE_HS_DBG_ASSERT(ble_hs_conn_find(conn->bhc_handle) == NULL); SLIST_INSERT_HEAD(&ble_hs_conns, conn, bhc_next); } @@ -433,14 +433,20 @@ ble_hs_conn_find_by_addr(const ble_addr_t *addr) if (ble_addr_cmp(&conn->bhc_peer_addr, addr) == 0) { return conn; } - if (conn->bhc_peer_addr.type < BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT) { - continue; - } - /*If type 0x02 or 0x03 is used, let's double check if address is good */ +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* Also match against the peer identity address when available. */ ble_hs_conn_addrs(conn, &addrs); if (ble_addr_cmp(&addrs.peer_id_addr, addr) == 0) { return conn; } +#else + if (conn->bhc_peer_addr.type >= BLE_ADDR_PUBLIC_ID) { + ble_hs_conn_addrs(conn, &addrs); + if (ble_addr_cmp(&addrs.peer_id_addr, addr) == 0) { + return conn; + } + } +#endif } } @@ -500,6 +506,9 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, { const uint8_t *our_id_addr_val; int rc; + + memset(addrs, 0, sizeof(*addrs)); + /* Determine our address information. */ addrs->our_id_addr.type = ble_hs_misc_own_addr_type_to_id(conn->bhc_our_addr_type); @@ -626,8 +635,14 @@ ble_hs_conn_timer(void) time_diff = conn->rx_frag_tmo - now; if (time_diff <= 0) { + int term_rc; + /* ACL reassembly has timed out.*/ - ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM); + term_rc = ble_gap_terminate_with_conn(conn, + BLE_ERR_REM_USER_CONN_TERM); + if (term_rc != 0 && next_exp_in > 1) { + next_exp_in = 1; + } continue; } @@ -645,8 +660,14 @@ ble_hs_conn_timer(void) */ time_diff = ble_att_svr_ticks_until_tmo(&conn->bhc_att_svr, now); if (time_diff <= 0) { + int term_rc; + /* Queued write has timed out.*/ - ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM); + term_rc = ble_gap_terminate_with_conn(conn, + BLE_ERR_REM_USER_CONN_TERM); + if (term_rc != 0 && next_exp_in > 1) { + next_exp_in = 1; + } continue; } @@ -702,10 +723,11 @@ ble_hs_conn_init(void) #endif os_mempool_unregister(&ble_hs_conn_pool); if (ble_hs_conn_ctx) { + /* Clear pool before freeing ctx to avoid UAF via the macro */ + memset(&ble_hs_conn_pool, 0, sizeof(ble_hs_conn_pool)); nimble_platform_mem_free(ble_hs_conn_ctx); ble_hs_conn_ctx = NULL; } - memset(&ble_hs_conn_pool, 0, sizeof(ble_hs_conn_pool)); #endif BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EOS); return BLE_HS_EOS; diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c index b867a0ba2..9e8295513 100644 --- a/nimble/host/src/ble_hs_hci.c +++ b/nimble/host/src/ble_hs_hci.c @@ -138,10 +138,10 @@ static struct err_code core_err_code_list[] = { { BLE_HS_EAUTHEN, ": BLE_HS_EAUTHEN (Insufficient authentication)" }, { BLE_HS_EAUTHOR, ": BLE_HS_EAUTHOR (Insufficient authorization)" }, { BLE_HS_EENCRYPT, ": BLE_HS_EENCRYPT (Insufficient encryption level)" }, - { BLE_HS_EENCRYPT_KEY_SZ, ": BLE_HS_EENCRYPT_KEY_SZ (Insufficient key size" }, - { BLE_HS_ESTORE_CAP, ": BLE_HS_ESTORE_CAP (BLE_HS_ESTORE_FAIL,)" }, + { BLE_HS_EENCRYPT_KEY_SZ, ": BLE_HS_EENCRYPT_KEY_SZ (Insufficient key size)" }, + { BLE_HS_ESTORE_CAP, ": BLE_HS_ESTORE_CAP (Storage capacity exceeded)" }, { BLE_HS_ESTORE_FAIL, ": BLE_HS_ESTORE_FAIL (Storage IO error)" }, - { BLE_HS_EPREEMPTED, ": BLE_HS_EPREEMPTED (ation was preempted)" }, + { BLE_HS_EPREEMPTED, ": BLE_HS_EPREEMPTED (Operation was preempted)" }, { BLE_HS_EDISABLED, ": BLE_HS_EDISABLED (Operation disabled)" }, { BLE_HS_ESTALLED, ": BLE_HS_ESTALLED (Operation stalled)" } }; @@ -268,7 +268,7 @@ typedef struct { uint16_t hci_max_pkts; uint64_t hci_sup_feat; uint8_t hci_version; - uint16_t hci_avial_pkts; + uint16_t hci_avail_pkts; struct os_mempool hci_frag_mempool; /* Memory pool for HCI fragments */ os_membuf_t *hci_frag_data; /* Memory buffer backing HCI pool */ @@ -286,7 +286,8 @@ static ble_hs_hci_ctx_t *ble_hs_hci_ctx = NULL; #define ble_hs_hci_max_pkts (ble_hs_hci_ctx->hci_max_pkts) #define ble_hs_hci_sup_feat (ble_hs_hci_ctx->hci_sup_feat) #define ble_hs_hci_version (ble_hs_hci_ctx->hci_version) -#define ble_hs_hci_avail_pkts (ble_hs_hci_ctx->hci_avial_pkts) +#undef ble_hs_hci_avail_pkts +#define ble_hs_hci_avail_pkts (ble_hs_hci_ctx->hci_avail_pkts) #define ble_hs_hci_frag_mempool (ble_hs_hci_ctx->hci_frag_mempool) #define ble_hs_hci_frag_data (ble_hs_hci_ctx->hci_frag_data) @@ -312,6 +313,9 @@ uint16_t ble_hs_hci_avail_pkts; static struct ble_hci_ev *l_ble_hs_hci_ack; #endif //BLE_STATIC_TO_DYNAMIC +/* Indicates HCI mutex / state has been initialized and can be safely read. */ +static uint8_t ble_hs_hci_initialized; + #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE #define BLE_HS_HCI_FRAG_DATABUF_SIZE \ (BLE_ACL_MAX_PKT_SIZE + \ @@ -365,13 +369,19 @@ ble_hs_hci_set_phony_ack_cb(ble_hs_hci_phony_ack_fn *cb) } #endif -static void +static int ble_hs_hci_lock(void) { int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_hci_ctx == NULL) { + return BLE_HS_ENOTSYNCED; + } +#endif rc = ble_npl_mutex_pend(&ble_hs_hci_mutex, BLE_NPL_TIME_FOREVER); BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED); + return 0; } static void @@ -379,6 +389,11 @@ ble_hs_hci_unlock(void) { int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_hci_ctx == NULL) { + return; + } +#endif rc = ble_npl_mutex_release(&ble_hs_hci_mutex); BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED); } @@ -391,6 +406,8 @@ ble_hs_hci_set_buf_sz(uint16_t pktlen, uint16_t max_pkts) return BLE_HS_EINVAL; } + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + ble_hs_hci_buf_sz = pktlen; ble_hs_hci_max_pkts = max_pkts; ble_hs_hci_avail_pkts = max_pkts; @@ -398,6 +415,26 @@ ble_hs_hci_set_buf_sz(uint16_t pktlen, uint16_t max_pkts) return 0; } +/** + * Returns the number of available ACL transmit buffers on the controller. + * This can be used by applications to throttle notification enqueuing. + */ +uint16_t +ble_hs_hci_get_avail_pkts(void) +{ + if (!ble_hs_hci_initialized) { + return 0; + } + + uint16_t avail_pkts; + + ble_hs_lock_nested(); + avail_pkts = ble_hs_hci_avail_pkts; + ble_hs_unlock_nested(); + + return avail_pkts; +} + /** * Increases the count of available controller ACL buffers. */ @@ -406,7 +443,13 @@ ble_hs_hci_add_avail_pkts(uint16_t delta) { BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - if (ble_hs_hci_avail_pkts + delta > UINT16_MAX) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_hci_ctx) { + return; + } +#endif + + if (delta > ble_hs_hci_max_pkts - ble_hs_hci_avail_pkts) { ble_hs_sched_reset(BLE_HS_ECONTROLLER); } else { ble_hs_hci_avail_pkts += delta; @@ -437,6 +480,7 @@ ble_hs_hci_rx_cmd_complete(const void *data, int len, /* TODO Process num_pkts field. */ + out_ack->bha_opcode = opcode; out_ack->bha_status = 0; out_ack->bha_params = NULL; out_ack->bha_params_len = 0; @@ -547,9 +591,10 @@ ble_hs_hci_wait_for_ack(void) if (ble_hs_hci_phony_ack_cb == NULL) { rc = BLE_HS_ETIMEOUT_HCI; } else { - l_ble_hs_hci_ack = ble_transport_alloc_cmd(); + l_ble_hs_hci_ack = (struct ble_hci_ev *)ble_transport_alloc_evt(0); BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack != NULL); - rc = ble_hs_hci_phony_ack_cb((void *)l_ble_hs_hci_ack, 260); + rc = ble_hs_hci_phony_ack_cb((void *)l_ble_hs_hci_ack, + MYNEWT_VAL(BLE_TRANSPORT_EVT_SIZE)); } #else rc = ble_npl_sem_pend(&ble_hs_hci_sem, @@ -575,7 +620,10 @@ ble_hs_hci_cmd_tx_no_rsp(uint16_t opcode, const void *cmd, uint8_t cmd_len) { int rc; - ble_hs_hci_lock(); + rc = ble_hs_hci_lock(); + if (rc != 0) { + return rc; + } rc = ble_hs_hci_cmd_send_buf(opcode, cmd, cmd_len); @@ -591,8 +639,14 @@ ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len, struct ble_hs_hci_ack ack; int rc; + rc = ble_hs_hci_lock(); + if (rc != 0) { + return rc; + } + /* Assert after lock: when BLE_STATIC_TO_DYNAMIC is enabled, l_ble_hs_hci_ack + * expands to (ble_hs_hci_ctx->hci_ack). Evaluating this before ble_hs_hci_lock() + * would dereference ble_hs_hci_ctx before its NULL guard is checked. */ BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack == NULL); - ble_hs_hci_lock(); rc = ble_hs_hci_cmd_send_buf(opcode, cmd, cmd_len); if (rc != 0) { @@ -1002,6 +1056,11 @@ ble_hs_hci_set_hci_version(uint8_t hci_version) uint8_t ble_hs_hci_get_hci_version(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_hci_ctx) { + return 0; + } +#endif return ble_hs_hci_version; } @@ -1045,11 +1104,15 @@ ble_hs_hci_init(void) } #endif + int init_ok = 1; + rc = ble_npl_sem_init(&ble_hs_hci_sem, 0); BLE_HS_DBG_ASSERT_EVAL(rc == 0); + init_ok = init_ok && (rc == 0); rc = ble_npl_mutex_init(&ble_hs_hci_mutex); BLE_HS_DBG_ASSERT_EVAL(rc == 0); + init_ok = init_ok && (rc == 0); rc = mem_init_mbuf_pool(ble_hs_hci_frag_data, &ble_hs_hci_frag_mempool, @@ -1059,12 +1122,16 @@ ble_hs_hci_init(void) "ble_hs_hci_frag"); BLE_HS_DBG_ASSERT_EVAL(rc == 0); + init_ok = init_ok && (rc == 0); + ble_hs_hci_initialized = init_ok; } void ble_hs_hci_deinit(void) { int rc; + ble_hs_hci_initialized = 0; + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_hci_ctx == NULL) { return; diff --git a/nimble/host/src/ble_hs_hci_cmd.c b/nimble/host/src/ble_hs_hci_cmd.c index 0b08127f1..e46a5a7c8 100644 --- a/nimble/host/src/ble_hs_hci_cmd.c +++ b/nimble/host/src/ble_hs_hci_cmd.c @@ -54,8 +54,11 @@ ble_hs_hci_cmd_transport(struct ble_hci_cmd *cmd) return BLE_HS_ENOMEM_EVT; default: - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); - return BLE_HS_EUNKNOWN; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + /* BLE_HS_HCI_ERR is defined for positive HCI error codes (0x01-0xFF). + * Negative or out-of-range codes (e.g. -ENOMEM from UART transport) + * must not be passed to the macro or they produce an invalid error value. */ + return (rc > 0 && rc <= 0xFF) ? BLE_HS_HCI_ERR(rc) : BLE_HS_EUNKNOWN; } } diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index 534bf835b..5c096e832 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -126,7 +126,7 @@ static ble_hs_hci_evt_fn ble_hs_hci_evt_hw_error; static ble_hs_hci_evt_fn ble_hs_hci_evt_num_completed_pkts; #if NIMBLE_BLE_CONNECT static ble_hs_hci_evt_fn ble_hs_hci_evt_disconn_complete; -#if MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_SM static ble_hs_hci_evt_fn ble_hs_hci_evt_encrypt_change; static ble_hs_hci_evt_fn ble_hs_hci_evt_enc_key_refresh; #endif @@ -258,7 +258,7 @@ static const struct ble_hs_hci_evt_dispatch_entry ble_hs_hci_evt_dispatch[] = { #if NIMBLE_BLE_CONNECT { BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP, ble_hs_hci_evt_rd_rem_ver_complete }, { BLE_HCI_EVCODE_DISCONN_CMP, ble_hs_hci_evt_disconn_complete }, -#if MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_SM { BLE_HCI_EVCODE_ENCRYPT_CHG, ble_hs_hci_evt_encrypt_change }, { BLE_HCI_EVCODE_ENC_KEY_REFRESH, ble_hs_hci_evt_enc_key_refresh }, #endif @@ -520,7 +520,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, /* Post event to interested application */ ble_gap_reattempt_count(handle, reattempt_conn.count); #if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_OBSERVER) - rc = ble_gap_master_connect_reattempt(ev->conn_handle); + rc = ble_gap_master_connect_reattempt(handle); if (rc != 0) { BLE_HS_LOG(INFO, "Master reconnect attempt failed; rc = %d", rc); } @@ -593,7 +593,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, return 0; } -#if MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_SM static int ble_hs_hci_evt_encrypt_change(uint8_t event_code, const void *data, unsigned int len) @@ -626,7 +626,7 @@ ble_hs_hci_evt_hw_error(uint8_t event_code, const void *data, unsigned int len) return 0; } -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT && NIMBLE_BLE_SM static int ble_hs_hci_evt_enc_key_refresh(uint8_t event_code, const void *data, unsigned int len) @@ -715,6 +715,9 @@ ble_hs_hci_evt_vs(uint8_t event_code, const void *data, unsigned int len) static int ble_hs_hci_evt_rx_test(uint8_t event_code, const void *data, unsigned int len) { + if (len < sizeof(struct ble_hci_ev_command_complete)) { + return BLE_HS_ECONTROLLER; + } ble_gap_rx_test_evt(data, len); return 0; @@ -799,7 +802,7 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) /* RPA needs to be resolved here, as controller is not aware of the * address is RPA in Host based RPA */ - if (ble_host_rpa_enabled() && ((!memcmp(evt.local_rpa, ble_hs_conn_null_addr, 6)) == 0)) { + if (ble_host_rpa_enabled() && (memcmp(evt.local_rpa, ble_hs_conn_null_addr, 6) == 0)) { uint8_t *local_id_rpa = ble_hs_get_rpa_local(); if (local_id_rpa) { memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN); @@ -952,7 +955,7 @@ ble_hs_hci_evt_le_adv_rpt_first_pass(const void *data, unsigned int len) rpt = data; - if (rpt->data_len > len) { + if (sizeof(*rpt) + 1 + rpt->data_len > len) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -992,12 +995,18 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) /* BLE Queue Congestion check*/ #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hci_ctx) { +#endif if (ble_get_adv_list_length() > BLE_ADV_LIST_MAX_LENGTH || ble_adv_list_count > BLE_ADV_LIST_MAX_COUNT) { ble_adv_list_refresh(); } ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); ble_adv_list_count++; ble_npl_mutex_release(&adv_list_lock); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + } +#endif #endif for (i = 0; i < ev->num_reports; i++) { @@ -1005,7 +1014,12 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) /* Avoiding further processing, if the adv report is from the same device*/ #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) - if (ble_check_adv_list(rpt->addr, rpt->addr_type) == true) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hci_ctx && +#else + if ( +#endif + ble_check_adv_list(rpt->addr, rpt->addr_type) == true) { data += sizeof(*rpt) + rpt->data_len + 1; continue; } @@ -1150,11 +1164,11 @@ int ble_hs_iso_evt_rx_cb_set(void *cb) { if (cb == NULL) { - return -BLE_HS_EINVAL; + return BLE_HS_EINVAL; } if (iso_evt_cb) { - return -BLE_HS_EALREADY; + return BLE_HS_EALREADY; } iso_evt_cb = cb; @@ -1370,7 +1384,7 @@ ble_hs_hci_evt_le_ext_adv_rpt_first_pass(const void *data, unsigned int len) report = data; - if (report->data_len > len) { + if (sizeof(*report) + report->data_len > len) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -1413,7 +1427,7 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data, desc.props = (report->evt_type) & 0x1F; if (desc.props & BLE_HCI_ADV_LEGACY_MASK) { - legacy_event_type = ble_hs_hci_decode_legacy_type(report->evt_type); + legacy_event_type = ble_hs_hci_decode_legacy_type(desc.props); if (legacy_event_type < 0) { report = (const void *) &report->data[report->data_len]; continue; @@ -1550,13 +1564,22 @@ ble_hs_hci_evt_le_periodic_adv_sync_transfer(uint8_t subevent, const void *data, { #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER) const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev = data; + struct ble_hci_ev_le_subev_periodic_adv_sync_transfer ev_copy; +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + /* V1 event is 4 bytes shorter (lacks PAwR fields); accept both lengths */ + unsigned int v1_len = sizeof(*ev) - 4; + if (len != sizeof(*ev) && len != v1_len) { +#else if (len != sizeof(*ev)) { +#endif BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBADDATA); return BLE_HS_EBADDATA; } - ble_gap_rx_periodic_adv_sync_transfer(ev); + memset(&ev_copy, 0, sizeof(ev_copy)); + memcpy(&ev_copy, ev, len); + ble_gap_rx_periodic_adv_sync_transfer(&ev_copy); #endif return 0; @@ -1634,6 +1657,22 @@ ble_hs_hci_evt_le_adv_set_terminated(uint8_t subevent, const void *data, ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle); pend_conn_complete_valid = false; } + } else { + /* Advertising set terminated with error. Clear a stale pending + * connection-complete only if it is itself an error (e.g. directed + * advertising timeout, status != 0). A real connection (status == 0) + * may belong to a *different* advertising set whose Advertising Set + * Terminated (success) event has not arrived yet; clearing it here + * would silently drop that connection at the host level. */ +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hci_ctx) { +#endif + if (!pend_conn_complete_valid || pend_conn_complete.status != 0) { + pend_conn_complete_valid = false; + } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + } +#endif } ble_gap_rx_adv_set_terminated(ev); #endif @@ -1772,7 +1811,6 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data const struct ble_hci_ev_le_subev_periodic_adv_resp_rep *ev = data; const struct periodic_adv_response *response; struct ble_gap_periodic_adv_response resp; - uint32_t size; if (len < 5) { // minimum size for fixed fields BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); @@ -1797,16 +1835,6 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data return BLE_HS_ECONTROLLER; } - /* TODO: compare with the total length including the response data. */ - size = sizeof(*ev); - for (uint8_t i = 0; i < ev->num_responses; i ++) { - size += sizeof(struct periodic_adv_response) + ev->responses[i].data_length; - } - if (len < size) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); - return BLE_HS_ECONTROLLER; - } - len -= sizeof(*ev); data += sizeof(*ev); @@ -2019,13 +2047,23 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev) STATS_INC(ble_hs_stats, hci_event); if(ev->opcode == BLE_HCI_EVCODE_COMMAND_COMPLETE) { - if (ev->length < sizeof(struct ble_hci_ev_command_complete)) { + if (ev->length < sizeof(struct ble_hci_ev_command_complete_nop)) { STATS_INC(ble_hs_stats, hci_unknown_event); rc = BLE_HS_ECONTROLLER; goto done; } /* Check if this Command complete has a parsable opcode */ struct ble_hci_ev_command_complete *cmd_complete = (void *) ev->data; + /* NOP command complete has no status byte; skip dispatch for it */ + if (le16toh(cmd_complete->opcode) == BLE_HCI_OPCODE_NOP) { + rc = 0; + goto done; + } + if (ev->length < sizeof(struct ble_hci_ev_command_complete)) { + STATS_INC(ble_hs_stats, hci_unknown_event); + rc = BLE_HS_ECONTROLLER; + goto done; + } entry = ble_hs_hci_evt_dispatch_find(cmd_complete->opcode); } else { @@ -2042,7 +2080,7 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev) BLE_HS_LOG(DEBUG, "ble_hs_event_rx_hci_ev; opcode=0x%x ", ev->opcode); /* For LE Meta, print subevent code */ - if(ev->opcode == 0x3e) { + if(ev->opcode == 0x3e && ev->length >= 1) { BLE_HS_LOG(DEBUG, "subevent: 0x%x", ev->data[0]); } @@ -2139,12 +2177,11 @@ void ble_adv_list_init(void) void ble_adv_list_deinit(void) { struct ble_addr_list_entry *device; - struct ble_addr_list_entry *temp; ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); - SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) { - SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next); + while ((device = SLIST_FIRST(&ble_adv_list)) != NULL) { + SLIST_REMOVE_HEAD(&ble_adv_list, next); nimble_platform_mem_free(device); } @@ -2162,6 +2199,13 @@ void ble_adv_list_add_packet(void *data) return; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hci_ctx) { + /* List takes ownership of data; free here to avoid leak when ctx absent. */ + nimble_platform_mem_free(data); + return; + } +#endif ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); device = (struct ble_addr_list_entry *)data; @@ -2175,6 +2219,11 @@ uint32_t ble_get_adv_list_length(void) uint32_t length = 0; struct ble_addr_list_entry *device; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hci_ctx) { + return 0; + } +#endif ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); SLIST_FOREACH(device, &ble_adv_list, next) { length++; @@ -2187,7 +2236,6 @@ uint32_t ble_get_adv_list_length(void) void ble_adv_list_refresh(void) { struct ble_addr_list_entry *device; - struct ble_addr_list_entry *temp; ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); @@ -2195,8 +2243,8 @@ void ble_adv_list_refresh(void) BLE_HS_LOG(DEBUG, "%s: ble_adv_list empty — reinitializing", __func__); SLIST_INIT(&ble_adv_list); } else { - SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) { - SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next); + while ((device = SLIST_FIRST(&ble_adv_list)) != NULL) { + SLIST_REMOVE_HEAD(&ble_adv_list, next); nimble_platform_mem_free(device); } } @@ -2216,6 +2264,12 @@ bool ble_check_adv_list(const uint8_t *addr, uint8_t addr_type) return found; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hci_ctx) { + return false; + } +#endif + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); SLIST_FOREACH(device, &ble_adv_list, next) { diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h index 13bdc91aa..3dd373a5c 100644 --- a/nimble/host/src/ble_hs_hci_priv.h +++ b/nimble/host/src/ble_hs_hci_priv.h @@ -83,7 +83,13 @@ struct ble_hs_hci_sup_cmd { uint8_t commands[64]; }; +uint16_t ble_hs_hci_get_avail_pkts(void); + +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#define ble_hs_hci_avail_pkts (ble_hs_hci_get_avail_pkts()) +#else extern uint16_t ble_hs_hci_avail_pkts; +#endif /* This function is not waiting for command status/complete HCI events */ int ble_hs_hci_cmd_tx_no_rsp(uint16_t opcode, const void *cmd, uint8_t cmd_len); @@ -145,9 +151,9 @@ int ble_hs_hci_rd_all_local_supp_features(uint8_t *status, uint8_t *max_page, int ble_hs_hci_rd_all_remote_features(uint16_t conn_handle, uint8_t page_requested); #if MYNEWT_VAL(BLE_MONITOR_ADV) -int ble_hs_hci_add_monitor_adv_list(uint8_t addr_type, uint8_t *addr, uint8_t rssi_low, - uint8_t rssi_high, uint8_t timeout); -int ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, uint8_t *addr); +int ble_hs_hci_add_monitor_adv_list(uint8_t addr_type, uint8_t *addr, int8_t rssi_low, + int8_t rssi_high, uint8_t timeout); +int ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, const uint8_t *addr); int ble_hs_hci_clear_monitor_adv_list(void); int ble_hs_hci_read_monitor_adv_list_size(uint8_t *out_number); int ble_hs_hci_enable_monitor_adv(uint8_t enable); diff --git a/nimble/host/src/ble_hs_hci_util.c b/nimble/host/src/ble_hs_hci_util.c index 6401075e9..ecb3f4f7a 100644 --- a/nimble/host/src/ble_hs_hci_util.c +++ b/nimble/host/src/ble_hs_hci_util.c @@ -80,6 +80,11 @@ ble_hs_hci_util_rand(void *dst, int len) return BLE_HS_EINVAL; } + if (dst == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + u8ptr = dst; while (len > 0) { rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RAND), @@ -495,7 +500,7 @@ ble_hs_hci_add_monitor_adv_list(uint8_t addr_type, uint8_t *addr, int8_t rssi_lo } int -ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, uint8_t *addr) +ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, const uint8_t *addr) { struct ble_hci_le_rmv_monitor_adv_list_cp cmd; diff --git a/nimble/host/src/ble_hs_id.c b/nimble/host/src/ble_hs_id.c index 9f6205b72..f77085c95 100644 --- a/nimble/host/src/ble_hs_id.c +++ b/nimble/host/src/ble_hs_id.c @@ -88,13 +88,14 @@ ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type) int ble_hs_id_set_pub(const uint8_t *pub_addr) { + ble_hs_lock(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_id_ensure_ctx()) { + ble_hs_unlock(); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } #endif - ble_hs_lock(); memcpy(ble_hs_id_pub, pub_addr, 6); ble_hs_unlock(); return 0; @@ -136,19 +137,19 @@ ble_hs_id_set_nrpa_rnd(void) ble_addr_t nrpa_addr; int rc; -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_id_ensure_ctx()) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; - } -#endif - rc = ble_hs_id_gen_rnd(1, &nrpa_addr); if (rc != 0) { return rc; } ble_hs_lock(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_id_ensure_ctx()) { + ble_hs_unlock(); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; + } +#endif /* set the NRPA address as pseudo random address in controller */ rc = ble_hs_hci_util_set_random_addr(nrpa_addr.val); @@ -182,15 +183,15 @@ ble_hs_id_set_pseudo_rnd(const uint8_t *rnd_addr) int rc; int ones; + ble_hs_lock(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_id_ensure_ctx()) { + ble_hs_unlock(); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } #endif - ble_hs_lock(); - /* Make sure random part of rnd_addr is not all ones or zeros. Reference: * Core v5.0, Vol 6, Part B, section 1.3.2.1 */ addr_type_byte = rnd_addr[5] & 0xc0; @@ -226,15 +227,16 @@ ble_hs_id_set_rnd(const uint8_t *rnd_addr) int rc; int ones; + ble_hs_lock(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_id_ensure_ctx()) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); - return BLE_HS_EINVAL; + rc = ble_hs_id_ensure_ctx(); + if (rc != 0) { + ble_hs_unlock(); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + return rc; } #endif - ble_hs_lock(); - /* Make sure random part of rnd_addr is not all ones or zeros. Reference: * Core v5.0, Vol 6, Part B, section 1.3.2.1 */ addr_type_byte = rnd_addr[5] & 0xc0; @@ -471,12 +473,13 @@ done: void ble_hs_id_reset(void) { + ble_hs_lock(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_id_ensure_ctx()) { + ble_hs_unlock(); return; } #endif - ble_hs_lock(); memset(ble_hs_id_pub, 0, sizeof ble_hs_id_pub); memset(ble_hs_id_rnd, 0, sizeof ble_hs_id_rnd); ble_hs_unlock(); diff --git a/nimble/host/src/ble_hs_iso.c b/nimble/host/src/ble_hs_iso.c index 3022f39fa..b2f90e303 100644 --- a/nimble/host/src/ble_hs_iso.c +++ b/nimble/host/src/ble_hs_iso.c @@ -183,6 +183,10 @@ ble_hs_hci_iso_tx_now(uint16_t conn_handle, const uint8_t *sdu, uint16_t sdu_len uint8_t *frag; int rc; + if (sdu == NULL) { + return BLE_HS_EINVAL; + } + #if MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL) /* Get the Controller ISO buffer needed for the SDU */ uint8_t count = ble_hs_hci_iso_buf_needed(sdu_len, ts_flag); @@ -222,7 +226,8 @@ ble_hs_hci_iso_tx_now(uint16_t conn_handle, const uint8_t *sdu, uint16_t sdu_len */ rc = ble_hci_trans_hs_iso_tx(frag, BLE_HCI_ISO_DATA_HDR_SZ + dlh_len + sdu_len, NULL); if (rc) { - return BLE_HS_EDONE; + nimble_platform_mem_free(frag); + return BLE_HS_ECONTROLLER; } #if MYNEWT_VAL(BLE_ISO_STD_FLOW_CTRL) @@ -267,8 +272,10 @@ ble_hs_iso_pkt_rx_cb_set(ble_hs_iso_pkt_rx_fn cb) int ble_hs_rx_iso_data(const uint8_t *data, uint16_t len, void *arg) { + int rc = 0; + if (ble_hs_iso_pkt_rx_cb) { - ble_hs_iso_pkt_rx_cb(data, len, arg); + rc = ble_hs_iso_pkt_rx_cb(data, len, arg); } else { BLE_HS_LOG(WARN, "ISO RX: no callback registered"); } @@ -277,7 +284,7 @@ ble_hs_rx_iso_data(const uint8_t *data, uint16_t len, void *arg) // TODO: add adapter for controller free. free((void *)data); - return 0; + return rc; } #endif /* MYNEWT_VAL(BLE_ISO) */ diff --git a/nimble/host/src/ble_hs_iso_hci.c b/nimble/host/src/ble_hs_iso_hci.c index f58ec7db8..be8b11863 100644 --- a/nimble/host/src/ble_hs_iso_hci.c +++ b/nimble/host/src/ble_hs_iso_hci.c @@ -49,8 +49,8 @@ ble_hs_hci_read_local_supp_codec_caps(uint8_t coding_fmt, uint16_t company_id, } cmd.coding_fmt = coding_fmt; - cmd.company_id = company_id; - cmd.vs_codec_id = vs_codec_id; + cmd.company_id = htole16(company_id); + cmd.vs_codec_id = htole16(vs_codec_id); cmd.logical_tpt_type = logical_transport_type; cmd.direction = direction; @@ -85,8 +85,8 @@ ble_hs_hci_read_local_supp_controller_delay(uint8_t coding_fmt, uint16_t company cmd = (void *)cmd_buf; cmd->coding_fmt = coding_fmt; - cmd->company_id = company_id; - cmd->vs_codec_id = vs_codec_id; + cmd->company_id = htole16(company_id); + cmd->vs_codec_id = htole16(vs_codec_id); cmd->logical_tpt_type = logical_transport_type; cmd->direction = direction; cmd->codec_cfg_len = codec_cfg_len; @@ -262,7 +262,7 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin } if (rsp_buf == NULL|| - rsp_len < sizeof(struct ble_hci_le_set_cig_params_rp) + cis_cnt * 2) { + rsp_len < sizeof(struct ble_hci_le_set_cig_params_test_rp) + cis_cnt * 2) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -624,7 +624,7 @@ ble_hs_hci_iso_test_end(uint16_t conn_handle, uint32_t *received_sdu_count, int rc; if (!received_sdu_count || !missed_sdu_count || !failed_sdu_count) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.conn_handle = htole16(conn_handle); @@ -636,6 +636,10 @@ ble_hs_hci_iso_test_end(uint16_t conn_handle, uint32_t *received_sdu_count, return rc; } + if (le16toh(rsp.conn_handle) != conn_handle) { + return BLE_HS_ECONTROLLER; + } + *received_sdu_count = le32toh(rsp.received_sdu_count); *missed_sdu_count = le32toh(rsp.missed_sdu_count); *failed_sdu_count = le32toh(rsp.failed_sdu_count); diff --git a/nimble/host/src/ble_hs_log.c b/nimble/host/src/ble_hs_log.c index 0db637cf3..ee5428c84 100644 --- a/nimble/host/src/ble_hs_log.c +++ b/nimble/host/src/ble_hs_log.c @@ -73,7 +73,9 @@ ble_hs_log_mbuf(const struct os_mbuf *om) chunk_len = BLE_HS_LOG_HEX_BYTES_PER_LINE; } - os_mbuf_copydata(om, offset, chunk_len, buf); + if (os_mbuf_copydata(om, offset, chunk_len, buf) != 0) { + break; + } ble_hs_log_debug_hex_chunk(buf, chunk_len); } #endif diff --git a/nimble/host/src/ble_hs_periodic_sync.c b/nimble/host/src/ble_hs_periodic_sync.c index d42cdd4a5..c4f2e908f 100644 --- a/nimble/host/src/ble_hs_periodic_sync.c +++ b/nimble/host/src/ble_hs_periodic_sync.c @@ -79,6 +79,8 @@ ble_hs_periodic_sync_alloc(void) { struct ble_hs_periodic_sync *psync; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + psync = os_memblock_get(&ble_hs_periodic_sync_pool); if (psync) { memset(psync, 0, sizeof(*psync)); @@ -119,8 +121,8 @@ ble_hs_periodic_sync_insert(struct ble_hs_periodic_sync *psync) { BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - BLE_HS_DBG_ASSERT_EVAL( - ble_hs_periodic_sync_find_by_handle(psync->sync_handle) == NULL); + BLE_HS_DBG_ASSERT( + ble_hs_periodic_sync_find_by_handle(psync->sync_handle) == NULL); SLIST_INSERT_HEAD(&g_ble_hs_periodic_sync_handles, psync, next); } @@ -128,10 +130,23 @@ ble_hs_periodic_sync_insert(struct ble_hs_periodic_sync *psync) void ble_hs_periodic_sync_remove(struct ble_hs_periodic_sync *psync) { + struct ble_hs_periodic_sync *cur; + struct ble_hs_periodic_sync *prev; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - SLIST_REMOVE(&g_ble_hs_periodic_sync_handles, psync, ble_hs_periodic_sync, - next); + prev = NULL; + SLIST_FOREACH(cur, &g_ble_hs_periodic_sync_handles, next) { + if (cur == psync) { + if (prev == NULL) { + SLIST_REMOVE_HEAD(&g_ble_hs_periodic_sync_handles, next); + } else { + SLIST_NEXT(prev, next) = SLIST_NEXT(cur, next); + } + return; + } + prev = cur; + } } struct ble_hs_periodic_sync * diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index 05edcb722..5d6beeb45 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/nimble/host/src/ble_hs_pvcy.c @@ -80,7 +80,7 @@ ble_hs_pvcy_set_addr_timeout(uint16_t timeout) struct ble_hci_le_set_rpa_tmo_cp cmd; if (timeout == 0 || timeout > BLE_MAX_RPA_TIMEOUT_VAL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.rpa_timeout = htole16(timeout); @@ -99,9 +99,12 @@ int ble_hs_set_rpa_timeout(uint16_t timeout) return BLE_HS_ENOMEM; } #endif - l_rpa_timeout = timeout; + int rc = ble_hs_pvcy_set_addr_timeout(timeout); + if (rc == 0) { + l_rpa_timeout = timeout; + } - return ble_hs_pvcy_set_addr_timeout(l_rpa_timeout); + return rc; } uint16_t ble_hs_get_rpa_timeout(void) @@ -260,6 +263,7 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, memcpy(peer_addr.val, addr, sizeof peer_addr.val); rc = ble_hs_pvcy_set_mode(&peer_addr, BLE_GAP_PRIVATE_MODE_DEVICE); if (rc != 0) { + ble_hs_pvcy_remove_entry(addr_type, addr); return rc; } #endif @@ -308,6 +312,13 @@ ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type, { int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; + } +#endif + STATS_INC(ble_hs_stats, pvcy_add_entry); /* No GAP procedures can be active when adding an entry to the resolving @@ -420,8 +431,13 @@ void ble_hs_pvcy_set_default_irk(void) } } - void ble_store_config_init(void); - ble_store_config_init(); + /* Only call ble_store_config_init on the first initialization. + * Calling it at runtime resets all in-memory bonds/CCCDs. + */ + if (!ble_hs_pvcy_ctx->pvcy_started) { + void ble_store_config_init(void); + ble_store_config_init(); + } #endif rc = ble_store_read_local_irk(&key_local_irk, &value_local_irk); @@ -457,16 +473,18 @@ void ble_hs_pvcy_set_default_irk(void) } } -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) void ble_hs_pvcy_irk_deinit(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_pvcy_ctx) { nimble_platform_mem_free(ble_hs_pvcy_ctx); ble_hs_pvcy_ctx = NULL; } -} +#else + ble_hs_pvcy_started = 0; #endif +} int ble_hs_pvcy_set_our_irk(const uint8_t *irk) @@ -574,12 +592,15 @@ ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode) return BLE_HS_EINVAL; } - if (addr->type > BLE_ADDR_RANDOM) { - return BLE_ERR_INV_HCI_CMD_PARMS; + /* Reject address types that are not in the expected set */ + if (addr->type != BLE_ADDR_PUBLIC && addr->type != BLE_ADDR_RANDOM && + addr->type != BLE_ADDR_PUBLIC_ID && addr->type != BLE_ADDR_RANDOM_ID) { + return BLE_HS_EINVAL; } cmd.mode = priv_mode; - cmd.peer_id_addr_type = addr->type; + /* HCI only accepts public (0) or random (1); map identity types to base */ + cmd.peer_id_addr_type = addr->type % 2; memcpy(cmd.peer_id_addr, addr->val, BLE_DEV_ADDR_LEN); return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -603,6 +624,14 @@ int ble_hs_pvcy_rpa_config(uint8_t enable) { int rc = 0; + bool was_resolv_enabled; + bool was_nrpa; + + if (enable != NIMBLE_HOST_DISABLE_PRIVACY && + enable != NIMBLE_HOST_ENABLE_RPA && + enable != NIMBLE_HOST_ENABLE_NRPA) { + return BLE_HS_EINVAL; + } if (enable != NIMBLE_HOST_DISABLE_PRIVACY) { rc = ble_hs_pvcy_ensure_started(); @@ -610,6 +639,9 @@ ble_hs_pvcy_rpa_config(uint8_t enable) return rc; } + was_resolv_enabled = is_ble_hs_resolv_enabled(); + was_nrpa = was_resolv_enabled && !ble_host_rpa_enabled(); + ble_hs_resolv_enable(true); /* Configure NRPA address related flags according to input parameter */ @@ -621,6 +653,16 @@ ble_hs_pvcy_rpa_config(uint8_t enable) /* Generate local RPA address and set it in controller */ rc = ble_hs_gen_own_private_rnd(); + if (rc != 0) { + if (!was_resolv_enabled) { + ble_hs_resolv_enable(false); + } + if (was_nrpa) { + ble_hs_resolv_nrpa_enable(); + } else { + ble_hs_resolv_nrpa_disable(); + } + } } else { ble_hs_resolv_enable(false); } diff --git a/nimble/host/src/ble_hs_pvcy_priv.h b/nimble/host/src/ble_hs_pvcy_priv.h index 7c7d16777..965e9b99c 100644 --- a/nimble/host/src/ble_hs_pvcy_priv.h +++ b/nimble/host/src/ble_hs_pvcy_priv.h @@ -50,9 +50,7 @@ bool ble_hs_pvcy_enabled(void); #endif bool ble_hs_pvcy_resolve_with_irk(const uint8_t rpa[6], const uint8_t irk[16]); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) void ble_hs_pvcy_irk_deinit(void); -#endif #ifdef __cplusplus } #endif diff --git a/nimble/host/src/ble_hs_resolv.c b/nimble/host/src/ble_hs_resolv.c index 86118096e..2638d9180 100644 --- a/nimble/host/src/ble_hs_resolv.c +++ b/nimble/host/src/ble_hs_resolv.c @@ -367,6 +367,7 @@ ble_rpa_resolv_add_peer_rec(uint8_t *peer_addr) p_dev_rec = &peer_dev_rec[ble_store_num_peer_dev_rec]; p_dev_rec->rec_used = 1; + p_dev_rec->rand_addr_type = BLE_ADDR_RANDOM; memcpy(p_dev_rec->pseudo_addr, peer_addr, BLE_DEV_ADDR_LEN); memcpy(p_dev_rec->rand_addr, peer_addr, BLE_DEV_ADDR_LEN); memcpy(p_dev_rec->identity_addr, peer_addr, BLE_DEV_ADDR_LEN); @@ -525,9 +526,9 @@ ble_hs_rand_prand_get(uint8_t *prand) { uint16_t sum; int rc; - int retry = 100; + int i; - while (retry--) { + for (i = 0; i < 100; i++) { /* Get 24 bits of random data */ rc = ble_hs_hci_util_rand(prand, 3); if (rc != 0) { @@ -537,20 +538,15 @@ ble_hs_rand_prand_get(uint8_t *prand) /* Prand cannot be all zeros or 1's. */ sum = prand[0] + prand[1] + prand[2]; if ((sum != 0) && (sum != (3 * 0xff))) { - break; + /* Upper two bits must be 01 */ + prand[2] &= ~0xc0; + prand[2] |= 0x40; + return 0; } } - if (retry <= 0) { - BLE_HS_LOG(ERROR, "Failed to generate random prand\n"); - return BLE_HS_ETIMEOUT; - } - - /* Upper two bits must be 01 */ - prand[2] &= ~0xc0; - prand[2] |= 0x40; - - return 0; + BLE_HS_LOG(ERROR, "Failed to generate random prand\n"); + return BLE_HS_ETIMEOUT; } static bool @@ -674,9 +670,9 @@ ble_hs_get_rpa_local(void) static void ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev) { -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_resolv_ctx == NULL) { rc = ble_hs_resolv_ctx_ensure_init(); if (rc != 0) { @@ -689,7 +685,10 @@ ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev) BLE_HS_LOG(DEBUG, "RPA/NRPA Timeout; start active adv & scan with new Private address \n"); ble_gap_preempt(); /* Generate local private address */ - ble_hs_gen_own_private_rnd(); + rc = ble_hs_gen_own_private_rnd(); + if (rc != 0) { + BLE_HS_LOG(ERROR, "RPA/NRPA generation failed; rc=%d\n", rc); + } ble_npl_callout_reset(&g_ble_hs_resolv_data.rpa_timer, (int32_t)g_ble_hs_resolv_data.rpa_tmo); ble_gap_preempt_done(); @@ -717,7 +716,9 @@ ble_hs_is_on_resolv_list(uint8_t *addr, uint8_t addr_type) struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[1]; for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) { - if ((!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN)) || (!memcmp(rl->rl_peer_rpa, addr, BLE_DEV_ADDR_LEN))) { + if (rl->rl_addr_type == addr_type && + ((!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN)) || + (!memcmp(rl->rl_peer_rpa, addr, BLE_DEV_ADDR_LEN)))) { return i; } ++rl; @@ -871,17 +872,17 @@ ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr) if (position) { memmove(&g_ble_hs_resolv_list[position], &g_ble_hs_resolv_list[position + 1], - (g_ble_hs_resolv_data.rl_cnt - position) * sizeof (struct + (g_ble_hs_resolv_data.rl_cnt - position - 1) * sizeof (struct ble_hs_resolv_entry)); --g_ble_hs_resolv_data.rl_cnt; rc = 0; } - /* As we are removing the RL record, it is needed to change - * peer_address to its latest received OTA address, this helps when existing bond at - * peer side is removed */ - ble_rpa_replace_id_with_rand_addr(&addr_type, ident_addr); + /* Only replace identity addr with RPA if the entry was actually removed */ + if (rc == 0) { + ble_rpa_replace_id_with_rand_addr(&addr_type, ident_addr); + } #endif return rc; @@ -1078,7 +1079,6 @@ ble_hs_resolv_rpa_addr(uint8_t *addr, uint8_t addr_type) { for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) { if(ble_hs_resolv_rpa(addr, rl->rl_peer_irk) == 0) { memcpy(g_ble_hs_resolv_list[i].rl_peer_rpa, addr, BLE_DEV_ADDR_LEN); - g_ble_hs_resolv_list[i].rl_addr_type = addr_type; return rl; } diff --git a/nimble/host/src/ble_hs_resolv_priv.h b/nimble/host/src/ble_hs_resolv_priv.h index 370f5a7d5..0afd027f2 100644 --- a/nimble/host/src/ble_hs_resolv_priv.h +++ b/nimble/host/src/ble_hs_resolv_priv.h @@ -109,6 +109,8 @@ int ble_hs_resolv_rpa(uint8_t *rpa, uint8_t *irk); /* Initialize resolv*/ void ble_hs_resolv_init(void); +uint8_t is_ble_hs_resolv_enabled(void); + #ifdef __cplusplus } #endif diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index d1a0f2436..ea89ee3e9 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -209,17 +209,21 @@ ble_hs_startup_read_buf_sz(void) } } + ble_hs_lock(); rc = ble_hs_hci_set_buf_sz(pktlen, max_pkts); + ble_hs_unlock(); if (rc != 0) { return rc; } #if MYNEWT_VAL(BLE_ISO) - ble_hs_lock(); - rc = ble_hs_hci_set_iso_buf_sz(iso_pktlen, iso_max_pkts); - ble_hs_unlock(); - if (rc != 0) { - return rc; + if (iso_pktlen != 0 && iso_max_pkts != 0) { + ble_hs_lock(); + rc = ble_hs_hci_set_iso_buf_sz(iso_pktlen, iso_max_pkts); + ble_hs_unlock(); + if (rc != 0) { + return rc; + } } #endif /* MYNEWT_VAL(BLE_ISO) */ @@ -400,7 +404,7 @@ ble_hs_startup_le_set_evmask_tx(void) #endif /* MYNEWT_VAL(BLE_ISO) */ #if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) - if (version >= BLE_HCI_VER_BCS_5_4) { + if (version >= BLE_HCI_VER_BCS_6_0) { /** * Enable the following LE events: * 0x0000080000000000 LE CS Read Remote Supported Capabilities Complete event diff --git a/nimble/host/src/ble_hs_stop.c b/nimble/host/src/ble_hs_stop.c index 1e9612b7a..aab83af65 100644 --- a/nimble/host/src/ble_hs_stop.c +++ b/nimble/host/src/ble_hs_stop.c @@ -90,11 +90,20 @@ ble_hs_stop_done(int status) slist = ble_hs_stop_listeners; SLIST_INIT(&ble_hs_stop_listeners); - ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF; + if (status == 0) { + ble_hs_enabled_state = BLE_HS_ENABLED_STATE_OFF; + } else { + ble_hs_enabled_state = BLE_HS_ENABLED_STATE_ON; + } ble_hs_unlock(); - SLIST_FOREACH(listener, &slist, link) { + if (status != 0) { + ble_hs_timer_resched(); + } + + struct ble_hs_stop_listener *listener_next; + SLIST_FOREACH_SAFE(listener, &slist, link, listener_next) { listener->fn(status, listener->arg); } } @@ -169,8 +178,7 @@ ble_hs_stop_terminate_timeout_cb(struct ble_npl_event *ev) BLE_HS_LOG(ERROR, "ble_hs_stop_terminate_timeout_cb," "%d connection(s) still up \n", ble_hs_stop_conn_cnt); - /* TODO: Shall we send error here? */ - ble_hs_stop_done(0); + ble_hs_stop_done(BLE_HS_ETIMEOUT); } /** @@ -208,8 +216,20 @@ static void ble_hs_stop_register_listener(struct ble_hs_stop_listener *listener, ble_hs_stop_fn *fn, void *arg) { + struct ble_hs_stop_listener *cur; + BLE_HS_DBG_ASSERT(fn != NULL); + /* Prevent duplicate insertion which would create a cycle in the list. */ + SLIST_FOREACH(cur, &ble_hs_stop_listeners, link) { + if (cur == listener) { + /* Update fn/arg in case caller re-registers with new values */ + listener->fn = fn; + listener->arg = arg; + return; + } + } + listener->fn = fn; listener->arg = arg; SLIST_INSERT_HEAD(&ble_hs_stop_listeners, listener, link); @@ -219,6 +239,10 @@ static int ble_hs_stop_begin(struct ble_hs_stop_listener *listener, ble_hs_stop_fn *fn, void *arg) { + if (listener != NULL && fn == NULL) { + return BLE_HS_EINVAL; + } + switch (ble_hs_enabled_state) { case BLE_HS_ENABLED_STATE_ON: /* Host is enabled; proceed with the stop procedure. */ @@ -339,12 +363,14 @@ ble_hs_stop_init(void) void ble_hs_stop_deinit(void) { - ble_npl_callout_deinit(&ble_hs_stop_terminate_tmo); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_stop_ctx) { + ble_npl_callout_deinit(&ble_hs_stop_terminate_tmo); nimble_platform_mem_free(ble_hs_stop_ctx); ble_hs_stop_ctx = NULL; } +#else + ble_npl_callout_deinit(&ble_hs_stop_terminate_tmo); #endif } diff --git a/nimble/host/src/ble_ibeacon.c b/nimble/host/src/ble_ibeacon.c index c6135c044..e0d3fb507 100644 --- a/nimble/host/src/ble_ibeacon.c +++ b/nimble/host/src/ble_ibeacon.c @@ -64,16 +64,19 @@ ble_ibeacon_set_adv_data(const void *uuid128, uint16_t major, return BLE_HS_EINVAL; } + ble_hs_lock(); + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -if (buf == NULL) { - /* Since the data used by buf is needed for adv reattempts, - * this data shall stay in the heap and not be freed. - */ - buf = nimble_platform_mem_calloc(BLE_IBEACON_MFG_DATA_SIZE, sizeof(uint8_t)); if (buf == NULL) { - return BLE_HS_ENOMEM; + /* Since the data used by buf is needed for adv reattempts, + * this data shall stay in the heap and not be freed. + */ + buf = nimble_platform_mem_calloc(BLE_IBEACON_MFG_DATA_SIZE, sizeof(uint8_t)); + if (buf == NULL) { + ble_hs_unlock(); + return BLE_HS_ENOMEM; + } } -} #endif /** Company identifier (Apple). */ @@ -106,5 +109,6 @@ if (buf == NULL) { BLE_HS_ADV_F_BREDR_UNSUP; rc = ble_gap_adv_set_fields(&fields); + ble_hs_unlock(); return rc; } diff --git a/nimble/host/src/ble_l2cap.c b/nimble/host/src/ble_l2cap.c index d0eb94699..f3daa7d6e 100644 --- a/nimble/host/src/ble_l2cap.c +++ b/nimble/host/src/ble_l2cap.c @@ -117,15 +117,15 @@ ble_l2cap_chan_free(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan) ble_l2cap_coc_cleanup_chan(conn, chan); -#if MYNEWT_VAL(BLE_HS_DEBUG) - memset(chan, 0xff, sizeof *chan); -#endif - #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_l2cap_ctx == NULL) { return; } #endif + +#if MYNEWT_VAL(BLE_HS_DEBUG) + memset(chan, 0xff, sizeof *chan); +#endif rc = os_memblock_put(&ble_l2cap_chan_pool, chan); BLE_HS_DBG_ASSERT_EVAL(rc == 0); @@ -241,9 +241,18 @@ ble_l2cap_reconfig(struct ble_l2cap_chan *chans[], uint8_t num, uint16_t new_mtu return BLE_HS_EINVAL; } + if (chans[0] == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + conn_handle = chans[0]->conn_handle; for (i = 1; i < num; i++) { + if (chans[i] == NULL) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } if (conn_handle != chans[i]->conn_handle) { BLE_HS_LOG(ERROR, "All channels should have same conn handle\n"); return BLE_HS_EINVAL; @@ -582,11 +591,16 @@ ble_l2cap_init(void) return 0; done: #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + ble_l2cap_sig_deinit(); + ble_l2cap_coc_deinit(); + ble_sm_deinit(); + #if !MYNEWT_VAL(MP_RUNTIME_ALLOC) nimble_platform_mem_free(ble_l2cap_chan_mem); ble_l2cap_chan_mem = NULL; #endif os_mempool_unregister(&ble_l2cap_chan_pool); + memset(&ble_l2cap_chan_pool, 0, sizeof(ble_l2cap_chan_pool)); nimble_platform_mem_free(ble_l2cap_ctx); ble_l2cap_ctx = NULL; #endif diff --git a/nimble/host/src/ble_l2cap_coc.c b/nimble/host/src/ble_l2cap_coc.c index cd1df00ef..f539e3167 100644 --- a/nimble/host/src/ble_l2cap_coc.c +++ b/nimble/host/src/ble_l2cap_coc.c @@ -106,11 +106,21 @@ ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, { struct ble_l2cap_coc_srv *srv; + if (psm < 0x0001 || psm > 0x00FF) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + if (cb == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } + if (mtu < BLE_L2CAP_COC_MIN_MTU) { + BLE_HS_LOG(ERROR, "%s mtu too small rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + ble_hs_lock(); if (ble_l2cap_coc_srv_find(psm) != NULL) { @@ -142,13 +152,13 @@ ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, static inline void ble_l2cap_set_used_cid(uint32_t *cid_mask, int bit) { - cid_mask[bit / 32] |= (1 << (bit % 32)); + cid_mask[bit / 32] |= (1U << (bit % 32)); } static inline void ble_l2cap_clear_used_cid(uint32_t *cid_mask, int bit) { - cid_mask[bit / 32] &= ~(1 << (bit % 32)); + cid_mask[bit / 32] &= ~(1U << (bit % 32)); } static inline int @@ -183,7 +193,7 @@ ble_l2cap_coc_get_cid(uint32_t *cid_mask) int bit; bit = ble_l2cap_get_first_available_bit(cid_mask); - if (bit < 0) { + if (bit < 0 || bit >= MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)) { return -1; } @@ -219,6 +229,7 @@ ble_l2cap_event_coc_received_data(struct ble_l2cap_chan *chan, { struct ble_l2cap_event event; + memset(&event, 0, sizeof event); event.type = BLE_L2CAP_EVENT_COC_DATA_RECEIVED; event.receive.conn_handle = chan->conn_handle; event.receive.chan = chan; @@ -244,7 +255,16 @@ ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan, struct os_mbuf **om) BLE_HS_DBG_ASSERT(rx != NULL); rx_sdu = rx->sdus[chan->coc_rx.current_sdu_idx]; - BLE_HS_DBG_ASSERT(rx_sdu != NULL); + if (rx_sdu == NULL) { + /* Peer used a credit while no RX SDU buffer is armed (app deferred + * ble_l2cap_recv_ready). This is a credit/buffer asymmetry — disconnect + * rather than dereference a NULL pointer. + */ + BLE_HS_LOG(ERROR, "CoC RX: no SDU buffer armed (idx=%d), disconnecting\n", + chan->coc_rx.current_sdu_idx); + ble_l2cap_disconnect(chan); + return BLE_HS_ENOMEM; + } om_total = OS_MBUF_PKTLEN(*om); @@ -522,8 +542,8 @@ ble_l2cap_coc_continue_tx(struct ble_l2cap_chan *chan) { struct ble_l2cap_coc_endpoint *tx; uint16_t len; - uint16_t left_to_send; - struct os_mbuf *txom; + uint32_t left_to_send; + struct os_mbuf *txom = NULL; struct ble_hs_conn *conn; uint16_t sdu_size_offset; int rc; @@ -549,7 +569,13 @@ ble_l2cap_coc_continue_tx(struct ble_l2cap_chan *chan) } /* Take into account peer MTU */ - len = min(left_to_send, chan->peer_coc_mps); + len = min(left_to_send, (uint32_t)chan->peer_coc_mps); + + /* Guard against underflow: first packet needs at least SDU_SIZE bytes */ + if (len < sdu_size_offset) { + rc = BLE_HS_EINVAL; + goto failed; + } /* Prepare packet */ txom = ble_hs_mbuf_l2cap_pkt(); @@ -690,9 +716,7 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx) ble_hs_lock(); - if (chan->coc_rx.sdus[0] != NULL && - chan->coc_rx.next_sdu_alloc_idx == chan->coc_rx.current_sdu_idx && - BLE_L2CAP_SDU_BUFF_CNT != 1) { + if (chan->coc_rx.sdus[chan->coc_rx.next_sdu_alloc_idx] != NULL) { ble_hs_unlock(); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBUSY); return BLE_HS_EBUSY; @@ -724,13 +748,18 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx) credits_to_send); ble_hs_lock(); /* Re-validate connection and channel after relock — they could have - * been freed while the lock was released. */ - /* Re-validate connection and channel after relock — they could have - * been freed while the lock was released. Credits were already updated - * at line 714 before unlocking; no further update needed here. */ + * been freed while the lock was released. coc_rx.credits was already + * updated before unlocking; no further update needed here. */ conn = ble_hs_conn_find(cached_conn_handle); if (conn != NULL) { c = ble_hs_conn_chan_find_by_scid(conn, cached_scid); + } else { + c = NULL; + } + + if (c == NULL) { + ble_hs_unlock(); + return 0; } } @@ -824,6 +853,14 @@ void ble_l2cap_coc_deinit(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_l2cap_coc_ctx) { + struct ble_l2cap_coc_srv *srv; + + /* Free all active server entries before destroying the pool */ + while ((srv = STAILQ_FIRST(&ble_l2cap_coc_srvs)) != NULL) { + STAILQ_REMOVE_HEAD(&ble_l2cap_coc_srvs, next); + os_memblock_put(&ble_l2cap_coc_srv_pool, srv); + } + #if !MYNEWT_VAL(MP_RUNTIME_ALLOC) if (ble_l2cap_coc_srv_mem) { nimble_platform_mem_free(ble_l2cap_coc_srv_mem); diff --git a/nimble/host/src/ble_l2cap_coc_priv.h b/nimble/host/src/ble_l2cap_coc_priv.h index 7bcd615ad..4db6988f9 100644 --- a/nimble/host/src/ble_l2cap_coc_priv.h +++ b/nimble/host/src/ble_l2cap_coc_priv.h @@ -33,6 +33,9 @@ extern "C" { #define BLE_L2CAP_COC_CID_START 0x0040 #define BLE_L2CAP_COC_CID_END 0x007F +/* Core Spec Vol 3 Part A §4.22 — LE Credit Based Flow Control min MTU */ +#define BLE_L2CAP_COC_MIN_MTU 23 + struct ble_l2cap_chan; #define BLE_L2CAP_COC_FLAG_STALLED 0x01 diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c index 7c688e394..7fdf6c853 100644 --- a/nimble/host/src/ble_l2cap_sig.c +++ b/nimble/host/src/ble_l2cap_sig.c @@ -410,12 +410,12 @@ static int ble_l2cap_sig_check_conn_params(const struct ble_gap_upd_params *params) { /* Check connection interval min */ - if ((params->itvl_min < BLE_HCI_CONN_ITVL_MIN) || + if ((params->itvl_min < BLE_HOST_CONN_PARAM_ITVL_MIN) || (params->itvl_min > BLE_HCI_CONN_ITVL_MAX)) { return BLE_ERR_INV_HCI_CMD_PARMS; } /* Check connection interval max */ - if ((params->itvl_max < BLE_HCI_CONN_ITVL_MIN) || + if ((params->itvl_max < BLE_HOST_CONN_PARAM_ITVL_MIN) || (params->itvl_max > BLE_HCI_CONN_ITVL_MAX) || (params->itvl_max < params->itvl_min)) { return BLE_ERR_INV_HCI_CMD_PARMS; @@ -432,6 +432,12 @@ ble_l2cap_sig_check_conn_params(const struct ble_gap_upd_params *params) return BLE_ERR_INV_HCI_CMD_PARMS; } + /* supervision_timeout must be greater than (1+latency)*itvl_max*2 (in units) */ + if ((uint32_t)params->supervision_timeout * 4 <= + (uint32_t)(1 + params->latency) * params->itvl_max) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + /* Check connection event length */ if (params->min_ce_len > params->max_ce_len) { return BLE_ERR_INV_HCI_CMD_PARMS; @@ -571,7 +577,8 @@ ble_l2cap_sig_update_rsp_rx(uint16_t conn_handle, done: ble_l2cap_sig_update_call_cb(proc, cb_status); ble_l2cap_sig_proc_free(proc); - return rc; + /* Silently ignore errors as this is a response signal */ + return 0; } int @@ -649,6 +656,29 @@ done: #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 +#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC) +static int +ble_l2cap_sig_reconfig_err2ble_hs_err(uint16_t reconfig_err) +{ + switch (reconfig_err) { + case BLE_L2CAP_ERR_RECONFIG_SUCCEED: + return 0; + case BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED: + case BLE_L2CAP_ERR_RECONFIG_REDUCTION_MPS_NOT_ALLOWED: + case BLE_L2CAP_ERR_RECONFIG_INVALID_DCID: + return BLE_HS_EREJECT; + case BLE_L2CAP_ERR_RECONFIG_UNACCEPTED_PARAM: + BLE_HS_LOG(ERROR, "%s reconfig_err=0x%04x rc=%d\n", __func__, + reconfig_err, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + default: + BLE_HS_LOG(ERROR, "%s reconfig_err=0x%04x rc=%d\n", __func__, + reconfig_err, BLE_HS_EUNKNOWN); + return BLE_HS_EUNKNOWN; + } +} +#endif + static int ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err) { @@ -679,6 +709,9 @@ ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err) case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS: BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; + case BLE_L2CAP_COC_ERR_INVALID_PARAMETERS: + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; default: BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; @@ -866,7 +899,7 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle, rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_SUCCEED); cid_cnt = (hdr->length - sizeof(*req)) / sizeof(uint16_t); - if (cid_cnt > BLE_L2CAP_MAX_COC_CONN_REQ) { + if (cid_cnt == 0 || cid_cnt > BLE_L2CAP_MAX_COC_CONN_REQ) { rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCEPTED_PARAM); goto failed; } @@ -878,7 +911,7 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle, * Core Specification Vol 3 Part A 4.28 */ chan[i] = ble_hs_conn_chan_find_by_scid(conn, host_dcid); - if (!chan[i]) { + if (!chan[i] || chan[i]->psm == 0) { rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_INVALID_DCID); goto failed; } @@ -973,7 +1006,8 @@ ble_l2cap_sig_credit_base_reconfig_rsp_rx(uint16_t conn_handle, } rsp = (struct ble_l2cap_sig_credit_base_reconfig_rsp *)(*om)->om_data; - ble_l2cap_sig_coc_reconfig_cb(proc, (rsp->result > 0) ? BLE_HS_EREJECT : 0); + ble_l2cap_sig_coc_reconfig_cb(proc, + ble_l2cap_sig_reconfig_err2ble_hs_err(le16toh(rsp->result))); done: if (rc != 0) { @@ -1213,21 +1247,26 @@ ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle, rsp = (struct ble_l2cap_sig_credit_base_connect_rsp *)(*om)->om_data; if (rsp->result) { - rc = ble_l2cap_sig_coc_err2ble_hs_err(le16toh(rsp->result)); + uint16_t result = le16toh(rsp->result); + rc = ble_l2cap_sig_coc_err2ble_hs_err(result); /* Below results means that some of the channels has not been created * and we have to look closer into the response. * Any other results means that all the connections has been refused. */ - if ((rsp->result != BLE_L2CAP_COC_ERR_NO_RESOURCES) && - (rsp->result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) && - (rsp->result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) { + if ((result != BLE_L2CAP_COC_ERR_NO_RESOURCES) && + (result != BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID) && + (result != BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED)) { goto done; } } ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - assert(conn != NULL); + if (conn == NULL) { + ble_hs_unlock(); + rc = BLE_HS_ENOTCONN; + goto done; + } for (i = 0; i < proc->connect.chan_cnt; i++) { chan = proc->connect.chan[i]; @@ -1375,6 +1414,10 @@ ble_l2cap_sig_coc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, rc = ble_l2cap_sig_tx(conn_handle, txom); if (rc != 0) { + /* Notify app before suppressing callback during channel teardown. */ + ble_l2cap_event_coc_connected(chan, rc); + /* Make sure we do not send disconnect event when removing channel */ + chan->cb = NULL; ble_hs_lock(); conn = ble_hs_conn_find_assert(conn_handle); ble_hs_conn_delete_chan(conn, chan); @@ -1432,6 +1475,14 @@ ble_l2cap_sig_coc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, goto done; } + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + rc = BLE_HS_ENOTCONN; + goto done; + } + /* Fill up remote configuration * Note MPS is the L2CAP MTU */ @@ -1440,9 +1491,6 @@ ble_l2cap_sig_coc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, chan->coc_tx.mtu = le16toh(rsp->mtu); chan->coc_tx.credits = le16toh(rsp->credits); - ble_hs_lock(); - conn = ble_hs_conn_find(conn_handle); - assert(conn != NULL); ble_hs_conn_chan_insert(conn, chan); ble_hs_unlock(); @@ -1469,6 +1517,9 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, int rc; if (!sdu_rx || !cb) { + if (sdu_rx != NULL) { + os_mbuf_free_chain(sdu_rx); + } BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -1477,8 +1528,8 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, conn = ble_hs_conn_find(conn_handle); if (!conn) { - ble_hs_unlock(); os_mbuf_free_chain(sdu_rx); + ble_hs_unlock(); return BLE_HS_ENOTCONN; } @@ -1491,6 +1542,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, proc = ble_l2cap_sig_proc_alloc(); if (!proc) { + chan->cb = NULL; ble_l2cap_chan_free(conn, chan); ble_hs_unlock(); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); @@ -1506,6 +1558,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ, proc->id, sizeof(*req), &txom); if (!req) { + chan->cb = NULL; ble_l2cap_chan_free(conn, chan); ble_hs_unlock(); rc = BLE_HS_ENOMEM; @@ -1525,6 +1578,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, if (rc != 0) { ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); + chan->cb = NULL; ble_l2cap_chan_free(conn, chan); ble_hs_unlock(); } @@ -1549,8 +1603,23 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, int rc; int i; int j; + bool sdu_rx_freed = false; + + if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) { + if (sdu_rx != NULL) { + for (i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } + } + return BLE_HS_EINVAL; + } if (!sdu_rx || !cb) { + if (sdu_rx != NULL) { + for (i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } + } BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -1559,23 +1628,23 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, conn = ble_hs_conn_find(conn_handle); if (!conn) { + for (i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } ble_hs_unlock(); return BLE_HS_ENOTCONN; } proc = ble_l2cap_sig_proc_alloc(); if (!proc) { + for (i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } ble_hs_unlock(); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } - if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) { - ble_hs_unlock(); - rc = BLE_HS_EINVAL; - goto done; - } - proc->op = BLE_L2CAP_SIG_PROC_OP_CONNECT; proc->id = ble_l2cap_sig_next_id(); proc->conn_handle = conn_handle; @@ -1600,6 +1669,12 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, proc->connect.chan[j]->cb = NULL; ble_l2cap_chan_free(conn, proc->connect.chan[j]); } + /* sdu_rx[i] was already freed internally by ble_l2cap_coc_chan_alloc; + * only free the remaining unprocessed buffers sdu_rx[i+1..num-1]. */ + for (j = i + 1; j < num; j++) { + os_mbuf_free_chain(sdu_rx[j]); + } + sdu_rx_freed = true; ble_hs_unlock(); rc = BLE_HS_ENOMEM; goto done; @@ -1620,8 +1695,23 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, ble_hs_unlock(); rc = ble_l2cap_sig_tx(proc->conn_handle, txom); + if (rc != 0) { + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + for (i = 0; i < proc->connect.chan_cnt; i++) { + proc->connect.chan[i]->cb = NULL; + ble_l2cap_chan_free(conn, proc->connect.chan[i]); + proc->connect.chan[i] = NULL; + } + ble_hs_unlock(); + } done: + if (rc != 0 && proc != NULL && proc->connect.chan_cnt == 0 && !sdu_rx_freed) { + for (i = 0; i < num; i++) { + os_mbuf_free_chain(sdu_rx[i]); + } + } ble_l2cap_sig_process_status(proc, rc); return rc; @@ -1664,7 +1754,7 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[], proc->reconfig.cids[i] = chans[i]->scid; } else { ble_hs_unlock(); - rc = BLE_HS_ENOMEM; + rc = BLE_HS_ENOENT; goto done; } } @@ -1751,7 +1841,8 @@ ble_l2cap_sig_disc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, if (!chan) { os_mbuf_free_chain(txom); ble_hs_unlock(); - ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, hdr->identifier, req->dcid, req->scid); + ble_l2cap_sig_reject_invalid_cid_tx(conn_handle, hdr->identifier, + le16toh(req->scid), le16toh(req->dcid)); return 0; } if (le16toh(req->scid) != chan->dcid) { @@ -1850,7 +1941,8 @@ ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, done: ble_l2cap_sig_proc_free(proc); - return rc; + /* Silently ignore errors as this is a response signal */ + return 0; } int @@ -1907,7 +1999,7 @@ ble_l2cap_sig_le_credits_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, rc = ble_hs_mbuf_pullup_base(om, sizeof(*req)); if (rc != 0) { - return 0; + return rc; } if (OS_MBUF_PKTLEN(*om) != sizeof(*req)) { @@ -2090,6 +2182,7 @@ ble_l2cap_sig_extract_expired(struct ble_l2cap_sig_proc_list *dst_list) if (time_diff < next_exp_in) { next_exp_in = time_diff; } + prev = proc; } proc = next; @@ -2186,6 +2279,11 @@ ble_l2cap_sig_timer(void) case BLE_L2CAP_SIG_PROC_OP_DISCONNECT: ble_l2cap_sig_coc_disconnect_cb(proc, BLE_HS_ETIMEOUT); break; +#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC) + case BLE_L2CAP_SIG_PROC_OP_RECONFIG: + ble_l2cap_sig_coc_reconfig_cb(proc, BLE_HS_ETIMEOUT); + break; +#endif #endif } diff --git a/nimble/host/src/ble_l2cap_sig_cmd.c b/nimble/host/src/ble_l2cap_sig_cmd.c index ec8364a33..d46b68703 100644 --- a/nimble/host/src/ble_l2cap_sig_cmd.c +++ b/nimble/host/src/ble_l2cap_sig_cmd.c @@ -71,7 +71,9 @@ ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason, } cmd->reason = htole16(reason); - memcpy(cmd->data, data, data_len); + if (data_len > 0) { + memcpy(cmd->data, data, data_len); + } STATS_INC(ble_l2cap_stats, sig_tx); return ble_l2cap_sig_tx(conn_handle, txom); @@ -85,8 +87,8 @@ ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id, uint16_t local_cid; uint16_t remote_cid; } data = { - .local_cid = dst_cid, - .remote_cid = src_cid, + .local_cid = htole16(dst_cid), + .remote_cid = htole16(src_cid), }; return ble_l2cap_sig_reject_tx(conn_handle, id, @@ -105,14 +107,13 @@ ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len, return NULL; } - if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) { + hdr = os_mbuf_extend(*txom, sizeof(*hdr) + len); + if (hdr == NULL) { os_mbuf_free_chain(*txom); *txom = NULL; return NULL; } - hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data; - hdr->op = opcode; hdr->identifier = id; hdr->length = htole16(len); diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index eb2e916d7..2d1a4e6bb 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -267,6 +267,11 @@ ble_sm_dbg_set_next_master_id_rand(uint64_t next_master_id_rand) void ble_sm_dbg_set_next_ltk(uint8_t *next_ltk) { + if (next_ltk == NULL) { + ble_sm_dbg_next_ltk_set = 0; + return; + } + memcpy(ble_sm_dbg_next_ltk, next_ltk, sizeof ble_sm_dbg_next_ltk); ble_sm_dbg_next_ltk_set = 1; @@ -994,7 +999,9 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle, struct ble_gap_repeat_pairing rp; struct ble_store_value_sec bond; int rc; + int delete_retries; + delete_retries = 0; do { #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) int defer_repeat_chk; @@ -1020,6 +1027,7 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle, case 0: break; case BLE_HS_ENOENT: + case BLE_HS_ENOTSUP: return 0; default: return rc; @@ -1056,6 +1064,19 @@ ble_sm_chk_repeat_pairing(uint16_t conn_handle, rp.new_bonding = !!(proc_flags & BLE_SM_PROC_F_BONDING); rc = ble_gap_repeat_pairing_event(&rp); + if (rc == BLE_GAP_REPEAT_PAIRING_RETRY) { + delete_retries++; + if (delete_retries >= 2) { + rc = ble_sm_read_bond(conn_handle, &bond); + if (rc == BLE_HS_ENOENT || rc == BLE_HS_ENOTSUP) { + return 0; + } + if (rc != 0) { + return rc; + } + rc = BLE_GAP_REPEAT_PAIRING_IGNORE; + } + } } while (rc == BLE_GAP_REPEAT_PAIRING_RETRY); BLE_HS_LOG(DEBUG, "silently ignoring pair request from bonded peer"); @@ -1192,12 +1213,14 @@ static void ble_sm_key_dist(struct ble_sm_proc *proc, uint8_t *out_init_key_dist, uint8_t *out_resp_key_dist) { + struct ble_sm_pair_cmd *pair_req; struct ble_sm_pair_cmd *pair_rsp; + pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1]; pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1]; - *out_init_key_dist = pair_rsp->init_key_dist; - *out_resp_key_dist = pair_rsp->resp_key_dist; + *out_init_key_dist = pair_req->init_key_dist & pair_rsp->init_key_dist; + *out_resp_key_dist = pair_req->resp_key_dist & pair_rsp->resp_key_dist; /* Encryption info and master ID are only sent in legacy pairing. */ if (proc->flags & BLE_SM_PROC_F_SC) { @@ -1315,6 +1338,10 @@ ble_sm_enc_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, BLE_HS_DBG_ASSERT(cmd != NULL); res->app_status = ble_sm_start_encrypt_tx(cmd); + if (res->app_status != 0) { + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + res->enc_cb = 1; + } } static void @@ -1449,7 +1476,8 @@ ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type, if (rc != 0) { return rc; } - if (value_sec->ediv != ediv || value_sec->rand_num != rand) { + if (!value_sec->ltk_present || + value_sec->ediv != ediv || value_sec->rand_num != rand) { return BLE_HS_ENOENT; } return rc; @@ -1484,7 +1512,7 @@ static int ble_sm_ltk_req_neg_reply_tx(uint16_t conn_handle) { struct ble_hci_le_lt_key_req_neg_reply_cp cmd; - struct ble_hci_le_lt_key_req_neg_reply_cp rsp; + struct ble_hci_le_lt_key_req_neg_reply_rp rsp; int rc; cmd.conn_handle = htole16(conn_handle); @@ -1514,6 +1542,7 @@ ble_sm_ltk_start_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, proc->state = BLE_SM_PROC_STATE_ENC_START; } else { res->enc_cb = 1; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; } } @@ -1590,6 +1619,8 @@ ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev) proc = ble_sm_proc_alloc(); if (proc == NULL) { res.app_status = BLE_HS_ENOMEM; + ble_sm_ltk_req_neg_reply_tx(conn_handle); + restore = 0; } else { proc->conn_handle = conn_handle; proc->state = BLE_SM_PROC_STATE_LTK_RESTORE; @@ -1618,9 +1649,19 @@ ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev) } if (restore) { - conn = ble_hs_conn_find_assert(conn_handle); - ble_hs_conn_addrs(conn, &addrs); - memcpy(peer_id_addr, addrs.peer_id_addr.val, 6); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + /* Connection gone; inform controller so it does not wait for an + * LTK reply until supervision timeout fires. */ + ble_sm_ltk_req_neg_reply_tx(conn_handle); + /* proc was inserted into the list; let ble_sm_process_result + * remove and free it via non-zero app_status. */ + res.app_status = BLE_HS_ENOTCONN; + res.execute = 0; + } else { + ble_hs_conn_addrs(conn, &addrs); + memcpy(peer_id_addr, addrs.peer_id_addr.val, 6); + } } ble_hs_unlock(); @@ -1798,11 +1839,12 @@ ble_sm_confirm_rx(uint16_t conn_handle, struct os_mbuf **om, static uint8_t ble_sm_state_after_pair(struct ble_sm_proc *proc) { +#if MYNEWT_VAL(BLE_SM_SC) if (proc->flags & BLE_SM_PROC_F_SC) { return BLE_SM_PROC_STATE_PUBLIC_KEY; - } else { - return BLE_SM_PROC_STATE_CONFIRM; } +#endif + return BLE_SM_PROC_STATE_CONFIRM; } static void @@ -1979,7 +2021,9 @@ ble_sm_verify_auth_requirements(uint8_t cmd) * on our side. If not, we can fail right away. */ if (!(cmd & BLE_SM_PAIR_AUTHREQ_SC)) { - if (MYNEWT_VAL(BLE_SM_LEGACY) == 0) { + if (MYNEWT_VAL(BLE_SM_LEGACY) == 0 || + ble_hs_cfg.sm_sc_only || + ble_hs_cfg.sm_sec_lvl == 4) { return false; } } @@ -2058,7 +2102,10 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, ble_hs_lock(); proc = ble_sm_proc_alloc(); - if (proc != NULL) { + if (proc == NULL) { + res->app_status = BLE_HS_ENOMEM; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + } else { proc->conn_handle = conn_handle; proc->state = BLE_SM_PROC_STATE_PAIR; ble_sm_insert(proc); @@ -2076,9 +2123,11 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, } else if (req->max_enc_key_size < BLE_SM_PAIR_KEY_SZ_MIN) { res->sm_err = BLE_SM_ERR_ENC_KEY_SZ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ); + res->enc_cb = 1; } else if (req->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) { res->sm_err = BLE_SM_ERR_INVAL; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); + res->enc_cb = 1; } else if (ble_hs_cfg.sm_sc_only && !(req->authreq & BLE_SM_PAIR_AUTHREQ_SC)) { /* Fail if Secure Connections Only mode is on and SC is not supported by peer */ @@ -2094,6 +2143,7 @@ ble_sm_pair_req_rx(uint16_t conn_handle, struct os_mbuf **om, } else if (!ble_sm_verify_auth_requirements(req->authreq)) { res->sm_err = BLE_SM_ERR_AUTHREQ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ); + res->enc_cb = 1; } else { /* The request looks good. Precalculate our pairing response and * determine some properties of the imminent link. We need this @@ -2158,18 +2208,22 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, struct os_mbuf **om, if (rsp->max_enc_key_size < BLE_SM_PAIR_KEY_SZ_MIN) { res->sm_err = BLE_SM_ERR_ENC_KEY_SZ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ); + res->enc_cb = 1; } else if (rsp->max_enc_key_size > BLE_SM_PAIR_KEY_SZ_MAX) { res->sm_err = BLE_SM_ERR_INVAL; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); + res->enc_cb = 1; } else if (ble_hs_cfg.sm_sc_only && (rsp->max_enc_key_size != BLE_SM_PAIR_KEY_SZ_MAX)) { /* Fail if Secure Connections Only mode is on and remote does not meet * key size requirements - MITM was checked in last step */ res->sm_err = BLE_SM_ERR_ENC_KEY_SZ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_ENC_KEY_SZ); + res->enc_cb = 1; } else if (!ble_sm_verify_auth_requirements(rsp->authreq)) { res->sm_err = BLE_SM_ERR_AUTHREQ; res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_AUTHREQ); + res->enc_cb = 1; } else { ble_sm_pair_cfg(proc); @@ -2259,8 +2313,8 @@ ble_sm_sec_req_rx(uint16_t conn_handle, struct os_mbuf **om, conn = ble_hs_conn_find_assert(conn_handle); - /* Check if pairing procedure is already in progress */ - if (ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_PAIR, 1, NULL)) { + /* Check if any SM procedure is already in progress */ + if (ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_NONE, -1, NULL)) { ble_hs_unlock(); res->app_status = 0; return; @@ -2745,6 +2799,13 @@ ble_sm_id_addr_info_rx(uint16_t conn_handle, struct os_mbuf **om, cmd = (struct ble_sm_id_addr_info *)(*om)->om_data; + if (cmd->addr_type != BLE_ADDR_PUBLIC && cmd->addr_type != BLE_ADDR_RANDOM) { + res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_INVAL); + res->sm_err = BLE_SM_ERR_INVAL; + res->enc_cb = 1; + return; + } + ble_hs_lock(); proc = ble_sm_proc_find(conn_handle, BLE_SM_PROC_STATE_KEY_EXCH, -1, NULL); @@ -2830,6 +2891,8 @@ ble_sm_fail_rx(uint16_t conn_handle, struct os_mbuf **om, res->app_status = BLE_HS_SM_PEER_ERR(cmd->reason); res->sm_err = cmd->reason; + } else { + res->sm_err = BLE_SM_ERR_UNSPECIFIED; } } @@ -2876,7 +2939,7 @@ ble_sm_incr_our_sign_counter(uint16_t conn_handle) } rc = ble_store_delete_our_sec(&key_sec); - if (rc != 0) { + if (rc != 0 && rc != BLE_HS_ENOENT) { return rc; } @@ -2926,11 +2989,6 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle) return BLE_HS_ENOMEM; } - rc = ble_store_delete_peer_sec(&key_sec); - if (rc != 0) { - return rc; - } - #if MYNEWT_VAL(BLE_HS_PVCY) #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) /* @@ -3101,6 +3159,14 @@ ble_sm_enc_initiate(uint16_t conn_handle, uint8_t key_size, struct ble_sm_proc *proc; struct hci_start_encrypt cmd; + if (key_size < BLE_SM_PAIR_KEY_SZ_MIN || key_size > BLE_SM_PAIR_KEY_SZ_MAX) { + return BLE_HS_EINVAL; + } + + if (ltk == NULL) { + return BLE_HS_EINVAL; + } + memset(&res, 0, sizeof res); /* Make sure a procedure isn't already in progress for this connection. */ @@ -3328,6 +3394,10 @@ ble_sm_connection_broken(uint16_t conn_handle) static int ble_sm_state_dispatch_init(void) { + if (ble_sm_state_dispatch != NULL) { + return 0; + } + ble_sm_state_dispatch = nimble_platform_mem_calloc(1, BLE_SM_PROC_STATE_CNT * sizeof(ble_sm_state_fn *)); if (!ble_sm_state_dispatch) { @@ -3460,7 +3530,7 @@ ble_sm_rx(struct ble_l2cap_chan *chan, struct os_mbuf **om) int rc; handle = ble_l2cap_get_conn_handle(chan); - if (!handle) { + if (handle == BLE_HS_CONN_HANDLE_NONE) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOTCONN); return BLE_HS_ENOTCONN; } diff --git a/nimble/host/src/ble_sm_alg.c b/nimble/host/src/ble_sm_alg.c index 286b3912f..c240b8dd6 100644 --- a/nimble/host/src/ble_sm_alg.c +++ b/nimble/host/src/ble_sm_alg.c @@ -93,6 +93,17 @@ static const char * const TAG = "ble_sm_alg"; #endif #endif +static void +ble_sm_alg_clear_buf(void *buf, size_t len) +{ + volatile uint8_t *vp = buf; + size_t i; + + for (i = 0; i < len; i++) { + vp[i] = 0; + } +} + static void ble_sm_alg_xor_128(const uint8_t *p, const uint8_t *q, uint8_t *r) { @@ -123,7 +134,9 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, status = psa_import_key(&key_attributes, tmp, 16, &key_id); if (status != PSA_SUCCESS) { ESP_LOGE(TAG, "Failed to import AES key: %d", status); + psa_reset_key_attributes(&key_attributes); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return BLE_HS_EUNKNOWN; } psa_reset_key_attributes(&key_attributes); @@ -137,6 +150,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, ESP_LOGE(TAG, "Encryption failed: %d", status); psa_destroy_key(key_id); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return BLE_HS_EUNKNOWN; } psa_destroy_key(key_id); @@ -147,6 +161,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, if (mbedtls_aes_setkey_enc(&s, tmp, 128) != 0) { mbedtls_aes_free(&s); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return BLE_HS_EUNKNOWN; } @@ -155,6 +170,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, if (mbedtls_aes_crypt_ecb(&s, MBEDTLS_AES_ENCRYPT, tmp, enc_data) != 0) { mbedtls_aes_free(&s); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return BLE_HS_EUNKNOWN; } @@ -165,6 +181,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, if (tc_aes128_set_encrypt_key(&s, tmp) == TC_CRYPTO_FAIL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return BLE_HS_EUNKNOWN; } @@ -172,12 +189,14 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, if (tc_aes_encrypt(enc_data, tmp, &s) == TC_CRYPTO_FAIL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return BLE_HS_EUNKNOWN; } #endif swap_in_place(enc_data, 16); + ble_sm_alg_clear_buf(tmp, sizeof(tmp)); return 0; } @@ -395,20 +414,32 @@ ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len, struct tc_cmac_struct state; if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) { + memset(&sched, 0, sizeof(sched)); + memset(&state, 0, sizeof(state)); + __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; } if (tc_cmac_update(&state, in, len) == TC_CRYPTO_FAIL) { + memset(&sched, 0, sizeof(sched)); + memset(&state, 0, sizeof(state)); + __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; } if (tc_cmac_final(out, &state) == TC_CRYPTO_FAIL) { + memset(&sched, 0, sizeof(sched)); + memset(&state, 0, sizeof(state)); + __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; } + memset(&sched, 0, sizeof(sched)); + memset(&state, 0, sizeof(state)); + __asm__ volatile("" : : "r"(&sched), "r"(&state) : "memory"); return 0; } #endif @@ -455,6 +486,9 @@ ble_sm_alg_f4(const uint8_t *u, const uint8_t *v, const uint8_t *x, swap_buf(xs, x, 16); rc = ble_sm_alg_aes_cmac(xs, m, sizeof(m), out_enc_data); + memset(xs, 0, sizeof(xs)); + /* Prevent compiler from optimizing away the sensitive-data clear */ + __asm__ volatile("" : : "r"(xs) : "memory"); if (rc != 0) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; @@ -586,6 +620,8 @@ ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, /* Zero sensitive key material from stack */ memset(ws, 0, sizeof(ws)); + /* Use a memory barrier to prevent compiler from optimizing out the memset */ + __asm__ volatile("" : : "r"(ws) : "memory"); if (rc != 0) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); @@ -621,6 +657,9 @@ ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x, /* reuse xs (key) as buffer for result */ rc = ble_sm_alg_aes_cmac(xs, m, sizeof(m), xs); if (rc != 0) { + memset(xs, 0, sizeof(xs)); + /* Prevent compiler from optimizing away the sensitive-data clear */ + __asm__ volatile("" : : "r"(xs) : "memory"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; } @@ -628,6 +667,9 @@ ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x, ble_sm_alg_log_buf("res", xs, 16); *passkey = get_be32(xs + 12) % 1000000; + memset(xs, 0, sizeof(xs)); + /* Prevent compiler from optimizing away the sensitive-data clear */ + __asm__ volatile("" : : "r"(xs) : "memory"); BLE_HS_LOG(DEBUG, " passkey=%u\n", *passkey); return 0; @@ -751,6 +793,9 @@ exit: if (rc != 0) { #if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (keypair_ptr) { +#ifndef CONFIG_MBEDTLS_VER_4_X_SUPPORT + mbedtls_ecp_keypair_free(keypair_ptr); +#endif nimble_platform_mem_free(keypair_ptr); keypair_ptr = NULL; } @@ -910,7 +955,16 @@ exit: void mbedtls_free_keypair(void) { #ifndef CONFIG_MBEDTLS_VER_4_X_SUPPORT +#if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (keypair_ptr == NULL) { + return; + } + mbedtls_ecp_keypair_free(keypair_ptr); + nimble_platform_mem_free(keypair_ptr); + keypair_ptr = NULL; +#else mbedtls_ecp_keypair_free(&keypair); +#endif #endif // CONFIG_MBEDTLS_VER_4_X_SUPPORT } #endif diff --git a/nimble/host/src/ble_sm_lgcy.c b/nimble/host/src/ble_sm_lgcy.c index 8dc4b9620..3fa11bb26 100644 --- a/nimble/host/src/ble_sm_lgcy.c +++ b/nimble/host/src/ble_sm_lgcy.c @@ -193,6 +193,7 @@ ble_sm_gen_stk(struct ble_sm_proc *proc) /* Zero sensitive key material from stack */ memset(key, 0, sizeof(key)); + __asm__ volatile("" : : "r"(key) : "memory"); return 0; } diff --git a/nimble/host/src/ble_sm_sc.c b/nimble/host/src/ble_sm_sc.c index b03725035..904a6d842 100644 --- a/nimble/host/src/ble_sm_sc.c +++ b/nimble/host/src/ble_sm_sc.c @@ -29,6 +29,10 @@ #if NIMBLE_BLE_CONNECT #if MYNEWT_VAL(BLE_SM_SC) +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) +void mbedtls_free_keypair(void); +#endif + #define BLE_SM_SC_PASSKEY_BYTES 4 #define BLE_SM_SC_PASSKEY_BITS 20 @@ -158,20 +162,16 @@ ble_sm_sc_io_action(struct ble_sm_proc *proc, uint8_t *action) pair_req = (struct ble_sm_pair_cmd *) &proc->pair_req[1]; pair_rsp = (struct ble_sm_pair_cmd *) &proc->pair_rsp[1]; + if (pair_req->oob_data_flag == BLE_SM_PAIR_OOB_YES || + pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES) { + *action = BLE_SM_IOACT_OOB_SC; #if MYNEWT_VAL(STATIC_PASSKEY) - /* Check if static passkey is enabled - if so, use static passkey action */ - if (ble_hs_cfg.sm_static_passkey) - { + } else if (ble_hs_cfg.sm_static_passkey) { *action = BLE_SM_IOACT_STATIC; proc->pair_alg = BLE_SM_PAIR_ALG_PASSKEY; proc->flags |= BLE_SM_PROC_F_AUTHENTICATED; return 0; - } #endif - - if (pair_req->oob_data_flag == BLE_SM_PAIR_OOB_YES || - pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES) { - *action = BLE_SM_IOACT_OOB_SC; } else if (!(pair_req->authreq & BLE_SM_PAIR_AUTHREQ_MITM) && !(pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_MITM)) { @@ -780,7 +780,7 @@ static void ble_sm_sc_dhkey_addrs(struct ble_sm_proc *proc, ble_addr_t *our_addr, ble_addr_t *peer_addr) { - struct ble_hs_conn_addrs addrs; + struct ble_hs_conn_addrs addrs = {0}; struct ble_hs_conn *conn; BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); @@ -1009,25 +1009,28 @@ ble_sm_sc_oob_data_check(struct ble_sm_proc *proc, int ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data) { + uint8_t pub_key[64]; int rc; + ble_hs_lock(); + rc = ble_sm_sc_ensure_keys_generated(); - if (rc) { + if (rc == 0) { + memcpy(pub_key, ble_sm_sc_pub_key, sizeof(pub_key)); + } + + ble_hs_unlock(); + + if (rc != 0) { return rc; } rc = ble_hs_hci_util_rand(oob_data->r, 16); - if (rc) { + if (rc != 0) { return rc; } - rc = ble_sm_alg_f4(ble_sm_sc_pub_key, ble_sm_sc_pub_key, oob_data->r, 0, - oob_data->c); - if (rc) { - return rc; - } - - return 0; + return ble_sm_alg_f4(pub_key, pub_key, oob_data->r, 0, oob_data->c); } void @@ -1042,9 +1045,13 @@ void ble_sm_sc_deinit(void) { if (ble_sm_sc_ctx) { +#if MYNEWT_VAL(BLE_CRYPTO_STACK_MBEDTLS) + mbedtls_free_keypair(); +#endif nimble_platform_mem_free(ble_sm_sc_ctx); ble_sm_sc_ctx = NULL; } + ble_sm_sc_keys_generated = 0; } #endif diff --git a/nimble/host/src/ble_store.c b/nimble/host/src/ble_store.c index bd1ea2b3d..734e9980d 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -27,7 +27,7 @@ int ble_store_read(int obj_type, const union ble_store_key *key, union ble_store_value *val) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT int rc; @@ -50,7 +50,7 @@ ble_store_read(int obj_type, const union ble_store_key *key, int ble_store_write(int obj_type, const union ble_store_value *val) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT int rc; while (1) { @@ -91,7 +91,7 @@ ble_store_write(int obj_type, const union ble_store_value *val) int ble_store_delete(int obj_type, const union ble_store_key *key) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT int rc; @@ -111,7 +111,7 @@ ble_store_delete(int obj_type, const union ble_store_key *key) #endif } -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT static int ble_store_status(struct ble_store_status_event *event) { @@ -132,9 +132,9 @@ ble_store_status(struct ble_store_status_event *event) int ble_store_overflow_event(int obj_type, const union ble_store_value *value) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT - struct ble_store_status_event event; + struct ble_store_status_event event = {0}; event.event_code = BLE_STORE_EVENT_OVERFLOW; event.overflow.obj_type = obj_type; @@ -149,9 +149,9 @@ ble_store_overflow_event(int obj_type, const union ble_store_value *value) int ble_store_full_event(int obj_type, uint16_t conn_handle) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT - struct ble_store_status_event event; + struct ble_store_status_event event = {0}; event.event_code = BLE_STORE_EVENT_FULL; event.full.obj_type = obj_type; @@ -228,11 +228,11 @@ ble_store_delete_our_sec(const struct ble_store_key_sec *key_sec) { #if NIMBLE_BLE_CONNECT - union ble_store_key *store_key; + union ble_store_key store_key = {0}; int rc; - store_key = (void *)key_sec; - rc = ble_store_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, store_key); + store_key.sec = *key_sec; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, &store_key); return rc; #else return BLE_HS_ENOTSUP; @@ -258,16 +258,16 @@ int ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec, struct ble_store_value_sec *value_sec) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT union ble_store_value *store_value; - union ble_store_key *store_key; + const union ble_store_key *store_key; int rc; BLE_HS_DBG_ASSERT(key_sec->peer_addr.type == BLE_ADDR_PUBLIC || key_sec->peer_addr.type == BLE_ADDR_RANDOM); - store_key = (void *)key_sec; + store_key = (const union ble_store_key *)key_sec; store_value = (void *)value_sec; rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_SEC, store_key, store_value); @@ -280,7 +280,7 @@ ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec, int ble_store_write_peer_sec(const struct ble_store_value_sec *value_sec) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT int rc; #if MYNEWT_VAL(BLE_DEFER_CONN_EVENTS) && MYNEWT_VAL(BLE_HS_PVCY) @@ -390,7 +390,7 @@ ble_store_read_cccd(const struct ble_store_key_cccd *key, int ble_store_write_cccd(const struct ble_store_value_cccd *value) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT union ble_store_value *store_value; int rc; @@ -406,7 +406,7 @@ ble_store_write_cccd(const struct ble_store_value_cccd *value) int ble_store_delete_cccd(const struct ble_store_key_cccd *key) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT union ble_store_key *store_key; int rc; @@ -443,11 +443,11 @@ ble_store_write_csfc(const struct ble_store_value_csfc *value) { #if NIMBLE_BLE_CONNECT - union ble_store_value *store_value; + union ble_store_value store_value = {0}; int rc; - store_value = (void *)value; - rc = ble_store_write(BLE_STORE_OBJ_TYPE_CSFC, store_value); + store_value.csfc = *value; + rc = ble_store_write(BLE_STORE_OBJ_TYPE_CSFC, &store_value); return rc; #else return BLE_HS_ENOTSUP; @@ -474,7 +474,7 @@ void ble_store_key_from_value_cccd(struct ble_store_key_cccd *out_key, const struct ble_store_value_cccd *value) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT out_key->peer_addr = value->peer_addr; out_key->chr_val_handle = value->chr_val_handle; @@ -503,10 +503,10 @@ ble_store_read_ead(const struct ble_store_key_ead *key, #if NIMBLE_BLE_CONNECT union ble_store_value *store_value; - union ble_store_key *store_key; + const union ble_store_key *store_key; int rc; - store_key = (void *)key; + store_key = (const union ble_store_key *)key; store_value = (void *)out_value; rc = ble_store_read(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, store_key, store_value); return rc; @@ -536,11 +536,11 @@ ble_store_delete_ead(const struct ble_store_key_ead *key) { #if NIMBLE_BLE_CONNECT - union ble_store_key *store_key; + union ble_store_key store_key = {0}; int rc; - store_key = (void *)key; - rc = ble_store_delete(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, store_key); + store_key.ead = *key; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, &store_key); return rc; #else return BLE_HS_ENOTSUP; @@ -563,7 +563,7 @@ int ble_store_read_local_irk(const struct ble_store_key_local_irk *key, struct ble_store_value_local_irk *out_value) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT union ble_store_value *store_value; union ble_store_key *store_key; @@ -583,13 +583,13 @@ ble_store_read_local_irk(const struct ble_store_key_local_irk *key, int ble_store_write_local_irk(const struct ble_store_value_local_irk *value) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT - union ble_store_value *store_value; + union ble_store_value store_value = {0}; int rc; - store_value = (void *)value; - rc = ble_store_write(BLE_STORE_OBJ_TYPE_LOCAL_IRK, store_value); + store_value.local_irk = *value; + rc = ble_store_write(BLE_STORE_OBJ_TYPE_LOCAL_IRK, &store_value); return rc; #else return BLE_HS_ENOTSUP; @@ -602,11 +602,11 @@ ble_store_delete_local_irk(const struct ble_store_key_local_irk *key) { #if NIMBLE_BLE_CONNECT - union ble_store_key *store_key; + union ble_store_key store_key = {0}; int rc; - store_key = (void *)key; - rc = ble_store_delete(BLE_STORE_OBJ_TYPE_LOCAL_IRK, store_key); + store_key.local_irk = *key; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_LOCAL_IRK, &store_key); return rc; #else return BLE_HS_ENOTSUP; @@ -664,11 +664,11 @@ ble_store_delete_rpa_rec(const struct ble_store_key_rpa_rec *key) { #if NIMBLE_BLE_CONNECT - union ble_store_key *store_key; + union ble_store_key store_key = {0}; int rc; - store_key = (void *)key; - rc = ble_store_delete(BLE_STORE_OBJ_TYPE_PEER_ADDR, store_key); + store_key.rpa_rec = *key; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_PEER_ADDR, &store_key); return rc; #else return BLE_HS_ENOTSUP; @@ -743,7 +743,7 @@ ble_store_iterate(int obj_type, ble_store_iterator_fn *callback, void *cookie) { -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT union ble_store_key key; union ble_store_value value; diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c index 755b78ecd..b754a7b1e 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -385,11 +385,12 @@ ble_store_util_ead_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, return rc; } - if (set.status != 0 && set.status != BLE_HS_ENOMEM) { + *out_num_peers = set.num_peers; + + if (set.status != 0) { return set.status; } - *out_num_peers = set.num_peers; return 0; #else return BLE_HS_ENOTSUP; @@ -400,7 +401,7 @@ ble_store_util_ead_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, int ble_store_util_delete_ead_oldest_peer(void) { -#if NIMBLE_BLE_CONNECT +#if NIMBLE_BLE_CONNECT && (MYNEWT_VAL(BLE_STORE_MAX_EADS) > 0) ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; int num_peers; diff --git a/nimble/host/src/ble_uuid.c b/nimble/host/src/ble_uuid.c index f15fbdff8..184cc76ed 100644 --- a/nimble/host/src/ble_uuid.c +++ b/nimble/host/src/ble_uuid.c @@ -133,7 +133,13 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2) switch (uuid1->type) { case BLE_UUID_TYPE_16: - return (int) BLE_UUID16(uuid1)->value - (int) BLE_UUID16(uuid2)->value; + if (BLE_UUID16(uuid1)->value < BLE_UUID16(uuid2)->value) { + return -1; + } + if (BLE_UUID16(uuid1)->value > BLE_UUID16(uuid2)->value) { + return 1; + } + return 0; case BLE_UUID_TYPE_32: if (BLE_UUID32(uuid1)->value < BLE_UUID32(uuid2)->value) { return -1; @@ -143,7 +149,7 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2) } return 0; case BLE_UUID_TYPE_128: - return memcmp(&BLE_UUID128(uuid1)->value, &BLE_UUID128(uuid2)->value, 16); + return memcmp(BLE_UUID128(uuid1)->value, BLE_UUID128(uuid2)->value, 16); } BLE_HS_DBG_ASSERT(0); @@ -154,6 +160,9 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2) void ble_uuid_copy(ble_uuid_any_t *dst, const ble_uuid_t *src) { + if (src == NULL || dst == NULL) { + return; + } VERIFY_UUID(src); switch (src->type) { @@ -212,16 +221,15 @@ ble_uuid_base_init(void) BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); return BLE_HS_ENOMEM; } + ble_uuid_base[0] = 0xfb; + ble_uuid_base[1] = 0x34; + ble_uuid_base[2] = 0x9b; + ble_uuid_base[3] = 0x5f; + ble_uuid_base[4] = 0x80; + ble_uuid_base[7] = 0x80; + ble_uuid_base[9] = 0x10; } - ble_uuid_base[0] = 0xfb; - ble_uuid_base[1] = 0x34; - ble_uuid_base[2] = 0x9b; - ble_uuid_base[3] = 0x5f; - ble_uuid_base[4] = 0x80; - ble_uuid_base[7] = 0x80; - ble_uuid_base[9] = 0x10; - return 0; } #endif @@ -249,13 +257,21 @@ ble_uuid_from_str(ble_uuid_any_t *uuid, const char *str) return BLE_HS_EINVAL; } + /* For 16-bit (6 chars: 0xXXXX) and 32-bit (10 chars: 0xXXXXXXXX) strings, + * require the "0x" prefix */ + if ((len == 6 || len == 10) && + !(str[0] == '0' && (str[1] == 'x' || str[1] == 'X'))) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } + str_ptr = &str[len - 2]; if (len <= BLE_UUID16_STR_MAX_LEN) { uuid->u.type = BLE_UUID_TYPE_16; } else if (len <= BLE_UUID32_STR_MAX_LEN) { uuid->u.type = BLE_UUID_TYPE_32; - } else if (len <= BLE_UUID128_STR_MAX_LEN) { + } else if (len == BLE_UUID128_STR_MAX_LEN || len == 32) { uuid->u.type = BLE_UUID_TYPE_128; } else { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); @@ -422,6 +438,7 @@ int ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om) { int len; + int rc; void *buf; VERIFY_UUID(uuid); @@ -439,7 +456,11 @@ ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om) return BLE_HS_ENOMEM; } - ble_uuid_flat(uuid, buf); + rc = ble_uuid_flat(uuid, buf); + if (rc != 0) { + os_mbuf_adj(om, -len); + return rc; + } return 0; } diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index a22f3ac90..5b3290c55 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -18,6 +18,7 @@ */ #include +#include #include #include "sysinit/sysinit.h" @@ -115,7 +116,7 @@ int ble_restore_our_sec_nvs(void) err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, &key); - if (err != ESP_OK) { + if (err != 0) { BLE_HS_LOG(DEBUG, "Error deleting from nvs"); return err; } @@ -128,7 +129,7 @@ int ble_restore_our_sec_nvs(void) err = ble_store_config_write(BLE_STORE_OBJ_TYPE_OUR_SEC, &val); - if (err != ESP_OK) { + if (err != 0) { BLE_HS_LOG(DEBUG, "Error writing to nvs"); return err; } @@ -164,7 +165,7 @@ int ble_restore_peer_sec_nvs(void) err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_PEER_SEC, &key); - if (err != ESP_OK) { + if (err != 0) { BLE_HS_LOG(DEBUG, "Error deleting from nvs %d ",err); return err; } @@ -176,7 +177,7 @@ int ble_restore_peer_sec_nvs(void) err = ble_store_config_write(BLE_STORE_OBJ_TYPE_PEER_SEC, &val); - if (err != ESP_OK) { + if (err != 0) { BLE_HS_LOG(DEBUG, "Error writing to nvs %d ",err); return err; } @@ -640,6 +641,7 @@ ble_store_config_find_ead(const struct ble_store_key_ead *key) static int ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead) { + struct ble_store_value_ead backup; int idx; int rc; @@ -648,6 +650,8 @@ ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead) return BLE_HS_ENOENT; } + backup = ble_store_config_eads[idx]; + rc = ble_store_config_delete_obj(ble_store_config_eads, sizeof *ble_store_config_eads, idx, @@ -658,6 +662,12 @@ ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead) rc = ble_store_config_persist_eads(); if (rc != 0) { + /* Restore the deleted entry to keep RAM consistent with storage */ + memmove(&ble_store_config_eads[idx + 1], + &ble_store_config_eads[idx], + (ble_store_config_num_eads - idx) * sizeof(*ble_store_config_eads)); + ble_store_config_eads[idx] = backup; + ble_store_config_num_eads++; return rc; } @@ -685,11 +695,13 @@ ble_store_config_write_ead(const struct ble_store_value_ead *value_ead) struct ble_store_key_ead key_ead; int idx; int rc; + bool is_new; ble_store_key_from_value_ead(&key_ead, value_ead); idx = ble_store_config_find_ead(&key_ead); + is_new = (idx == -1); - if (idx == -1) { + if (is_new) { if (ble_store_config_num_eads >= MYNEWT_VAL(BLE_STORE_MAX_EADS)) { BLE_HS_LOG(DEBUG, "error persisting ead; too many entries (%d)\n", ble_store_config_num_eads); @@ -705,6 +717,9 @@ ble_store_config_write_ead(const struct ble_store_value_ead *value_ead) rc = ble_store_config_persist_eads(); if (rc != 0) { + if (is_new) { + ble_store_config_num_eads--; + } return rc; } @@ -852,9 +867,11 @@ ble_store_config_find_rpa_rec(const struct ble_store_key_rpa_rec *key) for(i = 0; i < ble_store_config_num_rpa_recs; i++){ rpa_rec = ble_store_config_rpa_recs + i; - if (ble_addr_cmp(&rpa_rec->peer_rpa_addr, &key->peer_rpa_addr) && - ble_addr_cmp(&rpa_rec->peer_addr, &key->peer_rpa_addr)) { - continue; + if (ble_addr_cmp(&key->peer_rpa_addr, BLE_ADDR_ANY)) { + if (ble_addr_cmp(&rpa_rec->peer_rpa_addr, &key->peer_rpa_addr) && + ble_addr_cmp(&rpa_rec->peer_addr, &key->peer_rpa_addr)) { + continue; + } } if (key->idx > skipped) { skipped++; @@ -961,9 +978,14 @@ ble_store_config_find_csfc(const struct ble_store_key_csfc *key, /* If peer_addr is specified, search by peer_addr (common for write/read/delete) */ if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { + int skipped = 0; for (i = 0; i < num_value_csfc; i++) { cur = &value_csfc[i]; if (!ble_addr_cmp(&cur->peer_addr, &key->peer_addr)) { + if (skipped < key->idx) { + skipped++; + continue; + } return i; } } @@ -1258,5 +1280,8 @@ ble_store_config_deinit(void) nimble_platform_mem_free(ble_store_config_vars); ble_store_config_vars = NULL; } + ble_hs_cfg.store_read_cb = NULL; + ble_hs_cfg.store_write_cb = NULL; + ble_hs_cfg.store_delete_cb = NULL; } #endif 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 3b0e7198f..3fa2b6d0c 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/nimble/host/store/config/src/ble_store_config_conf.c @@ -88,7 +88,7 @@ ble_store_config_serialize_arr(const void *arr, int obj_sz, int num_objs, int arr_size; arr_size = obj_sz * num_objs; - assert(arr_size <= buf_sz); + assert(BASE64_ENCODE_SIZE(arr_size) <= buf_sz); base64_encode(arr, arr_size, out_buf, 1); } diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index 44304c3f4..3685de2e7 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -159,7 +159,10 @@ get_nvs_peer_record(char *key_string, struct ble_hs_dev_records *p_dev_rec) size_t required_size = 0; nvs_handle_t nimble_handle; - err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READWRITE, &nimble_handle); + err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READONLY, &nimble_handle); + if (err == ESP_ERR_NVS_NOT_FOUND) { + return ESP_ERR_NVS_NOT_FOUND; + } if (err != ESP_OK) { ESP_LOGE(TAG, "NVS open operation failed"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL); @@ -221,7 +224,10 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) size_t expected_size; nvs_handle_t nimble_handle; - err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READWRITE, &nimble_handle); + err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READONLY, &nimble_handle); + if (err == ESP_ERR_NVS_NOT_FOUND) { + return ESP_ERR_NVS_NOT_FOUND; + } if (err != ESP_OK) { ESP_LOGE(TAG, "NVS open operation failed"); BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL); @@ -450,6 +456,10 @@ ble_nvs_write_key_value(char *key, const void *value, size_t required_size) return 0; error: nvs_close(nimble_handle); + if (err == ESP_ERR_NVS_NOT_ENOUGH_SPACE || err == ESP_ERR_NVS_NO_FREE_PAGES) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_CAP); + return BLE_HS_ESTORE_CAP; + } BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL); return BLE_HS_ESTORE_FAIL; } @@ -1063,17 +1073,22 @@ int ble_store_persist_peer_records(void) peer_rec = peer_dev_rec[ble_store_num_peer_dev_rec - 1]; return ble_store_nvs_peer_records(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, &peer_rec); } else if (nvs_count > ble_store_num_peer_dev_rec) { - /* NVS db count more than RAM count, delete operation */ - nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0, - peer_dev_rec, - ble_store_num_peer_dev_rec); - if (nvs_idx == -1) { - ESP_LOGE(TAG, "NVS delete operation failed for peer records"); - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL); - return BLE_HS_ESTORE_FAIL; + /* NVS db count more than RAM count, delete all non-matching records */ + while (nvs_count > ble_store_num_peer_dev_rec) { + nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0, + peer_dev_rec, + ble_store_num_peer_dev_rec); + if (nvs_idx == -1) { + ESP_LOGE(TAG, "NVS delete operation failed for peer records"); + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ESTORE_FAIL); + return BLE_HS_ESTORE_FAIL; + } + ESP_LOGD(TAG, "Deleting peer record, nvs idx = %d", nvs_idx); + if (ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, nvs_idx) != 0) { + return BLE_HS_ESTORE_FAIL; + } + nvs_count--; } - ESP_LOGD(TAG, "Deleting peer record, nvs idx = %d", nvs_idx); - return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, nvs_idx); } return 0; } diff --git a/nimble/host/store/ram/src/ble_store_ram.c b/nimble/host/store/ram/src/ble_store_ram.c index 500292d92..ce2050e71 100644 --- a/nimble/host/store/ram/src/ble_store_ram.c +++ b/nimble/host/store/ram/src/ble_store_ram.c @@ -80,7 +80,7 @@ static void ble_store_ram_print_value_sec(const struct ble_store_value_sec *sec) { if (sec->ltk_present) { - BLE_HS_LOG(DEBUG, "ediv=%u rand=%llu authenticated=%d ltk=", + BLE_HS_LOG(DEBUG, "ediv=%u rand=%" PRIu64 " authenticated=%d ltk=", sec->ediv, sec->rand_num, sec->authenticated); ble_hs_log_flat_buf(sec->ltk, 16); BLE_HS_LOG(DEBUG, " "); @@ -124,12 +124,18 @@ ble_store_ram_find_sec(const struct ble_store_key_sec *key_sec, if (key_sec->idx < num_value_secs) { return key_sec->idx; } - } else if (key_sec->idx == 0) { + } else { + int skipped; + + skipped = 0; for (i = 0; i < num_value_secs; i++) { cur = &value_secs[i]; if (!ble_addr_cmp(&cur->peer_addr, &key_sec->peer_addr)) { - return i; + if (skipped == key_sec->idx) { + return i; + } + skipped++; } } } @@ -166,6 +172,10 @@ ble_store_ram_write_our_sec(const struct ble_store_value_sec *value_sec) struct ble_store_key_sec key_sec; int idx; + if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) == 0) { + return BLE_HS_EINVAL; + } + BLE_HS_LOG(DEBUG, "persisting our sec; "); ble_store_ram_print_value_sec(value_sec); @@ -213,6 +223,10 @@ ble_store_ram_delete_obj(void *values, int value_size, int idx, memmove(dst, src, (size_t)move_count * value_size); } + /* Zero the now-unused slot at the end to prevent stale key data in RAM */ + dst = (uint8_t *)values + (*num_values) * value_size; + memset(dst, 0, value_size); + return 0; } #endif @@ -305,6 +319,10 @@ ble_store_ram_write_peer_sec(const struct ble_store_value_sec *value_sec) struct ble_store_key_sec key_sec; int idx; + if (ble_addr_cmp(&value_sec->peer_addr, BLE_ADDR_ANY) == 0) { + return BLE_HS_EINVAL; + } + BLE_HS_LOG(DEBUG, "persisting peer sec; "); ble_store_ram_print_value_sec(value_sec); @@ -529,7 +547,7 @@ static int ble_store_ram_write_csfc(const struct ble_store_value_csfc *value_csfc) { #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) - struct ble_store_key_csfc key_csfc; + struct ble_store_key_csfc key_csfc = {0}; int idx; ble_store_key_from_value_csfc(&key_csfc, value_csfc); @@ -624,7 +642,7 @@ ble_store_ram_read_ead(const struct ble_store_key_ead *key_ead, static int ble_store_ram_write_ead(const struct ble_store_value_ead *value_ead) { - struct ble_store_key_ead key_ead; + struct ble_store_key_ead key_ead = {0}; int idx; ble_store_key_from_value_ead(&key_ead, value_ead); diff --git a/nimble/host/test/src/ble_hs_hci_test.c b/nimble/host/test/src/ble_hs_hci_test.c index 8dd7c7777..d888f5994 100644 --- a/nimble/host/test/src/ble_hs_hci_test.c +++ b/nimble/host/test/src/ble_hs_hci_test.c @@ -105,7 +105,9 @@ TEST_CASE_SELF(ble_hs_hci_acl_one_conn) ble_hs_test_util_init(); /* The controller has room for five 20-byte payloads. */ + ble_hs_lock(); rc = ble_hs_hci_set_buf_sz(20, 5); + ble_hs_unlock(); TEST_ASSERT_FATAL(rc == 0); TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5); @@ -190,7 +192,9 @@ TEST_CASE_SELF(ble_hs_hci_acl_two_conn) ble_hs_test_util_init(); /* The controller has room for five 20-byte payloads*/ + ble_hs_lock(); rc = ble_hs_hci_set_buf_sz(20, 5); + ble_hs_unlock(); TEST_ASSERT_FATAL(rc == 0); TEST_ASSERT_FATAL(ble_hs_hci_avail_pkts == 5); diff --git a/nimble/host/test/src/ble_hs_pvcy_test.c b/nimble/host/test/src/ble_hs_pvcy_test.c index 2c6650714..fb67b183e 100644 --- a/nimble/host/test/src/ble_hs_pvcy_test.c +++ b/nimble/host/test/src/ble_hs_pvcy_test.c @@ -236,7 +236,7 @@ ble_hs_pvcy_test_util_add_arbitrary_irk(bool scanning, bool connecting) ble_hs_pvcy_test_util_add_irk( &peer_addr, (uint8_t[16]){1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}, - ble_hs_pvcy_default_irk, + ble_hs_pvcy_get_default_irk(), scanning, connecting); } @@ -255,7 +255,7 @@ ble_hs_pvcy_test_util_restore_irk(const struct ble_store_value_sec *value_sec, ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec->peer_addr, value_sec->irk, - ble_hs_pvcy_default_irk, + ble_hs_pvcy_get_default_irk(), scanning, connecting); } @@ -287,7 +287,7 @@ TEST_CASE_SELF(ble_hs_pvcy_test_case_restore_irks) ble_hs_pvcy_test_util_start_host(1); ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec1.peer_addr, value_sec1.irk, - ble_hs_pvcy_default_irk, + ble_hs_pvcy_get_default_irk(), false, false); /* Two persisted IRKs. */ @@ -305,11 +305,11 @@ TEST_CASE_SELF(ble_hs_pvcy_test_case_restore_irks) ble_hs_pvcy_test_util_start_host(2); ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec1.peer_addr, value_sec1.irk, - ble_hs_pvcy_default_irk, + ble_hs_pvcy_get_default_irk(), false, false); ble_hs_pvcy_test_util_add_irk_verify_tx(&value_sec2.peer_addr, value_sec2.irk, - ble_hs_pvcy_default_irk, + ble_hs_pvcy_get_default_irk(), false, false); ble_hs_test_util_assert_mbufs_freed(NULL); diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 81142a88a..67c7acd19 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -1559,8 +1559,8 @@ struct ble_hci_le_cs_test_cp { struct ble_hci_le_add_monitor_adv_list_cp { uint8_t addr_type; uint8_t address[6]; - uint8_t rssi_low_threshold; - uint8_t rssi_high_threshold; + int8_t rssi_low_threshold; + int8_t rssi_high_threshold; uint8_t timeout; } __attribute__((packed)); diff --git a/nimble/transport/cdc/src/cdc_hci.c b/nimble/transport/cdc/src/cdc_hci.c index 08c8f984a..2ffa4feee 100644 --- a/nimble/transport/cdc/src/cdc_hci.c +++ b/nimble/transport/cdc/src/cdc_hci.c @@ -306,7 +306,9 @@ cdc_hci_acl_packet_write(struct usb_in_packet *packet, size_t offset) for (;;) { mb = os_mbuf_off(om, (int)new_offset - 1, &mbuf_offset); - assert(mb); + if (mb == NULL || new_offset < 1) { + break; + } /* mbuf_offset is == om_len when new_offset reached end of mbuf data */ if (mb->om_len == mbuf_offset) { break; diff --git a/nimble/transport/src/transport.c b/nimble/transport/src/transport.c index 1f7fcb2e4..56dfc2308 100644 --- a/nimble/transport/src/transport.c +++ b/nimble/transport/src/transport.c @@ -419,6 +419,14 @@ ble_transport_acl_put(struct os_mempool_ext *mpe, void *data, void *arg) struct os_mbuf *om; struct os_mbuf_pkthdr *pkthdr; bool from_ll; + uint16_t omp_flags; +#endif + +#if MYNEWT_VAL(BLE_TRANSPORT_INT_FLOW_CTL) + om = data; + pkthdr = OS_MBUF_PKTHDR(om); + omp_flags = pkthdr->omp_flags; + from_ll = (omp_flags & OMP_FLAG_FROM_MASK) == OMP_FLAG_FROM_LL; #endif #if MYNEWT_VAL(BLE_HS_FLOW_CTRL) @@ -430,10 +438,6 @@ ble_transport_acl_put(struct os_mempool_ext *mpe, void *data, void *arg) #endif #if MYNEWT_VAL(BLE_TRANSPORT_INT_FLOW_CTL) - om = data; - pkthdr = OS_MBUF_PKTHDR(om); - - from_ll = (pkthdr->omp_flags & OMP_FLAG_FROM_MASK) == OMP_FLAG_FROM_LL; if (from_ll && !err) { hci_ipc_put(HCI_IPC_TYPE_ACL); diff --git a/porting/nimble/src/hal_timer.c b/porting/nimble/src/hal_timer.c index 0b6183b7c..76a17afa7 100644 --- a/porting/nimble/src/hal_timer.c +++ b/porting/nimble/src/hal_timer.c @@ -190,6 +190,9 @@ nrf_timer_set_ocmp(struct nrf52_hal_timer *bsptimer, uint32_t expiry) /* CC too far ahead. Just make sure we set compare far ahead */ rtctimer->CC[NRF_RTC_TIMER_CC_INT] = cntr + (1UL << 23); } + /* Clear any stale TICK and COMPARE events before enabling interrupt */ + rtctimer->EVENTS_TICK = 0; + rtctimer->EVENTS_COMPARE[NRF_RTC_TIMER_CC_INT] = 0; rtctimer->INTENSET = NRF_TIMER_INT_MASK(NRF_RTC_TIMER_CC_INT); } } else { @@ -535,6 +538,7 @@ hal_timer_init(int timer_num, void *cfg) bsptimer->tmr_reg = hwtimer; bsptimer->tmr_irq_num = irq_num; + TAILQ_INIT(&bsptimer->hal_timer_q); /* Disable IRQ, set priority and set vector in table */ NVIC_DisableIRQ(irq_num); @@ -591,6 +595,7 @@ hal_timer_config(int timer_num, uint32_t freq_hz) bsptimer->tmr_freq = freq_hz; bsptimer->tmr_enabled = 1; + bsptimer->tmr_cntr = 0; OS_ENTER_CRITICAL(sr); @@ -619,6 +624,11 @@ hal_timer_config(int timer_num, uint32_t freq_hz) #endif /* Set timer to desired frequency */ + if (freq_hz == 0) { + rc = EINVAL; + goto err; + } + div = NRF52_MAX_TIMER_FREQ / freq_hz; /* @@ -707,6 +717,12 @@ hal_timer_deinit(int timer_num) NRF52_HAL_TIMER_RESOLVE(timer_num, bsptimer); OS_ENTER_CRITICAL(sr); + if (bsptimer->tmr_reg == NULL) { + /* Already deinitialized */ + OS_EXIT_CRITICAL(sr); + rc = EINVAL; + goto err; + } if (bsptimer->tmr_rtc) { rtctimer = (NRF_RTC_Type *)bsptimer->tmr_reg; rtctimer->INTENCLR = NRF_TIMER_INT_MASK(NRF_RTC_TIMER_CC_INT); @@ -748,6 +764,9 @@ hal_timer_get_resolution(int timer_num) NRF52_HAL_TIMER_RESOLVE(timer_num, bsptimer); + if (bsptimer->tmr_freq == 0) { + return 0; + } resolution = 1000000000 / bsptimer->tmr_freq; return resolution; @@ -781,11 +800,12 @@ hal_timer_read(int timer_num) return tcntr; - /* Assert here since there is no invalid return code */ + /* Abort on invalid timer_num since there is no valid return code. + * rc is set by NRF52_HAL_TIMER_RESOLVE but not used after abort(). */ err: - assert(0); - rc = 0; - return rc; + (void)rc; + abort(); + return 0; } /** @@ -803,6 +823,10 @@ hal_timer_delay(int timer_num, uint32_t ticks) { uint32_t until; + if (ticks == 0) { + return 0; + } + until = hal_timer_read(timer_num) + ticks; while ((int32_t)(hal_timer_read(timer_num) - until) <= 0) { /* Loop here till finished */ @@ -847,6 +871,10 @@ hal_timer_start(struct hal_timer *timer, uint32_t ticks) uint32_t tick; struct nrf52_hal_timer *bsptimer; + if (timer == NULL) { + return EINVAL; + } + /* Set the tick value at which the timer should expire */ bsptimer = (struct nrf52_hal_timer *)timer->bsp_timer; if (bsptimer->tmr_rtc) { diff --git a/porting/nimble/src/modlog.c b/porting/nimble/src/modlog.c index c634fa4c2..95cd7066b 100644 --- a/porting/nimble/src/modlog.c +++ b/porting/nimble/src/modlog.c @@ -9,9 +9,14 @@ void MODLOG_INFO(int mod, char * msg, ...) { memset(buffer, 0, 1000); va_start(args, msg); - len = sprintf(buffer, args); + len = vsnprintf(buffer, sizeof(buffer), msg, args); va_end(args); + if (len < 0) { + len = 0; + } else if (len >= (int)sizeof(buffer)) { + len = (int)sizeof(buffer) - 1; + } ble_log_write_hex(BLE_LOG_SRC_HOST, (uint8_t *)buffer, len); ble_log_flush(); } diff --git a/porting/nimble/src/nimble_port.c b/porting/nimble/src/nimble_port.c index 376ec1f9f..556c96e00 100644 --- a/porting/nimble/src/nimble_port.c +++ b/porting/nimble/src/nimble_port.c @@ -94,6 +94,8 @@ static struct ble_hs_stop_listener stop_listener; #endif +static bool nimble_port_run_active; + extern void os_msys_init(void); extern void os_mempool_module_init(void); #if MYNEWT_VAL(MP_RUNTIME_ALLOC) @@ -167,7 +169,17 @@ esp_err_t esp_nimble_init(void) /* Initialize the function pointers for OS porting */ npl_freertos_funcs_init(); - npl_freertos_mempool_init(); + if (npl_freertos_mempool_init() != 0) { + ESP_LOGE(NIMBLE_PORT_LOG_TAG, "npl mempool init failed\n"); + npl_freertos_funcs_deinit(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_npl_ctx) { + nimble_platform_mem_free(ble_npl_ctx); + ble_npl_ctx = NULL; + } +#endif + return ESP_FAIL; + } #if CONFIG_BT_CONTROLLER_ENABLED if(esp_nimble_hci_init() != ESP_OK) { @@ -219,6 +231,9 @@ esp_err_t esp_nimble_init(void) esp_err_t esp_nimble_deinit(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_npl_ctx == NULL) { + return ESP_OK; + } ble_npl_deiniting = true; #endif @@ -248,10 +263,10 @@ esp_err_t esp_nimble_deinit(void) #endif - ble_npl_eventq_deinit(&g_eventq_dflt); - ble_hs_deinit(); + ble_npl_eventq_deinit(&g_eventq_dflt); + #if !SOC_ESP_NIMBLE_CONTROLLER || !CONFIG_BT_CONTROLLER_ENABLED npl_freertos_funcs_deinit(); #endif @@ -392,13 +407,19 @@ nimble_port_stop(void) err = ble_hs_stop(&stop_listener, ble_hs_stop_cb, NULL); if (err != 0) { - ble_npl_sem_deinit(&ble_hs_stop_sem); - return err; + if (err != BLE_HS_EALREADY) { + ble_npl_sem_deinit(&ble_hs_stop_sem); + return err; + } + if (!nimble_port_run_active) { + ble_npl_sem_deinit(&ble_hs_stop_sem); + return ESP_OK; + } + } else { + /* Wait till the host stop procedure is complete */ + ble_npl_sem_pend(&ble_hs_stop_sem, BLE_NPL_TIME_FOREVER); } - /* Wait till the host stop procedure is complete */ - ble_npl_sem_pend(&ble_hs_stop_sem, BLE_NPL_TIME_FOREVER); - ble_npl_event_init(&ble_hs_ev_stop, nimble_port_stop_cb, NULL); ble_npl_eventq_put(&g_eventq_dflt, &ble_hs_ev_stop); @@ -417,14 +438,25 @@ IRAM_ATTR void nimble_port_run(void) { struct ble_npl_event *ev; + /* Snapshot stop_ev and stop_sem addresses before entering the loop. + * When BLE_STATIC_TO_DYNAMIC is enabled, after ble_npl_sem_release(stop_sem) + * at the end of stop-event handling, the caller of nimble_port_stop() can + * wake up and free ble_npl_ctx. Any subsequent use of the &ble_hs_ev_stop + * or &ble_hs_stop_sem macros (which dereference ble_npl_ctx) would be a + * use-after-free. Using local snapshots avoids that. + */ + struct ble_npl_event *stop_ev = &ble_hs_ev_stop; + struct ble_npl_sem *stop_sem = &ble_hs_stop_sem; + nimble_port_run_active = true; while (1) { ev = ble_npl_eventq_get(&g_eventq_dflt, BLE_NPL_TIME_FOREVER); if (ev) { ble_npl_event_run(ev); - if (ev == &ble_hs_ev_stop) { - ble_npl_event_deinit(&ble_hs_ev_stop); - ble_npl_sem_release(&ble_hs_stop_sem); + if (ev == stop_ev) { + ble_npl_event_deinit(stop_ev); + nimble_port_run_active = false; + ble_npl_sem_release(stop_sem); break; } } @@ -438,18 +470,18 @@ struct ble_npl_eventq * nimble_port_get_dflt_eventq(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + /* Check deiniting first: context may still be non-NULL during the deinit + * window after ble_npl_deiniting is set but before ble_npl_ctx is freed. + * Return the fallback to prevent callers from using a deinitialized queue. */ + if (ble_npl_deiniting) { + return &g_eventq_shutdown_fallback; + } + /* If context exists, return the real eventq */ if (ble_npl_ctx != NULL) { return &g_eventq_dflt; } - /* Context is NULL - either not yet allocated or already freed during deinit. - * If we're in shutdown, return the static fallback to prevent crashes. - * The fallback eventq is not functional but callers can safely reference it. */ - if (ble_npl_deiniting) { - return &g_eventq_shutdown_fallback; - } - /* Normal case: try to allocate context */ if (ble_npl_ensure_ctx()) { return NULL; diff --git a/porting/nimble/src/os_mbuf.c b/porting/nimble/src/os_mbuf.c index 3ea113c07..e40ee9226 100644 --- a/porting/nimble/src/os_mbuf.c +++ b/porting/nimble/src/os_mbuf.c @@ -121,7 +121,7 @@ os_mqueue_put(struct os_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf * int rc; /* Can only place the head of a chained mbuf on the queue. */ - if (!OS_MBUF_IS_PKTHDR(m)) { + if (m == NULL || !OS_MBUF_IS_PKTHDR(m)) { rc = OS_EINVAL; goto err; } @@ -153,15 +153,23 @@ os_msys_register(struct os_mbuf_pool *new_pool) pool = NULL; STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) { - if (new_pool->omp_databuf_len > pool->omp_databuf_len) { + if (new_pool->omp_databuf_len < pool->omp_databuf_len) { break; } } - if (pool) { - STAILQ_INSERT_AFTER(&g_msys_pool_list, pool, new_pool, omp_next); - } else { + if (pool == NULL) { STAILQ_INSERT_TAIL(&g_msys_pool_list, new_pool, omp_next); + } else if (pool == STAILQ_FIRST(&g_msys_pool_list)) { + STAILQ_INSERT_HEAD(&g_msys_pool_list, new_pool, omp_next); + } else { + struct os_mbuf_pool *prev; + + prev = STAILQ_FIRST(&g_msys_pool_list); + while (STAILQ_NEXT(prev, omp_next) != pool) { + prev = STAILQ_NEXT(prev, omp_next); + } + STAILQ_INSERT_AFTER(&g_msys_pool_list, prev, new_pool, omp_next); } return (0); @@ -216,7 +224,7 @@ os_msys_get(uint16_t dsize, uint16_t leadingspace) err: log_count ++; if ((log_count % 100) == 0) { - ESP_LOGI("ESP_LOG_INFO","_os_msys_find_pool failed (size %u)\n",dsize); + ESP_LOGE("nimble_mbuf", "_os_msys_find_pool failed (size %u)", dsize); log_count = 0; } @@ -241,7 +249,7 @@ os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len) err: log_count ++; if ((log_count % 100) == 0) { - ESP_LOGI("ESP_LOG_INFO","_os_msys_find_pool failed (size %u)\n",dsize); + ESP_LOGE("nimble_mbuf", "_os_msys_find_pool failed (size %u)", dsize); log_count = 0; } return (NULL); @@ -570,6 +578,8 @@ os_mbuf_dup(struct os_mbuf *om) if (OS_MBUF_IS_PKTHDR(om)) { _os_mbuf_copypkthdr(head, om); + /* Restore leading space that _os_mbuf_copypkthdr overwrote */ + head->om_data += OS_MBUF_LEADINGSPACE(om); } copy = head; } @@ -590,6 +600,10 @@ os_mbuf_off(const struct os_mbuf *om, int off, uint16_t *out_off) struct os_mbuf *next; struct os_mbuf *cur; + if (off < 0) { + return NULL; + } + /* Cast away const. */ cur = (struct os_mbuf *)om; @@ -618,7 +632,11 @@ os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst) unsigned int count; uint8_t *udst; - if (!len) { + if (off < 0 || len < 0) { + return -1; + } + + if (len == 0) { return 0; } @@ -872,6 +890,10 @@ os_mbuf_prepend(struct os_mbuf *om, int len) struct os_mbuf * os_mbuf_prepend_pullup(struct os_mbuf *om, uint16_t len) { + if (om == NULL) { + return NULL; + } + om = os_mbuf_prepend(om, len); if (om == NULL) { return NULL; @@ -894,6 +916,7 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len) uint16_t cur_off; int copylen; int rc; + int orig_len = len; /* Find the mbuf,offset pair for the start of the destination. */ cur = os_mbuf_off(om, off, &cur_off); @@ -936,7 +959,7 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len) /* Fix up the packet header, if one is present. */ if (OS_MBUF_IS_PKTHDR(om)) { OS_MBUF_PKTHDR(om)->omp_len = - max(OS_MBUF_PKTHDR(om)->omp_len, off + len); + max(OS_MBUF_PKTHDR(om)->omp_len, off + orig_len); } return 0; @@ -1052,6 +1075,7 @@ os_mbuf_pullup(struct os_mbuf *om, uint16_t len) if (OS_MBUF_IS_PKTHDR(om)) { _os_mbuf_copypkthdr(om2, om); + om->om_pkthdr_len = 0; } } space = OS_MBUF_TRAILINGSPACE(om2); @@ -1202,6 +1226,11 @@ os_mbuf_widen(struct os_mbuf *om, uint16_t off, uint16_t len) } edge_om->om_len = sub_off; + /* Find actual tail of gap chain (os_mbuf_append may have extended it). */ + while (SLIST_NEXT(prev, om_next) != NULL) { + prev = SLIST_NEXT(prev, om_next); + } + /* Insert the gap into the chain. */ SLIST_NEXT(prev, om_next) = SLIST_NEXT(edge_om, om_next); SLIST_NEXT(edge_om, om_next) = first_new; diff --git a/porting/nimble/src/os_msys_init.c b/porting/nimble/src/os_msys_init.c index e5ff4af60..2a7ae2856 100644 --- a/porting/nimble/src/os_msys_init.c +++ b/porting/nimble/src/os_msys_init.c @@ -149,7 +149,7 @@ os_msys_sanity_min_count(int idx) default: BLE_LL_ASSERT(0); - return ESP_OK; + return 0; } } @@ -291,6 +291,8 @@ os_msys_buf_free(void) os_mempool_unregister(&os_msys_init_2_mempool); #endif #endif + + os_msys_reset(); } void os_msys_init(void) diff --git a/porting/npl/dummy/src/npl_os_dummy.c b/porting/npl/dummy/src/npl_os_dummy.c index 059e1a689..f122d1348 100644 --- a/porting/npl/dummy/src/npl_os_dummy.c +++ b/porting/npl/dummy/src/npl_os_dummy.c @@ -101,7 +101,7 @@ ble_npl_mutex_pend(struct ble_npl_mutex *mu, ble_npl_time_t timeout) ble_npl_error_t ble_npl_mutex_release(struct ble_npl_mutex *mu) { - return BLE_NPL_ENOENT; + return BLE_NPL_OK; } ble_npl_error_t @@ -128,10 +128,11 @@ ble_npl_sem_get_count(struct ble_npl_sem *sem) return 0; } -void +int ble_npl_callout_init(struct ble_npl_callout *c, struct ble_npl_eventq *evq, ble_npl_event_fn *ev_cb, void *ev_arg) { + return 0; } ble_npl_error_t @@ -167,7 +168,8 @@ ble_npl_time_get(void) ble_npl_error_t ble_npl_time_ms_to_ticks(uint32_t ms, ble_npl_time_t *out_ticks) { - return BLE_NPL_ENOENT; + *out_ticks = ms; + return 0; } ble_npl_error_t diff --git a/porting/npl/esp-idf/src/nimble_port_freertos.c b/porting/npl/esp-idf/src/nimble_port_freertos.c index 40ffca3a5..ffd4a031b 100644 --- a/porting/npl/esp-idf/src/nimble_port_freertos.c +++ b/porting/npl/esp-idf/src/nimble_port_freertos.c @@ -21,10 +21,13 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "nimble/nimble_port.h" +#include "esp_log.h" #if CONFIG_BT_CONTROLLER_ENABLED #include "esp_bt.h" #endif +#define NIMBLE_PORT_LOG_TAG "BLE_INIT" + static TaskHandle_t host_task_h = NULL; /** @@ -35,13 +38,28 @@ static TaskHandle_t host_task_h = NULL; */ esp_err_t esp_nimble_enable(void *host_task) { + if (host_task == NULL) { + ESP_LOGE(NIMBLE_PORT_LOG_TAG, "esp_nimble_enable: host_task is NULL"); + return ESP_ERR_INVALID_ARG; + } + + if (host_task_h != NULL) { + return ESP_ERR_INVALID_STATE; + } + /* * Create task where NimBLE host will run. It is not strictly necessary to * have separate task for NimBLE host, but since something needs to handle * default queue it is just easier to make separate task which does this. */ - xTaskCreatePinnedToCore(host_task, "nimble_host", NIMBLE_HS_STACK_SIZE, - NULL, (configMAX_PRIORITIES - 4), &host_task_h, NIMBLE_CORE); + BaseType_t ret = xTaskCreatePinnedToCore(host_task, "nimble_host", + NIMBLE_HS_STACK_SIZE, NULL, + (configMAX_PRIORITIES - 4), + &host_task_h, NIMBLE_CORE); + if (ret != pdPASS) { + host_task_h = NULL; + return ESP_ERR_NO_MEM; + } return ESP_OK; } @@ -54,8 +72,9 @@ esp_err_t esp_nimble_enable(void *host_task) esp_err_t esp_nimble_disable(void) { if (host_task_h) { - vTaskDelete(host_task_h); + TaskHandle_t tmp = host_task_h; host_task_h = NULL; + vTaskDelete(tmp); } return ESP_OK; } @@ -69,7 +88,14 @@ esp_err_t esp_nimble_disable(void) void nimble_port_freertos_init(TaskFunction_t host_task_fn) { - esp_nimble_enable(host_task_fn); + esp_err_t err = esp_nimble_enable(host_task_fn); + if (err != ESP_OK) { + ESP_LOGE(NIMBLE_PORT_LOG_TAG, + "nimble_port_freertos_init: esp_nimble_enable failed (%d)", err); + /* BLE host task creation is critical; abort so the system does not + * continue in a broken state where all BLE operations will fail. */ + abort(); + } } /** diff --git a/porting/npl/freertos/src/nimble_port_freertos.c b/porting/npl/freertos/src/nimble_port_freertos.c index 40ffca3a5..e695e20f0 100644 --- a/porting/npl/freertos/src/nimble_port_freertos.c +++ b/porting/npl/freertos/src/nimble_port_freertos.c @@ -24,6 +24,9 @@ #if CONFIG_BT_CONTROLLER_ENABLED #include "esp_bt.h" #endif +#include "esp_log.h" + +#define NIMBLE_PORT_LOG_TAG "BLE_INIT" static TaskHandle_t host_task_h = NULL; @@ -35,13 +38,27 @@ static TaskHandle_t host_task_h = NULL; */ esp_err_t esp_nimble_enable(void *host_task) { + if (host_task == NULL) { + return ESP_ERR_INVALID_ARG; + } + + if (host_task_h != NULL) { + return ESP_ERR_INVALID_STATE; + } + /* * Create task where NimBLE host will run. It is not strictly necessary to * have separate task for NimBLE host, but since something needs to handle * default queue it is just easier to make separate task which does this. */ - xTaskCreatePinnedToCore(host_task, "nimble_host", NIMBLE_HS_STACK_SIZE, - NULL, (configMAX_PRIORITIES - 4), &host_task_h, NIMBLE_CORE); + BaseType_t ret = xTaskCreatePinnedToCore(host_task, "nimble_host", + NIMBLE_HS_STACK_SIZE, NULL, + (configMAX_PRIORITIES - 4), + &host_task_h, NIMBLE_CORE); + if (ret != pdPASS) { + host_task_h = NULL; + return ESP_ERR_NO_MEM; + } return ESP_OK; } @@ -54,8 +71,9 @@ esp_err_t esp_nimble_enable(void *host_task) esp_err_t esp_nimble_disable(void) { if (host_task_h) { - vTaskDelete(host_task_h); + TaskHandle_t tmp = host_task_h; host_task_h = NULL; + vTaskDelete(tmp); } return ESP_OK; } @@ -69,7 +87,12 @@ esp_err_t esp_nimble_disable(void) void nimble_port_freertos_init(TaskFunction_t host_task_fn) { - esp_nimble_enable(host_task_fn); + esp_err_t err = esp_nimble_enable(host_task_fn); + if (err != ESP_OK) { + ESP_LOGE(NIMBLE_PORT_LOG_TAG, + "nimble_port_freertos_init: esp_nimble_enable failed (%d)", err); + abort(); + } } /** diff --git a/porting/npl/freertos/src/npl_os_freertos.c b/porting/npl/freertos/src/npl_os_freertos.c index d665c87ad..9c0810222 100644 --- a/porting/npl/freertos/src/npl_os_freertos.c +++ b/porting/npl/freertos/src/npl_os_freertos.c @@ -305,7 +305,7 @@ npl_freertos_eventq_init(struct ble_npl_eventq *evq) BLE_LL_ASSERT(eventq); memset(eventq, 0, sizeof(*eventq)); - eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_eventq *)); + eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_event *)); BLE_LL_ASSERT(eventq->q); } #else @@ -315,7 +315,7 @@ npl_freertos_eventq_init(struct ble_npl_eventq *evq) BLE_LL_ASSERT(eventq); memset(eventq, 0, sizeof(*eventq)); - eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_eventq *)); + eventq->q = xQueueCreate(BLE_TOTAL_EV_COUNT, sizeof(struct ble_npl_event *)); BLE_LL_ASSERT(eventq->q); } #endif @@ -326,7 +326,9 @@ npl_freertos_eventq_deinit(struct ble_npl_eventq *evq) { struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq; - BLE_LL_ASSERT(eventq); + if (!eventq) { + return; + } vQueueDelete(eventq->q); #if OS_MEM_ALLOC os_memblock_put(&ble_freertos_evq_pool,eventq); @@ -401,28 +403,6 @@ npl_eventq_queued_get_isr(struct ble_npl_event_freertos *event) return queued; } -static void IRAM_ATTR -npl_eventq_queued_set_isr(struct ble_npl_event_freertos *event, bool queued) -{ - portENTER_CRITICAL_ISR(&ble_port_mutex); - event->queued = queued; - portEXIT_CRITICAL_ISR(&ble_port_mutex); -} - -static bool IRAM_ATTR -npl_eventq_queued_claim_isr(struct ble_npl_event_freertos *event) -{ - bool already; - - portENTER_CRITICAL_ISR(&ble_port_mutex); - already = event->queued; - if (!already) { - event->queued = true; - } - portEXIT_CRITICAL_ISR(&ble_port_mutex); - return already; -} - static void IRAM_ATTR npl_eventq_queued_set_task(struct ble_npl_event_freertos *event, bool queued) { @@ -478,23 +458,27 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) { struct ble_npl_event *ev = NULL; struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq; - BaseType_t woken; + BaseType_t woken = pdFALSE; BaseType_t ret; if (in_isr()) { BLE_LL_ASSERT(tmo == 0); + woken = pdFALSE; + + portENTER_CRITICAL_ISR(&ble_port_mutex); ret = xQueueReceiveFromISR(eventq->q, &ev, &woken); - if( woken == pdTRUE ) { + if (ret == pdPASS && ev != NULL) { + struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event; + if (event) { + event->queued = false; + } + } + portEXIT_CRITICAL_ISR(&ble_port_mutex); + + if (woken == pdTRUE) { portYIELD_FROM_ISR(); } BLE_LL_ASSERT(ret == pdPASS || ret == errQUEUE_EMPTY); - - if (ev) { - struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event; - if (event) { - npl_eventq_queued_set_isr(event, false); - } - } } else if (tmo == 0) { bool locked = npl_eventq_lock(); @@ -555,22 +539,30 @@ npl_freertos_eventq_get(struct ble_npl_eventq *evq, ble_npl_time_t tmo) void npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) { - BaseType_t woken; + BaseType_t woken = pdFALSE; BaseType_t ret; struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq; struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event; if (in_isr()) { - if (npl_eventq_queued_claim_isr(event)) { + woken = pdFALSE; + + portENTER_CRITICAL_ISR(&ble_port_mutex); + if (event->queued) { + portEXIT_CRITICAL_ISR(&ble_port_mutex); return; } + event->queued = true; ret = xQueueSendToBackFromISR(eventq->q, &ev, &woken); if (ret != pdPASS) { - npl_eventq_queued_set_isr(event, false); + event->queued = false; + portEXIT_CRITICAL_ISR(&ble_port_mutex); return; } - if( woken == pdTRUE ) { + portEXIT_CRITICAL_ISR(&ble_port_mutex); + + if (woken == pdTRUE) { portYIELD_FROM_ISR(); } return; @@ -591,7 +583,7 @@ npl_freertos_eventq_put(struct ble_npl_eventq *evq, struct ble_npl_event *ev) } } -void +IRAM_ATTR void npl_freertos_eventq_remove(struct ble_npl_eventq *evq, struct ble_npl_event *ev) { @@ -600,7 +592,7 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq, int i; int count; bool removed; - BaseType_t woken, woken2; + BaseType_t woken = pdFALSE, woken2 = pdFALSE; struct ble_npl_eventq_freertos *eventq = (struct ble_npl_eventq_freertos *)evq->eventq; struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)ev->event; @@ -649,8 +641,6 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq, portYIELD_FROM_ISR(); } } else { - removed = false; - bool locked = npl_eventq_lock(); if (!npl_eventq_queued_get_task(event)) { @@ -658,6 +648,7 @@ npl_freertos_eventq_remove(struct ble_npl_eventq *evq, return; } + removed = false; portENTER_CRITICAL(&ble_port_mutex); count = uxQueueMessagesWaiting(eventq->q); for (i = 0; i < count; i++) { @@ -695,12 +686,16 @@ npl_freertos_mutex_init(struct ble_npl_mutex *mu) mutex = (struct ble_npl_mutex_freertos *)mu->mutex; if (!mutex) { - return BLE_NPL_INVALID_PARAM; + return BLE_NPL_ENOMEM; } memset(mutex, 0, sizeof(*mutex)); mutex->handle = xSemaphoreCreateRecursiveMutex(); - BLE_LL_ASSERT(mutex->handle); + if (!mutex->handle) { + os_memblock_put(&ble_freertos_mutex_pool, mutex); + mu->mutex = NULL; + return BLE_NPL_ENOMEM; + } } #else if(!mu->mutex) { @@ -708,12 +703,16 @@ npl_freertos_mutex_init(struct ble_npl_mutex *mu) mutex = (struct ble_npl_mutex_freertos *)mu->mutex; if (!mutex) { - return BLE_NPL_INVALID_PARAM; + return BLE_NPL_ENOMEM; } memset(mutex, 0, sizeof(*mutex)); mutex->handle = xSemaphoreCreateRecursiveMutex(); - BLE_LL_ASSERT(mutex->handle); + if (!mutex->handle) { + nimble_platform_mem_free((void *)mutex); + mu->mutex = NULL; + return BLE_NPL_ENOMEM; + } } #endif @@ -822,7 +821,7 @@ npl_freertos_mutex_release(struct ble_npl_mutex *mu) BLE_LL_ASSERT(mutex->handle); if (in_isr()) { - BLE_LL_ASSERT(0); + return BLE_NPL_ERR_IN_ISR; } else { if (xSemaphoreGiveRecursive(mutex->handle) != pdPASS) { return BLE_NPL_BAD_MUTEX; @@ -842,12 +841,16 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens) semaphor = (struct ble_npl_sem_freertos *)sem->sem; if (!semaphor) { - return BLE_NPL_INVALID_PARAM; + return BLE_NPL_ENOMEM; } memset(semaphor, 0, sizeof(*semaphor)); semaphor->handle = xSemaphoreCreateCounting(128, tokens); - BLE_LL_ASSERT(semaphor->handle); + if (!semaphor->handle) { + os_memblock_put(&ble_freertos_sem_pool, semaphor); + sem->sem = NULL; + return BLE_NPL_ENOMEM; + } } #else if(!sem->sem) { @@ -855,12 +858,16 @@ npl_freertos_sem_init(struct ble_npl_sem *sem, uint16_t tokens) semaphor = (struct ble_npl_sem_freertos *)sem->sem; if (!semaphor) { - return BLE_NPL_INVALID_PARAM; + return BLE_NPL_ENOMEM; } memset(semaphor, 0, sizeof(*semaphor)); semaphor->handle = xSemaphoreCreateCounting(128, tokens); - BLE_LL_ASSERT(semaphor->handle); + if (!semaphor->handle) { + nimble_platform_mem_free((void *)semaphor); + sem->sem = NULL; + return BLE_NPL_ENOMEM; + } } #endif @@ -892,7 +899,7 @@ npl_freertos_sem_deinit(struct ble_npl_sem *sem) ble_npl_error_t npl_freertos_sem_pend(struct ble_npl_sem *sem, ble_npl_time_t timeout) { - BaseType_t woken; + BaseType_t woken = pdFALSE; BaseType_t ret; struct ble_npl_sem_freertos *semaphor = (struct ble_npl_sem_freertos *)sem->sem; @@ -919,7 +926,7 @@ ble_npl_error_t npl_freertos_sem_release(struct ble_npl_sem *sem) { BaseType_t ret; - BaseType_t woken; + BaseType_t woken = pdFALSE; struct ble_npl_sem_freertos *semaphor = (struct ble_npl_sem_freertos *)sem->sem; if (!semaphor) { @@ -952,11 +959,13 @@ ble_npl_event_fn_wrapper(void *arg) ble_npl_eventq_put(callout->evq, &callout->ev); } else { struct ble_npl_event_freertos *event = (struct ble_npl_event_freertos *)callout->ev.event; - event->fn(&callout->ev); + if (event && event->fn) { + event->fn(&callout->ev); + } } } -static +static IRAM_ATTR ble_npl_error_t esp_err_to_npl_error(esp_err_t err) { switch(err) { @@ -1024,6 +1033,7 @@ npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq } #else + callout->evq = evq; callout->handle = xTimerCreate("co", 1, pdFALSE, callout, os_callout_timer_cb); if (!callout->handle) { @@ -1069,6 +1079,7 @@ npl_freertos_callout_init(struct ble_npl_callout *co, struct ble_npl_eventq *evq return -1; } #else + callout->evq = evq; callout->handle = xTimerCreate("co", 1, pdFALSE, callout, os_callout_timer_cb); if (!callout->handle) { @@ -1125,11 +1136,17 @@ uint16_t npl_freertos_sem_get_count(struct ble_npl_sem *sem) { struct ble_npl_sem_freertos *semaphor = (struct ble_npl_sem_freertos *)sem->sem; + if (!semaphor || !semaphor->handle) { + return 0; + } + if (in_isr()) { + return uxQueueMessagesWaitingFromISR(semaphor->handle); + } return uxSemaphoreGetCount(semaphor->handle); } -ble_npl_error_t +IRAM_ATTR ble_npl_error_t npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks) { struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co; @@ -1139,10 +1156,10 @@ npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks) npl_freertos_eventq_remove(callout->evq, &callout->ev); } - return esp_err_to_npl_error(esp_timer_start_once(callout->handle, ticks*1000)); + return esp_err_to_npl_error(esp_timer_start_once(callout->handle, (uint64_t)ticks * 1000)); #else - BaseType_t woken1, woken2, woken3; + BaseType_t woken1 = pdFALSE, woken2 = pdFALSE, woken3 = pdFALSE; if (ticks == 0) { ticks = 1; @@ -1164,14 +1181,13 @@ npl_freertos_callout_reset(struct ble_npl_callout *co, ble_npl_time_t ticks) npl_freertos_eventq_remove(callout->evq, &callout->ev); } xTimerChangePeriod(callout->handle, ticks, portMAX_DELAY); - xTimerReset(callout->handle, portMAX_DELAY); } return BLE_NPL_OK; #endif } -void +IRAM_ATTR void npl_freertos_callout_stop(struct ble_npl_callout *co) { struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co; @@ -1181,9 +1197,19 @@ npl_freertos_callout_stop(struct ble_npl_callout *co) } #if CONFIG_BT_NIMBLE_USE_ESP_TIMER - esp_timer_stop(callout->handle); + if (!in_isr()) { + esp_timer_stop(callout->handle); + } #else - xTimerStop(callout->handle, portMAX_DELAY); + if (in_isr()) { + BaseType_t woken = pdFALSE; + xTimerStopFromISR(callout->handle, &woken); + if (woken == pdTRUE) { + portYIELD_FROM_ISR(); + } + } else { + xTimerStop(callout->handle, portMAX_DELAY); + } #endif if (callout->evq) { @@ -1195,6 +1221,9 @@ bool npl_freertos_callout_is_active(struct ble_npl_callout *co) { struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co; + if (!callout) { + return false; + } #if CONFIG_BT_NIMBLE_USE_ESP_TIMER return esp_timer_is_active(callout->handle); #else @@ -1206,15 +1235,28 @@ ble_npl_time_t npl_freertos_callout_get_ticks(struct ble_npl_callout *co) { #if CONFIG_BT_NIMBLE_USE_ESP_TIMER - /* Currently, esp_timer does not support an API which gets the expiry time for - * current timer. - * Returning 0 from here should not cause any effect. - * Drawback of this approach is that existing code to reset timer would be called - * more often (since the if condition to invoke reset timer would always succeed if - * timer is active). - */ + uint32_t exp = 0; - return 0; +#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0) + struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co; + if (!callout) { + return 0; + } + + uint64_t expiry = 0; + esp_err_t err; + + err = esp_timer_get_expiry_time((esp_timer_handle_t)(callout->handle), &expiry); + if (err != ESP_OK) { + return 0; + } + + npl_freertos_time_ms_to_ticks((uint32_t)(expiry / 1000), &exp); +#else + exp = 0; +#endif + + return exp; #else struct ble_npl_callout_freertos *callout = (struct ble_npl_callout_freertos *)co->co; return xTimerGetExpiryTime(callout->handle); @@ -1253,8 +1295,9 @@ npl_freertos_callout_remaining_ticks(struct ble_npl_callout *co, exp = xTimerGetExpiryTime(callout->handle); #endif - if (exp > now) { - rt = exp - now; + int32_t diff = (int32_t)(exp - now); + if (diff > 0) { + rt = (ble_npl_time_t)diff; } else { rt = 0; } @@ -1276,7 +1319,11 @@ npl_freertos_time_get(void) #if CONFIG_BT_NIMBLE_USE_ESP_TIMER return esp_timer_get_time() / 1000; #else - return xTaskGetTickCountFromISR(); + if (xPortInIsrContext()) { + return xTaskGetTickCountFromISR(); + } else { + return xTaskGetTickCount(); + } #endif } @@ -1322,7 +1369,7 @@ npl_freertos_time_ms_to_ticks32(uint32_t ms) #if CONFIG_BT_NIMBLE_USE_ESP_TIMER return ms; #else - return ms * configTICK_RATE_HZ / 1000; + return (ble_npl_time_t)((uint64_t)ms * configTICK_RATE_HZ / 1000); #endif } @@ -1332,7 +1379,7 @@ npl_freertos_time_ticks_to_ms32(ble_npl_time_t ticks) #if CONFIG_BT_NIMBLE_USE_ESP_TIMER return ticks; #else - return ticks * 1000 / configTICK_RATE_HZ; + return (uint32_t)((uint64_t)ticks * 1000 / configTICK_RATE_HZ); #endif } @@ -1457,6 +1504,9 @@ struct npl_funcs_t * npl_freertos_funcs_get(void) void npl_freertos_funcs_init(void) { + if (npl_funcs != NULL) { + return; + } npl_funcs = (struct npl_funcs_t *)nimble_platform_mem_calloc(1,sizeof(struct npl_funcs_t)); if(!npl_funcs) { assert(0); @@ -1589,7 +1639,6 @@ _error: } #endif - BLE_LL_ASSERT(rc == 0); return rc; } @@ -1600,6 +1649,12 @@ void npl_freertos_mempool_deinit(void) npl_eventq_sync = NULL; } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_freertos_ctx) { + return; + } +#endif + #if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED if (ble_freertos_ev_buf) { nimble_platform_mem_free(ble_freertos_ev_buf);