espressif/esp-nimble
1
0
Fork 0
mirror of https://github.com/espressif/esp-nimble.git synced 2026-07-29 22:27:52 +00:00
Code Issues Packages Projects Releases Wiki Activity

fix(nimble): Fix various issues for security/vulnerability in host

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