diff --git a/subsys/bluetooth/host/hci_core.c b/subsys/bluetooth/host/hci_core.c index f96b6876b7a42..50a413b0657c9 100644 --- a/subsys/bluetooth/host/hci_core.c +++ b/subsys/bluetooth/host/hci_core.c @@ -116,6 +116,9 @@ struct cmd_data { /** Used by bt_hci_cmd_send_sync. */ struct k_sem *sync; + + /** Response buffer. */ + struct net_buf **rsp; }; static struct cmd_data cmd_data[CONFIG_BT_BUF_CMD_TX_COUNT]; @@ -302,6 +305,7 @@ struct net_buf *bt_hci_cmd_create(uint16_t opcode, uint8_t param_len) cmd(buf)->opcode = opcode; cmd(buf)->sync = NULL; cmd(buf)->state = NULL; + cmd(buf)->rsp = NULL; hdr = net_buf_add(buf, sizeof(*hdr)); hdr->opcode = sys_cpu_to_le16(opcode); @@ -365,6 +369,7 @@ int bt_hci_cmd_send_sync(uint16_t opcode, struct net_buf *buf, */ k_sem_init(&sync_sem, 0, 1); cmd(buf)->sync = &sync_sem; + cmd(buf)->rsp = rsp; net_buf_put(&bt_dev.cmd_tx_queue, net_buf_ref(buf)); bt_tx_irq_raise(); @@ -423,13 +428,9 @@ int bt_hci_cmd_send_sync(uint16_t opcode, struct net_buf *buf, } } - LOG_DBG("rsp %p opcode 0x%04x len %u", buf, opcode, buf->len); + LOG_DBG("opcode 0x%04x completed", opcode); - if (rsp) { - *rsp = buf; - } else { - net_buf_unref(buf); - } + net_buf_unref(buf); return 0; } @@ -2404,12 +2405,13 @@ static void hci_reset_complete(struct net_buf *buf) atomic_set(bt_dev.flags, flags); } -static void hci_cmd_done(uint16_t opcode, uint8_t status, struct net_buf *evt_buf) +static void hci_cmd_done(uint16_t opcode, uint8_t status, struct net_buf *buf) { /* Original command buffer. */ - struct net_buf *buf = NULL; + struct net_buf *sent_cmd = NULL; + struct k_sem *sync; - LOG_DBG("opcode 0x%04x status 0x%02x buf %p", opcode, status, evt_buf); + LOG_DBG("opcode 0x%04x status 0x%02x rsp %p", opcode, status, buf); /* Unsolicited cmd complete. This does not complete a command. * The controller can send these for effect of the `ncmd` field. @@ -2419,47 +2421,45 @@ static void hci_cmd_done(uint16_t opcode, uint8_t status, struct net_buf *evt_bu } /* Take the original command buffer reference. */ - buf = atomic_ptr_clear((atomic_ptr_t *)&bt_dev.sent_cmd); + sent_cmd = atomic_ptr_clear((atomic_ptr_t *)&bt_dev.sent_cmd); - if (!buf) { + if (!sent_cmd) { LOG_ERR("No command sent for cmd complete 0x%04x", opcode); goto exit; } - if (cmd(buf)->opcode != opcode) { + if (cmd(sent_cmd)->opcode != opcode) { LOG_ERR("OpCode 0x%04x completed instead of expected 0x%04x", opcode, - cmd(buf)->opcode); - buf = atomic_ptr_set((atomic_ptr_t *)&bt_dev.sent_cmd, buf); - __ASSERT_NO_MSG(!buf); + cmd(sent_cmd)->opcode); + sent_cmd = atomic_ptr_set((atomic_ptr_t *)&bt_dev.sent_cmd, sent_cmd); + __ASSERT_NO_MSG(!sent_cmd); goto exit; } - /* Response data is to be delivered in the original command - * buffer. - */ - if (evt_buf != buf) { - net_buf_reset(buf); - bt_buf_set_type(buf, BT_BUF_EVT); - net_buf_reserve(buf, BT_BUF_RESERVE); - net_buf_add_mem(buf, evt_buf->data, evt_buf->len); - } - - if (cmd(buf)->state && !status) { - struct bt_hci_cmd_state_set *update = cmd(buf)->state; + if (cmd(sent_cmd)->state && !status) { + struct bt_hci_cmd_state_set *update = cmd(sent_cmd)->state; atomic_set_bit_to(update->target, update->bit, update->val); } /* If the command was synchronous wake up bt_hci_cmd_send_sync() */ - if (cmd(buf)->sync) { + sync = cmd(sent_cmd)->sync; + if (sync) { LOG_DBG("sync cmd released"); - cmd(buf)->status = status; - k_sem_give(cmd(buf)->sync); + cmd(sent_cmd)->status = status; + + /* If status is success and rsp not NULL */ + if (!status && cmd(sent_cmd)->rsp) { + *(cmd(sent_cmd)->rsp) = net_buf_ref(buf); + } + + cmd(sent_cmd)->sync = NULL; + k_sem_give(sync); } exit: - if (buf) { - net_buf_unref(buf); + if (sent_cmd) { + net_buf_unref(sent_cmd); } }