From 7526bde3a92c6320a26f82cc73e2864224db413d Mon Sep 17 00:00:00 2001 From: Rahul Tank Date: Mon, 23 Feb 2026 14:03:23 +0530 Subject: [PATCH] fix(nimble): Fix various issues for security/vulnerability in host --- nimble/host/include/host/ble_eddystone.h | 4 +- nimble/host/include/host/ble_ibeacon.h | 2 +- nimble/host/services/ans/src/ble_svc_ans.c | 60 +-- nimble/host/services/bas/src/ble_svc_bas.c | 37 +- nimble/host/services/bleuart/src/bleuart.c | 21 +- nimble/host/services/cte/src/ble_svc_cte.c | 101 +++-- nimble/host/services/cts/src/ble_svc_cts.c | 67 ++-- nimble/host/services/dis/src/ble_svc_dis.c | 50 ++- nimble/host/services/gap/src/ble_svc_gap.c | 106 ++++-- nimble/host/services/gatt/src/ble_svc_gatt.c | 36 +- .../hid/include/services/hid/ble_svc_hid.h | 2 +- nimble/host/services/hid/src/ble_svc_hid.c | 162 ++++++-- nimble/host/services/hr/src/ble_svc_hr.c | 25 +- nimble/host/services/htp/src/ble_svc_htp.c | 17 +- .../ias/include/services/ias/ble_svc_ias.h | 4 +- nimble/host/services/ias/src/ble_svc_ias.c | 27 +- nimble/host/services/ipss/src/ble_svc_ipss.c | 2 +- nimble/host/services/lls/src/ble_svc_lls.c | 24 +- nimble/host/services/prox/src/ble_svc_prox.c | 40 +- nimble/host/services/ras/src/ble_svc_ras.c | 124 +++--- .../services/ras/src/rrsp/ble_svc_ras_rrsp.c | 125 ++++--- nimble/host/services/sps/src/ble_svc_sps.c | 18 +- nimble/host/services/tps/src/ble_svc_tps.c | 14 +- nimble/host/src/ble_aes_ccm.c | 63 ++-- nimble/host/src/ble_att.c | 83 ++++- nimble/host/src/ble_att_clt.c | 102 ++--- nimble/host/src/ble_att_cmd.c | 62 ++- nimble/host/src/ble_att_svr.c | 148 ++++++-- nimble/host/src/ble_cs.c | 105 ++++-- nimble/host/src/ble_ead.c | 54 +-- nimble/host/src/ble_eatt.c | 128 +++++-- nimble/host/src/ble_eddystone.c | 67 +++- nimble/host/src/ble_gap.c | 352 ++++++++++++------ nimble/host/src/ble_gattc.c | 79 ++-- nimble/host/src/ble_gattc_cache.c | 78 ++-- nimble/host/src/ble_gattc_cache_conn.c | 112 ++++-- nimble/host/src/ble_gatts.c | 72 +++- nimble/host/src/ble_gatts_lcl.c | 31 +- nimble/host/src/ble_hs_adv.c | 31 +- nimble/host/src/ble_hs_atomic.c | 17 +- nimble/host/src/ble_hs_cfg.c | 14 +- nimble/host/src/ble_hs_conn.c | 72 ++-- nimble/host/src/ble_hs_flow.c | 36 +- nimble/host/src/ble_hs_hci.c | 75 ++-- nimble/host/src/ble_hs_hci_cmd.c | 5 +- nimble/host/src/ble_hs_hci_evt.c | 111 ++++-- nimble/host/src/ble_hs_hci_priv.h | 4 +- nimble/host/src/ble_hs_hci_util.c | 70 +++- nimble/host/src/ble_hs_id.c | 53 ++- nimble/host/src/ble_hs_id_priv.h | 2 +- nimble/host/src/ble_hs_log.c | 17 +- nimble/host/src/ble_hs_mbuf.c | 33 +- nimble/host/src/ble_hs_misc.c | 6 +- nimble/host/src/ble_hs_mqueue.c | 10 +- nimble/host/src/ble_hs_periodic_sync.c | 59 ++- nimble/host/src/ble_hs_priv.h | 1 + nimble/host/src/ble_hs_pvcy.c | 95 ++++- nimble/host/src/ble_hs_pvcy_priv.h | 5 +- nimble/host/src/ble_hs_resolv.c | 181 +++++---- nimble/host/src/ble_hs_shutdown.c | 17 +- nimble/host/src/ble_hs_startup.c | 23 +- nimble/host/src/ble_hs_stop.c | 36 +- nimble/host/src/ble_ibeacon.c | 15 +- nimble/host/src/ble_l2cap.c | 16 +- nimble/host/src/ble_l2cap_coc.c | 109 ++++-- nimble/host/src/ble_l2cap_sig.c | 190 +++++++--- nimble/host/src/ble_l2cap_sig_cmd.c | 5 +- nimble/host/src/ble_l2cap_sig_priv.h | 2 +- nimble/host/src/ble_sm.c | 71 +++- nimble/host/src/ble_sm_alg.c | 51 ++- nimble/host/src/ble_sm_cmd.c | 5 +- nimble/host/src/ble_sm_lgcy.c | 35 +- nimble/host/src/ble_sm_priv.h | 2 +- nimble/host/src/ble_sm_sc.c | 62 ++- nimble/host/src/ble_store.c | 26 +- nimble/host/src/ble_store_util.c | 44 ++- nimble/host/src/ble_uuid.c | 21 +- .../host/store/config/src/ble_store_config.c | 69 ++-- .../store/config/src/ble_store_config_conf.c | 37 +- nimble/host/store/config/src/ble_store_nvs.c | 64 +++- nimble/host/store/ram/src/ble_store_ram.c | 9 +- nimble/host/util/src/addr.c | 13 +- nimble/transport/src/transport.c | 5 + 83 files changed, 3145 insertions(+), 1283 deletions(-) diff --git a/nimble/host/include/host/ble_eddystone.h b/nimble/host/include/host/ble_eddystone.h index 76b7e2b01..89500ff78 100644 --- a/nimble/host/include/host/ble_eddystone.h +++ b/nimble/host/include/host/ble_eddystone.h @@ -75,7 +75,7 @@ struct ble_hs_adv_fields; * Other nonzero on failure. */ int ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, - void *uid, int8_t measured_power); + const void *uid, int8_t measured_power); /** * Configures the device to advertise Eddystone URL beacons. @@ -102,7 +102,7 @@ int ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, * Other nonzero on failure. */ int ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields, - uint8_t url_scheme, char *url_body, + uint8_t url_scheme, const char *url_body, uint8_t url_body_len, uint8_t suffix, int8_t measured_power); diff --git a/nimble/host/include/host/ble_ibeacon.h b/nimble/host/include/host/ble_ibeacon.h index fff7c57ae..5c1d482b3 100644 --- a/nimble/host/include/host/ble_ibeacon.h +++ b/nimble/host/include/host/ble_ibeacon.h @@ -24,7 +24,7 @@ extern "C" { #endif -int ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, +int ble_ibeacon_set_adv_data(const void *uuid128, uint16_t major, uint16_t minor, int8_t measured_power); #ifdef __cplusplus diff --git a/nimble/host/services/ans/src/ble_svc_ans.c b/nimble/host/services/ans/src/ble_svc_ans.c index bd444947c..c43e53f99 100644 --- a/nimble/host/services/ans/src/ble_svc_ans.c +++ b/nimble/host/services/ans/src/ble_svc_ans.c @@ -33,6 +33,9 @@ * for category ID and count. */ #define BLE_SVC_ANS_NEW_ALERT_MAX_LEN (BLE_SVC_ANS_INFO_STR_MAX_LEN + 2) +/* Category ID for "all categories" */ +#define BLE_SVC_ANS_CAT_ID_ALL 0xFF + /* Supported categories bitmasks */ static uint8_t ble_svc_ans_new_alert_cat; static uint8_t ble_svc_ans_unr_alert_cat; @@ -86,7 +89,7 @@ static const ble_uuid16_t uuid_chr_alert_notif_ctrl_pt = BLE_UUID16_INIT(BLE_SVC static const struct ble_gatt_chr_def ans_characteristics[] = { { - /** Supported New Alert Catagory + /** Supported New Alert Category * * This characteristic exposes what categories of new * alert are supported in the server. @@ -105,7 +108,7 @@ static const struct ble_gatt_chr_def ans_characteristics[] = { .val_handle = &ble_svc_ans_new_alert_val_handle, .flags = BLE_GATT_CHR_F_NOTIFY, }, { - /** Supported Unread Alert Catagory + /** Supported Unread Alert Category * * This characteristic exposes what categories of * unread alert are supported in the server. @@ -165,18 +168,22 @@ ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle, uint16_t uuid16; int rc; - /* ANS Control point command and catagory variables */ + /* ANS Control point command and category variables */ uint8_t cmd_id; uint8_t cat_id; uint8_t cat_bit_mask; int i; uuid16 = ble_uuid_u16(ctxt->chr->uuid); - assert(uuid16 != 0); + if (uuid16 == 0) { + return BLE_ATT_ERR_UNLIKELY; + } switch (uuid16) { case BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT: - assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); + if (ctxt->op != BLE_GATT_ACCESS_OP_READ_CHR) { + return BLE_ATT_ERR_UNLIKELY; + } rc = os_mbuf_append(ctxt->om, &ble_svc_ans_new_alert_cat, sizeof ble_svc_ans_new_alert_cat); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; @@ -195,10 +202,11 @@ ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle, return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } - assert(0); return BLE_ATT_ERR_UNLIKELY; case BLE_SVC_ANS_CHR_UUID16_SUP_UNR_ALERT_CAT: - assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); + if (ctxt->op != BLE_GATT_ACCESS_OP_READ_CHR) { + return BLE_ATT_ERR_UNLIKELY; + } rc = os_mbuf_append(ctxt->om, &ble_svc_ans_unr_alert_cat, sizeof ble_svc_ans_unr_alert_cat); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; @@ -236,7 +244,7 @@ ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle, /* Set cat_bit_mask to the appropriate bitmask based on cat_id */ if (cat_id < BLE_SVC_ANS_CAT_NUM) { cat_bit_mask = (1 << cat_id); - } else if (cat_id == 0xff) { + } else if (cat_id == BLE_SVC_ANS_CAT_ID_ALL) { cat_bit_mask = cat_id; } else { /* invalid category ID */ @@ -257,26 +265,26 @@ ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle, ble_svc_ans_unr_alert_cat &= ~cat_bit_mask; break; case BLE_SVC_ANS_CMD_NOT_NEW_ALERT_IMMEDIATE: - if (cat_id == 0xff) { + if (cat_id == BLE_SVC_ANS_CAT_ID_ALL) { /* If cat_id is 0xff, notify on all enabled categories */ - for (i = BLE_SVC_ANS_CAT_NUM - 1; i > 0; --i) { + for (i = BLE_SVC_ANS_CAT_NUM - 1; i >= 0; --i) { if ((ble_svc_ans_new_alert_cat >> i) & 0x01) { ble_svc_ans_new_alert_notify(i, NULL); } } - } else { + } else if ((ble_svc_ans_new_alert_cat >> cat_id) & 0x01){ ble_svc_ans_new_alert_notify(cat_id, NULL); } break; case BLE_SVC_ANS_CMD_NOT_UNR_ALERT_IMMEDIATE: - if (cat_id == 0xff) { + if (cat_id == BLE_SVC_ANS_CAT_ID_ALL) { /* If cat_id is 0xff, notify on all enabled categories */ - for (i = BLE_SVC_ANS_CAT_NUM - 1; i > 0; --i) { + for (i = BLE_SVC_ANS_CAT_NUM - 1; i >= 0; --i) { if ((ble_svc_ans_unr_alert_cat >> i) & 0x01) { ble_svc_ans_unr_alert_notify(i); } } - } else { + } else if ((ble_svc_ans_unr_alert_cat >> cat_id) & 0x01) { ble_svc_ans_unr_alert_notify(cat_id); } break; @@ -290,13 +298,12 @@ ble_svc_ans_access(uint16_t conn_handle, uint16_t attr_handle, return rc; default: - assert(0); return BLE_ATT_ERR_UNLIKELY; } } /** - * This function must be called with the connection handlewhen a gap + * This function must be called with the connection handle when a gap * connect event is received in order to send notifications to the * client. * @@ -335,7 +342,10 @@ ble_svc_ans_new_alert_add(uint8_t cat_id, const char * info_str) return BLE_HS_EINVAL; } - ble_svc_ans_new_alert_cnt[cat_id] += 1; + if (ble_svc_ans_new_alert_cnt[cat_id] < 0xFF) { + ble_svc_ans_new_alert_cnt[cat_id] += 1; + } + return ble_svc_ans_new_alert_notify(cat_id, info_str); } @@ -363,7 +373,10 @@ ble_svc_ans_unr_alert_add(uint8_t cat_id) return BLE_HS_EINVAL; } - ble_svc_ans_unr_alert_cnt[cat_id] += 1; + if (ble_svc_ans_unr_alert_cnt[cat_id] < 0xFF) { + ble_svc_ans_unr_alert_cnt[cat_id] += 1; + } + return ble_svc_ans_unr_alert_notify(cat_id); } @@ -383,7 +396,7 @@ ble_svc_ans_new_alert_notify(uint8_t cat_id, const char * info_str) { int info_str_len; - /* Clear notification to remove old infomation that may persist */ + /* Clear notification to remove old information that may persist */ memset(&ble_svc_ans_new_alert_val, '\0', BLE_SVC_ANS_NEW_ALERT_MAX_LEN); @@ -414,9 +427,9 @@ ble_svc_ans_new_alert_notify(uint8_t cat_id, const char * info_str) * Send an unread alert notification to the given category. * * @param cat_id The ID of the category to send the - * notificaiton to. + * notification to. * - * @return 0 on success, non-zer0 error code otherwise. + * @return 0 on success, non-zero error code otherwise. */ static int ble_svc_ans_unr_alert_notify(uint8_t cat_id) @@ -459,10 +472,10 @@ ble_svc_ans_chr_write(struct os_mbuf *om, uint16_t min_len, /** * Initialize the ANS with initial values for enabled categories * for new and unread alert characteristics. Bitwise or the - * catagory bitmasks to enable multiple catagories. + * category bitmasks to enable multiple categories. * * XXX: We should technically be able to change the new alert and - * unread alert catagories when we have no active connections. + * unread alert categories when we have no active connections. */ void ble_svc_ans_init(void) @@ -474,6 +487,7 @@ ble_svc_ans_init(void) rc = ble_gatts_count_cfg(ble_svc_ans_defs); SYSINIT_PANIC_ASSERT(rc == 0); + (void)rc; rc = ble_gatts_add_svcs(ble_svc_ans_defs); SYSINIT_PANIC_ASSERT(rc == 0); diff --git a/nimble/host/services/bas/src/ble_svc_bas.c b/nimble/host/services/bas/src/ble_svc_bas.c index f27866662..0587497f2 100644 --- a/nimble/host/services/bas/src/ble_svc_bas.c +++ b/nimble/host/services/bas/src/ble_svc_bas.c @@ -33,7 +33,7 @@ static uint16_t ble_svc_bas_battery_handle; #endif /* Battery level */ -uint8_t ble_svc_bas_battery_level; +static uint8_t ble_svc_bas_battery_level; /* Access function */ static int @@ -45,17 +45,17 @@ static const ble_uuid16_t uuid_chr_bat_lvl = BLE_UUID16_INIT(BLE_SVC_BAS_CHR_UUI static const struct ble_gatt_chr_def bas_characteristics[] = { { - /*** Battery level characteristic */ + /*** Battery level characteristic */ .uuid = &uuid_chr_bat_lvl.u, .access_cb = ble_svc_bas_access, #if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0 - .val_handle = &ble_svc_bas_battery_handle, + .val_handle = &ble_svc_bas_battery_handle, #endif .flags = BLE_GATT_CHR_F_READ | #if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0 - BLE_GATT_CHR_F_NOTIFY | + BLE_GATT_CHR_F_NOTIFY | #endif - MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_READ_PERM), + MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_READ_PERM), }, { 0, /* No more characteristics in this service. */ @@ -88,26 +88,37 @@ ble_svc_bas_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_SVC_BAS_CHR_UUID16_BATTERY_LEVEL: assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); rc = os_mbuf_append(ctxt->om, &ble_svc_bas_battery_level, - sizeof ble_svc_bas_battery_level); + sizeof(ble_svc_bas_battery_level)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; default: - assert(0); return BLE_ATT_ERR_UNLIKELY; } } +/** + * Get the current battery level. + * + * @return The current battery level (0-100). + */ +uint8_t +ble_svc_bas_battery_level_get(void) +{ + return ble_svc_bas_battery_level; +} + /** * Set the battery level, must be between 0 and 100. * If greater than 100, it will be silently set to 100. */ int -ble_svc_bas_battery_level_set(uint8_t level) { +ble_svc_bas_battery_level_set(uint8_t level) +{ if (level > 100) - level = 100; + level = 100; if (ble_svc_bas_battery_level != level) { - ble_svc_bas_battery_level = level; + ble_svc_bas_battery_level = level; #if MYNEWT_VAL(BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE) > 0 ble_gatts_chr_updated(ble_svc_bas_battery_handle); #endif @@ -115,6 +126,10 @@ ble_svc_bas_battery_level_set(uint8_t level) { return 0; } +/** + * Deinitialize the Battery Service. + * Note: Frees all registered GATT services as part of full BLE stack teardown. + */ void ble_svc_bas_deinit(void) { @@ -137,5 +152,7 @@ ble_svc_bas_init(void) rc = ble_gatts_add_svcs(ble_svc_bas_defs); SYSINIT_PANIC_ASSERT(rc == 0); + + ble_svc_bas_battery_level = 0; } #endif diff --git a/nimble/host/services/bleuart/src/bleuart.c b/nimble/host/services/bleuart/src/bleuart.c index 891d5faef..3e9f21764 100644 --- a/nimble/host/services/bleuart/src/bleuart.c +++ b/nimble/host/services/bleuart/src/bleuart.c @@ -32,18 +32,18 @@ #if MYNEWT_VAL(BLE_GATTS) /* ble uart attr read handle */ -uint16_t g_bleuart_attr_read_handle; +static uint16_t g_bleuart_attr_read_handle; /* ble uart attr write handle */ -uint16_t g_bleuart_attr_write_handle; +static uint16_t g_bleuart_attr_write_handle; /* Pointer to a console buffer */ -char *console_buf; +static char *console_buf; -uint16_t g_console_conn_handle; +static uint16_t g_console_conn_handle = BLE_HS_CONN_HANDLE_NONE; /** * The vendor specific "bleuart" service consists of one write no-rsp characteristic - * and one notification only read charateristic + * and one notification only read characteristic * o "write no-rsp": a single-byte characteristic that can be written only * over a non-encrypted connection * o "read": a single-byte characteristic that can always be read only via @@ -110,7 +110,6 @@ gatt_svr_chr_access_uart_write(uint16_t conn_handle, uint16_t attr_handle, console_write("\n", 1); return 0; default: - assert(0); return BLE_ATT_ERR_UNLIKELY; } } @@ -127,16 +126,11 @@ bleuart_gatt_svr_init(void) int rc; rc = ble_gatts_count_cfg(gatt_svr_svcs); - if (rc != 0) { - goto err; - } - - rc = ble_gatts_add_svcs(gatt_svr_svcs); if (rc != 0) { return rc; } -err: + rc = ble_gatts_add_svcs(gatt_svr_svcs); return rc; } @@ -156,6 +150,7 @@ bleuart_uart_read(void) 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; } off += rc; @@ -200,7 +195,7 @@ bleuart_init(void) rc = console_init(bleuart_uart_read); SYSINIT_PANIC_ASSERT(rc == 0); - console_buf = nimble_platform_mem_calloc(1,MYNEWT_VAL(BLEUART_MAX_INPUT)); + console_buf = nimble_platform_mem_calloc(1, MYNEWT_VAL(BLEUART_MAX_INPUT)); SYSINIT_PANIC_ASSERT(console_buf != NULL); } #endif diff --git a/nimble/host/services/cte/src/ble_svc_cte.c b/nimble/host/services/cte/src/ble_svc_cte.c index f751469fc..852b07177 100644 --- a/nimble/host/services/cte/src/ble_svc_cte.c +++ b/nimble/host/services/cte/src/ble_svc_cte.c @@ -86,14 +86,14 @@ typedef struct { uint8_t cte_transmit_duration; uint16_t cte_interval; uint8_t cte_phy; -}cte_instance_config_t; +} cte_instance_config_t; /* Structure to store CTE settings per client */ static cte_instance_config_t cte_config[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; /** - * @brief Constant Tone Extensio Service UUID + * @brief Constant Tone Extension Service UUID */ static const ble_uuid128_t cte_svc_uuid = BLE_UUID128_INIT(0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00, 0x00, 0x80, @@ -117,7 +117,7 @@ static int ble_svc_cte_adv_cte_interval_access(uint16_t conn_handle, uint16_t at static int ble_svc_cte_adv_cte_phy_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); /** - * @brief Constant Tone Extensio Service definition + * @brief Constant Tone Extension Service definition */ static const ble_uuid16_t uuid_svc_cte = BLE_UUID16_INIT(BLE_SVC_CTE_UUID16); @@ -172,7 +172,7 @@ static const struct ble_gatt_chr_def cte_characteristics[] = { static const struct ble_gatt_svc_def ble_svc_cte_defs[] = { { - /*** Service: Asset Tracking Service. */ + /*** Service: Constant Tone Extension Service. */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = &uuid_svc_cte.u, .characteristics = cte_characteristics, @@ -319,9 +319,9 @@ ble_svc_cte_two_octet_chr_write(struct os_mbuf *om, /** - * @brief Access callback for Constant Tone Extensio Service cte enable + * @brief Access callback for Constant Tone Extension Service cte enable * - * This function is called when a read or write operation is performed on the Constant Tone Extensio + * This function is called when a read or write operation is performed on the Constant Tone Extension * characteristic. It handles the read and write requests. * * @param conn_handle The connection handle @@ -352,26 +352,38 @@ static int ble_svc_cte_enable_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_GATT_ACCESS_OP_WRITE_CHR: // Handle write characteristic request - rc = ble_svc_cte_one_octet_chr_write(ctxt->om, - CTE_ENABLE_MIN_VALUE, - CTE_ENABLE_MAX_VALUE, - &config->cte_enable, NULL); + { + uint8_t old_enable = config->cte_enable; + rc = ble_svc_cte_one_octet_chr_write(ctxt->om, + CTE_ENABLE_MIN_VALUE, + CTE_ENABLE_MAX_VALUE, + &config->cte_enable, NULL); - if (rc == 0) { - if((config->cte_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) { - if(ble_gap_set_conn_cte_transmit_param(conn_handle, BLE_GAP_CTE_RSP_ALLOW_AOA_MASK, 0, NULL) != 0) - { - rc = 0xFC; - break; + if (rc == 0) { + if(((config->cte_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) && + ((old_enable & CTE_ENABLE_AOA_CONNECTION) != CTE_ENABLE_AOA_CONNECTION)) { + 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; + break; + } + if(ble_gap_conn_cte_rsp_enable(conn_handle, true) != 0) { + config->cte_enable = old_enable; + rc = 0xFC; + break; + } + } else if ((old_enable & CTE_ENABLE_AOA_CONNECTION) == CTE_ENABLE_AOA_CONNECTION) { + if(ble_gap_conn_cte_rsp_enable(conn_handle, false) != 0) { + config->cte_enable = old_enable; + rc = 0xFC; + break; + } } - if(ble_gap_conn_cte_rsp_enable(conn_handle, true) != 0) { - rc = 0xFC; - break; + if((config->cte_enable & CTE_ENABLE_AOD_ADVERTISING) == CTE_ENABLE_AOD_ADVERTISING) { + // TODO: Add Start advertising with CTE } - } - if((config->cte_enable & CTE_ENABLE_AOD_ADVERTISING) == CTE_ENABLE_AOD_ADVERTISING) { - // TODO: Add Start advertising with CTE - } + } } break; @@ -551,9 +563,47 @@ static int ble_svc_cte_adv_cte_phy_access(uint16_t conn_handle, uint16_t attr_ha return rc; } +static struct ble_gap_event_listener cte_gap_listener; + +static int +cte_gap_event(struct ble_gap_event *event, void *arg) +{ + int i; + + switch (event->type) { + case BLE_GAP_EVENT_CONNECT: + if (event->connect.status == 0) { + for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { + if (cte_config[i].conn_handle == 0xffff) { + cte_config[i].conn_handle = event->connect.conn_handle; + break; + } + } + } + break; + + case BLE_GAP_EVENT_DISCONNECT: + for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { + if (cte_config[i].conn_handle == event->disconnect.conn.conn_handle) { + cte_config[i].conn_handle = 0xffff; + cte_config[i].cte_enable = 0; + cte_config[i].cte_min_length = CTE_MIN_LEN_MIN_VALUE; + cte_config[i].cte_min_transmit_count = CTE_MIN_TX_COUNT_MIN_VALUE; + cte_config[i].cte_transmit_duration = CTE_TX_DURATION_MIN_VALUE; + cte_config[i].cte_interval = CTE_INTERVAL_MIN_VALUE; + cte_config[i].cte_phy = CTE_PHY_MIN_VALUE; + break; + } + } + break; + } + + return 0; +} + static void cte_init_config(void) { for (int i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { - cte_config[i].conn_handle = i; + cte_config[i].conn_handle = 0xffff; cte_config[i].cte_enable = 0; cte_config[i].cte_min_length = CTE_MIN_LEN_MIN_VALUE; cte_config[i].cte_min_transmit_count = CTE_MIN_TX_COUNT_MIN_VALUE; @@ -564,13 +614,14 @@ static void cte_init_config(void) { } /** - * @brief Initialize the Constant Tone Extensio Service + * @brief Initialize the Constant Tone Extension Service */ void ble_svc_cte_init(void) { int rc; cte_init_config(); + ble_gap_event_listener_register(&cte_gap_listener, cte_gap_event, NULL); rc = ble_gatts_count_cfg(ble_svc_cte_defs); assert(rc == 0); diff --git a/nimble/host/services/cts/src/ble_svc_cts.c b/nimble/host/services/cts/src/ble_svc_cts.c index 95d5559ec..4643f1001 100644 --- a/nimble/host/services/cts/src/ble_svc_cts.c +++ b/nimble/host/services/cts/src/ble_svc_cts.c @@ -29,17 +29,17 @@ #include #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_CTS_SERVICE -struct ble_svc_cts_cfg cts_cfg = {0}; +static struct ble_svc_cts_cfg cts_cfg = {0}; /* characteristic values */ -struct ble_svc_cts_curr_time current_local_time_val; -struct ble_svc_cts_local_time_info local_time_info_val; -struct ble_svc_cts_reference_time_info ref_time_info_val; +static struct ble_svc_cts_curr_time current_local_time_val; +static struct ble_svc_cts_local_time_info local_time_info_val; +static struct ble_svc_cts_reference_time_info ref_time_info_val; /* Characteristic value handles */ -uint16_t ble_svc_cts_curr_time_handle; -uint16_t ble_svc_cts_local_time_info_handle; -uint16_t ble_svc_cts_ref_time_handle; +static uint16_t ble_svc_cts_curr_time_handle; +static uint16_t ble_svc_cts_local_time_info_handle; +static uint16_t ble_svc_cts_ref_time_handle; /* Access function */ static int @@ -56,24 +56,24 @@ static const struct ble_gatt_chr_def cts_characteristics[] = { /*** Current Time characteristic */ .uuid = &uuid_chr_current_time.u, .access_cb = ble_svc_cts_access, - .val_handle = &ble_svc_cts_curr_time_handle, + .val_handle = &ble_svc_cts_curr_time_handle, .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | /* optional */ - BLE_GATT_CHR_F_NOTIFY - }, { + BLE_GATT_CHR_F_NOTIFY + }, { /*** Local info characteristic */ .uuid = &uuid_chr_loc_time_info.u, .access_cb = ble_svc_cts_access, .val_handle = &ble_svc_cts_local_time_info_handle, .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE - }, { + }, { /*** Reference time info Characteristic */ .uuid = &uuid_chr_ref_time_info.u, .access_cb = ble_svc_cts_access, .val_handle = &ble_svc_cts_ref_time_handle, .flags = BLE_GATT_CHR_F_READ - }, { + }, { 0, /* No more characteristics in this service. */ }, }; @@ -173,31 +173,30 @@ ble_svc_cts_access(uint16_t conn_handle, uint16_t attr_handle, return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; case BLE_GATT_ACCESS_OP_WRITE_CHR: - rc = ble_hs_mbuf_to_flat(ctxt->om, &curr_time, sizeof curr_time, NULL); - assert(rc == 0); + rc = ble_svc_cts_chr_write(ctxt->om, sizeof(curr_time), sizeof(curr_time), &curr_time, NULL); + if (rc != 0) { + return rc == BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN ? rc : BLE_ATT_ERR_UNLIKELY; + } rc = ble_svc_cts_curr_time_validate(curr_time); if(rc != 0) { return rc; } - rc = ble_svc_cts_chr_write(ctxt->om, 0, sizeof current_local_time_val, ¤t_local_time_val, NULL); - if(rc != 0) { - return BLE_ATT_ERR_INSUFFICIENT_RES; - } - if(cts_cfg.set_time_cb == NULL) { return BLE_ATT_ERR_UNLIKELY; } + memcpy(¤t_local_time_val, &curr_time, sizeof(current_local_time_val)); rc = cts_cfg.set_time_cb(curr_time); if(rc != 0) { return BLE_ATT_ERR_UNLIKELY; } /* schedule notifications for subscribed peers */ - if(rc == 0) { - ble_gatts_chr_updated(attr_handle); - } + ble_gatts_chr_updated(attr_handle); return 0; + default: + return BLE_ATT_ERR_UNLIKELY; } + break; case BLE_SVC_CTS_CHR_UUID16_LOCAL_TIME_INFO: assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR || ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR); @@ -217,16 +216,14 @@ ble_svc_cts_access(uint16_t conn_handle, uint16_t attr_handle, return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; case BLE_GATT_ACCESS_OP_WRITE_CHR: - rc = ble_hs_mbuf_to_flat(ctxt->om, &local_time_info, 2, NULL); - assert(rc == 0); + rc = ble_svc_cts_chr_write(ctxt->om, sizeof(local_time_info), sizeof(local_time_info), &local_time_info, NULL); + if (rc != 0) { + return rc == BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN ? rc : BLE_ATT_ERR_UNLIKELY; + } rc = ble_svc_cts_local_time_info_validate(local_time_info); if(rc != 0) { return rc; } - rc = ble_svc_cts_chr_write(ctxt->om, 0, sizeof local_time_info_val, &local_time_info_val, NULL); - if(rc != 0) { - return BLE_ATT_ERR_INSUFFICIENT_RES; - } if(cts_cfg.set_local_time_info_cb == NULL) { return BLE_ATT_ERR_UNLIKELY; @@ -236,13 +233,17 @@ ble_svc_cts_access(uint16_t conn_handle, uint16_t attr_handle, if(rc != 0) { return BLE_ATT_ERR_UNLIKELY; } - if(rc == 0) { - /* set the adjust reason mask */ - /* notify the connected clients about the change in timezone and time */ - ble_gatts_chr_updated(ble_svc_cts_curr_time_handle); - } + + memcpy(&local_time_info_val, &local_time_info, sizeof(local_time_info_val)); + /* set the adjust reason mask (Time Zone and DST change) */ + current_local_time_val.adjust_reason |= (1 << 2) | (1 << 3); + /* notify the connected clients about the change in timezone and time */ + ble_gatts_chr_updated(ble_svc_cts_curr_time_handle); return 0; + default: + return BLE_ATT_ERR_UNLIKELY; } + break; case BLE_SVC_CTS_CHR_UUID16_REF_TIME_INFO: assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); if(cts_cfg.ref_time_info_cb != NULL) { diff --git a/nimble/host/services/dis/src/ble_svc_dis.c b/nimble/host/services/dis/src/ble_svc_dis.c index c8c5e97ce..5161fad51 100644 --- a/nimble/host/services/dis/src/ble_svc_dis.c +++ b/nimble/host/services/dis/src/ble_svc_dis.c @@ -200,7 +200,7 @@ ble_svc_dis_access(uint16_t conn_handle, uint16_t attr_handle, #if (MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_READ_PERM) >= 0) case BLE_SVC_DIS_CHR_UUID16_MODEL_NUMBER: info = ble_svc_dis_data.model_number; -#ifdef MYNEWT_VAL_BLE_SVC_DIS_MODEL_NUMBER_NAME_DEFAULT +#ifdef MYNEWT_VAL_BLE_SVC_DIS_MODEL_NUMBER_DEFAULT if (info == NULL) { info = MYNEWT_VAL(BLE_SVC_DIS_MODEL_NUMBER_DEFAULT); } @@ -265,19 +265,43 @@ ble_svc_dis_access(uint16_t conn_handle, uint16_t attr_handle, info = MYNEWT_VAL(BLE_SVC_DIS_SYSTEM_ID_DEFAULT); } #endif - break; + { + if (info == NULL) { + uint8_t zeroed_sysid[8] = {0}; + int rc = os_mbuf_append(ctxt->om, zeroed_sysid, 8); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + /* Ensure we don't over-read if the provided data is shorter than 8 bytes */ + uint8_t sysid[8] = {0}; + size_t len = strlen(info); + if (len > 8) len = 8; + memcpy(sysid, info, len); + int rc = os_mbuf_append(ctxt->om, sysid, 8); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } #endif #if (MYNEWT_VAL(BLE_SVC_DIS_PNP_ID_READ_PERM) >= 0) case BLE_SVC_DIS_CHR_UUID16_PNP_ID: info = ble_svc_dis_data.pnp_id; -#ifdef MYNEWT_VAL_BLE_SVC_PNP_SYSTEM_ID_DEFAULT +#ifdef MYNEWT_VAL_BLE_SVC_DIS_PNP_ID_DEFAULT if (info == NULL) { - info = MYNEWT_VAL(BLE_SVC_PNP_SYSTEM_ID_DEFAULT); + info = MYNEWT_VAL(BLE_SVC_DIS_PNP_ID_DEFAULT); } #endif - uint8_t flag = 0x01; - os_mbuf_append(ctxt->om, &flag, sizeof flag); - break; + { + if (info == NULL) { + uint8_t zeroed_pnpid[7] = {0}; + int rc = os_mbuf_append(ctxt->om, zeroed_pnpid, 7); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + /* Ensure we don't over-read if the provided data is shorter than 7 bytes */ + uint8_t pnpid[7] = {0}; + size_t len = strlen(info); + if (len > 7) len = 7; + memcpy(pnpid, info, len); + int rc = os_mbuf_append(ctxt->om, pnpid, 7); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } #endif case BLE_SVC_DIS_CHR_UUID16_IEEE_REG_CERT_LIST: info = ble_svc_dis_data.ieee; @@ -287,12 +311,12 @@ ble_svc_dis_access(uint16_t conn_handle, uint16_t attr_handle, info = ble_svc_dis_data.udi; if (info == NULL) { uint8_t flag = 0x00; - os_mbuf_append(ctxt->om, &flag, sizeof(flag)); + int rc = os_mbuf_append(ctxt->om, &flag, sizeof(flag)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } break; default: - assert(0); return BLE_ATT_ERR_UNLIKELY; } @@ -453,6 +477,12 @@ void ble_svc_dis_deinit(void) { ble_gatts_free_svcs(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_dis_data_ptr != NULL) { + nimble_platform_mem_free(ble_svc_dis_data_ptr); + ble_svc_dis_data_ptr = NULL; + } +#endif } /** @@ -466,7 +496,7 @@ ble_svc_dis_init(void) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_svc_dis_data_ptr != NULL) { nimble_platform_mem_free(ble_svc_dis_data_ptr); - ble_svc_dis_data_ptr = NULL; + ble_svc_dis_data_ptr = NULL; } rc = ble_svc_dis_init_dynamic(); diff --git a/nimble/host/services/gap/src/ble_svc_gap.c b/nimble/host/services/gap/src/ble_svc_gap.c index fb8a0881e..1749a2053 100644 --- a/nimble/host/services/gap/src/ble_svc_gap.c +++ b/nimble/host/services/gap/src/ble_svc_gap.c @@ -29,11 +29,11 @@ #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_GAP_SERVICE #define PPCP_ENABLED \ - MYNEWT_VAL(BLE_ROLE_PERIPHERAL) && \ + (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) && \ (MYNEWT_VAL(BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL) || \ MYNEWT_VAL(BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL) || \ MYNEWT_VAL(BLE_SVC_GAP_PPCP_SLAVE_LATENCY) || \ - MYNEWT_VAL(BLE_SVC_GAP_PPCP_SUPERVISION_TMO)) + MYNEWT_VAL(BLE_SVC_GAP_PPCP_SUPERVISION_TMO))) #define BLE_SVC_GAP_NAME_MAX_LEN \ MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME_MAX_LENGTH) @@ -56,7 +56,7 @@ ble_hs_gap_svc_ctx_t *ble_hs_gap_svc_ctx = NULL; #define ble_svc_gap_name (ble_hs_gap_svc_ctx->svc_gap_name) #define ble_svc_gap_appearance (ble_hs_gap_svc_ctx->svc_gap_appearance) #if MYNEWT_VAL(ENC_ADV_DATA) -#define km (ble_hs_gap_svc_ctx->km) +#define ble_svc_gap_km (ble_hs_gap_svc_ctx->km) #endif // ENC_ADV_DATA #if MYNEWT_VAL(ENC_ADV_DATA) static uint16_t ble_svc_gap_enc_adv_data_handle; @@ -69,7 +69,7 @@ static char ble_svc_gap_name[BLE_SVC_GAP_NAME_MAX_LEN + 1] = #if MYNEWT_VAL(ENC_ADV_DATA) static uint16_t ble_svc_gap_enc_adv_data_handle; -static struct key_material km = { +static struct key_material ble_svc_gap_km = { .session_key = {0}, .iv = {0}, }; @@ -77,7 +77,7 @@ static struct key_material km = { #endif // BLE_STATIC_TO_DYNAMIC #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -int ble_svc_gap_appearance_init(void); +static int ble_svc_gap_appearance_init(void); #endif #if NIMBLE_BLE_CONNECT @@ -186,14 +186,27 @@ static const struct ble_gatt_svc_def ble_svc_gap_defs[] = { static int ble_svc_gap_device_name_read_access(struct ble_gatt_access_ctxt *ctxt) { + const char *name; + size_t name_len; + size_t read_len; int rc; #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - rc = os_mbuf_append(ctxt->om, ble_svc_gap_name ? ble_svc_gap_name : "", - strlen(ble_svc_gap_name ? ble_svc_gap_name : "")); + if (!ble_hs_gap_svc_ctx || !ble_hs_gap_svc_ctx->svc_gap_name) { + name = ""; + } else { + name = ble_svc_gap_name; + } #else - rc = os_mbuf_append(ctxt->om, ble_svc_gap_name, strlen(ble_svc_gap_name)); + name = ble_svc_gap_name; #endif + name_len = strlen(name); + if (ctxt->offset > name_len) { + return BLE_ATT_ERR_INVALID_OFFSET; + } + read_len = name_len - ctxt->offset; + + rc = os_mbuf_append(ctxt->om, name + ctxt->offset, read_len); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } @@ -214,6 +227,9 @@ ble_svc_gap_device_name_write_access(struct ble_gatt_access_ctxt *ctxt) } #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_gap_svc_ctx) { + return BLE_ATT_ERR_UNLIKELY; + } // Free old name if already allocated if (ble_hs_gap_svc_ctx->svc_gap_name) { nimble_platform_mem_free(ble_hs_gap_svc_ctx->svc_gap_name); @@ -377,8 +393,12 @@ ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle, #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_SVC_GAP_CHR_UUID16_KEY_MATERIAL: - rc = os_mbuf_append(ctxt->om, &(km.session_key), sizeof(km.session_key)); - rc = os_mbuf_append(ctxt->om, &(km.iv), sizeof(km.iv)); + assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); + rc = os_mbuf_append(ctxt->om, &(ble_svc_gap_km.session_key), sizeof(ble_svc_gap_km.session_key)); + if (rc != 0) { + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + rc = os_mbuf_append(ctxt->om, &(ble_svc_gap_km.iv), sizeof(ble_svc_gap_km.iv)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; #endif @@ -400,6 +420,11 @@ ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle, const char * ble_svc_gap_device_name(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (!ble_hs_gap_svc_ctx || !ble_hs_gap_svc_ctx->svc_gap_name) { + return ""; + } +#endif return ble_svc_gap_name; } @@ -408,6 +433,10 @@ ble_svc_gap_device_name_set(const char *name) { int len; + if (!name) { + return BLE_HS_EINVAL; + } + len = strlen(name); if (len > BLE_SVC_GAP_NAME_MAX_LEN) { return BLE_HS_EINVAL; @@ -415,10 +444,10 @@ ble_svc_gap_device_name_set(const char *name) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_gap_svc_ctx == NULL) { - ble_hs_gap_svc_ctx = nimble_platform_mem_calloc ( 1, sizeof(*ble_hs_gap_svc_ctx)); + ble_hs_gap_svc_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_gap_svc_ctx)); - if (!ble_hs_gap_svc_ctx) { - return BLE_HS_ENOMEM; + if (!ble_hs_gap_svc_ctx) { + return BLE_HS_ENOMEM; } } // Free old name if already allocated @@ -442,11 +471,14 @@ uint16_t ble_svc_gap_device_appearance(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (!ble_hs_gap_svc_ctx) { - ble_svc_gap_appearance_init(); - } + if (!ble_hs_gap_svc_ctx) { + int rc = ble_svc_gap_appearance_init(); + if (rc != 0) { + return 0; /* Safe default on allocation failure */ + } + } #endif - return ble_svc_gap_appearance; + return ble_svc_gap_appearance; } int @@ -454,11 +486,14 @@ ble_svc_gap_device_appearance_set(uint16_t appearance) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (!ble_hs_gap_svc_ctx) { - ble_svc_gap_appearance_init(); - } + if (!ble_hs_gap_svc_ctx) { + int rc = ble_svc_gap_appearance_init(); + if (rc != 0) { + return rc; + } + } #endif - ble_svc_gap_appearance = appearance; + ble_svc_gap_appearance = appearance; return 0; } @@ -473,22 +508,25 @@ ble_svc_gap_set_chr_changed_cb(ble_svc_gap_chr_changed_fn *cb) int ble_svc_gap_device_key_material_set(uint8_t *session_key, uint8_t *iv) { - memcpy(&km.session_key, session_key, BLE_EAD_KEY_SIZE); - memcpy(&km.iv, iv, BLE_EAD_IV_SIZE); + if (!session_key || !iv) { + return BLE_HS_EINVAL; + } + 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); return 0; } #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -int +static int ble_svc_gap_appearance_init(void) { if (ble_hs_gap_svc_ctx == NULL) { - ble_hs_gap_svc_ctx = nimble_platform_mem_calloc ( 1, sizeof(*ble_hs_gap_svc_ctx)); + ble_hs_gap_svc_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_gap_svc_ctx)); - if (!ble_hs_gap_svc_ctx) { - return BLE_HS_ENOMEM; + if (!ble_hs_gap_svc_ctx) { + return BLE_HS_ENOMEM; } } @@ -497,9 +535,13 @@ ble_svc_gap_appearance_init(void) return 0; } -int +static int ble_svc_gap_init_name(void) { + if (!ble_hs_gap_svc_ctx) { + return BLE_HS_EINVAL; + } + const char *default_name = MYNEWT_VAL(BLE_SVC_GAP_DEVICE_NAME); size_t len = strlen(default_name); @@ -517,7 +559,7 @@ ble_svc_gap_init_name(void) return 0; } -void +static void ble_svc_gap_deinit_name(void) { if (!ble_hs_gap_svc_ctx) { @@ -563,10 +605,10 @@ ble_svc_gap_deinit(void) { ble_gatts_free_svcs(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_gap_svc_ctx ) { - ble_svc_gap_deinit_name(); + if (ble_hs_gap_svc_ctx) { + ble_svc_gap_deinit_name(); nimble_platform_mem_free(ble_hs_gap_svc_ctx); - ble_hs_gap_svc_ctx = NULL; + ble_hs_gap_svc_ctx = NULL; } #endif } diff --git a/nimble/host/services/gatt/src/ble_svc_gatt.c b/nimble/host/services/gatt/src/ble_svc_gatt.c index 7f2821ce7..a6cdfcfd8 100644 --- a/nimble/host/services/gatt/src/ble_svc_gatt.c +++ b/nimble/host/services/gatt/src/ble_svc_gatt.c @@ -37,7 +37,7 @@ static uint16_t ble_svc_gatt_end_handle; #define BLE_SVC_GATT_SRV_SUP_FEAT_EATT_BIT (0x00) /* Client supported features */ -#define BLE_SVC_GATT_CLI_SUP_FEAT_ROBUST_CATCHING_BIT (0x00) +#define BLE_SVC_GATT_CLI_SUP_FEAT_ROBUST_CACHING_BIT (0x00) #define BLE_SVC_GATT_CLI_SUP_FEAT_EATT_BIT (0x01) #define BLE_SVC_GATT_CLI_SUP_FEAT_MULT_NTF_BIT (0x02) @@ -66,7 +66,7 @@ static const ble_uuid16_t uuid_chr_db_hash = BLE_UUID16_INIT(BLE_SVC_GATT_CHR_DA static const struct ble_gatt_chr_def gatt_characteristics[] = { { - .uuid = &uuid_chr_svc_change.u, + .uuid = &uuid_chr_svc_change.u, .access_cb = ble_svc_gatt_access, .val_handle = &ble_svc_gatt_changed_val_handle, .flags = BLE_GATT_CHR_F_INDICATE, @@ -113,13 +113,15 @@ static int ble_svc_gatt_srv_sup_feat_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { + int rc; + if (ctxt->op != BLE_GATT_ACCESS_OP_READ_CHR) { return BLE_ATT_ERR_WRITE_NOT_PERMITTED; } - os_mbuf_append(ctxt->om, &ble_svc_gatt_local_srv_sup_feat, sizeof(ble_svc_gatt_local_srv_sup_feat)); + rc = os_mbuf_append(ctxt->om, &ble_svc_gatt_local_srv_sup_feat, sizeof(ble_svc_gatt_local_srv_sup_feat)); - return 0; + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } static int @@ -134,7 +136,10 @@ ble_svc_gatt_cl_sup_feat_access(uint16_t conn_handle, uint16_t attr_handle, if (rc != 0) { return BLE_ATT_ERR_UNLIKELY; } - os_mbuf_append(ctxt->om, &supported_feat, sizeof(supported_feat)); + rc = os_mbuf_append(ctxt->om, &supported_feat, sizeof(supported_feat)); + if (rc != 0) { + return BLE_ATT_ERR_UNLIKELY; + } return 0; } if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { @@ -161,7 +166,7 @@ ble_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle, u8p = os_mbuf_extend(ctxt->om, 4); if (u8p == NULL) { - return BLE_HS_ENOMEM; + return BLE_ATT_ERR_INSUFFICIENT_RES; } @@ -172,23 +177,22 @@ ble_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle, #else int rc; - if(ctxt->chr == &ble_svc_gatt_defs[0].characteristics[0]){ + if (ctxt->chr == &ble_svc_gatt_defs[0].characteristics[0]) { u8p = os_mbuf_extend(ctxt->om, 4); if (u8p == NULL) { - return BLE_HS_ENOMEM; + return BLE_ATT_ERR_INSUFFICIENT_RES; } put_le16(u8p + 0, ble_svc_gatt_start_handle); put_le16(u8p + 2, ble_svc_gatt_end_handle); - } - if (attr_handle == ble_svc_gatt_db_hash_handle) { + } else if (attr_handle == ble_svc_gatt_db_hash_handle) { assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); uint8_t database_hash[16] = {0}; rc = ble_gatts_calculate_hash(database_hash); - if(rc != 0) { + if (rc != 0) { return BLE_ATT_ERR_UNLIKELY; } rc = os_mbuf_append(ctxt->om, database_hash, sizeof(database_hash)); - if(rc != 0) { + if (rc != 0) { return BLE_ATT_ERR_UNLIKELY; } } @@ -219,14 +223,14 @@ ble_svc_gatt_changed(uint16_t start_handle, uint16_t end_handle) } #if MYNEWT_VAL(BLE_GATT_CACHING) -uint16_t ble_svc_gatt_changed_handle() { +uint16_t ble_svc_gatt_changed_handle(void) { return ble_svc_gatt_changed_val_handle; } -uint16_t ble_svc_gatt_hash_handle() { +uint16_t ble_svc_gatt_hash_handle(void) { return ble_svc_gatt_db_hash_handle; } -uint16_t ble_svc_gatt_csf_handle() { +uint16_t ble_svc_gatt_csf_handle(void) { return ble_svc_gatt_client_supp_feature_handle; } #endif @@ -258,7 +262,7 @@ ble_svc_gatt_init(void) } if (MYNEWT_VAL(BLE_GATT_CACHING) > 0) { - ble_svc_gatt_local_cl_sup_feat |= (1 << BLE_SVC_GATT_CLI_SUP_FEAT_ROBUST_CATCHING_BIT); + ble_svc_gatt_local_cl_sup_feat |= (1 << BLE_SVC_GATT_CLI_SUP_FEAT_ROBUST_CACHING_BIT); } } diff --git a/nimble/host/services/hid/include/services/hid/ble_svc_hid.h b/nimble/host/services/hid/include/services/hid/ble_svc_hid.h index c0d11cc07..2a39241d4 100644 --- a/nimble/host/services/hid/include/services/hid/ble_svc_hid.h +++ b/nimble/host/services/hid/include/services/hid/ble_svc_hid.h @@ -99,7 +99,7 @@ struct ble_svc_hid_params{ uint8_t report_map[REPORT_MAP_SIZE]; uint16_t report_map_handle; uint16_t external_rpt_ref; - uint8_t report_map_len; + uint16_t report_map_len; /* hid info char */ uint32_t hid_info; diff --git a/nimble/host/services/hid/src/ble_svc_hid.c b/nimble/host/services/hid/src/ble_svc_hid.c index 3c0a6a836..840fc4c20 100644 --- a/nimble/host/services/hid/src/ble_svc_hid.c +++ b/nimble/host/services/hid/src/ble_svc_hid.c @@ -38,7 +38,9 @@ /* maximum 7 characteristics except report characteristic */ #define HID_MAX_CHRS (HID_MAX_SVC_INSTANCES * \ - ((MYNEWT_VAL(BLE_SVC_HID_MAX_RPTS) + 7))) + ((MYNEWT_VAL(BLE_SVC_HID_MAX_RPTS) + 8))) +#define HID_MAX_DSCS (HID_MAX_SVC_INSTANCES * \ + ((2 * MYNEWT_VAL(BLE_SVC_HID_MAX_RPTS) + 2))) /* 16 bit UUIDs */ static const ble_uuid16_t uuid_ext_rpt_ref = BLE_UUID16_INIT(BLE_SVC_HID_DSC_UUID16_EXT_RPT_REF); static const ble_uuid16_t uuid_report_map = BLE_UUID16_INIT(BLE_SVC_HID_CHR_UUID16_REPORT_MAP); @@ -54,23 +56,23 @@ static const ble_uuid16_t uuid_hid_svc = BLE_UUID16_INIT(BLE_SVC_HID_UUID16); #if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) static struct ble_svc_hid_params hid_instances[HID_MAX_SVC_INSTANCES]; -static uint8_t ble_svc_hid_dsc_index = 0; // used to store the current index in the dscs array -static struct ble_gatt_dsc_def ble_svc_hid_dscs[HID_MAX_CHRS]; +static uint16_t ble_svc_hid_dsc_index = 0; // used to store the current index in the dscs array +static struct ble_gatt_dsc_def ble_svc_hid_dscs[HID_MAX_DSCS]; static struct ble_gatt_chr_def ble_svc_hid_chrs[HID_MAX_CHRS]; static struct ble_gatt_svc_def ble_svc_hid_defs[HID_MAX_SVC_INSTANCES]; -static uint8_t ble_svc_hid_chr_index = 0; // used to store the current index in the chrs array -static uint8_t ble_svc_hid_svc_index = 0; // used to store the current index in the svcs array +static uint16_t ble_svc_hid_chr_index = 0; // used to store the current index in the chrs array +static uint16_t ble_svc_hid_svc_index = 0; // used to store the current index in the svcs array #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) typedef struct { struct ble_svc_hid_params _hid_instances[HID_MAX_SVC_INSTANCES]; - uint8_t _ble_svc_hid_dsc_index; - struct ble_gatt_dsc_def _ble_svc_hid_dscs[HID_MAX_CHRS]; + uint16_t _ble_svc_hid_dsc_index; + struct ble_gatt_dsc_def _ble_svc_hid_dscs[HID_MAX_DSCS]; struct ble_gatt_chr_def _ble_svc_hid_chrs[HID_MAX_CHRS]; struct ble_gatt_svc_def _ble_svc_hid_defs[HID_MAX_SVC_INSTANCES]; - uint8_t _ble_svc_hid_chr_index; - uint8_t _ble_svc_hid_svc_index; + uint16_t _ble_svc_hid_chr_index; + uint16_t _ble_svc_hid_svc_index; } ble_svc_hid_static_vars_t; static ble_svc_hid_static_vars_t * ble_svc_hid_static_vars = NULL; @@ -114,7 +116,7 @@ ble_svc_hid_chr_write(struct os_mbuf *om, uint16_t min_len, static struct ble_gatt_dsc_def* ble_svc_hid_get_dsc(uint8_t num) { - if (ble_svc_hid_dsc_index + num - 1 >= HID_MAX_CHRS) { + if (ble_svc_hid_dsc_index + num - 1 >= HID_MAX_DSCS) { return NULL; } ble_svc_hid_dsc_index = ble_svc_hid_dsc_index + num; @@ -160,7 +162,7 @@ find_rpt_by_handle(uint16_t handle) } } } - /* return some non-zero value */ + /* Report not found */ return NULL; } @@ -175,16 +177,16 @@ ble_svc_get_svc_block(void) } /* fill protocol mode char */ -static void +static int fill_proto_mode(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; if (!hid_instances[instance].proto_mode_present) { - return; + return 0; } demo_chr = (struct ble_gatt_chr_def) { - /*** Report Map characteristic */ + /*** Protocol Mode characteristic */ .uuid = &uuid_proto_mode.u, .access_cb = ble_svc_hid_access, .val_handle = &hid_instances[instance].proto_mode_handle, @@ -199,31 +201,35 @@ fill_proto_mode(uint8_t instance) }; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return BLE_HS_ENOMEM; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); + return 0; } /* fill boot keyboard inp char */ -void +static int fill_boot_kbd_inp(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; uint16_t write_flags; if (!hid_instances[instance].kbd_inp_present) { - return; + return 0; } write_flags = BLE_GATT_CHR_F_WRITE | #if MYNEWT_VAL(BLE_SM_SC_LVL) == 2 BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_SC_LVL) == 3 - BLE_GATT_CHR_F_WRITE_ENC | BLE_GATT_CHR_F_WRITE_AUTHEN +#elif MYNEWT_VAL(BLE_SM_LVL) == 3 + BLE_GATT_CHR_F_WRITE_ENC | BLE_GATT_CHR_F_WRITE_AUTHEN | #endif 0; write_flags = (hid_instances[instance].kbd_inp_write_perm ? write_flags : 0); demo_chr = (struct ble_gatt_chr_def) { - /*** Report Map characteristic */ + /*** Boot Keyboard Input characteristic */ .uuid = &uuid_boot_kbd_inp.u, .access_cb = ble_svc_hid_access, .val_handle = &hid_instances[instance].kbd_inp_handle, @@ -238,20 +244,24 @@ fill_boot_kbd_inp(uint8_t instance) }; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return BLE_HS_ENOMEM; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); + return 0; } /* fill boot keyboard out char */ -void +static int fill_boot_kbd_out(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; if (!hid_instances[instance].kbd_out_present) { - return; + return 0; } demo_chr = (struct ble_gatt_chr_def) { - /*** Report Map characteristic */ + /*** Boot Keyboard Output characteristic */ .uuid = &uuid_boot_kbd_out.u, .access_cb = ble_svc_hid_access, .val_handle = &hid_instances[instance].kbd_out_handle, @@ -267,10 +277,14 @@ fill_boot_kbd_out(uint8_t instance) }; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return BLE_HS_ENOMEM; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); + return 0; } /* fill boot mouse inp char */ -void +static void fill_boot_mouse_inp(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; @@ -283,14 +297,14 @@ fill_boot_mouse_inp(uint8_t instance) write_flags = BLE_GATT_CHR_F_WRITE | #if MYNEWT_VAL(BLE_SM_SC_LVL) == 2 BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_SC_LVL) == 3 - BLE_GATT_CHR_F_WRITE_ENC | BLE_GATT_CHR_F_WRITE_AUTHEN +#elif MYNEWT_VAL(BLE_SM_LVL) == 3 + BLE_GATT_CHR_F_WRITE_ENC | BLE_GATT_CHR_F_WRITE_AUTHEN | #endif 0; write_flags = (hid_instances[instance].mouse_inp_write_perm ? write_flags : 0); demo_chr = (struct ble_gatt_chr_def) { - /*** Report Map characteristic */ + /*** Boot Mouse Input characteristic */ .uuid = &uuid_boot_mouse_inp.u, .access_cb = ble_svc_hid_access, .val_handle = &hid_instances[instance].mouse_inp_handle, @@ -305,10 +319,13 @@ fill_boot_mouse_inp(uint8_t instance) }; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); } /* create report map char */ -static void +static int fill_rpt_map(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; @@ -341,15 +358,22 @@ fill_rpt_map(uint8_t instance) /* logic : allocate the discriptor needed and then allocate one more and assign 0 to it, this is done to indicate there are no more descriptors */ dsc = ble_svc_hid_get_dsc(2); + if (!dsc) { + return BLE_HS_ENOMEM; + } memcpy(dsc, demo_dsc, 2 * sizeof(struct ble_gatt_dsc_def)); - demo_chr.descriptors = dsc, + demo_chr.descriptors = dsc; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return BLE_HS_ENOMEM; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); + return 0; } /* create report chars */ -static void +static int fill_reports(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; @@ -375,18 +399,21 @@ fill_reports(uint8_t instance) /* logic : allocate the discriptor needed and then allocate one more and assign 0 to it, this is done to indicate there are no more descriptors */ dsc = ble_svc_hid_get_dsc(2); + if (!dsc) { + return BLE_HS_ENOMEM; + } demo_dsc[0].arg = &hid_instances[instance].rpts[i].handle; memcpy(dsc, demo_dsc, 2 * sizeof(struct ble_gatt_dsc_def)); switch (hid_instances[instance].rpts[i].type) { case BLE_SVC_HID_RPT_TYPE_INPUT: - demo_chr.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE; + demo_chr.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_WRITE; break; case BLE_SVC_HID_RPT_TYPE_OUTPUT: - demo_chr.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_WRITE; + demo_chr.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP; break; case BLE_SVC_HID_RPT_TYPE_FEATURE: - demo_chr.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE; + demo_chr.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE; break; default : assert(0); @@ -404,8 +431,13 @@ fill_reports(uint8_t instance) demo_chr.descriptors = dsc; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return BLE_HS_ENOMEM; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); } + + return 0; } static void @@ -428,6 +460,9 @@ fill_hid_info(uint8_t instance) 0, }; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); } @@ -451,6 +486,9 @@ fill_ctrl_pt(uint8_t instance) 0, }; chr = ble_svc_hid_get_chr_block(); + if (!chr) { + return; + } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); } @@ -458,12 +496,16 @@ fill_ctrl_pt(uint8_t instance) * allocating one more chr block with * value set to zero */ -static void +static int ble_svc_hid_end_chrs(void) { struct ble_gatt_chr_def *chr; chr = ble_svc_hid_get_chr_block(); + if (chr == NULL) { + return BLE_HS_ENOMEM; + } memset(chr, 0, sizeof(struct ble_gatt_chr_def)); + return 0; } /** * HID access function @@ -473,7 +515,12 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { - uint16_t uuid16 = ble_uuid_u16(ctxt->chr->uuid); + uint16_t uuid16; + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR || ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + uuid16 = ble_uuid_u16(ctxt->chr->uuid); + } else { + uuid16 = ble_uuid_u16(ctxt->dsc->uuid); + } int rc; struct report *rpt; uint16_t rpt_ref_val = 0; @@ -506,6 +553,9 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, because find_rpt_by_handle already checks for the instance */ assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC); rpt = find_rpt_by_handle(*(uint16_t *)ctxt->dsc->arg); + if (rpt == NULL) { + return BLE_ATT_ERR_ATTR_NOT_FOUND; + } rpt_ref_val = (0x00FF & rpt->id) | ((0x00FF & (uint16_t)rpt->type) << 8); /* check if this should be opposite */ rc = os_mbuf_append(ctxt->om, &rpt_ref_val, sizeof rpt_ref_val); @@ -613,13 +663,16 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_SVC_HID_CHR_UUID16_RPT: /* this will work without any check of instance */ rpt = find_rpt_by_handle(*(ctxt->chr->val_handle)); + if (rpt == NULL) { + return BLE_ATT_ERR_ATTR_NOT_FOUND; + } assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR || ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR); if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { rc = os_mbuf_append(ctxt->om, rpt->data, rpt->len); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = ble_svc_hid_chr_write(ctxt->om, 0, RPT_MAX_LEN, &(rpt->data), &out_rpt_len); + rc = ble_svc_hid_chr_write(ctxt->om, 0, RPT_MAX_LEN, rpt->data, &out_rpt_len); if (rc != 0) { return BLE_ATT_ERR_INSUFFICIENT_RES; } @@ -669,21 +722,41 @@ ble_svc_hid_add(struct ble_svc_hid_params params) goto error; } + if (params.rpts_len > MYNEWT_VAL(BLE_SVC_HID_MAX_RPTS)) { + rc = BLE_HS_EINVAL; + goto error; + } + memcpy(&hid_instances[svc_idx], ¶ms, sizeof(struct ble_svc_hid_params)); /* get the pointer to the first characteristic */ chr_idx = ble_svc_hid_get_curr_chr_idx(); /* Fill protocol mode characteristic */ - fill_proto_mode(svc_idx); + rc = fill_proto_mode(svc_idx); + if (rc != 0) { + goto error; + } /* Fill report map characteristic */ - fill_rpt_map(svc_idx); + rc = fill_rpt_map(svc_idx); + if (rc != 0) { + goto error; + } /* Fill report characteristics */ - fill_reports(svc_idx); + rc = fill_reports(svc_idx); + if (rc !=0 ) { + goto error; + } /* Fill the boot keyboard input characteristic */ - fill_boot_kbd_inp(svc_idx); + rc = fill_boot_kbd_inp(svc_idx); + if (rc != 0) { + goto error; + } /* Fill the boot keyboard output characteristic */ - fill_boot_kbd_out(svc_idx); + rc = fill_boot_kbd_out(svc_idx); + if (rc != 0) { + goto error; + } /* Fill the boot mouse input characteristic */ fill_boot_mouse_inp(svc_idx); /* Fill the hid info characteristic */ @@ -691,9 +764,16 @@ ble_svc_hid_add(struct ble_svc_hid_params params) /* Fill the control point characteristic */ fill_ctrl_pt(svc_idx); /* End the characteristics with the characteristic with empty block */ - ble_svc_hid_end_chrs(); + rc = ble_svc_hid_end_chrs(); + if (rc != 0){ + goto error; + } svc = ble_svc_get_svc_block(); + if (svc == NULL) { + rc = BLE_HS_ENOMEM; + goto error; + } svc->type = BLE_GATT_SVC_TYPE_PRIMARY; svc->uuid = &uuid_hid_svc.u; svc->characteristics = ble_svc_hid_chrs + chr_idx; diff --git a/nimble/host/services/hr/src/ble_svc_hr.c b/nimble/host/services/hr/src/ble_svc_hr.c index 4c1aefaa7..5fdfbfc41 100644 --- a/nimble/host/services/hr/src/ble_svc_hr.c +++ b/nimble/host/services/hr/src/ble_svc_hr.c @@ -16,7 +16,7 @@ #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_HR_SERVICE /* Characteristic values */ static uint8_t ble_svc_hr_measurement; -static uint16_t ble_svc_hr_body_sensor_loc; +static uint8_t ble_svc_hr_body_sensor_loc; static uint8_t ble_svc_hr_ctrl_pt; /* Characteristic value handles */ @@ -30,7 +30,7 @@ static int ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); -int ble_svc_hr_notify_measurement(void); +static 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, @@ -71,7 +71,7 @@ static const struct ble_gatt_chr_def hr_characteristics[] = { .uuid = &uuid_chr_hr_cp.u, .access_cb = ble_svc_hr_access, .val_handle = &ble_svc_hr_ctrl_pt_val_handle, - .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE, + .flags = BLE_GATT_CHR_F_WRITE, }, { 0, /* No more characteristics in this service. */ } @@ -117,18 +117,13 @@ ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_SVC_HR_CHR_UUID16_CTRL_PT: if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = ble_svc_hr_chr_write(ctxt->om, 0, sizeof(ble_svc_hr_ctrl_pt), + rc = ble_svc_hr_chr_write(ctxt->om, sizeof(ble_svc_hr_ctrl_pt), sizeof(ble_svc_hr_ctrl_pt), &ble_svc_hr_ctrl_pt, NULL); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - rc = os_mbuf_append(ctxt->om, &ble_svc_hr_ctrl_pt, - sizeof(ble_svc_hr_ctrl_pt)); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } else { - return BLE_SVC_HS_ERR_CMD_NOT_SUPPORTED; } + return BLE_SVC_HS_ERR_CMD_NOT_SUPPORTED; default: assert(0); @@ -147,7 +142,9 @@ ble_svc_hr_access(uint16_t conn_handle, uint16_t attr_handle, void ble_svc_hr_on_gap_connect(uint16_t conn_handle) { - ble_svc_hr_conn_handle[conn_handle] = conn_handle; + if (conn_handle <= MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + ble_svc_hr_conn_handle[conn_handle] = conn_handle; + } } /** @@ -160,7 +157,9 @@ ble_svc_hr_on_gap_connect(uint16_t conn_handle) void ble_svc_hr_on_gap_disconnect(uint16_t conn_handle) { - ble_svc_hr_conn_handle[conn_handle] = -1; + if (conn_handle <= MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + ble_svc_hr_conn_handle[conn_handle] = -1; + } } /** @@ -180,7 +179,7 @@ ble_svc_hr_notify_measurement(void) txom = ble_hs_mbuf_from_flat(&ble_svc_hr_measurement, sizeof(ble_svc_hr_measurement)); if (!txom) { - return ESP_FAIL; + return BLE_HS_ENOMEM; } rc = ble_gatts_notify_custom(ble_svc_hr_conn_handle[i], diff --git a/nimble/host/services/htp/src/ble_svc_htp.c b/nimble/host/services/htp/src/ble_svc_htp.c index 7e1413ba8..73f09f1c5 100644 --- a/nimble/host/services/htp/src/ble_svc_htp.c +++ b/nimble/host/services/htp/src/ble_svc_htp.c @@ -38,7 +38,7 @@ ble_svc_htp_chr_write(struct os_mbuf *om, uint16_t min_len, static const struct ble_gatt_svc_def ble_svc_htp_defs[] = { { - /*** Health Thermomter Service. */ + /*** Health Thermometer Service. */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = BLE_UUID16_DECLARE(BLE_SVC_HTP_UUID16), .characteristics = (struct ble_gatt_chr_def[]) @@ -49,7 +49,7 @@ static const struct ble_gatt_svc_def ble_svc_htp_defs[] = { .val_handle = &ble_svc_htp_temp_measurement_val_handle, .flags = BLE_GATT_CHR_F_INDICATE, }, { - /** Temparature Type Characteristic */ + /** Temperature Type Characteristic */ .uuid = BLE_UUID16_DECLARE(BLE_SVC_HTP_CHR_UUID16_TEMP_TYPE), .access_cb = ble_svc_htp_access, .val_handle = &ble_svc_htp_temp_type_val_handle, @@ -87,7 +87,7 @@ static const struct ble_gatt_svc_def ble_svc_htp_defs[] = { }; /** - * HR access function + * HTP access function */ static int ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, @@ -136,6 +136,9 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, void ble_svc_htp_on_disconnect(uint16_t conn_handle) { + if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + return; + } conn_chr_subs[conn_handle].chr_subs[TEMP_MEASUREMENT] = false; conn_chr_subs[conn_handle].chr_subs[INTERMEDIATE_TEMP] = false; conn_chr_subs[conn_handle].chr_subs[MEASUREMENT_ITVL] = false; @@ -147,6 +150,9 @@ ble_svc_htp_on_disconnect(uint16_t conn_handle) bool ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr) { + if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + return false; + } return conn_chr_subs[conn_handle].chr_subs[chr]; } @@ -161,6 +167,9 @@ ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr) void ble_svc_htp_subscribe(uint16_t conn_handle, uint16_t attr_handle) { + if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + return; + } if (attr_handle == ble_svc_htp_temp_measurement_val_handle) { conn_chr_subs[conn_handle].chr_subs[TEMP_MEASUREMENT] = true; @@ -197,6 +206,7 @@ ble_svc_htp_notify(uint16_t conn_handle, float temp, bool temp_unit) rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp)); if (rc != 0) { + os_mbuf_free_chain(txom); goto err; } @@ -237,6 +247,7 @@ ble_svc_htp_indicate(uint16_t conn_handle, float temp, bool temp_unit) rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp)); if (rc != 0) { + os_mbuf_free_chain(txom); return rc; } diff --git a/nimble/host/services/ias/include/services/ias/ble_svc_ias.h b/nimble/host/services/ias/include/services/ias/ble_svc_ias.h index 3bce343b5..2ba2f5f36 100644 --- a/nimble/host/services/ias/include/services/ias/ble_svc_ias.h +++ b/nimble/host/services/ias/include/services/ias/ble_svc_ias.h @@ -17,8 +17,8 @@ * under the License. */ -#ifndef H_BLE_IAS_TPS_ -#define H_BLE_IAS_TPS_ +#ifndef H_BLE_SVC_IAS_ +#define H_BLE_SVC_IAS_ #ifdef __cplusplus extern "C" { diff --git a/nimble/host/services/ias/src/ble_svc_ias.c b/nimble/host/services/ias/src/ble_svc_ias.c index ddd1656d0..a132f53ca 100644 --- a/nimble/host/services/ias/src/ble_svc_ias.c +++ b/nimble/host/services/ias/src/ble_svc_ias.c @@ -105,15 +105,24 @@ ble_svc_ias_access(uint16_t conn_handle, uint16_t attr_handle, switch (ctxt->op) { case BLE_GATT_ACCESS_OP_WRITE_CHR: - rc = ble_svc_ias_chr_write(ctxt->om, - sizeof ble_svc_ias_alert_level, - sizeof ble_svc_ias_alert_level, - &ble_svc_ias_alert_level, NULL); - /* Call the IAS event function */ - if (ble_svc_ias_cb_fn) { - ble_svc_ias_cb_fn(ble_svc_ias_alert_level); - } - return rc; + { + uint8_t alert_level; + rc = ble_svc_ias_chr_write(ctxt->om, + sizeof alert_level, + sizeof alert_level, + &alert_level, NULL); + if (rc != 0) { + return rc; + } + if (alert_level > BLE_SVC_IAS_ALERT_LEVEL_HIGH_ALERT) { + return BLE_ATT_ERR_VALUE_NOT_ALLOWED; + } + ble_svc_ias_alert_level = alert_level; + if (ble_svc_ias_cb_fn) { + ble_svc_ias_cb_fn(ble_svc_ias_alert_level); + } + } + return 0; default: assert(0); diff --git a/nimble/host/services/ipss/src/ble_svc_ipss.c b/nimble/host/services/ipss/src/ble_svc_ipss.c index 3cdaccf92..bc9c0cd65 100644 --- a/nimble/host/services/ipss/src/ble_svc_ipss.c +++ b/nimble/host/services/ipss/src/ble_svc_ipss.c @@ -28,7 +28,7 @@ static const ble_uuid16_t uuid_svc_ipss = BLE_UUID16_INIT(BLE_SVC_IPSS_UUID16); static const struct ble_gatt_svc_def ble_svc_ipss_defs[] = { { - /*** Service: GATT */ + /*** Service: IPSS */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = &uuid_svc_ipss.u, .characteristics = NULL, diff --git a/nimble/host/services/lls/src/ble_svc_lls.c b/nimble/host/services/lls/src/ble_svc_lls.c index d298d77b2..d59eb2668 100644 --- a/nimble/host/services/lls/src/ble_svc_lls.c +++ b/nimble/host/services/lls/src/ble_svc_lls.c @@ -100,8 +100,10 @@ static int ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { - assert(ctxt->chr == &ble_svc_lls_defs[0].characteristics[0]); int rc; + + assert(ctxt->chr == &ble_svc_lls_defs[0].characteristics[0]); + switch (ctxt->op) { case BLE_GATT_ACCESS_OP_READ_CHR: rc = os_mbuf_append(ctxt->om, &ble_svc_lls_alert_level, @@ -109,10 +111,17 @@ ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle, return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; case BLE_GATT_ACCESS_OP_WRITE_CHR: + uint8_t level; rc = ble_svc_lls_chr_write(ctxt->om, - sizeof ble_svc_lls_alert_level, - sizeof ble_svc_lls_alert_level, - &ble_svc_lls_alert_level, NULL); + sizeof level, + sizeof level, + &level, NULL); + if (rc == 0) { + if (level > BLE_SVC_LLS_ALERT_LEVEL_HIGH_ALERT) { + return BLE_ATT_ERR_VALUE_NOT_ALLOWED; + } + ble_svc_lls_alert_level = level; + } return rc; default: @@ -170,8 +179,7 @@ ble_svc_lls_alert_level_set(uint8_t alert_level) return BLE_HS_EINVAL; } - memcpy(&ble_svc_lls_alert_level, &alert_level, - sizeof alert_level); + ble_svc_lls_alert_level = alert_level; return 0; } @@ -183,7 +191,7 @@ ble_svc_lls_set_cb(ble_svc_lls_event_fn *cb) } /** - * Initialize the IAS package. + * Initialize the LLS package. */ void ble_svc_lls_init(void) @@ -198,5 +206,7 @@ ble_svc_lls_init(void) rc = ble_gatts_add_svcs(ble_svc_lls_defs); SYSINIT_PANIC_ASSERT(rc == 0); + + ble_svc_lls_alert_level = BLE_SVC_LLS_ALERT_LEVEL_NO_ALERT; } #endif diff --git a/nimble/host/services/prox/src/ble_svc_prox.c b/nimble/host/services/prox/src/ble_svc_prox.c index 49c67dd53..fad972344 100644 --- a/nimble/host/services/prox/src/ble_svc_prox.c +++ b/nimble/host/services/prox/src/ble_svc_prox.c @@ -16,7 +16,7 @@ #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_PROX_SERVICE /* Characteristic values */ static uint8_t ble_svc_prox_link_loss_alert; -static int8_t ble_svc_prox_alert; +static uint8_t ble_svc_prox_alert; static uint8_t ble_svc_prox_tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO; /* Characteristic value handles */ @@ -55,7 +55,7 @@ static const struct ble_gatt_svc_def ble_svc_prox_defs[] = { .uuid = BLE_UUID16_DECLARE(BLE_SVC_PROX_CHR_UUID16_ALERT_LVL), .access_cb = ble_svc_prox_link_loss_access, .val_handle = &ble_svc_prox_link_loss_val_handle, - .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY, + .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY, }, { 0, /* No more characteristics in this service. */ } @@ -71,7 +71,7 @@ static const struct ble_gatt_svc_def ble_svc_prox_defs[] = { .uuid = BLE_UUID16_DECLARE(BLE_SVC_PROX_CHR_UUID16_ALERT_LVL), .access_cb = ble_svc_prox_imm_alert_access, .val_handle = &ble_svc_prox_immediate_alert_loc_val_handle, - .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY, + .flags = BLE_GATT_CHR_F_WRITE_NO_RSP, }, { 0, /* No more characteristics in this service. */ } @@ -109,7 +109,7 @@ static const struct ble_gatt_svc_def ble_svc_prox_defs[] = { }, }; -void +static void ble_prox_prph_task(void *pvParameters) { while (1) { @@ -125,6 +125,10 @@ ble_prox_prph_task(void *pvParameters) static void ble_prox_prph_alert_unalert(uint16_t conn_handle) { + if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + MODLOG_DFLT(ERROR, "conn_handle %d exceeds max connections", conn_handle); + return; + } if (ble_svc_prox_alert != 0 && !ble_svc_prox_alert_conn[conn_handle]) { MODLOG_DFLT(INFO, "Path loss exceeded threshold, starting alert for device with " "conn_handle %d", conn_handle); @@ -153,11 +157,15 @@ ble_svc_prox_link_loss_access(uint16_t conn_handle, uint16_t attr_handle, 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, 0, sizeof(ble_svc_prox_link_loss_alert), + 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; + } 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)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } - return BLE_ATT_ERR_INSUFFICIENT_RES; + return BLE_ATT_ERR_UNLIKELY; default: assert(0); @@ -178,7 +186,10 @@ ble_svc_prox_imm_alert_access(uint16_t conn_handle, uint16_t attr_handle, switch (uuid16) { case BLE_SVC_PROX_CHR_UUID16_ALERT_LVL: if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - ble_svc_prox_alert = ctxt->om->om_data[0]; + 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; + } MODLOG_DFLT(INFO, "Path loss = %d", ble_svc_prox_alert); ble_prox_prph_alert_unalert(conn_handle); @@ -205,11 +216,20 @@ ble_svc_prox_tx_pwr_access(uint16_t conn_handle, uint16_t attr_handle, switch (uuid16) { case BLE_SVC_PROX_CHR_UUID16_TX_PWR_LVL: - assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR); + if (ctxt->op != BLE_GATT_ACCESS_OP_READ_CHR) { + return BLE_ATT_ERR_UNLIKELY; + } rc = os_mbuf_append(ctxt->om, &ble_svc_prox_tx_pwr_lvl, sizeof(ble_svc_prox_tx_pwr_lvl)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + case 0x2904: /* Presentation Format Descriptor UUID */ + if (ctxt->op != BLE_GATT_ACCESS_OP_READ_DSC) { + return BLE_ATT_ERR_UNLIKELY; + } + /* Return empty for now */ + return 0; + default: assert(0); return BLE_ATT_ERR_UNLIKELY; @@ -255,7 +275,9 @@ ble_svc_prox_init(void) rc = ble_gatts_add_svcs(ble_svc_prox_defs); SYSINIT_PANIC_ASSERT(rc == 0); - xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", 4096, NULL, 10, NULL); + static TaskHandle_t ble_prox_task_handle; + BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", 4096, NULL, 10, &ble_prox_task_handle); + SYSINIT_PANIC_ASSERT(ret == pdPASS); /* Initializing alert array */ for (int i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { diff --git a/nimble/host/services/ras/src/ble_svc_ras.c b/nimble/host/services/ras/src/ble_svc_ras.c index 23ce9056b..a197977f2 100644 --- a/nimble/host/services/ras/src/ble_svc_ras.c +++ b/nimble/host/services/ras/src/ble_svc_ras.c @@ -28,7 +28,9 @@ #include "nimble/hci_common.h" #include "esp_nimble_mem.h" +#ifndef MIN #define MIN(a, b) ((a) < (b) ? (a) : (b)) +#endif static uint16_t ble_svc_ras_feat_val_handle; static uint32_t ble_svc_ras_feat_val; @@ -45,6 +47,7 @@ static uint8_t ble_svc_ras_cp_val[RASCP_CMD_OPCODE_LEN + sizeof(uint16_t)] ; static uint16_t ble_svc_ras_od_rd_val_handle; static struct segment *ble_svc_ras_od_rd_val; +static uint16_t ble_svc_ras_od_rd_seg_len; static uint16_t ble_svc_ras_rt_rd_val_handle; static uint16_t ble_svc_ras_rt_rd_val; @@ -61,7 +64,7 @@ void ble_gatts_indicate_ranging_data_ready(uint16_t ranging_counter) void ble_gatts_indicate_control_point_response(uint16_t attr_handle , uint16_t ranging_counter) { - /* Indication control point reponse only when all the idication for on_demad_rd is sent for all segments;*/ + /* Indication control point response only when all the indication for on_demand_rd is sent for all segments */ if (attr_handle == ble_svc_ras_od_rd_val_handle) { MODLOG_DFLT(INFO, "Indicate control point response\n"); ble_svc_ras_cp_val[0] = RASCP_RSP_OPCODE_COMPLETE_RD_RSP; @@ -94,14 +97,11 @@ static void reset_ranging_buffer(void) ranging_buffers[i].subevent_cursor = 0; } } -struct ranging_buffer *ranging_buffer_alloc(uint16_t conn_handle , uint16_t ranging_counter) +struct ranging_buffer *ranging_buffer_alloc(uint16_t conn_handle, uint16_t ranging_counter) { - uint16_t conn_buffer_count = 0; - - for (uint8_t i = 0; i < sizeof(ranging_buffers)/sizeof(ranging_buffers[0]) ; i++) { + for (uint8_t i = 0; i < sizeof(ranging_buffers)/sizeof(ranging_buffers[0]); i++) { if (ranging_buffers[i].conn == -1) { - conn_buffer_count++; - ranging_buffer_init(conn_handle, &ranging_buffers[i] , ranging_counter); + ranging_buffer_init(conn_handle, &ranging_buffers[i], ranging_counter); return &ranging_buffers[i]; } } @@ -221,7 +221,7 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl attr_handle); } - uuid = ble_uuid_u16(ctxt->chr->uuid); + uuid = ble_uuid_u16(ctxt->chr->uuid); if (uuid == BLE_SVC_RAS_CHR_UUID_FEATURES_VAL) { ble_svc_ras_feat_val |= RETRIEVE_LST_SEG_BIT | ABORT_OP_BIT | FLTR_RANGING_DATA_BIT; MODLOG_DFLT(INFO, "ble_svc_ras_feat_val = %02x\n",ble_svc_ras_feat_val); @@ -241,13 +241,15 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl } } else if (uuid == BLE_SVC_RAS_CHR_UUID_ONDEMAND_RD_VAL) { - // print the segament data - MODLOG_DFLT(INFO,"ble_svc_ras_od_rd_val\n"); - MODLOG_DFLT(INFO, "ble_svc_ras_od_rd_val.data = %2x %02x %02x \n",ble_svc_ras_od_rd_val->header.first_seg,ble_svc_ras_od_rd_val->header.last_seg,ble_svc_ras_od_rd_val->header.seg_counter); + /* print the segment data */ + if (ble_svc_ras_od_rd_val == NULL) { + return BLE_ATT_ERR_UNLIKELY; + } + MODLOG_DFLT(INFO, "ble_svc_ras_od_rd_val\n"); if (attr_handle == ble_svc_ras_od_rd_val_handle) { rc = os_mbuf_append(ctxt->om, ble_svc_ras_od_rd_val, - sizeof(struct segment )+100); + ble_svc_ras_od_rd_seg_len); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } } else if (uuid == BLE_SVC_RAS_CHR_UUID_REALTIME_RD_VAL) { @@ -275,6 +277,7 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } } + return 0; case BLE_GATT_ACCESS_OP_WRITE_CHR: if (conn_handle != BLE_HS_CONN_HANDLE_NONE) { @@ -294,34 +297,50 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handl "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 */ + if (ble_svc_ras_od_rd_val == NULL) { + return BLE_ATT_ERR_UNLIKELY; + } + rc = gatt_svr_write(ctxt->om, - sizeof(ble_svc_ras_od_rd_val), - sizeof(ble_svc_ras_od_rd_val), - ble_svc_ras_od_rd_val, NULL); - ble_gatts_chr_updated(attr_handle); - MODLOG_DFLT(INFO, "Notification/Indication scheduled for " - "all subscribed peers.\n"); + 0, + ble_svc_ras_od_rd_seg_len, + ble_svc_ras_od_rd_val, NULL); + 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_cp_val_handle) { MODLOG_DFLT(INFO, "write for control point val "); rc = gatt_svr_write(ctxt->om, - sizeof(ble_svc_ras_cp_val), + RASCP_CMD_OPCODE_LEN, sizeof(ble_svc_ras_cp_val), &ble_svc_ras_cp_val, NULL); + if (rc != 0) { + return rc; + } MODLOG_DFLT(INFO, "ble_svc_gap_cp_val = %02x %02x %02x \n",ble_svc_ras_cp_val[0],ble_svc_ras_cp_val[1],ble_svc_ras_cp_val[2]); if (ble_svc_ras_cp_val[0] == RASCP_OPCODE_GET_RD) { // n = size of (rangeing_buffer[i]) / mut -4); /* Run a loop to send n segmet . for now sending only 1 segment*/ ble_gatts_chr_updated(ble_svc_ras_od_rd_val_handle); } else if (ble_svc_ras_cp_val[0] == RASCP_OPCODE_ACK_RD) { - MODLOG_DFLT(INFO, "ack revied\n"); + MODLOG_DFLT(INFO, "ack received\n"); + /* Free the acknowledged segment */ + 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; + } /* Reset the ranging buffers */ 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_gatts_chr_updated(ble_svc_ras_cp_val_handle); - MODLOG_DFLT(INFO, "Sucssefully completed the Ranging procedure\n"); + MODLOG_DFLT(INFO, "Successfully completed the Ranging procedure\n"); } MODLOG_DFLT(INFO, "Notification/Indication scheduled for " @@ -387,30 +406,44 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) { 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 antena path mask using bitmask*/ + /* convert antenna path mask using bitmask */ buf->ranging_data.ranging_header.antenna_paths_mask = ranging_subevent.subev_result.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"); + 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_continue.procedure_done_status; - subevent_hdr->subevent_done_status = ranging_subevent.subev_result_continue.subevent_done_status; + 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 + ranging_subevent.subev_result_continue.num_steps_reported; + subevent_hdr->num_steps_reported = ranging_subevent.subev_result.num_steps_reported; - /* Add step data to buf*/ + /* 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++) { - const struct cs_steps_data *step=&ranging_subevent.subev_result.steps[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; + } 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; } /* Create RAS segment*/ struct segment *ras_segment; @@ -423,32 +456,19 @@ void ble_gatts_store_ranging_data(struct ble_cs_event ranging_subevent) { return; } ras_segment->header.first_seg = true; - ras_segment->header.last_seg = true; - ras_segment->header.seg_counter = 0; // First segment + ras_segment->header.seg_counter = 0; /* First segment */ uint16_t buf_len = sizeof(struct ranging_header) + buf->subevent_cursor; - uint16_t remaining = buf_len - 0; - uint16_t pull_bytes = MIN(max_data_len, remaining); - if (max_data_len < remaining) { - // More segments to come - pull_bytes = max_data_len; - }else { - pull_bytes = remaining; + uint16_t pull_bytes = MIN(max_data_len, buf_len); + + ras_segment->header.last_seg = (buf_len <= max_data_len); + memcpy(ras_segment->data, &buf->ranging_data.buf[0], pull_bytes); + + /* Free previous segment if any */ + if (ble_svc_ras_od_rd_val != NULL) { + nimble_platform_mem_free(ble_svc_ras_od_rd_val); } - - memcpy( ras_segment->data, &buf->ranging_data.buf[0], pull_bytes); - - ble_svc_ras_od_rd_val= ras_segment; // Store the segment in the global variable - // ble_gatts_chr_updated(ble_svc_ras_od_rd_val_handle); - - // 4 bytes for header and CRC - // now max data len is att mtu -4 - // count number of egment required based on actudal rd_buffer len - // noe set buf to data [] of segment ensuring only one segment is created - // if more than one segment is required then set first_seg and last_seg accordingly - // and set seg_counter to 0 for first segment and increment it for each segment - // and set the data len to actual data len - - // now once cp control point command is recived we will append the segment to os_mbuf buffer and indicate it to the client + ble_svc_ras_od_rd_val = ras_segment; + ble_svc_ras_od_rd_seg_len = sizeof(struct segment_header) + pull_bytes; } void custom_gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) 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 b56ede7ea..531c75929 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 @@ -18,19 +18,19 @@ */ #include - #include #include #include "host/ble_hs.h" #include "host/ble_uuid.h" #include "services/ras/ble_svc_ras.h" /* Char values */ -static int32_t ble_svc_ras_feat_val; +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 uint16_t ble_svc_ras_cp_val; static uint16_t ble_svc_ras_feat_val_handle; +static uint16_t ble_svc_ras_od_val_handle; static uint16_t ble_svc_ras_rd_val_handle; static uint16_t ble_svc_ras_rd_ov_val_handle; static uint16_t ble_svc_ras_cp_val_handle; @@ -39,42 +39,62 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); +static int +gatt_svr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len, + void *dst, uint16_t *len) +{ + uint16_t om_len; + int rc; + + om_len = OS_MBUF_PKTLEN(om); + if (om_len < min_len || om_len > max_len) { + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + + rc = ble_hs_mbuf_to_flat(om, dst, max_len, len); + if (rc != 0) { + return BLE_ATT_ERR_UNLIKELY; + } + + return 0; +} + static const struct ble_gatt_svc_def gatt_svr_svcs[] = { { /* Service: Ranging Data Service */ .type = BLE_GATT_SVC_TYPE_PRIMARY, - .uuid = BLE_UUID16_DECLARE(BLE_UUID_RANGING_SERVICE_VAL), + .uuid = BLE_UUID16_DECLARE(BLE_SVC_RAS_RANGING_SERVICE_VAL), .characteristics = (struct ble_gatt_chr_def[]) { { /* Characteristic: Feature Value */ - .uuid = BLE_UUID16_DECLARE(BLE_UUID_RAS_FEATURES_VAL), - .access_cb = gatt_svr_chr_access_ras_feature, + .uuid = BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_UUID_FEATURES_VAL), + .access_cb = gatt_svr_chr_access_ras_val, .val_handle = &ble_svc_ras_feat_val_handle, - .flags = BLE_GATT_CHR_F_READ|BLE_GATT_CHR_F_READ_ENC, + .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC, },{ /* Characteristic: On demand ranging data */ - .uuid = BLE_UUID16_DECLARE(BLE_UUID_RAS_ONDEMAND_RD_VAL), - .access_cb = gatt_svr_chr_access_ras_feature, - .val_handle = ble_svc_ras_od_val_handle, - .flags = BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE |BLE_GATT_CHR_F_READ_ENC, + .uuid = BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_UUID_ONDEMAND_RD_VAL), + .access_cb = gatt_svr_chr_access_ras_val, + .val_handle = &ble_svc_ras_od_val_handle, + .flags = BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE, },{ /* Characteristic: RAS Control Point */ - .uuid = BLE_UUID16_DECLARE(BLE_UUID_RAS_CP_VAL), - .access_cb = gatt_svr_chr_access_ras_feature, - .val_handle = ble_svc_ras_cp_val_handle, - .flags = BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_INDICATE |BLE_GATT_CHR_F_READ_ENC , + .uuid = BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_UUID_CP_VAL), + .access_cb = gatt_svr_chr_access_ras_val, + .val_handle = &ble_svc_ras_cp_val_handle, + .flags = BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_INDICATE, },{ /* Characteristic: RAS Data Ready */ - .uuid = BLE_UUID16_DECLARE(BLE_UUID_RAS_RD_READY_VAL), - .access_cb = gatt_svr_chr_access_ras_feature, - .val_handle = ble_svc_ras_rd_val_handle, - .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_READ_ENC, + .uuid = BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_UUID_RD_READY_VAL), + .access_cb = gatt_svr_chr_access_ras_val, + .val_handle = &ble_svc_ras_rd_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC, },{ /* Characteristic: RAS data overwritten */ - .uuid = BLE_UUID16_DECLARE(BLE_UUID_RAS_RD_OVERWRITTEN_VAL), - .access_cb = gatt_svr_chr_access_ras_feature, - .val_handle=ble_svc_ras_rd_ov_val_handle, - .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_READ_ENC, + .uuid = BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_UUID_RD_OVERWRITTEN_VAL), + .access_cb = gatt_svr_chr_access_ras_val, + .val_handle = &ble_svc_ras_rd_ov_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC, },{ 0, /* No more characteristics in this service */ }, @@ -90,7 +110,6 @@ static int gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { - const ble_uuid_t *uuid; int rc; switch (ctxt->op) { @@ -102,17 +121,23 @@ gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handle, MODLOG_DFLT(INFO, "Characteristic read by NimBLE stack; attr_handle=%d\n", attr_handle); } - uuid = ctxt->chr->uuid; - if(uuid == BLE_UUID_RAS_FEATURES_VAL){ - - if (attr_handle == ras_feat_val_handle) { - rc = os_mbuf_append(ctxt->om, - &ras_feat_val, - sizeof(ras_feat_val)); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } + if (attr_handle == ble_svc_ras_feat_val_handle) { + rc = os_mbuf_append(ctxt->om, + &ble_svc_ras_feat_val, + sizeof(ble_svc_ras_feat_val)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } else if (attr_handle == ble_svc_ras_rd_val_handle) { + rc = os_mbuf_append(ctxt->om, + &ble_svc_ras_rd_val, + sizeof(ble_svc_ras_rd_val)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } else if (attr_handle == ble_svc_ras_rd_ov_val_handle) { + rc = os_mbuf_append(ctxt->om, + &ble_svc_ras_rd_ov_val, + sizeof(ble_svc_ras_rd_ov_val)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } - goto unknown; + return BLE_ATT_ERR_UNLIKELY; case BLE_GATT_ACCESS_OP_WRITE_CHR: if (conn_handle != BLE_HS_CONN_HANDLE_NONE) { @@ -122,29 +147,21 @@ gatt_svr_chr_access_ras_val(uint16_t conn_handle, uint16_t attr_handle, MODLOG_DFLT(INFO, "Characteristic write by NimBLE stack; attr_handle=%d", attr_handle); } - uuid = ctxt->chr->uuid; - if (attr_handle == ras_feat_val_handle) { + if (attr_handle == ble_svc_ras_cp_val_handle) { rc = gatt_svr_write(ctxt->om, - sizeof(ras_feat_val), - sizeof(ras_feat_val), - &ras_feat_val, NULL); - ble_gatts_chr_updated(attr_handle); - MODLOG_DFLT(INFO, "Notification/Indication scheduled for " - "all subscribed peers.\n"); + RASCP_CMD_OPCODE_LEN, + sizeof(ble_svc_ras_cp_val), + &ble_svc_ras_cp_val, NULL); return rc; } - goto unknown; + return BLE_ATT_ERR_UNLIKELY; + default: - goto unknown; + break; } -unknown: - /* Unknown characteristic/descriptor; - * The NimBLE host should not have called this function; - */ - assert(0); - return BLE_ATT_ERR_UNLIKELY; - + /* Unknown characteristic/descriptor */ + return BLE_ATT_ERR_UNLIKELY; } void @@ -174,19 +191,15 @@ custom_gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) break; default: - assert(0); break; } } int -gatt_svr_init(void) +ble_svc_ras_rrsp_init(void) { int rc; - ble_svc_gap_init(); - ble_svc_gatt_init(); - rc = ble_gatts_count_cfg(gatt_svr_svcs); if (rc != 0) { return rc; diff --git a/nimble/host/services/sps/src/ble_svc_sps.c b/nimble/host/services/sps/src/ble_svc_sps.c index 8ea4a1c46..1bb97cd5a 100644 --- a/nimble/host/services/sps/src/ble_svc_sps.c +++ b/nimble/host/services/sps/src/ble_svc_sps.c @@ -81,7 +81,7 @@ static const struct ble_gatt_chr_def sps_characteristics[] = { }; static const struct ble_gatt_svc_def ble_svc_sps_defs[] = { - { /*** Service: Device Information Service (SPS). */ + { /*** Service: Scan Parameters Service (SPS). */ .type = BLE_GATT_SVC_TYPE_PRIMARY, .uuid = &uuid_svc_sps.u, .characteristics = sps_characteristics, @@ -122,21 +122,21 @@ ble_svc_sps_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { uint16_t uuid = ble_uuid_u16(ctxt->chr->uuid); - uint32_t write_val; + uint8_t buf[4]; int rc; switch(uuid) { case BLE_SVC_SPS_CHR_UUID16_SCAN_ITVL_WINDOW: assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR); - rc = ble_svc_sps_chr_write(ctxt->om, 0, sizeof(ble_scan_itvl) + sizeof(ble_scan_window), &write_val, NULL); + rc = ble_svc_sps_chr_write(ctxt->om, 4, 4, buf, NULL); if(rc == 0) { - ble_scan_itvl = (write_val & 0xffff0000) >> 16; - ble_scan_window = (write_val & 0x0000ffff); + ble_scan_itvl = get_le16(buf); + ble_scan_window = get_le16(buf + 2); + if (ble_svc_sps_cb_fn) { + ble_svc_sps_cb_fn(ble_scan_itvl, ble_scan_window); + } } - if (ble_svc_sps_cb_fn) { - ble_svc_sps_cb_fn(ble_scan_itvl, ble_scan_window); - } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; case BLE_SVC_SPS_CHR_UUID16_SCAN_REFRESH: assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR && conn_handle == BLE_HS_CONN_HANDLE_NONE); rc = os_mbuf_append(ctxt->om, &ble_scan_refresh, diff --git a/nimble/host/services/tps/src/ble_svc_tps.c b/nimble/host/services/tps/src/ble_svc_tps.c index 77efdc284..230c0c1d7 100644 --- a/nimble/host/services/tps/src/ble_svc_tps.c +++ b/nimble/host/services/tps/src/ble_svc_tps.c @@ -18,7 +18,6 @@ */ #include -#include #include "sysinit/sysinit.h" #include "syscfg/syscfg.h" #include "host/ble_hs.h" @@ -31,8 +30,6 @@ #include "../src/ble_hs_hci_priv.h" #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_TPS_SERVICE -int8_t ble_svc_tps_tx_power_level; - /* Access function */ static int ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle, @@ -48,7 +45,7 @@ static const struct ble_gatt_chr_def tps_characteristics[] = { .access_cb = ble_svc_tps_access, .flags = BLE_GATT_CHR_F_READ, }, { - 0, /* No more characeteristics in this service */ + 0, /* No more characteristics in this service */ }, }; @@ -72,27 +69,26 @@ static int ble_svc_tps_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { + int8_t tx_power_level; int rc; assert(ctxt->chr == &ble_svc_tps_defs[0].characteristics[0]); switch (ctxt->op) { case BLE_GATT_ACCESS_OP_READ_CHR: - rc = ble_hs_hci_util_read_adv_tx_pwr(&ble_svc_tps_tx_power_level); + rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_power_level); if (rc != 0) { return BLE_ATT_ERR_UNLIKELY; } - rc = os_mbuf_append(ctxt->om, &ble_svc_tps_tx_power_level, - sizeof ble_svc_tps_tx_power_level); + rc = os_mbuf_append(ctxt->om, &tx_power_level, + sizeof tx_power_level); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; default: assert(0); return BLE_ATT_ERR_UNLIKELY; } - - return 0; } /** diff --git a/nimble/host/src/ble_aes_ccm.c b/nimble/host/src/ble_aes_ccm.c index 7d4478d80..303391b2f 100644 --- a/nimble/host/src/ble_aes_ccm.c +++ b/nimble/host/src/ble_aes_ccm.c @@ -7,22 +7,13 @@ #include #include #include "host/ble_aes_ccm.h" -#include "../src/ble_hs_conn_priv.h" #if MYNEWT_VAL(ENC_ADV_DATA) -#ifdef __cplusplus -extern "C" { -#endif - #ifndef min #define min(a, b) ((a) < (b) ? (a) : (b)) #endif -#ifndef max -#define max(a, b) ((a) > (b) ? (a) : (b)) -#endif - #define sys_put_be16(a,b) put_be16(b, a) const char * @@ -112,8 +103,8 @@ static inline void xor16(uint8_t *dst, const uint8_t *a, const uint8_t *b) } /* pmsg is assumed to have the nonce already present in bytes 1-13 */ -static int ble_aes_ccm_calculate_X0(const uint8_t key[16], const uint8_t *aad, uint8_t aad_len, - size_t mic_size, uint8_t msg_len, uint8_t b[16], +static int ble_aes_ccm_calculate_X0(const uint8_t key[16], const uint8_t *aad, uint16_t aad_len, + size_t mic_size, uint16_t msg_len, uint8_t b[16], uint8_t X0[16]) { int i, j, err; @@ -194,8 +185,14 @@ static int ble_aes_ccm_auth(const uint8_t key[16], uint8_t nonce[13], if (err) { return err; } + if (msg_len > 0xffff) { + return BLE_HS_EINVAL; + } - ble_aes_ccm_calculate_X0(key, aad, aad_len, mic_size, msg_len, b, Xn); + err = ble_aes_ccm_calculate_X0(key, aad, aad_len, mic_size, msg_len, b, Xn); + if (err) { + return err; + } for (j = 0; j < blk_cnt; j++) { /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ @@ -268,24 +265,31 @@ int ble_aes_ccm_decrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t { uint8_t mic[16]; uint8_t key_reversed[16]; + int rc; if (aad_len >= 0xff00 || mic_size > sizeof(mic)) { return BLE_HS_EINVAL; } + /* Validate MIC size per RFC 3610 */ + if (mic_size < 4 || mic_size > 16 || (mic_size % 2) != 0) { + return BLE_HS_EINVAL; + } + /** Setting the correct endian-ness of the key */ for (int i = 0; i < 16; i++) { key_reversed[i] = key[15 - i]; } - ble_aes_ccm_crypt(key_reversed, nonce, enc_msg, out_msg, msg_len); + rc = ble_aes_ccm_crypt(key_reversed, nonce, enc_msg, out_msg, msg_len); + if (rc != 0) { + return rc; + } - ble_aes_ccm_auth(key_reversed, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size); - - /*if (memcmp(mic, enc_msg + msg_len, mic_size)) { - printf("\n%s return here", __func__); - return -EBADMSG; - }*/ + rc = ble_aes_ccm_auth(key_reversed, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size); + if (rc != 0) { + return rc; + } return 0; } @@ -303,22 +307,29 @@ int ble_aes_ccm_encrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t return BLE_HS_EINVAL; } + /* Validate MIC size per RFC 3610 */ + if (mic_size < 4 || mic_size > 16 || (mic_size % 2) != 0) { + return BLE_HS_EINVAL; + } + /* Correcting the endian-ness of the key */ for (int i = 0; i < 16; i++) { key_reversed[i] = key[15 - i]; } /** Calculating MIC */ - ble_aes_ccm_auth(key_reversed, nonce, msg, msg_len, aad, aad_len, mic, mic_size); + int rc = ble_aes_ccm_auth(key_reversed, nonce, msg, msg_len, aad, aad_len, mic, mic_size); + if (rc != 0) { + return rc; + } - /** Encrypting advertisment */ - ble_aes_ccm_crypt(key_reversed, nonce, msg, out_msg, msg_len); + /** Encrypting advertisement */ + rc = ble_aes_ccm_crypt(key_reversed, nonce, msg, out_msg, msg_len); + if (rc != 0) { + return rc; + } return 0; } #endif /* ENC_ADV_DATA */ - -#ifdef __cplusplus -} -#endif diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c index 50a4f164e..6f9889036 100644 --- a/nimble/host/src/ble_att.c +++ b/nimble/host/src/ble_att.c @@ -28,13 +28,9 @@ #define min(a, b) ((a) < (b) ? (a) : (b)) #endif -#ifndef max -#define max(a, b) ((a) > (b) ? (a) : (b)) -#endif - static uint16_t ble_att_preferred_mtu_val; -/** Dispatch table for incoming ATT requests. Sorted by op code. */ +/** Dispatch table for incoming ATT requests. */ typedef int ble_att_rx_fn(uint16_t conn_handle, uint16_t cid, struct os_mbuf **om); struct ble_att_rx_dispatch_entry { uint8_t bde_op; @@ -143,6 +139,12 @@ STATS_NAME_START(ble_att_stats) STATS_NAME(ble_att_stats, multi_notify_req_tx) STATS_NAME(ble_att_stats, write_cmd_rx) STATS_NAME(ble_att_stats, write_cmd_tx) + STATS_NAME(ble_att_stats, read_mult_var_req_rx) + STATS_NAME(ble_att_stats, read_mult_var_req_tx) + STATS_NAME(ble_att_stats, read_mult_var_rsp_rx) + STATS_NAME(ble_att_stats, read_mult_var_rsp_tx) + STATS_NAME(ble_att_stats, signed_write_cmd_rx) + STATS_NAME(ble_att_stats, signed_write_cmd_tx) STATS_NAME_END(ble_att_stats) static const struct ble_att_rx_dispatch_entry * @@ -293,6 +295,18 @@ ble_att_inc_tx_stat(uint8_t att_op) STATS_INC(ble_att_stats, write_cmd_tx); break; + case BLE_ATT_OP_READ_MULT_VAR_REQ: + STATS_INC(ble_att_stats, read_mult_var_req_tx); + break; + + case BLE_ATT_OP_READ_MULT_VAR_RSP: + STATS_INC(ble_att_stats, read_mult_var_rsp_tx); + break; + + case BLE_ATT_OP_SIGNED_WRITE_CMD: + STATS_INC(ble_att_stats, signed_write_cmd_tx); + break; + default: break; } @@ -414,6 +428,18 @@ ble_att_inc_rx_stat(uint8_t att_op) STATS_INC(ble_att_stats, write_cmd_rx); break; + case BLE_ATT_OP_READ_MULT_VAR_REQ: + STATS_INC(ble_att_stats, read_mult_var_req_rx); + break; + + case BLE_ATT_OP_READ_MULT_VAR_RSP: + STATS_INC(ble_att_stats, read_mult_var_rsp_rx); + break; + + case BLE_ATT_OP_SIGNED_WRITE_CMD: + STATS_INC(ble_att_stats, signed_write_cmd_rx); + break; + default: break; } @@ -506,6 +532,11 @@ static void ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle, uint16_t cid, struct os_mbuf **om) { + /* Guard against NULL or already-freed mbuf */ + if (om == NULL || *om == NULL) { + return; + } + /* If this is command (bit6 is set to 1), do nothing */ if (op & 0x40) { BLE_HS_LOG(INFO, "ATT command discarded (no response required); " @@ -515,11 +546,14 @@ ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle, } #if MYNEWT_VAL(BLE_GATTS) os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om)); - ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, - BLE_ATT_ERR_REQ_NOT_SUPPORTED); -#endif - + if (ble_att_svr_tx_error_rsp(conn_handle, cid, *om, op, 0, + BLE_ATT_ERR_REQ_NOT_SUPPORTED) == 0) { + *om = NULL; + } +#else + os_mbuf_free_chain(*om); *om = NULL; +#endif } static void @@ -588,6 +622,11 @@ ble_att_rx(struct ble_l2cap_chan *chan) { uint16_t conn_handle; + /* Validate channel and mbuf in release builds. */ + if (chan == NULL || chan->rx_buf == NULL) { + return BLE_HS_EINVAL; + } + conn_handle = ble_l2cap_get_conn_handle(chan); if (conn_handle == BLE_HS_CONN_HANDLE_NONE) { return BLE_HS_ENOTCONN; @@ -727,16 +766,19 @@ int ble_att_set_default_bearer_using_cid(uint16_t conn_handle, uint16_t cid) { #if MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0 struct ble_hs_conn * conn; + int rc; ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + rc = BLE_HS_ENOTCONN; + } else { + conn->default_cid = cid; + rc = 0; + } ble_hs_unlock(); - if (conn == NULL) { - return BLE_HS_ENOTCONN; - } - conn->default_cid = cid; - return 0; + return rc; #endif return BLE_HS_ENOTSUP; } @@ -745,12 +787,14 @@ uint16_t ble_att_get_default_bearer_cid(uint16_t conn_handle) { #if MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0 struct ble_hs_conn * conn; + uint16_t default_cid = 0; conn = ble_hs_conn_find(conn_handle); - if (conn == NULL) { - return 0; + if (conn != NULL) { + default_cid = conn->default_cid; } - return conn->default_cid; + + return default_cid; #endif return 0; } @@ -769,7 +813,10 @@ ble_att_init(void) return BLE_HS_EOS; } - ble_eatt_init(ble_att_rx_extended); + rc = ble_eatt_init(ble_att_rx_extended); + if (rc != 0) { + return rc; + } return 0; } diff --git a/nimble/host/src/ble_att_clt.c b/nimble/host/src/ble_att_clt.c index 6031e982f..634972b6d 100644 --- a/nimble/host/src/ble_att_clt.c +++ b/nimble/host/src/ble_att_clt.c @@ -51,7 +51,7 @@ ble_att_clt_rx_error(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) rsp = (struct ble_att_error_rsp *)(*rxom)->om_data; ble_gattc_rx_err(conn_handle, cid, le16toh(rsp->baep_handle), - le16toh(rsp->baep_error_code)); + rsp->baep_error_code); return 0; } @@ -81,6 +81,7 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu) } else if (chan->flags & BLE_L2CAP_CHAN_F_TXED_MTU) { rc = BLE_HS_EALREADY; } else { + chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU; rc = 0; } ble_hs_unlock(); @@ -98,18 +99,15 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu) rc = ble_att_tx(conn_handle, BLE_L2CAP_CID_ATT, txom); if (rc != 0) { - return rc; + /* Rollback the flag so future MTU exchanges are not blocked */ + 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_lock(); - - rc = ble_att_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT, &conn, &chan); - if (rc == 0) { - chan->flags |= BLE_L2CAP_CHAN_F_TXED_MTU; - } - - ble_hs_unlock(); - return rc; } @@ -144,11 +142,11 @@ ble_att_clt_rx_mtu(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) ble_hs_unlock(); if (rc == 0) { - ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu); + ble_gap_mtu_event(conn_handle, cid, mtu); } } - ble_gattc_rx_mtu(conn_handle, BLE_L2CAP_CID_ATT, rc, mtu); + ble_gattc_rx_mtu(conn_handle, cid, rc, mtu); return rc; } @@ -251,11 +249,13 @@ ble_att_clt_rx_find_info(uint16_t conn_handle, uint16_t cid, struct os_mbuf **om rsp = (struct ble_att_find_info_rsp *)(*om)->om_data; + uint8_t format = rsp->bafp_format; + /* Strip the response base from the front of the mbuf. */ os_mbuf_adj((*om), sizeof(*rsp)); while (OS_MBUF_PKTLEN(*om) > 0) { - rc = ble_att_clt_parse_find_info_entry(om, rsp->bafp_format, &idata); + rc = ble_att_clt_parse_find_info_entry(om, format, &idata); if (rc != 0) { goto done; } @@ -297,6 +297,10 @@ ble_att_clt_tx_find_type_value(uint16_t conn_handle, uint16_t cid, return BLE_HS_EINVAL; } + if (value_len < 0 || (value_len > 0 && attribute_value == NULL)) { + return BLE_HS_EINVAL; + } + req = ble_att_cmd_get(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, sizeof(*req) + value_len, &txom); if (req == NULL) { @@ -306,7 +310,9 @@ ble_att_clt_tx_find_type_value(uint16_t conn_handle, uint16_t cid, req->bavq_start_handle = htole16(start_handle); req->bavq_end_handle = htole16(end_handle); req->bavq_attr_type = htole16(attribute_type); - memcpy(req->bavq_value, attribute_value, value_len); + if (value_len > 0) { + memcpy(req->bavq_value, attribute_value, value_len); + } return ble_att_tx(conn_handle, cid, txom); } @@ -460,7 +466,6 @@ ble_att_clt_tx_read(uint16_t conn_handle, uint16_t cid, uint16_t handle) struct ble_att_read_req *req; struct os_mbuf *txom; - int rc; if (handle == 0) { return BLE_HS_EINVAL; @@ -473,12 +478,7 @@ ble_att_clt_tx_read(uint16_t conn_handle, uint16_t cid, uint16_t handle) req->barq_handle = htole16(handle); - rc = ble_att_tx(conn_handle, cid, txom); - if (rc != 0) { - return rc; - } - - return 0; + return ble_att_tx(conn_handle, cid, txom); } int @@ -506,7 +506,6 @@ ble_att_clt_tx_read_blob(uint16_t conn_handle, uint16_t cid, uint16_t handle, ui struct ble_att_read_blob_req *req; struct os_mbuf *txom; - int rc; if (handle == 0) { return BLE_HS_EINVAL; @@ -520,12 +519,7 @@ ble_att_clt_tx_read_blob(uint16_t conn_handle, uint16_t cid, uint16_t handle, ui req->babq_handle = htole16(handle); req->babq_offset = htole16(offset); - rc = ble_att_tx(conn_handle, cid, txom); - if (rc != 0) { - return rc; - } - - return 0; + return ble_att_tx(conn_handle, cid, txom); } int @@ -738,8 +732,7 @@ ble_att_clt_tx_write_cmd(uint16_t conn_handle, uint16_t cid, #if MYNEWT_VAL(BLE_HS_DEBUG) uint8_t b; - int rc; - int i; + int i, rc; BLE_HS_LOG(DEBUG, "ble_att_clt_tx_write_cmd(): "); for (i = 0; i < OS_MBUF_PKTLEN(txom); i++) { @@ -787,13 +780,31 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han struct os_mbuf *txom2; uint8_t cmac[16]; uint8_t *message = NULL; - uint8_t len; + uint16_t len; + uint16_t payload_len; int rc; + +#if MYNEWT_VAL(BLE_HS_DEBUG) + uint8_t b; int i; BLE_HS_LOG(DEBUG, "ble_att_clt_tx_signed_write_cmd(): "); for (i = 0; i < OS_MBUF_PKTLEN(txom); i++) { - BLE_HS_LOG(DEBUG, "0x%02x", (OS_MBUF_DATA(txom, uint8_t *))[i]); + if (i != 0) { + BLE_HS_LOG(DEBUG, ":"); + } + rc = os_mbuf_copydata(txom, i, 1, &b); + assert(rc == 0); + BLE_HS_LOG(DEBUG, "0x%02x", b); + } +#endif + + payload_len = OS_MBUF_PKTLEN(txom); + + /* Validate payload length to prevent overflow */ + if (payload_len > (UINT16_MAX - BLE_ATT_SIGNED_WRITE_DATA_OFFSET - sizeof(counter))) { + rc = BLE_HS_EINVAL; + goto err; } cmd = ble_att_cmd_get(BLE_ATT_OP_SIGNED_WRITE_CMD, @@ -807,8 +818,12 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han /* Message to be signed is opcode||handle||message||sign_counter, * where || represents concatenation */ - len = BLE_ATT_SIGNED_WRITE_DATA_OFFSET + OS_MBUF_PKTLEN(txom) + sizeof(counter); - message = nimble_platform_mem_calloc(1,len); + len = BLE_ATT_SIGNED_WRITE_DATA_OFFSET + payload_len + sizeof(counter); + message = nimble_platform_mem_calloc(1, len); + if (message == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } /** Copying opcode and handle */ rc = os_mbuf_copydata(txom2, 0, BLE_ATT_SIGNED_WRITE_DATA_OFFSET, message); @@ -817,13 +832,13 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han } /** Copying message */ - rc = os_mbuf_copydata(txom, 0, OS_MBUF_PKTLEN(txom), &message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET]); + rc = os_mbuf_copydata(txom, 0, payload_len, &message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET]); if (rc != 0) { goto err; } /** Copying sign counter */ - memcpy(&message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET + OS_MBUF_PKTLEN(txom)], &counter, sizeof(counter)); + memcpy(&message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET + payload_len], &counter, sizeof(counter)); /* ble_sm_alg_aes_cmac takes data in little-endian format, * so converting it to LE. @@ -863,15 +878,16 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han if (message != NULL) { nimble_platform_mem_free(message); - message = NULL; + message = NULL; } os_mbuf_concat(txom2, txom); return ble_att_tx(conn_handle, cid, txom2); err: if (message != NULL) { nimble_platform_mem_free(message); - message = NULL; + message = NULL; } + /* Do not free txom here; the caller ble_gattc_signed_write is responsible for it on error. */ os_mbuf_free_chain(txom2); return rc; } @@ -980,7 +996,6 @@ ble_att_clt_tx_exec_write(uint16_t conn_handle, uint16_t cid, uint8_t flags) struct ble_att_exec_write_req *req; struct os_mbuf *txom; - int rc; req = ble_att_cmd_get(BLE_ATT_OP_EXEC_WRITE_REQ, sizeof(*req), &txom); if (req == NULL) { @@ -989,12 +1004,7 @@ ble_att_clt_tx_exec_write(uint16_t conn_handle, uint16_t cid, uint8_t flags) req->baeq_flags = flags; - rc = ble_att_tx(conn_handle, cid, txom); - if (rc != 0) { - return rc; - } - - return 0; + return ble_att_tx(conn_handle, cid, txom); } int diff --git a/nimble/host/src/ble_att_cmd.c b/nimble/host/src/ble_att_cmd.c index 36f7c9aa1..ef764a8e2 100644 --- a/nimble/host/src/ble_att_cmd.c +++ b/nimble/host/src/ble_att_cmd.c @@ -31,6 +31,10 @@ ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom) { struct ble_att_hdr *hdr; + if (txom == NULL) { + return NULL; + } + if (os_mbuf_extend(txom, sizeof(*hdr) + len) == NULL) { os_mbuf_free_chain(txom); return NULL; @@ -46,18 +50,25 @@ ble_att_cmd_prepare(uint8_t opcode, size_t len, struct os_mbuf *txom) void * ble_att_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom) { + void *result; + *txom = ble_hs_mbuf_l2cap_pkt(); if (*txom == NULL) { return NULL; } - return ble_att_cmd_prepare(opcode, len, *txom); + result = ble_att_cmd_prepare(opcode, len, *txom); + if (result == NULL) { + *txom = NULL; /* mbuf was freed inside ble_att_cmd_prepare */ + } + return result; } int ble_att_tx_with_conn(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan, struct os_mbuf *txom) { int rc; + bool is_request; struct os_mbuf_pkthdr *omp; if (!txom) { @@ -75,7 +86,9 @@ ble_att_tx_with_conn(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan, stru BLE_HS_DBG_ASSERT_EVAL(txom->om_len >= 1); - if (ble_att_is_request_op(txom->om_data[0])) { + is_request = ble_att_is_request_op(txom->om_data[0]); + + if (is_request) { if (conn->client_att_busy) { BLE_EATT_LOG_DEBUG("ATT Queue %p, client busy %d\n", txom, conn->client_att_busy); STAILQ_INSERT_TAIL(&conn->att_tx_q, OS_MBUF_PKTHDR(txom), omp_next); @@ -87,9 +100,15 @@ ble_att_tx_with_conn(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan, stru ble_att_inc_tx_stat(txom->om_data[0]); ble_att_truncate_to_mtu(chan, txom); + /* Save is_request before ble_l2cap_tx, which frees txom */ rc = ble_l2cap_tx(conn, chan, txom); - assert(rc == 0); - return rc; + if (rc != 0) { + if (is_request) { + conn->client_att_busy = false; + } + return rc; + } + return 0; } int @@ -125,10 +144,22 @@ ble_att_init_parse(uint8_t op, const void *payload, { const uint8_t *u8ptr; - BLE_HS_DBG_ASSERT(actual_len >= min_len); + /* Validate payload pointer */ + if (payload == NULL) { + return NULL; + } + + /* Validate length - critical for preventing buffer over-reads from remote peers */ + if (actual_len < min_len) { + return NULL; + } u8ptr = payload; - BLE_HS_DBG_ASSERT(u8ptr[0] == op); + + /* Validate opcode - ensures we're parsing the correct PDU type */ + if (u8ptr[0] != op) { + return NULL; + } return u8ptr + 1; } @@ -138,7 +169,15 @@ ble_att_init_write(uint8_t op, void *payload, int min_len, int actual_len) { uint8_t *u8ptr; - BLE_HS_DBG_ASSERT(actual_len >= min_len); + /* Validate payload pointer */ + if (payload == NULL) { + return NULL; + } + + /* Validate buffer length - critical for preventing buffer overflows */ + if (actual_len < min_len) { + return NULL; + } u8ptr = payload; u8ptr[0] = op; @@ -154,6 +193,11 @@ ble_att_error_rsp_parse(const void *payload, int len, src = ble_att_init_parse(BLE_ATT_OP_ERROR_RSP, payload, BLE_ATT_ERROR_RSP_SZ, len); + if (src == NULL) { + /* Validation failed - zero out destination to avoid using uninitialized data */ + memset(dst, 0, sizeof(*dst)); + return; + } dst->baep_req_op = src->baep_req_op; dst->baep_handle = le16toh(src->baep_handle); @@ -168,6 +212,10 @@ ble_att_error_rsp_write(void *payload, int len, dst = ble_att_init_write(BLE_ATT_OP_ERROR_RSP, payload, BLE_ATT_ERROR_RSP_SZ, len); + if (dst == NULL) { + /* Validation failed - cannot write */ + return; + } dst->baep_req_op = src->baep_req_op; dst->baep_handle = htole16(src->baep_handle); diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c index e2f16c9d4..94333b235 100644 --- a/nimble/host/src/ble_att_svr.c +++ b/nimble/host/src/ble_att_svr.c @@ -91,7 +91,6 @@ static os_membuf_t ble_att_svr_prep_entry_mem[ #endif static struct os_mempool ble_att_svr_prep_entry_pool; -static struct os_mempool ble_att_svr_entry_pool; #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -211,8 +210,19 @@ int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle) { struct ble_att_svr_entry *entry; for (idx = start_handle; idx <= end_group_handle; idx++) { entry = ble_att_svr_find_by_handle(idx); + if (entry == NULL) { + /* Handle not found - skip (gaps in ATT handle space are common) */ + if (idx == UINT16_MAX) { + break; /* Prevent wrap-around infinite loop */ + } + continue; + } STAILQ_REMOVE(&ble_att_svr_list, entry, ble_att_svr_entry, ha_next); ble_att_svr_entry_free(entry); + + if (idx == UINT16_MAX) { + break; /* Prevent wrap-around infinite loop */ + } } return 0; } @@ -404,6 +414,11 @@ ble_att_svr_check_perms(uint16_t conn_handle, int is_read, } ble_hs_unlock(); + if (conn == NULL) { + *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN; + return BLE_HS_ATT_ERR(*out_att_err); + } + rc = ble_store_read_peer_sec(&key_sec, &value_sec); if (rc == 0 && value_sec.ltk_present) { *out_att_err = BLE_ATT_ERR_INSUFFICIENT_ENC; @@ -426,10 +441,15 @@ ble_att_svr_check_perms(uint16_t conn_handle, int is_read, if (author) { /* XXX: Prompt user for authorization. */ + int authorized = 0; ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); + if (conn != NULL) { + authorized = conn->bhc_sec_state.authorize; + } ble_hs_unlock(); - if(!conn->bhc_sec_state.authorize){ + + if (conn == NULL || !authorized) { rc = ble_gap_authorize_event(conn_handle, entry->ha_handle_id, is_read); if (rc == BLE_GAP_AUTHORIZE_REJECT) { *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHOR; @@ -809,6 +829,7 @@ ble_att_svr_build_mtu_rsp(uint16_t conn_handle, struct os_mbuf **rxom, if (cmd == NULL) { *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -1081,8 +1102,9 @@ ble_att_svr_fill_type_value_entry(struct os_mbuf *om, uint16_t first, uint16_t u16; int rsp_sz; int rc; + int orig_len = OS_MBUF_PKTHDR(om)->omp_len; - rsp_sz = OS_MBUF_PKTHDR(om)->omp_len + 4; + rsp_sz = orig_len + 4; if (rsp_sz > mtu) { return 0; } @@ -1090,18 +1112,21 @@ ble_att_svr_fill_type_value_entry(struct os_mbuf *om, uint16_t first, put_le16(&u16, first); rc = os_mbuf_append(om, &u16, 2); if (rc != 0) { - *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES; - return BLE_HS_ENOMEM; + goto fail; } put_le16(&u16, last); rc = os_mbuf_append(om, &u16, 2); if (rc != 0) { - *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES; - return BLE_HS_ENOMEM; + goto fail; } return BLE_HS_EAGAIN; + +fail: + os_mbuf_adj(om, orig_len - OS_MBUF_PKTLEN(om)); + *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + return BLE_HS_ENOMEM; } static int @@ -1380,12 +1405,13 @@ static void ble_att_svr_make_conn_aware(uint16_t conn_handle) { ble_hs_conn_addrs(conn, &addrs); for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { - if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, - addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val)) { + if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, + addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { ble_gatts_conn_aware_states[i].aware = false; ble_gatts_conn_aware_states[i].half_aware = 1; } } + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); } static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) { @@ -1393,6 +1419,7 @@ static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) { struct ble_hs_conn_addrs addrs; conn = ble_hs_conn_find_assert(conn_handle); + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); if (conn->bhc_gatt_svr.half_aware == 1) { conn->bhc_gatt_svr.half_aware = 0; @@ -1401,7 +1428,7 @@ static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) { ble_hs_conn_addrs(conn, &addrs); for(int i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, - addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val)) { + addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { ble_gatts_conn_aware_states[i].half_aware = 0; ble_gatts_conn_aware_states[i].aware = true; } @@ -1412,6 +1439,7 @@ static bool ble_att_svr_check_conn_aware(uint16_t conn_handle) { static uint8_t * ble_att_svr_get_csfs(uint16_t conn_handle) { struct ble_hs_conn *conn; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); conn = ble_hs_conn_find_assert(conn_handle); return conn->bhc_gatt_svr.peer_cl_sup_feat; } @@ -1573,10 +1601,10 @@ ble_att_svr_rx_read_type(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx start_handle = le16toh(req->batq_start_handle); end_handle = le16toh(req->batq_end_handle); + err_handle = start_handle; if (start_handle > end_handle || start_handle == 0) { att_err = BLE_ATT_ERR_INVALID_HANDLE; - err_handle = start_handle; rc = BLE_HS_EBADDATA; goto done; } @@ -1604,7 +1632,7 @@ ble_att_svr_rx_read_type(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx ble_hs_conn_addrs(conn, &addrs); for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, - addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val)) { + addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { ble_gatts_conn_aware_states[i].aware = true; ble_gatts_conn_aware_states[i].half_aware = 0; } @@ -1821,6 +1849,7 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle, uint16_t cid, *err_handle = handle; goto done; } + } rc = 0; @@ -1849,8 +1878,7 @@ ble_att_svr_rx_read_mult(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 ) { + if(ble_att_svr_get_csfs(conn_handle)[0] & 1) { if (!ble_att_svr_check_conn_aware(conn_handle)) { att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC; rc = BLE_HS_EREJECT; @@ -1863,11 +1891,12 @@ ble_att_svr_rx_read_mult(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx #endif rc = ble_att_svr_build_read_mult_rsp(conn_handle, cid, rxom, &txom, &att_err, - &err_handle); + &err_handle); + if (rc != 0) { + goto done; + } -#if MYNEWT_VAL(BLE_GATT_CACHING) - done : -#endif +done: return ble_att_svr_tx_rsp(conn_handle, cid, rc, txom, BLE_ATT_OP_READ_MULT_REQ, att_err, err_handle); } @@ -1902,6 +1931,12 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid, } tmp = os_msys_get_pkthdr(2, 0); + if (tmp == NULL) { + *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + *err_handle = 0; + rc = BLE_HS_ENOMEM; + goto done; + } /* Iterate through requested handles, reading the corresponding attribute * for each. Stop when there are no more handles to process, or the @@ -1929,7 +1964,12 @@ ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid, goto done; } tuple_len = OS_MBUF_PKTLEN(tmp); - rc = os_mbuf_append(txom, &tuple_len, sizeof(tuple_len)); + if (OS_MBUF_PKTLEN(txom) + 2 + tuple_len > mtu) { + rc = 0; + goto done; + } + uint16_t le_len = htole16(tuple_len); + rc = os_mbuf_append(txom, &le_len, sizeof(le_len)); if (rc != 0) { *err_handle = handle; goto done; @@ -1973,8 +2013,7 @@ ble_att_svr_rx_read_mult_var(uint16_t conn_handle, uint16_t cid, struct os_mbuf #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 ) { + if(ble_att_svr_get_csfs(conn_handle)[0] & 1) { if (!ble_att_svr_check_conn_aware(conn_handle)) { att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC; rc = BLE_HS_EREJECT; @@ -1987,11 +2026,12 @@ ble_att_svr_rx_read_mult_var(uint16_t conn_handle, uint16_t cid, struct os_mbuf #endif rc = ble_att_svr_build_read_mult_rsp_var(conn_handle, cid, rxom, &txom, &att_err, - &err_handle); + &err_handle); + if (rc != 0) { + goto done; + } -#if MYNEWT_VAL(BLE_GATT_CACHING) done: -#endif return ble_att_svr_tx_rsp(conn_handle, cid, rc, txom, BLE_ATT_OP_READ_MULT_VAR_REQ, att_err, err_handle); @@ -2203,7 +2243,6 @@ ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle, uint16_t cid, } rc = 0; - done: if (rc == 0) { if (start_group_handle != 0) { @@ -2216,7 +2255,7 @@ done: * end handle of 0xffff so that the client knows there are no * more attributes without needing to send a follow-up request. */ - end_group_handle = 0xffff; + end_group_handle = 0xFFFF; } rc = ble_att_svr_read_group_type_entry_write(txom, mtu, @@ -2291,9 +2330,6 @@ ble_att_svr_rx_read_group_type(uint16_t conn_handle, uint16_t cid, struct os_mbu ble_hs_lock(); ble_att_svr_make_conn_aware(conn_handle); ble_hs_unlock(); - if(rc != 0) { - goto done; - } #endif om_uuid_len = OS_MBUF_PKTHDR(*rxom)->omp_len - sizeof(*req); @@ -2342,12 +2378,15 @@ 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; } if (ble_att_cmd_prepare(BLE_ATT_OP_WRITE_RSP, 0, txom) == NULL) { *att_err = BLE_ATT_ERR_INSUFFICIENT_RES; rc = BLE_HS_ENOMEM; + txom = NULL; goto done; } @@ -2428,6 +2467,7 @@ int ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) { #if !MYNEWT_VAL(BLE_ATT_SVR_WRITE_NO_RSP) + os_mbuf_free_chain(*rxom); return BLE_HS_ENOTSUP; #endif @@ -2436,6 +2476,7 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf * if ((ble_att_svr_get_csfs(conn_handle)[0] & 1) && !ble_att_svr_check_conn_aware(conn_handle)) { ble_hs_unlock(); + os_mbuf_free_chain(*rxom); return BLE_HS_EREJECT; } ble_hs_unlock(); @@ -2520,6 +2561,10 @@ ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf * /* Authentication procedure */ len = OS_MBUF_PKTLEN(*rxom) + sizeof(value_sec.sign_counter) + 1; message = nimble_platform_mem_calloc(1,len); + if (message == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } message[0] = BLE_ATT_OP_SIGNED_WRITE_CMD; os_mbuf_copydata(*rxom, 0, OS_MBUF_PKTLEN(*rxom), &message[1]); @@ -2925,7 +2970,12 @@ ble_att_svr_rx_prep_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r } /* adjust for ATT header */ - os_mbuf_prepend(txom, 1); + txom = os_mbuf_prepend(txom, 1); + if (txom == NULL) { + att_err = BLE_ATT_ERR_INSUFFICIENT_RES; + rc = BLE_HS_ENOMEM; + goto done; + } txom->om_data[0] = BLE_ATT_OP_PREP_WRITE_RSP; rc = 0; @@ -2948,7 +2998,7 @@ ble_att_svr_rx_exec_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **r struct ble_hs_conn *conn; struct os_mbuf *txom; uint16_t err_handle; - uint8_t att_err; + uint8_t att_err = 0; uint8_t flags; int rc; @@ -2958,8 +3008,7 @@ ble_att_svr_rx_exec_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 ) { + if(ble_att_svr_get_csfs(conn_handle)[0] & 1) { if (!ble_att_svr_check_conn_aware(conn_handle)) { att_err = BLE_ATT_ERR_DB_OUT_OF_SYNC; rc = BLE_HS_EREJECT; @@ -3059,6 +3108,8 @@ ble_att_svr_rx_notify(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom) * security established shall be pass to application */ if (ble_hs_cfg.sm_sec_lvl >= 2 && !sec_state.encrypted) { + os_mbuf_free_chain(*rxom); + *rxom = NULL; return 0; } @@ -3089,7 +3140,8 @@ 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) { - return BLE_HS_ENOMEM; + rc = BLE_HS_ENOMEM; + goto done; } req = (struct ble_att_tuple_list *)(*rxom)->om_data; @@ -3115,6 +3167,7 @@ ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid, struct os_mbuf * rc = os_mbuf_appendfrom(tmp, *rxom, 0, attr_len); if (rc) { + os_mbuf_free_chain(tmp); BLE_HS_LOG_ERROR("not enough resources, aborting"); rc = BLE_ATT_ERR_INSUFFICIENT_RES; goto done; @@ -3256,6 +3309,8 @@ ble_att_svr_rx_indicate(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxo * security established shall be pass to application */ if (ble_hs_cfg.sm_sec_lvl >= 2 && !sec_state.encrypted) { + os_mbuf_free_chain(*rxom); + *rxom = NULL; goto done; } @@ -3522,6 +3577,12 @@ int ble_att_get_database_size(int *out_size) for (entry = STAILQ_FIRST(&ble_att_svr_list); entry != NULL; entry = STAILQ_NEXT(entry, ha_next)) { + + if (entry->ha_uuid->type != BLE_UUID_TYPE_16) { + /* Only include if fill function is also updated to handle these */ + continue; + } + uuid = (ble_uuid16_t*)entry->ha_uuid; if(uuid->value == BLE_ATT_UUID_PRIMARY_SERVICE || @@ -3539,16 +3600,22 @@ int ble_att_get_database_size(int *out_size) return rc; } - /* handle + type + inc_svc_att_handle + end_grp_handle + uuid */ - size += (8 + ble_uuid_length(&service_uuid.u)); + /* handle + type + inc_svc_att_handle + end_grp_handle + [uuid] */ + size += (4 + (service_uuid.u.type == BLE_UUID_TYPE_16 ? 6 : 4)); } else if(uuid->value == BLE_ATT_UUID_CHARACTERISTIC) { /* uuid is stored in the value attribute */ - entry = STAILQ_NEXT(entry, ha_next); + struct ble_att_svr_entry *next_entry = STAILQ_NEXT(entry, ha_next); + if (next_entry == NULL) { + size += 4; // Updated + continue; + } /* handle(2 bytes) + type(2 bytes) + properties(1 byte) + val_handle(2 bytes) + uuid */ - size += (7 + ble_uuid_length(entry->ha_uuid)); + size += (7 + ble_uuid_length(next_entry->ha_uuid)); + /* Advance entry to skip the value attribute which was just accounted for */ + entry = next_entry; } else if(uuid->value == 0x2901 || uuid->value == 0x2902 || @@ -3558,7 +3625,7 @@ int ble_att_get_database_size(int *out_size) /* handle + type */ size += 4; } - else if(uuid->value == 0x2901) { + else if(uuid->value == 0x2900) { /* handle + type + value(2 bytes) */ size += 6; } @@ -3582,6 +3649,9 @@ int ble_att_fill_database_info(uint8_t *out_data) for (entry = STAILQ_FIRST(&ble_att_svr_list); entry != NULL; entry = STAILQ_NEXT(entry, ha_next)) { + if (entry->ha_uuid->type != BLE_UUID_TYPE_16) { + continue; + } uuid = BLE_UUID16(entry->ha_uuid); if(uuid->value == BLE_ATT_UUID_PRIMARY_SERVICE || uuid->value == BLE_ATT_UUID_SECONDARY_SERVICE) { diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c index f969927a4..d397677fc 100644 --- a/nimble/host/src/ble_cs.c +++ b/nimble/host/src/ble_cs.c @@ -219,9 +219,9 @@ ble_cs_call_result_cb(const struct ble_hci_ev_le_subev_cs_subevent_result *ev) event.type = BLE_CS_EVENT_SUBEVET_RESULT; event.subev_result.conn_handle = le16toh(ev->conn_handle); event.subev_result.config_id = ev->config_id; - event.subev_result.start_acl_conn_event_counter=ev->start_acl_conn_event_counter; - event.subev_result.procedure_counter=ev->procedure_counter; - event.subev_result.frequency_compensation=ev->frequency_compensation; + event.subev_result.start_acl_conn_event_counter = le16toh(ev->start_acl_conn_event_counter); + event.subev_result.procedure_counter = le16toh(ev->procedure_counter); + event.subev_result.frequency_compensation = le16toh(ev->frequency_compensation); event.subev_result.reference_power_level=ev->reference_power_level; event.subev_result.procedure_done_status=ev->procedure_done_status; event.subev_result.subevent_done_status=ev->subevent_done_status; @@ -241,7 +241,7 @@ static void ble_cs_call_result_continue_cb(const struct ble_hci_ev_le_subev_cs_s event.subev_result_continue.conn_handle=le16toh(ev->conn_handle); event.subev_result_continue.config_id=ev->config_id; event.subev_result_continue.procedure_done_status=ev->procedure_done_status; - event.subev_result_continue.subevent_done_status=ev->procedure_done_status; + event.subev_result_continue.subevent_done_status=ev->subevent_done_status; event.subev_result_continue.abort_reason=ev->abort_reason; event.subev_result_continue.num_antenna_paths=ev->num_antenna_paths; event.subev_result_continue.num_steps_reported=ev->num_steps_reported; @@ -356,7 +356,9 @@ ble_cs_set_def_settings(const struct ble_cs_set_def_settings_cp *cmd, BLE_HCI_OCF_LE_CS_SET_DEF_SETTINGS), &cp, sizeof(cp), &rp, sizeof(rp)); - rsp->conn_handle = le16toh(rp.conn_handle); + if (rc == 0) { + rsp->conn_handle = le16toh(rp.conn_handle); + } return rc; } @@ -480,7 +482,9 @@ ble_cs_set_proc_params(const struct ble_cs_set_proc_params_cp *cmd, BLE_HCI_OCF_LE_CS_SET_PROC_PARAMS), &cp, sizeof(cp), &rp, sizeof(rp)); - rsp->conn_handle = le16toh(rp.conn_handle); + if (rc == 0) { + rsp->conn_handle = le16toh(rp.conn_handle); + } return rc; } @@ -525,7 +529,11 @@ ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data set_cmd.role_enable = 0x00; - rc = ble_gap_conn_find(ev->conn_handle, &desc); + uint16_t conn_handle = le16toh(ev->conn_handle); + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc != 0) { + return rc; + } if (desc.role == BLE_GAP_ROLE_MASTER) { set_cmd.role_enable |= 1; } else{ @@ -538,7 +546,7 @@ ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data rc = ble_cs_set_def_settings(&set_cmd, &set_rsp); if (rc) { - BLE_HS_LOG(INFO, "Failed to set the default CS settings, err %dt", rc); + BLE_HS_LOG(INFO, "Failed to set the default CS settings, err %d", rc); return rc; } @@ -568,7 +576,7 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, int rc=0; if (len != sizeof(*ev) || ev->status) { - //return BLE_HS_ECONTROLLER; + return BLE_HS_ECONTROLLER; } cmd.conn_handle = le16toh(ev->conn_handle); @@ -597,7 +605,10 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, cmd.mode_0_steps = 0x03; /* Take the Initiator role */ - ble_gap_conn_find(ev->conn_handle, &desc); + rc = ble_gap_conn_find(cmd.conn_handle, &desc); + if (rc != 0) { + return rc; + } cmd.role= 0x00; if (desc.role == BLE_GAP_ROLE_MASTER) { @@ -645,7 +656,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, const struct ble_hci_ev_le_subev_cs_sec_enable_complete *ev = data; struct ble_gap_conn_desc desc; - if (len != sizeof(*ev)) { + if (len != sizeof(*ev) || ev->status) { BLE_HS_LOG(INFO, "Failed to enable CS security BLE_HS_ECNOTEROLLER"); return BLE_HS_ECONTROLLER; @@ -688,7 +699,8 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, rc = ble_cs_set_proc_params(&cmd, &rsp); if (rc) { BLE_HS_LOG(INFO, "Failed to set CS procedure parameters"); - }else{ + return rc; + } else { BLE_HS_LOG(INFO, "CS procedure parameters set"); } @@ -696,18 +708,22 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, enable_cmd.config_id = 0x00; enable_cmd.enable = 0x01; - ble_gap_conn_find(ev->conn_handle, &desc); -if (desc.role == BLE_GAP_ROLE_MASTER) { - - rc = ble_cs_proc_enable(&enable_cmd); - if (rc) { - BLE_HS_LOG(INFO, "Failed to enable CS procedure"); - }else{ - BLE_HS_LOG(INFO, "CS procedure enabled"); + uint16_t conn_handle = le16toh(ev->conn_handle); + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc != 0) { + return rc; } - return rc; -} -return 0; + if (desc.role == BLE_GAP_ROLE_MASTER) { + + rc = ble_cs_proc_enable(&enable_cmd); + if (rc) { + BLE_HS_LOG(INFO, "Failed to enable CS procedure"); + } else { + BLE_HS_LOG(INFO, "CS procedure enabled"); + } + return rc; + } + return 0; } int @@ -727,18 +743,23 @@ ble_hs_hci_evt_le_cs_config_complete(uint8_t subevent, const void *data, cmd.conn_handle = le16toh(ev->conn_handle); /* Exchange CS security keys */ - rc = ble_gap_conn_find(ev->conn_handle, &desc); + uint16_t conn_handle = le16toh(ev->conn_handle); + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc != 0) { + return rc; + } if (desc.role == BLE_GAP_ROLE_MASTER) { rc = ble_cs_sec_enable(&cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to enable CS security"); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), BLE_ERR_UNSPECIFIED); + return rc; } } - return 0; + return rc; } int @@ -769,10 +790,21 @@ ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data, steps_remaining = event->num_steps_reported; step_ptr = (void *)event->steps; - while (steps_remaining) { + uint8_t *data_end = (uint8_t *)data + len; + + while (steps_remaining > 0) { + if ((uint8_t *)step_ptr + sizeof(struct cs_steps_data) > data_end) { + MODLOG_DFLT(INFO, "Invalid step_ptr bounds"); + return BLE_HS_ECONTROLLER; + } step_size = sizeof(struct cs_steps_data) + ((struct cs_steps_data *)step_ptr)->data_len; + if ((uint8_t *)step_ptr + step_size > data_end || step_size <= 0) { + MODLOG_DFLT(INFO, "Invalid step_size"); + return BLE_HS_ECONTROLLER; + } + expected_len += step_size; - step_ptr += step_size; + step_ptr = (uint8_t *)step_ptr + step_size; steps_remaining--; } @@ -798,11 +830,22 @@ ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data expected_len = sizeof(*event); steps_remaining = event->num_steps_reported; step_ptr = (void *)event->steps; + uint8_t *data_end = (uint8_t *)data + len; - while (steps_remaining) { + while (steps_remaining > 0) { + /* Check bounds before dereferencing step_ptr */ + if ((uint8_t *)step_ptr + sizeof(struct cs_steps_data) > data_end) { + MODLOG_DFLT(INFO, "Invalid step_ptr bounds in continue event"); + return BLE_HS_ECONTROLLER; + } step_size = sizeof(struct cs_steps_data) + ((struct cs_steps_data *)step_ptr)->data_len; - step_ptr += step_size; + if ((uint8_t *)step_ptr + step_size > data_end || step_size <= 0) { + MODLOG_DFLT(INFO, "Invalid step_size in continue event"); + return BLE_HS_ECONTROLLER; + } + expected_len += step_size; + step_ptr += step_size; steps_remaining--; } @@ -850,7 +893,7 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p rc = ble_cs_rd_rem_supp_cap(&cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to read local supported CS capabilities," - "err %dt", rc); + "err %d", rc); } return rc; diff --git a/nimble/host/src/ble_ead.c b/nimble/host/src/ble_ead.c index be497b525..79b325f0e 100644 --- a/nimble/host/src/ble_ead.c +++ b/nimble/host/src/ble_ead.c @@ -4,24 +4,20 @@ * SPDX-License-Identifier: Unlicense OR CC0-1.0 */ -#include "os/queue.h" + #include #include "host/ble_ead.h" #include "host/ble_aes_ccm.h" #if MYNEWT_VAL(ENC_ADV_DATA) -#ifdef __cplusplus -extern "C" { -#endif +static const uint8_t ble_ead_aad[] = {0xEA}; -static uint8_t ble_ead_aad[] = {0xEA}; - -static int ble_ead_rand(void *buf, size_t len) +static int ble_ead_rand(void *buf, int len) { int rc; rc = ble_hs_hci_util_rand(buf, len); if (rc != 0) { - return -1; + return rc; } return 0; } @@ -44,18 +40,23 @@ static int ble_ead_generate_nonce(const uint8_t iv[BLE_EAD_IV_SIZE], const uint8_t randomizer[BLE_EAD_RANDOMIZER_SIZE], uint8_t *nonce) { uint8_t new_randomizer[BLE_EAD_RANDOMIZER_SIZE]; + const uint8_t *rand_src = randomizer; - if (randomizer == NULL) { + if (iv == NULL || nonce == NULL) { + return BLE_HS_EINVAL; + } + + if (rand_src == NULL) { int err; err = ble_ead_generate_randomizer(new_randomizer); if (err != 0) { BLE_HS_LOG(DEBUG, "Failed to generate Randomizer"); - return -1; + return err; } - randomizer = new_randomizer; + rand_src = new_randomizer; } - memcpy(&nonce[0], randomizer, BLE_EAD_RANDOMIZER_SIZE); + memcpy(&nonce[0], rand_src, BLE_EAD_RANDOMIZER_SIZE); memcpy(&nonce[BLE_EAD_RANDOMIZER_SIZE], iv, BLE_EAD_IV_SIZE); return 0; @@ -71,18 +72,18 @@ static int ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_ /** Nonce is concatenation of Randomizer and IV */ err = ble_ead_generate_nonce(iv, randomizer, nonce); if (err != 0) { - return -1; + return err; } - /** Copying Randomizer to the start of encrypted advertisment data */ + /** Copying Randomizer to the start of encrypted advertisement data */ memcpy(encrypted_payload, nonce, BLE_EAD_RANDOMIZER_SIZE); err = ble_aes_ccm_encrypt(session_key, nonce, payload, payload_size, ble_ead_aad, BLE_EAD_AAD_SIZE, &encrypted_payload[BLE_EAD_RANDOMIZER_SIZE], BLE_EAD_MIC_SIZE); if (err != 0) { - BLE_HS_LOG(DEBUG, "Failed to encrypt the payload (ble_ccm_encrypt err %d)", err); - return -1; + BLE_HS_LOG(DEBUG, "Failed to encrypt the payload (err %d)", err); + return err; } return 0; @@ -116,6 +117,11 @@ int ble_ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i "Randomizer and the MIC."); } + /* 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)) { + return BLE_HS_EINVAL; + } + return ead_encrypt(session_key, iv, NULL, payload, payload_size, encrypted_payload); } @@ -125,6 +131,12 @@ static int ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_ { int err; uint8_t nonce[BLE_EAD_NONCE_SIZE]; + + /* Defense-in-depth: Validate size to prevent underflow (size_t is unsigned) */ + if (encrypted_payload_size < BLE_EAD_RANDOMIZER_SIZE + BLE_EAD_MIC_SIZE) { + return BLE_HS_EINVAL; + } + const uint8_t *encrypted_ad_data = &encrypted_payload[BLE_EAD_RANDOMIZER_SIZE]; size_t encrypted_ad_data_size = encrypted_payload_size - BLE_EAD_RANDOMIZER_SIZE; size_t payload_size = encrypted_ad_data_size - BLE_EAD_MIC_SIZE; @@ -133,15 +145,15 @@ static int ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_ err = ble_ead_generate_nonce(iv, randomizer, nonce); if (err != 0) { - return -1; + return err; } err = ble_aes_ccm_decrypt(session_key, nonce, encrypted_ad_data, payload_size, ble_ead_aad, BLE_EAD_AAD_SIZE, payload, BLE_EAD_MIC_SIZE); if (err != 0) { - BLE_HS_LOG(DEBUG, "Failed to decrypt the data (ble_ccm_decrypt err %d)", err); - return -1; + BLE_HS_LOG(DEBUG, "Failed to decrypt the data"); + return BLE_HS_EAUTHEN; } return 0; @@ -182,7 +194,3 @@ int ble_ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i } #endif /* ENC_ADV_DATA */ - -#ifdef __cplusplus -} -#endif diff --git a/nimble/host/src/ble_eatt.c b/nimble/host/src/ble_eatt.c index 4ccc7202c..fd6d816d0 100644 --- a/nimble/host/src/ble_eatt.c +++ b/nimble/host/src/ble_eatt.c @@ -29,9 +29,7 @@ #include "ble_l2cap_priv.h" #include "ble_eatt_priv.h" #include "services/gatt/ble_svc_gatt.h" -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) #include "esp_nimble_mem.h" -#endif struct ble_eatt { SLIST_ENTRY(ble_eatt) next; @@ -77,9 +75,7 @@ static struct os_mempool ble_eatt_conn_pool; struct os_mbuf_pool ble_eatt_sdu_os_mbuf_pool; static struct os_mempool ble_eatt_sdu_mbuf_mempool; static struct ble_gap_event_listener ble_eatt_listener; -struct os_mbuf_pool ble_eatt_sdu_os_mbuf_pool; -static struct ble_npl_event g_read_sup_cl_feat_ev; -static struct ble_npl_event g_read_sup_srv_feat_ev; + static struct ble_eatt_list g_ble_eatt_list; #endif @@ -110,13 +106,12 @@ static ble_eatt_ctx_t *ble_eatt_ctx; #define ble_eatt_sdu_os_mbuf_pool (ble_eatt_ctx->sdu_os_mbuf_pool) #define ble_eatt_sdu_mbuf_mempool (ble_eatt_ctx->sdu_mbuf_mempool) #define ble_eatt_listener (ble_eatt_ctx->eatt_listener) -#define g_read_sup_cl_feat_ev (ble_eatt_ctx->read_sup_cl_feat_ev) -#define g_read_sup_srv_feat_ev (ble_eatt_ctx->read_sup_srv_feat_ev) #define g_ble_eatt_list (ble_eatt_ctx->eatt_list) #endif static void ble_eatt_setup_cb(struct ble_npl_event *ev); static void ble_eatt_start(uint16_t conn_handle); +static void ble_gatt_eatt_read_cl_uuid(struct ble_npl_event *ev); static struct ble_eatt * ble_eatt_find_not_busy(uint16_t conn_handle) @@ -203,17 +198,27 @@ ble_eatt_wakeup_cb(struct ble_npl_event *ev) struct os_mbuf *txom; struct os_mbuf_pkthdr *omp; struct ble_l2cap_chan_info info; + int rc; eatt = ble_npl_event_get_arg(ev); assert(eatt); - omp = STAILQ_FIRST(&eatt->eatt_tx_q); - if (omp != NULL) { + if (!eatt->chan) { + return; + } + + while ((omp = STAILQ_FIRST(&eatt->eatt_tx_q)) != NULL) { STAILQ_REMOVE_HEAD(&eatt->eatt_tx_q, omp_next); txom = OS_MBUF_PKTHDR_TO_MBUF(omp); ble_l2cap_get_chan_info(eatt->chan, &info); - ble_eatt_tx(eatt->conn_handle, info.dcid, txom); + rc = ble_eatt_tx(eatt->conn_handle, info.scid, txom); + + /* Break if channel is stalled or busy - both will reschedule wakeup */ + if (rc == BLE_HS_ESTALLED || rc == BLE_HS_EBUSY) { + break; + } + /* On success (0) or other errors: continue to next packet */ } } @@ -274,8 +279,9 @@ ble_eatt_l2cap_event_fn(struct ble_l2cap_event *event, void *arg) break; case BLE_L2CAP_EVENT_COC_DISCONNECTED: BLE_EATT_LOG_DEBUG("eatt: Disconnected \n"); - ble_eatt_free(eatt); + /* Call event handler before freeing to avoid use-after-free */ ble_gap_eatt_event(event->disconnect.conn_handle, 1, event->disconnect.chan->scid); + ble_eatt_free(eatt); break; case BLE_L2CAP_EVENT_COC_ACCEPT: BLE_EATT_LOG_DEBUG("eatt: Accept request\n"); @@ -297,7 +303,15 @@ ble_eatt_l2cap_event_fn(struct ble_l2cap_event *event, void *arg) ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); break; case BLE_L2CAP_EVENT_COC_DATA_RECEIVED: - assert(eatt->chan == event->receive.chan); + if (eatt->chan != event->receive.chan) { + os_mbuf_free_chain(event->receive.sdu_rx); + return BLE_HS_EUNKNOWN; + } + if (OS_MBUF_PKTLEN(event->receive.sdu_rx) < 1) { + os_mbuf_free_chain(event->receive.sdu_rx); + ble_l2cap_disconnect(eatt->chan); + return BLE_HS_EREJECT; + } opcode = event->receive.sdu_rx->om_data[0]; if (ble_att_is_response_op(opcode)) { ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); @@ -309,11 +323,16 @@ ble_eatt_l2cap_event_fn(struct ble_l2cap_event *event, void *arg) * * Disconnect peer with invalid behavior. */ + os_mbuf_free_chain(event->receive.sdu_rx); ble_l2cap_disconnect(eatt->chan); return BLE_HS_EREJECT; } - assert(!ble_gap_conn_find(event->receive.conn_handle, &desc)); + if (ble_gap_conn_find(event->receive.conn_handle, &desc)) { + os_mbuf_free_chain(event->receive.sdu_rx); + ble_l2cap_disconnect(eatt->chan); + return BLE_HS_EREJECT; + } /* As per BLE 5.4 Standard, Vol. 3, Part G, section 5.3.2 * (ENHANCED ATT BEARER L2CAP INTEROPERABILITY REQUIREMENTS: * Channel Requirements): @@ -323,6 +342,7 @@ ble_eatt_l2cap_event_fn(struct ble_l2cap_event *event, void *arg) * encryption. */ if (!desc.sec_state.encrypted) { + os_mbuf_free_chain(event->receive.sdu_rx); ble_l2cap_disconnect(eatt->chan); return BLE_HS_EREJECT; } @@ -398,24 +418,25 @@ ble_gatt_eatt_read_cl_uuid_cb(uint16_t conn_handle, if (error == NULL || (error->status != 0 && error->status != BLE_HS_EDONE)) { BLE_EATT_LOG_DEBUG("eatt: Cannot find Client Supported features on peer device\n"); - return BLE_HS_EDONE; - } - - if (attr == NULL) { - BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n"); - return BLE_HS_EDONE; + return 0; } if (error->status == 0) { + if (attr == NULL) { + BLE_EATT_LOG_ERROR("eatt: Invalid attribute \n"); + return 0; + } + client_supported_feat = MYNEWT_VAL(BLE_CLIENT_SUPPORTED_FEATURES); rc = ble_gattc_write_flat(conn_handle, attr->handle, &client_supported_feat, 1, ble_gatt_eatt_write_cl_cb, NULL); BLE_EATT_LOG_DEBUG("eatt: %s , write rc = %d \n", __func__, rc); - assert(rc == 0); - return 0; + if (rc != 0) { + return rc; + } } - return BLE_HS_EDONE; + return 0; } static int @@ -443,8 +464,11 @@ ble_gatt_eatt_read_uuid_cb(uint16_t conn_handle, } if (supported_features & 0x01) { - ble_npl_event_set_arg(&g_read_sup_cl_feat_ev, (void *)((uintptr_t) conn_handle)); - ble_npl_eventq_put(ble_hs_evq_get(), &g_read_sup_cl_feat_ev); + 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; } @@ -455,6 +479,7 @@ ble_gatt_eatt_read_svr_uuid(struct ble_npl_event *ev) uint16_t conn_handle; conn_handle = (uint16_t)((uintptr_t)(ble_npl_event_get_arg(ev))); + nimble_platform_mem_free(ev); ble_gattc_read_by_uuid(conn_handle, 1, 0xffff, BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_SERVER_SUPPORTED_FEAT_UUID16), @@ -467,6 +492,7 @@ ble_gatt_eatt_read_cl_uuid(struct ble_npl_event *ev) uint16_t conn_handle; conn_handle = (uint16_t)((uintptr_t)(ble_npl_event_get_arg(ev))); + nimble_platform_mem_free(ev); ble_gattc_read_by_uuid(conn_handle, 1, 0xffff, BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_CLIENT_SUPPORTED_FEAT_UUID16), @@ -496,13 +522,23 @@ ble_eatt_gap_event(struct ble_gap_event *event, void *arg) BLE_EATT_LOG_DEBUG("eatt: Encryption enabled, connecting EATT (conn_handle=0x%04x)\n", event->enc_change.conn_handle); - ble_npl_event_set_arg(&g_read_sup_srv_feat_ev, (void *)((uintptr_t)(event->enc_change.conn_handle))); - ble_npl_eventq_put(ble_hs_evq_get(), &g_read_sup_srv_feat_ev); + 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_svr_uuid, (void *)((uintptr_t)(event->enc_change.conn_handle))); + ble_npl_eventq_put(ble_hs_evq_get(), ev); + } break; case BLE_GAP_EVENT_DISCONNECT: - eatt = ble_eatt_find_by_conn_handle(event->disconnect.conn.conn_handle); - assert(eatt == NULL); + while ((eatt = ble_eatt_find_by_conn_handle(event->disconnect.conn.conn_handle)) != NULL) { + if (eatt->chan == NULL) { + /* No active L2CAP channel; safe to free here */ + ble_eatt_free(eatt); + } else { + /* L2CAP disconnect callback will handle cleanup */ + break; + } + } break; default: break; @@ -539,7 +575,7 @@ ble_eatt_release_chan(uint16_t conn_handle, uint8_t op) eatt = ble_eatt_find_by_conn_handle_and_busy_op(conn_handle, op); if (!eatt) { BLE_EATT_LOG_DEBUG("ble_eatt_release_chan:" - "EATT not found for conn_handle 0x%04x, operation 0x%02\n", + "EATT not found for conn_handle 0x%04x, operation 0x%02x\n", conn_handle, op); return; } @@ -569,13 +605,19 @@ ble_eatt_tx(uint16_t conn_handle, uint16_t cid, struct os_mbuf *txom) if (rc == BLE_HS_ESTALLED) { BLE_EATT_LOG_DEBUG("ble_eatt_tx: Eatt stalled"); + /* L2CAP owns the mbuf; COC_TX_UNSTALLED event will fire when ready */ + /* Do NOT re-queue - this would cause use-after-free */ + return rc; } else if (rc == BLE_HS_EBUSY) { BLE_EATT_LOG_DEBUG("ble_eatt_tx: Message queued"); STAILQ_INSERT_HEAD(&eatt->eatt_tx_q, OS_MBUF_PKTHDR(txom), omp_next); ble_npl_eventq_put(ble_hs_evq_get(), &eatt->wakeup_ev); + return rc; } else { BLE_EATT_LOG_ERROR("eatt: %s, ERROR %d ", __func__, rc); - assert(0); + /* Free mbuf and return error gracefully instead of crashing */ + os_mbuf_free_chain(txom); + return rc; } done: return 0; @@ -696,13 +738,33 @@ ble_eatt_init(ble_eatt_att_rx_fn att_rx_cb) BLE_HS_DBG_ASSERT_EVAL(rc == 0); rc = ble_gap_event_listener_register(&ble_eatt_listener, ble_eatt_gap_event, NULL); - ble_l2cap_create_server(BLE_EATT_PSM, MYNEWT_VAL(BLE_EATT_MTU), ble_eatt_l2cap_event_fn, NULL); + if (rc != 0) { + goto err; + } - ble_npl_event_init(&g_read_sup_srv_feat_ev, ble_gatt_eatt_read_svr_uuid, NULL); - ble_npl_event_init(&g_read_sup_cl_feat_ev, ble_gatt_eatt_read_cl_uuid, NULL); + rc = ble_l2cap_create_server(BLE_EATT_PSM, MYNEWT_VAL(BLE_EATT_MTU), ble_eatt_l2cap_event_fn, NULL); + if (rc != 0) { + ble_gap_event_listener_unregister(&ble_eatt_listener); + goto err; + } + + SLIST_INIT(&g_ble_eatt_list); ble_eatt_att_rx_cb = att_rx_cb; return 0; + +err: +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) + nimble_platform_mem_free(ble_eatt_sdu_coc_mem); + ble_eatt_sdu_coc_mem = NULL; + nimble_platform_mem_free(ble_eatt_conn_mem); + ble_eatt_conn_mem = NULL; +#endif + nimble_platform_mem_free(ble_eatt_ctx); + ble_eatt_ctx = NULL; +#endif + return rc; } #endif diff --git a/nimble/host/src/ble_eddystone.c b/nimble/host/src/ble_eddystone.c index eccb3e988..f3578c742 100644 --- a/nimble/host/src/ble_eddystone.c +++ b/nimble/host/src/ble_eddystone.c @@ -31,6 +31,10 @@ #define BLE_EDDYSTONE_FRAME_TYPE_UID 0x00 #define BLE_EDDYSTONE_FRAME_TYPE_URL 0x10 +#define BLE_EDDYSTONE_UID_LEN 16 +#define BLE_EDDYSTONE_UID_RFU_LEN 2 +#define BLE_EDDYSTONE_UID_SVC_DATA_LEN (1 + BLE_EDDYSTONE_UID_LEN + BLE_EDDYSTONE_UID_RFU_LEN) + static ble_uuid16_t ble_eddystone_uuids16[BLE_EDDYSTONE_MAX_UUIDS16 + 1]; static uint8_t ble_eddystone_svc_data[BLE_EDDYSTONE_MAX_SVC_DATA_LEN]; @@ -88,8 +92,15 @@ ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields, ble_eddystone_uuids16[0] = (ble_uuid16_t) BLE_UUID16_INIT(BLE_EDDYSTONE_SERVICE_UUID); - memcpy(ble_eddystone_uuids16 + 1, adv_fields->uuids16, - adv_fields->num_uuids16 * sizeof(ble_uuid16_t)); + + /* Only copy if there are UUIDs and the pointer is valid */ + if (adv_fields->num_uuids16 > 0) { + if (adv_fields->uuids16 == NULL) { + return BLE_HS_EINVAL; + } + memcpy(ble_eddystone_uuids16 + 1, adv_fields->uuids16, + adv_fields->num_uuids16 * sizeof(ble_uuid16_t)); + } adv_fields->uuids16 = ble_eddystone_uuids16; adv_fields->num_uuids16++; adv_fields->uuids16_is_complete = 1; @@ -108,28 +119,36 @@ ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields, int ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, - void *uid, int8_t measured_power) + const void *uid, int8_t measured_power) { uint8_t *svc_data; int rc; + /* Validate inputs before modifying global state */ + if (adv_fields == NULL) { + return BLE_HS_EINVAL; + } + if (uid == NULL) { + return BLE_HS_EINVAL; + } + if (measured_power < -127 || measured_power > 20) { + return BLE_HS_EINVAL; + } + /* Eddystone UUID and frame type (0). */ svc_data = ble_eddystone_set_svc_data_base(BLE_EDDYSTONE_FRAME_TYPE_UID); /* Measured Power ranging data (Calibrated tx power at 0 meters). */ - if (measured_power < -100 || measured_power > 20) { - return BLE_HS_EINVAL; - } svc_data[0] = measured_power; /* UID. */ - memcpy(svc_data + 1, uid, 16); + memcpy(svc_data + 1, uid, BLE_EDDYSTONE_UID_LEN); /* Reserved. */ - svc_data[17] = 0x00; - svc_data[18] = 0x00; + svc_data[1 + BLE_EDDYSTONE_UID_LEN] = 0x00; + svc_data[1 + BLE_EDDYSTONE_UID_LEN + 1] = 0x00; - rc = ble_eddystone_set_adv_data_gen(adv_fields, 19); + rc = ble_eddystone_set_adv_data_gen(adv_fields, BLE_EDDYSTONE_UID_SVC_DATA_LEN); if (rc != 0) { return rc; } @@ -139,7 +158,7 @@ ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, int ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields, - uint8_t url_scheme, char *url_body, + uint8_t url_scheme, const char *url_body, uint8_t url_body_len, uint8_t url_suffix, int8_t measured_power) { @@ -147,24 +166,40 @@ ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields, int url_len; int rc; + /* Validate all inputs before modifying global state */ + if (adv_fields == NULL) { + return BLE_HS_EINVAL; + } + if (url_body == NULL && url_body_len > 0) { + return BLE_HS_EINVAL; + } + if (measured_power < -127 || measured_power > 20) { + return BLE_HS_EINVAL; + } + if (url_scheme > BLE_EDDYSTONE_URL_SCHEME_HTTPS) { + return BLE_HS_EINVAL; + } + if (url_suffix != BLE_EDDYSTONE_URL_SUFFIX_NONE && url_suffix > BLE_EDDYSTONE_URL_SUFFIX_GOV) { + return BLE_HS_EINVAL; + } + url_len = url_body_len; if (url_suffix != BLE_EDDYSTONE_URL_SUFFIX_NONE) { url_len++; } - if (url_len > BLE_EDDYSTONE_URL_MAX_LEN) { + if (url_len < 1 || url_len > BLE_EDDYSTONE_URL_MAX_LEN) { return BLE_HS_EINVAL; } svc_data = ble_eddystone_set_svc_data_base(BLE_EDDYSTONE_FRAME_TYPE_URL); /* Measured Power ranging data (Calibrated tx power at 0 meters). */ - if (measured_power < -100 || measured_power > 20) { - return BLE_HS_EINVAL; - } svc_data[0] = measured_power; svc_data[1] = url_scheme; - memcpy(svc_data + 2, url_body, url_body_len); + if (url_body_len > 0) { + memcpy(svc_data + 2, url_body, url_body_len); + } if (url_suffix != BLE_EDDYSTONE_URL_SUFFIX_NONE) { svc_data[2 + url_body_len] = url_suffix; } diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index f0d93f46f..8819d6d9a 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -623,23 +623,6 @@ ble_gap_fill_conn_desc(struct ble_hs_conn *conn, ble_hs_conn_addrs(conn, &addrs); desc->our_id_addr = addrs.our_id_addr; -#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - /* Check if the privacy is enabled, change the address type accordingly - * */ - if (ble_host_rpa_enabled()) - { - uint8_t *local_id = NULL; - struct ble_hs_resolv_entry *rl = NULL; - rl = ble_hs_resolv_list_find(conn->bhc_peer_addr.val); - - if (rl != NULL) { - /* Get public ID address here */ - ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL); - memcpy(desc->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN); - desc->our_id_addr.type = BLE_ADDR_PUBLIC; - } - } -#endif desc->peer_id_addr = addrs.peer_id_addr; desc->our_ota_addr = addrs.our_ota_addr; desc->peer_ota_addr = addrs.peer_ota_addr; @@ -722,19 +705,31 @@ ble_gap_read_rem_ver_info(uint16_t conn_handle, uint8_t *version, uint16_t *manu #if NIMBLE_BLE_CONNECT struct ble_hs_conn *conn; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - ble_hs_unlock(); - if (conn == NULL ) { + ble_hs_unlock(); return BLE_HS_ENOTCONN; } - *version = conn->bhc_rd_rem_ver_params.version; - *manufacturer = conn->bhc_rd_rem_ver_params.manufacturer; - *subversion = conn->bhc_rd_rem_ver_params.subversion; + struct ble_gap_read_rem_ver_params params = conn->bhc_rd_rem_ver_params; + ble_hs_unlock(); + + if (version != NULL) { + *version = params.version; + } + if (manufacturer != NULL) { + *manufacturer = params.manufacturer; + } + if (subversion != NULL) { + *subversion = params.subversion; + } #endif return 0; } @@ -806,6 +801,11 @@ ble_gap_set_priv_mode(const ble_addr_t *peer_addr, uint8_t priv_mode) if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } + + if (peer_addr == NULL) { + return BLE_HS_EINVAL; + } + #if MYNEWT_VAL(BLE_HS_PVCY) return ble_hs_pvcy_set_mode(peer_addr, priv_mode); #else @@ -851,8 +851,12 @@ ble_gap_read_le_phy(uint16_t conn_handle, uint8_t *tx_phy, uint8_t *rx_phy) return BLE_HS_ECONTROLLER; } - *tx_phy = rsp.tx_phy; - *rx_phy = rsp.rx_phy; + if (tx_phy != NULL) { + *tx_phy = rsp.tx_phy; + } + if (rx_phy != NULL) { + *rx_phy = rsp.rx_phy; + } return 0; #else @@ -995,7 +999,11 @@ uint8_t* ble_resolve_adv_data(const uint8_t *adv_data, uint8_t adv_type, uint8_t rc = ble_hs_adv_find_field(adv_type, adv_data, adv_data_len, &fields); /*Fill adv field*/ if (rc == 0) { - *length = fields->length -1 ; /* minus length of type*/ + if (fields->length > 0) { + *length = fields->length - 1; /* minus length of type*/ + } else { + *length = 0; + } data = fields->value; /* Type specific adv data*/ return (uint8_t*)data; } @@ -1534,21 +1542,14 @@ ble_gap_master_ticks_until_exp(void) static uint32_t ble_gap_slave_ticks_until_exp(void) { - ble_npl_stime_t ticks; - if (ble_gap_slave[0].op == BLE_GAP_OP_NULL || !ble_gap_slave[0].exp_set) { /* Timer not set; infinity ticks until next event. */ return BLE_HS_FOREVER; } - ticks = ble_gap_slave[0].exp_os_ticks - ble_npl_time_get(); - if (ticks > 0) { - /* Timer not expired yet. */ - return ticks; - } + ble_npl_stime_t ticks = (ble_npl_stime_t)(ble_gap_slave[0].exp_os_ticks - ble_npl_time_get()); - /* Timer just expired. */ - return 0; + return (ticks > 0) ? (uint32_t)ticks : 0; } #endif @@ -1727,7 +1728,6 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - ble_hs_unlock(); // Send disconnect event in slave role if connect was sent #if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) @@ -1744,11 +1744,13 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) } } + ble_hs_unlock(); + /* If we never posted a connect event for a slave (send == 0), treat this as * a connection failure and post a connect-failed event instead of a * disconnect. This allows applications to restart advertising. */ - if (!send && !(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) + if (conn != NULL && !send && !(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { ble_gap_event_connect_call(conn_handle, BLE_HS_EAGAIN); } @@ -2496,6 +2498,10 @@ ble_gap_rx_connless_iq_report(const struct ble_hci_ev_le_subev_connless_iq_rpt * /* The handle must be in the list */ ble_hs_lock(); psync = ble_hs_periodic_sync_find_by_handle(sync_handle); + if (psync == NULL) { + ble_hs_unlock(); + return; + } cb = psync->cb; cb_arg = psync->cb_arg; ble_hs_unlock(); @@ -2719,7 +2725,11 @@ ble_gap_rx_periodic_adv_subev_data_req(const struct ble_hci_ev_le_subev_periodic event.periodic_adv_subev_data_req.subevent_start = ev->subevent_start; event.periodic_adv_subev_data_req.subevent_data_count = ev->subevent_data_count; - ble_gap_slave_extract_cb(ev->adv_handle, &cb, &cb_arg); + if (ev->adv_handle >= BLE_ADV_INSTANCES) { + return; + } + + ble_gap_slave_get_cb(ev->adv_handle, &cb, &cb_arg); if (cb != NULL) { cb(&event, cb_arg); } @@ -2740,7 +2750,11 @@ ble_gap_rx_periodic_adv_response(const struct ble_gap_periodic_adv_response resp memcpy(&event.periodic_adv_response, &resp, sizeof(struct ble_gap_periodic_adv_response)); - ble_gap_slave_extract_cb(resp.adv_handle, &cb, &cb_arg); + 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); } @@ -2996,7 +3010,7 @@ ble_gap_rx_conn_complete(struct ble_gap_conn_complete *evt, uint8_t instance) ble_hs_conn_addrs(conn, &addrs); for (int i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if (memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, - addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val)) { + addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { conn->bhc_gatt_svr.half_aware = ble_gatts_conn_aware_states[i].half_aware; conn->bhc_gatt_svr.aware_state = ble_gatts_conn_aware_states[i].aware; } @@ -3080,25 +3094,28 @@ ble_gap_rx_rd_rem_sup_feat_complete(const struct ble_hci_ev_le_subev_rd_rem_used } if ((conn != NULL) && (conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { + if (ev->status == 0) { #if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) - conn->supported_feat = !!(get_le32(ev->features) & BLE_HS_HCI_LE_FEAT_CONN_PARAM_REQUEST); + conn->supported_feat = !!(get_le32(ev->features) & BLE_HS_HCI_LE_FEAT_CONN_PARAM_REQUEST); #else - conn->supported_feat = get_le32(ev->features); + conn->supported_feat = get_le32(ev->features); #endif + } ble_gap_rd_rem_ver_tx(ev->conn_handle); } else { - if ((conn != NULL) && (ev->status == 0)) { + if (conn != NULL) { + if (ev->status == 0) { #if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) - conn->supported_feat = !!(get_le32(ev->features) & BLE_HS_HCI_LE_FEAT_CONN_PARAM_REQUEST); + conn->supported_feat = !!(get_le32(ev->features) & BLE_HS_HCI_LE_FEAT_CONN_PARAM_REQUEST); #else - conn->supported_feat = get_le32(ev->features); + conn->supported_feat = get_le32(ev->features); #endif - } - - if (conn != NULL) { + } ble_gap_event_connect_call(ev->conn_handle, ev->status); conn->slave_conn = 1; + } else if (ev->status != 0) { + ble_gap_event_connect_call(ev->conn_handle, ev->status); } } #endif @@ -3121,16 +3138,19 @@ ble_gap_rx_rd_rem_ver_info_complete(const struct ble_hci_ev_rd_rem_ver_info_cmp return; } + /* Check conn before dereferencing */ + if (conn == NULL) { + return; + } + 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; - if ((conn != NULL) && !(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { + if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { ble_gap_rd_rem_sup_feat_tx(ev->conn_handle); } else { - if (conn != NULL) { - ble_gap_event_connect_call(ev->conn_handle, 0); - } + ble_gap_event_connect_call(ev->conn_handle, 0); } #endif } @@ -3936,13 +3956,14 @@ ble_gap_adv_set_data(const uint8_t *data, int data_len) return BLE_HS_EDISABLED; } - /* Check for valid parameters */ - if (((data == NULL) && (data_len != 0)) || - (data_len > BLE_HCI_MAX_ADV_DATA_LEN)) { + if (data_len < 0 || data_len > BLE_HCI_MAX_ADV_DATA_LEN || + ((data == NULL) && (data_len != 0))) { return BLE_ERR_INV_HCI_CMD_PARMS; } - memcpy(cmd.adv_data, data, data_len); + if (data_len > 0) { + memcpy(cmd.adv_data, data, data_len); + } cmd.adv_data_len = data_len; opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_DATA); @@ -4293,7 +4314,7 @@ ble_gap_ext_adv_set_addr(uint8_t instance, const ble_addr_t *addr) int rc; ble_addr_t invalid_non_rpa_addr, invalid_static_rand_addr; - if (instance >= BLE_ADV_INSTANCES || addr->type != BLE_ADDR_RANDOM) { + if (addr == NULL || instance >= BLE_ADV_INSTANCES || addr->type != BLE_ADDR_RANDOM) { return BLE_HS_EINVAL; } @@ -4350,6 +4371,14 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) return BLE_HS_EINVAL; } + if (duration < 0 || duration > 0xFFFF) { + return BLE_HS_EINVAL; + } + + if (max_events < 0 || max_events > 0xFF) { + return BLE_HS_EINVAL; + } + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -4568,6 +4597,10 @@ ble_gap_ext_adv_set_data_validate(uint8_t instance, struct os_mbuf *data) static int ble_gap_ext_adv_set(uint8_t instance, uint16_t opcode, struct os_mbuf **data) { + if (data == NULL || *data == NULL) { + return BLE_HS_EINVAL; + } + /* in that case we always fit all data in single HCI command */ #if MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE) <= BLE_HCI_MAX_EXT_ADV_DATA_LEN #if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -4701,7 +4734,13 @@ done: static int ble_gap_ext_adv_rsp_set_validate(uint8_t instance, struct os_mbuf *data) { - uint16_t len = OS_MBUF_PKTLEN(data); + uint16_t len; + + if (data != NULL) { + len = OS_MBUF_PKTLEN(data); + } else { + len = 0; + } if (!ble_gap_slave[instance].configured) { return BLE_HS_EINVAL; @@ -4916,12 +4955,19 @@ ble_gap_periodic_adv_params_validate( return BLE_HS_EINVAL; } - if (params->itvl_min && params->itvl_min < 6) { + if ((params->itvl_min && params->itvl_min < 6) || + (params->itvl_max && params->itvl_max < 6)) { return BLE_HS_EINVAL; } - if (params->itvl_max && params->itvl_max < 6) { - return BLE_HS_EINVAL; + + if (params->itvl_min && params->itvl_max && params->itvl_min > params->itvl_max) { + return BLE_HS_EINVAL; } + + if ((params->itvl_min == 0) != (params->itvl_max == 0)) { + return BLE_HS_EINVAL; + } + return 0; } @@ -4949,7 +4995,7 @@ 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(); - return ENOMEM; + return BLE_HS_ENOMEM; } /* Periodic advertising shall not be configured while it is already @@ -5419,7 +5465,7 @@ ble_gap_periodic_adv_sync_create_cancel(void) if (ble_gap_sync.op != BLE_GAP_OP_SYNC) { ble_hs_unlock(); - return BLE_HS_EBUSY; + return BLE_HS_EALREADY; } opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -5762,6 +5808,10 @@ 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) { + return BLE_HS_EINVAL; + } + if ((peer_addr->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -5786,7 +5836,7 @@ 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->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) { + if (peer_addr == NULL || (peer_addr->type > BLE_ADDR_RANDOM) || (adv_sid > 0x0f)) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -5810,6 +5860,10 @@ ble_gap_clear_periodic_adv_list(void) uint16_t opcode; int rc = 0; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST); rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, NULL, 0); @@ -5824,6 +5878,14 @@ ble_gap_read_periodic_adv_list_size(uint8_t *per_adv_list_size) uint16_t opcode; int rc = 0; + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + + if (per_adv_list_size == NULL) { + return BLE_HS_EINVAL; + } + opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE); rc = ble_hs_hci_cmd_tx(opcode, NULL, 0, &rsp, sizeof(rsp)); @@ -5902,7 +5964,12 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents, cmd->num_subevents = 0; buf_offset = 0; for (int i = 0; i < num_subevents; i++) { - if ((buf_offset + OS_MBUF_PKTLEN(params[i].data) + param_size) > buf_size) { + if (!params[i].data || OS_MBUF_PKTLEN(params[i].data) > UINT8_MAX) { + ble_hs_unlock(); + rc = BLE_HS_EINVAL; + goto done; + } + if ((2 + buf_offset + OS_MBUF_PKTLEN(params[i].data) + param_size) > buf_size) { if (!cmd->num_subevents) { ble_hs_unlock(); rc = BLE_HS_EINVAL; @@ -5927,7 +5994,7 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents, (void*)&subev_data_len); subevents->subevent_data_length = OS_MBUF_PKTLEN(params[i].data); subevents = (struct periodic_adv_subevents *)( - (uint32_t)subevents + sizeof(struct periodic_adv_subevents) + subev_data_len); + (uintptr_t)subevents + sizeof(struct periodic_adv_subevents) + subev_data_len); } rc = ble_hs_hci_cmd_tx(opcode, cmd, buf_offset + 2, NULL, 0); @@ -5951,7 +6018,11 @@ int ble_gap_periodic_adv_set_response_data(uint16_t sync_handle, struct ble_gap_periodic_adv_response_params *param, struct os_mbuf *data) { - struct ble_hci_le_set_periodic_adv_response_data *cmd; + if (param == NULL) { + return BLE_HS_EINVAL; + } + + struct ble_hci_le_set_periodic_adv_response_data *cmd; uint16_t opcode; int len = sizeof(*cmd); int data_len = 0; @@ -5977,7 +6048,11 @@ 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); - return ble_hs_hci_cmd_tx(opcode, cmd, len + data_len, NULL, 0); + if (len + data_len > 255) { + return BLE_HS_EINVAL; + } + + return ble_hs_hci_cmd_tx(opcode, cmd, len + data_len, NULL, 0); } int ble_gap_periodic_adv_sync_subev(uint16_t sync_handle, uint8_t include_tx_power, @@ -5990,9 +6065,11 @@ int ble_gap_periodic_adv_sync_subev(uint16_t sync_handle, uint8_t include_tx_pow uint16_t opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_SYNC_SUBEVENT); int rc; + ble_hs_lock(); psync = ble_hs_periodic_sync_find_by_handle(sync_handle); + ble_hs_unlock(); + if (!psync) { - ble_hs_unlock(); return BLE_HS_ENOTCONN; } @@ -6096,6 +6173,10 @@ ble_gap_ext_disc_enable_tx(uint8_t enable, uint8_t filter_duplicates, int ble_gap_set_connless_cte_transmit_params(uint8_t instance, const struct ble_gap_periodic_adv_cte_params *params) { + if (params == NULL) { + return BLE_HS_EINVAL; + } + uint8_t buf[sizeof(struct ble_hci_le_set_connless_cte_tx_params_cp) + params->switching_pattern_length]; struct ble_hci_le_set_connless_cte_tx_params_cp *cmd = (void *)buf; @@ -6140,6 +6221,10 @@ int ble_gap_set_connless_iq_sampling_enable(uint16_t sync_handle, uint8_t sampling_enable, uint8_t max_sampled_ctes, const struct ble_gap_cte_sampling_params *cte_sampling_params) { + if (cte_sampling_params == 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]; struct ble_hci_le_set_connless_iq_sampling_enable_cp *cmd = (struct ble_hci_le_set_connless_iq_sampling_enable_cp*)buf; @@ -6164,19 +6249,19 @@ 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) { + return BLE_HS_EDISABLED; + } + uint8_t buf[sizeof(struct ble_hci_le_set_conn_cte_rx_params_cp) + cte_sampling_params->switching_pattern_length]; 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); uint16_t opcode; - if (!ble_hs_is_enabled()) { - return BLE_HS_EDISABLED; - } - opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_CONN_CTE_RX_PARAMS); - cmd->conn_handle = conn_handle; + 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; @@ -6612,6 +6697,10 @@ ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms, #if MYNEWT_VAL(BLE_EXT_ADV) struct ble_gap_ext_disc_params p = {0}; + if (disc_params == NULL) { + return BLE_HS_EINVAL; + } + if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -8025,7 +8114,7 @@ static struct ble_gap_update_entry * ble_gap_update_entry_find(uint16_t conn_handle, struct ble_gap_update_entry **out_prev) { - struct ble_gap_update_entry *entry; + struct ble_gap_update_entry *entry = NULL; struct ble_gap_update_entry *prev; BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); @@ -8319,10 +8408,14 @@ done: } ble_hs_lock(); - if (rc == 0) { + dup = ble_gap_update_entry_find(conn_handle, NULL); + if (rc == 0 && dup == NULL) { SLIST_INSERT_HEAD(&ble_gap_update_entries, entry, next); } else { ble_gap_update_entry_free(entry); + if (dup != NULL) { + rc = BLE_HS_EALREADY; + } STATS_INC(ble_gap_stats, update_fail); } ble_hs_unlock(); @@ -8469,14 +8562,16 @@ done: int ble_gap_dev_authorization(uint16_t conn_handle, bool authorized) { + struct ble_hs_conn *conn; + ble_hs_lock(); - struct ble_hs_conn *conn = ble_hs_conn_find(conn_handle); - ble_hs_unlock(); + conn = ble_hs_conn_find(conn_handle); if (conn != NULL) { if (!(conn->bhc_sec_state.authenticated)) { // Device should be authenticated before authorization BLE_HS_LOG(ERROR, "Authorized should occur after successful Authentication(MITM protection) \n"); + ble_hs_unlock(); return BLE_HS_EAUTHOR; } @@ -8487,7 +8582,10 @@ ble_gap_dev_authorization(uint16_t conn_handle, bool authorized) conn->bhc_sec_state.authorize = 0; } - } else { + } + ble_hs_unlock(); + + if (conn == NULL) { // Connection handle not found BLE_HS_LOG(ERROR, "Can't find connection \n"); return BLE_HS_ENOTCONN; @@ -8610,6 +8708,13 @@ ble_gap_unpair(const ble_addr_t *peer_addr) return BLE_HS_EINVAL; } + /* We cannot delete entry from resolving list if there is ongoing + * discovery or advertising in progress. Check early before any + * state modifications to avoid inconsistency. */ + if (ble_gap_adv_active() || ble_gap_disc_active()) { + return BLE_HS_EBUSY; + } + ble_hs_lock(); conn = ble_hs_conn_find_by_addr(peer_addr); @@ -8619,10 +8724,6 @@ ble_gap_unpair(const ble_addr_t *peer_addr) ble_hs_unlock(); -#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - ble_hs_pvcy_remove_entry(peer_addr->type, peer_addr->val); -#endif - memset(&key,0,sizeof(key)); key.rpa_rec.peer_rpa_addr = *peer_addr; @@ -8630,6 +8731,14 @@ ble_gap_unpair(const ble_addr_t *peer_addr) if (!rc) { new_addr = &(value.rpa_rec.peer_addr); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_hs_pvcy_remove_entry(new_addr->type, new_addr->val); +#endif + } else { +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* If no rpa_rec, remove original addr directly */ + ble_hs_pvcy_remove_entry(peer_addr->type, peer_addr->val); +#endif } memset(&key, 0, sizeof(key)); @@ -8640,14 +8749,6 @@ ble_gap_unpair(const ble_addr_t *peer_addr) // Checking if the device is in ble_store if (!rc) { if (value.sec.irk_present) { - /* We cannot delete entry from resolving list if there is ongoing - * discovery or advertising in progress */ - - if (ble_gap_adv_active() || - ble_gap_disc_active()) { - return BLE_HS_EBUSY; - } - #if MYNEWT_VAL(BLE_HS_PVCY) // Delete the IRK as it is Distributed rc = ble_hs_pvcy_remove_entry(key.sec.peer_addr.type,key.sec.peer_addr.val); @@ -8753,7 +8854,7 @@ ble_gap_unpair_oldest_except(const ble_addr_t *peer_addr) } if (i >= num_peers) { - return BLE_HS_ENOMEM; + return BLE_HS_ENOENT; } return ble_gap_unpair(&peer_id_addrs[i]); @@ -8947,9 +9048,12 @@ ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle, uint16_t start_handle; uint16_t end_handle; - if (attr_handle == ble_gattc_cache_conn_get_svc_changed_handle(conn_handle)) { - start_handle = get_le16(om->om_data); - end_handle = get_le16(om->om_data + 2); + uint16_t svc_changed_handle = ble_gattc_cache_conn_get_svc_changed_handle(conn_handle); + if (svc_changed_handle != 0xFFFF && attr_handle == svc_changed_handle && OS_MBUF_PKTLEN(om) >= 4) { + uint8_t buf[4]; + os_mbuf_copydata(om, 0, 4, buf); + start_handle = get_le16(buf); + end_handle = get_le16(buf + 2); ble_gattc_cache_conn_update(conn_handle, start_handle, end_handle); } #endif @@ -9312,7 +9416,7 @@ ble_gap_preempt_done(void) if (slaves[i].cb) { #if MYNEWT_VAL(BLE_EXT_ADV) event.adv_complete.instance = i; - event.adv_complete.conn_handle = i; + event.adv_complete.conn_handle = BLE_HS_CONN_HANDLE_NONE; #endif ble_gap_call_event_cb(&event, slaves[i].cb, slaves[i].arg); } @@ -9564,7 +9668,7 @@ ble_gap_set_path_loss_reporting_param(uint16_t conn_handle, cmd.high_hysteresis = high_hysteresis; cmd.low_threshold = low_threshold; cmd.low_hysteresis = low_hysteresis; - cmd.min_time_spent = min_time_spent; + cmd.min_time_spent = htole16(min_time_spent); return ble_hs_hci_cmd_tx(opcode, &cmd, sizeof(cmd), NULL, 0); #else @@ -9643,12 +9747,16 @@ int ble_gap_read_local_irk(uint8_t * out_irk) { const uint8_t * local_irk; + int rc; if (out_irk == NULL) { return BLE_HS_EINVAL; } - ble_hs_pvcy_our_irk(&local_irk); + rc = ble_hs_pvcy_our_irk(&local_irk); + if (rc != 0) { + return rc; + } memcpy(out_irk, local_irk, 16); return 0; @@ -9659,11 +9767,33 @@ ble_gap_read_local_irk(uint8_t * out_irk) static int ble_gap_pcl_param_validate(struct ble_gap_set_auto_pcl_params *params) { - if(params->m1_upper_limit < params->m1_lower_limit || - params->m2_upper_limit < params->m2_lower_limit || - params->s2_upper_limit < params->s2_lower_limit || - params->s8_upper_limit < params->s8_lower_limit) - return BLE_HS_EINVAL; + int8_t eff_upper, eff_lower; + + 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) + return BLE_HS_EINVAL; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + eff_lower = params->m2_lower_limit ? params->m2_lower_limit : ESP_2M_LOW; + eff_upper = params->m2_upper_limit ? params->m2_upper_limit : ESP_2M_HIGH; + if (eff_upper < eff_lower) + return BLE_HS_EINVAL; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + eff_lower = params->s2_lower_limit ? params->s2_lower_limit : ESP_S2_LOW; + eff_upper = params->s2_upper_limit ? params->s2_upper_limit : ESP_S2_HIGH; + if (eff_upper < eff_lower) { + return BLE_HS_EINVAL; + } + + eff_lower = params->s8_lower_limit ? params->s8_lower_limit : ESP_S8_LOW; + eff_upper = params->s8_upper_limit ? params->s8_upper_limit : ESP_S8_HIGH; + if (eff_upper < eff_lower) { + return BLE_HS_EINVAL; + } +#endif return 0; } @@ -9804,7 +9934,7 @@ int ble_gap_set_adv_constant_did(uint16_t handle, uint8_t enable, uint16_t did) #if MYNEWT_VAL(BLE_SCAN_ALLOW_ENH_ADI_FILTER) /* filter list in format struct ble_gap_adi_filter_entry */ -int ble_gap_config_ext_scan_adi_filter(uint8_t enable, uint8_t did_filter, uint8_t sid_cnt, +int ble_gap_config_ext_scan_adi_filter(uint8_t enable, uint8_t did_filter, uint8_t sid_cnt, struct ble_gap_adi_filter_entry *filter_list[]) { uint8_t vs_cmd[64]; @@ -9829,7 +9959,21 @@ int ble_gap_config_ext_scan_adi_filter(uint8_t enable, uint8_t did_filter, uint8 int ble_gap_set_data_related_addr_change_param(uint8_t adv_handle, uint8_t change_reason) { - return ble_hs_hci_util_set_data_addr_change(adv_handle, change_reason); + int rc; + + if (!ble_hs_is_enabled()) { + return BLE_HS_EDISABLED; + } + + if (adv_handle >= BLE_ADV_INSTANCES) { + return BLE_HS_EINVAL; + } + + ble_hs_lock(); + rc = ble_hs_hci_util_set_data_addr_change(adv_handle, change_reason); + ble_hs_unlock(); + + return rc; } #endif diff --git a/nimble/host/src/ble_gattc.c b/nimble/host/src/ble_gattc.c index d70644f0c..68d9fbfc2 100644 --- a/nimble/host/src/ble_gattc.c +++ b/nimble/host/src/ble_gattc.c @@ -1432,9 +1432,8 @@ ble_gattc_ticks_until_resume(void) } now = ble_npl_time_get(); - diff = ble_gattc_resume_at - now; + diff = (int32_t)(ble_gattc_resume_at - now); if (diff <= 0) { - /* Timer already expired; resume immediately. */ return 0; } @@ -1575,7 +1574,7 @@ ble_gattc_recover_gatt_proc(uint16_t conn_handle, int enc_status) proc->write_long.attr.offset, om, proc->write_long.cb, proc->write_long.cb_arg); break; - case BLE_GATT_OP_WRITE_RELIABLE: + case BLE_GATT_OP_WRITE_RELIABLE: for (int i = 0; i < proc->write_reliable.num_attrs; i++) { attrs[i].handle = proc->write_reliable.attrs[i].handle; attrs[i].offset = 0; @@ -2348,6 +2347,9 @@ ble_gattc_find_inc_svcs_rx_read_rsp(struct ble_gattc_proc *proc, int status, /* Report discovered service to application. */ service.start_handle = proc->find_inc_svcs.cur_start; service.end_handle = proc->find_inc_svcs.cur_end; +#if (MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)) + service.handle = proc->find_inc_svcs.prev_handle; +#endif rc = ble_gattc_find_inc_svcs_cb(proc, 0, 0, &service); if (rc != 0) { @@ -3240,13 +3242,19 @@ ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t start_handle, } done: - if (rc != 0) { - STATS_INC(ble_gattc_stats, disc_all_dscs_fail); + /* 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_dscs_fail); } ble_gattc_process_status(proc, rc); @@ -5362,28 +5370,37 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, int rc = 0; int i = 0; + int last_appended_idx = 0; uint16_t cur_chr_cnt = 0; /* mtu = MTU - 1 octet (OP code) */ uint16_t mtu = ble_att_mtu(conn_handle) - 1; struct os_mbuf *txom; struct ble_hs_conn *conn; + bool free_unused_tuples = false; txom = ble_hs_mbuf_att_pkt(); if (txom == NULL) { return BLE_HS_ENOMEM; } + ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); if (conn == NULL) { - return ENOTCONN; + ble_hs_unlock(); + os_mbuf_free_chain(txom); + return BLE_HS_ENOTCONN; } + /* Read peer client supported features under lock */ + uint8_t peer_supports_multi_notify = (conn->bhc_gatt_svr.peer_cl_sup_feat[0] & 0x04); + ble_hs_unlock(); + STATS_INC(ble_gattc_stats, multi_notify); ble_gattc_log_multi_notify(tuples, chr_count); /* Read missing values */ for (i = 0; i < chr_count; i++) { - if (tuples->handle == 0) { + if (tuples[i].handle == 0) { rc = BLE_HS_EINVAL; goto done; } @@ -5393,7 +5410,8 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, */ rc = ble_att_svr_read_local(tuples[i].handle, &tuples[i].value); if (rc != 0) { - BLE_HS_LOG(ERROR, "Attribute read failed (err=0x%02x), rc"); + BLE_HS_LOG(ERROR, "Attribute read failed (err=0x%02x)", rc); + free_unused_tuples = true; goto done; } } @@ -5401,7 +5419,7 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, /* If peer does not support fall back to multiple single value * Notifications */ - if ((conn->bhc_gatt_svr.peer_cl_sup_feat[0] & 0x04) == 0) { + 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); if (rc != 0) { @@ -5428,7 +5446,8 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, /* buffer was consumed, allocate new one */ txom = ble_hs_mbuf_att_pkt(); if (txom == NULL) { - return BLE_HS_ENOMEM; + rc = BLE_HS_ENOMEM; + goto done; } } @@ -5442,20 +5461,35 @@ ble_gatts_notify_multiple_custom(uint16_t conn_handle, /* Value */ os_mbuf_concat(txom, tuples[i].value); cur_chr_cnt++; + last_appended_idx = i; /* Track the last appended index */ } if (cur_chr_cnt == 1) { - rc = ble_att_clt_tx_notify(conn_handle, tuples[chr_count].handle, - tuples[chr_count].value); + /* Use the last appended index, not chr_count which may be out of bounds */ + rc = ble_att_clt_tx_notify(conn_handle, tuples[last_appended_idx].handle, + tuples[last_appended_idx].value); } else { rc = ble_att_clt_tx_notify_mult(conn_handle, txom); } done: if (rc != 0) { + /* Free any mbufs that were allocated by ble_att_svr_read_local */ + for (i = 0; i < chr_count; i++) { + os_mbuf_free_chain(tuples[i].value); + tuples[i].value = NULL; + } STATS_INC(ble_gattc_stats, multi_notify_fail); + if (free_unused_tuples) { + for (i = 0; i < chr_count; i++) { + if (tuples[i].value != NULL) { + os_mbuf_free_chain(tuples[i].value); + } + } + } } + os_mbuf_free_chain(txom); /* 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); @@ -5878,14 +5912,16 @@ ble_gattc_rx_err(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t s proc = ble_gattc_extract_first_by_conn_cid_op(conn_handle, cid, BLE_GATT_OP_NONE); #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) - ble_gap_conn_find(conn_handle, &desc); - proc->error.att_handle = handle; - proc->error.status = status; + if (proc != NULL) { + ble_gap_conn_find(conn_handle, &desc); + proc->error.att_handle = handle; + proc->error.status = status; + } #endif if (proc != NULL) { #if MYNEWT_VAL(BLE_GATTC_AUTO_PAIR) - if (desc.sec_state.encrypted == 0 && + if (proc != NULL && desc.sec_state.encrypted == 0 && (status == BLE_ATT_ERR_INSUFFICIENT_ENC || status == BLE_ATT_ERR_INSUFFICIENT_AUTHEN)) { rc = ble_gap_security_initiate(conn_handle); @@ -6293,12 +6329,13 @@ ble_gattc_connection_broken(uint16_t conn_handle) ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - ble_hs_unlock(); - - while ((omp = STAILQ_FIRST(&conn->att_tx_q)) != NULL) { - STAILQ_REMOVE_HEAD(&conn->att_tx_q, omp_next); - os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); + if (conn != NULL) { + while ((omp = STAILQ_FIRST(&conn->att_tx_q)) != NULL) { + STAILQ_REMOVE_HEAD(&conn->att_tx_q, omp_next); + os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); + } } + ble_hs_unlock(); } /** diff --git a/nimble/host/src/ble_gattc_cache.c b/nimble/host/src/ble_gattc_cache.c index 68730b8ce..4b64a84e5 100644 --- a/nimble/host/src/ble_gattc_cache.c +++ b/nimble/host/src/ble_gattc_cache.c @@ -27,6 +27,9 @@ #include "nimble/storage_port.h" #include "host/ble_gatt.h" #include "esp_nimble_mem.h" +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +#include "nvs.h" +#endif #define GATT_CACHE_PREFIX "gatt_cache" #define INVALID_ADDR_NUM 0xff @@ -39,7 +42,7 @@ static const char *cache_addr = "cache_addr_tab"; static uint8_t ble_gattc_cache_find_addr(ble_addr_t addr); static uint8_t ble_gattc_cache_find_hash(uint8_t * hash_key); #if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -static uint8_t svc_end_handle; +static uint16_t svc_end_handle; struct cache_fn_mapping cache_fn; #endif @@ -65,7 +68,7 @@ static cache_env_t *cache_env = NULL; #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) typedef struct { - uint8_t _svc_end_handle; + uint16_t _svc_end_handle; struct cache_fn_mapping _cache_fn; cache_env_t * _cache_env; } ble_gattc_cache_static_vars_t; @@ -135,7 +138,7 @@ static int cacheErase(cache_handle_t handle) { if (cache_fn.erase_all) { - cache_fn.erase_all(handle); + return cache_fn.erase_all(handle); } return -1; } @@ -278,7 +281,7 @@ ble_gattc_cacheReset(ble_addr_t *addr) cache_fn.close(cache_env->addr_fp); } cache_env->is_open = false; - BLE_HS_LOG(DEBUG, "%s erased entire cache from NVS"); + BLE_HS_LOG(DEBUG, "%s erased entire cache from NVS", __func__); } else { BLE_HS_LOG(INFO, "cache_env status is error"); } @@ -310,14 +313,14 @@ ble_gattc_cache_find_addr(ble_addr_t addr) uint8_t rc = 0; rc = ble_gattc_cache_static_vars_init(); if (rc != 0) { - return rc; + return INVALID_ADDR_NUM; /* Return invalid index on error */ } #endif if (cache_env == NULL) { cache_env = nimble_platform_mem_calloc(1, sizeof(cache_env_t)); if (cache_env == NULL) { - return BLE_HS_ENOMEM; + return INVALID_ADDR_NUM; } } @@ -434,6 +437,7 @@ ble_gattc_fill_nv_attr(struct ble_gattc_cache_conn *peer, size_t num_attr, struc int index = 0; int svc_index = 0; + svc_end_handle = 0; // Reset to support multi-peer caches SLIST_FOREACH(svc, &peer->svcs, next) { ble_gattc_fill_nv_attr_entry(svc->svc.uuid, svc->svc.start_handle, svc->svc.end_handle, @@ -475,12 +479,15 @@ static int ble_gattc_cache_addr_save(uint8_t *out_index, ble_addr_t addr, uint8_t * hash_key) { int rc; - uint8_t num = ++(cache_env->num_addr); uint8_t index = 0; uint8_t insert_ind = 0; uint8_t i = 0; uint8_t *p_buf; + if (cache_env == NULL) { + return BLE_HS_EUNKNOWN; + } + p_buf = nimble_platform_mem_calloc(1,MAX_ADDR_LIST_CACHE_BUF); if (p_buf == NULL) { return BLE_HS_ENOMEM; @@ -491,29 +498,28 @@ ble_gattc_cache_addr_save(uint8_t *out_index, ble_addr_t addr, uint8_t * hash_ke BLE_HS_LOG(DEBUG, "Hash key already present in the cache list"); if ((index = ble_gattc_cache_find_addr(addr)) != INVALID_ADDR_NUM) { - BLE_HS_LOG(DEBUG, "tBD address already present in the cache list"); + BLE_HS_LOG(DEBUG, "BD address already present in the cache list"); /* If the bd_addr already in the address list, update the hash key in it. */ insert_ind = index; } else { - /* - If the bd_addr isn't in the address list, added the bd_addr to the last of the - address list. - */ - if(num > MYNEWT_VAL(BLE_GATT_CACHING_MAX_CONNS)) { + /* Address not found - need to allocate a new slot */ + if(cache_env->num_addr >= MYNEWT_VAL(BLE_GATT_CACHING_MAX_CONNS)) { nimble_platform_mem_free(p_buf); return BLE_HS_ENOMEM; } BLE_HS_LOG(DEBUG, "BD addr not present"); - insert_ind = num - 1; + insert_ind = cache_env->num_addr; + cache_env->num_addr++; /* Increment only when adding new address */ } } else { BLE_HS_LOG(DEBUG, "Hash key not present, saving new data"); - if(num > MYNEWT_VAL(BLE_GATT_CACHING_MAX_CONNS)) { + if(cache_env->num_addr >= MYNEWT_VAL(BLE_GATT_CACHING_MAX_CONNS)) { nimble_platform_mem_free(p_buf); return BLE_HS_ENOMEM; } - insert_ind = num - 1; + insert_ind = cache_env->num_addr; + cache_env->num_addr++; /* Increment only when adding new address */ } print_hash_key(hash_key); @@ -523,9 +529,9 @@ ble_gattc_cache_addr_save(uint8_t *out_index, ble_addr_t addr, uint8_t * hash_ke memcpy(&cache_env->cache_addr[insert_ind].addr, &addr, sizeof(ble_addr_t)); cache_handle_t *fp = &cache_env->addr_fp; - uint16_t length = num * (sizeof(ble_addr_t) + (sizeof(uint8_t) * 16)); + uint16_t length = cache_env->num_addr * (sizeof(ble_addr_t) + (sizeof(uint8_t) * 16)); - for (i = 0; i < num; i++) { + for (i = 0; i < cache_env->num_addr; i++) { memcpy(p_buf + i * (sizeof(ble_addr_t) + sizeof(uint8_t) * 16), &cache_env->cache_addr[i].addr, sizeof(ble_addr_t)); @@ -627,7 +633,12 @@ ble_gattc_cache_load_nv_attr(uint8_t index, int *num_attr) size_t length = 0; struct ble_gatt_nv_attr *nv_attr; - cacheRead(cache_env->cache_addr[index].cache_fp, getKeyname(&cache_env->cache_addr[index].addr), NULL, &length); + rc = cacheRead(cache_env->cache_addr[index].cache_fp, getKeyname(&cache_env->cache_addr[index].addr), NULL, &length); + + if (rc != 0 || length == 0) { + BLE_HS_LOG(ERROR, "Failed to read cache length, rc = %d, length = %zu", rc, length); + return NULL; + } *num_attr = length / (sizeof(ble_gatt_nv_attr)); @@ -827,6 +838,7 @@ ble_gattc_cache_load(ble_addr_t peer_addr) 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); return BLE_HS_EINVAL; } @@ -855,15 +867,20 @@ ble_gattc_cache_load(ble_addr_t peer_addr) } nimble_platform_mem_free(nv_attr); cache_index = ble_gattc_cache_find_addr(peer_addr); + if (cache_index == INVALID_ADDR_NUM) { + BLE_HS_LOG(ERROR, "Address not found in cache"); + return BLE_HS_ENOENT; + } ble_gattc_cache_conn_load_hash(cache_env->cache_addr[cache_index].addr, cache_env->cache_addr[cache_index].hash_key); + cacheClose(peer_addr); return rc; } int ble_gattc_cache_check_hash(struct ble_gattc_cache_conn *peer, struct os_mbuf *om) { - if (peer == NULL || om == NULL) { + if (peer == NULL || om == NULL || om->om_len != 16) { BLE_HS_LOG(ERROR, "Check hash failed"); return -1; } @@ -953,25 +970,31 @@ ble_gattc_cache_init(void *storage_cb) /* Read previously saved blob if available */ if ((rc = cacheRead(fp, cache_key, p_buf, &length)) != 0) { - if (rc != 0) { - BLE_HS_LOG(DEBUG, "%s, Line = %d, storage flash get blob data fail, err_code = 0x%x", - __func__, __LINE__, rc); - } + 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) /* - * rc == 0x1102 indicates NVS key not found. * 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 == 0x1102){ + if (rc == ESP_ERR_NVS_NOT_FOUND){ no_cached_blob = 1; + nimble_platform_mem_free(p_buf); + return 0; } #endif goto error; } num_addr = length / (sizeof(ble_addr_t) + sizeof(uint8_t) * 16); + + if (num_addr > MAX_DEVICE_IN_CACHE) { + BLE_HS_LOG(ERROR, "Corrupted cache: num_addr %d exceeds max %d", num_addr, MAX_DEVICE_IN_CACHE); + rc = BLE_HS_EBADDATA; + goto error; + } + cache_env->num_addr = num_addr; BLE_HS_LOG(DEBUG, "Number of address loaded = %d", cache_env->num_addr); @@ -1002,6 +1025,9 @@ 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) { diff --git a/nimble/host/src/ble_gattc_cache_conn.c b/nimble/host/src/ble_gattc_cache_conn.c index de5edbf72..bdbbd210e 100644 --- a/nimble/host/src/ble_gattc_cache_conn.c +++ b/nimble/host/src/ble_gattc_cache_conn.c @@ -354,7 +354,7 @@ ble_gattc_cache_conn_dsc_add(ble_addr_t peer_addr, uint16_t chr_val_handle, if (prev == NULL) { SLIST_INSERT_HEAD(&chr->dscs, dsc, next); } else { - SLIST_NEXT(prev, next) = dsc; + SLIST_INSERT_AFTER(prev, dsc, next); } BLE_HS_LOG(DEBUG, "Descriptor added with handle = %d", dsc->dsc.handle); @@ -545,12 +545,12 @@ ble_gattc_cache_conn_chr_add(ble_addr_t peer_addr, uint16_t svc_start_handle, if (prev == NULL) { SLIST_INSERT_HEAD(&svc->chrs, chr, next); } else { - SLIST_NEXT(prev, next) = chr; + SLIST_INSERT_AFTER(prev, chr, next); } /* ble_gattc_db_hash_chr_present(chr->chr.uuid.u16); */ - BLE_HS_LOG(DEBUG, "Characteristc added with def_handle = %d and val_handle = %d", + BLE_HS_LOG(DEBUG, "Characteristic added with def_handle = %d and val_handle = %d", chr->chr.def_handle, chr->chr.val_handle); return 0; @@ -997,7 +997,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.val_handle >= start_handle && chr->chr.val_handle <= end_handle) { + if (chr->chr.def_handle >= start_handle && chr->chr.def_handle <= end_handle) { //Updated db_size++; } } @@ -1006,14 +1006,11 @@ ble_gattc_get_db_size_with_type(struct ble_gattc_cache_conn *peer, uint8_t type, case BLE_GATT_DB_DESCRIPTOR: // Iterate through characteristics and their descriptors. SLIST_FOREACH(chr, &svc->chrs, next) { - if (chr->chr.val_handle >= start_handle && chr->chr.val_handle <= end_handle) { - if (chr->chr.val_handle == char_handle) { - SLIST_FOREACH(dsc, &chr->dscs, next) { - if (dsc->dsc.handle >= start_handle && - dsc->dsc.handle <= end_handle && - chr->chr.val_handle == char_handle) { - db_size++; - } + if (chr->chr.val_handle == char_handle) { + SLIST_FOREACH(dsc, &chr->dscs, next) { + if (dsc->dsc.handle >= start_handle && + dsc->dsc.handle <= end_handle) { + db_size++; } } } @@ -1412,6 +1409,13 @@ void ble_gattc_get_gatt_db(uint16_t conn_handle, { struct ble_gattc_cache_conn *cache_conn; + if (db != NULL) { + *db = NULL; + } + if (count != NULL) { + *count = 0; + } + cache_conn = ble_gattc_cache_conn_find(conn_handle); if (cache_conn == NULL) { @@ -1538,14 +1542,19 @@ static void service_sanity_check(struct ble_gattc_cache_conn_svc_list *svcs) struct ble_gattc_cache_conn_chr *chr, *prev = NULL; struct ble_gattc_cache_conn_dsc *dsc; uint16_t end_handle = 0; + SLIST_FOREACH(svc, svcs, next) { if (svc->svc.end_handle == 65535) { + end_handle = svc->svc.start_handle; + prev = NULL; SLIST_FOREACH(chr, &svc->chrs, next) { end_handle = chr->chr.val_handle; prev = chr; } - SLIST_FOREACH(dsc, &prev->dscs, next) { - end_handle = dsc->dsc.handle; + if (prev != NULL) { + SLIST_FOREACH(dsc, &prev->dscs, next) { + end_handle = dsc->dsc.handle; + } } svc->svc.end_handle = end_handle; } @@ -1568,10 +1577,12 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) /* cache the database only if the connection is trusted or database hash exists */ + 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(); chr = ble_gattc_cache_conn_chr_find_uuid(peer, @@ -1579,7 +1590,7 @@ ble_gattc_cache_conn_disc_complete(struct ble_gattc_cache_conn *peer, int rc) BLE_UUID16_DECLARE(BLE_SVC_GATT_CHR_DATABASE_HASH_UUID16)); if (bonded || chr != NULL) { /* persist the cache */ - ble_gattc_cacheReset(&hs_conn->bhc_peer_addr); + ble_gattc_cacheReset(&peer_addr); ble_gattc_cache_conn_cache_peer(peer); /* TODO */ } } else { @@ -2028,6 +2039,7 @@ ble_gattc_cache_conn_update(uint16_t conn_handle, uint16_t start_handle, uint16_ peer = ble_gattc_cache_conn_find(conn_handle); if (peer == NULL) { BLE_HS_LOG(ERROR, "Cannot find connection with conn_handle %d", conn_handle); + return; //Update } peer->cache_state = CACHE_INVALID; @@ -2048,16 +2060,20 @@ int ble_gattc_cache_refresh(ble_addr_t peer_addr) struct ble_gattc_cache_conn *conn; int rc; + ble_hs_lock(); hs_conn = ble_hs_conn_find_by_addr(&peer_addr); if (hs_conn == NULL) { - BLE_HS_LOG(ERROR, "GATT cache refresh: connection not found for peer: %02x:%02x:%02x:%02x:%02x:%02x\n", - peer_addr.val[0], peer_addr.val[1], peer_addr.val[2], - peer_addr.val[3], peer_addr.val[4], peer_addr.val[5]); + ble_hs_unlock(); + BLE_HS_LOG(ERROR, "GATT cache refresh: connection not found for peer: %02x:%02x:%02x:%02x:%02x:%02x\n", + peer_addr.val[0], peer_addr.val[1], peer_addr.val[2], + peer_addr.val[3], peer_addr.val[4], peer_addr.val[5]); return BLE_HS_ENOTCONN; } + ble_hs_unlock(); + /* Lookup cache connection entry */ conn = ble_gattc_cache_conn_find_by_addr(peer_addr); @@ -2141,8 +2157,11 @@ int ble_gattc_cache_clean(ble_addr_t peer_addr) struct ble_hs_conn *hs_conn; struct ble_gattc_cache_conn *conn; + ble_hs_lock(); + // Check if the device is connected hs_conn = ble_hs_conn_find_by_addr(&peer_addr); + if (hs_conn == NULL) { BLE_HS_LOG(WARN, "GATT cache clean: No active connection found for peer:" " %02x:%02x:%02x:%02x:%02x:%02x, Attempting cache clean anyway.", @@ -2150,12 +2169,14 @@ int ble_gattc_cache_clean(ble_addr_t peer_addr) peer_addr.val[3], peer_addr.val[4], peer_addr.val[5]); ble_gattc_cacheReset(&peer_addr); + ble_hs_unlock(); return 0; } // Find cached GATT services for this peer conn = ble_gattc_cache_conn_find_by_addr(peer_addr); if (conn == NULL) { + ble_hs_unlock(); BLE_HS_LOG(WARN, "GATT cache clean: no cache entry found for peer."); return BLE_HS_EUNKNOWN; } @@ -2164,6 +2185,7 @@ int ble_gattc_cache_clean(ble_addr_t peer_addr) ble_gattc_cacheReset(&peer_addr); conn->cache_state = CACHE_INVALID; + ble_hs_unlock(); BLE_HS_LOG(INFO, "GATT cache cleaned for peer: %02x:%02x:%02x:%02x:%02x:%02x", peer_addr.val[0], peer_addr.val[1], peer_addr.val[2], @@ -2250,7 +2272,7 @@ ble_gattc_cache_conn_free_all_mem(void) } int -ble_gattc_cache_conn_init() +ble_gattc_cache_conn_init(void) { int rc; int max_ble_gattc_cache_conns; @@ -2418,15 +2440,19 @@ static int ble_gattc_cache_conn_verify(struct ble_gattc_cache_conn *conn) return 0; } + int bonded = 0; ble_hs_lock(); gap_conn = ble_hs_conn_find(conn->conn_handle); + if (gap_conn != NULL) { + bonded = gap_conn->bhc_sec_state.bonded; + } ble_hs_unlock(); if (gap_conn == NULL) { return BLE_HS_ENOTCONN; } if (conn->cache_state == CACHE_LOADED) { - if (gap_conn->bhc_sec_state.bonded) { + if (bonded) { conn->cache_state = CACHE_VERIFIED; return 0; } @@ -2465,13 +2491,19 @@ static void ble_gattc_cache_search_all_svcs_cb(struct ble_npl_event *ev) op = &conn->pending_op; dcb = op->cb; + + if (SLIST_EMPTY(&conn->svcs)) { + dcb(conn->conn_handle, ble_gattc_cache_error(BLE_HS_EDONE, 0), NULL, op->cb_arg); + return; + } + 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); } } status = BLE_HS_EDONE; - dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg); + dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); return; } @@ -2548,7 +2580,7 @@ ble_gattc_cache_conn_search_svc_by_uuid_cb(struct ble_npl_event *ev) } } status = BLE_HS_EDONE; - dcb(conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg); + dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); //Updated return; } @@ -2612,13 +2644,18 @@ ble_gattc_cache_conn_search_inc_svcs_cb(struct ble_npl_event *ev) #if MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) svc = ble_gattc_cache_conn_svc_find_range(conn, op->start_handle); + if (svc == NULL) { + status = BLE_HS_ENOENT; + dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); + return; + } SLIST_FOREACH(incl_svc, &svc->incl_svc, next) { dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &incl_svc->svc, op->cb_arg); } status = BLE_HS_EDONE; - dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &incl_svc->svc, op->cb_arg); + dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); //Updated #else SLIST_FOREACH(svc, &conn->svcs, next) { if (svc->type == BLE_GATT_SVC_TYPE_SECONDARY && @@ -2627,7 +2664,7 @@ ble_gattc_cache_conn_search_inc_svcs_cb(struct ble_npl_event *ev) } } status = BLE_HS_EDONE; - dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), &svc->svc, op->cb_arg); + dcb(conn->conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); #endif return; } @@ -2692,12 +2729,18 @@ ble_gattc_cache_conn_search_all_chrs_cb(struct ble_npl_event *ev) op = &conn->pending_op; dcb = op->cb; svc = ble_gattc_cache_conn_svc_find_range(conn, op->start_handle); + + if (svc == NULL) { + dcb(conn_handle, ble_gattc_cache_error(BLE_HS_EDONE, 0), NULL, op->cb_arg); + return; + } + /* return all chrs */ SLIST_FOREACH(chr, &svc->chrs, next) { dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, op->cb_arg); } status = BLE_HS_EDONE; - dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, op->cb_arg); + dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); return; } @@ -2755,6 +2798,11 @@ ble_gattc_cache_conn_search_chrs_by_uuid_cb(struct ble_npl_event *ev) op = &conn->pending_op; dcb = op->cb; svc = ble_gattc_cache_conn_svc_find_range(conn, op->start_handle); + if (svc == NULL) { + status = BLE_HS_ENOENT; + dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); + return; + } /* return all chrs */ SLIST_FOREACH(chr, &svc->chrs, next) { if (ble_uuid_cmp(&chr->chr.uuid.u, op->uuid) == 0) { @@ -2762,7 +2810,7 @@ ble_gattc_cache_conn_search_chrs_by_uuid_cb(struct ble_npl_event *ev) } } status = BLE_HS_EDONE; - dcb(conn_handle, ble_gattc_cache_error(status, 0), &chr->chr, op->cb_arg); + dcb(conn_handle, ble_gattc_cache_error(status, 0), NULL, op->cb_arg); return; } @@ -2820,12 +2868,22 @@ ble_gattc_cache_conn_search_all_dscs_cb(struct ble_npl_event *ev) op = &conn->pending_op; dcb = op->cb; svc = ble_gattc_cache_conn_svc_find_range(conn, op->start_handle); + if (svc == NULL) { + dcb(conn_handle, ble_gattc_cache_error(BLE_HS_EINVAL, 0), 0, NULL, op->cb_arg); + return; + } + chr = ble_gattc_cache_conn_chr_find_range(svc, op->start_handle); + if (chr == NULL) { + dcb(conn_handle, ble_gattc_cache_error(BLE_HS_EINVAL, 0), 0, NULL, op->cb_arg); + return; + } + SLIST_FOREACH(dsc, &chr->dscs, next) { dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, op->cb_arg); } status = BLE_HS_EDONE; - dcb(conn_handle, ble_gattc_cache_error(status, 0), chr->chr.val_handle, &dsc->dsc, op->cb_arg); + dcb(conn_handle, ble_gattc_cache_error(status, 0), 0, NULL, op->cb_arg); return; } diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index 4a865956a..60794deac 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -816,6 +816,7 @@ ble_gatts_dsc_access(uint16_t conn_handle, uint16_t attr_handle, gatt_ctxt.op = ble_gatts_dsc_op(att_op); gatt_ctxt.dsc = dsc_def; + gatt_ctxt.offset = offset; ble_gatts_dsc_inc_stat(gatt_ctxt.op); rc = ble_gatts_val_access(conn_handle, attr_handle, offset, &gatt_ctxt, om, @@ -1031,7 +1032,6 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle, *out_cur_clt_cfg_flags = clt_cfg->flags; gatt_op = ble_gatts_dsc_op(att_op); - ble_gatts_dsc_inc_stat(gatt_op); switch (gatt_op) { case BLE_GATT_ACCESS_OP_READ_DSC: @@ -1649,7 +1649,7 @@ ble_gatts_connection_broken(uint16_t conn_handle) ble_hs_conn_addrs(conn, &addrs); for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, - addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val)) { + addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { //Check Thoroughly if(conn->bhc_gatt_svr.half_aware) { ble_gatts_conn_aware_states[i].aware = false; ble_gatts_conn_aware_states[i].half_aware = 0; @@ -1956,7 +1956,7 @@ ble_gatts_start(void) goto done; } - memset (ble_gatts_clt_cfgs, 0, sizeof *ble_gatts_clt_cfgs); + memset(ble_gatts_clt_cfgs, 0, ble_gatts_clt_cfg_size()); #endif /* Fill the cache. */ @@ -2187,7 +2187,7 @@ ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle) #if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int clt_cfg_idx; #endif - int persist; + int persist = 0; int rc; #if MYNEWT_VAL(BLE_GATT_CACHING) uint16_t svc_change_handle; @@ -2684,6 +2684,11 @@ ble_gatts_bonding_established(uint16_t conn_handle) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) 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) { + /* Allocation failed - skip caching state update */ + ble_hs_unlock(); + return; + } } #endif memset(&ble_gatts_conn_aware_states[new_idx], 0, @@ -2739,7 +2744,7 @@ ble_gatts_bonding_restored(uint16_t conn_handle) ble_hs_conn_addrs(conn, &addrs); for(i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS); i++) { if(memcmp(ble_gatts_conn_aware_states[i].peer_id_addr, - addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val)) { + addrs.peer_id_addr.val, sizeof addrs.peer_id_addr.val) == 0) { // Check Thoroughly conn->bhc_gatt_svr.half_aware = ble_gatts_conn_aware_states[i].half_aware; conn->bhc_gatt_svr.aware_state = ble_gatts_conn_aware_states[i].aware; } @@ -2759,7 +2764,10 @@ ble_gatts_bonding_restored(uint16_t conn_handle) ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - BLE_HS_DBG_ASSERT(conn != NULL); + if (conn == NULL) { + ble_hs_unlock(); + break; + } #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, @@ -2815,12 +2823,26 @@ ble_gatts_bonding_restored(uint16_t conn_handle) } memset(&csfc_key, 0, sizeof csfc_key); + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + return; + } csfc_key.peer_addr = conn->bhc_peer_addr; + ble_hs_unlock(); rc = ble_store_read_csfc(&csfc_key, &csfc_value); if (rc != 0) { return; } + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_unlock(); + return; + } memcpy(conn->bhc_gatt_svr.peer_cl_sup_feat, csfc_value.csfc, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); + ble_hs_unlock(); } #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) @@ -2988,8 +3010,8 @@ ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid, if (ble_uuid_cmp(cur->ha_uuid, dsc_uuid) == 0) { if (out_handle != NULL) { *out_handle = cur->ha_handle_id; - return 0; } + return 0; } cur = STAILQ_NEXT(cur, ha_next); } @@ -3007,7 +3029,7 @@ static void ble_gatts_add_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint1 STAILQ_INSERT_TAIL(clt_cfgs, cfg, next); } -static void ble_gatts_remove_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle) { +static int ble_gatts_remove_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle) { struct ble_gatts_clt_cfg *cfg; STAILQ_FOREACH(cfg, clt_cfgs, next) { @@ -3015,10 +3037,14 @@ static void ble_gatts_remove_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, ui break; } } - if (cfg != NULL) { - STAILQ_REMOVE(clt_cfgs, cfg, ble_gatts_clt_cfg, next); - ble_gatts_clt_cfg_free(cfg); + + if (cfg == NULL) { + return BLE_HS_ENOENT; } + + STAILQ_REMOVE(clt_cfgs, cfg, ble_gatts_clt_cfg, next); + ble_gatts_clt_cfg_free(cfg); + return 0; } #if MYNEWT_VAL(BLE_GATT_CACHING) @@ -3036,6 +3062,7 @@ arg[1] : affected chr_val_handle arg[2] : allowed_flags */ static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) { + int rc; uint16_t action = ((uint16_t *) arg)[0]; uint16_t chr_val_handle = ((uint16_t *) arg)[1]; uint16_t allowed_flags; @@ -3049,10 +3076,13 @@ static int ble_gatts_update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) { return 0; case 2: /* removed */ - ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + rc = ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle); - (conn->bhc_gatt_svr.num_clt_cfgs)--; + if (rc == 0) { + (conn->bhc_gatt_svr.num_clt_cfgs)--; + } return 0; + default: BLE_HS_DBG_ASSERT(0); } @@ -3093,15 +3123,19 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { ble_hs_cfg.gatts_register_cb, ble_hs_cfg.gatts_register_arg); if (rc != 0) { - goto done; #if BLE_HS_DEBUG BLE_HS_DBG_ASSERT(0); /* memory leak expected */ #endif + goto done; } ble_gatts_free_svc_defs(); /* Fill the cache. */ cfg = ble_gatts_get_last_cfg(&ble_gatts_clt_cfgs); - ha = ble_att_svr_find_by_handle(cfg->chr_val_handle - 1); + if (cfg == NULL) { + ha = NULL; + } else { + ha = ble_att_svr_find_by_handle(cfg->chr_val_handle - 1); + } 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); @@ -3116,11 +3150,19 @@ int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { } i = 0; entry = ble_gatts_find_svc_entry(&svcs[i]); + if (entry == NULL) { + rc = BLE_HS_ENOENT; + goto done; + } start_handle = entry->handle; while(svcs[i].type != BLE_GATT_SVC_TYPE_END) { i++; } entry = ble_gatts_find_svc_entry(&svcs[i - 1]); + if (entry == NULL) { + rc = BLE_HS_ENOENT; + goto done; + } end_handle = entry->end_group_handle; #if MYNEWT_VAL(BLE_GATT_CACHING) /* make all bonded connections unaware */ diff --git a/nimble/host/src/ble_gatts_lcl.c b/nimble/host/src/ble_gatts_lcl.c index 7e636f81f..cccbee1e1 100644 --- a/nimble/host/src/ble_gatts_lcl.c +++ b/nimble/host/src/ble_gatts_lcl.c @@ -71,27 +71,29 @@ static const char * const ble_gatt_dsc_f_names[] = { #define BLE_CHR_FLAGS_STR_LEN 180 static char * -ble_gatts_flags_to_str(uint16_t flags, char *buf, +ble_gatts_flags_to_str(uint32_t flags, char *buf, const char * const *names) { int bit; bool non_empty = false; size_t length = 0; - buf[0] = '\0'; strcpy(buf, "["); length += 1; for (bit = 0; names[bit]; ++bit) { - if (flags & (1 << bit)) { - length += strlen(names[bit]); - if (length + 1 >= BLE_CHR_FLAGS_STR_LEN) { - return buf; + if (flags & ((uint32_t)1 << bit)) { + size_t name_len = strlen(names[bit]); + size_t extra = (non_empty ? 1 : 0); /* separator */ + /* Check if we have room for separator + name + closing bracket */ + if (length + extra + name_len + 1 >= BLE_CHR_FLAGS_STR_LEN) { + break; } if (non_empty) { strcat(buf, "|"); length += 1; } strcat(buf, names[bit]); + length += name_len; non_empty = true; } } @@ -209,6 +211,10 @@ ble_gatt_show_local_inc_svc(const struct ble_gatt_svc_def *svc, const struct ble_gatt_svc_def **includes; int num = 0; + if (svc->includes == NULL) { + return 0; + } + for (includes = &svc->includes[0]; *includes != NULL; ++includes) { console_printf("included service\n"); console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s " @@ -229,10 +235,17 @@ ble_gatt_show_local_svc(const struct ble_gatt_svc_def *svc, { char uuid_buf[BLE_UUID_STR_LEN]; char flags_buf[BLE_CHR_FLAGS_STR_LEN]; + const char *svc_type_str; - console_printf("%s service\n", - svc->type == BLE_GATT_SVC_TYPE_PRIMARY ? - "primary" : "secondary"); + if (svc->type == BLE_GATT_SVC_TYPE_PRIMARY) { + svc_type_str = "primary"; + } else if (svc->type == BLE_GATT_SVC_TYPE_SECONDARY) { + svc_type_str = "secondary"; + } else { + svc_type_str = "unknown"; + } + + console_printf("%s service\n", svc_type_str); console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s " "%s\n", " ", "uuid", ble_uuid_to_str(svc->uuid, uuid_buf)); diff --git a/nimble/host/src/ble_hs_adv.c b/nimble/host/src/ble_hs_adv.c index 5f5a3e7fb..a24ac2775 100644 --- a/nimble/host/src/ble_hs_adv.c +++ b/nimble/host/src/ble_hs_adv.c @@ -129,6 +129,10 @@ ble_hs_adv_set_array_uuid16(uint8_t type, uint8_t num_elems, int rc; int i; + if (num_elems > 127) { + return BLE_HS_EINVAL; + } + rc = ble_hs_adv_set_hdr(type, num_elems * 2, max_len, dst, dst_len, om); if (rc != 0) { @@ -160,6 +164,10 @@ ble_hs_adv_set_array_uuid32(uint8_t type, uint8_t num_elems, int rc; int i; + if ((uint16_t)num_elems * 4 > 255) { // Prevent truncation, actual AD field limited by BLE_HS_ADV_MAX_FIELD_SZ + return BLE_HS_EMSGSIZE; + } + rc = ble_hs_adv_set_hdr(type, num_elems * 4, max_len, dst, dst_len, om); if (rc != 0) { @@ -195,6 +203,10 @@ ble_hs_adv_set_array_uuid128(uint8_t type, uint8_t num_elems, int rc; int i; + if (num_elems > 15) { + return BLE_HS_EINVAL; + } + rc = ble_hs_adv_set_hdr(type, num_elems * 16, max_len, dst, dst_len, om); if (rc != 0) { @@ -225,6 +237,10 @@ ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, const uint16_t *elems, int rc; int i; + if (num_elems > 127) { + return BLE_HS_EINVAL; + } + rc = ble_hs_adv_set_hdr(type, num_elems * sizeof *elems, max_len, dst, dst_len, om); if (rc != 0) { @@ -572,14 +588,20 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields, BLE_HS_ADV_LE_SUPP_FEAT_LEN, adv_fields->le_supp_feat, dst, &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } } /*** 0x2f - Advertising interval - long. */ - if (adv_fields->adv_itvl_long_is_present && adv_fields->adv_itvl_long > 0xFFFF) { + if (adv_fields->adv_itvl_long_is_present) { rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_ADV_ITVL_LONG, BLE_HS_ADV_ADV_ITVL_LONG_LEN, &adv_fields->adv_itvl_long, dst, &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } } #if MYNEWT_VAL(ENC_ADV_DATA) @@ -590,6 +612,9 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields, adv_fields->enc_adv_data_len, adv_fields->enc_adv_data, dst, &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } } #endif #endif @@ -812,6 +837,10 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, } *total_len = src[0] + 1; + if (*total_len < 2) { + return BLE_HS_EBADDATA; + } + if (src_len < *total_len) { return BLE_HS_EMSGSIZE; } diff --git a/nimble/host/src/ble_hs_atomic.c b/nimble/host/src/ble_hs_atomic.c index f26ba7acc..af62ec3aa 100644 --- a/nimble/host/src/ble_hs_atomic.c +++ b/nimble/host/src/ble_hs_atomic.c @@ -23,22 +23,35 @@ int ble_hs_atomic_conn_delete(uint16_t conn_handle) { struct ble_hs_conn *conn; + int rc; ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); if (conn != NULL) { ble_hs_conn_remove(conn); +#if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER) if (conn->psync) { + ble_hs_periodic_sync_remove(conn->psync); ble_hs_periodic_sync_free(conn->psync); } +#endif +#else +#if MYNEWT_VAL(BLE_PERIODIC_ADV) + if (conn->psync) { + ble_hs_periodic_sync_remove(conn->psync); + ble_hs_periodic_sync_free(conn->psync); + } +#endif #endif ble_hs_conn_free(conn); - + rc = 0; + } else { + rc = BLE_HS_ENOTCONN; } ble_hs_unlock(); - return conn != NULL ? 0 : BLE_HS_ENOTCONN; + return rc; } void diff --git a/nimble/host/src/ble_hs_cfg.c b/nimble/host/src/ble_hs_cfg.c index ee5b965af..9b4167855 100644 --- a/nimble/host/src/ble_hs_cfg.c +++ b/nimble/host/src/ble_hs_cfg.c @@ -24,14 +24,26 @@ struct ble_hs_cfg ble_hs_cfg = { /** Security manager settings. */ .sm_io_cap = MYNEWT_VAL(BLE_SM_IO_CAP), .sm_oob_data_flag = MYNEWT_VAL(BLE_SM_OOB_DATA_FLAG), + /* Note: If bonding is enabled, ensure key distribution is also enabled (BLE_SM_OUR/THEIR_KEY_DIST) */ .sm_bonding = MYNEWT_VAL(BLE_SM_BONDING), .sm_mitm = MYNEWT_VAL(BLE_SM_MITM), .sm_sc = MYNEWT_VAL(BLE_SM_SC), .sm_sc_only = MYNEWT_VAL(BLE_SM_SC_ONLY), .sm_sec_lvl = MYNEWT_VAL(BLE_SM_SC_LVL), .sm_keypress = MYNEWT_VAL(BLE_SM_KEYPRESS), + + /** EATT settings. */ + .eatt = MYNEWT_VAL(BLE_EATT_CHAN_NUM), + + /** Security manager settings (continued). */ .sm_our_key_dist = MYNEWT_VAL(BLE_SM_OUR_KEY_DIST), .sm_their_key_dist = MYNEWT_VAL(BLE_SM_THEIR_KEY_DIST), - .eatt = MYNEWT_VAL(BLE_EATT_CHAN_NUM), + + /** GATT caching settings. */ .gatt_use_cache = MYNEWT_VAL(BLE_GATT_CACHING), + +#if MYNEWT_VAL(STATIC_PASSKEY) + .sm_static_passkey = 0, + .sm_static_passkey_val = 0, +#endif }; diff --git a/nimble/host/src/ble_hs_conn.c b/nimble/host/src/ble_hs_conn.c index efe6dc377..2fbab681d 100644 --- a/nimble/host/src/ble_hs_conn.c +++ b/nimble/host/src/ble_hs_conn.c @@ -76,15 +76,22 @@ ble_hs_conn_can_alloc(void) return 0; #endif -#if MYNEWT_VAL(BLE_GATTS) - return ble_hs_conn_pool.mp_num_free >= 1 && - ble_l2cap_chan_pool.mp_num_free >= BLE_HS_CONN_MIN_CHANS && - ble_gatts_conn_can_alloc(); + int result; + ble_hs_lock(); + +#if MYNEWT_VAL(BLE_GATTS) + result = ble_hs_conn_pool.mp_num_free >= 1 && + ble_l2cap_chan_pool.mp_num_free >= BLE_HS_CONN_MIN_CHANS && + ble_gatts_conn_can_alloc(); #else - return ble_hs_conn_pool.mp_num_free >= 1 && - ble_l2cap_chan_pool.mp_num_free >= BLE_HS_CONN_MIN_CHANS; + result = ble_hs_conn_pool.mp_num_free >= 1 && + ble_l2cap_chan_pool.mp_num_free >= BLE_HS_CONN_MIN_CHANS; #endif + + ble_hs_unlock(); + + return result; } struct ble_l2cap_chan * @@ -130,7 +137,7 @@ bool ble_hs_conn_chan_exist(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan) { #if !NIMBLE_BLE_CONNECT - return NULL; + return false; #endif struct ble_l2cap_chan *tmp; @@ -206,6 +213,7 @@ ble_hs_conn_alloc(uint16_t conn_handle) } rc = ble_hs_conn_chan_insert(conn, chan); if (rc != 0) { + ble_l2cap_chan_free(conn, chan); goto err; } @@ -215,6 +223,7 @@ ble_hs_conn_alloc(uint16_t conn_handle) } rc = ble_hs_conn_chan_insert(conn, chan); if (rc != 0) { + ble_l2cap_chan_free(conn, chan); goto err; } @@ -227,6 +236,7 @@ ble_hs_conn_alloc(uint16_t conn_handle) } rc = ble_hs_conn_chan_insert(conn, chan); if (rc != 0) { + ble_l2cap_chan_free(conn, chan); goto err; } #if MYNEWT_VAL(BLE_GATTS) @@ -261,6 +271,10 @@ ble_hs_conn_delete_chan(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan) #endif } + if (chan == NULL) { + return; + } + SLIST_REMOVE(&conn->bhc_channels, chan, ble_l2cap_chan, next); ble_l2cap_chan_free(conn, chan); } @@ -307,6 +321,12 @@ ble_hs_conn_free(struct ble_hs_conn *conn) os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); } + /* Clear ATT TX queue */ + while ((omp = STAILQ_FIRST(&conn->att_tx_q)) != NULL) { + STAILQ_REMOVE_HEAD(&conn->att_tx_q, omp_next); + os_mbuf_free_chain(OS_MBUF_PKTHDR_TO_MBUF(omp)); + } + #if MYNEWT_VAL(BLE_HS_DEBUG) memset(conn, 0xff, sizeof *conn); #endif @@ -485,11 +505,15 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, our_id_addr_val = conn->bhc_our_rnd_addr; } else { rc = ble_hs_id_addr(addrs->our_id_addr.type, &our_id_addr_val, NULL); - assert(rc == 0); + if (rc != 0) { + return; + } } #else rc = ble_hs_id_addr(addrs->our_id_addr.type, &our_id_addr_val, NULL); - assert(rc == 0); + if (rc != 0) { + return; + } #endif memcpy(addrs->our_id_addr.val, our_id_addr_val, 6); @@ -518,14 +542,16 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, if (ble_host_rpa_enabled()) { uint8_t *local_id = NULL; - ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL); + rc = ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL); /* RL is present: populate our id addr with public ID */ - memcpy(addrs->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN); - addrs->our_id_addr.type = BLE_ADDR_PUBLIC; - BLE_HS_LOG(DEBUG, "Revised our id addr:\n"); - ble_hs_log_flat_buf(our_id_addr_val, BLE_DEV_ADDR_LEN); - BLE_HS_LOG(DEBUG, "\n"); + if (rc == 0 && local_id != NULL) { + memcpy(addrs->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN); + addrs->our_id_addr.type = BLE_ADDR_PUBLIC; + BLE_HS_LOG(DEBUG, "Revised our id addr:\n"); + ble_hs_log_flat_buf(local_id, BLE_DEV_ADDR_LEN); + BLE_HS_LOG(DEBUG, "\n"); + } } } #endif @@ -561,10 +587,9 @@ ble_hs_conn_timer(void) #endif struct ble_hs_conn *conn; + struct ble_hs_conn *tmp; ble_npl_time_t now = ble_npl_time_get(); int32_t next_exp_in = BLE_HS_FOREVER; - int32_t next_exp_in_new; - bool next_exp_in_updated; int32_t time_diff; ble_hs_lock(); @@ -574,9 +599,8 @@ ble_hs_conn_timer(void) * so connection is disconnected. * 2. Otherwise, determine when the next timeout will occur. */ - SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) { + SLIST_FOREACH_SAFE(conn, &ble_hs_conns, bhc_next, tmp) { if (!(conn->bhc_flags & BLE_HS_CONN_F_TERMINATING)) { - next_exp_in_updated = false; #if MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) != 0 /* Check each connection's rx fragment timer. If too much time @@ -598,8 +622,7 @@ ble_hs_conn_timer(void) /* Determine if this connection is the soonest to time out. */ if (time_diff < next_exp_in) { - next_exp_in_new = time_diff; - next_exp_in_updated = true; + next_exp_in = time_diff; } } #endif @@ -618,14 +641,9 @@ ble_hs_conn_timer(void) /* Determine if this connection is the soonest to time out. */ if (time_diff < next_exp_in) { - next_exp_in_new = time_diff; - next_exp_in_updated = true; + next_exp_in = time_diff; } #endif - - if (next_exp_in_updated) { - next_exp_in = next_exp_in_new; - } } } diff --git a/nimble/host/src/ble_hs_flow.c b/nimble/host/src/ble_hs_flow.c index 1548d3b93..ee19c95ae 100644 --- a/nimble/host/src/ble_hs_flow.c +++ b/nimble/host/src/ble_hs_flow.c @@ -78,6 +78,7 @@ ble_hs_flow_mbuf_index(const struct os_mbuf *om) idx = (addr - mp->mp_membuf_addr) / mp->mp_block_size; BLE_HS_DBG_ASSERT(mp->mp_membuf_addr + idx * mp->mp_block_size == addr); + BLE_HS_DBG_ASSERT(idx >= 0 && idx < MYNEWT_VAL(BLE_TRANSPORT_ACL_FROM_LL_COUNT)); return idx; } @@ -112,8 +113,6 @@ ble_hs_flow_tx_num_comp_pkts(void) cmd->h[0].handle = htole16(conn->bhc_handle); cmd->h[0].count = htole16(conn->bhc_completed_pkts); - conn->bhc_completed_pkts = 0; - /* The host-number-of-completed-packets command does not elicit a * response from the controller, so don't use the normal blocking * HCI API when sending it. @@ -125,6 +124,8 @@ ble_hs_flow_tx_num_comp_pkts(void) if (rc != 0) { return rc; } + + conn->bhc_completed_pkts = 0; } } @@ -139,12 +140,13 @@ ble_hs_flow_event_cb(struct ble_npl_event *ev) ble_hs_lock(); if (ble_hs_flow_num_completed_pkts > 0) { + uint16_t snapshot = ble_hs_flow_num_completed_pkts; rc = ble_hs_flow_tx_num_comp_pkts(); if (rc != 0) { ble_hs_sched_reset(rc); } - ble_hs_flow_num_completed_pkts = 0; + ble_hs_flow_num_completed_pkts -= snapshot; } ble_hs_unlock(); @@ -168,7 +170,7 @@ ble_hs_flow_inc_completed_pkts(struct ble_hs_conn *conn) } /* If the number of free buffers is at or below the configured threshold, - * send an immediate number-of-copmleted-packets event. + * send an immediate number-of-completed-packets event. */ num_free = MYNEWT_VAL(BLE_TRANSPORT_ACL_FROM_LL_COUNT) - ble_hs_flow_num_completed_pkts; @@ -193,7 +195,13 @@ ble_hs_flow_acl_free(struct os_mempool_ext *mpe, void *data, void *arg) #if MYNEWT_VAL(MP_RUNTIME_ALLOC) // For runtime allocation, get the connection handle from HCI ACL data - const struct hci_data_hdr *hdr = (void *)om->om_data; + // Check if the header was stripped by comparing om_data with the start of the buffer + const struct hci_data_hdr *hdr; + if (om->om_data == om->om_databuf) { + hdr = (void *)om->om_data; + } else { + hdr = (void *)((uint8_t *)om->om_data - BLE_HCI_DATA_HDR_SZ); + } conn_handle = BLE_HCI_DATA_HANDLE(hdr->hdh_handle_pb_bc); #else int idx = ble_hs_flow_mbuf_index(om); @@ -231,7 +239,10 @@ ble_hs_flow_connection_broken(uint16_t conn_handle) #if MYNEWT_VAL(BLE_HS_FLOW_CTRL) && \ MYNEWT_VAL(BLE_HS_FLOW_CTRL_TX_ON_DISCONNECT) ble_hs_lock(); - ble_hs_flow_tx_num_comp_pkts(); + int rc = ble_hs_flow_tx_num_comp_pkts(); + if (rc != 0) { + ble_hs_sched_reset(rc); + } ble_hs_unlock(); #endif } @@ -300,26 +311,18 @@ ble_hs_flow_startup(void) /* Flow control successfully enabled. */ ble_hs_flow_num_completed_pkts = 0; + ble_npl_callout_init(&ble_hs_flow_timer, ble_hs_evq_get(), + ble_hs_flow_event_cb, NULL); #if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED ble_hci_trans_set_acl_free_cb(ble_hs_flow_acl_free, NULL); #else ble_transport_register_put_acl_from_ll_cb(ble_hs_flow_acl_free); #endif - ble_npl_callout_init(&ble_hs_flow_timer, ble_hs_evq_get(), - ble_hs_flow_event_cb, NULL); #endif return 0; } -void -ble_hs_flow_stop(void) -{ -#if MYNEWT_VAL(BLE_HS_FLOW_CTRL) - ble_npl_callout_deinit(&ble_hs_flow_timer); -#endif -} - void ble_hs_flow_init(void) { @@ -327,6 +330,7 @@ ble_hs_flow_init(void) #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (!ble_hs_flow_ctx) { ble_hs_flow_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_flow_ctx)); + BLE_HS_DBG_ASSERT(ble_hs_flow_ctx); if (!ble_hs_flow_ctx) { return; } diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c index 686ca4979..a6df9dfd1 100644 --- a/nimble/host/src/ble_hs_hci.c +++ b/nimble/host/src/ble_hs_hci.c @@ -31,9 +31,6 @@ #include "hci_log/bt_hci_log.h" #include "host/ble_hs.h" #endif // (BT_HCI_LOG_INCLUDED == TRUE) -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -#include "esp_nimble_mem.h" -#endif #define BLE_HCI_CMD_TIMEOUT_MS 2000 #if MYNEWT_VAL(BLE_ERR_NAME) @@ -267,8 +264,8 @@ typedef struct { struct ble_npl_sem hci_sem; struct os_mbuf_pool hci_frag_mbuf_pool; uint16_t hci_buf_sz; - uint8_t hci_max_pkts; - uint32_t hci_sup_feat; + uint16_t hci_max_pkts; + uint64_t hci_sup_feat; uint8_t hci_version; uint16_t hci_avial_pkts; @@ -296,13 +293,12 @@ static ble_hs_hci_ctx_t *ble_hs_hci_ctx = NULL; #else static struct ble_npl_mutex ble_hs_hci_mutex; static struct ble_npl_sem ble_hs_hci_sem; -static struct os_mbuf_pool ble_hs_hci_frag_mbuf_pool; static uint16_t ble_hs_hci_buf_sz; -static uint8_t ble_hs_hci_max_pkts; +static uint16_t ble_hs_hci_max_pkts; /* For now 32-bits of features is enough */ -static uint32_t ble_hs_hci_sup_feat; +static uint64_t ble_hs_hci_sup_feat; static uint8_t ble_hs_hci_version; /** @@ -613,7 +609,8 @@ ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len, /* on success we should always get full response */ if (!rc && (ack.bha_params_len != rsp_len)) { - BLE_HS_LOG(INFO, "Received status %d \n", rc); + BLE_HS_LOG(ERROR, "Received status %d \n", rc); + rc = BLE_HS_ECONTROLLER; ble_hs_sched_reset(rc); goto done; } @@ -708,6 +705,15 @@ ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg) switch (ev->opcode) { case BLE_HCI_EVCODE_COMMAND_COMPLETE: + if (ev->length < 3 || (cmd_complete->opcode != BLE_HCI_OPCODE_NOP && ev->length < 4)) { + BLE_HS_LOG(ERROR, "Invalid COMPLETE len=%d\n", ev->length); +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + ble_transport_free(BLE_HCI_EVT, hci_ev); +#else + ble_transport_free(hci_ev); +#endif + return BLE_HS_ECONTROLLER; + } enqueue = (cmd_complete->opcode == BLE_HCI_OPCODE_NOP); /* Check for BLE transmit opcodes which come in command complete */ @@ -722,6 +728,15 @@ ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg) } break; case BLE_HCI_EVCODE_COMMAND_STATUS: + if (ev->length < sizeof(struct ble_hci_ev_command_status)) { + BLE_HS_LOG(ERROR, "Invalid STATUS len=%d\n", ev->length); +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + ble_transport_free(BLE_HCI_EVT, hci_ev); +#else + ble_transport_free(hci_ev); +#endif + return BLE_HS_ECONTROLLER; + } enqueue = (cmd_status->opcode == BLE_HCI_OPCODE_NOP); break; default: @@ -951,12 +966,12 @@ ble_hs_hci_acl_tx(struct ble_hs_conn *conn, struct os_mbuf **om) #endif void -ble_hs_hci_set_le_supported_feat(uint32_t feat) +ble_hs_hci_set_le_supported_feat(uint64_t feat) { ble_hs_hci_sup_feat = feat; } -uint32_t +uint64_t ble_hs_hci_get_le_supported_feat(void) { return ble_hs_hci_sup_feat; @@ -985,20 +1000,21 @@ ble_hs_hci_init(void) ble_hs_hci_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_hci_ctx)); if (!ble_hs_hci_ctx) { BLE_HS_DBG_ASSERT_EVAL(0); + return; } } -#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) size_t frag_data_size = BLE_HS_HCI_FRAG_MEMPOOL_SIZE * sizeof(os_membuf_t); if (!ble_hs_hci_frag_data) { ble_hs_hci_frag_data = nimble_platform_mem_calloc(1, frag_data_size); if (!ble_hs_hci_frag_data) { nimble_platform_mem_free(ble_hs_hci_ctx); + ble_hs_hci_ctx = NULL; BLE_HS_DBG_ASSERT_EVAL(0); + return; } } -#endif #endif rc = ble_npl_sem_init(&ble_hs_hci_sem, 0); @@ -1020,22 +1036,33 @@ ble_hs_hci_init(void) void ble_hs_hci_deinit(void) { int rc; + +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_hci_ctx == NULL) { + return; + } +#endif + + /* Clean up mempool first to ensure blocks are free */ + os_mempool_clear(&ble_hs_hci_frag_mempool); + rc = ble_npl_mutex_deinit(&ble_hs_hci_mutex); BLE_HS_DBG_ASSERT_EVAL(rc == 0); rc = ble_npl_sem_deinit(&ble_hs_hci_sem); BLE_HS_DBG_ASSERT_EVAL(rc == 0); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_hci_ctx) { -#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) - if (ble_hs_hci_frag_data) { - nimble_platform_mem_free(ble_hs_hci_frag_data); - ble_hs_hci_frag_data = NULL; - } -#endif - nimble_platform_mem_free(ble_hs_hci_ctx); - ble_hs_hci_ctx = NULL; +#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) && MYNEWT_VAL(MP_RUNTIME_ALLOC) + if (ble_hs_hci_frag_data) { + nimble_platform_mem_free(ble_hs_hci_frag_data); + ble_hs_hci_frag_data = NULL; } - +#endif +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_hci_frag_data) { + nimble_platform_mem_free(ble_hs_hci_frag_data); + ble_hs_hci_frag_data = NULL; + } + nimble_platform_mem_free(ble_hs_hci_ctx); + ble_hs_hci_ctx = NULL; #endif } diff --git a/nimble/host/src/ble_hs_hci_cmd.c b/nimble/host/src/ble_hs_hci_cmd.c index e92ca0408..a561b047b 100644 --- a/nimble/host/src/ble_hs_hci_cmd.c +++ b/nimble/host/src/ble_hs_hci_cmd.c @@ -20,7 +20,6 @@ #include #include #include -#include #include "os/os.h" #include "nimble/hci_common.h" #include "ble_hs_priv.h" @@ -67,6 +66,9 @@ ble_hs_hci_cmd_send(uint16_t opcode, uint8_t len, const void *cmddata) int rc; cmd = (struct ble_hci_cmd *)ble_transport_alloc_cmd(); + if (cmd == NULL) { + return BLE_HS_ENOMEM_EVT; + } BLE_HS_DBG_ASSERT(cmd != NULL); buf = (uint8_t *)cmd; @@ -106,6 +108,7 @@ ble_hs_hci_cmd_send(uint16_t opcode, uint8_t len, const void *cmddata) if (rc == 0) { STATS_INC(ble_hs_stats, hci_cmd); } else { + ble_transport_free(buf); BLE_HS_LOG(DEBUG, "ble_hs_hci_cmd_send failure; rc=%d\n", rc); } diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index f17d5f2a2..21a9afa7d 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -75,6 +75,7 @@ typedef struct { (MYNEWT_VAL(BLE_EXT_ADV) && (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_BROADCASTER)) && \ (NIMBLE_BLE_CONNECT || (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_BROADCASTER))))) struct ble_gap_conn_complete conn_complete; + bool conn_complete_valid; #endif }ble_hci_ctx_t; @@ -90,6 +91,7 @@ static ble_hci_ctx_t *ble_hci_ctx; (MYNEWT_VAL(BLE_EXT_ADV) && (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_BROADCASTER)) && \ (NIMBLE_BLE_CONNECT || (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_BROADCASTER))))) #define pend_conn_complete (ble_hci_ctx->conn_complete) +#define pend_conn_complete_valid (ble_hci_ctx->conn_complete_valid) #endif #endif @@ -214,6 +216,7 @@ struct host_hci_stats { #if NIMBLE_BLE_CONNECT || \ (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_BROADCASTER)) static struct ble_gap_conn_complete pend_conn_complete; +static bool pend_conn_complete_valid; #endif #endif #endif @@ -406,6 +409,11 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, { const struct ble_hci_ev_disconn_cmp *ev = data; const struct ble_hs_conn *conn; +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + uint32_t conn_flags = 0; + int conn_slave = 0; + bool conn_found = false; +#endif #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) && MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) bool should_reattempt = false; @@ -419,16 +427,21 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, conn = ble_hs_conn_find(le16toh(ev->conn_handle)); if (conn != NULL) { ble_hs_hci_add_avail_pkts(conn->bhc_outstanding_pkts); +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + conn_flags = conn->bhc_flags; + conn_slave = conn->slave_conn; + conn_found = true; +#endif } ble_hs_unlock(); #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) - if (conn) { + if (conn_found) { uint16_t handle; int rc; #if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) - should_reattempt = (!conn->slave_conn && ev->reason == BLE_ERR_CONN_SPVN_TMO); + should_reattempt = (!conn_slave && ev->reason == BLE_ERR_CONN_SPVN_TMO); #endif /* For master role, check if failure reason is 0x3E, to restart connect attempt * For slave role, check whether @@ -442,7 +455,7 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, } #endif - if ((conn->bhc_flags & BLE_HS_CONN_F_MASTER) && \ + if ((conn_flags & BLE_HS_CONN_F_MASTER) && \ (ev->reason == BLE_ERR_CONN_ESTABLISHMENT)) { // master if (reattempt_conn.count < MAX_REATTEMPT_ALLOWED) { /* Go for connection */ @@ -466,12 +479,12 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, } } #if MYNEWT_VAL(BT_NIMBLE_MEM_OPTIMIZATION) - else if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER) && \ + else if (!(conn_flags & BLE_HS_CONN_F_MASTER) && \ ((ev->reason == BLE_ERR_CONN_ESTABLISHMENT) || should_reattempt)) { #else - else if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER) && \ + else if (!(conn_flags & BLE_HS_CONN_F_MASTER) && \ ((ev->reason == BLE_ERR_CONN_ESTABLISHMENT) || \ - (!conn->slave_conn && ev->reason == BLE_ERR_CONN_SPVN_TMO))) { //slave + (!conn_slave && ev->reason == BLE_ERR_CONN_SPVN_TMO))) { //slave #endif BLE_HS_LOG(INFO, "Reattempt advertising; reason: 0x%x, status = %x", ev->reason, ev->status); @@ -682,9 +695,21 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, const struct ble_hci_ev_le_subev_enh_conn_complete *ev = data; struct ble_gap_conn_complete evt; +#if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + if (subevent == BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE) { + if (len != sizeof(*ev) - 3) { + return BLE_HS_ECONTROLLER; + } + } else { + if (len != sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + } +#else if (len != sizeof(*ev)) { return BLE_HS_ECONTROLLER; } +#endif memset(&evt, 0, sizeof(evt)); @@ -731,6 +756,7 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, /* store this until we get set terminated event with adv handle */ memcpy(&pend_conn_complete, &evt, sizeof(evt)); + pend_conn_complete_valid = true; return 0; } #endif @@ -803,6 +829,7 @@ ble_hs_hci_evt_le_conn_complete(uint8_t subevent, const void *data, /* store this until we get set terminated event with adv handle */ memcpy(&pend_conn_complete, &evt, sizeof(evt)); + pend_conn_complete_valid = true; return 0; } #endif @@ -879,20 +906,23 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) 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); #endif for (i = 0; i < ev->num_reports; i++) { + rpt = data; /* Avoiding further processing, if the adv report is from the same device*/ #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) - if (ble_check_adv_list(ev->reports[i].addr, ev->reports[i].addr_type) == true) { + if (ble_check_adv_list(rpt->addr, rpt->addr_type) == true) { + data += sizeof(*rpt) + rpt->data_len + 1; continue; } #endif - rpt = data; - data += sizeof(rpt) + rpt->data_len + 1; + data += sizeof(*rpt) + rpt->data_len + 1; desc.event_type = rpt->type; desc.addr.type = rpt->addr_type; @@ -928,7 +958,7 @@ ble_hs_hci_evt_le_dir_adv_rpt(uint8_t subevent, const void *data, unsigned int l struct ble_gap_disc_desc desc = {0}; int i; - if (len < sizeof(*ev) || len != ev->num_reports * sizeof(ev->reports[0])) { + if (len < sizeof(*ev) || len != sizeof(*ev) + ev->num_reports * sizeof(ev->reports[0])) { return BLE_HS_ECONTROLLER; } @@ -1260,7 +1290,10 @@ ble_hs_hci_evt_le_adv_set_terminated(uint8_t subevent, const void *data, } #endif /* ignore return code as we need to terminate advertising set anyway */ - ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle); + if (pend_conn_complete_valid) { + ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle); + pend_conn_complete_valid = false; + } } ble_gap_rx_adv_set_terminated(ev); #endif @@ -1329,11 +1362,35 @@ ble_hs_hci_evt_le_periodic_adv_subev_data_req(uint8_t subevent, const void *data static int ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data, unsigned int len) { + //Check properly + const uint8_t *ev_data = data; + uint8_t num_responses; + uint32_t offset; + 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 + return BLE_HS_ECONTROLLER; + } + num_responses = ev_data[4]; // ev->num_responses + offset = 5; + for (uint8_t i = 0; i < num_responses; i++) { + if (offset + 6 > len) { + return BLE_HS_ECONTROLLER; + } + uint8_t data_length = ev_data[offset + 5]; // data_length field position + if (offset + 6 + data_length > len) { + return BLE_HS_ECONTROLLER; + } + offset += 6 + data_length; + } + if (offset != len) { // exact match + 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 ++) { @@ -1560,6 +1617,11 @@ 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)) { + 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; entry = ble_hs_hci_evt_dispatch_find(cmd_complete->opcode); @@ -1587,6 +1649,7 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev) rc = entry->cb(ev->opcode, ev->data, ev->length); } +done: #if MYNEWT_VAL(MP_RUNTIME_ALLOC) ble_transport_free(BLE_HCI_EVT, (uint8_t *)ev); #else @@ -1753,9 +1816,11 @@ uint32_t ble_get_adv_list_length(void) uint32_t length = 0; struct ble_addr_list_entry *device; + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); SLIST_FOREACH(device, &ble_adv_list, next) { length++; } + ble_npl_mutex_release(&adv_list_lock); return length; } @@ -1764,25 +1829,19 @@ void ble_adv_list_refresh(void) { struct ble_addr_list_entry *device; struct ble_addr_list_entry *temp; - ble_adv_list_count = 0; - - if (SLIST_EMPTY(&ble_adv_list)) { - BLE_HS_LOG(ERROR, "%s: ble_adv_list empty — reinitializing", __func__); - SLIST_INIT(&ble_adv_list); - ble_adv_list_count = 0; - return; - } - if (SLIST_EMPTY(&ble_adv_list)) { - BLE_HS_LOG(ERROR, "%s ble_adv_list is empty", __func__); - return; - } 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); - nimble_platform_mem_free(device); + if (SLIST_EMPTY(&ble_adv_list)) { + 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); + nimble_platform_mem_free(device); + } } + ble_adv_list_count = 0; ble_npl_mutex_release(&adv_list_lock); } diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h index b21a5ad99..9b53df424 100644 --- a/nimble/host/src/ble_hs_hci_priv.h +++ b/nimble/host/src/ble_hs_hci_priv.h @@ -88,8 +88,8 @@ int ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len, void ble_hs_hci_init(void); void ble_hs_hci_deinit(void); -void ble_hs_hci_set_le_supported_feat(uint32_t feat); -uint32_t ble_hs_hci_get_le_supported_feat(void); +void ble_hs_hci_set_le_supported_feat(uint64_t feat); +uint64_t ble_hs_hci_get_le_supported_feat(void); void ble_hs_hci_set_hci_version(uint8_t hci_version); uint8_t ble_hs_hci_get_hci_version(void); diff --git a/nimble/host/src/ble_hs_hci_util.c b/nimble/host/src/ble_hs_hci_util.c index bf9fc4bfc..88fa2bbff 100644 --- a/nimble/host/src/ble_hs_hci_util.c +++ b/nimble/host/src/ble_hs_hci_util.c @@ -26,10 +26,6 @@ #define min(a, b) ((a) < (b) ? (a) : (b)) #endif -#ifndef max -#define max(a, b) ((a) > (b) ? (a) : (b)) -#endif - uint16_t ble_hs_hci_util_handle_pb_bc_join(uint16_t handle, uint8_t pb, uint8_t bc) { @@ -37,9 +33,9 @@ ble_hs_hci_util_handle_pb_bc_join(uint16_t handle, uint8_t pb, uint8_t bc) BLE_HS_DBG_ASSERT(pb <= 0x03); BLE_HS_DBG_ASSERT(bc <= 0x03); - return (handle << 0) | - (pb << 12) | - (bc << 14); + return ((handle & 0x0FFF) << 0) | + ((pb & 0x03) << 12) | + ((bc & 0x03) << 14); } int @@ -48,6 +44,10 @@ ble_hs_hci_util_read_adv_tx_pwr(int8_t *out_tx_pwr) struct ble_hci_le_rd_adv_chan_txpwr_rp rsp; int rc; + if (out_tx_pwr == NULL) { + return BLE_HS_EINVAL; + } + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR), NULL, 0, &rsp, sizeof(rsp)); @@ -73,6 +73,10 @@ ble_hs_hci_util_rand(void *dst, int len) int chunk_sz; int rc; + if (len <= 0) { + 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), @@ -81,7 +85,7 @@ ble_hs_hci_util_rand(void *dst, int len) return rc; } - chunk_sz = min(len, sizeof(rsp)); + chunk_sz = min(len, (int)sizeof(rsp.random_number)); memcpy(u8ptr, &rsp.random_number, chunk_sz); len -= chunk_sz; @@ -99,6 +103,10 @@ ble_hs_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi) int rc; + if (out_rssi == NULL) { + return BLE_HS_EINVAL; + } + cmd.handle = htole16(conn_handle); rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_STATUS_PARAMS, @@ -122,6 +130,10 @@ ble_hs_hci_util_set_random_addr(const uint8_t *addr) { struct ble_hci_le_set_rand_addr_cp cmd; + if (addr == NULL) { + return BLE_HS_EINVAL; + } + memcpy(cmd.addr, addr, BLE_DEV_ADDR_LEN); return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -172,6 +184,10 @@ ble_hs_hci_util_read_sugg_def_data_len(uint16_t *out_sugg_max_tx_octets, struct ble_hci_le_rd_sugg_def_data_len_rp rsp; int rc; + if (out_sugg_max_tx_octets == NULL || out_sugg_max_tx_time == NULL) { + return BLE_HS_EINVAL; + } + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_SUGG_DEF_DATA_LEN), NULL, 0, &rsp, sizeof(rsp)); @@ -246,6 +262,10 @@ ble_hs_hci_read_chan_map(uint16_t conn_handle, uint8_t *out_chan_map) struct ble_hci_le_rd_chan_map_rp rsp; int rc; + if (out_chan_map == NULL) { + return BLE_HS_EINVAL; + } + cmd.conn_handle = htole16(conn_handle); rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -259,7 +279,7 @@ ble_hs_hci_read_chan_map(uint16_t conn_handle, uint8_t *out_chan_map) return BLE_HS_ECONTROLLER; } - memcpy(out_chan_map, rsp.chan_map, 5); + memcpy(out_chan_map, rsp.chan_map, sizeof(rsp.chan_map)); return 0; } @@ -269,6 +289,10 @@ ble_hs_hci_set_chan_class(const uint8_t *chan_map) { struct ble_hci_le_set_host_chan_class_cp cmd; + if (chan_map == NULL) { + return BLE_HS_EINVAL; + } + memcpy(cmd.chan_map, chan_map, sizeof(cmd.chan_map)); return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -348,7 +372,7 @@ ble_hs_hci_dtm_enh_tx_start(const uint8_t tx_chan, const uint8_t test_data_len, int ble_hs_hci_dtm_enh_rx_start(const uint8_t rx_chan, const uint8_t index, - const uint8_t phy) + const uint8_t phy) { struct ble_hci_le_rx_test_v2_cp cmd; uint16_t opcode; @@ -370,6 +394,10 @@ ble_hs_hci_rd_all_local_supp_features(uint8_t* status, uint8_t* max_page, struct ble_hci_le_rd_all_local_feat_rp rsp; int rc; + if (status == NULL || max_page == NULL || le_features == NULL) { + return BLE_HS_EINVAL; + } + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ALL_LOCAL_SUP_FEAT), NULL, 0, &rsp, sizeof(rsp)); @@ -381,7 +409,7 @@ ble_hs_hci_rd_all_local_supp_features(uint8_t* status, uint8_t* max_page, *status = rsp.status; *max_page = rsp.max_page; - memcpy(le_features, rsp.le_features, 248); + memcpy(le_features, rsp.le_features, sizeof(rsp.le_features)); return 0; } @@ -405,13 +433,17 @@ 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) +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) { struct ble_hci_le_add_monitor_adv_list_cp cmd; + if (addr == NULL) { + return BLE_HS_EINVAL; + } + cmd.addr_type = addr_type; - memcpy(cmd.address, addr, 6); + memcpy(cmd.address, addr, BLE_DEV_ADDR_LEN); cmd.rssi_low_threshold = rssi_low; cmd.rssi_high_threshold = rssi_high; cmd.timeout = timeout; @@ -426,8 +458,12 @@ ble_hs_hci_rmv_monitor_adv_list(uint8_t addr_type, uint8_t *addr) { struct ble_hci_le_rmv_monitor_adv_list_cp cmd; + if (addr == NULL) { + return BLE_HS_EINVAL; + } + cmd.addr_type = addr_type; - memcpy(cmd.address, addr, 6); + memcpy(cmd.address, addr, BLE_DEV_ADDR_LEN); return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RMV_MONITOR_ADV_LIST), &cmd, sizeof(cmd), @@ -448,6 +484,10 @@ ble_hs_hci_read_monitor_adv_list_size(uint8_t *out_number) int rc; struct ble_hci_le_rd_monitor_adv_list_size_rp rsp; + if (out_number == NULL) { + return BLE_HS_EINVAL; + } + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_READ_MONITOR_ADV_LIST_SIZE), NULL, 0, &rsp, sizeof(rsp)); diff --git a/nimble/host/src/ble_hs_id.c b/nimble/host/src/ble_hs_id.c index 16140b652..35704bac0 100644 --- a/nimble/host/src/ble_hs_id.c +++ b/nimble/host/src/ble_hs_id.c @@ -74,24 +74,25 @@ ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type) bool rc = 0; /* According to spec v4.2, Vol 6, Part B, section 1.3.2.2, the two most * significant bits of RPA shall be equal to 0 and 1 */ - if (addr_type && ((addr[5] & 0xc0) == 0x40)) { + if (addr_type == BLE_ADDR_RANDOM && ((addr[5] & 0xc0) == 0x40)) { rc = 1; } return rc; } #endif -void +int ble_hs_id_set_pub(const uint8_t *pub_addr) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_id_ensure_ctx()) { - return; + return BLE_HS_ENOMEM; } #endif ble_hs_lock(); memcpy(ble_hs_id_pub, pub_addr, 6); ble_hs_unlock(); + return 0; } int @@ -124,13 +125,22 @@ ble_hs_id_gen_rnd(int nrpa, ble_addr_t *out_addr) * Appropriate error code if failure. */ int -ble_hs_id_set_nrpa_rnd() +ble_hs_id_set_nrpa_rnd(void) { ble_addr_t nrpa_addr; int rc; - ble_hs_id_gen_rnd(1, &nrpa_addr); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_id_ensure_ctx()) { + return BLE_HS_ENOMEM; + } +#endif + + rc = ble_hs_id_gen_rnd(1, &nrpa_addr); + if (rc != 0) { + return rc; + } ble_hs_lock(); @@ -164,22 +174,27 @@ ble_hs_id_set_pseudo_rnd(const uint8_t *rnd_addr) { uint8_t addr_type_byte; int rc; - int i, rnd_part_sum = 0; + int ones; + +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_id_ensure_ctx()) { + return BLE_HS_ENOMEM; + } +#endif ble_hs_lock(); - /* Make sure all bits of rnd_addr are neither one nor zero (3rd, 4th and - * 5th bytes of rnd_addr(RPA) are prand) Vol 6, Part B, section 1.3.2.2 - * The two most significant bits of RPA shall be equal to 0 and 1 */ + /* 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; - for (i = 3; i < BLE_DEV_ADDR_LEN; i++) { - rnd_part_sum += *(rnd_addr + i); - } - rnd_part_sum -= addr_type_byte; - /* All ones in random part: 3*(0xFF) - 0x40 = 0x2BD */ + /* count bits set to 1 in random part of prand (22 bits) */ + ones = __builtin_popcount(rnd_addr[3]); + ones += __builtin_popcount(rnd_addr[4]); + ones += __builtin_popcount(rnd_addr[5] & 0x3f); + if ((addr_type_byte != 0x40) || - (rnd_part_sum == 0) || (rnd_part_sum == 0x2BD)) { + (ones == 0 || ones == 22)) { rc = BLE_HS_EINVAL; goto done; } @@ -275,6 +290,8 @@ ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr, const uint8_t *id_addr; int nrpa; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + switch (id_addr_type) { case BLE_ADDR_PUBLIC: case BLE_ADDR_PUBLIC_ID: @@ -449,8 +466,10 @@ ble_hs_id_reset(void) 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(); } /** @@ -460,12 +479,14 @@ ble_hs_id_reset(void) void ble_hs_id_rnd_reset(void) { - + ble_hs_lock(); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_id_ensure_ctx()) { + ble_hs_unlock(); return; } #endif memset(ble_hs_id_rnd, 0, sizeof ble_hs_id_rnd); + ble_hs_unlock(); } diff --git a/nimble/host/src/ble_hs_id_priv.h b/nimble/host/src/ble_hs_id_priv.h index 2657a9203..952bb9bae 100644 --- a/nimble/host/src/ble_hs_id_priv.h +++ b/nimble/host/src/ble_hs_id_priv.h @@ -26,7 +26,7 @@ extern "C" { #endif -void ble_hs_id_set_pub(const uint8_t *pub_addr); +int ble_hs_id_set_pub(const uint8_t *pub_addr); int ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr, int *out_is_nrpa); int ble_hs_id_use_addr(uint8_t addr_type); diff --git a/nimble/host/src/ble_hs_log.c b/nimble/host/src/ble_hs_log.c index 0670dca6e..5aafd61ec 100644 --- a/nimble/host/src/ble_hs_log.c +++ b/nimble/host/src/ble_hs_log.c @@ -26,12 +26,17 @@ struct log ble_hs_log; void ble_hs_log_mbuf(const struct os_mbuf *om) { - uint8_t u8; + const struct os_mbuf *cur; int i; - for (i = 0; i < OS_MBUF_PKTLEN(om); i++) { - os_mbuf_copydata(om, i, 1, &u8); - BLE_HS_LOG(DEBUG, "0x%02x ", u8); + if (om == NULL) { + return; + } + + for (cur = om; cur != NULL; cur = SLIST_NEXT(cur, om_next)) { + for (i = 0; i < cur->om_len; i++) { + BLE_HS_LOG(DEBUG, "0x%02x ", cur->om_data[i]); + } } } @@ -41,6 +46,10 @@ ble_hs_log_flat_buf(const void *data, int len) const uint8_t *u8ptr; int i; + if (data == NULL) { + return; + } + u8ptr = data; for (i = 0; i < len; i++) { BLE_HS_LOG(DEBUG, "0x%02x ", u8ptr[i]); diff --git a/nimble/host/src/ble_hs_mbuf.c b/nimble/host/src/ble_hs_mbuf.c index 649f3106d..a3674d91b 100644 --- a/nimble/host/src/ble_hs_mbuf.c +++ b/nimble/host/src/ble_hs_mbuf.c @@ -39,7 +39,7 @@ ble_hs_mbuf_gen_pkt(uint16_t leading_space) return NULL; } - if (om->om_omp->omp_databuf_len < leading_space) { + if (OS_MBUF_TRAILINGSPACE(om) < leading_space) { rc = os_mbuf_free_chain(om); BLE_HS_DBG_ASSERT_EVAL(rc == 0); return NULL; @@ -72,8 +72,10 @@ struct os_mbuf * ble_hs_mbuf_acl_pkt(void) { #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE + /* +1 for HCI packet type indicator byte */ return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + 1); #else + /* +1 for HCI packet type indicator byte */ return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + BLE_HS_CTRL_DATA_HDR_SZ + 1); #endif } @@ -91,8 +93,10 @@ struct os_mbuf * ble_hs_mbuf_l2cap_pkt(void) { #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE + /* +1 for HCI packet type indicator byte */ return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + BLE_L2CAP_HDR_SZ + 1); #else + /* +1 for HCI packet type indicator byte */ return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + BLE_L2CAP_HDR_SZ + BLE_HS_CTRL_DATA_HDR_SZ + 1); #endif } @@ -103,9 +107,16 @@ ble_hs_mbuf_att_pkt(void) /* Prepare write request and response are the larget ATT commands which * contain attribute data. */ - return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + - BLE_L2CAP_HDR_SZ + - BLE_ATT_PREP_WRITE_CMD_BASE_SZ); +#if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE + return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + + BLE_L2CAP_HDR_SZ + + BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1); +#else + return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + + BLE_L2CAP_HDR_SZ + + BLE_HS_CTRL_DATA_HDR_SZ + + BLE_ATT_PREP_WRITE_CMD_BASE_SZ + 1); +#endif } struct os_mbuf * @@ -114,6 +125,10 @@ ble_hs_mbuf_from_flat(const void *buf, uint16_t len) struct os_mbuf *om; int rc; + if (buf == NULL && len > 0) { + return NULL; + } + om = ble_hs_mbuf_att_pkt(); if (om == NULL) { return NULL; @@ -135,6 +150,10 @@ ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len, uint16_t copy_len; int rc; + if (om == NULL) { + return BLE_HS_EINVAL; + } + if (OS_MBUF_PKTLEN(om) <= max_len) { copy_len = OS_MBUF_PKTLEN(om); } else { @@ -146,7 +165,7 @@ ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len, return BLE_HS_EUNKNOWN; } - if (copy_len > max_len) { + if (OS_MBUF_PKTLEN(om) > max_len) { rc = BLE_HS_EMSGSIZE; } else { rc = 0; @@ -161,6 +180,10 @@ ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len, int ble_hs_mbuf_pullup_base(struct os_mbuf **om, int base_len) { + if (base_len <= 0) { + return BLE_HS_EINVAL; + } + if (OS_MBUF_PKTLEN(*om) < base_len) { return BLE_HS_EBADDATA; } diff --git a/nimble/host/src/ble_hs_misc.c b/nimble/host/src/ble_hs_misc.c index 4cbfec2bf..a7934336d 100644 --- a/nimble/host/src/ble_hs_misc.c +++ b/nimble/host/src/ble_hs_misc.c @@ -18,7 +18,6 @@ */ #include -#include #include "os/os.h" #include "ble_hs_priv.h" @@ -122,21 +121,22 @@ ble_hs_misc_restore_one_irk(int obj_type, union ble_store_value *val, void *cookie) { const struct ble_store_value_sec *sec; - int rc = -1; BLE_HS_DBG_ASSERT(obj_type == BLE_STORE_OBJ_TYPE_PEER_SEC); sec = &val->sec; if (sec->irk_present) { #if MYNEWT_VAL(BLE_HS_PVCY) + int rc; rc = ble_hs_pvcy_add_entry(sec->peer_addr.val, sec->peer_addr.type, sec->irk); -#endif if (rc != 0) { BLE_HS_LOG(ERROR, "failed to configure restored IRK\n"); } +#endif } + /* We return 0 ("best effort") even if ble_hs_pvcy_add_entry fails. */ return 0; } diff --git a/nimble/host/src/ble_hs_mqueue.c b/nimble/host/src/ble_hs_mqueue.c index 99628c1fb..419865e2f 100644 --- a/nimble/host/src/ble_hs_mqueue.c +++ b/nimble/host/src/ble_hs_mqueue.c @@ -32,6 +32,12 @@ ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg) void ble_mqueue_deinit(struct ble_mqueue *mq) { + struct os_mbuf *om; + + while ((om = ble_mqueue_get(mq)) != NULL) { + os_mbuf_free_chain(om); + } + ble_npl_event_deinit(&mq->ev); } @@ -66,7 +72,7 @@ ble_mqueue_put(struct ble_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(om)) { + if (om == NULL || !OS_MBUF_IS_PKTHDR(om)) { rc = OS_EINVAL; goto err; } @@ -77,7 +83,7 @@ ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf STAILQ_INSERT_TAIL(&mq->head, mp, omp_next); OS_EXIT_CRITICAL(sr); - /* Only post an event to the queue if its specified */ + /* Only post an event to the queue if it's specified */ if (evq) { ble_npl_eventq_put(evq, &mq->ev); } diff --git a/nimble/host/src/ble_hs_periodic_sync.c b/nimble/host/src/ble_hs_periodic_sync.c index 71ca5eee4..e6a03ea94 100644 --- a/nimble/host/src/ble_hs_periodic_sync.c +++ b/nimble/host/src/ble_hs_periodic_sync.c @@ -65,6 +65,14 @@ ble_hs_periodic_sync_ensure_ctx(void) } #endif +/** + * Allocates an entry from the periodic sync pool. + * + * @return The allocated entry on success; + * NULL on failure. + * + * @note This function expects the host lock to be held. + */ struct ble_hs_periodic_sync * ble_hs_periodic_sync_alloc(void) { @@ -78,11 +86,20 @@ ble_hs_periodic_sync_alloc(void) return psync; } +/** + * Frees an entry to the periodic sync pool. + * + * @param psync The entry to free. + * + * @note This function expects the host lock to be held. + */ void ble_hs_periodic_sync_free(struct ble_hs_periodic_sync *psync) { int rc; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + if (psync == NULL) { return; } @@ -157,6 +174,10 @@ ble_hs_periodic_sync_find(const ble_addr_t *addr, uint8_t sid) /** * Retrieves the first periodic discovery handle in the list. + * + * @note The returned pointer is only valid while the host lock is held. + * The caller should hold the lock or use the handle immediately + * with caution (TOCTOU risk). */ struct ble_hs_periodic_sync * ble_hs_periodic_sync_first(void) @@ -170,10 +191,10 @@ ble_hs_periodic_sync_first(void) return psync; } -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) void ble_hs_periodic_sync_deinit(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_periodic_ctx == NULL) { return; @@ -182,13 +203,22 @@ ble_hs_periodic_sync_deinit(void) if (ble_hs_psync_elem_mem) { nimble_platform_mem_free(ble_hs_psync_elem_mem); + ble_hs_psync_elem_mem = NULL; } #endif memset(&ble_hs_periodic_sync_pool, 0, sizeof(ble_hs_periodic_sync_pool)); nimble_platform_mem_free(ble_hs_periodic_ctx); ble_hs_periodic_ctx = NULL; -} +#else +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + if (ble_hs_psync_elem_mem) + { + nimble_platform_mem_free(ble_hs_psync_elem_mem); + ble_hs_psync_elem_mem = NULL; + } #endif +#endif +} int ble_hs_periodic_sync_init(void) @@ -214,6 +244,18 @@ ble_hs_periodic_sync_init(void) return 0; } +#if !MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) && MYNEWT_VAL(MP_RUNTIME_ALLOC) + if (!ble_hs_psync_elem_mem) { + ble_hs_psync_elem_mem = nimble_platform_mem_calloc(1, + OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS), + sizeof(struct ble_hs_periodic_sync)) * + sizeof(os_membuf_t)); + } + if (!ble_hs_psync_elem_mem) { + return BLE_HS_ENOMEM; + } +#endif + #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) #if !MYNEWT_VAL(MP_RUNTIME_ALLOC) if (!ble_hs_psync_elem_mem) @@ -227,18 +269,16 @@ ble_hs_periodic_sync_init(void) if (ble_hs_psync_elem_mem == NULL) { -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_hs_periodic_ctx) { nimble_platform_mem_free(ble_hs_periodic_ctx); ble_hs_periodic_ctx = NULL; } -#endif return BLE_HS_ENOMEM; } +#endif +#endif memset(&ble_hs_periodic_sync_pool, 0, sizeof(ble_hs_periodic_sync_pool)); -#endif -#endif rc = os_mempool_init(&ble_hs_periodic_sync_pool, MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS), @@ -250,6 +290,7 @@ ble_hs_periodic_sync_init(void) if (ble_hs_psync_elem_mem) { nimble_platform_mem_free(ble_hs_psync_elem_mem); + ble_hs_psync_elem_mem = NULL; } #endif if (ble_hs_periodic_ctx) @@ -257,6 +298,12 @@ ble_hs_periodic_sync_init(void) nimble_platform_mem_free(ble_hs_periodic_ctx); ble_hs_periodic_ctx = NULL; } +#elif MYNEWT_VAL(MP_RUNTIME_ALLOC) + if (ble_hs_psync_elem_mem) + { + nimble_platform_mem_free(ble_hs_psync_elem_mem); + ble_hs_psync_elem_mem = NULL; + } #endif return BLE_HS_EOS; } diff --git a/nimble/host/src/ble_hs_priv.h b/nimble/host/src/ble_hs_priv.h index fbf09c41a..09218878a 100644 --- a/nimble/host/src/ble_hs_priv.h +++ b/nimble/host/src/ble_hs_priv.h @@ -159,6 +159,7 @@ struct ble_mqueue { }; int ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg); +void ble_mqueue_deinit(struct ble_mqueue *mq); struct os_mbuf *ble_mqueue_get(struct ble_mqueue *mq); int ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om); diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index 08a52eaae..6279ac88a 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/nimble/host/src/ble_hs_pvcy.c @@ -51,16 +51,34 @@ uint8_t ble_hs_pvcy_default_irk[16]; uint16_t l_rpa_timeout; #endif +#define BLE_MAX_RPA_TIMEOUT_VAL 0xA1B8 + +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) +void ble_store_config_init(void); +#endif + +const uint8_t * +ble_hs_pvcy_get_default_irk(void) +{ +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx) { + return ble_hs_pvcy_ctx->pvcy_default_irk; + } + return NULL; +#else + return ble_hs_pvcy_default_irk; +#endif +} + static int ble_hs_pvcy_set_addr_timeout(uint16_t timeout) { - struct ble_hci_le_set_rpa_tmo_cp cmd; - #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) return ble_hs_resolv_set_rpa_tmo(timeout); -#endif +#else + struct ble_hci_le_set_rpa_tmo_cp cmd; - if (timeout == 0 || timeout > 0xA1B8) { + if (timeout == 0 || timeout > BLE_MAX_RPA_TIMEOUT_VAL) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -69,10 +87,16 @@ ble_hs_pvcy_set_addr_timeout(uint16_t timeout) return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RPA_TMO), &cmd, sizeof(cmd), NULL, 0); +#endif } -int ble_hs_set_rpa_timeout (uint16_t timeout) +int ble_hs_set_rpa_timeout(uint16_t timeout) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return BLE_HS_ENOMEM; + } +#endif l_rpa_timeout = timeout; return ble_hs_pvcy_set_addr_timeout(l_rpa_timeout); @@ -80,12 +104,22 @@ int ble_hs_set_rpa_timeout (uint16_t timeout) uint16_t ble_hs_get_rpa_timeout(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return 0; + } +#endif return l_rpa_timeout; } void ble_hs_reset_rpa_timeout(void) { - l_rpa_timeout = 0 ; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return; + } +#endif + l_rpa_timeout = 0; } #if (!MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) @@ -108,14 +142,18 @@ ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr) struct ble_hci_le_rmv_resolve_list_cp cmd; int rc; + BLE_HS_DBG_ASSERT(addr != NULL); + if (addr_type > BLE_ADDR_RANDOM) { - addr_type = addr_type % 2; + addr_type %= 2; } cmd.peer_addr_type = addr_type; memcpy(cmd.peer_id_addr, addr, BLE_DEV_ADDR_LEN); #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_hs_lock(); rc = ble_hs_resolv_list_rmv(addr_type, &cmd.peer_id_addr[0]); + ble_hs_unlock(); #else rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RMV_RESOLV_LIST), @@ -141,6 +179,9 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, struct ble_hci_le_add_resolv_list_cp cmd; int rc; + BLE_HS_DBG_ASSERT(addr != NULL); + BLE_HS_DBG_ASSERT(irk != NULL); + if (addr_type > BLE_ADDR_RANDOM) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -219,6 +260,12 @@ ble_hs_pvcy_ensure_started(void) int rc; uint16_t rpa_timeout; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return BLE_HS_ENOMEM; + } +#endif + if (ble_hs_pvcy_started) { return 0; } @@ -309,7 +356,10 @@ void ble_hs_pvcy_set_default_irk(void) value_local_irk.addr.type = BLE_ADDR_PUBLIC; - ble_store_write_local_irk(&value_local_irk); + rc = ble_store_write_local_irk(&value_local_irk); + if (rc != 0) { + BLE_HS_LOG(WARN, "Failed to persist local IRK (rc=%d)", rc); + } } } @@ -331,6 +381,12 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) uint8_t new_irk[16]; int rc; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return BLE_HS_ENOMEM; + } +#endif + if (irk != NULL) { memcpy(new_irk, irk, 16); } else { @@ -379,9 +435,9 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) * such case. Peer IRK should be left all-zero since this is not for an * actual peer. */ + uint8_t zero_irk[16] = {0}; memset(tmp_addr, 0, 6); - memset(new_irk, 0, 16); - rc = ble_hs_pvcy_add_entry(tmp_addr, 0, new_irk); + rc = ble_hs_pvcy_add_entry(tmp_addr, 0, zero_irk); if (rc != 0) { return rc; } @@ -395,6 +451,16 @@ ble_hs_pvcy_our_irk(const uint8_t **out_irk) { /* XXX: Return error if privacy not supported. */ +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return BLE_HS_ENOMEM; + } +#endif + + if (out_irk == NULL) { + return BLE_HS_EINVAL; + } + *out_irk = ble_hs_pvcy_irk; return 0; } @@ -404,6 +470,10 @@ ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode) { struct ble_hci_le_set_privacy_mode_cp cmd; + if (addr == NULL) { + return BLE_HS_EINVAL; + } + if (addr->type > BLE_ADDR_RANDOM) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -421,6 +491,11 @@ ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode) bool ble_hs_pvcy_enabled(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hs_pvcy_ctx == NULL) { + return false; + } +#endif return ble_hs_pvcy_started; } diff --git a/nimble/host/src/ble_hs_pvcy_priv.h b/nimble/host/src/ble_hs_pvcy_priv.h index a68ec4ab9..3167fdc9b 100644 --- a/nimble/host/src/ble_hs_pvcy_priv.h +++ b/nimble/host/src/ble_hs_pvcy_priv.h @@ -27,11 +27,12 @@ extern "C" { #endif #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -extern uint8_t *ble_hs_pvcy_default_irk; +/* Accessor for default IRK */ #else extern uint8_t ble_hs_pvcy_default_irk[16]; #endif +const uint8_t *ble_hs_pvcy_get_default_irk(void); void ble_hs_pvcy_set_default_irk(void); int ble_hs_pvcy_set_our_irk(const uint8_t *irk); int ble_hs_pvcy_our_irk(const uint8_t **out_irk); @@ -45,7 +46,9 @@ 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 0222fa6e1..d6f24b874 100644 --- a/nimble/host/src/ble_hs_resolv.c +++ b/nimble/host/src/ble_hs_resolv.c @@ -73,6 +73,11 @@ ble_rpa_get_num_peer_dev_records(void) void ble_rpa_set_num_peer_dev_records(int num_rec) { + if (num_rec < 0 || num_rec > BLE_RESOLV_LIST_SIZE) { + ble_store_num_peer_dev_rec = 0; + BLE_HS_LOG(ERROR, "Invalid peer dev record count %d, resetting to 0\n", num_rec); + return; + } ble_store_num_peer_dev_rec = num_rec; } @@ -81,6 +86,8 @@ ble_rpa_remove_peer_dev_rec(struct ble_hs_dev_records *p_dev_rec) { int i; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + for (i = 0; i < ble_store_num_peer_dev_rec; i++) { if (!(memcmp(p_dev_rec, &peer_dev_rec[i], sizeof(struct ble_hs_dev_records)))) { @@ -96,9 +103,9 @@ ble_rpa_remove_peer_dev_rec(struct ble_hs_dev_records *p_dev_rec) ble_store_num_peer_dev_rec--; if ((i != ble_store_num_peer_dev_rec) && (ble_store_num_peer_dev_rec != 0)) { memmove(&peer_dev_rec[i], &peer_dev_rec[i + 1], - (ble_store_num_peer_dev_rec - i) * sizeof(struct ble_hs_dev_records )); - memset(&peer_dev_rec[ble_store_num_peer_dev_rec], 0, sizeof(struct ble_hs_dev_records)); + (ble_store_num_peer_dev_rec - i) * sizeof(struct ble_hs_dev_records)); } + memset(&peer_dev_rec[ble_store_num_peer_dev_rec], 0, sizeof(struct ble_hs_dev_records)); BLE_HS_LOG(DEBUG, " RPA: removed device at index = %d, no. of peer records" " = %d\n", i, ble_store_num_peer_dev_rec); @@ -110,28 +117,11 @@ ble_rpa_remove_peer_dev_rec(struct ble_hs_dev_records *p_dev_rec) static void ble_rpa_peer_dev_rec_clear_all(void) { - uint8_t i; - int num_peer_dev_rec = ble_store_num_peer_dev_rec; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - /* As NVS record need to be deleted one by one, we loop through - * peer_records */ - for (i = 0; i < num_peer_dev_rec; i++) { - ble_store_num_peer_dev_rec--; - - if (ble_store_num_peer_dev_rec > 1) { - /* Copy (n-1) valid elements, only if the array can hold at least 2 */ -#if BLE_RESOLV_LIST_SIZE > 1 - size_t move_count = (ble_store_num_peer_dev_rec - 1) * sizeof(peer_dev_rec[0]); - __builtin_memmove(&peer_dev_rec[0], &peer_dev_rec[1], move_count); -#endif - } - - memset(&peer_dev_rec[ble_store_num_peer_dev_rec], 0, - sizeof(struct ble_hs_dev_records)); - - ble_store_persist_peer_records(); - } - return; + ble_store_num_peer_dev_rec = 0; + memset(peer_dev_rec, 0, sizeof(peer_dev_rec)); + ble_store_persist_peer_records(); } /* Find peer device record with the address value. @@ -144,7 +134,13 @@ struct ble_hs_dev_records * ble_rpa_find_peer_dev_rec(uint8_t *addr) { struct ble_hs_dev_records *p_dev_rec = &peer_dev_rec[0]; - uint8_t i; + int i; + + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + + if (addr == NULL) { + return NULL; + } for (i = 0; i < ble_store_num_peer_dev_rec; i++, p_dev_rec++) { if (p_dev_rec->rec_used) { @@ -162,14 +158,17 @@ ble_rpa_find_peer_dev_rec(uint8_t *addr) static struct ble_hs_dev_records * ble_rpa_find_peer_dev_by_irk(uint8_t *irk) { + if (ble_store_num_peer_dev_rec > BLE_RESOLV_LIST_SIZE || irk == NULL) { + return NULL; + } struct ble_hs_dev_records *p_dev_rec = &peer_dev_rec[0]; - uint8_t i; + int i; // changed to int to avoid overflow for (i = 0; i < ble_store_num_peer_dev_rec; i++, p_dev_rec++) { if ((p_dev_rec->rec_used) && (!memcmp(irk, p_dev_rec->peer_sec.irk, 16))) { return p_dev_rec; - } + } } return NULL; } @@ -207,8 +206,6 @@ ble_rpa_replace_id_with_rand_addr(uint8_t *addr_type, uint8_t *peer_addr) p_addr.type = *addr_type; memcpy(&p_addr.val[0], peer_addr, BLE_DEV_ADDR_LEN); - ble_hs_lock(); - conn = ble_hs_conn_find_by_addr(&p_addr); /* Rewrite the peer address history in ble_hs_conn. Need to take * this step to avoid taking wrong address during re-pairing @@ -221,8 +218,6 @@ ble_rpa_replace_id_with_rand_addr(uint8_t *addr_type, uint8_t *peer_addr) BLE_HS_LOG(DEBUG, "\n Replace Identity addr with random addr received at" " start of the connection\n"); } - - ble_hs_unlock(); } } return; @@ -237,8 +232,14 @@ ble_rpa_replace_id_with_rand_addr(uint8_t *addr_type, uint8_t *peer_addr) int ble_rpa_resolv_add_peer_rec(uint8_t *peer_addr) { - struct ble_hs_dev_records *p_dev_rec = - &peer_dev_rec[ble_store_num_peer_dev_rec]; + struct ble_hs_dev_records *p_dev_rec; + + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + + if (ble_store_num_peer_dev_rec >= BLE_RESOLV_LIST_SIZE) { + return BLE_HS_ESTORE_CAP; + } + p_dev_rec = &peer_dev_rec[ble_store_num_peer_dev_rec]; p_dev_rec->rec_used = 1; memcpy(p_dev_rec->pseudo_addr, peer_addr, BLE_DEV_ADDR_LEN); @@ -257,6 +258,8 @@ ble_rpa_find_rl_from_peer_records(uint8_t *peer_addr, uint8_t *peer_addr_type) int i; int rc = 0; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + for (i = (ble_store_num_peer_dev_rec - 1); i >= 0; i--) { p_dev_rec = &peer_dev_rec[i]; /* If the record is not used, skip */ @@ -313,7 +316,11 @@ ble_rpa_replace_peer_params_with_rl(uint8_t *peer_addr, uint8_t *addr_type, struct ble_hs_resolv_entry **rl) { struct ble_hs_resolv_entry *rl_tmp = NULL; - bool is_rpa = ble_hs_is_rpa(peer_addr, *addr_type); + bool is_rpa; + + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + + is_rpa = ble_hs_is_rpa(peer_addr, *addr_type); if (is_rpa) { ble_hs_log_flat_buf(peer_addr, BLE_DEV_ADDR_LEN); @@ -355,17 +362,18 @@ ble_host_rpa_enabled(void) return false; } -static void +static int ble_hs_rand_prand_get(uint8_t *prand) { uint16_t sum; - uint8_t rc; + int rc; + int retry = 100; - while (1) { + while (retry--) { /* Get 24 bits of random data */ rc = ble_hs_hci_util_rand(prand, 3); if (rc != 0) { - return; + return rc; } /* Prand cannot be all zeros or 1's. */ @@ -375,9 +383,33 @@ ble_hs_rand_prand_get(uint8_t *prand) } } + 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; +} + +static bool +is_irk_nonzero(uint8_t *irk) +{ + int i; + bool rc = false; + + for (i = 0; i < 16; ++i) { + if (*irk != 0) { + rc = true; + break; + } + ++irk; + } + + return rc; } /** @@ -404,7 +436,9 @@ ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local) /* Get prand */ prand = addr + 3; - ble_hs_rand_prand_get(prand); + if (ble_hs_rand_prand_get(prand) != 0) { + return; + } /* Calculate hash, hash = ah(local IRK, prand) */ memcpy(ecb.key, irk, 16); @@ -473,22 +507,6 @@ ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev) return; } -static bool -is_irk_nonzero(uint8_t *irk) -{ - int i; - bool rc = false; - - for (i = 0; i < 16; ++i) { - if (*irk != 0) { - rc = true; - break; - } - ++irk; - } - - return rc; -} /** * Used to determine if the device is on the resolving list. @@ -528,6 +546,8 @@ ble_hs_is_on_resolv_list(uint8_t *addr, uint8_t addr_type) struct ble_hs_resolv_entry * ble_hs_resolv_list_find(uint8_t *addr) { + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) int i; struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[1]; @@ -546,6 +566,8 @@ ble_hs_resolv_list_find(uint8_t *addr) } ++rl; } +#else + (void)addr; #endif return NULL; } @@ -558,20 +580,24 @@ ble_hs_resolv_list_find(uint8_t *addr) int ble_hs_resolv_list_add(uint8_t *cmdbuf) { + const struct ble_hci_le_add_resolv_list_cp *cmd = + (const struct ble_hci_le_add_resolv_list_cp *)cmdbuf; uint8_t addr_type; - uint8_t *ident_addr; + const uint8_t *ident_addr; struct ble_hs_resolv_entry *rl; struct ble_hs_dev_records *p_dev_rec = NULL; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + /* Check if we have any open entries */ if (g_ble_hs_resolv_data.rl_cnt >= BLE_RESOLV_LIST_SIZE) { return BLE_HS_ENOMEM; } - addr_type = cmdbuf[0]; - ident_addr = cmdbuf + 1; + addr_type = cmd->peer_addr_type; + ident_addr = cmd->peer_id_addr; - if (ble_hs_is_on_resolv_list(ident_addr, addr_type)) { + if (ble_hs_is_on_resolv_list((uint8_t *)ident_addr, addr_type)) { return BLE_HS_EINVAL; } @@ -580,8 +606,8 @@ ble_hs_resolv_list_add(uint8_t *cmdbuf) rl->rl_addr_type = addr_type; memcpy(rl->rl_identity_addr, ident_addr, BLE_DEV_ADDR_LEN); - swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16); - swap_buf(rl->rl_local_irk, cmdbuf + 23, 16); + swap_buf(rl->rl_peer_irk, cmd->peer_irk, 16); + swap_buf(rl->rl_local_irk, cmd->local_irk, 16); if (is_irk_nonzero(rl->rl_peer_irk)) { p_dev_rec = ble_rpa_find_peer_dev_by_irk(rl->rl_peer_irk); @@ -613,6 +639,8 @@ ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr) { int rc = BLE_HS_ENOENT; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) int position; @@ -644,14 +672,28 @@ ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr) void ble_hs_resolv_list_clear_all(void) { + struct ble_hs_resolv_entry local_entry; + bool restore_local = false; + + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + + /* Preserve local device entry at index 0 if it exists */ + if (g_ble_hs_resolv_data.rl_cnt > 0) { + memcpy(&local_entry, &g_ble_hs_resolv_list[0], sizeof(local_entry)); + restore_local = true; + } + g_ble_hs_resolv_data.rl_cnt = 0; memset(g_ble_hs_resolv_list, 0, BLE_RESOLV_LIST_SIZE * sizeof(struct ble_hs_resolv_entry)); + if (restore_local) { + memcpy(&g_ble_hs_resolv_list[0], &local_entry, sizeof(local_entry)); + g_ble_hs_resolv_data.rl_cnt = 1; + } + /* Now delete peer device records as well */ ble_rpa_peer_dev_rec_clear_all(); - - return; } /** @@ -707,6 +749,8 @@ ble_hs_resolv_enable(bool enable) int ble_hs_resolv_set_rpa_tmo(uint16_t tmo_secs) { + uint32_t ticks; + /* Though the check validates smaller timeout values, it is recommended to * set it to enough bigger value (~15 minutes). There is no point in * changing RPA address aggressively and ending up sacrificing normal BLE @@ -716,7 +760,12 @@ ble_hs_resolv_set_rpa_tmo(uint16_t tmo_secs) return BLE_HS_EINVAL; } - g_ble_hs_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(tmo_secs * 1000); + ticks = ble_npl_time_ms_to_ticks32(tmo_secs * 1000); + if (ticks > INT32_MAX) { + return BLE_HS_EINVAL; + } + + g_ble_hs_resolv_data.rpa_tmo = ticks; /* If resolving is not enabled, we are done here. */ if (!is_ble_hs_resolv_enabled()) { @@ -732,6 +781,12 @@ ble_hs_resolv_set_rpa_tmo(uint16_t tmo_secs) struct ble_hs_resolv_entry * ble_hs_resolv_rpa_addr(uint8_t *addr, uint8_t addr_type) { + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + + if (!ble_hs_is_rpa(addr, addr_type)) { + return NULL; + } + #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) int i; struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[1]; diff --git a/nimble/host/src/ble_hs_shutdown.c b/nimble/host/src/ble_hs_shutdown.c index f29d4a669..a22743ee4 100644 --- a/nimble/host/src/ble_hs_shutdown.c +++ b/nimble/host/src/ble_hs_shutdown.c @@ -30,8 +30,15 @@ static struct ble_hs_stop_listener ble_hs_shutdown_stop_listener; static void ble_hs_shutdown_stop_cb(int status, void *arg) { + (void)arg; + SYSDOWN_ASSERT_ACTIVE(); + if (status != 0) { + BLE_HS_LOG(ERROR, "ble_hs_shutdown: stop completed with error; " + "status=%d\n", status); + } + /* Indicate to sysdown that the host is fully shut down. */ sysdown_release(); } @@ -41,6 +48,8 @@ ble_hs_shutdown(int reason) { int rc; + (void)reason; + /* Ensure this function only gets called by sysdown. */ SYSDOWN_ASSERT_ACTIVE(); @@ -54,6 +63,7 @@ ble_hs_shutdown(int reason) case BLE_HS_EBUSY: /* Already stopping. Wait for result to be reported asynchronously. */ + /* Note: ble_hs_stop() currently remaps EBUSY to 0 internally; kept here for robustness if the API changes. */ return SYSDOWN_IN_PROGRESS; case BLE_HS_EALREADY: @@ -61,8 +71,13 @@ ble_hs_shutdown(int reason) return SYSDOWN_COMPLETE; default: + /* If ble_hs_stop() failed after initiating the stop procedure, the + * callback has already been (or will be) called, so we must return + * SYSDOWN_IN_PROGRESS to remain consistent with the sysdown_release() + * call. + */ BLE_HS_LOG(ERROR, "ble_hs_shutdown: failed to stop host; rc=%d\n", rc); - return SYSDOWN_COMPLETE; + return SYSDOWN_IN_PROGRESS; } } diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index 511ecc379..1cde3be4c 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -40,7 +40,7 @@ ble_hs_startup_read_sup_f_tx(void) /* for now we don't use it outside of init sequence so check this here * LE Supported (Controller) byte 4, bit 6 */ - if (!(le64toh(rsp.features) & 0x0000006000000000)) { + if (!(le64toh(rsp.features) & 0x0000004000000000)) { BLE_HS_LOG(ERROR, "Controller doesn't support LE\n"); return BLE_HS_ECONTROLLER; } @@ -76,7 +76,7 @@ ble_hs_startup_le_read_sup_f_tx(void) rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT), - NULL,0, &rsp, sizeof(rsp)); + NULL, 0, &rsp, sizeof(rsp)); if (rc != 0) { return rc; } @@ -171,7 +171,10 @@ ble_hs_startup_read_bd_addr(void) return rc; } - ble_hs_id_set_pub(rsp.addr); + rc = ble_hs_id_set_pub(rsp.addr); + if (rc != 0) { + return rc; + } return 0; } @@ -316,15 +319,15 @@ ble_hs_startup_le_set_evmask_tx(void) if (version >= BLE_HCI_VER_BCS_6_0) { /** - * Enable following LE events: - * 0x0000040000000000 LE Read All Remote Features Complete event - * 0x0008000000000000 LE Monitored Advertisers Report event - */ + * Enable following LE events: + * 0x0000040000000000 LE Read All Remote Features Complete event + * 0x0008000000000000 LE Monitored Advertisers Report event + */ #if MYNEWT_VAL(BLE_MONITOR_ADV) mask |= 0x0008000000000000; #endif - mask |= 0x0000040000000000; + mask |= 0x0000040000000000; } cmd.event_mask = htole64(mask); @@ -456,7 +459,9 @@ ble_hs_startup_go(void) ble_hs_pvcy_set_our_irk(NULL); #endif - /* If flow control is enabled, configure the controller to use it. */ + /* If flow control is enabled, configure the controller to use it. + * Ignore return code (flow control failure is non-fatal). + */ ble_hs_flow_startup(); return 0; diff --git a/nimble/host/src/ble_hs_stop.c b/nimble/host/src/ble_hs_stop.c index fbb7b2ffb..95f34a9ac 100644 --- a/nimble/host/src/ble_hs_stop.c +++ b/nimble/host/src/ble_hs_stop.c @@ -58,7 +58,6 @@ static struct ble_gap_event_listener ble_hs_stop_gap_listener; /** * List of stop listeners. These are notified when a stop procedure completes. */ -//SLIST_HEAD(ble_hs_stop_listener_slist, ble_hs_stop_listener); static struct ble_hs_stop_listener_slist ble_hs_stop_listeners; /* Track number of connections */ @@ -66,6 +65,7 @@ static uint8_t ble_hs_stop_conn_cnt; static struct ble_npl_callout ble_hs_stop_terminate_tmo; #endif + /** * Called when a stop procedure has completed. */ @@ -79,6 +79,11 @@ ble_hs_stop_done(int status) ble_hs_lock(); + if (ble_hs_enabled_state != BLE_HS_ENABLED_STATE_STOPPING) { + ble_hs_unlock(); + return; + } + ble_gap_event_listener_unregister(&ble_hs_stop_gap_listener); slist = ble_hs_stop_listeners; @@ -138,7 +143,7 @@ ble_hs_stop_terminate_conn(struct ble_hs_conn *conn, void *arg) int rc; rc = ble_gap_terminate_with_conn(conn, BLE_ERR_REM_USER_CONN_TERM); - if (rc == 0) { + if (rc == 0 || rc == BLE_HS_EALREADY) { /* Terminate procedure successfully initiated. Let the GAP event * handler deal with the result. */ @@ -179,12 +184,17 @@ ble_hs_stop_gap_event(struct ble_gap_event *event, void *arg) /* Only process connection termination events. */ if (event->type == BLE_GAP_EVENT_DISCONNECT || event->type == BLE_GAP_EVENT_TERM_FAILURE) { + ble_hs_lock(); + if (ble_hs_stop_conn_cnt > 0) { + ble_hs_stop_conn_cnt--; - ble_hs_stop_conn_cnt--; - - if (ble_hs_stop_conn_cnt == 0) { - ble_hs_stop_done(0); + if (ble_hs_stop_conn_cnt == 0) { + ble_hs_unlock(); + ble_hs_stop_done(0); + return 0; + } } + ble_hs_unlock(); } return 0; @@ -270,6 +280,7 @@ ble_hs_stop(struct ble_hs_stop_listener *listener, /* Check for active periodic sync first and terminate it all */ rc = ble_hs_stop_terminate_all_periodic_sync(); if (rc != 0) { + ble_hs_stop_done(rc); return rc; } #endif @@ -277,17 +288,21 @@ ble_hs_stop(struct ble_hs_stop_listener *listener, rc = ble_gap_event_listener_register(&ble_hs_stop_gap_listener, ble_hs_stop_gap_event, NULL); if (rc != 0) { + ble_hs_stop_done(rc); return rc; } ble_hs_lock(); + ble_hs_stop_conn_cnt = 0; ble_hs_conn_foreach(ble_hs_stop_terminate_conn, NULL); - ble_hs_unlock(); - - if (ble_hs_stop_conn_cnt > 0) { + uint8_t cnt = ble_hs_stop_conn_cnt; + if (cnt > 0) { ble_npl_callout_reset(&ble_hs_stop_terminate_tmo, ble_npl_time_ms_to_ticks32(BLE_HOST_STOP_TIMEOUT_MS)); - } else { + } + ble_hs_unlock(); + + if (cnt == 0) { /* No connections, stop is completed */ ble_hs_stop_done(0); } @@ -303,6 +318,7 @@ ble_hs_stop_init(void) ble_hs_stop_ctx = nimble_platform_mem_calloc(1, sizeof(*ble_hs_stop_ctx)); if (!ble_hs_stop_ctx) { MODLOG_DFLT(ERROR, "Failed to allocate memory for ble_hs_stop_ctx\n"); + assert(0); return; } } diff --git a/nimble/host/src/ble_ibeacon.c b/nimble/host/src/ble_ibeacon.c index bbd353cc9..79ab8afa8 100644 --- a/nimble/host/src/ble_ibeacon.c +++ b/nimble/host/src/ble_ibeacon.c @@ -39,13 +39,21 @@ * Other nonzero on failure. */ int -ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, +ble_ibeacon_set_adv_data(const void *uuid128, uint16_t major, uint16_t minor, int8_t measured_power) { struct ble_hs_adv_fields fields; - uint8_t buf[BLE_IBEACON_MFG_DATA_SIZE]; + static uint8_t buf[BLE_IBEACON_MFG_DATA_SIZE]; int rc; + /* Validate inputs before any buffer writes */ + if (uuid128 == NULL) { + return BLE_HS_EINVAL; + } + if (measured_power < -126 || measured_power > 20) { + return BLE_HS_EINVAL; + } + /** Company identifier (Apple). */ buf[0] = 0x4c; buf[1] = 0x00; @@ -62,9 +70,6 @@ ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, put_be16(buf + 22, minor); /* Measured Power ranging data (Calibrated tx power at 1 meters). */ - if (measured_power < -126 || measured_power > 20) { - return BLE_HS_EINVAL; - } buf[24] = measured_power; memset(&fields, 0, sizeof fields); diff --git a/nimble/host/src/ble_l2cap.c b/nimble/host/src/ble_l2cap.c index 18b3f0d20..7dbfff6c0 100644 --- a/nimble/host/src/ble_l2cap.c +++ b/nimble/host/src/ble_l2cap.c @@ -631,7 +631,8 @@ ble_l2cap_init(void) STATS_HDR(ble_l2cap_stats), STATS_SIZE_INIT_PARMS(ble_l2cap_stats, STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_l2cap_stats), "ble_l2cap"); if (rc != 0) { - return BLE_HS_EOS; + rc = BLE_HS_EOS; + goto done; } return 0; @@ -659,13 +660,14 @@ ble_l2cap_deinit(void) } #endif memset(&ble_l2cap_chan_pool, 0, sizeof(ble_l2cap_chan_pool)); - } - ble_l2cap_sig_deinit(); - ble_l2cap_coc_deinit(); - ble_sm_deinit(); - nimble_platform_mem_free(ble_l2cap_ctx); - ble_l2cap_ctx = NULL; + ble_l2cap_sig_deinit(); + ble_l2cap_coc_deinit(); + ble_sm_deinit(); + + 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 ac0621721..cc5461414 100644 --- a/nimble/host/src/ble_l2cap_coc.c +++ b/nimble/host/src/ble_l2cap_coc.c @@ -78,6 +78,9 @@ ble_l2cap_coc_dbg_assert_srv_not_inserted(struct ble_l2cap_coc_srv *srv) #endif } +static struct ble_l2cap_coc_srv * +ble_l2cap_coc_srv_find(uint16_t psm); + static struct ble_l2cap_coc_srv * ble_l2cap_coc_srv_alloc(void) { @@ -102,8 +105,20 @@ ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, { struct ble_l2cap_coc_srv * srv; + if (cb == NULL) { + return BLE_HS_EINVAL; + } + + ble_hs_lock(); + + if (ble_l2cap_coc_srv_find(psm) != NULL) { + ble_hs_unlock(); + return BLE_HS_EALREADY; + } + srv = ble_l2cap_coc_srv_alloc(); if (!srv) { + ble_hs_unlock(); return BLE_HS_ENOMEM; } @@ -116,6 +131,8 @@ ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, STAILQ_INSERT_HEAD(&ble_l2cap_coc_srvs, srv, next); + ble_hs_unlock(); + return 0; } @@ -176,6 +193,12 @@ ble_l2cap_coc_srv_find(uint16_t psm) { struct ble_l2cap_coc_srv *cur, *srv; +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_l2cap_coc_ctx == NULL) { + return NULL; + } +#endif + srv = NULL; STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) { if (cur->psm == psm) { @@ -198,7 +221,11 @@ ble_l2cap_event_coc_received_data(struct ble_l2cap_chan *chan, event.receive.chan = chan; event.receive.sdu_rx = om; - chan->cb(&event, chan->cb_arg); + if (chan->cb) { + chan->cb(&event, chan->cb_arg); + } else { + os_mbuf_free_chain(om); + } } static int @@ -234,11 +261,10 @@ ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan) om_total, sdu_len + 2); /* We should receive payload of size sdu_len + 2 bytes of sdu_len field */ - if (om_total > sdu_len + 2) { + if ((uint32_t)om_total > (uint32_t)sdu_len + 2) { BLE_HS_LOG(ERROR, "Payload larger than expected (%d>%d)\n", om_total, sdu_len + 2); /* Disconnect peer with invalid behaviour */ - rx->sdu = NULL; rx->data_offset = 0; ble_l2cap_disconnect(chan); return BLE_HS_EBADDATA; @@ -259,36 +285,38 @@ ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan) rc = os_mbuf_appendfrom(rx->sdu, *om, 0, om_total - BLE_L2CAP_SDU_SIZE); if (rc != 0) { - /* FIXME: User shall give us big enough buffer. - * need to handle it better - */ - BLE_HS_LOG(INFO, "Could not append data rc=%d\n", rc); - assert(0); + BLE_HS_LOG(ERROR, "Could not append data rc=%d\n", rc); + ble_l2cap_disconnect(chan); + return rc; } /* In RX case data_offset keeps incoming SDU len */ rx->data_offset = sdu_len; } else { - BLE_HS_LOG(DEBUG, "Continuation...received %d\n", (*om)->om_len); + BLE_HS_LOG(DEBUG, "Continuation...received %d\n", om_total); - if (OS_MBUF_PKTLEN(rx->sdu) + (*om)->om_len > rx->data_offset) { + if (OS_MBUF_PKTLEN(rx->sdu) + om_total > rx->data_offset) { /* Disconnect peer with invalid behaviour */ BLE_HS_LOG(ERROR, "Payload larger than expected (%d>%d)\n", - OS_MBUF_PKTLEN(rx->sdu) + (*om)->om_len, rx->data_offset); - rx->sdu = NULL; + OS_MBUF_PKTLEN(rx->sdu) + om_total, rx->data_offset); rx->data_offset = 0; ble_l2cap_disconnect(chan); return BLE_HS_EBADDATA; } rc = os_mbuf_appendfrom(rx->sdu, *om, 0, om_total); if (rc != 0) { - /* FIXME: need to handle it better */ - BLE_HS_LOG(DEBUG, "Could not append data rc=%d\n", rc); - assert(0); + BLE_HS_LOG(ERROR, "Could not append data rc=%d\n", rc); + ble_l2cap_disconnect(chan); + return rc; } } + if (rx->credits == 0) { + BLE_HS_LOG(ERROR, "RX credits underflow, disconnecting\n"); + ble_l2cap_disconnect(chan); + return BLE_HS_EBADDATA; + } rx->credits--; if (OS_MBUF_PKTLEN(rx->sdu) == rx->data_offset) { @@ -331,6 +359,15 @@ ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan) void ble_l2cap_coc_set_new_mtu_mps(struct ble_l2cap_chan *chan, uint16_t mtu, uint16_t mps) { + if (chan == NULL) { + BLE_HS_LOG(WARN, "chan NULL in set_new_mtu_mps"); + return; + } + if (mps == 0) { + BLE_HS_LOG(ERROR, "mps cannot be 0\n"); + return; + } + chan->my_coc_mps = mps; chan->coc_rx.mtu = mtu; chan->initial_credits = mtu / chan->my_coc_mps; @@ -345,16 +382,24 @@ ble_l2cap_coc_chan_alloc(struct ble_hs_conn *conn, uint16_t psm, uint16_t mtu, void *cb_arg) { struct ble_l2cap_chan *chan; + int cid; chan = ble_l2cap_chan_alloc(conn->bhc_handle); if (!chan) { + os_mbuf_free_chain(sdu_rx); return NULL; } chan->psm = psm; chan->cb = cb; chan->cb_arg = cb_arg; - chan->scid = ble_l2cap_coc_get_cid(conn->l2cap_coc_cid_mask); + cid = ble_l2cap_coc_get_cid(conn->l2cap_coc_cid_mask); + if (cid < 0) { + chan->cb = NULL; + ble_l2cap_chan_free(conn, chan); + return NULL; + } + chan->scid = cid; chan->my_coc_mps = MYNEWT_VAL(BLE_L2CAP_COC_MPS); chan->rx_fn = ble_l2cap_coc_rx_fn; chan->coc_rx.mtu = mtu; @@ -616,8 +661,6 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx) return BLE_HS_EINVAL; } - chan->coc_rx.sdu = sdu_rx; - ble_hs_lock(); conn = ble_hs_conn_find_assert(chan->conn_handle); c = ble_hs_conn_chan_find_by_scid(conn, chan->scid); @@ -625,16 +668,29 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx) ble_hs_unlock(); return BLE_HS_ENOENT; } + if (chan->coc_rx.sdu != NULL) { + ble_hs_unlock(); + return BLE_HS_EBUSY; + } + chan->coc_rx.sdu = sdu_rx; /* We want to back only that much credits which remote side is missing * to be able to send complete SDU. */ if (chan->disable_auto_credit_update == false && chan->coc_rx.credits < c->initial_credits) { - ble_hs_unlock(); - ble_l2cap_sig_le_credits(chan->conn_handle, chan->scid, - c->initial_credits - chan->coc_rx.credits); - ble_hs_lock(); + uint16_t credits_to_send = c->initial_credits - chan->coc_rx.credits; + uint16_t cached_conn_handle = chan->conn_handle; + uint16_t cached_scid = chan->scid; + chan->coc_rx.credits = c->initial_credits; + + ble_hs_unlock(); + ble_l2cap_sig_le_credits(cached_conn_handle, cached_scid, credits_to_send); + ble_hs_lock(); + conn = ble_hs_conn_find(cached_conn_handle); + if (conn != NULL) { + c = ble_hs_conn_chan_find_by_scid(conn, cached_scid); + } } ble_hs_unlock(); @@ -682,7 +738,7 @@ ble_l2cap_coc_init(void) return BLE_HS_ENOMEM; } } -#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) // Doubt +#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) size_t srv_mem_bytes = OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM), sizeof (struct ble_l2cap_coc_srv)) * sizeof(os_membuf_t); @@ -707,10 +763,12 @@ ble_l2cap_coc_init(void) "ble_l2cap_coc_srv_pool"); if (rc != 0) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) -#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) +#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) nimble_platform_mem_free(ble_l2cap_coc_srv_mem); #endif memset(&ble_l2cap_coc_srv_pool, 0, sizeof(ble_l2cap_coc_srv_pool)); + nimble_platform_mem_free(ble_l2cap_coc_ctx); + ble_l2cap_coc_ctx = NULL; #endif } @@ -721,13 +779,14 @@ void ble_l2cap_coc_deinit(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_l2cap_coc_ctx) { -#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) // Doubt +#if !MYNEWT_VAL(MP_RUNTIME_ALLOC) if (ble_l2cap_coc_srv_mem) { nimble_platform_mem_free(ble_l2cap_coc_srv_mem); ble_l2cap_coc_srv_mem = NULL; } #endif memset(&ble_l2cap_coc_srv_pool, 0, sizeof(ble_l2cap_coc_srv_pool)); + STAILQ_INIT(&ble_l2cap_coc_srvs); nimble_platform_mem_free(ble_l2cap_coc_ctx); ble_l2cap_coc_ctx = NULL; diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c index 8bda0baee..8f258015b 100644 --- a/nimble/host/src/ble_l2cap_sig.c +++ b/nimble/host/src/ble_l2cap_sig.c @@ -543,7 +543,8 @@ ble_l2cap_sig_update_rsp_rx(uint16_t conn_handle, } if (OS_MBUF_PKTLEN(*om) != BLE_L2CAP_SIG_UPDATE_RSP_SZ) { - return BLE_HS_EBADDATA; + cb_status = BLE_HS_EBADDATA; + goto done; } rsp = (struct ble_l2cap_sig_update_rsp *)(*om)->om_data; @@ -699,7 +700,7 @@ ble_l2cap_sig_ble_hs_err2coc_err(uint16_t ble_hs_err) } static void -ble_l2cap_event_coc_connected(struct ble_l2cap_chan *chan, uint16_t status) +ble_l2cap_event_coc_connected(struct ble_l2cap_chan *chan, int status) { struct ble_l2cap_event event = { }; @@ -761,7 +762,7 @@ ble_l2cap_sig_coc_connect_cb(struct ble_l2cap_sig_proc *proc, int status) /* Free not connected channels*/ ble_hs_lock(); - conn = ble_hs_conn_find(chan->conn_handle); + conn = ble_hs_conn_find(proc->conn_handle); for (i = 0; i < proc->connect.chan_cnt; i++) { chan = proc->connect.chan[i]; if (chan) { @@ -793,7 +794,9 @@ ble_l2cap_event_coc_reconfigured(uint16_t conn_handle, uint16_t status, event.reconfigured.chan = chan; event.reconfigured.status = status; - chan->cb(&event, chan->cb_arg); + if (chan != NULL && chan->cb != NULL) { + chan->cb(&event, chan->cb_arg); + } } static int @@ -810,6 +813,9 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle, int rc; uint8_t cid_cnt; uint8_t reduction_mps = 0; + uint16_t host_mtu = 0; + uint16_t host_mps = 0; + uint16_t host_dcid = 0; rc = ble_hs_mbuf_pullup_base(om, hdr->length); if (rc != 0) { @@ -840,8 +846,9 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle, } req = (struct ble_l2cap_sig_credit_base_reconfig_req *)(*om)->om_data; - - if ((req->mps < BLE_L2CAP_ECOC_MIN_MTU) || (req->mtu < BLE_L2CAP_ECOC_MIN_MTU)) { + host_mtu = le16toh(req->mtu); + host_mps = le16toh(req->mps); + if ((host_mps < BLE_L2CAP_ECOC_MIN_MTU) || (host_mtu < BLE_L2CAP_ECOC_MIN_MTU)) { rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM); goto failed; } @@ -856,17 +863,22 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle, } for (i = 0; i < cid_cnt; i++) { - chan[i] = ble_hs_conn_chan_find_by_dcid(conn, req->dcids[i]); + host_dcid = le16toh(req->dcids[i]); + /* DCID in request corresponds to the destination CIDs of the device sending the request. + * Hence, it corresponds to the source CID of the device receiving the request. + * Core Specification Vol 3 Part A 4.28 + */ + chan[i] = ble_hs_conn_chan_find_by_scid(conn, host_dcid); if (!chan[i]) { rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_INVALID_DCID); goto failed; } - if (chan[i]->peer_coc_mps > req->mps) { + if (chan[i]->peer_coc_mps > host_mps) { reduction_mps++; } - if (chan[i]->coc_tx.mtu > req->mtu) { + if (chan[i]->coc_tx.mtu > host_mtu) { rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED); goto failed; } @@ -880,8 +892,8 @@ ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle, ble_hs_unlock(); for (i = 0; i < cid_cnt; i++) { - chan[i]->coc_tx.mtu = req->mtu; - chan[i]->peer_coc_mps = req->mps; + chan[i]->coc_tx.mtu = host_mtu; + chan[i]->peer_coc_mps = host_mps; ble_l2cap_event_coc_reconfigured(conn_handle, 0, chan[i], true); } @@ -911,7 +923,7 @@ ble_l2cap_sig_coc_reconfig_cb(struct ble_l2cap_sig_proc *proc, int status) for (i = 0; i< proc->reconfig.cid_cnt; i++) { chan[i] = ble_hs_conn_chan_find_by_scid(conn, proc->reconfig.cids[i]); - if (status == 0) { + if (chan[i] && status == 0) { ble_l2cap_coc_set_new_mtu_mps(chan[i], proc->reconfig.new_mtu, proc->reconfig.new_mps); } } @@ -919,7 +931,9 @@ ble_l2cap_sig_coc_reconfig_cb(struct ble_l2cap_sig_proc *proc, int status) ble_hs_unlock(); for (i = 0; i < proc->reconfig.cid_cnt; i++) { - ble_l2cap_event_coc_reconfigured(proc->conn_handle, status, chan[i], false); + if (chan[i]) { + ble_l2cap_event_coc_reconfigured(proc->conn_handle, status, chan[i], false); + } } } @@ -941,18 +955,25 @@ ble_l2cap_sig_credit_base_reconfig_rsp_rx(uint16_t conn_handle, rc = ble_hs_mbuf_pullup_base(om, hdr->length); if (rc != 0) { - return rc; + goto done; } if (OS_MBUF_PKTLEN(*om) != sizeof(*rsp)) { - return BLE_HS_EBADDATA; + rc = BLE_HS_EBADDATA; + goto done; } 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); + +done: + if (rc != 0) { + ble_l2cap_sig_coc_reconfig_cb(proc, rc); + } + ble_l2cap_sig_proc_free(proc); - return 0; + return rc ? rc : 0; } static int @@ -964,9 +985,10 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_credit_base_connect_req *req; struct os_mbuf *txom; struct ble_l2cap_sig_credit_base_connect_rsp *rsp; - struct ble_l2cap_chan *chans[5] = { 0 }; + struct ble_l2cap_chan *chans[BLE_L2CAP_MAX_COC_CONN_REQ] = { 0 }; struct ble_hs_conn *conn; uint16_t scid; + uint16_t result; uint8_t num_of_scids; uint8_t chan_created = 0; int i; @@ -1005,7 +1027,7 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle, req = (struct ble_l2cap_sig_credit_base_connect_req *)(*om)->om_data; num_of_scids = (hdr->length - sizeof(*req)) / sizeof(uint16_t); - if (num_of_scids > 5) { + if (num_of_scids > BLE_L2CAP_MAX_COC_CONN_REQ) { rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS); goto failed; } @@ -1024,6 +1046,13 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle, rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID); goto failed; } + /* Check for duplicate SCIDs within the same request */ + for (int j = 0; j < i; j++) { + if (le16toh(req->scids[j]) == scid) { + rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID); + goto failed; + } + } } /* Let us try to connect channels */ @@ -1093,23 +1122,40 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle, done: ble_hs_unlock(); + result = rsp->result; rc = ble_l2cap_sig_tx(conn_handle, txom); if (rc != 0) { - ble_hs_lock(); - conn = ble_hs_conn_find_assert(conn_handle); + /* Notify application of failure first, then clean up */ for (i = 0; i < num_of_scids; i++) { if (chans[i]) { - ble_hs_conn_delete_chan(conn, chans[i]); + ble_l2cap_event_coc_connected(chans[i], rc); + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn) { + ble_hs_conn_delete_chan(conn, chans[i]); + } + ble_hs_unlock(); + chans[i] = NULL; } } - ble_hs_unlock(); return 0; } /* Notify user about connection status */ for (i = 0; i < num_of_scids; i++) { if (chans[i]) { - ble_l2cap_event_coc_connected(chans[i], rc); + if (result == 0) { + ble_l2cap_event_coc_connected(chans[i], 0); + } else { + ble_l2cap_event_coc_connected(chans[i], BLE_HS_EUNKNOWN); + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + if (conn) { + ble_hs_conn_delete_chan(conn, chans[i]); + } + ble_hs_unlock(); + chans[i] = NULL; + } } } @@ -1130,9 +1176,9 @@ ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_credit_base_connect_rsp *rsp; struct ble_l2cap_chan *chan; struct ble_hs_conn *conn; - int rc; + int rc = 0; int i; - uint16_t duplicated_cids[5] = {}; + uint16_t duplicated_cids[BLE_L2CAP_MAX_COC_CONN_REQ] = {0}; #if !BLE_MONITOR BLE_HS_LOG(DEBUG, "L2CAP LE COC connection response received\n"); @@ -1150,6 +1196,11 @@ ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle, goto done; } + if (hdr->length < sizeof(*rsp) + (proc->connect.chan_cnt * sizeof(uint16_t))) { + rc = BLE_HS_EBADDATA; + goto done; + } + rsp = (struct ble_l2cap_sig_credit_base_connect_rsp *)(*om)->om_data; if (rsp->result) { @@ -1178,8 +1229,11 @@ ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle, chan->dcid = 0; continue; } - if (ble_hs_conn_chan_find_by_dcid(conn, rsp->dcids[i])) { - duplicated_cids[i] = rsp->dcids[i]; + + uint16_t peer_dcid = le16toh(rsp->dcids[i]); + + if (ble_hs_conn_chan_find_by_dcid(conn, peer_dcid)) { + duplicated_cids[i] = peer_dcid; chan->dcid = 0; continue; } @@ -1195,13 +1249,19 @@ ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle, ble_hs_unlock(); done: - for (i = 0; i < 5; i++){ + for (i = 0; i < BLE_L2CAP_MAX_COC_CONN_REQ; i++) { if (duplicated_cids[i] != 0){ ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - chan = ble_hs_conn_chan_find_by_dcid(conn, duplicated_cids[i]); + if (conn != NULL) { + chan = ble_hs_conn_chan_find_by_dcid(conn, duplicated_cids[i]); + } else { + chan = NULL; + } ble_hs_unlock(); - rc = ble_l2cap_sig_disconnect(chan); + if (chan != NULL) { + rc = ble_l2cap_sig_disconnect(chan); + } } } @@ -1349,7 +1409,8 @@ ble_l2cap_sig_coc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, } if (OS_MBUF_PKTLEN(*om) != sizeof(*rsp)) { - return BLE_HS_EBADDATA; + rc = BLE_HS_EBADDATA; + goto done; } rsp = (struct ble_l2cap_sig_le_con_rsp *)(*om)->om_data; @@ -1449,7 +1510,7 @@ ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, rc = ble_l2cap_sig_tx(proc->conn_handle, txom); if (rc != 0) { ble_hs_lock(); - conn = ble_hs_conn_find_assert(conn_handle); + conn = ble_hs_conn_find(conn_handle); ble_l2cap_chan_free(conn, chan); ble_hs_unlock(); } @@ -1493,6 +1554,12 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, 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; @@ -1514,7 +1581,7 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, for (j = 0; j < i; j++) { /* Clear callback to make sure "Disconnected event" to the user */ - chan[j].cb = NULL; + proc->connect.chan[j]->cb = NULL; ble_l2cap_chan_free(conn, proc->connect.chan[j]); } ble_hs_unlock(); @@ -1523,6 +1590,7 @@ ble_l2cap_sig_ecoc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, } proc->connect.chan[i] = chan; } + proc->connect.chan_cnt = num; req->psm = htole16(psm); @@ -1568,6 +1636,12 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[], return BLE_HS_ENOMEM; } + if (num == 0 || num > BLE_L2CAP_MAX_COC_CONN_REQ) { + ble_hs_unlock(); + rc = BLE_HS_EINVAL; + goto done; + } + for (i = 0; i < num; i++) { if (ble_hs_conn_chan_exist(conn, chans[i])) { proc->reconfig.cids[i] = chans[i]->scid; @@ -1645,6 +1719,8 @@ ble_l2cap_sig_disc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, conn = ble_hs_conn_find_assert(conn_handle); if (OS_MBUF_PKTLEN(*om) != sizeof(*req)) { + os_mbuf_free_chain(txom); + ble_hs_unlock(); return BLE_HS_EBADDATA; } @@ -1730,11 +1806,14 @@ ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, rc = ble_hs_mbuf_pullup_base(om, sizeof(*rsp)); if (rc != 0) { + ble_l2cap_sig_coc_disconnect_cb(proc, rc); goto done; } if (OS_MBUF_PKTLEN(*om) != sizeof(*rsp)) { - return BLE_HS_EBADDATA; + rc = BLE_HS_EBADDATA; + ble_l2cap_sig_coc_disconnect_cb(proc, rc); + goto done; } chan = proc->disconnect.chan; @@ -1753,7 +1832,7 @@ ble_l2cap_sig_disc_rsp_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, done: ble_l2cap_sig_proc_free(proc); - return 0; + return rc; } int @@ -1862,7 +1941,7 @@ ble_l2cap_sig_rx_reject(uint16_t conn_handle, return 0; } - switch (proc->id) { + switch (proc->op) { #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 case BLE_L2CAP_SIG_PROC_OP_CONNECT: ble_l2cap_sig_coc_connect_cb(proc, BLE_HS_EREJECT); @@ -1910,6 +1989,10 @@ ble_l2cap_sig_rx(struct ble_l2cap_chan *chan) os_mbuf_adj(*om, BLE_L2CAP_SIG_HDR_SZ); if (OS_MBUF_PKTLEN(*om) != hdr.length) { + if (hdr.op != BLE_L2CAP_SIG_OP_REJECT) { + ble_l2cap_sig_reject_tx(conn_handle, hdr.identifier, + BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD, NULL, 0); + } return BLE_HS_EBADDATA; } @@ -2001,9 +2084,33 @@ void ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason) { struct ble_l2cap_sig_proc *proc; + struct ble_l2cap_sig_proc *prev = NULL; + struct ble_l2cap_sig_proc *next; + struct ble_l2cap_sig_proc_list temp_list; + STAILQ_INIT(&temp_list); - /* Report a failure for each timed out procedure. */ - while ((proc = STAILQ_FIRST(&ble_l2cap_sig_procs)) != NULL) { + ble_hs_lock(); + + proc = STAILQ_FIRST(&ble_l2cap_sig_procs); + while (proc != NULL) { + next = STAILQ_NEXT(proc, next); + if (proc->conn_handle == conn_handle) { + if (prev == NULL) { + STAILQ_REMOVE_HEAD(&ble_l2cap_sig_procs, next); + } else { + STAILQ_REMOVE_AFTER(&ble_l2cap_sig_procs, prev, next); + } + STAILQ_INSERT_TAIL(&temp_list, proc, next); + } else { + prev = proc; + } + proc = next; + } + + ble_hs_unlock(); + + while ((proc = STAILQ_FIRST(&temp_list)) != NULL) { + STAILQ_REMOVE_HEAD(&temp_list, next); switch(proc->op) { case BLE_L2CAP_SIG_PROC_OP_UPDATE: ble_l2cap_sig_update_call_cb(proc, reason); @@ -2021,12 +2128,9 @@ ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason) break; #endif #endif - } - - STAILQ_REMOVE_HEAD(&ble_l2cap_sig_procs, next); - ble_l2cap_sig_proc_free(proc); + } + ble_l2cap_sig_proc_free(proc); } - } /** diff --git a/nimble/host/src/ble_l2cap_sig_cmd.c b/nimble/host/src/ble_l2cap_sig_cmd.c index 48b35249e..50d2cf8d7 100644 --- a/nimble/host/src/ble_l2cap_sig_cmd.c +++ b/nimble/host/src/ble_l2cap_sig_cmd.c @@ -57,7 +57,7 @@ ble_l2cap_sig_hdr_parse(void *payload, uint16_t len, int ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason, - void *data, int data_len) + void *data, uint16_t data_len) { struct ble_l2cap_sig_reject *cmd; struct os_mbuf *txom; @@ -71,7 +71,7 @@ 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); - STATS_INC(ble_l2cap_stats, sig_rx); + STATS_INC(ble_l2cap_stats, sig_tx); return ble_l2cap_sig_tx(conn_handle, txom); } @@ -105,6 +105,7 @@ ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len, if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) { os_mbuf_free_chain(*txom); + *txom = NULL; return NULL; } diff --git a/nimble/host/src/ble_l2cap_sig_priv.h b/nimble/host/src/ble_l2cap_sig_priv.h index 0c89b95a5..0e985f564 100644 --- a/nimble/host/src/ble_l2cap_sig_priv.h +++ b/nimble/host/src/ble_l2cap_sig_priv.h @@ -119,7 +119,7 @@ void ble_l2cap_sig_hdr_parse(void *payload, uint16_t len, struct ble_l2cap_sig_hdr *hdr); int ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason, - void *data, int data_len); + void *data, uint16_t data_len); int ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id, uint16_t src_cid, uint16_t dst_cid); int ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom); diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index 33fb13cba..d24f098a4 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -573,6 +573,8 @@ ble_sm_ia_ra(struct ble_sm_proc *proc, struct ble_hs_conn_addrs addrs; struct ble_hs_conn *conn; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + conn = ble_hs_conn_find_assert(proc->conn_handle); ble_hs_conn_addrs(conn, &addrs); @@ -607,6 +609,10 @@ ble_sm_persist_keys(struct ble_sm_proc *proc) conn = ble_hs_conn_find(proc->conn_handle); BLE_HS_DBG_ASSERT(conn != NULL); + if (conn == NULL) { + ble_hs_unlock(); + return; + } /* If we got an identity address, use that for key storage. */ if (proc->peer_keys.addr_valid) { @@ -667,6 +673,8 @@ ble_sm_persist_keys(struct ble_sm_proc *proc) ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type); } + ble_addr_t peer_rpa_addr = conn->bhc_peer_rpa_addr; + ble_hs_unlock(); if (identity_ev) { @@ -687,8 +695,8 @@ ble_sm_persist_keys(struct ble_sm_proc *proc) value_rpa_rec.peer_addr.type = peer_addr.type; memcpy(value_rpa_rec.peer_addr.val, peer_addr.val, sizeof peer_addr.val); - value_rpa_rec.peer_rpa_addr.type = conn->bhc_peer_rpa_addr.type; - memcpy(value_rpa_rec.peer_rpa_addr.val, conn->bhc_peer_rpa_addr.val, sizeof conn->bhc_peer_rpa_addr.val); + value_rpa_rec.peer_rpa_addr.type = peer_rpa_addr.type; + memcpy(value_rpa_rec.peer_rpa_addr.val, peer_rpa_addr.val, sizeof peer_rpa_addr.val); ble_store_write_rpa_rec(&value_rpa_rec); } @@ -805,9 +813,9 @@ ble_sm_extract_expired(struct ble_sm_proc_list *dst_list) if (time_diff < next_exp_in) { next_exp_in = time_diff; } + prev = proc; } - prev = proc; proc = next; } @@ -1050,7 +1058,7 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res) ble_sm_exec(proc, res, res->state_arg); } - if (res->app_status != 0) { + if (res && res->app_status != 0) { rm = 1; } @@ -1076,10 +1084,16 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res) } #if MYNEWT_VAL(BLE_RESTART_PAIR) - if (res->app_status == 518 ) { + if (res->app_status == BLE_HS_HCI_ERR(BLE_ERR_PINKEY_MISSING)) { + ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); - + if (conn == NULL) { + ble_hs_unlock(); + ble_sm_proc_free(proc); + break; + } conn_flags = conn->bhc_flags; + ble_hs_unlock(); ble_sm_proc_free(proc); @@ -1095,7 +1109,7 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res) /* Persist keys if bonding has successfully completed. */ if (res->app_status == 0 && rm && - proc->flags & BLE_SM_PROC_F_BONDING) { + (proc->flags & BLE_SM_PROC_F_BONDING)) { ble_sm_persist_keys(proc); } @@ -1387,6 +1401,12 @@ ble_sm_retrieve_ltk(uint16_t ediv, uint64_t rand, uint8_t peer_addr_type, key_sec.ediv_rand_present = 1; rc = ble_store_read_our_sec(&key_sec, value_sec); + if (rc != 0) { + return rc; + } + if (value_sec->ediv != ediv || value_sec->rand_num != rand) { + return BLE_HS_ENOENT; + } return rc; } @@ -1478,12 +1498,20 @@ ble_sm_ltk_restore_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, /* Application does not have the requested key in its database. Send a * negative reply to the controller. */ - ble_sm_ltk_req_neg_reply_tx(proc->conn_handle); - res->app_status = BLE_HS_ENOENT; + int rc; + rc = ble_sm_ltk_req_neg_reply_tx(proc->conn_handle); + if (rc != 0) { + res->app_status = rc; + res->enc_cb = 1; /* Notify application of failure, similar to reply branch */ + } else { + res->app_status = 0; + } } if (res->app_status == 0) { + /* Slave LTK restore complete, only wait for enc change event, no further exec needed */ proc->state = BLE_SM_PROC_STATE_ENC_RESTORE; + res->restore = 1; } } @@ -1874,6 +1902,7 @@ ble_sm_pair_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, } rc = ble_sm_tx(proc->conn_handle, txom); + txom = NULL; if (rc != 0) { goto err; } @@ -2373,10 +2402,13 @@ ble_sm_key_exch_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, if (ble_host_rpa_enabled()) { uint8_t *local_id_rpa = NULL; + int rc_id; - ble_hs_id_addr(BLE_ADDR_PUBLIC, (void *) &local_id_rpa, NULL); - memcpy(addrs.our_id_addr.val, local_id_rpa, 6); - addrs.our_id_addr.type = BLE_ADDR_PUBLIC; + rc_id = ble_hs_id_addr(BLE_ADDR_PUBLIC, (void *) &local_id_rpa, NULL); + if (rc_id == 0 && local_id_rpa != NULL) { + memcpy(addrs.our_id_addr.val, local_id_rpa, 6); + addrs.our_id_addr.type = BLE_ADDR_PUBLIC; + } } #endif addr_info->addr_type = addrs.our_id_addr.type; @@ -2775,8 +2807,9 @@ ble_sm_incr_peer_sign_counter(uint16_t conn_handle) #if MYNEWT_VAL(BLE_HS_PVCY) if (value_sec.irk_present == 1) { ble_hs_pvcy_remove_entry(value_sec.peer_addr.type, value_sec.peer_addr.val); - // No need to check if the above command fails or passes - // Proceed with trying to write the new sign counter + /* No need to check if the above command fails or passes. + * Proceed with trying to write the new sign counter. + */ } #endif @@ -3190,7 +3223,7 @@ static void ble_sm_state_dispatch_deinit(void) { if (ble_sm_state_dispatch) { nimble_platform_mem_free(ble_sm_state_dispatch); - ble_sm_state_dispatch = NULL; + ble_sm_state_dispatch = NULL; } } #endif @@ -3214,9 +3247,9 @@ ble_sm_init(void) if (!ble_sm_proc_mem) { ble_sm_proc_mem = nimble_platform_mem_calloc(1,proc_mem_size * sizeof(os_membuf_t)); if (!ble_sm_proc_mem) { - // free the allocated memory + /* free the allocated memory */ nimble_platform_mem_free(ble_sm_ctx); - ble_sm_ctx = NULL; + ble_sm_ctx = NULL; return BLE_HS_ENOMEM; } } @@ -3233,7 +3266,7 @@ ble_sm_init(void) if (rc != 0) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - ble_sm_deinit(); + ble_sm_deinit(); #endif return rc; } @@ -3242,7 +3275,7 @@ ble_sm_init(void) rc = ble_sm_state_dispatch_init(); if (rc != 0) { - ble_sm_deinit(); + ble_sm_deinit(); return rc; } diff --git a/nimble/host/src/ble_sm_alg.c b/nimble/host/src/ble_sm_alg.c index 4dc696f31..be45d5bcf 100644 --- a/nimble/host/src/ble_sm_alg.c +++ b/nimble/host/src/ble_sm_alg.c @@ -288,6 +288,9 @@ ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len, exit: mbedtls_cipher_free(&ctx); + if (rc != 0) { + rc = BLE_HS_EUNKNOWN; + } return rc; } @@ -402,7 +405,8 @@ ble_sm_alg_f5(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, rc = ble_sm_alg_aes_cmac(salt, ws, 32, t); if (rc != 0) { - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto exit; } ble_sm_alg_log_buf("t", t, 16); @@ -416,7 +420,8 @@ ble_sm_alg_f5(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, rc = ble_sm_alg_aes_cmac(t, m, sizeof(m), mackey); if (rc != 0) { - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto exit; } ble_sm_alg_log_buf("mackey", mackey, 16); @@ -428,14 +433,23 @@ ble_sm_alg_f5(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, rc = ble_sm_alg_aes_cmac(t, m, sizeof(m), ltk); if (rc != 0) { - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto exit; } ble_sm_alg_log_buf("ltk", ltk, 16); swap_in_place(ltk, 16); + rc = 0; - return 0; +exit: + /* Zero sensitive key material from stack */ + memset(ws, 0, sizeof(ws)); + memset(t, 0, sizeof(t)); + /* Use a memory barrier to prevent compiler from optimizing out the memsets */ + __asm__ volatile("" : : "r"(ws), "r"(t) : "memory"); + + return rc; } int @@ -465,16 +479,18 @@ ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, swap_buf(m + 48, iocap, 3); m[51] = a1t; - memcpy(m + 52, a1, 6); swap_buf(m + 52, a1, 6); m[58] = a2t; - memcpy(m + 59, a2, 6); swap_buf(m + 59, a2, 6); swap_buf(ws, w, 16); rc = ble_sm_alg_aes_cmac(ws, m, sizeof(m), check); + + /* Zero sensitive key material from stack */ + memset(ws, 0, sizeof(ws)); + if (rc != 0) { return BLE_HS_EUNKNOWN; } @@ -604,22 +620,34 @@ exit: keypair_ptr = NULL; } #endif - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto exit_cleanup; } #else if (uECC_valid_public_key(pk, uECC_secp256r1()) < 0) { - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto exit_cleanup; } rc = uECC_shared_secret(pk, priv, dh, uECC_secp256r1()); if (rc == TC_CRYPTO_FAIL) { - return BLE_HS_EUNKNOWN; + rc = BLE_HS_EUNKNOWN; + goto exit_cleanup; } #endif swap_buf(out_dhkey, dh, 32); - return 0; + rc = 0; + +exit_cleanup: + /* Zero sensitive key material from stack */ + memset(dh, 0, sizeof(dh)); + memset(priv, 0, sizeof(priv)); + /* Use a memory barrier to prevent compiler from optimizing out the memsets */ + __asm__ __volatile__("" : : "r"(dh), "r"(priv) : "memory"); + + return rc; } /* based on Core Specification 4.2 Vol 3. Part H 2.3.5.6.1 */ @@ -653,6 +681,9 @@ mbedtls_gen_keypair(uint8_t *public_key, uint8_t *private_key) #if MYNEWT_VAL(BLE_SM_SC) && MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (!keypair_ptr) { keypair_ptr = nimble_platform_mem_calloc(1, sizeof(mbedtls_ecp_keypair)); + if (!keypair_ptr) { + return BLE_HS_ENOMEM; + } } #endif diff --git a/nimble/host/src/ble_sm_cmd.c b/nimble/host/src/ble_sm_cmd.c index 9f2099273..85f33e7ac 100644 --- a/nimble/host/src/ble_sm_cmd.c +++ b/nimble/host/src/ble_sm_cmd.c @@ -18,7 +18,6 @@ */ #include -#include #include "nimble/ble.h" #include "nimble/nimble_opt.h" #include "host/ble_sm.h" @@ -26,7 +25,7 @@ #if NIMBLE_BLE_CONNECT void * -ble_sm_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom) +ble_sm_cmd_get(uint8_t opcode, uint16_t len, struct os_mbuf **txom) { #if NIMBLE_BLE_SM struct ble_sm_hdr *hdr; @@ -40,6 +39,7 @@ ble_sm_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom) data = os_mbuf_extend(*txom, sizeof(*hdr) + len); if (data == NULL) { os_mbuf_free_chain(*txom); + *txom = NULL; return NULL; } @@ -62,6 +62,7 @@ ble_sm_tx(uint16_t conn_handle, struct os_mbuf *txom) int rc; BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + BLE_HS_DBG_ASSERT(txom != NULL); STATS_INC(ble_l2cap_stats, sm_tx); diff --git a/nimble/host/src/ble_sm_lgcy.c b/nimble/host/src/ble_sm_lgcy.c index 050b86b89..5917e1fc2 100644 --- a/nimble/host/src/ble_sm_lgcy.c +++ b/nimble/host/src/ble_sm_lgcy.c @@ -36,8 +36,8 @@ #define IOACT_INPUT BLE_SM_IOACT_INPUT #define IOACT_DISP BLE_SM_IOACT_DISP -/* This is the initiator passkey action action depending on the io - * capabilties of both parties +/* This is the initiator passkey action depending on the io + * capabilities of both parties */ static const uint8_t ble_sm_lgcy_init_ioa[5 /*resp*/ ][5 /*init*/ ] = { @@ -48,7 +48,7 @@ static const uint8_t ble_sm_lgcy_init_ioa[5 /*resp*/ ][5 /*init*/ ] = {IOACT_DISP, IOACT_DISP, IOACT_INPUT, IOACT_NONE, IOACT_DISP}, }; -/* This is the responder passkey action action depending on the io +/* This is the responder passkey action depending on the io * capabilities of both parties */ static const uint8_t ble_sm_lgcy_resp_ioa[5 /*resp*/ ][5 /*init*/ ] = @@ -68,20 +68,17 @@ ble_sm_lgcy_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) { + /* OOB takes precedence over static passkey per BLE spec */ + *action = BLE_SM_IOACT_OOB; #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; } else if (!(pair_req->authreq & BLE_SM_PAIR_AUTHREQ_MITM) && !(pair_rsp->authreq & BLE_SM_PAIR_AUTHREQ_MITM)) { @@ -152,6 +149,7 @@ ble_sm_lgcy_confirm_exec(struct ble_sm_proc *proc, struct ble_sm_result *res) } rc = ble_sm_tx(proc->conn_handle, txom); + txom = NULL; /* ble_sm_tx consumes txom on all paths */ if (rc != 0) { goto err; } @@ -178,6 +176,9 @@ ble_sm_gen_stk(struct ble_sm_proc *proc) uint8_t key[16]; int rc; + BLE_HS_DBG_ASSERT(proc->key_size >= BLE_SM_PAIR_KEY_SZ_MIN && + proc->key_size <= BLE_SM_PAIR_KEY_SZ_MAX); + rc = ble_sm_alg_s1(proc->tk, proc->rands, proc->randm, key); if (rc != 0) { return rc; @@ -188,6 +189,9 @@ ble_sm_gen_stk(struct ble_sm_proc *proc) /* Ensure proper key size */ memset(proc->ltk + proc->key_size, 0, sizeof key - proc->key_size); + /* Zero sensitive key material from stack */ + memset(key, 0, sizeof(key)); + return 0; } @@ -242,8 +246,13 @@ ble_sm_lgcy_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res) return; } - if (memcmp(proc->confirm_peer, confirm_val, 16) != 0) { - /* Random number mismatch. */ + /* Use constant-time comparison to avoid timing side-channel leaks */ + uint8_t diff = 0; + for (int i = 0; i < 16; i++) { + diff |= proc->confirm_peer[i] ^ confirm_val[i]; + } + if (diff != 0) { + /* Confirm value mismatch. */ res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH); res->sm_err = BLE_SM_ERR_CONFIRM_MISMATCH; res->enc_cb = 1; diff --git a/nimble/host/src/ble_sm_priv.h b/nimble/host/src/ble_sm_priv.h index 4834a9467..fd77d817d 100644 --- a/nimble/host/src/ble_sm_priv.h +++ b/nimble/host/src/ble_sm_priv.h @@ -436,7 +436,7 @@ void ble_sm_deinit(void); #endif struct ble_l2cap_chan *ble_sm_create_chan(uint16_t handle); -void *ble_sm_cmd_get(uint8_t opcode, size_t len, struct os_mbuf **txom); +void *ble_sm_cmd_get(uint8_t opcode, uint16_t len, struct os_mbuf **txom); int ble_sm_tx(uint16_t conn_handle, struct os_mbuf *txom); #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) diff --git a/nimble/host/src/ble_sm_sc.c b/nimble/host/src/ble_sm_sc.c index 7be961da2..f05c3a444 100644 --- a/nimble/host/src/ble_sm_sc.c +++ b/nimble/host/src/ble_sm_sc.c @@ -261,18 +261,10 @@ ble_sm_sc_ensure_keys_generated(void) } BLE_HS_LOG(DEBUG, "our pubkey="); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) ble_hs_log_flat_buf(ble_sm_sc_pub_key, 64); -#else - ble_hs_log_flat_buf(&ble_sm_sc_pub_key, 64); -#endif BLE_HS_LOG(DEBUG, "\n"); BLE_HS_LOG(DEBUG, "our privkey="); -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) ble_hs_log_flat_buf(ble_sm_sc_priv_key, 32); -#else - ble_hs_log_flat_buf(&ble_sm_sc_priv_key, 32); -#endif BLE_HS_LOG(DEBUG, "\n"); return 0; @@ -482,6 +474,7 @@ ble_sm_sc_random_exec(struct ble_sm_proc *proc, struct ble_sm_result *res) cmd = ble_sm_cmd_get(BLE_SM_OP_PAIR_RANDOM, sizeof(*cmd), &txom); if (cmd == NULL) { rc = BLE_HS_ENOMEM; + res->app_status = rc; res->enc_cb = 1; res->sm_err = BLE_SM_ERR_UNSPECIFIED; return; @@ -507,7 +500,13 @@ ble_sm_sc_random_exec(struct ble_sm_proc *proc, struct ble_sm_result *res) } rc = ble_sm_sc_io_action(proc, &ioact); - BLE_HS_DBG_ASSERT(rc == 0); + if (rc != 0) { + BLE_HS_DBG_ASSERT(0); + res->app_status = rc; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + res->enc_cb = 1; + return; + } if (ble_sm_ioact_state(ioact) == proc->state && !(proc->flags & BLE_SM_PROC_F_IO_INJECTED)) { @@ -587,11 +586,21 @@ ble_sm_sc_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res) proc->peer_keys.key_size = proc->key_size; if (proc->flags & BLE_SM_PROC_F_INITIATOR) { - ble_sm_sc_random_advance(proc); + rc = ble_sm_sc_random_advance(proc); + if (rc != 0) { + res->app_status = rc; + res->enc_cb = 1; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + return; + } rc = ble_sm_sc_io_action(proc, &ioact); if (rc != 0) { BLE_HS_DBG_ASSERT(0); + res->app_status = rc; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + res->enc_cb = 1; + return; } if (ble_sm_ioact_state(ioact) == proc->state && @@ -657,6 +666,10 @@ ble_sm_sc_public_key_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, rc = ble_sm_sc_io_action(proc, &ioact); if (rc != 0) { BLE_HS_DBG_ASSERT(0); + res->app_status = rc; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + res->enc_cb = 1; + return; } if (ble_sm_ioact_state(ioact) == proc->state) { @@ -734,6 +747,11 @@ ble_sm_sc_public_key_rx(uint16_t conn_handle, struct os_mbuf **om, rc = ble_sm_sc_io_action(proc, &ioact); if (rc != 0) { BLE_HS_DBG_ASSERT(0); + res->app_status = rc; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + res->enc_cb = 1; + ble_hs_unlock(); + return; } if (ble_sm_ioact_state(ioact) == proc->state) { @@ -760,6 +778,8 @@ ble_sm_sc_dhkey_addrs(struct ble_sm_proc *proc, ble_addr_t *our_addr, struct ble_hs_conn_addrs addrs; struct ble_hs_conn *conn; + BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); + conn = ble_hs_conn_find_assert(proc->conn_handle); ble_hs_conn_addrs(conn, &addrs); @@ -852,7 +872,7 @@ ble_sm_dhkey_check_process(struct ble_sm_proc *proc, iocap = &pair_rsp->io_cap; if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) { - if (pair_rsp->oob_data_flag) { + if (pair_rsp->oob_data_flag && proc->oob_data_local) { memcpy(proc->tk, proc->oob_data_local->r, 16); } else { memset(proc->tk, 0, 16); @@ -865,7 +885,7 @@ ble_sm_dhkey_check_process(struct ble_sm_proc *proc, iocap = &pair_req->io_cap; if (proc->pair_alg == BLE_SM_PAIR_ALG_OOB) { - if (pair_req->oob_data_flag) { + if (pair_req->oob_data_flag && proc->oob_data_local) { memcpy(proc->tk, proc->oob_data_local->r, 16); } else { memset(proc->tk, 0, 16); @@ -902,6 +922,10 @@ ble_sm_dhkey_check_process(struct ble_sm_proc *proc, rc = ble_sm_sc_io_action(proc, &ioact); if (rc != 0) { BLE_HS_DBG_ASSERT(0); + res->app_status = rc; + res->sm_err = BLE_SM_ERR_UNSPECIFIED; + res->enc_cb = 1; + return; } if (ble_sm_ioact_state(ioact) == proc->state) { @@ -965,9 +989,15 @@ ble_sm_sc_oob_data_check(struct ble_sm_proc *proc, rsp_oob_present = pair_rsp->oob_data_flag == BLE_SM_PAIR_OOB_YES; if (proc->flags & BLE_SM_PROC_F_INITIATOR) { - return req_oob_present == oob_data_remote_present; + /* Verify: our OOB claim matches reality, and if peer expects + * our OOB data (rsp flag set), we actually have local data */ + return (req_oob_present == oob_data_remote_present) && + (!rsp_oob_present || oob_data_local_present); } else { - return rsp_oob_present == oob_data_remote_present; + /* Verify: our OOB claim matches reality, and if peer expects + * our OOB data (req flag set), we actually have local data */ + return (rsp_oob_present == oob_data_remote_present) && + (!req_oob_present || oob_data_local_present); } } @@ -976,10 +1006,6 @@ ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data) { int rc; -#if !MYNEWT_VAL(BLE_SM_SC) - return BLE_HS_ENOTSUP; -#endif - rc = ble_sm_sc_ensure_keys_generated(); if (rc) { return rc; diff --git a/nimble/host/src/ble_store.c b/nimble/host/src/ble_store.c index 283fd373e..b1c66d114 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -52,13 +52,15 @@ ble_store_write(int obj_type, const union ble_store_value *val) #if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) int rc; - if (ble_hs_cfg.store_write_cb == NULL) { - return BLE_HS_ENOTSUP; - } - while (1) { ble_hs_lock(); - rc = ble_hs_cfg.store_write_cb(obj_type, val); + + if (ble_hs_cfg.store_write_cb == NULL) { + ble_hs_unlock(); + return BLE_HS_ENOTSUP; + } + + rc = ble_hs_cfg.store_write_cb(obj_type, val); ble_hs_unlock(); switch (rc) { @@ -240,7 +242,6 @@ int ble_store_delete_peer_sec(const struct ble_store_key_sec *key_sec) { #if NIMBLE_BLE_CONNECT - union ble_store_key *store_key; int rc; @@ -269,11 +270,7 @@ ble_store_read_peer_sec(const struct ble_store_key_sec *key_sec, store_value = (void *)value_sec; rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_SEC, store_key, store_value); - if (rc != 0) { - return rc; - } - - return 0; + return rc; #else return BLE_HS_ENOTSUP; #endif @@ -504,7 +501,6 @@ ble_store_key_from_value_ead(struct ble_store_key_ead *out_key, #endif } #endif -// local irk #if MYNEWT_VAL(BLE_HS_PVCY) int @@ -572,7 +568,6 @@ ble_store_key_from_value_local_irk(struct ble_store_key_local_irk *out_key, #endif } -// int ble_store_read_rpa_rec(const struct ble_store_key_rpa_rec *key, struct ble_store_value_rpa_rec *out_value) @@ -737,7 +732,10 @@ ble_store_iterate(int obj_type, } while (1) { - *pidx = idx; + *pidx = (uint8_t)idx; + if (idx > UINT8_MAX) { + return 0; + } rc = ble_store_read(obj_type, &key, &value); switch (rc) { case 0: diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c index 57c239285..df49e1ee2 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -45,9 +45,27 @@ ble_store_util_iter_unique_peer(int obj_type, set = arg; - /* Do nothing if this peer is a duplicate. */ + ble_addr_t peer_addr; + switch (obj_type) { + case BLE_STORE_OBJ_TYPE_OUR_SEC: + case BLE_STORE_OBJ_TYPE_PEER_SEC: + peer_addr = val->sec.peer_addr; + break; +#if MYNEWT_VAL(ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + peer_addr = val->ead.peer_addr; + break; +#endif + case BLE_STORE_OBJ_TYPE_LOCAL_IRK: + peer_addr = val->local_irk.addr; + break; + default: + BLE_HS_DBG_ASSERT(0); + return BLE_HS_EINVAL; + } + /* Use peer_addr in loop and assignment */ for (i = 0; i < set->num_peers; i++) { - if (ble_addr_cmp(set->peer_id_addrs + i, &val->sec.peer_addr) == 0) { + if (ble_addr_cmp(set->peer_id_addrs + i, &peer_addr) == 0) { return 0; } } @@ -55,10 +73,10 @@ ble_store_util_iter_unique_peer(int obj_type, if (set->num_peers >= set->max_peers) { /* Overflow; abort the iterate procedure. */ set->status = BLE_HS_ENOMEM; - return 1; + return BLE_HS_ENOMEM; } - set->peer_id_addrs[set->num_peers] = val->sec.peer_addr; + set->peer_id_addrs[set->num_peers] = peer_addr; set->num_peers++; return 0; @@ -84,6 +102,7 @@ ble_store_util_bonded_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, int max_peers) { #if NIMBLE_BLE_CONNECT + BLE_HS_DBG_ASSERT(out_num_peers != NULL); struct ble_store_util_peer_set set = { .peer_id_addrs = out_peer_id_addrs, @@ -101,6 +120,12 @@ ble_store_util_bonded_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, } *out_num_peers = set.num_peers; + + /* This holds the status of the operation */ + if (set.status != 0) { + return set.status; + } + return 0; #else return BLE_HS_ENOTSUP; @@ -248,6 +273,8 @@ ble_store_util_count(int type, int *out_count) int rc; + BLE_HS_DBG_ASSERT(out_count != NULL); + *out_count = 0; rc = ble_store_iterate(type, ble_store_util_iter_count, @@ -314,6 +341,8 @@ ble_store_util_ead_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, { #if NIMBLE_BLE_CONNECT + BLE_HS_DBG_ASSERT(out_num_peers != NULL); + struct ble_store_util_peer_set set = { .peer_id_addrs = out_peer_id_addrs, .num_peers = 0, @@ -329,6 +358,10 @@ 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) { + return set.status; + } + *out_num_peers = set.num_peers; return 0; #else @@ -392,9 +425,10 @@ ble_store_util_status_rr(struct ble_store_status_event *event, void *arg) case BLE_STORE_OBJ_TYPE_PEER_ADDR: return ble_gap_unpair_oldest_peer(); case BLE_STORE_OBJ_TYPE_CCCD: - case BLE_STORE_OBJ_TYPE_CSFC: /* Try unpairing oldest peer except current peer */ return ble_gap_unpair_oldest_except(&event->overflow.value->cccd.peer_addr); + case BLE_STORE_OBJ_TYPE_CSFC: + return ble_gap_unpair_oldest_except(&event->overflow.value->csfc.peer_addr); #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: return ble_store_util_delete_ead_oldest_peer(); diff --git a/nimble/host/src/ble_uuid.c b/nimble/host/src/ble_uuid.c index 9b2dd1a5f..822f0ffcc 100644 --- a/nimble/host/src/ble_uuid.c +++ b/nimble/host/src/ble_uuid.c @@ -82,9 +82,15 @@ ble_uuid_cmp(const ble_uuid_t *uuid1, const ble_uuid_t *uuid2) case BLE_UUID_TYPE_16: return (int) BLE_UUID16(uuid1)->value - (int) BLE_UUID16(uuid2)->value; case BLE_UUID_TYPE_32: - return (int) BLE_UUID32(uuid1)->value - (int) BLE_UUID32(uuid2)->value; + if (BLE_UUID32(uuid1)->value < BLE_UUID32(uuid2)->value) { + return -1; + } + if (BLE_UUID32(uuid1)->value > BLE_UUID32(uuid2)->value) { + return 1; + } + 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); @@ -247,6 +253,11 @@ ble_uuid_to_mbuf(const ble_uuid_t *uuid, struct os_mbuf *om) len = ble_uuid_length(uuid); + /* ble_uuid_flat expands 32-bit UUIDs to full 128-bit base UUID */ + if (uuid->type == BLE_UUID_TYPE_32) { + len = 16; + } + buf = os_mbuf_extend(om, len); if (buf == NULL) { return BLE_HS_ENOMEM; @@ -275,7 +286,7 @@ ble_uuid_flat(const ble_uuid_t *uuid, void *dst) break; case BLE_UUID_TYPE_32: memcpy(dst, ble_uuid_base, 16); - put_le32(dst + 12, BLE_UUID32(uuid)->value); + put_le32((uint8_t *)dst + 12, BLE_UUID32(uuid)->value); break; case BLE_UUID_TYPE_128: memcpy(dst, BLE_UUID128(uuid)->value, 16); @@ -292,6 +303,10 @@ ble_uuid_length(const ble_uuid_t *uuid) { VERIFY_UUID(uuid); + if (uuid->type == BLE_UUID_TYPE_32) { + return 16; + } + return uuid->type >> 3; } diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index 8e7eb5515..8871a4adb 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -26,6 +26,7 @@ #include "store/config/ble_store_config.h" #include "ble_store_config_priv.h" #include "esp_nimble_mem.h" +#include "../src/ble_hs_priv.h" #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) ble_store_config_vars_t * ble_store_config_vars = NULL; @@ -87,7 +88,7 @@ int ble_store_config_compare_bond_count(const void *a, const void *b) { const struct ble_store_value_sec *sec_a = (const struct ble_store_value_sec *)a; const struct ble_store_value_sec *sec_b = (const struct ble_store_value_sec *)b; - return sec_a->bond_count - sec_b->bond_count; + return (sec_a->bond_count > sec_b->bond_count) - (sec_a->bond_count < sec_b->bond_count); } /* This function gets the stored device records of OUR_SEC object type, arranges them in order of their bond count, @@ -95,7 +96,7 @@ int ble_store_config_compare_bond_count(const void *a, const void *b) { */ int ble_restore_our_sec_nvs(void) { - esp_err_t err; + int err; struct ble_store_value_sec temp_our_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; int temp_count = 0; @@ -127,11 +128,15 @@ int ble_restore_our_sec_nvs(void) err = ble_store_config_write(BLE_STORE_OBJ_TYPE_OUR_SEC, &val); if (err != ESP_OK) { - BLE_HS_LOG(DEBUG, "Error writing record to NVS"); + BLE_HS_LOG(DEBUG, "Error writing to nvs"); return err; } } + /* The global array ble_store_config_our_secs and its count are already correctly updated + * by the ble_store_config_write calls in the loop above. Overwriting them here with + * temp_our_secs would revert the bond_count reset. */ + return 0; } @@ -140,7 +145,7 @@ int ble_restore_our_sec_nvs(void) */ int ble_restore_peer_sec_nvs(void) { - esp_err_t err; + int err; struct ble_store_value_sec temp_peer_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; int temp_count = 0; @@ -159,24 +164,27 @@ int ble_restore_peer_sec_nvs(void) err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_PEER_SEC, &key); if (err != ESP_OK) { - BLE_HS_LOG(DEBUG, "Error deleting from nvs"); + BLE_HS_LOG(DEBUG, "Error deleting from nvs %d ",err); return err; } } for (int i = 0; i < temp_count; i++) { - union ble_store_value val; val.sec = temp_peer_secs[i]; err = ble_store_config_write(BLE_STORE_OBJ_TYPE_PEER_SEC, &val); if (err != ESP_OK) { - BLE_HS_LOG(DEBUG, "Error writing record to NVS"); + BLE_HS_LOG(DEBUG, "Error writing to nvs %d ",err); return err; } } + /* The global array ble_store_config_peer_secs and its count are already correctly updated + * by the ble_store_config_write calls in the loop above. Overwriting them here with + * temp_peer_secs would revert the bond_count reset. */ + return 0; } #endif @@ -259,7 +267,6 @@ ble_store_config_find_sec(const struct ble_store_key_sec *key_sec, return i; } - return -1; } #endif @@ -339,7 +346,9 @@ ble_store_config_delete_obj(void *values, int value_size, int idx, { uint8_t *dst; uint8_t *src; - uint8_t move_count; + size_t move_count; + + BLE_HS_DBG_ASSERT(idx >= 0 && idx < *num_values && *num_values > 0); (*num_values)--; if (idx < *num_values) { @@ -715,10 +724,10 @@ ble_store_config_write_ead(const struct ble_store_value_ead *value_ead) // local irk +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) static int ble_store_config_find_local_irk(const struct ble_store_key_local_irk *key) { -#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) struct ble_store_value_local_irk *local_irk; int skipped; int i; @@ -740,13 +749,14 @@ ble_store_config_find_local_irk(const struct ble_store_key_local_irk *key) return i; } -#endif return -1; } +#endif static int ble_store_config_delete_local_irk(const struct ble_store_key_local_irk *key_irk) { +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) int idx; int rc; @@ -767,14 +777,17 @@ ble_store_config_delete_local_irk(const struct ble_store_key_local_irk *key_irk) if (rc != 0) { return rc; } - return 0; +#else + return BLE_HS_ENOTSUP; +#endif } static int ble_store_config_read_local_irk(const struct ble_store_key_local_irk *key_irk, struct ble_store_value_local_irk *value_irk) { +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) int idx; idx = ble_store_config_find_local_irk(key_irk); @@ -784,6 +797,9 @@ ble_store_config_read_local_irk(const struct ble_store_key_local_irk *key_irk, *value_irk = ble_store_config_local_irks[idx]; return 0; +#else + return BLE_HS_ENOENT; +#endif } static int @@ -791,6 +807,8 @@ ble_store_config_write_local_irk(const struct ble_store_value_local_irk *value_i { #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) struct ble_store_key_local_irk key_irk; + struct ble_store_value_local_irk old_val; + bool appended = false; int idx; int rc; @@ -799,19 +817,25 @@ ble_store_config_write_local_irk(const struct ble_store_value_local_irk *value_i if (idx == -1) { if (ble_store_config_num_local_irks >= 1) { - BLE_HS_LOG(DEBUG, "error persisting ead; too many entries (%d)\n", + BLE_HS_LOG(DEBUG, "error persisting local_irk; too many entries (%d)\n", ble_store_config_num_local_irks); return BLE_HS_ESTORE_CAP; } idx = ble_store_config_num_local_irks; ble_store_config_num_local_irks++; + appended = true; } + old_val = ble_store_config_local_irks[idx]; ble_store_config_local_irks[idx] = *value_irk; rc = ble_store_config_persist_local_irk(); if (rc != 0) { + ble_store_config_local_irks[idx] = old_val; + if (appended) { + ble_store_config_num_local_irks--; + } return rc; } @@ -837,7 +861,8 @@ 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)) { + 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) { @@ -943,20 +968,20 @@ ble_store_config_find_csfc(const struct ble_store_key_csfc *key, const struct ble_store_value_csfc *cur; int i; - if (!ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { - if (key->idx < num_value_csfc) { - return key->idx; - } - } else if (key->idx == 0) { + /* If peer_addr is specified, search by peer_addr (common for write/read/delete) */ + if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { for (i = 0; i < num_value_csfc; i++) { cur = &value_csfc[i]; - if (!ble_addr_cmp(&cur->peer_addr, &key->peer_addr)) { return i; } } + } else { + /* For ANY peer use idx as direct array index (enumerate scenario) */ + if (key->idx < num_value_csfc) { + return key->idx; + } } - return -1; } #endif @@ -1206,6 +1231,8 @@ ble_store_config_init(void) if (ble_store_config_vars == NULL) { ble_store_config_vars = nimble_platform_mem_calloc(1, sizeof(ble_store_config_vars_t)); if (ble_store_config_vars == NULL) { + MODLOG_DFLT(ERROR, "Failed to allocate memory for ble_store_config_vars\n"); + assert(0); return; } } 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 09dd0e4c7..3b0e7198f 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/nimble/host/store/config/src/ble_store_config_conf.c @@ -97,16 +97,32 @@ static int ble_store_config_deserialize_arr(const char *enc, void *out_arr, int obj_sz, + int max_objs, int *out_num_objs) { - int len; + size_t dec_len; + unsigned char *dec_data = base64_decode(enc, strlen(enc), &dec_len); - len = base64_decode(enc, out_arr); - if (len < 0) { + if (dec_data == NULL) { return OS_EINVAL; } - *out_num_objs = len / obj_sz; + if (dec_len % obj_sz != 0) { + free(dec_data); + return OS_EINVAL; + } + + /* Validate decoded data fits within destination array */ + if ((int)(dec_len / obj_sz) > max_objs) { + free(dec_data); + return OS_EINVAL; + } + + memcpy(out_arr, dec_data, dec_len); + *out_num_objs = dec_len / obj_sz; + + free(dec_data); + return 0; } @@ -115,12 +131,17 @@ ble_store_config_conf_set(int argc, char **argv, char *val) { int rc; + if (val == NULL) { + return OS_EINVAL; + } + if (argc == 1) { if (strcmp(argv[0], "our_sec") == 0) { rc = ble_store_config_deserialize_arr( val, ble_store_config_our_secs, sizeof *ble_store_config_our_secs, + MYNEWT_VAL(BLE_STORE_MAX_BONDS), &ble_store_config_num_our_secs); return rc; } else if (strcmp(argv[0], "peer_sec") == 0) { @@ -128,6 +149,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) val, ble_store_config_peer_secs, sizeof *ble_store_config_peer_secs, + MYNEWT_VAL(BLE_STORE_MAX_BONDS), &ble_store_config_num_peer_secs); return rc; } else if (strcmp(argv[0], "cccd") == 0) { @@ -135,6 +157,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) val, ble_store_config_cccds, sizeof *ble_store_config_cccds, + MYNEWT_VAL(BLE_STORE_MAX_CCCDS), &ble_store_config_num_cccds); return rc; } @@ -144,6 +167,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) val, ble_store_config_csfcs, sizeof *ble_store_config_csfcs, + MYNEWT_VAL(BLE_STORE_MAX_CSFCS), &ble_store_config_num_csfcs); return rc; } @@ -154,6 +178,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) val, ble_store_config_eads, sizeof *ble_store_config_eads, + MYNEWT_VAL(BLE_STORE_MAX_EADS), &ble_store_config_num_eads); return rc; } @@ -164,6 +189,7 @@ ble_store_config_conf_set(int argc, char **argv, char *val) val, ble_store_config_rpa_recs, sizeof *ble_store_config_rpa_recs, + MYNEWT_VAL(BLE_STORE_MAX_BONDS), &ble_store_config_num_rpa_recs); return rc; } @@ -230,6 +256,7 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), ble_store_config_num_rpa_recs, buf.rpa_rec, sizeof buf.rpa_rec); + func("ble_hs/rpa_rec", buf.rpa_rec); #endif return 0; } @@ -325,7 +352,7 @@ ble_store_config_persist_csfcs(void) int ble_store_config_persist_eads(void) { - char buf[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ]; + char buf[BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ]; int rc; ble_store_config_serialize_arr(ble_store_config_eads, sizeof *ble_store_config_eads, diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index 3f7512bea..31c63a4c5 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -104,12 +104,34 @@ get_nvs_matching_index(void *nvs_val, void *db_list, int db_num, size_t return -1; } +#if MYNEWT_VAL(ENC_ADV_DATA) +/* EAD entries contain a pointer (km), so raw memcmp() is not valid. + * Match by stable key (peer_addr) to determine if an NVS entry exists in RAM. + */ +static int +get_nvs_matching_ead_index(const struct ble_store_value_ead *nvs_val, + void *db_list, int db_num) +{ + struct ble_store_value_ead *db_item = (struct ble_store_value_ead *)db_list; + int i; + + for (i = 0; i < db_num; i++) { + if (ble_addr_cmp(&nvs_val->peer_addr, &db_item->peer_addr) == 0) { + return i; + } + db_item++; + } + + return -1; +} +#endif + static int get_nvs_max_obj_value(int obj_type) { /* If host based privacy is enabled */ if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { - return (MYNEWT_VAL(BLE_STORE_MAX_BONDS) + 1); + return MYNEWT_VAL(BLE_STORE_MAX_BONDS); } else { if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { return MYNEWT_VAL(BLE_STORE_MAX_CCCDS); @@ -231,10 +253,10 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) &required_size); #endif } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { - err = nvs_get_blob (nimble_handle, key_string, &val->local_irk, - &required_size); + err = nvs_get_blob(nimble_handle, key_string, &val->local_irk, + &required_size); - } else if(obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ + } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR) { err = nvs_get_blob(nimble_handle, key_string, &val->rpa_rec, &required_size); @@ -294,23 +316,27 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) if (value) { #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { - err = get_nvs_matching_index(&p_dev_rec.peer_sec, - &((struct ble_hs_dev_records *)value)->peer_sec, - num_value, - sizeof(struct ble_hs_peer_sec)); + struct ble_hs_dev_records *recs = value; + err = -1; + for (int j = 0; j < num_value; j++) { + if (memcmp(&p_dev_rec.peer_sec, &recs[j].peer_sec, sizeof(struct ble_hs_peer_sec)) == 0) { + err = j; + break; + } + } } else #endif { if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { - err = get_nvs_matching_index(&cur.sec, value, num_value, + err = get_nvs_matching_index(&cur.cccd, value, num_value, sizeof(struct ble_store_value_cccd)); } else if (obj_type == BLE_STORE_OBJ_TYPE_CSFC) { err = get_nvs_matching_index(&cur.csfc, value, num_value, sizeof(struct ble_store_value_csfc)); #if MYNEWT_VAL(ENC_ADV_DATA) } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { - err = get_nvs_matching_index(&cur.sec, value, num_value, - sizeof(struct ble_store_value_ead)); + err = get_nvs_matching_ead_index(&cur.ead, value, + num_value); #endif } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { err = get_nvs_matching_index(&cur.local_irk, value, num_value, @@ -320,7 +346,7 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) err = get_nvs_matching_index(&cur.rpa_rec,value,num_value, sizeof(struct ble_store_value_rpa_rec)); } else { - err = get_nvs_matching_index(&cur.cccd, value, num_value, + err = get_nvs_matching_index(&cur.sec, value, num_value, sizeof(struct ble_store_value_sec)); } } @@ -337,6 +363,10 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) } if (empty == 0) { + if (value != NULL) { + /* No non-matching entry found */ + return -1; + } return count; } else { return (max_limit + 1); @@ -354,7 +384,7 @@ ble_nvs_delete_value(int obj_type, int index) nvs_handle_t nimble_handle; char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; - if (index > get_nvs_max_obj_value(obj_type)) { + if (index <= 0 || index > get_nvs_max_obj_value(obj_type)) { ESP_LOGE(TAG, "Invalid index provided to delete"); return BLE_HS_EUNKNOWN; } @@ -425,7 +455,7 @@ static int ble_store_nvs_write(int obj_type, const union ble_store_value *val) { char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; - int8_t write_key_index = 0; + int write_key_index = 0; write_key_index = get_nvs_db_attribute(obj_type, 1, NULL, 0); if (write_key_index == -1) { @@ -472,7 +502,7 @@ static int ble_store_nvs_peer_records(int obj_type, const struct ble_hs_dev_records *p_dev_rec) { char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; - int8_t write_key_index = 0; + int write_key_index = 0; write_key_index = get_nvs_db_attribute(obj_type, 1, NULL, 0); if (write_key_index == -1) { @@ -623,7 +653,7 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num) static int ble_nvs_restore_sec_keys(void) { - esp_err_t err; + int err; int flag = 0; extern int ble_store_config_compare_bond_count(const void *a, const void *b); @@ -865,7 +895,7 @@ int ble_store_config_persist_local_irk(void) nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_LOCAL_IRK, 0, NULL, 0); if (nvs_count == -1) { - ESP_LOGE(TAG, "NVS operation failed while persisting EAD"); + ESP_LOGE(TAG, "NVS operation failed while persisting Local IRK"); return BLE_HS_ESTORE_FAIL; } diff --git a/nimble/host/store/ram/src/ble_store_ram.c b/nimble/host/store/ram/src/ble_store_ram.c index 0f59d8621..e16dc04f6 100644 --- a/nimble/host/store/ram/src/ble_store_ram.c +++ b/nimble/host/store/ram/src/ble_store_ram.c @@ -19,7 +19,7 @@ /** * This file implements a simple in-RAM key database for BLE host security - * material and CCCDs. As this database is only ble_store_ramd in RAM, its + * material and CCCDs. As this database is only stored in RAM, its * contents are lost when the application terminates. */ @@ -211,14 +211,15 @@ ble_store_ram_write_our_sec(const struct ble_store_value_sec *value_sec) } -#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) || MYNEWT_VAL(BLE_STORE_MAX_CCCDS) +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) || MYNEWT_VAL(BLE_STORE_MAX_CCCDS) || \ + MYNEWT_VAL(BLE_STORE_MAX_CSFCS) || MYNEWT_VAL(ENC_ADV_DATA) static int ble_store_ram_delete_obj(void *values, int value_size, int idx, int *num_values) { uint8_t *dst; uint8_t *src; - uint8_t move_count; + int move_count; (*num_values)--; if (idx < *num_values) { @@ -227,7 +228,7 @@ ble_store_ram_delete_obj(void *values, int value_size, int idx, src = dst + value_size; move_count = *num_values - idx; - memmove(dst, src, move_count); + memmove(dst, src, (size_t)move_count * value_size); } return 0; diff --git a/nimble/host/util/src/addr.c b/nimble/host/util/src/addr.c index 94d4433df..9d486a023 100644 --- a/nimble/host/util/src/addr.c +++ b/nimble/host/util/src/addr.c @@ -44,9 +44,14 @@ ble_hs_util_load_rand_addr(ble_addr_t *addr) #endif #if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED - rc = esp_ble_hw_get_static_addr((esp_ble_addr_t *)addr); - if (rc == 0) { - return 0; + { + esp_ble_addr_t esp_addr; + rc = esp_ble_hw_get_static_addr(&esp_addr); + if (rc == 0) { + addr->type = BLE_ADDR_RANDOM; + memcpy(addr->val, esp_addr.val, sizeof(addr->val)); + return 0; + } } #endif return rc; @@ -55,7 +60,7 @@ ble_hs_util_load_rand_addr(ble_addr_t *addr) static int ble_hs_util_ensure_rand_addr(void) { - ble_addr_t addr; + ble_addr_t addr = {0}; int rc; /* If we already have a random address, then we are done. */ diff --git a/nimble/transport/src/transport.c b/nimble/transport/src/transport.c index fbe21c26e..ba52e94b7 100644 --- a/nimble/transport/src/transport.c +++ b/nimble/transport/src/transport.c @@ -40,7 +40,12 @@ void os_msys_buf_free(void); #else #define POOL_CMD_COUNT (1) #endif + +#if !(SOC_ESP_NIMBLE_CONTROLLER) && CONFIG_BT_CONTROLLER_ENABLED +#define POOL_CMD_SIZE (259) +#else #define POOL_CMD_SIZE (258) +#endif #define POOL_EVT_COUNT (MYNEWT_VAL(BLE_TRANSPORT_EVT_COUNT)) #define POOL_EVT_LO_COUNT (MYNEWT_VAL(BLE_TRANSPORT_EVT_DISCARDABLE_COUNT))