espressif/esp-nimble
1
0
Fork 0
mirror of https://github.com/espressif/esp-nimble.git synced 2026-10-01 21:17:21 +00:00
Code Issues Packages Projects Releases Wiki Activity

fix(nimble): Fixes to ble_hs_hci_evt.c

Browse Source
This commit is contained in:
Rahul Tank
2026-08-12 21:41:29 +05:30
parent 90da8f8339
commit 95ff74ce05
23 changed files with 902 additions and 338 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nimble/host/services/bas/src/ble_svc_bas.c
+15 -4
View File
@@ -18,6 +18,7 @@
*/
#include <assert.h>
#include <stdbool.h>
#include <string.h>
#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
nimble/host/services/hid/src/ble_svc_hid.c
+68 -41
View File
@@ -18,6 +18,7 @@
*/
#include <assert.h>
#include <stdint.h>
#include <string.h>
#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);
nimble/host/services/htp/include/services/htp/ble_svc_htp.h
+3
View File
@@ -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);
nimble/host/services/htp/src/ble_svc_htp.c
+134 -42
View File
@@ -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
nimble/host/services/prox/include/services/prox/ble_svc_prox.h
+6
View File
@@ -7,6 +7,8 @@
#ifndef H_BLE_SVC_PROX_
#define H_BLE_SVC_PROX_
#include <stdint.h>
#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
nimble/host/services/prox/src/ble_svc_prox.c
+129 -34
View File
@@ -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
nimble/host/src/ble_att_clt.c
+51 -9
View File
@@ -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
nimble/host/src/ble_gap.c
+1 -1
View File
@@ -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;
nimble/host/src/ble_gatts_lcl.c
+21
View File
@@ -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
nimble/host/src/ble_hs_hci.c
+27 -10
View File
@@ -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);
nimble/host/src/ble_hs_hci_cmd.c
+13
View File
@@ -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);
}
nimble/host/src/ble_hs_hci_evt.c
+235 -106
View File
@@ -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) {
nimble/host/src/ble_hs_hci_util.c
+3 -2
View File
@@ -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));
nimble/host/src/ble_hs_iso_hci.c
+58 -44
View File
@@ -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);
nimble/host/src/ble_hs_pvcy.c
+1 -1
View File
@@ -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);
nimble/host/src/ble_hs_resolv.c
+48 -23
View File
@@ -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;
}
nimble/host/src/ble_hs_resolv_priv.h
+1 -1
View File
@@ -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);
nimble/host/src/ble_ibeacon.c
+1 -1
View File
@@ -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 */
nimble/host/src/ble_l2cap_sig.c
-1
View File
@@ -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;
nimble/host/src/ble_sm_alg.c
+33 -1
View File
@@ -19,6 +19,7 @@
*/
#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#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
nimble/host/src/ble_uuid.c
+36 -15
View File
@@ -21,6 +21,7 @@
#include <stdio.h>
#include <string.h>
#include <errno.h>
#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
nimble/transport/src/monitor.c
+10 -2
View File
@@ -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);
porting/nimble/src/os_cputime_pwr2.c
+8
View File
@@ -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
}
/**