/* * Copyright (c) 2019 Oticon A/S * * SPDX-License-Identifier: Apache-2.0 */ /** * @brief HCI interface application */ #include #include #include #include #include #include #include #include #include #include #include "edtt_driver.h" #include "bs_tracing.h" #include "commands.h" #if defined(CONFIG_BT_HCI_CORE_LOG_LEVEL_DBG) #define LOG_LEVEL LOG_LEVEL_DBG #else #define LOG_LEVEL CONFIG_BT_LOG_LEVEL #endif #include LOG_MODULE_REGISTER(hci_test_app); static uint16_t waiting_opcode; static enum commands_t waiting_response; static uint8_t m_events; struct EdttIxit { uint8_t refMajor; uint8_t refMinor; uint16_t len; uint8_t *pVal; }; /*! \brief Implementation eXtra Information for Test (IXIT) definitions */ #if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS_MAX_NSE) const uint8_t TSPX_max_cis_nse = CONFIG_BT_CTLR_CONN_ISO_STREAMS_MAX_NSE; #endif /*! \brief Persistent LL IXIT values. */ static struct EdttIxit llIxits[] = { #if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS_MAX_NSE) {7, 14, 1, &TSPX_max_cis_nse}, #endif }; /** * @brief Clean out excess bytes from the input buffer */ static void read_excess_bytes(uint16_t size) { if (size > 0) { uint8_t buffer[size]; edtt_read((uint8_t *)buffer, size, EDTTT_BLOCK); LOG_ERR("command size wrong! (%u extra bytes removed)", size); } } /** * @brief Provide an error response when an HCI command send failed */ static void error_response(int error) { uint16_t response = sys_cpu_to_le16(waiting_response); int le_error = sys_cpu_to_le32(error); uint16_t size = sys_cpu_to_le16(sizeof(le_error)); edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)&le_error, sizeof(le_error), EDTTT_BLOCK); waiting_response = CMD_NOTHING; waiting_opcode = 0; } /** * @brief Allocate buffer for HCI command and fill in opCode for the command */ static struct net_buf *hci_cmd_create(uint16_t opcode, uint8_t param_len) { struct bt_hci_cmd_hdr *hdr; struct net_buf *buf; buf = bt_buf_get_tx(BT_BUF_CMD, K_FOREVER, NULL, 0); __ASSERT_NO_MSG(buf); hdr = net_buf_add(buf, sizeof(*hdr)); hdr->opcode = sys_cpu_to_le16(opcode); hdr->param_len = param_len; return buf; } /** * @brief Allocate buffer for ACL Data Package and fill in Header */ static struct net_buf *acl_data_create(struct bt_hci_acl_hdr *le_hdr) { struct net_buf *buf; struct bt_hci_acl_hdr *hdr; buf = bt_buf_get_tx(BT_BUF_ACL_OUT, K_FOREVER, NULL, 0); __ASSERT_NO_MSG(buf); hdr = net_buf_add(buf, sizeof(*hdr)); *hdr = *le_hdr; return buf; } #if defined(CONFIG_BT_ISO) /** * @brief Allocate buffer for ISO Data Package and fill in Header */ static struct net_buf *iso_data_create(struct bt_hci_iso_hdr *le_hdr) { struct net_buf *buf; struct bt_hci_iso_hdr *hdr; buf = bt_buf_get_tx(BT_BUF_ISO_OUT, K_FOREVER, NULL, 0); __ASSERT_NO_MSG(buf); hdr = net_buf_add(buf, sizeof(*hdr)); *hdr = *le_hdr; return buf; } #endif /* defined(CONFIG_BT_ISO) */ /** * @brief Allocate buffer for HCI command, fill in parameters and send the * command... */ static int send_hci_command(uint16_t opcode, uint8_t param_len, uint16_t response) { struct net_buf *buf; void *cp; int err = 0; waiting_response = response; buf = hci_cmd_create(waiting_opcode = opcode, param_len); if (buf) { if (param_len) { cp = net_buf_add(buf, param_len); edtt_read((uint8_t *)cp, param_len, EDTTT_BLOCK); } err = bt_send(buf); if (err) { LOG_ERR("Failed to send HCI command %d (err %d)", opcode, err); error_response(err); } } else { LOG_ERR("Failed to create buffer for HCI command 0x%04x", opcode); error_response(-1); } return err; } /** * @brief Echo function - echo input received... */ static void echo(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_ECHO_RSP); uint16_t le_size = sys_cpu_to_le16(size); edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&le_size, sizeof(le_size), EDTTT_BLOCK); if (size > 0) { uint8_t buff[size]; edtt_read(buff, size, EDTTT_BLOCK); edtt_write(buff, size, EDTTT_BLOCK); } } NET_BUF_POOL_FIXED_DEFINE(event_pool, 32, BT_BUF_RX_SIZE + 4, 4, NULL); static K_FIFO_DEFINE(event_queue); static K_FIFO_DEFINE(rx_queue); NET_BUF_POOL_FIXED_DEFINE(data_pool, CONFIG_BT_CTLR_RX_BUFFERS + 14, BT_BUF_ACL_SIZE(CONFIG_BT_BUF_ACL_TX_SIZE) + 4, 4, NULL); static K_FIFO_DEFINE(data_queue); #if defined(CONFIG_BT_ISO) NET_BUF_POOL_FIXED_DEFINE(iso_data_pool, CONFIG_BT_ISO_RX_BUF_COUNT + 14, BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_RX_MTU) + sizeof(uint32_t), 8, NULL); static K_FIFO_DEFINE(iso_data_queue); #endif /* CONFIG_BT_ISO */ /** * @brief Handle Command Complete HCI event... */ static void command_complete(struct net_buf *buf) { struct bt_hci_evt_cmd_complete *evt = (void *)buf->data; uint16_t opcode = sys_le16_to_cpu(evt->opcode); uint16_t response = sys_cpu_to_le16(waiting_response); struct net_buf_simple_state state; uint16_t size; net_buf_simple_save(&buf->b, &state); net_buf_pull(buf, sizeof(*evt)); size = sys_cpu_to_le16(buf->len); if (opcode == waiting_opcode) { LOG_DBG("Command complete for 0x%04x", waiting_opcode); edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK); waiting_opcode = 0; } else { LOG_WRN("Not waiting for 0x(%04x) command status," " expected 0x(%04x)", opcode, waiting_opcode); } net_buf_simple_restore(&buf->b, &state); } /** * @brief Handle Command Status HCI event... */ static void command_status(struct net_buf *buf) { struct bt_hci_evt_cmd_status *evt = (void *)buf->data; uint16_t opcode = sys_le16_to_cpu(evt->opcode); uint16_t response = sys_cpu_to_le16(waiting_response); struct net_buf_simple_state state; uint8_t status = evt->status; uint16_t size; net_buf_simple_save(&buf->b, &state); net_buf_pull(buf, sizeof(*evt)); size = sys_cpu_to_le16(buf->len) + 1; if (opcode == waiting_opcode) { LOG_DBG("Command status for 0x%04x", waiting_opcode); edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)&status, sizeof(status), EDTTT_BLOCK); edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK); waiting_opcode = 0; } else { LOG_WRN("Not waiting for 0x(%04x) command status," " expected 0x(%04x)", opcode, waiting_opcode); } net_buf_simple_restore(&buf->b, &state); } /** * @brief Remove an event from the event queue */ static void discard_event(void) { struct net_buf *buf = k_fifo_get(&event_queue, K_FOREVER); net_buf_unref(buf); m_events--; } /** * @brief Allocate and store an event in the event queue */ static struct net_buf *queue_event(struct net_buf *buf) { struct net_buf *evt; evt = net_buf_alloc(&event_pool, K_NO_WAIT); if (evt) { bt_buf_set_type(evt, BT_BUF_EVT); net_buf_add_le32(evt, sys_cpu_to_le32(k_uptime_get())); net_buf_add_mem(evt, buf->data, buf->len); k_fifo_put(&event_queue, evt); m_events++; } return evt; } /** * @brief Thread to service events and ACL/ISO data packets from the HCI input queue */ static void service_events(void *p1, void *p2, void *p3) { struct net_buf *buf, *evt; while (1) { buf = k_fifo_get(&rx_queue, K_FOREVER); if (bt_buf_get_type(buf) == BT_BUF_EVT) { evt = queue_event(buf); if (!evt) { bs_trace_raw_time(4, "Failed to allocated buffer " "for event!\n"); LOG_WRN("No event in queue"); } struct bt_hci_evt_hdr *hdr = (void *)buf->data; net_buf_pull(buf, sizeof(*hdr)); switch (hdr->evt) { case BT_HCI_EVT_CMD_COMPLETE: if (!evt) { discard_event(); evt = queue_event(buf); } command_complete(buf); break; case BT_HCI_EVT_CMD_STATUS: if (!evt) { discard_event(); evt = queue_event(buf); } command_status(buf); break; default: break; } } else if (bt_buf_get_type(buf) == BT_BUF_ACL_IN) { struct net_buf *data; data = net_buf_alloc(&data_pool, K_NO_WAIT); if (data) { bt_buf_set_type(data, BT_BUF_ACL_IN); net_buf_add_le32(data, sys_cpu_to_le32(k_uptime_get())); net_buf_add_mem(data, buf->data, buf->len); k_fifo_put(&data_queue, data); } #if defined(CONFIG_BT_ISO) } else if (bt_buf_get_type(buf) == BT_BUF_ISO_IN) { struct net_buf *data; data = net_buf_alloc(&iso_data_pool, K_NO_WAIT); if (data) { bt_buf_set_type(data, BT_BUF_ISO_IN); net_buf_add_le32(data, sys_cpu_to_le32(k_uptime_get())); net_buf_add_mem(data, buf->data, buf->len); k_fifo_put(&iso_data_queue, data); } #endif /* CONFIG_BT_ISO */ } net_buf_unref(buf); k_yield(); } } /** * @brief Flush all HCI events from the input-copy queue */ static void flush_events(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_FLUSH_EVENTS_RSP); struct net_buf *buf; while ((buf = k_fifo_get(&event_queue, K_NO_WAIT))) { net_buf_unref(buf); m_events--; } read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); } /** * @brief Get next available HCI event from the input-copy queue */ static void get_event(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_GET_EVENT_RSP); struct net_buf *buf; read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); buf = k_fifo_get(&event_queue, K_FOREVER); if (buf) { size = sys_cpu_to_le16(buf->len); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK); net_buf_unref(buf); m_events--; } else { edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); } } /** * @brief Get next available HCI events from the input-copy queue */ static void get_events(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_GET_EVENT_RSP); struct net_buf *buf; uint8_t count = m_events; read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&count, sizeof(count), EDTTT_BLOCK); while (count--) { buf = k_fifo_get(&event_queue, K_FOREVER); size = sys_cpu_to_le16(buf->len); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK); net_buf_unref(buf); m_events--; } } /** * @brief Check whether an HCI event is available in the input-copy queue */ static void has_event(uint16_t size) { struct has_event_resp { uint16_t response; uint16_t size; uint8_t count; } __packed; struct has_event_resp le_response = { .response = sys_cpu_to_le16(CMD_HAS_EVENT_RSP), .size = sys_cpu_to_le16(1), .count = m_events }; if (size > 0) { read_excess_bytes(size); } edtt_write((uint8_t *)&le_response, sizeof(le_response), EDTTT_BLOCK); } /** * @brief Flush all ACL Data Packages from the input-copy queue */ static void le_flush_data(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_LE_FLUSH_DATA_RSP); struct net_buf *buf; while ((buf = k_fifo_get(&data_queue, K_NO_WAIT))) { net_buf_unref(buf); } read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); } /** * @brief Check whether an ACL Data Package is available in the input-copy queue */ static void le_data_ready(uint16_t size) { struct has_data_resp { uint16_t response; uint16_t size; uint8_t empty; } __packed; struct has_data_resp le_response = { .response = sys_cpu_to_le16(CMD_LE_DATA_READY_RSP), .size = sys_cpu_to_le16(1), .empty = 0 }; if (size > 0) { read_excess_bytes(size); } if (k_fifo_is_empty(&data_queue)) { le_response.empty = 1; } edtt_write((uint8_t *)&le_response, sizeof(le_response), EDTTT_BLOCK); } /** * @brief Get next available HCI Data Package from the input-copy queue */ static void le_data_read(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_LE_DATA_READ_RSP); struct net_buf *buf; read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); buf = k_fifo_get(&data_queue, K_FOREVER); if (buf) { size = sys_cpu_to_le16(buf->len); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK); net_buf_unref(buf); } else { edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); } } /** * @brief Write ACL Data Package to the Controller... */ static void le_data_write(uint16_t size) { struct data_write_resp { uint16_t code; uint16_t size; uint8_t status; } __packed; struct data_write_resp response = { .code = sys_cpu_to_le16(CMD_LE_DATA_WRITE_RSP), .size = sys_cpu_to_le16(1), .status = 0 }; struct net_buf *buf; struct bt_hci_acl_hdr hdr; int err; if (size >= sizeof(hdr)) { edtt_read((uint8_t *)&hdr, sizeof(hdr), EDTTT_BLOCK); size -= sizeof(hdr); buf = acl_data_create(&hdr); if (buf) { uint16_t hdr_length = sys_le16_to_cpu(hdr.len); uint8_t *pdata = net_buf_add(buf, hdr_length); if (size >= hdr_length) { edtt_read(pdata, hdr_length, EDTTT_BLOCK); size -= hdr_length; } err = bt_send(buf); if (err) { LOG_ERR("Failed to send ACL Data (err %d)", err); } } else { err = -2; /* Failed to allocate data buffer */ LOG_ERR("Failed to create buffer for ACL Data."); } } else { /* Size too small for header (handle and data length) */ err = -3; } read_excess_bytes(size); response.status = sys_cpu_to_le32(err); edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); } #if defined(CONFIG_BT_ISO) /** * @brief Flush all ISO Data Packages from the input-copy queue */ static void le_flush_iso_data(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_LE_FLUSH_ISO_DATA_RSP); struct net_buf *buf; while ((buf = k_fifo_get(&iso_data_queue, K_NO_WAIT))) { net_buf_unref(buf); } read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); } /** * @brief Check whether an ISO Data Package is available in the input-copy queue */ static void le_iso_data_ready(uint16_t size) { struct has_iso_data_resp { uint16_t response; uint16_t size; uint8_t empty; } __packed; struct has_iso_data_resp le_response = { .response = sys_cpu_to_le16(CMD_LE_ISO_DATA_READY_RSP), .size = sys_cpu_to_le16(1), .empty = 0 }; if (size > 0) { read_excess_bytes(size); } if (k_fifo_is_empty(&iso_data_queue)) { le_response.empty = 1; } edtt_write((uint8_t *)&le_response, sizeof(le_response), EDTTT_BLOCK); } /** * @brief Get next available ISO Data Package from the input-copy queue */ static void le_iso_data_read(uint16_t size) { uint16_t response = sys_cpu_to_le16(CMD_LE_ISO_DATA_READ_RSP); struct net_buf *buf; read_excess_bytes(size); size = 0; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); buf = k_fifo_get(&iso_data_queue, K_FOREVER); if (buf) { size = sys_cpu_to_le16(buf->len); edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK); net_buf_unref(buf); } else { edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); } } /** * @brief Write ISO Data Package to the Controller... */ static void le_iso_data_write(uint16_t size) { struct iso_data_write_resp { uint16_t code; uint16_t size; uint8_t status; } __packed; struct iso_data_write_resp response = { .code = sys_cpu_to_le16(CMD_LE_ISO_DATA_WRITE_RSP), .size = sys_cpu_to_le16(1), .status = 0 }; struct net_buf *buf; struct bt_hci_iso_hdr hdr; int err; if (size >= sizeof(hdr)) { edtt_read((uint8_t *)&hdr, sizeof(hdr), EDTTT_BLOCK); size -= sizeof(hdr); buf = iso_data_create(&hdr); if (buf) { uint16_t hdr_length = sys_le16_to_cpu(hdr.len); uint8_t *pdata = net_buf_add(buf, hdr_length); if (size >= hdr_length) { edtt_read(pdata, hdr_length, EDTTT_BLOCK); size -= hdr_length; } err = bt_send(buf); if (err) { LOG_ERR("Failed to send ISO Data (err %d)", err); } } else { err = -2; /* Failed to allocate data buffer */ LOG_ERR("Failed to create buffer for ISO Data."); } } else { /* Size too small for header (handle and data length) */ err = -3; } read_excess_bytes(size); response.status = sys_cpu_to_le32(err); edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); } #endif /* CONFIG_BT_ISO */ /** * @brief Read 'Implementation eXtra Information for Test' value */ static void le_ixit_value_read(uint16_t size) { uint8_t profileId; uint8_t refMajor; uint8_t refMinor; struct EdttIxit *pIxitArray; int ixitArraySize; struct EdttIxit *pIxitElement = NULL; /* * CMD_GET_IXIT_VALUE_REQ payload layout * * ... * [ 4] PROFILE_ID[0] * [ 5] IXIT_Reference_Major * [ 6] IXIT_Reference_Minor */ edtt_read((uint8_t *)&profileId, sizeof(profileId), EDTTT_BLOCK); edtt_read((uint8_t *)&refMajor, sizeof(refMajor), EDTTT_BLOCK); edtt_read((uint8_t *)&refMinor, sizeof(refMinor), EDTTT_BLOCK); switch (profileId) { case PROFILE_ID_LL: pIxitArray = llIxits; ixitArraySize = ARRAY_SIZE(llIxits); break; default: pIxitArray = NULL; ixitArraySize = 0; } for (int i = 0; i < ixitArraySize; i++) { if (pIxitArray[i].refMajor == refMajor && pIxitArray[i].refMinor == refMinor) { pIxitElement = &pIxitArray[i]; break; } } struct ixit_value_get_resp { uint16_t code; uint16_t size; uint8_t data[]; } __packed; struct ixit_value_get_resp response = { .code = sys_cpu_to_le16(CMD_GET_IXIT_VALUE_RSP), .size = sys_cpu_to_le16(pIxitElement ? pIxitElement->len : 0), }; edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK); if (pIxitElement) { edtt_write(pIxitElement->pVal, pIxitElement->len, EDTTT_BLOCK); } } static K_THREAD_STACK_DEFINE(service_events_stack, CONFIG_BT_HCI_TX_STACK_SIZE); static struct k_thread service_events_data; /** * @brief Zephyr application main entry... */ int main(void) { int err; uint16_t command; uint16_t size; uint16_t opcode; /** * Initialize HCI command opcode and response variables... */ waiting_opcode = 0; waiting_response = CMD_NOTHING; m_events = 0; /** * Initialize Bluetooth stack in raw mode... */ err = bt_enable_raw(&rx_queue); if (err) { LOG_ERR("Bluetooth initialization failed (err %d)", err); return 0; } /** * Initialize and start EDTT system... */ #if defined(CONFIG_ARCH_POSIX) enable_edtt_mode(); set_edtt_autoshutdown(true); #endif edtt_start(); /** * Initialize and start thread to service HCI events and ACL data... */ k_thread_create(&service_events_data, service_events_stack, K_THREAD_STACK_SIZEOF(service_events_stack), service_events, NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT); while (1) { /** * Wait for a command to arrive - then read and execute command */ edtt_read((uint8_t *)&command, sizeof(command), EDTTT_BLOCK); command = sys_le16_to_cpu(command); edtt_read((uint8_t *)&size, sizeof(size), EDTTT_BLOCK); size = sys_le16_to_cpu(size); bs_trace_raw_time(4, "command 0x%04X received (size %u) " "events=%u\n", command, size, m_events); switch (command) { case CMD_ECHO_REQ: echo(size); break; case CMD_FLUSH_EVENTS_REQ: flush_events(size); break; case CMD_HAS_EVENT_REQ: has_event(size); break; case CMD_GET_EVENT_REQ: { uint8_t multiple; edtt_read((uint8_t *)&multiple, sizeof(multiple), EDTTT_BLOCK); if (multiple) { get_events(--size); } else { get_event(--size); } } break; case CMD_LE_FLUSH_DATA_REQ: le_flush_data(size); break; case CMD_LE_DATA_READY_REQ: le_data_ready(size); break; case CMD_LE_DATA_WRITE_REQ: le_data_write(size); break; case CMD_LE_DATA_READ_REQ: le_data_read(size); break; #if defined(CONFIG_BT_ISO) case CMD_LE_FLUSH_ISO_DATA_REQ: le_flush_iso_data(size); break; case CMD_LE_ISO_DATA_READY_REQ: le_iso_data_ready(size); break; case CMD_LE_ISO_DATA_WRITE_REQ: le_iso_data_write(size); break; case CMD_LE_ISO_DATA_READ_REQ: le_iso_data_read(size); break; #endif /* CONFIG_BT_ISO */ case CMD_GET_IXIT_VALUE_REQ: le_ixit_value_read(size); break; default: if (size >= 2) { edtt_read((uint8_t *)&opcode, sizeof(opcode), EDTTT_BLOCK); send_hci_command(sys_le16_to_cpu(opcode), size - 2, command + 1); } } } return 0; }