diff --git a/nimble/host/services/bas/src/ble_svc_bas.c b/nimble/host/services/bas/src/ble_svc_bas.c index cd50a76c0..d49c1d4c6 100644 --- a/nimble/host/services/bas/src/ble_svc_bas.c +++ b/nimble/host/services/bas/src/ble_svc_bas.c @@ -18,6 +18,7 @@ */ #include +#include #include #include "sysinit/sysinit.h" @@ -35,6 +36,7 @@ static uint16_t ble_svc_bas_battery_handle; /* Battery level */ static uint8_t ble_svc_bas_battery_level; +static bool ble_svc_bas_initialized; /* Access function */ static int @@ -135,6 +137,7 @@ void ble_svc_bas_deinit(void) { ble_gatts_free_svcs(); + ble_svc_bas_initialized = false; } /** @@ -145,17 +148,25 @@ ble_svc_bas_init(void) { int rc; - /* Ensure this function only gets called by sysinit. */ - SYSINIT_ASSERT_ACTIVE(); + if (ble_svc_bas_initialized) { + return; + } ble_svc_bas_battery_level = 0; rc = ble_gatts_count_cfg(ble_svc_bas_defs); - SYSINIT_PANIC_ASSERT(rc == 0); + if (rc != 0) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + return; + } rc = ble_gatts_add_svcs(ble_svc_bas_defs); - SYSINIT_PANIC_ASSERT(rc == 0); + if (rc != 0) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, rc); + return; + } ble_svc_bas_battery_level = 0; + ble_svc_bas_initialized = true; } #endif diff --git a/nimble/host/services/hid/src/ble_svc_hid.c b/nimble/host/services/hid/src/ble_svc_hid.c index 1eb422f98..45699182e 100644 --- a/nimble/host/services/hid/src/ble_svc_hid.c +++ b/nimble/host/services/hid/src/ble_svc_hid.c @@ -18,6 +18,7 @@ */ #include +#include #include #include "sysinit/sysinit.h" @@ -135,23 +136,28 @@ ble_svc_hid_get_chr_block(void) } /*returns current chr index */ -static uint8_t +static uint16_t ble_svc_hid_get_curr_chr_idx(void) { return ble_svc_hid_chr_index; } /*returns current svc index */ -static uint8_t +static uint16_t get_curr_svc_idx(void) { +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_hid_static_vars == NULL) { + return 0; + } +#endif return ble_svc_hid_svc_index; } struct report * find_rpt_by_handle(uint16_t handle) { - uint8_t instance, instances; + uint16_t instance, instances; int i; instances = get_curr_svc_idx(); @@ -194,7 +200,7 @@ fill_proto_mode(uint8_t instance) .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE_NO_RSP | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_WRITE_AUTHEN | BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC | #endif @@ -224,7 +230,7 @@ fill_boot_kbd_inp(uint8_t instance) write_flags = BLE_GATT_CHR_F_WRITE | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_WRITE_ENC | BLE_GATT_CHR_F_WRITE_AUTHEN | #endif 0; @@ -237,9 +243,10 @@ fill_boot_kbd_inp(uint8_t instance) .val_handle = &hid_instances[instance].kbd_inp_handle, .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY | #if MYNEWT_VAL(BLE_SM_LVL) == 2 - BLE_GATT_CHR_F_READ_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 - BLE_GATT_CHR_F_READ_AUTHEN | + BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_NOTIFY_INDICATE_ENC | +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 + BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_NOTIFY_INDICATE_AUTHEN | + BLE_GATT_CHR_F_NOTIFY_INDICATE_ENC | BLE_GATT_CHR_F_READ_ENC | #endif write_flags, @@ -272,7 +279,7 @@ fill_boot_kbd_out(uint8_t instance) BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_WRITE | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_WRITE_AUTHEN | BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC | #endif @@ -301,7 +308,7 @@ fill_boot_mouse_inp(uint8_t instance) write_flags = BLE_GATT_CHR_F_WRITE | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_WRITE_ENC | BLE_GATT_CHR_F_WRITE_AUTHEN | #endif 0; @@ -315,7 +322,7 @@ fill_boot_mouse_inp(uint8_t instance) .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_READ_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_ENC | #endif @@ -353,7 +360,7 @@ fill_rpt_map(uint8_t instance) .flags = BLE_GATT_CHR_F_READ | #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_READ_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_ENC | #endif @@ -429,7 +436,7 @@ fill_reports(uint8_t instance) demo_chr.flags |= ( #if MYNEWT_VAL(BLE_SM_LVL) == 2 BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC | -#elif MYNEWT_VAL(BLE_SM_LVL) == 3 +#elif MYNEWT_VAL(BLE_SM_LVL) >= 3 BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_WRITE_AUTHEN | BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC | #endif @@ -448,7 +455,7 @@ fill_reports(uint8_t instance) return 0; } -static void +static int fill_hid_info(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; @@ -469,12 +476,14 @@ fill_hid_info(uint8_t instance) }; chr = ble_svc_hid_get_chr_block(); if (!chr) { - return; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); + return 0; } -static void +static int fill_ctrl_pt(uint8_t instance) { struct ble_gatt_chr_def *chr, demo_chr; @@ -495,9 +504,11 @@ fill_ctrl_pt(uint8_t instance) }; chr = ble_svc_hid_get_chr_block(); if (!chr) { - return; + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + return BLE_HS_ENOMEM; } memcpy(chr, &demo_chr, sizeof(struct ble_gatt_chr_def)); + return 0; } /** @@ -534,11 +545,11 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, struct report *rpt; uint16_t rpt_ref_val = 0; uint16_t out_rpt_len = 0; - uint8_t instances = get_curr_svc_idx(); + uint16_t instances = get_curr_svc_idx(); uint16_t handle; uint8_t val; - for (int instance = 0; instance < instances; instance++) { + for (uint16_t instance = 0; instance < instances; instance++) { switch (uuid16) { case BLE_SVC_HID_CHR_UUID16_REPORT_MAP: if (hid_instances[instance].report_map_handle != attr_handle) { @@ -585,13 +596,14 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, } assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR); /* check if the value is correct */ - rc = ble_hs_mbuf_to_flat(ctxt->om, &val, sizeof(uint8_t), NULL); - if(rc != 0) { - return BLE_ATT_ERR_INSUFFICIENT_RES; + rc = ble_svc_hid_chr_write(ctxt->om, sizeof(val), sizeof(val), + &val, NULL); + if (rc != 0) { + return rc; } if(val == 0 || val == 1) { - rc = ble_svc_hid_chr_write(ctxt->om, 0, sizeof hid_instances[instance].ctrl_pt, &hid_instances[instance].ctrl_pt, NULL); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + hid_instances[instance].ctrl_pt = val; + return 0; } return BLE_ATT_ERR_UNLIKELY; @@ -606,7 +618,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, 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, sizeof(hid_instances[instance].kbd_out_rpt), &hid_instances[instance].kbd_out_rpt, NULL); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } return 0; case BLE_SVC_HID_CHR_UUID16_BOOT_KBD_INP: @@ -620,10 +632,13 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, 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, sizeof(hid_instances[instance].kbd_inp_rpt), hid_instances[instance].kbd_inp_rpt, NULL); + if (rc != 0) { + return rc; + } if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) { ble_gatts_chr_updated(*(ctxt->chr->val_handle)); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return 0; } return 0; case BLE_SVC_HID_CHR_UUID16_BOOT_MOUSE_INP: @@ -638,7 +653,7 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { rc = ble_svc_hid_chr_write(ctxt->om, 0, sizeof(hid_instances[instance].mouse_inp_rpt), hid_instances[instance].mouse_inp_rpt, &out_rpt_len); if (rc != 0) { - return BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } hid_instances[instance].mouse_inp_rpt_len = out_rpt_len; if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) { @@ -658,13 +673,14 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { /* check if the value is correct */ - rc = ble_hs_mbuf_to_flat(ctxt->om, &val, sizeof(uint8_t), NULL); - if(rc != 0) { - return BLE_ATT_ERR_INSUFFICIENT_RES; + rc = ble_svc_hid_chr_write(ctxt->om, sizeof(val), sizeof(val), + &val, NULL); + if (rc != 0) { + return rc; } if(val == 0 || val == 1) { - rc = ble_svc_hid_chr_write(ctxt->om, 0, sizeof(hid_instances[instance].proto_mode), &hid_instances[instance].proto_mode, NULL); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + hid_instances[instance].proto_mode = val; + return 0; } return BLE_ATT_ERR_UNLIKELY; } @@ -681,15 +697,15 @@ ble_svc_hid_access(uint16_t conn_handle, uint16_t attr_handle, 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, UINT8_MAX, rpt->data, &out_rpt_len); if (rc != 0) { - return BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } rpt->len = out_rpt_len; if (ctxt->chr->flags & BLE_GATT_CHR_F_NOTIFY) { ble_gatts_chr_updated(*(ctxt->chr->val_handle)); } - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return 0; } return 0; @@ -708,10 +724,10 @@ ble_svc_hid_add(struct ble_svc_hid_params params) /* Ensure this function only gets called by sysinit. */ SYSINIT_ASSERT_ACTIVE(); - uint8_t svc_idx; + uint16_t svc_idx; int rc = 0; struct ble_gatt_svc_def *svc; - uint8_t chr_idx; + uint16_t chr_idx; #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) if (ble_svc_hid_static_vars == NULL) { @@ -769,9 +785,15 @@ ble_svc_hid_add(struct ble_svc_hid_params params) /* Fill the boot mouse input characteristic */ fill_boot_mouse_inp(svc_idx); /* Fill the hid info characteristic */ - fill_hid_info(svc_idx); + rc = fill_hid_info(svc_idx); + if (rc != 0) { + goto error; + } /* Fill the control point characteristic */ - fill_ctrl_pt(svc_idx); + rc = fill_ctrl_pt(svc_idx); + if (rc != 0) { + goto error; + } /* End the characteristics with the characteristic with empty block */ rc = ble_svc_hid_end_chrs(); if (rc != 0){ @@ -789,9 +811,7 @@ ble_svc_hid_add(struct ble_svc_hid_params params) return 0; error: -#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) ble_svc_hid_reset(); -#endif return rc; } @@ -842,6 +862,7 @@ void ble_svc_hid_deinit(void) { ble_gatts_free_svcs(); + ble_svc_hid_reset(); } /** @@ -855,6 +876,12 @@ ble_svc_hid_init(void) /* Ensure this function only gets called by sysinit. */ SYSINIT_ASSERT_ACTIVE(); +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_svc_hid_static_vars == NULL) { + return; + } +#endif + rc = ble_svc_hid_end(); SYSINIT_PANIC_ASSERT(rc == 0); diff --git a/nimble/host/services/htp/include/services/htp/ble_svc_htp.h b/nimble/host/services/htp/include/services/htp/ble_svc_htp.h index 97c40451f..d8741c325 100644 --- a/nimble/host/services/htp/include/services/htp/ble_svc_htp.h +++ b/nimble/host/services/htp/include/services/htp/ble_svc_htp.h @@ -45,6 +45,9 @@ bool ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr); void ble_svc_htp_subscribe(uint16_t conn_handle, uint16_t attr_handle); +void ble_svc_htp_subscribe_state(uint16_t conn_handle, uint16_t attr_handle, + bool subscribed); + int ble_svc_htp_indicate(uint16_t conn_handle, float temp, bool temp_unit); int ble_svc_htp_notify(uint16_t conn_handle, float temp, bool temp_unit); diff --git a/nimble/host/services/htp/src/ble_svc_htp.c b/nimble/host/services/htp/src/ble_svc_htp.c index 118b35647..be165d29d 100644 --- a/nimble/host/services/htp/src/ble_svc_htp.c +++ b/nimble/host/services/htp/src/ble_svc_htp.c @@ -16,7 +16,7 @@ #if MYNEWT_VAL(BLE_GATTS) && CONFIG_BT_NIMBLE_HTP_SERVICE /* Characteristic values */ -static uint16_t ble_svc_htp_temp_type; +static uint8_t ble_svc_htp_temp_type; static uint16_t ble_svc_htp_temp_msr_itvl; /* Health thermometer characteristic value handles */ @@ -25,7 +25,62 @@ static uint16_t ble_svc_htp_temp_type_val_handle; static uint16_t ble_svc_htp_intr_temp_val_handle; static uint16_t ble_svc_htp_msr_itvl_val_handle; -static struct chr_subscribe conn_chr_subs[MYNEWT_VAL(BLE_MAX_CONNECTIONS) + 1]; +static struct { + uint16_t conn_handle; + struct chr_subscribe chr_subs; +} conn_chr_subs[MYNEWT_VAL(BLE_MAX_CONNECTIONS) + 1]; + +#define BLE_SVC_HTP_TEMP_MSR_MIN_ITVL 0x0001 +#define BLE_SVC_HTP_TEMP_MSR_MAX_ITVL 0xffff +#define BLE_SVC_HTP_CONN_HANDLE_NONE 0xffff + +static int +ble_svc_htp_conn_slot(uint16_t conn_handle) +{ + int free_slot; + int i; + + free_slot = -1; + for (i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { + if (conn_chr_subs[i].conn_handle == conn_handle) { + return i; + } + if (free_slot < 0 && + conn_chr_subs[i].conn_handle == BLE_SVC_HTP_CONN_HANDLE_NONE) { + free_slot = i; + } + } + + if (free_slot >= 0) { + conn_chr_subs[free_slot].conn_handle = conn_handle; + memset(&conn_chr_subs[free_slot].chr_subs, 0, + sizeof(conn_chr_subs[free_slot].chr_subs)); + } + + return free_slot; +} + +static uint32_t +ble_svc_htp_temp_to_ieee11073(float temp) +{ + int32_t mantissa; + uint8_t exponent; + + exponent = (uint8_t)-2; + if (temp >= 0) { + mantissa = (int32_t)(temp * 100.0f + 0.5f); + } else { + mantissa = (int32_t)(temp * 100.0f - 0.5f); + } + + if (mantissa > 0x7fffff) { + mantissa = 0x7fffff; + } else if (mantissa < -0x800000) { + mantissa = -0x800000; + } + + return ((uint32_t)exponent << 24) | ((uint32_t)mantissa & 0x00ffffff); +} static int ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, @@ -98,7 +153,12 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, uint16_t uuid16; int rc; - uuid16 = ble_uuid_u16(ctxt->chr->uuid); + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC || + ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { + uuid16 = ble_uuid_u16(ctxt->dsc->uuid); + } else { + uuid16 = ble_uuid_u16(ctxt->chr->uuid); + } assert(uuid16 != 0); switch (uuid16) { @@ -116,10 +176,11 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, case BLE_SVC_HTP_CHR_UUID16_MEASUREMENT_ITVL: if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = ble_svc_htp_chr_write(ctxt->om, 0, sizeof(ble_svc_htp_temp_msr_itvl), + rc = ble_svc_htp_chr_write(ctxt->om, sizeof(ble_svc_htp_temp_msr_itvl), + sizeof(ble_svc_htp_temp_msr_itvl), &ble_svc_htp_temp_msr_itvl, NULL); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + return rc; } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { rc = os_mbuf_append(ctxt->om, &ble_svc_htp_temp_msr_itvl, sizeof(ble_svc_htp_temp_msr_itvl)); @@ -128,8 +189,23 @@ ble_svc_htp_access(uint16_t conn_handle, uint16_t attr_handle, return BLE_SVC_HS_ERR_OUT_OF_RANGE; } + case BLE_SVC_HTP_DSC_UUID16_VALID_RANGE: { + uint8_t valid_range[4]; + + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { + return BLE_ATT_ERR_WRITE_NOT_PERMITTED; + } + + valid_range[0] = BLE_SVC_HTP_TEMP_MSR_MIN_ITVL & 0xff; + valid_range[1] = BLE_SVC_HTP_TEMP_MSR_MIN_ITVL >> 8; + valid_range[2] = BLE_SVC_HTP_TEMP_MSR_MAX_ITVL & 0xff; + valid_range[3] = BLE_SVC_HTP_TEMP_MSR_MAX_ITVL >> 8; + + rc = os_mbuf_append(ctxt->om, valid_range, sizeof(valid_range)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: - assert(0); return BLE_ATT_ERR_UNLIKELY; } } @@ -137,12 +213,15 @@ 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)) { + int slot = ble_svc_htp_conn_slot(conn_handle); + + if (slot < 0) { 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; + + conn_chr_subs[slot].conn_handle = BLE_SVC_HTP_CONN_HANDLE_NONE; + memset(&conn_chr_subs[slot].chr_subs, 0, + sizeof(conn_chr_subs[slot].chr_subs)); } /** @@ -151,10 +230,12 @@ 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)) { + int slot = ble_svc_htp_conn_slot(conn_handle); + + if (chr < TEMP_MEASUREMENT || chr > MEASUREMENT_ITVL || slot < 0) { return false; } - return conn_chr_subs[conn_handle].chr_subs[chr]; + return conn_chr_subs[slot].chr_subs.chr_subs[chr]; } /** @@ -166,22 +247,32 @@ ble_svc_htp_is_subscribed(uint16_t conn_handle, int chr) * @return 0 on success, non-zero error code otherwise. */ void -ble_svc_htp_subscribe(uint16_t conn_handle, uint16_t attr_handle) +ble_svc_htp_subscribe_state(uint16_t conn_handle, uint16_t attr_handle, + bool subscribed) { - if (conn_handle > MYNEWT_VAL(BLE_MAX_CONNECTIONS)) { + int slot = ble_svc_htp_conn_slot(conn_handle); + + if (slot < 0) { return; } + if (attr_handle == ble_svc_htp_temp_measurement_val_handle) { - conn_chr_subs[conn_handle].chr_subs[TEMP_MEASUREMENT] = true; + conn_chr_subs[slot].chr_subs.chr_subs[TEMP_MEASUREMENT] = subscribed; } else if (attr_handle == ble_svc_htp_intr_temp_val_handle) { - conn_chr_subs[conn_handle].chr_subs[INTERMEDIATE_TEMP] = true; + conn_chr_subs[slot].chr_subs.chr_subs[INTERMEDIATE_TEMP] = subscribed; } else if (attr_handle == ble_svc_htp_msr_itvl_val_handle) { - conn_chr_subs[conn_handle].chr_subs[MEASUREMENT_ITVL] = true; + conn_chr_subs[slot].chr_subs.chr_subs[MEASUREMENT_ITVL] = subscribed; } } +void +ble_svc_htp_subscribe(uint16_t conn_handle, uint16_t attr_handle) +{ + ble_svc_htp_subscribe_state(conn_handle, attr_handle, true); +} + /** * Send a notification for intermediate temperature * @@ -194,31 +285,27 @@ ble_svc_htp_notify(uint16_t conn_handle, float temp, bool temp_unit) struct os_mbuf *txom = NULL; /* 0th byte is flag, next 4 bytes is the temperature */ - uint8_t flags = {0x00}; + uint8_t measurement[5]; + uint32_t temp_ieee11073; + measurement[0] = 0x00; if (temp_unit) { - flags |= 1 << 0; /* Setting 0 th bit of flags to 1 if temp unit is Fahrenheit */ + measurement[0] |= 1 << 0; /* Temperature unit is Fahrenheit. */ } - txom = ble_hs_mbuf_from_flat(&flags, sizeof(flags)); + temp_ieee11073 = ble_svc_htp_temp_to_ieee11073(temp); + measurement[1] = temp_ieee11073 & 0xff; + measurement[2] = (temp_ieee11073 >> 8) & 0xff; + measurement[3] = (temp_ieee11073 >> 16) & 0xff; + measurement[4] = (temp_ieee11073 >> 24) & 0xff; + + txom = ble_hs_mbuf_from_flat(measurement, sizeof(measurement)); if (!txom) { return ESP_FAIL; } - rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp)); - if (rc != 0) { - os_mbuf_free_chain(txom); - goto err; - } - rc = ble_gatts_notify_custom(conn_handle, ble_svc_htp_intr_temp_val_handle, txom); - if (rc != 0) { - goto err; - } - - ble_gatts_chr_updated(ble_svc_htp_intr_temp_val_handle); -err: return rc; } @@ -235,23 +322,25 @@ ble_svc_htp_indicate(uint16_t conn_handle, float temp, bool temp_unit) /* 0th byte is flag, next 4 bytes is the temperature */ - uint8_t flags = {0x00}; + uint8_t measurement[5]; + uint32_t temp_ieee11073; + measurement[0] = 0x00; if (temp_unit) { - flags |= 1 << 0; /* Setting 0 th bit of flags to 1 if temp unit is Fahrenheit */ + measurement[0] |= 1 << 0; /* Temperature unit is Fahrenheit. */ } - txom = ble_hs_mbuf_from_flat(&flags, sizeof(flags)); + temp_ieee11073 = ble_svc_htp_temp_to_ieee11073(temp); + measurement[1] = temp_ieee11073 & 0xff; + measurement[2] = (temp_ieee11073 >> 8) & 0xff; + measurement[3] = (temp_ieee11073 >> 16) & 0xff; + measurement[4] = (temp_ieee11073 >> 24) & 0xff; + + txom = ble_hs_mbuf_from_flat(measurement, sizeof(measurement)); if (!txom) { return ESP_FAIL; } - rc = os_mbuf_copyinto(txom, sizeof(flags), &temp, sizeof(temp)); - if (rc != 0) { - os_mbuf_free_chain(txom); - return rc; - } - rc = ble_gatts_indicate_custom(conn_handle, ble_svc_htp_temp_measurement_val_handle, txom); return rc; @@ -300,6 +389,9 @@ ble_svc_htp_init(void) ble_svc_htp_temp_type = 2; ble_svc_htp_temp_msr_itvl = 2; /* 2 sec */ - memset(&conn_chr_subs, 0, sizeof(conn_chr_subs)); + for (int i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { + conn_chr_subs[i].conn_handle = BLE_SVC_HTP_CONN_HANDLE_NONE; + memset(&conn_chr_subs[i].chr_subs, 0, sizeof(conn_chr_subs[i].chr_subs)); + } } #endif diff --git a/nimble/host/services/prox/include/services/prox/ble_svc_prox.h b/nimble/host/services/prox/include/services/prox/ble_svc_prox.h index 8b1e4c62a..f60db29fb 100644 --- a/nimble/host/services/prox/include/services/prox/ble_svc_prox.h +++ b/nimble/host/services/prox/include/services/prox/ble_svc_prox.h @@ -7,6 +7,8 @@ #ifndef H_BLE_SVC_PROX_ #define H_BLE_SVC_PROX_ +#include + #ifdef __cplusplus extern "C" { #endif @@ -33,6 +35,10 @@ struct ble_hs_cfg; */ void ble_svc_prox_init(void); +void ble_svc_prox_on_disconnect(uint16_t conn_handle); + +void ble_svc_prox_set_tx_power_level(int8_t tx_pwr_lvl); + #ifdef __cplusplus } #endif diff --git a/nimble/host/services/prox/src/ble_svc_prox.c b/nimble/host/services/prox/src/ble_svc_prox.c index 6a7bb0bf6..1a8a116a9 100644 --- a/nimble/host/services/prox/src/ble_svc_prox.c +++ b/nimble/host/services/prox/src/ble_svc_prox.c @@ -16,19 +16,24 @@ #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_tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO; +static uint8_t ble_svc_prox_alert; +static int8_t ble_svc_prox_tx_pwr_lvl; -#define BLE_SVC_PROX_HIGH_THRESHOLD (-70) -#define BLE_SVC_PROX_LOW_THRESHOLD (-100) +#define BLE_SVC_PROX_ALERT_NONE 0 +#define BLE_SVC_PROX_ALERT_MILD 1 +#define BLE_SVC_PROX_ALERT_HIGH 2 +#define BLE_SVC_PROX_CONN_HANDLE_NONE 0xffff /* Characteristic value handles */ static uint16_t ble_svc_prox_link_loss_val_handle; static uint16_t ble_svc_prox_immediate_alert_loc_val_handle; static uint16_t ble_svc_prox_tx_pwr_lvl_val_handle; -static bool ble_svc_prox_alert_conn[MYNEWT_VAL(BLE_MAX_CONNECTIONS) + 1]; +static struct { + uint16_t conn_handle; + uint8_t link_loss_alert; + bool immediate_alert; +} ble_svc_prox_conn[MYNEWT_VAL(BLE_MAX_CONNECTIONS) + 1]; static int ble_svc_prox_link_loss_access(uint16_t conn_handle, uint16_t attr_handle, @@ -48,6 +53,32 @@ ble_svc_prox_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len, void *dst, uint16_t *len); +static int +ble_svc_prox_conn_slot(uint16_t conn_handle) +{ + int free_slot; + int i; + + free_slot = -1; + for (i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { + if (ble_svc_prox_conn[i].conn_handle == conn_handle) { + return i; + } + if (free_slot < 0 && + ble_svc_prox_conn[i].conn_handle == BLE_SVC_PROX_CONN_HANDLE_NONE) { + free_slot = i; + } + } + + if (free_slot >= 0) { + ble_svc_prox_conn[free_slot].conn_handle = conn_handle; + ble_svc_prox_conn[free_slot].link_loss_alert = BLE_SVC_PROX_ALERT_NONE; + ble_svc_prox_conn[free_slot].immediate_alert = false; + } + + return free_slot; +} + static const struct ble_gatt_svc_def ble_svc_prox_defs[] = { { /*** Link Loss Service. */ @@ -118,8 +149,9 @@ ble_prox_prph_task(void *pvParameters) { while (1) { for (int i = 0; i <= MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) { - if (ble_svc_prox_alert_conn[i]) { - MODLOG_DFLT(INFO, "Path loss increased for device connected with conn_handle %d", i); + if (ble_svc_prox_conn[i].immediate_alert) { + MODLOG_DFLT(INFO, "Immediate alert active for device connected with conn_handle %d", + ble_svc_prox_conn[i].conn_handle); } } vTaskDelay(1000 / portTICK_PERIOD_MS); @@ -129,20 +161,25 @@ 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); + int slot; + + slot = ble_svc_prox_conn_slot(conn_handle); + if (slot < 0) { + MODLOG_DFLT(ERROR, "No proximity state slot for conn_handle %d", conn_handle); return; } - if (ble_svc_prox_alert > BLE_SVC_PROX_HIGH_THRESHOLD && - !ble_svc_prox_alert_conn[conn_handle]) { - MODLOG_DFLT(INFO, "Path loss exceeded threshold, starting alert for device with " - "conn_handle %d", conn_handle); - ble_svc_prox_alert_conn[conn_handle] = true; - } else if (ble_svc_prox_alert < BLE_SVC_PROX_LOW_THRESHOLD && - ble_svc_prox_alert_conn[conn_handle]) { - MODLOG_DFLT(INFO, "Path loss lower than threshold, stopping alert for device with " - "conn_handle %d", conn_handle); - ble_svc_prox_alert_conn[conn_handle] = false; + + if (ble_svc_prox_alert == BLE_SVC_PROX_ALERT_NONE) { + if (ble_svc_prox_conn[slot].immediate_alert) { + MODLOG_DFLT(INFO, "Stopping alert for device with conn_handle %d", conn_handle); + } + ble_svc_prox_conn[slot].immediate_alert = false; + } else { + if (!ble_svc_prox_conn[slot].immediate_alert) { + MODLOG_DFLT(INFO, "Starting alert level %d for device with conn_handle %d", + ble_svc_prox_alert, conn_handle); + } + ble_svc_prox_conn[slot].immediate_alert = true; } } @@ -163,12 +200,25 @@ 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, 1, sizeof(ble_svc_prox_link_loss_alert), - &ble_svc_prox_link_loss_alert, NULL); + int slot = ble_svc_prox_conn_slot(conn_handle); + if (slot < 0) { + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + rc = ble_svc_prox_chr_write(ctxt->om, 1, + sizeof(ble_svc_prox_conn[slot].link_loss_alert), + &ble_svc_prox_conn[slot].link_loss_alert, NULL); + if (rc == 0 && + ble_svc_prox_conn[slot].link_loss_alert > BLE_SVC_PROX_ALERT_HIGH) { + return BLE_ATT_ERR_UNLIKELY; + } 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)); + int slot = ble_svc_prox_conn_slot(conn_handle); + if (slot < 0) { + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + rc = os_mbuf_append(ctxt->om, &ble_svc_prox_conn[slot].link_loss_alert, + sizeof(ble_svc_prox_conn[slot].link_loss_alert)); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } return BLE_ATT_ERR_UNLIKELY; @@ -194,9 +244,12 @@ ble_svc_prox_imm_alert_access(uint16_t conn_handle, uint16_t attr_handle, if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { int rc = ble_svc_prox_chr_write(ctxt->om, 1, 1, &ble_svc_prox_alert, NULL); if (rc != 0) { - return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + return rc; } - MODLOG_DFLT(INFO, "Path loss = %d", ble_svc_prox_alert); + if (ble_svc_prox_alert > BLE_SVC_PROX_ALERT_HIGH) { + return BLE_ATT_ERR_UNLIKELY; + } + MODLOG_DFLT(INFO, "Alert level = %d", ble_svc_prox_alert); ble_prox_prph_alert_unalert(conn_handle); return 0; @@ -217,7 +270,11 @@ ble_svc_prox_tx_pwr_access(uint16_t conn_handle, uint16_t attr_handle, uint16_t uuid16; int rc; - uuid16 = ble_uuid_u16(ctxt->chr->uuid); + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + uuid16 = ble_uuid_u16(ctxt->dsc->uuid); + } else { + uuid16 = ble_uuid_u16(ctxt->chr->uuid); + } assert(uuid16 != 0); switch (uuid16) { @@ -230,11 +287,18 @@ ble_svc_prox_tx_pwr_access(uint16_t conn_handle, uint16_t attr_handle, return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; case 0x2904: /* Presentation Format Descriptor UUID */ + { + static const uint8_t tx_pwr_presentation_format[7] = { + 0x0c, 0x00, 0x28, 0x27, 0x01, 0x00, 0x00 + }; + if (ctxt->op != BLE_GATT_ACCESS_OP_READ_DSC) { return BLE_ATT_ERR_UNLIKELY; } - /* Return empty for now */ - return 0; + rc = os_mbuf_append(ctxt->om, tx_pwr_presentation_format, + sizeof(tx_pwr_presentation_format)); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } default: assert(0); @@ -267,6 +331,32 @@ ble_svc_prox_chr_write(struct os_mbuf *om, uint16_t min_len, return 0; } +void +ble_svc_prox_on_disconnect(uint16_t conn_handle) +{ + int slot; + + slot = ble_svc_prox_conn_slot(conn_handle); + if (slot < 0) { + return; + } + + if (ble_svc_prox_conn[slot].link_loss_alert != BLE_SVC_PROX_ALERT_NONE) { + MODLOG_DFLT(INFO, "Link loss alert level %d for device with conn_handle %d", + ble_svc_prox_conn[slot].link_loss_alert, conn_handle); + } + + ble_svc_prox_conn[slot].conn_handle = BLE_SVC_PROX_CONN_HANDLE_NONE; + ble_svc_prox_conn[slot].link_loss_alert = BLE_SVC_PROX_ALERT_NONE; + ble_svc_prox_conn[slot].immediate_alert = false; +} + +void +ble_svc_prox_set_tx_power_level(int8_t tx_pwr_lvl) +{ + ble_svc_prox_tx_pwr_lvl = tx_pwr_lvl; +} + void ble_svc_prox_init(void) { @@ -281,13 +371,18 @@ ble_svc_prox_init(void) rc = ble_gatts_add_svcs(ble_svc_prox_defs); SYSINIT_PANIC_ASSERT(rc == 0); - 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++) { - ble_svc_prox_alert_conn[i] = false; + ble_svc_prox_conn[i].conn_handle = BLE_SVC_PROX_CONN_HANDLE_NONE; + ble_svc_prox_conn[i].link_loss_alert = BLE_SVC_PROX_ALERT_NONE; + ble_svc_prox_conn[i].immediate_alert = false; + } + + static TaskHandle_t ble_prox_task_handle; + if (ble_prox_task_handle == NULL) { + BaseType_t ret = xTaskCreate(ble_prox_prph_task, "ble_prox_prph_task", + 4096, NULL, 10, &ble_prox_task_handle); + SYSINIT_PANIC_ASSERT(ret == pdPASS); } } #endif diff --git a/nimble/host/src/ble_att_clt.c b/nimble/host/src/ble_att_clt.c index 14495d067..0fe421457 100644 --- a/nimble/host/src/ble_att_clt.c +++ b/nimble/host/src/ble_att_clt.c @@ -95,6 +95,12 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, uint16_t mtu) req = ble_att_cmd_get(BLE_ATT_OP_MTU_REQ, sizeof(*req), &txom); if (req == NULL) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); + 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(); return BLE_HS_ENOMEM; } @@ -259,6 +265,11 @@ ble_att_clt_rx_find_info(uint16_t conn_handle, uint16_t cid, struct os_mbuf **om /* Strip the response base from the front of the mbuf. */ os_mbuf_adj((*om), sizeof(*rsp)); + if (OS_MBUF_PKTLEN(*om) == 0) { + rc = BLE_HS_EBADDATA; + goto done; + } + while (OS_MBUF_PKTLEN(*om) > 0) { rc = ble_att_clt_parse_find_info_entry(om, format, &idata); if (rc != 0) { @@ -357,6 +368,11 @@ ble_att_clt_rx_find_type_value(uint16_t conn_handle, uint16_t cid, struct os_mbu struct ble_att_find_type_value_hinfo hinfo; int rc; + if (OS_MBUF_PKTLEN(*rxom) == 0) { + rc = BLE_HS_EBADDATA; + goto done; + } + /* Parse the Handles-Information-List field, passing each entry to GATT. */ rc = 0; while (OS_MBUF_PKTLEN(*rxom) > 0) { @@ -368,6 +384,7 @@ ble_att_clt_rx_find_type_value(uint16_t conn_handle, uint16_t cid, struct os_mbu ble_gattc_rx_find_type_value_hinfo(conn_handle, cid, &hinfo); } +done: /* Notify GATT client that the full response has been parsed. */ ble_gattc_rx_find_type_value_complete(conn_handle, cid, rc); @@ -571,7 +588,9 @@ ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid, int i; uint8_t op; - if (num_handles < 1) { + if (num_handles < 2 || + BLE_ATT_READ_MULT_REQ_BASE_SZ + + sizeof(req->handles[0]) * num_handles > ble_att_mtu_by_cid(conn_handle, cid)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); return BLE_HS_EINVAL; } @@ -632,6 +651,7 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle, uint16_t cid, struct ble_att_read_group_type_req *req; struct os_mbuf *txom; + int rc; if (start_handle == 0 || start_handle > end_handle) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); @@ -647,7 +667,11 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle, uint16_t cid, req->bagq_start_handle = htole16(start_handle); req->bagq_end_handle = htole16(end_handle); - ble_uuid_flat(uuid, req->uuid); + rc = ble_uuid_flat(uuid, req->uuid); + if (rc != 0) { + os_mbuf_free_chain(txom); + return rc; + } return ble_att_tx(conn_handle, cid, txom); } @@ -726,6 +750,7 @@ ble_att_clt_tx_write_req(uint16_t conn_handle, uint16_t cid, uint16_t handle, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_WRITE + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -750,6 +775,7 @@ ble_att_clt_tx_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t handle, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_WRITE_NO_RSP + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -804,9 +830,10 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han { struct ble_att_signed_write_cmd *cmd; - struct os_mbuf *txom2; + struct os_mbuf *txom2 = NULL; uint8_t cmac[16]; uint8_t *message = NULL; + uint32_t counter_le; uint16_t len; uint16_t payload_len; int rc; @@ -865,7 +892,9 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han } /** Copying sign counter */ - memcpy(&message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET + payload_len], &counter, sizeof(counter)); + counter_le = htole32(counter); + memcpy(&message[BLE_ATT_SIGNED_WRITE_DATA_OFFSET + payload_len], + &counter_le, sizeof(counter_le)); /* ble_sm_alg_aes_cmac takes data in little-endian format, * so converting it to LE. @@ -893,7 +922,7 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han swap_in_place(cmac, sizeof(cmac)); /* Creating final signed message */ - rc = os_mbuf_append(txom, (void *)&counter, sizeof(counter)); + rc = os_mbuf_append(txom, (void *)&counter_le, sizeof(counter_le)); if (rc != 0) { goto err; } @@ -907,14 +936,20 @@ ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t cid, uint16_t han nimble_platform_mem_free(message); message = NULL; } - os_mbuf_concat(txom2, txom); - return ble_att_tx(conn_handle, cid, txom2); + rc = os_mbuf_appendfrom(txom2, txom, 0, OS_MBUF_PKTLEN(txom)); + if (rc != 0) { + goto err; + } + rc = ble_att_tx(conn_handle, cid, txom2); + if (rc == 0) { + os_mbuf_free_chain(txom); + } + return rc; err: if (message != NULL) { nimble_platform_mem_free(message); message = NULL; } - /* Do not free txom here; the caller ble_gattc_signed_write is responsible for it on error. */ os_mbuf_free_chain(txom2); return rc; } @@ -929,6 +964,7 @@ ble_att_clt_tx_prep_write(uint16_t conn_handle, uint16_t cid, uint16_t handle, uint16_t offset, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_PREP_WRITE + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -1057,6 +1093,7 @@ ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_NOTIFY + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif @@ -1081,7 +1118,9 @@ ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle, cid = ble_eatt_get_available_chan_cid(conn_handle, BLE_GATT_OP_DUMMY); rc = ble_att_tx(conn_handle, cid, txom2); - ble_eatt_release_chan(conn_handle, BLE_GATT_OP_DUMMY); + if (cid != BLE_L2CAP_CID_ATT) { + ble_eatt_release_chan(conn_handle, BLE_GATT_OP_DUMMY); + } return rc; err: @@ -1097,6 +1136,7 @@ int ble_att_clt_tx_multi_notify(uint16_t conn_handle, struct os_mbuf * om) { #if !NIMBLE_BLE_ATT_CLT_NOTIFY_MULT + os_mbuf_free_chain(om); return BLE_HS_ENOTSUP; #endif @@ -1105,6 +1145,7 @@ ble_att_clt_tx_multi_notify(uint16_t conn_handle, struct os_mbuf * om) int rc; if (ble_att_cmd_get(BLE_ATT_OP_NOTIFY_MULTI_REQ, 0, &txom) == NULL) { + os_mbuf_free_chain(om); return BLE_HS_ENOMEM; } @@ -1126,6 +1167,7 @@ ble_att_clt_tx_indicate(uint16_t conn_handle, uint16_t cid, uint16_t handle, struct os_mbuf *txom) { #if !NIMBLE_BLE_ATT_CLT_INDICATE + os_mbuf_free_chain(txom); return BLE_HS_ENOTSUP; #endif diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index eea90e007..c5cecbbb7 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -10227,7 +10227,7 @@ ble_gap_end_test_evt(const void *buf, uint8_t len) status = cmd_complete->status; ptr = (uint8_t *)cmd_complete->return_params; - num_pkt = htole16(*ptr); + num_pkt = get_le16(ptr); memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_TEST_UPDATE; diff --git a/nimble/host/src/ble_gatts_lcl.c b/nimble/host/src/ble_gatts_lcl.c index 80179b44b..65a6e6e7a 100644 --- a/nimble/host/src/ble_gatts_lcl.c +++ b/nimble/host/src/ble_gatts_lcl.c @@ -29,6 +29,8 @@ #if NIMBLE_BLE_CONNECT static const ble_uuid_t *uuid_ccc = BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16); +static const ble_uuid_t *uuid_cep = + BLE_UUID16_DECLARE(BLE_GATT_DSC_EXT_PROP_UUID16); #if MYNEWT_VAL(BLE_CPFD_CAFD) static const ble_uuid_t *uuid_cpfd = @@ -151,6 +153,23 @@ ble_gatt_show_local_chr(const struct ble_gatt_svc_def *svc, handle++; } + if (chr->flags & (BLE_GATT_CHR_F_RELIABLE_WRITE | + BLE_GATT_CHR_F_AUX_WRITE)) { + console_printf("cep descriptor\n"); + console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s " + "%s\n", " ", "uuid", + ble_uuid_to_str(uuid_cep, uuid_buf)); + console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s " + "%d\n", " ", "handle", handle); + console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s " + "%d\n", " ", "min_key_size", 0); + console_printf("%" FIELD_INDENT "s %" FIELD_NAME_LEN "s " + "%s\n", " ", "flags", + ble_gatts_flags_to_str(BLE_ATT_F_READ, + flags_buf, ble_gatt_dsc_f_names)); + handle++; + } + #if MYNEWT_VAL(BLE_CPFD_CAFD) cpfd_count = 0; for (cpfd = chr->cpfd; cpfd && cpfd->format; ++cpfd) { @@ -269,7 +288,9 @@ ble_gatt_show_local_svc(const struct ble_gatt_svc_def *svc, void ble_gatts_show_local(void) { + ble_hs_lock(); ble_gatts_lcl_svc_foreach(ble_gatt_show_local_svc, NULL); + ble_hs_unlock(); } #endif diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c index a70fd237e..5ae17db0a 100644 --- a/nimble/host/src/ble_hs_hci.c +++ b/nimble/host/src/ble_hs_hci.c @@ -34,6 +34,10 @@ #endif // (BT_HCI_LOG_INCLUDED == TRUE) #define BLE_HCI_CMD_TIMEOUT_MS 2000 +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif + #if MYNEWT_VAL(BLE_ERR_NAME) /* HCI ERROR */ #define BLE_ERR_UNKNOWN_HCI_CMD 0x01 @@ -319,6 +323,7 @@ static struct ble_hci_ev *l_ble_hs_hci_ack; #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE #define BLE_HS_HCI_FRAG_DATABUF_SIZE \ (BLE_ACL_MAX_PKT_SIZE + \ + 1 + \ BLE_HCI_DATA_HDR_SZ + \ sizeof (struct os_mbuf_pkthdr) + \ sizeof (struct ble_mbuf_hdr) + \ @@ -326,6 +331,7 @@ static struct ble_hci_ev *l_ble_hs_hci_ack; #else #define BLE_HS_HCI_FRAG_DATABUF_SIZE \ (BLE_ACL_MAX_PKT_SIZE + \ + 1 + \ BLE_HCI_DATA_HDR_SZ + \ BLE_HS_CTRL_DATA_HDR_SZ + \ sizeof (struct os_mbuf_pkthdr) + \ @@ -410,7 +416,8 @@ ble_hs_hci_add_avail_pkts(uint16_t delta) { BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task()); - if (ble_hs_hci_avail_pkts + delta > UINT16_MAX) { + if (delta > ble_hs_hci_max_pkts || + ble_hs_hci_avail_pkts > ble_hs_hci_max_pkts - delta) { ble_hs_sched_reset(BLE_HS_ECONTROLLER); } else { ble_hs_hci_avail_pkts += delta; @@ -595,8 +602,8 @@ ble_hs_hci_cmd_tx(uint16_t opcode, const void *cmd, uint8_t cmd_len, struct ble_hs_hci_ack ack; int rc; - BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack == NULL); ble_hs_hci_lock(); + BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack == NULL); rc = ble_hs_hci_cmd_send_buf(opcode, cmd, cmd_len); if (rc != 0) { @@ -660,8 +667,9 @@ ble_hs_hci_send_vs_cmd(uint16_t ocf, const void *cmdbuf, uint8_t cmdlen, static void ble_hs_hci_rx_ack(uint8_t *ack_ev) { - if (ble_npl_sem_get_count(&ble_hs_hci_sem) > 0) { - /* This ack is unexpected; ignore it. */ + if (ble_npl_sem_get_count(&ble_hs_hci_sem) > 0 || + l_ble_hs_hci_ack != NULL) { + /* This ack is unexpected or duplicated; ignore it. */ #if MYNEWT_VAL(MP_RUNTIME_ALLOC) ble_transport_free(BLE_HCI_EVT, ack_ev); #else @@ -669,7 +677,6 @@ ble_hs_hci_rx_ack(uint8_t *ack_ev) #endif return; } - BLE_HS_DBG_ASSERT(l_ble_hs_hci_ack == NULL); /* Unblock the application now that the HCI command buffer is populated * with the acknowledgement. @@ -709,7 +716,7 @@ ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg) #if ((BT_HCI_LOG_INCLUDED == TRUE) && SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED) uint16_t len = hci_ev[1] + 3; if (host_recv_adv_packet(hci_ev)) { - bt_hci_log_record_hci_adv(HCI_LOG_DATA_TYPE_ADV, &hci_ev[1], len - 2); + bt_hci_log_record_hci_adv(HCI_LOG_DATA_TYPE_ADV, &hci_ev[2], hci_ev[1]); } else { bt_hci_log_record_hci_data(0x04, &hci_ev[0], len - 1); } @@ -780,7 +787,7 @@ ble_hs_hci_max_acl_payload_sz(void) * data portion of HCI LE ACL Data Packets sent from the Host to the * Controller. */ - return ble_hs_hci_buf_sz; + return MIN(ble_hs_hci_buf_sz, BLE_ACL_MAX_PKT_SIZE - BLE_HCI_DATA_HDR_SZ); } #endif @@ -800,9 +807,9 @@ ble_hs_hci_frag_alloc(uint16_t frag_size, void *arg) #endif if (om != NULL) { #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE - om->om_data += BLE_HCI_DATA_HDR_SZ; + om->om_data += 1 + BLE_HCI_DATA_HDR_SZ; #else - om->om_data += BLE_HCI_DATA_HDR_SZ + BLE_HS_CTRL_DATA_HDR_SZ; + om->om_data += 1 + BLE_HCI_DATA_HDR_SZ + BLE_HS_CTRL_DATA_HDR_SZ; #endif return om; } @@ -894,7 +901,8 @@ ble_hs_hci_acl_tx_now(struct ble_hs_conn *conn, struct os_mbuf **om) /* Send fragments until the entire packet has been sent. */ while (txom != NULL && ble_hs_hci_avail_pkts > 0) { #if SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED - frag = mem_split_frag(&txom, BLE_ACL_MAX_PKT_SIZE, ble_hs_hci_frag_alloc, NULL); + frag = mem_split_frag(&txom, BLE_ACL_MAX_PKT_SIZE - BLE_HCI_DATA_HDR_SZ, + ble_hs_hci_frag_alloc, NULL); #else frag = mem_split_frag(&txom, ble_hs_hci_max_acl_payload_sz(), ble_hs_hci_frag_alloc, NULL); #endif @@ -1072,6 +1080,15 @@ void ble_hs_hci_deinit(void) } #endif + if (l_ble_hs_hci_ack != NULL) { +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + ble_transport_free(BLE_HCI_EVT, (uint8_t *) l_ble_hs_hci_ack); +#else + ble_transport_free((uint8_t *) l_ble_hs_hci_ack); +#endif + l_ble_hs_hci_ack = NULL; + } + /* Clean up mempool first to ensure blocks are free */ os_mempool_clear(&ble_hs_hci_frag_mempool); diff --git a/nimble/host/src/ble_hs_hci_cmd.c b/nimble/host/src/ble_hs_hci_cmd.c index f03dee86b..006e89b7d 100644 --- a/nimble/host/src/ble_hs_hci_cmd.c +++ b/nimble/host/src/ble_hs_hci_cmd.c @@ -65,6 +65,10 @@ ble_hs_hci_cmd_transport(struct ble_hci_cmd *cmd) BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM_EVT); return BLE_HS_ENOMEM_EVT; + case BLE_HS_ETIMEOUT_HCI: + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ETIMEOUT_HCI); + return BLE_HS_ETIMEOUT_HCI; + default: BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EUNKNOWN); return BLE_HS_EUNKNOWN; @@ -108,6 +112,7 @@ ble_hs_hci_cmd_send(uint16_t opcode, uint8_t len, const void *cmddata) #if !(SOC_ESP_NIMBLE_CONTROLLER) && CONFIG_BT_CONTROLLER_ENABLED buf--; + buf[0] = 0x01; #endif #if ((BT_HCI_LOG_INCLUDED == TRUE) && SOC_ESP_NIMBLE_CONTROLLER && CONFIG_BT_CONTROLLER_ENABLED) @@ -119,7 +124,15 @@ 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 { +#if !(SOC_ESP_NIMBLE_CONTROLLER) && CONFIG_BT_CONTROLLER_ENABLED + /* ESP VHCI transport consumes the command buffer even on error. */ +#else +#if MYNEWT_VAL(MP_RUNTIME_ALLOC) + ble_transport_free(BLE_HCI_CMD, buf); +#else ble_transport_free(buf); +#endif +#endif 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 ab9b7a878..589681dc9 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -126,7 +126,7 @@ static ble_hs_hci_evt_fn ble_hs_hci_evt_hw_error; static ble_hs_hci_evt_fn ble_hs_hci_evt_num_completed_pkts; #if NIMBLE_BLE_CONNECT static ble_hs_hci_evt_fn ble_hs_hci_evt_disconn_complete; -#if MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_SM static ble_hs_hci_evt_fn ble_hs_hci_evt_encrypt_change; static ble_hs_hci_evt_fn ble_hs_hci_evt_enc_key_refresh; #endif @@ -248,7 +248,7 @@ static bool pend_conn_complete_valid; /** Dispatch table for incoming HCI events. Sorted by event code field. */ struct ble_hs_hci_evt_dispatch_entry { - uint8_t event_code; + uint16_t event_code; ble_hs_hci_evt_fn *cb; }; @@ -258,7 +258,7 @@ static const struct ble_hs_hci_evt_dispatch_entry ble_hs_hci_evt_dispatch[] = { #if NIMBLE_BLE_CONNECT { BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP, ble_hs_hci_evt_rd_rem_ver_complete }, { BLE_HCI_EVCODE_DISCONN_CMP, ble_hs_hci_evt_disconn_complete }, -#if MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_SM { BLE_HCI_EVCODE_ENCRYPT_CHG, ble_hs_hci_evt_encrypt_change }, { BLE_HCI_EVCODE_ENC_KEY_REFRESH, ble_hs_hci_evt_enc_key_refresh }, #endif @@ -268,11 +268,11 @@ static const struct ble_hs_hci_evt_dispatch_entry ble_hs_hci_evt_dispatch[] = { { BLE_HCI_EVCODE_VS, ble_hs_hci_evt_vs }, #endif #if MYNEWT_VAL(BLE_DTM_MODE_TEST) - { BLE_HCI_OCF_LE_RX_TEST, ble_hs_hci_evt_rx_test }, - { BLE_HCI_OCF_LE_TX_TEST, ble_hs_hci_evt_tx_test }, - { BLE_HCI_OCF_LE_TEST_END, ble_hs_hci_evt_end_test }, - { BLE_HCI_OCF_LE_RX_TEST_V2, ble_hs_hci_evt_rx_test }, - { BLE_HCI_OCF_LE_TX_TEST_V2, ble_hs_hci_evt_tx_test }, + { BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RX_TEST), ble_hs_hci_evt_rx_test }, + { BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_TX_TEST), ble_hs_hci_evt_tx_test }, + { BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_TEST_END), ble_hs_hci_evt_end_test }, + { BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RX_TEST_V2), ble_hs_hci_evt_rx_test }, + { BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_TX_TEST_V2), ble_hs_hci_evt_tx_test }, #endif }; @@ -422,7 +422,7 @@ void ble_hs_hci_ctx_free(void) (sizeof ble_hs_hci_evt_le_dispatch / sizeof ble_hs_hci_evt_le_dispatch[0]) static const struct ble_hs_hci_evt_dispatch_entry * -ble_hs_hci_evt_dispatch_find(uint8_t event_code) +ble_hs_hci_evt_dispatch_find(uint16_t event_code) { const struct ble_hs_hci_evt_dispatch_entry *entry; unsigned int i; @@ -439,6 +439,26 @@ ble_hs_hci_evt_dispatch_find(uint8_t event_code) static const uint8_t ble_hs_conn_null_addr[6]; +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static inline void +ble_hs_hci_evt_resolve_rpa(ble_addr_t *addr) +{ + struct ble_hs_resolv_entry *rl; + + ble_hs_lock(); + rl = ble_hs_resolv_rpa_addr(addr->val, addr->type); + if (rl != NULL) { + if (addr->type == 1) { + rl->rl_isrpa = 1; + } + + memcpy(addr->val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN); + addr->type = rl->rl_addr_type; + } + ble_hs_unlock(); +} +#endif + static ble_hs_hci_evt_le_fn * ble_hs_hci_evt_le_dispatch_find(uint8_t event_code) { @@ -593,14 +613,14 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, return 0; } -#if MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_SM static int ble_hs_hci_evt_encrypt_change(uint8_t event_code, const void *data, unsigned int len) { const struct ble_hci_ev_enrypt_chg *ev = data; - if (len != sizeof (*ev)) { + if (len < sizeof (*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -616,7 +636,7 @@ ble_hs_hci_evt_hw_error(uint8_t event_code, const void *data, unsigned int len) { const struct ble_hci_ev_hw_error *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -626,14 +646,14 @@ ble_hs_hci_evt_hw_error(uint8_t event_code, const void *data, unsigned int len) return 0; } -#if NIMBLE_BLE_CONNECT && MYNEWT_VAL(BLE_SM_SC) +#if NIMBLE_BLE_CONNECT && NIMBLE_BLE_SM static int ble_hs_hci_evt_enc_key_refresh(uint8_t event_code, const void *data, unsigned int len) { const struct ble_hci_ev_enc_key_refresh *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -672,6 +692,8 @@ ble_hs_hci_evt_num_completed_pkts(uint8_t event_code, const void *data, if (conn != NULL) { if (conn->bhc_outstanding_pkts < num_pkts) { ble_hs_sched_reset(BLE_HS_ECONTROLLER); + ble_hs_unlock(); + return BLE_HS_ECONTROLLER; } else { conn->bhc_outstanding_pkts -= num_pkts; } @@ -717,6 +739,11 @@ ble_hs_hci_evt_vs(uint8_t event_code, const void *data, unsigned int len) static int ble_hs_hci_evt_rx_test(uint8_t event_code, const void *data, unsigned int len) { + if (len < sizeof(struct ble_hci_ev_command_complete)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); + return BLE_HS_ECONTROLLER; + } + ble_gap_rx_test_evt(data, len); return 0; @@ -725,6 +752,11 @@ ble_hs_hci_evt_rx_test(uint8_t event_code, const void *data, unsigned int len) static int ble_hs_hci_evt_tx_test(uint8_t event_code, const void *data, unsigned int len) { + if (len < sizeof(struct ble_hci_ev_command_complete)) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); + return BLE_HS_ECONTROLLER; + } + ble_gap_tx_test_evt(data, len); return 0; @@ -733,6 +765,11 @@ ble_hs_hci_evt_tx_test(uint8_t event_code, const void *data, unsigned int len) static int ble_hs_hci_evt_end_test(uint8_t event_code, const void *data, unsigned int len) { + if (len < sizeof(struct ble_hci_ev_command_complete) + 2) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); + return BLE_HS_ECONTROLLER; + } + ble_gap_end_test_evt(data, len); return 0; @@ -755,7 +792,8 @@ ble_hs_hci_evt_le_meta(uint8_t event_code, const void *data, unsigned int len) return fn(ev->subevent, data, len); } - return 0; + STATS_INC(ble_hs_stats, hci_unknown_event); + return BLE_HS_ENOTSUP; } @@ -801,7 +839,8 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) /* RPA needs to be resolved here, as controller is not aware of the * address is RPA in Host based RPA */ - if (ble_host_rpa_enabled() && ((!memcmp(evt.local_rpa, ble_hs_conn_null_addr, 6)) == 0)) { + if (ble_host_rpa_enabled() && + memcmp(evt.local_rpa, ble_hs_conn_null_addr, 6) == 0) { uint8_t *local_id_rpa = ble_hs_get_rpa_local(); memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN); } @@ -840,15 +879,22 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, #endif #if MYNEWT_VAL(BLE_PERIODIC_ADV_WITH_RESPONSES) + uint8_t instance = 0; + if (subevent == BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE) { evt.adv_handle = 0xFF; evt.sync_handle = 0xFFFF; } else { evt.adv_handle = ev->adv_handle; evt.sync_handle = ev->sync_handle; + if (evt.adv_handle < BLE_ADV_INSTANCES) { + instance = evt.adv_handle; + } } -#endif + return ble_gap_rx_conn_complete(&evt, instance); +#else return ble_gap_rx_conn_complete(&evt, 0); +#endif } @@ -950,7 +996,7 @@ ble_hs_hci_evt_le_adv_rpt_first_pass(const void *data, unsigned int len) rpt = data; - if (rpt->data_len > len) { + if (len < sizeof(*rpt) + 1 + rpt->data_len) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -993,9 +1039,10 @@ 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); + if (ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) == BLE_NPL_OK) { + ble_adv_list_count++; + ble_npl_mutex_release(&adv_list_lock); + } #endif for (i = 0; i < ev->num_reports; i++) { @@ -1023,19 +1070,7 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) */ desc.ota_addr = desc.addr; - struct ble_hs_resolv_entry *rl = NULL; - ble_hs_lock(); - rl = ble_hs_resolv_rpa_addr(desc.addr.val, desc.addr.type); - - if (rl != NULL) { - if (desc.addr.type == 1) { - rl->rl_isrpa = 1; - } - - memcpy(desc.addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN); - desc.addr.type = rl->rl_addr_type; - } - ble_hs_unlock(); + ble_hs_hci_evt_resolve_rpa(&desc.addr); #endif desc.length_data = rpt->data_len; @@ -1073,8 +1108,9 @@ ble_hs_hci_evt_le_dir_adv_rpt(uint8_t subevent, const void *data, unsigned int l desc.rssi = ev->reports[i].rssi; #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) - /* Directed adv path does not perform identity-swap; mirror OTA addr. */ desc.ota_addr = desc.addr; + ble_hs_hci_evt_resolve_rpa(&desc.addr); + ble_hs_hci_evt_resolve_rpa(&desc.direct_addr); #endif ble_gap_rx_adv_report(&desc); } @@ -1121,6 +1157,8 @@ ble_hs_hci_evt_rd_rem_ver_complete(uint8_t subevent, const void *data, static int ble_hs_hci_decode_legacy_type(uint16_t evt_type) { + evt_type &= 0x001F; + switch (evt_type) { case BLE_HCI_LEGACY_ADV_EVTYPE_ADV_IND: return BLE_HCI_ADV_RPT_EVTYPE_ADV_IND; @@ -1137,6 +1175,7 @@ ble_hs_hci_decode_legacy_type(uint16_t evt_type) return -1; } } +#endif #if MYNEWT_VAL(BLE_ISO) typedef void (*ble_hs_hci_evt_iso_fn)(uint8_t event, const void *data, @@ -1144,18 +1183,24 @@ typedef void (*ble_hs_hci_evt_iso_fn)(uint8_t event, const void *data, static ble_hs_hci_evt_iso_fn iso_evt_cb; +static ble_hs_hci_evt_iso_fn +ble_hs_hci_evt_iso_cb_get(void) +{ + ble_hs_hci_evt_iso_fn cb; + + ble_hs_lock(); + cb = iso_evt_cb; + ble_hs_unlock(); + + return cb; +} + int ble_hs_iso_evt_rx_cb_set(void *cb) { - if (cb == NULL) { - return -BLE_HS_EINVAL; - } - - if (iso_evt_cb) { - return -BLE_HS_EALREADY; - } - - iso_evt_cb = cb; + ble_hs_lock(); + iso_evt_cb = (ble_hs_hci_evt_iso_fn)cb; + ble_hs_unlock(); return 0; } @@ -1163,8 +1208,10 @@ ble_hs_iso_evt_rx_cb_set(void *cb) static int ble_hs_hci_evt_le_cis_disconn(const struct ble_hci_ev_disconn_cmp *ev) { - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_EVCODE_DISCONN_CMP, (const void *)ev, sizeof(*ev), false); + ble_hs_hci_evt_iso_fn cb = ble_hs_hci_evt_iso_cb_get(); + + if (cb) { + cb(BLE_HCI_EVCODE_DISCONN_CMP, (const void *)ev, sizeof(*ev), false); } else { ble_gap_rx_cis_disconn(ev); } @@ -1176,14 +1223,16 @@ static int ble_hs_hci_evt_le_cis_estab(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_cis_established *ev = data; + ble_hs_hci_evt_iso_fn cb; if (len != sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_CIS_ESTABLISHED, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_CIS_ESTABLISHED, data, len, true); } else { ble_gap_rx_cis_estab(ev); } @@ -1195,14 +1244,16 @@ static int ble_hs_hci_evt_le_cis_request(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_cis_request *ev = data; + ble_hs_hci_evt_iso_fn cb; if (len != sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_CIS_REQUEST, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_CIS_REQUEST, data, len, true); } else { ble_gap_rx_cis_request(ev); } @@ -1214,6 +1265,7 @@ static int ble_hs_hci_evt_le_create_big_comp(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_create_big_complete *ev = data; + ble_hs_hci_evt_iso_fn cb; if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); @@ -1226,8 +1278,9 @@ ble_hs_hci_evt_le_create_big_comp(uint8_t subevent, const void *data, unsigned i return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_CREATE_BIG_COMPLETE, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_CREATE_BIG_COMPLETE, data, len, true); } else { ble_gap_rx_create_big_comp(ev); } @@ -1239,14 +1292,16 @@ static int ble_hs_hci_evt_le_term_big_comp(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_terminate_big_complete *ev = data; + ble_hs_hci_evt_iso_fn cb; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_TERMINATE_BIG_COMPLETE, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_TERMINATE_BIG_COMPLETE, data, len, true); } else { ble_gap_rx_term_big_comp(ev); } @@ -1258,6 +1313,7 @@ static int ble_hs_hci_evt_le_big_sync_estab(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_big_sync_established *ev = data; + ble_hs_hci_evt_iso_fn cb; if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); @@ -1270,8 +1326,9 @@ ble_hs_hci_evt_le_big_sync_estab(uint8_t subevent, const void *data, unsigned in return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_BIG_SYNC_ESTABLISHED, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_BIG_SYNC_ESTABLISHED, data, len, true); } else { ble_gap_rx_big_sync_estab(ev); } @@ -1283,14 +1340,16 @@ static int ble_hs_hci_evt_le_big_sync_lost(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_big_sync_lost *ev = data; + ble_hs_hci_evt_iso_fn cb; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_BIG_SYNC_LOST, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_BIG_SYNC_LOST, data, len, true); } else { ble_gap_rx_big_sync_lost(ev); } @@ -1302,14 +1361,16 @@ static int ble_hs_hci_evt_le_biginfo_adv_rpt(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_biginfo_adv_report *ev = data; + ble_hs_hci_evt_iso_fn cb; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_BIGINFO_ADV_REPORT, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_BIGINFO_ADV_REPORT, data, len, true); } else { ble_gap_rx_biginfo_adv_rpt(ev); } @@ -1322,14 +1383,16 @@ static int ble_hs_hci_evt_le_cis_estab_v2(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_cis_established_v2 *ev = data; + ble_hs_hci_evt_iso_fn cb; if (len != sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } - if (iso_evt_cb) { - iso_evt_cb(BLE_HCI_LE_SUBEV_CIS_ESTABLISHED_V2, data, len, true); + cb = ble_hs_hci_evt_iso_cb_get(); + if (cb) { + cb(BLE_HCI_LE_SUBEV_CIS_ESTABLISHED_V2, data, len, true); } else { ble_gap_rx_cis_estab_v2(ev); } @@ -1339,6 +1402,7 @@ ble_hs_hci_evt_le_cis_estab_v2(uint8_t subevent, const void *data, unsigned int #endif /* MYNEWT_VAL(BLE_ISO_CIS_ESTAB_V2) */ #endif /* MYNEWT_VAL(BLE_ISO) */ +#if MYNEWT_VAL(BLE_EXT_ADV) && NIMBLE_BLE_SCAN static int ble_hs_hci_evt_le_ext_adv_rpt_first_pass(const void *data, unsigned int len) { @@ -1368,7 +1432,7 @@ ble_hs_hci_evt_le_ext_adv_rpt_first_pass(const void *data, unsigned int len) report = data; - if (report->data_len > len) { + if (len < sizeof(*report) + report->data_len) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -1396,6 +1460,7 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data, const struct ble_hci_ev_le_subev_ext_adv_rpt *ev = data; const struct ext_adv_report *report; struct ble_gap_ext_disc_desc desc; + uint16_t evt_type; int legacy_event_type; int rc; int i; @@ -1409,9 +1474,10 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data, for (i = 0; i < ev->num_reports; i++) { memset(&desc, 0, sizeof(desc)); - desc.props = (report->evt_type) & 0x1F; + evt_type = le16toh(report->evt_type); + desc.props = evt_type & 0x1F; if (desc.props & BLE_HCI_ADV_LEGACY_MASK) { - legacy_event_type = ble_hs_hci_decode_legacy_type(report->evt_type); + legacy_event_type = ble_hs_hci_decode_legacy_type(evt_type); if (legacy_event_type < 0) { report = (const void *) &report->data[report->data_len]; continue; @@ -1419,7 +1485,7 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data, desc.legacy_event_type = legacy_event_type; desc.data_status = BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE; } else { - switch(report->evt_type & BLE_HCI_ADV_DATA_STATUS_MASK) { + switch(evt_type & BLE_HCI_ADV_DATA_STATUS_MASK) { case BLE_HCI_ADV_DATA_STATUS_COMPLETE: desc.data_status = BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE; break; @@ -1445,7 +1511,12 @@ ble_hs_hci_evt_le_ext_adv_rpt(uint8_t subevent, const void *data, desc.sid = report->sid; desc.prim_phy = report->pri_phy; desc.sec_phy = report->sec_phy; - desc.periodic_adv_itvl = report->periodic_itvl; + desc.periodic_adv_itvl = le16toh(report->periodic_itvl); + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_hs_hci_evt_resolve_rpa(&desc.addr); + ble_hs_hci_evt_resolve_rpa(&desc.direct_addr); +#endif ble_gap_rx_ext_adv_report(&desc); @@ -1463,7 +1534,7 @@ ble_hs_hci_evt_le_periodic_adv_sync_estab(uint8_t subevent, const void *data, { const struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -1499,7 +1570,7 @@ ble_hs_hci_evt_le_periodic_adv_sync_lost(uint8_t subevent, const void *data, { const struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBADDATA); return BLE_HS_EBADDATA; } @@ -1550,7 +1621,7 @@ ble_hs_hci_evt_le_periodic_adv_sync_transfer(uint8_t subevent, const void *data, #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER) const struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBADDATA); return BLE_HS_EBADDATA; } @@ -1569,7 +1640,7 @@ ble_hs_hci_evt_le_biginfo_adv_report(uint8_t subevent, const void *data, { const struct ble_hci_ev_le_subev_biginfo_adv_report *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EBADDATA); return BLE_HS_EBADDATA; } @@ -1607,7 +1678,7 @@ ble_hs_hci_evt_le_adv_set_terminated(uint8_t subevent, const void *data, #if MYNEWT_VAL(BLE_EXT_ADV) const struct ble_hci_ev_le_subev_adv_set_terminated *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -1623,16 +1694,18 @@ ble_hs_hci_evt_le_adv_set_terminated(uint8_t subevent, const void *data, if (ev->status == 0) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) - if (ble_hs_hci_ensure_ctx()) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); - return BLE_HS_ENOMEM; - } + if (ble_hs_hci_ensure_ctx() == 0) { #endif /* ignore return code as we need to terminate advertising set anyway */ - if (pend_conn_complete_valid) { - ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle); - pend_conn_complete_valid = false; + if (pend_conn_complete_valid) { + ble_gap_rx_conn_complete(&pend_conn_complete, ev->adv_handle); + pend_conn_complete_valid = false; + } +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + } else { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ENOMEM); } +#endif } ble_gap_rx_adv_set_terminated(ev); #endif @@ -1771,7 +1844,6 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data const struct ble_hci_ev_le_subev_periodic_adv_resp_rep *ev = data; const struct periodic_adv_response *response; struct ble_gap_periodic_adv_response resp; - uint32_t size; if (len < 5) { // minimum size for fixed fields BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); @@ -1796,16 +1868,6 @@ ble_hs_hci_evt_le_periodic_adv_subev_resp_rep(uint8_t subevent, const void *data return BLE_HS_ECONTROLLER; } - /* TODO: compare with the total length including the response data. */ - size = sizeof(*ev); - for (uint8_t i = 0; i < ev->num_responses; i ++) { - size += sizeof(struct periodic_adv_response) + ev->responses[i].data_length; - } - if (len < size) { - BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); - return BLE_HS_ECONTROLLER; - } - len -= sizeof(*ev); data += sizeof(*ev); @@ -1845,7 +1907,7 @@ ble_hs_hci_evt_le_conn_upd_complete(uint8_t subevent, const void *data, { const struct ble_hci_ev_le_subev_conn_upd_complete *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -1876,15 +1938,22 @@ ble_hs_hci_evt_le_lt_key_req(uint8_t subevent, const void *data, unsigned int le return BLE_HS_ECONTROLLER; } +#if NIMBLE_BLE_SM + return ble_sm_ltk_req_rx(ev); +#else ble_sm_ltk_req_rx(ev); - return 0; +#endif } static int ble_hs_hci_evt_le_conn_parm_req(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_rem_conn_param_req *ev = data; + struct ble_hci_le_rem_conn_params_nrr_cp cmd; + uint16_t max_interval; + uint16_t latency; + uint16_t timeout; if (len != sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); @@ -1901,6 +1970,18 @@ ble_hs_hci_evt_le_conn_parm_req(uint8_t subevent, const void *data, unsigned int BLE_HS_DBG_ASSERT(le16toh(ev->timeout) >= BLE_HCI_CONN_SPVN_TIMEOUT_MIN); BLE_HS_DBG_ASSERT(le16toh(ev->timeout) <= BLE_HCI_CONN_SPVN_TIMEOUT_MAX); + max_interval = le16toh(ev->max_interval); + latency = le16toh(ev->latency); + timeout = le16toh(ev->timeout); + if ((uint32_t)timeout * 4 <= (uint32_t)max_interval * (latency + 1)) { + cmd.conn_handle = ev->conn_handle; + cmd.reason = BLE_ERR_CONN_PARMS; + + return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR), + &cmd, sizeof(cmd), NULL, 0); + } + ble_gap_rx_param_req(ev); return 0; @@ -1912,7 +1993,7 @@ ble_hs_hci_evt_le_phy_update_complete(uint8_t subevent, const void *data, { const struct ble_hci_ev_le_subev_phy_update_complete *ev = data; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_ECONTROLLER); return BLE_HS_ECONTROLLER; } @@ -2012,20 +2093,28 @@ int ble_hs_hci_evt_process(struct ble_hci_ev *ev) { const struct ble_hs_hci_evt_dispatch_entry *entry; + struct ble_hci_ev_command_complete *cmd_complete; + uint16_t opcode; int rc; /* Count events received */ STATS_INC(ble_hs_stats, hci_event); if(ev->opcode == BLE_HCI_EVCODE_COMMAND_COMPLETE) { - if (ev->length < sizeof(struct ble_hci_ev_command_complete)) { + if (ev->length < sizeof(struct ble_hci_ev_command_complete_nop)) { STATS_INC(ble_hs_stats, hci_unknown_event); rc = BLE_HS_ECONTROLLER; goto done; } - /* Check if this Command complete has a parsable opcode */ - struct ble_hci_ev_command_complete *cmd_complete = (void *) ev->data; - entry = ble_hs_hci_evt_dispatch_find(cmd_complete->opcode); + cmd_complete = (void *) ev->data; + opcode = le16toh(cmd_complete->opcode); + if (opcode != BLE_HCI_OPCODE_NOP && + ev->length < sizeof(struct ble_hci_ev_command_complete)) { + STATS_INC(ble_hs_stats, hci_unknown_event); + rc = BLE_HS_ECONTROLLER; + goto done; + } + entry = ble_hs_hci_evt_dispatch_find(opcode); } else { entry = ble_hs_hci_evt_dispatch_find(ev->opcode); @@ -2124,6 +2213,19 @@ struct ble_addr_list_entry SLIST_HEAD(ble_device_list, ble_addr_list_entry) ble_adv_list; +static bool +ble_adv_list_ready(void) +{ +#if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) + if (ble_hci_ctx == NULL) { + BLE_HS_LOG(ERROR, "%s list context is NULL", __func__); + return false; + } +#endif + + return true; +} + void ble_adv_list_init(void) { #if MYNEWT_VAL(BLE_STATIC_TO_DYNAMIC) @@ -2132,7 +2234,10 @@ void ble_adv_list_init(void) } #endif - ble_npl_mutex_init(&adv_list_lock); + if (ble_npl_mutex_init(&adv_list_lock) != BLE_NPL_OK) { + BLE_HS_LOG(ERROR, "%s mutex init failed", __func__); + return; + } SLIST_INIT(&ble_adv_list); ble_adv_list_count = 0; @@ -2143,7 +2248,10 @@ void ble_adv_list_deinit(void) struct ble_addr_list_entry *device; struct ble_addr_list_entry *temp; - ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); + if (!ble_adv_list_ready() || + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + return; + } SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) { SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next); @@ -2158,15 +2266,26 @@ void ble_adv_list_deinit(void) void ble_adv_list_add_packet(void *data) { struct ble_addr_list_entry *device; + struct ble_addr_list_entry *entry; if (!data) { BLE_HS_LOG(ERROR, "%s data is NULL", __func__); return; } - ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); + if (!ble_adv_list_ready() || + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + return; + } device = (struct ble_addr_list_entry *)data; + SLIST_FOREACH(entry, &ble_adv_list, next) { + if (entry == device) { + ble_npl_mutex_release(&adv_list_lock); + return; + } + } + SLIST_INSERT_HEAD(&ble_adv_list, device, next); ble_npl_mutex_release(&adv_list_lock); @@ -2177,7 +2296,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); + if (!ble_adv_list_ready() || + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + return 0; + } + SLIST_FOREACH(device, &ble_adv_list, next) { length++; } @@ -2191,7 +2314,10 @@ void ble_adv_list_refresh(void) struct ble_addr_list_entry *device; struct ble_addr_list_entry *temp; - ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); + if (!ble_adv_list_ready() || + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + return; + } if (SLIST_EMPTY(&ble_adv_list)) { BLE_HS_LOG(DEBUG, "%s: ble_adv_list empty -- reinitializing", __func__); @@ -2218,7 +2344,10 @@ bool ble_check_adv_list(const uint8_t *addr, uint8_t addr_type) return found; } - ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); + if (!ble_adv_list_ready() || + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER) != BLE_NPL_OK) { + return found; + } SLIST_FOREACH(device, &ble_adv_list, next) { if (!memcmp(addr, device->addr.val, BLE_DEV_ADDR_LEN) && device->addr.type == addr_type) { diff --git a/nimble/host/src/ble_hs_hci_util.c b/nimble/host/src/ble_hs_hci_util.c index 6401075e9..896463784 100644 --- a/nimble/host/src/ble_hs_hci_util.c +++ b/nimble/host/src/ble_hs_hci_util.c @@ -439,13 +439,14 @@ ble_hs_hci_rd_all_local_supp_features(uint8_t* status, uint8_t* max_page, 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)); + &rsp.max_page, + sizeof(rsp) - sizeof(rsp.status)); if (rc != 0) { return rc; } - *status = rsp.status; + *status = 0; *max_page = rsp.max_page; memcpy(le_features, rsp.le_features, sizeof(rsp.le_features)); diff --git a/nimble/host/src/ble_hs_iso_hci.c b/nimble/host/src/ble_hs_iso_hci.c index f58ec7db8..5b7b541b8 100644 --- a/nimble/host/src/ble_hs_iso_hci.c +++ b/nimble/host/src/ble_hs_iso_hci.c @@ -12,6 +12,8 @@ #include "ble_hs_priv.h" #if MYNEWT_VAL(BLE_ISO) +#define BLE_HS_ISO_HCI_24_BIT_MAX 0xFFFFFFUL + int ble_hs_hci_iso_disconnect(uint16_t cis_handle, uint8_t reason) { @@ -29,7 +31,7 @@ int ble_hs_hci_read_local_supp_codec(uint8_t *rsp_buf, uint8_t rsp_len) { if (rsp_buf == NULL || rsp_len < 2) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_INFO_PARAMS, @@ -45,12 +47,12 @@ ble_hs_hci_read_local_supp_codec_caps(uint8_t coding_fmt, uint16_t company_id, struct ble_hci_ip_rd_local_supp_codec_caps_cp cmd; if (rsp_buf == NULL || rsp_len == 0) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.coding_fmt = coding_fmt; - cmd.company_id = company_id; - cmd.vs_codec_id = vs_codec_id; + cmd.company_id = htole16(company_id); + cmd.vs_codec_id = htole16(vs_codec_id); cmd.logical_tpt_type = logical_transport_type; cmd.direction = direction; @@ -75,18 +77,18 @@ ble_hs_hci_read_local_supp_controller_delay(uint8_t coding_fmt, uint16_t company if ((codec_cfg_len && codec_cfg == NULL) || min_controller_delay == NULL || max_controller_delay == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } if (sizeof(*cmd) + codec_cfg_len > 255) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd = (void *)cmd_buf; cmd->coding_fmt = coding_fmt; - cmd->company_id = company_id; - cmd->vs_codec_id = vs_codec_id; + cmd->company_id = htole16(company_id); + cmd->vs_codec_id = htole16(vs_codec_id); cmd->logical_tpt_type = logical_transport_type; cmd->direction = direction; cmd->codec_cfg_len = codec_cfg_len; @@ -116,7 +118,7 @@ ble_hs_hci_cfg_data_path(uint8_t data_path_direction, uint8_t data_path_id, uint8_t cmd_len = sizeof(*cmd) + vs_cfg_len; if (vs_cfg_len > 252 || (vs_cfg_len && vs_cfg == NULL)) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd = (void *)cmd_buf; @@ -141,7 +143,7 @@ ble_hs_hci_read_buf_sz_v2(uint16_t *data_len, uint8_t *data_packets, if (data_len == NULL || data_packets == NULL || iso_data_len == NULL || iso_data_packets == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, @@ -168,7 +170,7 @@ ble_hs_hci_read_iso_tx_sync(uint16_t conn_handle, uint16_t *packet_seq_num, int rc; if (packet_seq_num == NULL || timestamp == NULL || timeoffset == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.conn_handle = htole16(conn_handle); @@ -202,13 +204,16 @@ ble_hs_hci_set_cig_params(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uint32_t struct ble_hci_le_cis_params *cis_param; uint8_t cmd_len; - if (sizeof(*cmd) + (cis_cnt * sizeof(*cis_params)) > 255 || cis_cnt == 0 || cis_params == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (sizeof(*cmd) + (cis_cnt * sizeof(*cis_params)) > 255 || + cis_cnt == 0 || cis_params == NULL || + sdu_interval_c_to_p > BLE_HS_ISO_HCI_24_BIT_MAX || + sdu_interval_p_to_c > BLE_HS_ISO_HCI_24_BIT_MAX) { + return BLE_HS_EINVAL; } if (rsp_buf == NULL || rsp_len < sizeof(struct ble_hci_le_set_cig_params_rp) + cis_cnt * 2) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } uint8_t cmd_buf[sizeof(*cmd) + cis_cnt * sizeof(*cis_params)]; @@ -218,8 +223,8 @@ ble_hs_hci_set_cig_params(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uint32_t cis_param = cmd->cis; cmd->cig_id = cig_id; - memcpy(cmd->sdu_interval_c_to_p, &sdu_interval_c_to_p, sizeof(cmd->sdu_interval_c_to_p)); - memcpy(cmd->sdu_interval_p_to_c, &sdu_interval_p_to_c, sizeof(cmd->sdu_interval_p_to_c)); + put_le24(cmd->sdu_interval_c_to_p, sdu_interval_c_to_p); + put_le24(cmd->sdu_interval_p_to_c, sdu_interval_p_to_c); cmd->worst_sca = slaves_clock_accuracy; cmd->packing = packing; cmd->framing = framing; @@ -242,7 +247,9 @@ ble_hs_hci_set_cig_params(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uint32_t return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_CIG_PARAMS), - cmd_buf, cmd_len, rsp_buf, rsp_len); + cmd_buf, cmd_len, rsp_buf, + sizeof(struct ble_hci_le_set_cig_params_rp) + + cis_cnt * 2); } #if MYNEWT_VAL(BLE_ISO_TEST) @@ -257,13 +264,15 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin struct ble_hci_le_cis_params_test *cis_param; uint16_t cmd_len; - if (cis_cnt == 0 || cis_cnt > 17 || cis_params == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (cis_cnt == 0 || cis_cnt > 17 || cis_params == NULL || + sdu_interval_c_to_p > BLE_HS_ISO_HCI_24_BIT_MAX || + sdu_interval_p_to_c > BLE_HS_ISO_HCI_24_BIT_MAX) { + return BLE_HS_EINVAL; } if (rsp_buf == NULL|| rsp_len < sizeof(struct ble_hci_le_set_cig_params_rp) + cis_cnt * 2) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } uint8_t cmd_buf[sizeof(*cmd) + cis_cnt * sizeof(*cis_params)]; @@ -273,8 +282,8 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin cis_param = cmd->cis; cmd->cig_id = cig_id; - memcpy(cmd->sdu_interval_c_to_p, &sdu_interval_c_to_p, sizeof(cmd->sdu_interval_c_to_p)); - memcpy(cmd->sdu_interval_p_to_c, &sdu_interval_p_to_c, sizeof(cmd->sdu_interval_p_to_c)); + put_le24(cmd->sdu_interval_c_to_p, sdu_interval_c_to_p); + put_le24(cmd->sdu_interval_p_to_c, sdu_interval_p_to_c); cmd->ft_c_to_p = ft_c_to_p; cmd->ft_p_to_c = ft_p_to_c; cmd->iso_interval = htole16(iso_interval); @@ -301,7 +310,9 @@ ble_hs_hci_set_cig_params_test(uint8_t cig_id, uint32_t sdu_interval_c_to_p, uin return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_CIG_PARAMS_TEST), - cmd_buf, cmd_len, rsp_buf, rsp_len); + cmd_buf, cmd_len, rsp_buf, + sizeof(struct ble_hci_le_set_cig_params_rp) + + cis_cnt * 2); } #endif /* MYNEWT_VAL(BLE_ISO_TEST) */ @@ -312,8 +323,8 @@ ble_hs_hci_create_cis(uint8_t cis_cnt, const struct ble_hci_le_create_cis_params struct ble_hci_le_create_cis_params *param; uint16_t cmd_len; - if (cis_cnt == 0 || params == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (cis_cnt == 0 || cis_cnt > 0x1f || params == NULL) { + return BLE_HS_EINVAL; } uint8_t cmd_buf[sizeof(*cmd) + cis_cnt * sizeof(*params)]; @@ -384,8 +395,9 @@ ble_hs_hci_create_big(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, { struct ble_hci_le_create_big_cp cmd; - if (num_bis == 0 || (encryption && broadcast_code == NULL)) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (num_bis == 0 || sdu_interval > BLE_HS_ISO_HCI_24_BIT_MAX || + (encryption && broadcast_code == NULL)) { + return BLE_HS_EINVAL; } memset(&cmd, 0, sizeof(cmd)); @@ -393,9 +405,9 @@ ble_hs_hci_create_big(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, cmd.big_handle = big_handle; cmd.adv_handle = adv_handle; cmd.num_bis = num_bis; - memcpy(cmd.sdu_interval, &sdu_interval, sizeof(cmd.sdu_interval)); - cmd.max_sdu = max_sdu; - cmd.max_transport_latency = max_transport_latency; + put_le24(cmd.sdu_interval, sdu_interval); + cmd.max_sdu = htole16(max_sdu); + cmd.max_transport_latency = htole16(max_transport_latency); cmd.rtn = rtn; cmd.phy = phy; cmd.packing = packing; @@ -420,8 +432,9 @@ ble_hs_hci_create_big_test(uint8_t big_handle, uint8_t adv_handle, uint8_t num_b { struct ble_hci_le_create_big_test_cp cmd; - if (num_bis == 0 || (encryption && broadcast_code == NULL)) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (num_bis == 0 || sdu_interval > BLE_HS_ISO_HCI_24_BIT_MAX || + (encryption && broadcast_code == NULL)) { + return BLE_HS_EINVAL; } memset(&cmd, 0, sizeof(cmd)); @@ -429,11 +442,11 @@ ble_hs_hci_create_big_test(uint8_t big_handle, uint8_t adv_handle, uint8_t num_b cmd.big_handle = big_handle; cmd.adv_handle = adv_handle; cmd.num_bis = num_bis; - memcpy(cmd.sdu_interval, &sdu_interval, sizeof(cmd.sdu_interval)); - cmd.iso_interval = iso_interval; + put_le24(cmd.sdu_interval, sdu_interval); + cmd.iso_interval = htole16(iso_interval); cmd.nse = nse; - cmd.max_sdu = max_sdu; - cmd.max_pdu = max_pdu; + cmd.max_sdu = htole16(max_sdu); + cmd.max_pdu = htole16(max_pdu); cmd.phy = phy; cmd.packing = packing; cmd.framing = framing; @@ -476,7 +489,7 @@ ble_hs_hci_big_create_sync(uint8_t big_handle, uint16_t sync_handle, if ((encryption && broadcast_code == NULL) || num_bis == 0 || num_bis > MYNEWT_VAL(BLE_ISO_BIS_PER_BIG) || bis_index == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd = (void *)cmd_buf; @@ -521,11 +534,12 @@ ble_hs_hci_setup_iso_data_path(uint16_t conn_handle, uint8_t data_path_direction uint8_t cmd_buf[sizeof(*cmd) + codec_cfg_len]; if (codec_cfg_len && codec_cfg == NULL) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } - if (sizeof(struct ble_hci_le_setup_iso_data_path_cp) + codec_cfg_len > 255) { - return BLE_ERR_INV_HCI_CMD_PARMS; + if (sizeof(struct ble_hci_le_setup_iso_data_path_cp) + codec_cfg_len > 255 || + controller_delay > BLE_HS_ISO_HCI_24_BIT_MAX) { + return BLE_HS_EINVAL; } cmd = (void *)cmd_buf; @@ -536,7 +550,7 @@ ble_hs_hci_setup_iso_data_path(uint16_t conn_handle, uint8_t data_path_direction cmd->codec_id[0] = coding_fmt; put_le16(cmd->codec_id + 1, company_id); put_le16(cmd->codec_id + 3, vs_codec_id); - memcpy(cmd->controller_delay, &controller_delay, sizeof(cmd->controller_delay)); + put_le24(cmd->controller_delay, controller_delay); cmd->codec_config_len = codec_cfg_len; if (codec_cfg_len) { memcpy(cmd->codec_config, codec_cfg, codec_cfg_len); @@ -596,7 +610,7 @@ ble_hs_hci_iso_read_test_counters(uint16_t conn_handle, uint32_t *received_sdu_c int rc; if (!received_sdu_count || !missed_sdu_count || !failed_sdu_count) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.conn_handle = htole16(conn_handle); @@ -624,7 +638,7 @@ ble_hs_hci_iso_test_end(uint16_t conn_handle, uint32_t *received_sdu_count, int rc; if (!received_sdu_count || !missed_sdu_count || !failed_sdu_count) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.conn_handle = htole16(conn_handle); @@ -658,7 +672,7 @@ ble_hs_hci_read_iso_link_quality(uint16_t conn_handle, uint32_t *tx_unacked_pkts !tx_last_subev_pkts || !retransmitted_pkts || !crc_error_pkts || !rx_unreceived_pkts || !duplicate_pkts) { - return BLE_ERR_INV_HCI_CMD_PARMS; + return BLE_HS_EINVAL; } cmd.conn_handle = htole16(conn_handle); diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index 35fdbae7b..263f5d706 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/nimble/host/src/ble_hs_pvcy.c @@ -412,7 +412,7 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) ble_hs_resolv_enable(0); } - ble_hs_resolv_list_clear_all(); + ble_hs_resolv_list_clear_all(false); if (rpa_state) { ble_hs_resolv_enable(1); diff --git a/nimble/host/src/ble_hs_resolv.c b/nimble/host/src/ble_hs_resolv.c index d7b49d172..92d918af5 100644 --- a/nimble/host/src/ble_hs_resolv.c +++ b/nimble/host/src/ble_hs_resolv.c @@ -368,7 +368,7 @@ ble_host_rpa_enabled(void) static int ble_hs_rand_prand_get(uint8_t *prand) { - uint16_t sum; + uint32_t prand_rand; int rc; int retry = 100; @@ -379,14 +379,14 @@ ble_hs_rand_prand_get(uint8_t *prand) return rc; } - /* Prand cannot be all zeros or 1's. */ - sum = prand[0] + prand[1] + prand[2]; - if ((sum != 0) && (sum != (3 * 0xff))) { + /* The 22-bit random part of prand cannot be all zeros or all ones. */ + prand_rand = get_le24(prand) & 0x3fffff; + if (prand_rand != 0 && prand_rand != 0x3fffff) { break; } } - if (retry <= 0) { + if (retry < 0) { BLE_HS_LOG(ERROR, "Failed to generate random prand\n"); return BLE_HS_ETIMEOUT; } @@ -421,13 +421,15 @@ is_irk_nonzero(uint8_t *irk) * @param rl * @param local */ -static void +static int ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local) { uint8_t *irk = NULL; uint8_t *prand = NULL; struct ble_encryption_block ecb = {0}; uint8_t *addr = NULL; + uint8_t new_addr[BLE_DEV_ADDR_LEN]; + int rc; if (local) { addr = rl->rl_local_rpa; @@ -437,10 +439,13 @@ ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local) irk = rl->rl_peer_irk; } + memcpy(new_addr, addr, sizeof(new_addr)); + /* Get prand */ - prand = addr + 3; - if (ble_hs_rand_prand_get(prand) != 0) { - return; + prand = new_addr + 3; + rc = ble_hs_rand_prand_get(prand); + if (rc != 0) { + return rc; } /* Calculate hash, hash = ah(local IRK, prand) */ @@ -455,15 +460,18 @@ ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local) swap_in_place(ecb.plain_text, 16); /* Calculate hash */ - if (ble_sm_alg_encrypt(ecb.key, ecb.plain_text, ecb.cipher_text) != 0) { - /* We can't do much here if the encryption fails */ - return; + rc = ble_sm_alg_encrypt(ecb.key, ecb.plain_text, ecb.cipher_text); + if (rc != 0) { + return rc; } swap_in_place(ecb.cipher_text, 16); - addr[0] = ecb.cipher_text[15]; - addr[1] = ecb.cipher_text[14]; - addr[2] = ecb.cipher_text[13]; + new_addr[0] = ecb.cipher_text[15]; + new_addr[1] = ecb.cipher_text[14]; + new_addr[2] = ecb.cipher_text[13]; + memcpy(addr, new_addr, sizeof(new_addr)); + + return 0; } /* Called to generate private (RPA/NRPA) address and this address is set in controller as @@ -472,14 +480,27 @@ ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local) int ble_hs_gen_own_private_rnd(void) { + uint8_t addr[BLE_DEV_ADDR_LEN]; + int rc; + if (nrpa_pvcy) { return ble_hs_id_set_nrpa_rnd(); } struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[0]; - ble_hs_resolv_gen_priv_addr(rl, 1); - return ble_hs_id_set_pseudo_rnd(rl->rl_local_rpa); + ble_hs_lock(); + rc = ble_hs_resolv_gen_priv_addr(rl, 1); + if (rc == 0) { + memcpy(addr, rl->rl_local_rpa, sizeof(addr)); + } + ble_hs_unlock(); + + if (rc != 0) { + return rc; + } + + return ble_hs_id_set_pseudo_rnd(addr); } /* Called to fetch local RPA address */ @@ -626,8 +647,11 @@ ble_hs_resolv_list_add(uint8_t *cmdbuf) /* generate a local and peer RPAs now, those will be updated by timer * when resolution is enabled */ - ble_hs_resolv_gen_priv_addr(rl, 1); - ble_hs_resolv_gen_priv_addr(rl, 0); + if (ble_hs_resolv_gen_priv_addr(rl, 1) != 0 || + ble_hs_resolv_gen_priv_addr(rl, 0) != 0) { + memset(rl, 0, sizeof(*rl)); + return BLE_HS_EUNKNOWN; + } ++(g_ble_hs_resolv_data.rl_cnt); BLE_HS_LOG(DEBUG, "Device added to RL, Resolving list count = %d\n", g_ble_hs_resolv_data.rl_cnt); @@ -675,15 +699,17 @@ ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr) * Clear the resolving list */ void -ble_hs_resolv_list_clear_all(void) +ble_hs_resolv_list_clear_all(bool preserve_local) { 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) { + /* Index 0 is the local identity. Some callers clear only peer state, + * while local IRK replacement must clear it too. + */ + if (preserve_local && g_ble_hs_resolv_data.rl_cnt > 0) { memcpy(&local_entry, &g_ble_hs_resolv_list[0], sizeof(local_entry)); restore_local = true; } @@ -801,7 +827,6 @@ ble_hs_resolv_rpa_addr(uint8_t *addr, uint8_t addr_type) { for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) { if(ble_hs_resolv_rpa(addr, rl->rl_peer_irk) == 0) { memcpy(g_ble_hs_resolv_list[i].rl_peer_rpa, addr, BLE_DEV_ADDR_LEN); - g_ble_hs_resolv_list[i].rl_addr_type = addr_type; return rl; } diff --git a/nimble/host/src/ble_hs_resolv_priv.h b/nimble/host/src/ble_hs_resolv_priv.h index 370f5a7d5..ec66d422b 100644 --- a/nimble/host/src/ble_hs_resolv_priv.h +++ b/nimble/host/src/ble_hs_resolv_priv.h @@ -69,7 +69,7 @@ uint8_t *ble_hs_get_rpa_local(void); /* Remove a device from the resolving list */ int ble_hs_resolv_list_rmv(uint8_t, uint8_t *); /* Clear the resolving list and peer dev record */ -void ble_hs_resolv_list_clear_all(void); +void ble_hs_resolv_list_clear_all(bool preserve_local); /* Address resolution enable command */ void ble_hs_resolv_enable(bool); diff --git a/nimble/host/src/ble_ibeacon.c b/nimble/host/src/ble_ibeacon.c index 61f71e74c..40770dd62 100644 --- a/nimble/host/src/ble_ibeacon.c +++ b/nimble/host/src/ble_ibeacon.c @@ -44,7 +44,7 @@ ble_ibeacon_set_adv_data(const void *uuid128, uint16_t major, uint16_t minor, int8_t measured_power) { struct ble_hs_adv_fields fields; - static uint8_t buf[BLE_IBEACON_MFG_DATA_SIZE]; + uint8_t buf[BLE_IBEACON_MFG_DATA_SIZE]; int rc; /* Validate inputs before any buffer writes */ diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c index b31055ff4..3e031e455 100644 --- a/nimble/host/src/ble_l2cap_sig.c +++ b/nimble/host/src/ble_l2cap_sig.c @@ -1029,7 +1029,6 @@ ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle, uint16_t scid; uint16_t result; unsigned int num_of_scids; - uint8_t chan_created = 0; int i; uint8_t len; diff --git a/nimble/host/src/ble_sm_alg.c b/nimble/host/src/ble_sm_alg.c index 71773e0b5..6cb79550c 100644 --- a/nimble/host/src/ble_sm_alg.c +++ b/nimble/host/src/ble_sm_alg.c @@ -19,6 +19,7 @@ */ #include +#include #include #include "syscfg/syscfg.h" #include "nimble/nimble_opt.h" @@ -47,6 +48,16 @@ static struct trng_dev *g_trng; #endif +static void +ble_sm_alg_secure_zero(void *buf, size_t len) +{ + volatile uint8_t *p = buf; + + while (len--) { + *p++ = 0; + } +} + static void ble_sm_alg_xor_128(const uint8_t *p, const uint8_t *q, uint8_t *r) { @@ -64,6 +75,7 @@ ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, int ret = 0; struct mbedtls_aes_context ctx; uint8_t tmp[16]; + int rc = 0; swap_buf(tmp, key, 16); @@ -203,7 +215,12 @@ ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len, return BLE_HS_EUNKNOWN; } - return 0; + rc = 0; + +done: + ble_sm_alg_secure_zero(&state, sizeof(state)); + ble_sm_alg_secure_zero(&sched, sizeof(sched)); + return rc; } #if MYNEWT_VAL(BLE_SM_SC) @@ -211,9 +228,24 @@ ble_sm_alg_aes_cmac(const uint8_t *key, const uint8_t *in, size_t len, static void ble_sm_alg_log_buf(const char *name, const uint8_t *buf, int len) { +#ifdef ESP_PLATFORM + char str[1 + 2 * 80]; + int i; + int pos; + + if (len > 80) { + len = 80; + } + + for (i = 0, pos = 0; i < len; i++) { + pos += snprintf(str + pos, sizeof(str) - pos, "%02x", buf[i]); + } + BLE_HS_LOG(DEBUG, " %s=%s\n", name, str); +#else BLE_HS_LOG(DEBUG, " %s=", name); ble_hs_log_flat_buf(buf, len); BLE_HS_LOG(DEBUG, "\n"); +#endif } int diff --git a/nimble/host/src/ble_uuid.c b/nimble/host/src/ble_uuid.c index 44c78a59b..f0af93f06 100644 --- a/nimble/host/src/ble_uuid.c +++ b/nimble/host/src/ble_uuid.c @@ -21,6 +21,7 @@ #include #include #include +#include "os/os.h" #include "os/os_mbuf.h" #include "nimble/ble.h" #include "ble_hs_priv.h" @@ -217,12 +218,19 @@ ble_uuid_to_str(const ble_uuid_t *uuid, char *dst) static int ble_uuid_base_init(void) { + static const uint8_t uuid_base_template[16] = { + 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, + 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; uint8_t *temp; + os_sr_t sr; - /* Check again after potential memory barrier - double-checked locking pattern */ + OS_ENTER_CRITICAL(sr); if (ble_uuid_base != NULL) { + OS_EXIT_CRITICAL(sr); return 0; } + OS_EXIT_CRITICAL(sr); temp = nimble_platform_mem_calloc(1, sizeof(uint8_t) * 16); if (temp == NULL) { @@ -230,20 +238,16 @@ ble_uuid_base_init(void) return BLE_HS_ENOMEM; } - /* Initialize the base UUID before publishing the pointer */ - temp[0] = 0xfb; - temp[1] = 0x34; - temp[2] = 0x9b; - temp[3] = 0x5f; - temp[4] = 0x80; - temp[7] = 0x80; - temp[9] = 0x10; + memcpy(temp, uuid_base_template, sizeof(uuid_base_template)); - /* Only set the pointer if it's still NULL (handles race condition) */ + OS_ENTER_CRITICAL(sr); if (ble_uuid_base == NULL) { ble_uuid_base = temp; - } else { - /* Another thread beat us to it - free our allocation */ + temp = NULL; + } + OS_EXIT_CRITICAL(sr); + + if (temp != NULL) { nimble_platform_mem_free(temp); } @@ -277,8 +281,18 @@ ble_uuid_from_str(ble_uuid_any_t *uuid, const char *str) str_ptr = &str[len - 2]; if (len <= BLE_UUID16_STR_MAX_LEN) { + if (len == BLE_UUID16_STR_MAX_LEN && + (str[0] != '0' || (str[1] != 'x' && str[1] != 'X'))) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } uuid->u.type = BLE_UUID_TYPE_16; } else if (len <= BLE_UUID32_STR_MAX_LEN) { + if (len == BLE_UUID32_STR_MAX_LEN && + (str[0] != '0' || (str[1] != 'x' && str[1] != 'X'))) { + BLE_HS_LOG(ERROR, "%s rc=%d\n", __func__, BLE_HS_EINVAL); + return BLE_HS_EINVAL; + } uuid->u.type = BLE_UUID_TYPE_32; } else if (len <= BLE_UUID128_STR_MAX_LEN) { uuid->u.type = BLE_UUID_TYPE_128; @@ -535,9 +549,16 @@ ble_uuid_length(const ble_uuid_t *uuid) void ble_uuid_deinit(void) { - if (ble_uuid_base) { - nimble_platform_mem_free(ble_uuid_base); - ble_uuid_base = NULL; + uint8_t *temp; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + temp = ble_uuid_base; + ble_uuid_base = NULL; + OS_EXIT_CRITICAL(sr); + + if (temp) { + nimble_platform_mem_free(temp); } } #endif diff --git a/nimble/transport/src/monitor.c b/nimble/transport/src/monitor.c index 62be5b1e6..13c3850e9 100644 --- a/nimble/transport/src/monitor.c +++ b/nimble/transport/src/monitor.c @@ -504,9 +504,17 @@ ble_monitor_out(int c) int ble_transport_to_ll_cmd(void *buf) { - struct ble_hci_cmd *cmd = buf; + uint8_t *cmd_buf = buf; + struct ble_hci_cmd *cmd; - ble_monitor_send(BLE_MONITOR_OPCODE_COMMAND_PKT, buf, cmd->length + +#if !(SOC_ESP_NIMBLE_CONTROLLER) && CONFIG_BT_CONTROLLER_ENABLED + if (cmd_buf[0] == 0x01) { + cmd_buf++; + } +#endif + + cmd = (void *)cmd_buf; + ble_monitor_send(BLE_MONITOR_OPCODE_COMMAND_PKT, cmd_buf, cmd->length + sizeof(*cmd)); return ble_transport_to_ll_cmd_impl(buf); diff --git a/porting/nimble/src/os_cputime_pwr2.c b/porting/nimble/src/os_cputime_pwr2.c index 8f820a663..771090650 100644 --- a/porting/nimble/src/os_cputime_pwr2.c +++ b/porting/nimble/src/os_cputime_pwr2.c @@ -46,6 +46,9 @@ uint32_t os_cputime_usecs_to_ticks(uint32_t usecs) { +#if MYNEWT_VAL(OS_CPUTIME_FREQ) == 32000 + return ((uint64_t)usecs * MYNEWT_VAL(OS_CPUTIME_FREQ)) / 1000000; +#else uint64_t ticks; /* @@ -59,6 +62,7 @@ os_cputime_usecs_to_ticks(uint32_t usecs) //ticks += ((uint64_t)usecs * (1526122139+1)) >> 32; return ticks >> 32; +#endif } /** @@ -77,6 +81,9 @@ os_cputime_usecs_to_ticks(uint32_t usecs) uint32_t os_cputime_ticks_to_usecs(uint32_t ticks) { +#if MYNEWT_VAL(OS_CPUTIME_FREQ) == 32000 + return ((uint64_t)ticks * 1000000) / MYNEWT_VAL(OS_CPUTIME_FREQ); +#else uint32_t usecs; uint32_t shift; @@ -85,6 +92,7 @@ os_cputime_ticks_to_usecs(uint32_t ticks) usecs = ((ticks >> shift) * 15625) + (((ticks & ~(~0U << shift)) * 15625) >> shift); return usecs; +#endif } /**