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

fix: fix set pawr subev data exceeds 255

Browse Source
This commit is contained in:
cjin
2025-12-11 08:43:41 +08:00
parent 9551ac31af
commit 32e8ddac58
1 changed files with 42 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nimble/host/src/ble_gap.c
+42 -18
View File
@@ -5964,7 +5964,7 @@ ble_gap_calc_periodic_adv_data_size(uint8_t num_subevents,
const struct ble_gap_set_periodic_adv_subev_data_params *params) {
int len = 0;
const struct ble_gap_set_periodic_adv_subev_data_params *param;
int raw_subev_size = sizeof(*param) - sizeof(struct os_mbuf*);
uint8_t raw_subev_size = sizeof(struct periodic_adv_subevents);
for(int i = 0; i < num_subevents; i++) {
param = params + i;
@@ -5973,31 +5973,36 @@ ble_gap_calc_periodic_adv_data_size(uint8_t num_subevents,
}
return len;
}
int
ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents,
const struct ble_gap_set_periodic_adv_subev_data_params *params)
{
struct ble_hci_le_set_periodic_adv_subev_data_cp *cmd;
struct periodic_adv_subevents *subevents;
uint8_t len = ble_gap_calc_periodic_adv_data_size(num_subevents, params);
uint8_t buf[len + 2];
uint16_t cmd_len = ble_gap_calc_periodic_adv_data_size(num_subevents, params) + 2;
uint16_t opcode;
uint16_t subev_data_len;
uint8_t buf_size;
uint8_t param_size;
uint8_t buf_offset;
int rc;
/* Check if we can set all of data in one hci command. */
if (cmd_len >= 0xff) {
buf_size = 0xff;
} else {
buf_size = cmd_len;
}
uint8_t buf[buf_size];
if (instance >= BLE_ADV_INSTANCES) {
rc = BLE_HS_EINVAL;
goto done;
}
if (!ble_hs_is_enabled()) {
rc = BLE_HS_EDISABLED;
goto done;
}
/* Check if we can set all of data in one hci command. */
if ((len + 2) >= 0xff) {
rc = BLE_HS_EINVAL;
rc = BLE_HS_EDISABLED;
goto done;
}
@@ -6013,15 +6018,34 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents,
opcode = BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_PERIODIC_ADV_SUBEV_DATA);
cmd = (void *) buf;
param_size = sizeof(struct periodic_adv_subevents);
subevents = cmd->subevents;
cmd->adv_handle = instance;
cmd->num_subevents = num_subevents;
cmd->num_subevents = 0;
buf_offset = 0;
for (int i = 0; i < num_subevents; i++) {
subevents->subevent = params[i].subevent;
subevents->response_slot_start = params[i].response_slot_start;
subevents->response_slot_count = params[i].response_slot_count;
if ((buf_offset + OS_MBUF_PKTLEN(params[i].data) + param_size) > buf_size) {
if (!cmd->num_subevents) {
ble_hs_unlock();
rc = BLE_HS_EINVAL;
goto done;
} else {
rc = ble_hs_hci_cmd_tx(opcode, cmd, buf_offset + 2, NULL, 0);
if (rc != 0) {
ble_hs_unlock();
goto done;
}
subevents = cmd->subevents;
cmd->num_subevents = 0;
buf_offset = 0;
}
}
buf_offset += OS_MBUF_PKTLEN(params[i].data) + param_size;
subevents->subevent = params[i].subevent;
subevents->response_slot_start = params[i].response_slot_start;
subevents->response_slot_count = params[i].response_slot_count;
cmd->num_subevents ++;
ble_hs_mbuf_to_flat(params[i].data, subevents->subevent_data, OS_MBUF_PKTLEN(params[i].data),
(void*)&subev_data_len);
subevents->subevent_data_length = OS_MBUF_PKTLEN(params[i].data);
@@ -6029,7 +6053,7 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents,
(uint32_t)subevents + sizeof(struct periodic_adv_subevents) + subev_data_len);
}
rc = ble_hs_hci_cmd_tx(opcode, cmd, len + 2, NULL, 0);
rc = ble_hs_hci_cmd_tx(opcode, cmd, buf_offset + 2, NULL, 0);
if (rc != 0) {
ble_hs_unlock();
goto done;
@@ -6041,7 +6065,7 @@ ble_gap_set_periodic_adv_subev_data(uint8_t instance, uint8_t num_subevents,
done:
for (int i = 0; i < num_subevents; i++) {
os_mbuf_free_chain(params[i].data);
os_mbuf_free_chain(params[i].data);
}
return rc;
}