summaryrefslogtreecommitdiff
path: root/emulator
diff options
context:
space:
mode:
Diffstat (limited to 'emulator')
-rw-r--r--emulator/amp.c1056
-rw-r--r--emulator/amp.h32
-rw-r--r--emulator/b1ee.c256
-rw-r--r--emulator/btdev.c1296
-rw-r--r--emulator/btdev.h77
-rw-r--r--emulator/bthost.c1111
-rw-r--r--emulator/bthost.h69
-rw-r--r--emulator/main.c146
-rw-r--r--emulator/server.c110
-rw-r--r--emulator/server.h11
-rw-r--r--emulator/vhci.c49
-rw-r--r--emulator/vhci.h8
12 files changed, 4066 insertions, 155 deletions
diff --git a/emulator/amp.c b/emulator/amp.c
new file mode 100644
index 00000000..714854b7
--- /dev/null
+++ b/emulator/amp.c
@@ -0,0 +1,1056 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011-2012 Intel Corporation
+ * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+
+#include "monitor/mainloop.h"
+#include "monitor/bt.h"
+
+#include "amp.h"
+
+#define le16_to_cpu(val) (val)
+#define le32_to_cpu(val) (val)
+#define cpu_to_le16(val) (val)
+#define cpu_to_le32(val) (val)
+
+#define PHY_MODE_IDLE 0x00
+#define PHY_MODE_INITIATOR 0x01
+#define PHY_MODE_ACCEPTOR 0x02
+
+#define MAX_ASSOC_LEN 672
+
+struct bt_amp {
+ volatile int ref_count;
+ int vhci_fd;
+
+ char phylink_path[32];
+ int phylink_fd;
+
+ uint8_t event_mask[16];
+ uint16_t manufacturer;
+ uint8_t commands[64];
+ uint8_t features[8];
+
+ uint8_t amp_status;
+ uint8_t amp_type;
+ uint8_t local_assoc[MAX_ASSOC_LEN];
+ uint16_t local_assoc_len;
+ uint8_t remote_assoc[MAX_ASSOC_LEN];
+ uint16_t remote_assoc_len;
+
+ uint8_t phy_mode;
+ uint8_t phy_handle;
+ uint16_t logic_handle;
+};
+
+static void reset_defaults(struct bt_amp *amp)
+{
+ memset(amp->event_mask, 0, sizeof(amp->event_mask));
+ amp->event_mask[1] |= 0x20; /* Command Complete */
+ amp->event_mask[1] |= 0x40; /* Command Status */
+ amp->event_mask[1] |= 0x80; /* Hardware Error */
+ amp->event_mask[2] |= 0x01; /* Flush Occurred */
+ amp->event_mask[2] |= 0x04; /* Number of Completed Packets */
+ amp->event_mask[3] |= 0x02; /* Data Buffer Overflow */
+ amp->event_mask[3] |= 0x20; /* QoS Violation */
+ amp->event_mask[7] |= 0x01; /* Enhanced Flush Complete */
+
+ amp->event_mask[8] |= 0x01; /* Physical Link Complete */
+ amp->event_mask[8] |= 0x02; /* Channel Selected */
+ amp->event_mask[8] |= 0x04; /* Disconnection Physical Link Complete */
+ amp->event_mask[8] |= 0x08; /* Physical Link Loss Early Warning */
+ amp->event_mask[8] |= 0x10; /* Physical Link Recovery */
+ amp->event_mask[8] |= 0x20; /* Logical Link Complete */
+ amp->event_mask[8] |= 0x40; /* Disconection Logical Link Complete */
+ amp->event_mask[8] |= 0x80; /* Flow Specification Modify Complete */
+ amp->event_mask[9] |= 0x01; /* Number of Completed Data Blocks */
+ amp->event_mask[9] |= 0x02; /* AMP Start Test */
+ amp->event_mask[9] |= 0x04; /* AMP Test End */
+ amp->event_mask[9] |= 0x08; /* AMP Receiver Report */
+ amp->event_mask[9] |= 0x10; /* Short Range Mode Change Complete */
+ amp->event_mask[9] |= 0x20; /* AMP Status Change */
+
+ amp->manufacturer = 0x003f; /* Bluetooth SIG (63) */
+
+ memset(amp->commands, 0, sizeof(amp->commands));
+ amp->commands[5] |= 0x40; /* Set Event Mask */
+ amp->commands[5] |= 0x80; /* Reset */
+ //amp->commands[6] |= 0x01; /* Set Event Filter */
+ //amp->commands[7] |= 0x04; /* Read Connection Accept Timeout */
+ //amp->commands[7] |= 0x08; /* Write Connection Accept Timeout */
+ //amp->commands[10] |= 0x80; /* Host Number of Completed Packets */
+ //amp->commands[11] |= 0x01; /* Read Link Supervision Timeout */
+ //amp->commands[11] |= 0x02; /* Write Link Supervision Timeout */
+ amp->commands[14] |= 0x08; /* Read Local Version Information */
+ amp->commands[14] |= 0x10; /* Read Local Supported Commands */
+ amp->commands[14] |= 0x20; /* Read Local Supported Features */
+ amp->commands[14] |= 0x80; /* Read Buffer Size */
+ //amp->commands[15] |= 0x04; /* Read Failed Contact Counter */
+ //amp->commands[15] |= 0x08; /* Reset Failed Contact Counter */
+ //amp->commands[15] |= 0x10; /* Read Link Quality */
+ //amp->commands[15] |= 0x20; /* Read RSSI */
+ //amp->commands[16] |= 0x04; /* Enable Device Under Test Mode */
+ //amp->commands[19] |= 0x40; /* Enhanced Flush */
+
+ amp->commands[21] |= 0x01; /* Create Physical Link */
+ amp->commands[21] |= 0x02; /* Accept Physical Link */
+ amp->commands[21] |= 0x04; /* Disconnect Phyiscal Link */
+ amp->commands[21] |= 0x08; /* Create Logical Link */
+ amp->commands[21] |= 0x10; /* Accept Logical Link */
+ amp->commands[21] |= 0x20; /* Disconnect Logical Link */
+ amp->commands[21] |= 0x40; /* Logical Link Cancel */
+ //amp->commands[21] |= 0x80; /* Flow Specification Modify */
+ //amp->commands[22] |= 0x01; /* Read Logical Link Accept Timeout */
+ //amp->commands[22] |= 0x02; /* Write Logical Link Accept Timeout */
+ amp->commands[22] |= 0x04; /* Set Event Mask Page 2 */
+ amp->commands[22] |= 0x08; /* Read Location Data */
+ amp->commands[22] |= 0x10; /* Write Location Data */
+ amp->commands[22] |= 0x20; /* Read Local AMP Info */
+ amp->commands[22] |= 0x40; /* Read Local AMP ASSOC */
+ amp->commands[22] |= 0x80; /* Write Remote AMP ASSOC */
+ amp->commands[23] |= 0x01; /* Read Flow Control Mode */
+ amp->commands[23] |= 0x02; /* Write Flow Control Mode */
+ amp->commands[23] |= 0x04; /* Read Data Block Size */
+ //amp->commands[23] |= 0x20; /* Enable AMP Receiver Reports */
+ //amp->commands[23] |= 0x40; /* AMP Test End */
+ //amp->commands[23] |= 0x80; /* AMP Test */
+ //amp->commands[24] |= 0x04; /* Read Best Effort Flush Timeout */
+ //amp->commands[24] |= 0x08; /* Write Best Effort Flush Timeout */
+ //amp->commands[24] |= 0x10; /* Short Range Mode */
+
+ memset(amp->features, 0, sizeof(amp->features));
+
+ amp->amp_status = 0x01; /* Used for Bluetooth only */
+ amp->amp_type = 0x42; /* Fake virtual AMP type */
+
+ memset(amp->local_assoc, 0, sizeof(amp->local_assoc));
+ amp->local_assoc_len = 0;
+
+ memset(amp->remote_assoc, 0, sizeof(amp->remote_assoc));
+ amp->remote_assoc_len = 0;
+
+ amp->phy_mode = PHY_MODE_IDLE;
+ amp->phy_handle = 0x00; /* Invalid physical link handle */
+ amp->logic_handle = 0x0000;
+}
+
+static void send_packet(struct bt_amp *amp, const void *data, uint16_t len)
+{
+ if (write(amp->vhci_fd, data, len) < 0)
+ fprintf(stderr, "Write to /dev/vhci failed\n");
+}
+
+static void send_event(struct bt_amp *amp, uint8_t event,
+ const void *data, uint8_t len)
+{
+ struct bt_hci_evt_hdr *hdr;
+ uint16_t pkt_len;
+ void *pkt_data;
+
+ pkt_len = 1 + sizeof(*hdr) + len;
+
+ pkt_data = alloca(pkt_len);
+ if (!pkt_data)
+ return;
+
+ ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
+
+ hdr = pkt_data + 1;
+ hdr->evt = event;
+ hdr->plen = len;
+
+ if (len > 0)
+ memcpy(pkt_data + 1 + sizeof(*hdr), data, len);
+
+ send_packet(amp, pkt_data, pkt_len);
+}
+
+static void cmd_complete(struct bt_amp *amp, uint16_t opcode,
+ const void *data, uint8_t len)
+{
+ struct bt_hci_evt_hdr *hdr;
+ struct bt_hci_evt_cmd_complete *cc;
+ uint16_t pkt_len;
+ void *pkt_data;
+
+ pkt_len = 1 + sizeof(*hdr) + sizeof(*cc) + len;
+
+ pkt_data = alloca(pkt_len);
+ if (!pkt_data)
+ return;
+
+ ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
+
+ hdr = pkt_data + 1;
+ hdr->evt = BT_HCI_EVT_CMD_COMPLETE;
+ hdr->plen = sizeof(*cc) + len;
+
+ cc = pkt_data + 1 + sizeof(*hdr);
+ cc->ncmd = 0x01;
+ cc->opcode = cpu_to_le16(opcode);
+
+ if (len > 0)
+ memcpy(pkt_data + 1 + sizeof(*hdr) + sizeof(*cc), data, len);
+
+ send_packet(amp, pkt_data, pkt_len);
+}
+
+static void cmd_status(struct bt_amp *amp, uint8_t status, uint16_t opcode)
+{
+ struct bt_hci_evt_hdr *hdr;
+ struct bt_hci_evt_cmd_status *cs;
+ uint16_t pkt_len;
+ void *pkt_data;
+
+ pkt_len = 1 + sizeof(*hdr) + sizeof(*cs);
+
+ pkt_data = alloca(pkt_len);
+ if (!pkt_data)
+ return;
+
+ ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
+
+ hdr = pkt_data + 1;
+ hdr->evt = BT_HCI_EVT_CMD_STATUS;
+ hdr->plen = sizeof(*cs);
+
+ cs = pkt_data + 1 + sizeof(*hdr);
+ cs->status = status;
+ cs->ncmd = 0x01;
+ cs->opcode = cpu_to_le16(opcode);
+
+ send_packet(amp, pkt_data, pkt_len);
+}
+
+static void cmd_set_event_mask(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_set_event_mask *cmd = data;
+ uint8_t status;
+
+ memcpy(amp->event_mask, cmd->mask, 8);
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(amp, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
+}
+
+static void cmd_reset(struct bt_amp *amp, const void *data, uint8_t size)
+{
+ uint8_t status;
+
+ reset_defaults(amp);
+
+ amp->local_assoc[0] = 0x00;
+ amp->local_assoc_len = 1;
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(amp, BT_HCI_CMD_RESET, &status, sizeof(status));
+}
+
+static void cmd_read_local_version(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_local_version rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.hci_ver = 0x05;
+ rsp.hci_rev = cpu_to_le16(0x0000);
+ rsp.lmp_ver = 0x01;
+ rsp.manufacturer = cpu_to_le16(amp->manufacturer);
+ rsp.lmp_subver = cpu_to_le16(0x0000);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
+}
+
+static void cmd_read_local_commands(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_local_commands rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ memcpy(rsp.commands, amp->commands, 64);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
+}
+
+static void cmd_read_local_features(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_local_features rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ memcpy(rsp.features, amp->features, 8);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
+}
+
+static void cmd_read_buffer_size(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_buffer_size rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.acl_mtu = cpu_to_le16(0x0000);
+ rsp.sco_mtu = 0x00;
+ rsp.acl_max_pkt = cpu_to_le16(0x0000);
+ rsp.sco_max_pkt = cpu_to_le16(0x0000);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
+}
+
+static void evt_phy_link_complete(struct bt_amp *amp)
+{
+ struct bt_hci_evt_phy_link_complete evt;
+
+ evt.status = BT_HCI_ERR_SUCCESS;
+ evt.phy_handle = amp->phy_handle;
+
+ send_event(amp, BT_HCI_EVT_PHY_LINK_COMPLETE, &evt, sizeof(evt));
+}
+
+static void evt_disconn_phy_link_complete(struct bt_amp *amp, uint8_t reason)
+{
+ struct bt_hci_evt_disconn_phy_link_complete evt;
+
+ evt.status = BT_HCI_ERR_SUCCESS;
+ evt.phy_handle = amp->phy_handle;
+ evt.reason = reason;
+
+ send_event(amp, BT_HCI_EVT_DISCONN_PHY_LINK_COMPLETE,
+ &evt, sizeof(evt));
+}
+
+static void link_callback(int fd, uint32_t events, void *user_data)
+{
+ struct bt_amp *amp = user_data;
+
+ if (events & (EPOLLERR | EPOLLHUP)) {
+ close(fd);
+ mainloop_remove_fd(fd);
+
+ evt_disconn_phy_link_complete(amp, 0x13);
+
+ amp->phy_mode = PHY_MODE_IDLE;
+ amp->phy_handle = 0x00;
+ return;
+ }
+}
+
+static void cmd_create_phy_link(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_create_phy_link *cmd = data;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_CREATE_PHY_LINK);
+ return;
+ }
+
+ if (amp->phy_mode != PHY_MODE_IDLE) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_CREATE_PHY_LINK);
+ return;
+ }
+
+ amp->phy_mode = PHY_MODE_INITIATOR;
+ amp->phy_handle = cmd->phy_handle;
+
+ cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_CREATE_PHY_LINK);
+}
+
+static void cmd_accept_phy_link(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_accept_phy_link *cmd = data;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_ACCEPT_PHY_LINK);
+ return;
+ }
+
+ if (amp->phy_mode != PHY_MODE_IDLE) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_ACCEPT_PHY_LINK);
+ return;
+ }
+
+ amp->phy_mode = PHY_MODE_ACCEPTOR;
+ amp->phy_handle = cmd->phy_handle;
+
+ cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_ACCEPT_PHY_LINK);
+}
+
+static void cmd_disconn_phy_link(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_disconn_phy_link *cmd = data;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_DISCONN_PHY_LINK);
+ return;
+ }
+
+ if (amp->phy_mode == PHY_MODE_IDLE) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_DISCONN_PHY_LINK);
+ return;
+ }
+
+ if (cmd->phy_handle != amp->phy_handle) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_DISCONN_PHY_LINK);
+ return;
+ }
+
+ cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_DISCONN_PHY_LINK);
+
+ mainloop_remove_fd(amp->phylink_fd);
+ close(amp->phylink_fd);
+
+ evt_disconn_phy_link_complete(amp, cmd->reason);
+
+ amp->phy_mode = PHY_MODE_IDLE;
+ amp->phy_handle = 0x00;
+}
+
+static void evt_logic_link_complete(struct bt_amp *amp)
+{
+ struct bt_hci_evt_logic_link_complete evt;
+
+ evt.status = BT_HCI_ERR_SUCCESS;
+ evt.handle = htobs(amp->logic_handle);
+ evt.phy_handle = amp->phy_handle;
+ evt.flow_spec = 0x00;
+
+ send_event(amp, BT_HCI_EVT_LOGIC_LINK_COMPLETE, &evt, sizeof(evt));
+}
+
+static void evt_disconn_logic_link_complete(struct bt_amp *amp, uint8_t reason)
+{
+ struct bt_hci_evt_disconn_logic_link_complete evt;
+
+ evt.status = BT_HCI_ERR_SUCCESS;
+ evt.handle = htobs(amp->logic_handle);
+ evt.reason = reason;
+
+ send_event(amp, BT_HCI_EVT_DISCONN_LOGIC_LINK_COMPLETE,
+ &evt, sizeof(evt));
+}
+
+static void cmd_create_logic_link(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_create_logic_link *cmd = data;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_CREATE_LOGIC_LINK);
+ return;
+ }
+
+ if (amp->phy_mode != PHY_MODE_IDLE) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_CREATE_LOGIC_LINK);
+ return;
+ }
+
+ if (amp->logic_handle != 0x00) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_CREATE_LOGIC_LINK);
+ return;
+ }
+
+ cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_CREATE_LOGIC_LINK);
+
+ amp->logic_handle = 0x0042;
+
+ evt_logic_link_complete(amp);
+}
+
+static void cmd_accept_logic_link(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_accept_logic_link *cmd = data;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_ACCEPT_LOGIC_LINK);
+ return;
+ }
+
+ if (amp->phy_mode != PHY_MODE_IDLE) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_ACCEPT_LOGIC_LINK);
+ return;
+ }
+
+ if (amp->logic_handle != 0x00) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_ACCEPT_LOGIC_LINK);
+ return;
+ }
+
+ cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_ACCEPT_LOGIC_LINK);
+
+ amp->logic_handle = 0x0023;
+
+ evt_logic_link_complete(amp);
+}
+
+static void cmd_disconn_logic_link(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_disconn_logic_link *cmd = data;
+
+ if (cmd->handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_DISCONN_LOGIC_LINK);
+ return;
+ }
+
+ if (cmd->handle != amp->logic_handle) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_DISCONN_LOGIC_LINK);
+ return;
+ }
+
+ cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_DISCONN_LOGIC_LINK);
+
+ evt_disconn_logic_link_complete(amp, 0x13);
+
+ amp->logic_handle = 0x0000;
+}
+
+static void cmd_logic_link_cancel(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_logic_link_cancel *cmd = data;
+ struct bt_hci_rsp_logic_link_cancel rsp;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_LOGIC_LINK_CANCEL);
+ return;
+ }
+
+ if (amp->phy_mode != PHY_MODE_IDLE) {
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_LOGIC_LINK_CANCEL);
+ return;
+ }
+
+ amp->logic_handle = 0x0000;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.phy_handle = amp->phy_handle;
+ rsp.flow_spec = 0x00;
+
+ cmd_complete(amp, BT_HCI_CMD_LOGIC_LINK_CANCEL, &rsp, sizeof(rsp));
+}
+
+static void cmd_set_event_mask_page2(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_set_event_mask_page2 *cmd = data;
+ uint8_t status;
+
+ memcpy(amp->event_mask + 8, cmd->mask, 8);
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(amp, BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
+ &status, sizeof(status));
+}
+
+static void cmd_read_location_data(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_location_data rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.domain_aware = 0x00;
+ rsp.domain[0] = 0x58;
+ rsp.domain[1] = 0x58;
+ rsp.domain_options = 0x58;
+ rsp.options = 0x00;
+
+ cmd_complete(amp, BT_HCI_CMD_READ_LOCATION_DATA, &rsp, sizeof(rsp));
+}
+
+static void cmd_write_location_data(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_write_location_data *cmd = data;
+ uint8_t status;
+
+ if (cmd->domain_aware > 0x01) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_WRITE_LOCATION_DATA);
+ return;
+ }
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(amp, BT_HCI_CMD_WRITE_LOCATION_DATA,
+ &status, sizeof(status));
+}
+
+static void cmd_read_flow_control_mode(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_flow_control_mode rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.mode = 0x01;
+
+ cmd_complete(amp, BT_HCI_CMD_READ_FLOW_CONTROL_MODE,
+ &rsp, sizeof(rsp));
+}
+
+static void cmd_write_flow_control_mode(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_write_flow_control_mode *cmd = data;
+ uint8_t status;
+
+ if (cmd->mode != 0x01) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_WRITE_FLOW_CONTROL_MODE);
+ return;
+ }
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(amp, BT_HCI_CMD_WRITE_FLOW_CONTROL_MODE,
+ &status, sizeof(status));
+}
+
+static void cmd_read_data_block_size(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_data_block_size rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.max_acl_len = cpu_to_le16(1492);
+ rsp.block_len = cpu_to_le16(1492);
+ rsp.num_blocks = cpu_to_le16(1);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_DATA_BLOCK_SIZE, &rsp, sizeof(rsp));
+}
+
+static void cmd_read_local_amp_info(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ struct bt_hci_rsp_read_local_amp_info rsp;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.amp_status = amp->amp_status;
+ rsp.total_bw = cpu_to_le32(24000);
+ rsp.max_bw = cpu_to_le32(24000);
+ rsp.min_latency = cpu_to_le32(100);
+ rsp.max_pdu = cpu_to_le32(1492);
+ rsp.amp_type = amp->amp_type;
+ rsp.pal_cap = cpu_to_le16(0x0001);
+ rsp.max_assoc_len = cpu_to_le16(MAX_ASSOC_LEN);
+ rsp.max_flush_to = cpu_to_le32(20000);
+ rsp.be_flush_to = cpu_to_le32(20000);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_AMP_INFO, &rsp, sizeof(rsp));
+}
+
+static void cmd_read_local_amp_assoc(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_read_local_amp_assoc *cmd = data;
+ struct bt_hci_rsp_read_local_amp_assoc rsp;
+ uint16_t len_so_far, remain_assoc_len, fragment_len;
+
+ if (cmd->phy_handle != amp->phy_handle) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_READ_LOCAL_AMP_ASSOC);
+ return;
+ }
+
+ len_so_far = le16_to_cpu(cmd->len_so_far);
+ remain_assoc_len = amp->local_assoc_len - len_so_far;
+ fragment_len = remain_assoc_len > 248 ? 248 : remain_assoc_len;
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.phy_handle = cmd->phy_handle;
+ rsp.remain_assoc_len = cpu_to_le16(remain_assoc_len);
+ memcpy(rsp.assoc_fragment, amp->local_assoc + len_so_far,
+ fragment_len);
+
+ cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_AMP_ASSOC,
+ &rsp, 4 + fragment_len);
+}
+
+static int create_unix_server(const char *path)
+{
+ struct sockaddr_un addr;
+ int fd;
+
+ fd = socket(PF_UNIX, SOCK_SEQPACKET, 0);
+ if (fd < 0)
+ return -1;
+
+ memset(&addr, 0, sizeof(addr));
+ addr.sun_family = AF_UNIX;
+ addr.sun_path[0] = '\0';
+ strcpy(addr.sun_path + 1, path);
+
+ if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ if (listen(fd, 1) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ return fd;
+}
+
+static int connect_unix_client(const char *path)
+{
+ struct sockaddr_un addr;
+ int fd;
+
+ fd = socket(PF_UNIX, SOCK_SEQPACKET | SOCK_CLOEXEC | SOCK_NONBLOCK, 0);
+ if (fd < 0)
+ return -1;
+
+ memset(&addr, 0, sizeof(addr));
+ addr.sun_family = AF_UNIX;
+ addr.sun_path[0] = '\0';
+ strcpy(addr.sun_path + 1, path);
+
+ if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ return fd;
+}
+
+static void accept_callback(int fd, uint32_t events, void *user_data)
+{
+ struct bt_amp *amp = user_data;
+ struct sockaddr_un addr;
+ socklen_t len;
+ int new_fd;
+
+ if (events & (EPOLLERR | EPOLLHUP)) {
+ mainloop_remove_fd(fd);
+ return;
+ }
+
+ memset(&addr, 0, sizeof(addr));
+ len = sizeof(addr);
+
+ new_fd = accept4(fd, (struct sockaddr *) &addr, &len,
+ SOCK_CLOEXEC | SOCK_NONBLOCK);
+ if (new_fd < 0)
+ return;
+
+ mainloop_remove_fd(fd);
+ close(fd);
+
+ amp->phylink_fd = new_fd;
+
+ evt_phy_link_complete(amp);
+
+ mainloop_add_fd(new_fd, EPOLLIN, link_callback, amp, NULL);
+}
+
+static void connect_callback(int fd, uint32_t events, void *user_data)
+{
+ struct bt_amp *amp = user_data;
+
+ if (events & (EPOLLERR | EPOLLHUP)) {
+ mainloop_remove_fd(fd);
+ return;
+ }
+
+ mainloop_remove_fd(fd);
+
+ evt_phy_link_complete(amp);
+
+ mainloop_add_fd(fd, EPOLLIN, link_callback, amp, NULL);
+}
+
+static void cmd_write_remote_amp_assoc(struct bt_amp *amp,
+ const void *data, uint8_t size)
+{
+ const struct bt_hci_cmd_write_remote_amp_assoc *cmd = data;
+ struct bt_hci_rsp_write_remote_amp_assoc rsp;
+ int fd;
+
+ if (cmd->phy_handle == 0x00) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
+ return;
+ }
+
+ if (cmd->phy_handle != amp->phy_handle) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
+ BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
+ return;
+ }
+
+ switch (amp->phy_mode) {
+ case PHY_MODE_INITIATOR:
+ strcpy(amp->phylink_path, "amp");
+
+ fd = create_unix_server(amp->phylink_path);
+ if (fd < 0) {
+ cmd_status(amp, BT_HCI_ERR_UNSPECIFIED_ERROR,
+ BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
+ return;
+ }
+
+ amp->local_assoc[0] = 0x01;
+ memcpy(amp->local_assoc + 1, amp->phylink_path,
+ strlen(amp->phylink_path) + 1);
+ amp->local_assoc_len = strlen(amp->phylink_path) + 2;
+
+ mainloop_add_fd(fd, EPOLLIN, accept_callback, amp, NULL);
+
+ amp->phylink_fd = fd;
+ break;
+
+ case PHY_MODE_ACCEPTOR:
+ if (cmd->assoc_fragment[0] != 0x01) {
+ cmd_status(amp, BT_HCI_ERR_UNSPECIFIED_ERROR,
+ BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
+ return;
+ }
+
+ memcpy(amp->phylink_path, cmd->assoc_fragment + 1,
+ cmd->remain_assoc_len - 1);
+
+ fd = connect_unix_client(amp->phylink_path);
+ if (fd < 0) {
+ cmd_status(amp, BT_HCI_ERR_UNSPECIFIED_ERROR,
+ BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
+ return;
+ }
+
+ mainloop_add_fd(fd, EPOLLOUT, connect_callback, amp, NULL);
+
+ amp->phylink_fd = fd;
+ break;
+
+ default:
+ cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
+ BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
+ return;
+ }
+
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.phy_handle = amp->phy_handle;
+
+ cmd_complete(amp, BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC, &rsp, sizeof(rsp));
+
+ if (amp->phy_mode == PHY_MODE_INITIATOR) {
+ struct bt_hci_evt_channel_selected evt;
+
+ evt.phy_handle = amp->phy_handle;
+
+ send_event(amp, BT_HCI_EVT_CHANNEL_SELECTED, &evt, sizeof(evt));
+ }
+}
+
+static const struct {
+ uint16_t opcode;
+ void (*func) (struct bt_amp *amp, const void *data, uint8_t size);
+ uint8_t size;
+ bool fixed;
+} cmd_table[] = {
+ { BT_HCI_CMD_SET_EVENT_MASK, cmd_set_event_mask, 8, true },
+ { BT_HCI_CMD_RESET, cmd_reset, 0, true },
+ { BT_HCI_CMD_READ_LOCAL_VERSION, cmd_read_local_version, 0, true },
+ { BT_HCI_CMD_READ_LOCAL_COMMANDS, cmd_read_local_commands, 0, true },
+ { BT_HCI_CMD_READ_LOCAL_FEATURES, cmd_read_local_features, 0, true },
+ { BT_HCI_CMD_READ_BUFFER_SIZE, cmd_read_buffer_size, 0, true },
+
+ { BT_HCI_CMD_CREATE_PHY_LINK,
+ cmd_create_phy_link, 3, false },
+ { BT_HCI_CMD_ACCEPT_PHY_LINK,
+ cmd_accept_phy_link, 3, false },
+ { BT_HCI_CMD_DISCONN_PHY_LINK,
+ cmd_disconn_phy_link, 2, true },
+ { BT_HCI_CMD_CREATE_LOGIC_LINK,
+ cmd_create_logic_link, 33, true },
+ { BT_HCI_CMD_ACCEPT_LOGIC_LINK,
+ cmd_accept_logic_link, 33, true },
+ { BT_HCI_CMD_DISCONN_LOGIC_LINK,
+ cmd_disconn_logic_link, 2, true },
+ { BT_HCI_CMD_LOGIC_LINK_CANCEL,
+ cmd_logic_link_cancel, 2, true },
+ { BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
+ cmd_set_event_mask_page2, 8, true },
+ { BT_HCI_CMD_READ_LOCATION_DATA,
+ cmd_read_location_data, 0, true },
+ { BT_HCI_CMD_WRITE_LOCATION_DATA,
+ cmd_write_location_data, 5, true },
+ { BT_HCI_CMD_READ_FLOW_CONTROL_MODE,
+ cmd_read_flow_control_mode, 0, true },
+ { BT_HCI_CMD_WRITE_FLOW_CONTROL_MODE,
+ cmd_write_flow_control_mode, 1, true },
+ { BT_HCI_CMD_READ_DATA_BLOCK_SIZE,
+ cmd_read_data_block_size, 0, true },
+ { BT_HCI_CMD_READ_LOCAL_AMP_INFO,
+ cmd_read_local_amp_info, 0, true },
+ { BT_HCI_CMD_READ_LOCAL_AMP_ASSOC,
+ cmd_read_local_amp_assoc, 5, true },
+ { BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC,
+ cmd_write_remote_amp_assoc, 6, false },
+ { }
+};
+
+static void process_command(struct bt_amp *amp, const void *data, size_t size)
+{
+ const struct bt_hci_cmd_hdr *hdr = data;
+ uint16_t opcode;
+ unsigned int i;
+
+ if (size < sizeof(*hdr))
+ return;
+
+ data += sizeof(*hdr);
+ size -= sizeof(*hdr);
+
+ opcode = le16_to_cpu(hdr->opcode);
+
+ if (hdr->plen != size) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
+ return;
+ }
+
+ for (i = 0; cmd_table[i].func; i++) {
+ if (cmd_table[i].opcode != opcode)
+ continue;
+
+ if ((cmd_table[i].fixed && size != cmd_table[i].size) ||
+ size < cmd_table[i].size) {
+ cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
+ return;
+ }
+
+ cmd_table[i].func(amp, data, size);
+ return;
+ }
+
+ cmd_status(amp, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
+}
+
+static void vhci_read_callback(int fd, uint32_t events, void *user_data)
+{
+ struct bt_amp *amp = user_data;
+ unsigned char buf[4096];
+ ssize_t len;
+
+ if (events & (EPOLLERR | EPOLLHUP))
+ return;
+
+ len = read(amp->vhci_fd, buf, sizeof(buf));
+ if (len < 1)
+ return;
+
+ switch (buf[0]) {
+ case BT_H4_CMD_PKT:
+ process_command(amp, buf + 1, len - 1);
+ break;
+ }
+}
+
+struct bt_amp *bt_amp_new(void)
+{
+ unsigned char setup_cmd[2];
+ struct bt_amp *amp;
+
+ amp = calloc(1, sizeof(*amp));
+ if (!amp)
+ return NULL;
+
+ reset_defaults(amp);
+
+ amp->vhci_fd = open("/dev/vhci", O_RDWR);
+ if (amp->vhci_fd < 0) {
+ free(amp);
+ return NULL;
+ }
+
+ setup_cmd[0] = HCI_VENDOR_PKT;
+ setup_cmd[1] = HCI_AMP;
+
+ if (write(amp->vhci_fd, setup_cmd, sizeof(setup_cmd)) < 0) {
+ close(amp->vhci_fd);
+ free(amp);
+ return NULL;
+ }
+
+ mainloop_add_fd(amp->vhci_fd, EPOLLIN, vhci_read_callback, amp, NULL);
+
+ return bt_amp_ref(amp);
+}
+
+struct bt_amp *bt_amp_ref(struct bt_amp *amp)
+{
+ if (!amp)
+ return NULL;
+
+ __sync_fetch_and_add(&amp->ref_count, 1);
+
+ return amp;
+}
+
+void bt_amp_unref(struct bt_amp *amp)
+{
+ if (!amp)
+ return;
+
+ if (__sync_sub_and_fetch(&amp->ref_count, 1))
+ return;
+
+ mainloop_remove_fd(amp->vhci_fd);
+
+ close(amp->vhci_fd);
+
+ free(amp);
+}
diff --git a/emulator/amp.h b/emulator/amp.h
new file mode 100644
index 00000000..189dfb7d
--- /dev/null
+++ b/emulator/amp.h
@@ -0,0 +1,32 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011-2012 Intel Corporation
+ * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#include <stdbool.h>
+
+struct bt_amp;
+
+struct bt_amp *bt_amp_new(void);
+
+struct bt_amp *bt_amp_ref(struct bt_amp *amp);
+void bt_amp_unref(struct bt_amp *amp);
diff --git a/emulator/b1ee.c b/emulator/b1ee.c
new file mode 100644
index 00000000..17a60fca
--- /dev/null
+++ b/emulator/b1ee.c
@@ -0,0 +1,256 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011-2012 Intel Corporation
+ * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <netdb.h>
+#include <arpa/inet.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+
+#include "monitor/mainloop.h"
+
+#define DEFAULT_SERVER "b1ee.com"
+#define DEFAULT_HOST_PORT "45550" /* 0xb1ee */
+#define DEFAULT_SNIFFER_PORT "45551" /* 0xb1ef */
+
+static int sniffer_fd;
+static int server_fd;
+static int vhci_fd;
+
+static void sniffer_read_callback(int fd, uint32_t events, void *user_data)
+{
+ static uint8_t buf[4096];
+ ssize_t len;
+
+ if (events & (EPOLLERR | EPOLLHUP))
+ return;
+
+again:
+ len = recv(fd, buf, sizeof(buf), MSG_DONTWAIT);
+ if (len < 0) {
+ if (errno == EAGAIN)
+ goto again;
+ return;
+ }
+
+ printf("Sniffer received: %zi bytes\n", len);
+}
+
+static uint8_t *server_pkt_data;
+static uint8_t server_pkt_type;
+static uint16_t server_pkt_expect;
+static uint16_t server_pkt_len;
+static uint16_t server_pkt_offset;
+
+static void server_read_callback(int fd, uint32_t events, void *user_data)
+{
+ static uint8_t buf[4096];
+ uint8_t *ptr = buf;
+ ssize_t len;
+ uint16_t count;
+
+ if (events & (EPOLLERR | EPOLLHUP))
+ return;
+
+again:
+ len = recv(fd, buf + server_pkt_offset,
+ sizeof(buf) - server_pkt_offset, MSG_DONTWAIT);
+ if (len < 0) {
+ if (errno == EAGAIN)
+ goto again;
+ return;
+ }
+
+ count = server_pkt_offset + len;
+
+ while (count > 0) {
+ hci_event_hdr *evt_hdr;
+
+ if (!server_pkt_data) {
+ server_pkt_type = ptr[0];
+
+ switch (server_pkt_type) {
+ case HCI_EVENT_PKT:
+ if (count < HCI_EVENT_HDR_SIZE + 1) {
+ server_pkt_offset += len;
+ return;
+ }
+ evt_hdr = (hci_event_hdr *) (ptr + 1);
+ server_pkt_expect = HCI_EVENT_HDR_SIZE +
+ evt_hdr->plen + 1;
+ server_pkt_data = malloc(server_pkt_expect);
+ server_pkt_len = 0;
+ break;
+ default:
+ fprintf(stderr, "Unknown packet from server\n");
+ return;
+ }
+
+ server_pkt_offset = 0;
+ }
+
+ if (count >= server_pkt_expect) {
+ ssize_t written;
+
+ memcpy(server_pkt_data + server_pkt_len,
+ ptr, server_pkt_expect);
+ ptr += server_pkt_expect;
+ count -= server_pkt_expect;
+
+ written = write(vhci_fd, server_pkt_data,
+ server_pkt_len + server_pkt_expect);
+ if (written != server_pkt_len + server_pkt_expect)
+ fprintf(stderr, "Write to /dev/vhci failed\n");
+
+ free(server_pkt_data);
+ server_pkt_data = NULL;
+ } else {
+ memcpy(server_pkt_data + server_pkt_len, ptr, count);
+ server_pkt_len += count;
+ server_pkt_expect -= count;
+ count = 0;
+ }
+ }
+}
+
+static void vhci_read_callback(int fd, uint32_t events, void *user_data)
+{
+ unsigned char buf[4096];
+ ssize_t len, written;
+
+ if (events & (EPOLLERR | EPOLLHUP))
+ return;
+
+ len = read(fd, buf, sizeof(buf));
+ if (len < 0)
+ return;
+
+ written = write(server_fd, buf, len);
+ if (written != len)
+ fprintf(stderr, "Write to server failed\n");
+}
+
+static void signal_callback(int signum, void *user_data)
+{
+ switch (signum) {
+ case SIGINT:
+ case SIGTERM:
+ mainloop_quit();
+ break;
+ }
+}
+
+static int do_connect(const char *node, const char *service)
+{
+ struct addrinfo hints;
+ struct addrinfo *info, *res;
+ int err, fd = -1;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = PF_UNSPEC;
+ hints.ai_socktype = SOCK_STREAM;
+
+ err = getaddrinfo(DEFAULT_SERVER, DEFAULT_HOST_PORT, &hints, &res);
+ if (err) {
+ perror(gai_strerror(err));
+ exit(1);
+ }
+
+ for (info = res; info; info = info->ai_next) {
+ char str[INET6_ADDRSTRLEN];
+
+ inet_ntop(info->ai_family, info->ai_addr->sa_data,
+ str, sizeof(str));
+
+ fd = socket(info->ai_family, info->ai_socktype,
+ info->ai_protocol);
+ if (fd < 0)
+ continue;
+
+ printf("Trying to connect to %s on port %s\n", str, service);
+
+ if (connect(fd, res->ai_addr, res->ai_addrlen) < 0) {
+ perror("Failed to connect");
+ continue;
+ }
+
+ printf("Successfully connected to %s on port %s\n",
+ str, service);
+ break;
+ }
+
+ freeaddrinfo(res);
+
+ if (res == NULL)
+ exit(1);
+
+ return fd;
+}
+
+int main(int argc, char *argv[])
+{
+ const char sniff_cmd[] = { 0x01, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+ ssize_t written;
+ sigset_t mask;
+
+ server_fd = do_connect(DEFAULT_SERVER, DEFAULT_HOST_PORT);
+ sniffer_fd = do_connect(DEFAULT_SERVER, DEFAULT_SNIFFER_PORT);
+
+ written = write(sniffer_fd, sniff_cmd, sizeof(sniff_cmd));
+ if (written < 0)
+ perror("Failed to enable sniffer");
+
+ vhci_fd = open("/dev/vhci", O_RDWR | O_NONBLOCK);
+ if (vhci_fd < 0) {
+ perror("Failed to /dev/vhci");
+ close(server_fd);
+ exit(1);
+ }
+
+ mainloop_init();
+
+ sigemptyset(&mask);
+ sigaddset(&mask, SIGINT);
+ sigaddset(&mask, SIGTERM);
+
+ mainloop_set_signal(&mask, signal_callback, NULL, NULL);
+
+ mainloop_add_fd(sniffer_fd, EPOLLIN, sniffer_read_callback, NULL, NULL);
+ mainloop_add_fd(server_fd, EPOLLIN, server_read_callback, NULL, NULL);
+ mainloop_add_fd(vhci_fd, EPOLLIN, vhci_read_callback, NULL, NULL);
+
+ return mainloop_run();
+}
diff --git a/emulator/btdev.c b/emulator/btdev.c
index 7d4517a5..5f04bd17 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -6,18 +6,18 @@
* Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
*
- * This program is distributed in the hope that it will be useful,
+ * This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
*
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
@@ -30,24 +30,48 @@
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
+#include <alloca.h>
-#include "bt.h"
+#include "monitor/bt.h"
#include "btdev.h"
#define le16_to_cpu(val) (val)
+#define le32_to_cpu(val) (val)
#define cpu_to_le16(val) (val)
+#define cpu_to_le32(val) (val)
+
+#define has_bredr(btdev) (!((btdev)->features[4] & 0x20))
+#define has_le(btdev) (!!((btdev)->features[4] & 0x40))
+
+struct hook {
+ btdev_hook_func handler;
+ void *user_data;
+ enum btdev_hook_type type;
+ uint16_t opcode;
+};
+
+#define MAX_HOOK_ENTRIES 16
struct btdev {
+ enum btdev_type type;
+
struct btdev *conn;
+ btdev_command_func command_handler;
+ void *command_data;
+
btdev_send_func send_handler;
void *send_data;
+ struct hook *hook_list[MAX_HOOK_ENTRIES];
+
uint16_t manufacturer;
uint8_t version;
uint16_t revision;
uint8_t commands[64];
+ uint8_t max_page;
uint8_t features[8];
+ uint8_t feat_page_2[8];
uint16_t acl_mtu;
uint16_t acl_max_pkt;
uint8_t country_code;
@@ -57,6 +81,7 @@ struct btdev {
uint16_t default_link_policy;
uint8_t event_mask[8];
+ uint8_t event_mask_page2[8];
uint8_t event_filter;
uint8_t name[248];
uint8_t dev_class[3];
@@ -64,21 +89,63 @@ struct btdev {
uint16_t conn_accept_timeout;
uint16_t page_timeout;
uint8_t scan_enable;
+ uint16_t page_scan_interval;
+ uint16_t page_scan_window;
+ uint16_t page_scan_type;
uint8_t auth_enable;
+ uint16_t inquiry_scan_interval;
+ uint16_t inquiry_scan_window;
uint8_t inquiry_mode;
- uint8_t afh_assess_mode;
+ uint8_t afh_assessment_mode;
uint8_t ext_inquiry_fec;
uint8_t ext_inquiry_rsp[240];
uint8_t simple_pairing_mode;
uint8_t le_supported;
uint8_t le_simultaneous;
uint8_t le_event_mask[8];
+ uint8_t le_adv_data[31];
+ uint8_t le_adv_data_len;
+ uint8_t le_scan_enable;
+ uint8_t le_filter_dup;
+ uint8_t le_adv_enable;
+
+ uint16_t sync_train_interval;
+ uint32_t sync_train_timeout;
+ uint8_t sync_train_service_data;
};
#define MAX_BTDEV_ENTRIES 16
static struct btdev *btdev_list[MAX_BTDEV_ENTRIES] = { };
+static int get_hook_index(struct btdev *btdev, enum btdev_hook_type type,
+ uint16_t opcode)
+{
+ int i;
+
+ for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
+ if (btdev->hook_list[i] == NULL)
+ continue;
+
+ if (btdev->hook_list[i]->type == type &&
+ btdev->hook_list[i]->opcode == opcode)
+ return i;
+ }
+
+ return -1;
+}
+
+static bool run_hooks(struct btdev *btdev, enum btdev_hook_type type,
+ uint16_t opcode, const void *data, uint16_t len)
+{
+ int index = get_hook_index(btdev, type, opcode);
+ if (index < 0)
+ return true;
+
+ return btdev->hook_list[index]->handler(data, len,
+ btdev->hook_list[index]->user_data);
+}
+
static inline int add_btdev(struct btdev *btdev)
{
int i, index = -1;
@@ -160,30 +227,140 @@ static void hexdump(const unsigned char *buf, uint16_t len)
}
}
-static void get_bdaddr(uint16_t id, uint8_t *bdaddr)
+static void get_bdaddr(uint16_t id, uint8_t index, uint8_t *bdaddr)
{
bdaddr[0] = id & 0xff;
bdaddr[1] = id >> 8;
- bdaddr[2] = 0x00;
+ bdaddr[2] = index;
bdaddr[3] = 0x01;
bdaddr[4] = 0xaa;
bdaddr[5] = 0x00;
}
-struct btdev *btdev_create(uint16_t id)
+static void set_common_commands_all(struct btdev *btdev)
{
- struct btdev *btdev;
+ btdev->commands[5] |= 0x40; /* Set Event Mask */
+ btdev->commands[5] |= 0x80; /* Reset */
+ btdev->commands[14] |= 0x08; /* Read Local Version */
+ btdev->commands[14] |= 0x10; /* Read Local Supported Commands */
+ btdev->commands[14] |= 0x20; /* Read Local Supported Features */
+ btdev->commands[14] |= 0x80; /* Read Buffer Size */
+}
- btdev = malloc(sizeof(*btdev));
- if (!btdev)
- return NULL;
+static void set_common_commands_bredrle(struct btdev *btdev)
+{
+ btdev->commands[0] |= 0x20; /* Disconnect */
+ btdev->commands[2] |= 0x80; /* Read Remote Version Information */
+ btdev->commands[10] |= 0x40; /* Host Buffer Size */
+ btdev->commands[15] |= 0x02; /* Read BD ADDR */
+}
- memset(btdev, 0, sizeof(*btdev));
+static void set_bredr_commands(struct btdev *btdev)
+{
+ set_common_commands_all(btdev);
+ set_common_commands_bredrle(btdev);
+
+ btdev->commands[0] |= 0x01; /* Inquiry */
+ btdev->commands[0] |= 0x02; /* Inquiry Cancel */
+ btdev->commands[0] |= 0x10; /* Create Connection */
+ btdev->commands[0] |= 0x40; /* Add SCO Connection */
+ btdev->commands[0] |= 0x80; /* Cancel Create Connection */
+ btdev->commands[1] |= 0x01; /* Accept Connection Request */
+ btdev->commands[1] |= 0x02; /* Reject Connection Request */
+ btdev->commands[2] |= 0x08; /* Remote Name Request */
+ btdev->commands[2] |= 0x10; /* Cancel Remote Name Request */
+ btdev->commands[2] |= 0x20; /* Read Remote Supported Features */
+ btdev->commands[2] |= 0x40; /* Read Remote Extended Features */
+ btdev->commands[5] |= 0x08; /* Read Default Link Policy */
+ btdev->commands[5] |= 0x10; /* Write Default Link Policy */
+ btdev->commands[6] |= 0x01; /* Set Event Filter */
+ btdev->commands[6] |= 0x20; /* Read Stored Link Key */
+ btdev->commands[6] |= 0x40; /* Write Stored Link Key */
+ btdev->commands[6] |= 0x80; /* Delete Stored Link Key */
+ btdev->commands[7] |= 0x01; /* Write Local Name */
+ btdev->commands[7] |= 0x02; /* Read Local Name */
+ btdev->commands[7] |= 0x04; /* Read Connection Accept Timeout */
+ btdev->commands[7] |= 0x08; /* Write Connection Accept Timeout */
+ btdev->commands[7] |= 0x10; /* Read Page Timeout */
+ btdev->commands[7] |= 0x20; /* Write Page Timeout */
+ btdev->commands[7] |= 0x40; /* Read Scan Enable */
+ btdev->commands[7] |= 0x80; /* Write Scan Enable */
+ btdev->commands[8] |= 0x01; /* Read Page Scan Activity */
+ btdev->commands[8] |= 0x02; /* Write Page Scan Activity */
+ btdev->commands[8] |= 0x04; /* Read Inquiry Scan Activity */
+ btdev->commands[8] |= 0x08; /* Write Inquiry Scan Activity */
+ btdev->commands[8] |= 0x10; /* Read Authentication Enable */
+ btdev->commands[8] |= 0x20; /* Write Authentication Enable */
+ btdev->commands[9] |= 0x01; /* Read Class Of Device */
+ btdev->commands[9] |= 0x02; /* Write Class Of Device */
+ btdev->commands[9] |= 0x04; /* Read Voice Setting */
+ btdev->commands[9] |= 0x08; /* Write Voice Setting */
+ btdev->commands[11] |= 0x10; /* Write Current IAC LAP */
+ btdev->commands[12] |= 0x40; /* Read Inquiry Mode */
+ btdev->commands[12] |= 0x80; /* Write Inquiry Mode */
+ btdev->commands[13] |= 0x01; /* Read Page Scan Type */
+ btdev->commands[13] |= 0x02; /* Write Page Scan Type */
+ btdev->commands[13] |= 0x04; /* Read AFH Assess Mode */
+ btdev->commands[13] |= 0x08; /* Write AFH Assess Mode */
+ btdev->commands[14] |= 0x40; /* Read Local Extended Features */
+ btdev->commands[15] |= 0x01; /* Read Country Code */
+ btdev->commands[16] |= 0x04; /* Enable Device Under Test Mode */
+ btdev->commands[16] |= 0x08; /* Setup Synchronous Connection */
+ btdev->commands[17] |= 0x01; /* Read Extended Inquiry Response */
+ btdev->commands[17] |= 0x02; /* Write Extended Inquiry Response */
+ btdev->commands[17] |= 0x20; /* Read Simple Pairing Mode */
+ btdev->commands[17] |= 0x40; /* Write Simple Pairing Mode */
+ btdev->commands[17] |= 0x80; /* Read Local OOB Data */
+ btdev->commands[18] |= 0x01; /* Read Inquiry Response TX Power */
+ btdev->commands[18] |= 0x02; /* Write Inquiry Response TX Power */
+ btdev->commands[23] |= 0x04; /* Read Data Block Size */
+}
- btdev->manufacturer = 63;
- btdev->version = 0x06;
- btdev->revision = 0x0000;
+static void set_le_commands(struct btdev *btdev)
+{
+ set_common_commands_all(btdev);
+ set_common_commands_bredrle(btdev);
+
+ btdev->commands[24] |= 0x20; /* Read LE Host Supported */
+ btdev->commands[24] |= 0x20; /* Write LE Host Supported */
+ btdev->commands[25] |= 0x01; /* LE Set Event Mask */
+ btdev->commands[25] |= 0x02; /* LE Read Buffer Size */
+ btdev->commands[25] |= 0x04; /* LE Read Local Features */
+ btdev->commands[25] |= 0x20; /* LE Set Adv Parameters */
+ btdev->commands[25] |= 0x40; /* LE Read Adv TX Power */
+ btdev->commands[25] |= 0x80; /* LE Set Adv Data */
+ btdev->commands[26] |= 0x02; /* LE Set Adv Enable */
+ btdev->commands[26] |= 0x04; /* LE Set Scan Parameters */
+ btdev->commands[26] |= 0x08; /* LE Set Scan Enable */
+ btdev->commands[26] |= 0x40; /* LE Read White List Size */
+ btdev->commands[27] |= 0x80; /* LE Rand */
+ btdev->commands[28] |= 0x08; /* LE Read Supported States */
+ btdev->commands[28] |= 0x10; /* LE Receiver Test */
+ btdev->commands[28] |= 0x20; /* LE Transmitter Test */
+ btdev->commands[28] |= 0x40; /* LE Test End */
+}
+
+static void set_bredrle_commands(struct btdev *btdev)
+{
+ set_bredr_commands(btdev);
+ set_le_commands(btdev);
+
+ /* Extra BR/EDR commands we want to only support for >= 4.0
+ * adapters.
+ */
+ btdev->commands[22] |= 0x04; /* Set Event Mask Page 2 */
+ btdev->commands[31] |= 0x80; /* Read Sync Train Parameters */
+}
+
+static void set_amp_commands(struct btdev *btdev)
+{
+ set_common_commands_all(btdev);
+
+ btdev->commands[22] |= 0x20; /* Read Local AMP Info */
+}
+static void set_bredrle_features(struct btdev *btdev)
+{
btdev->features[0] |= 0x04; /* Encryption */
btdev->features[0] |= 0x20; /* Role switch */
btdev->features[0] |= 0x80; /* Sniff mode */
@@ -207,14 +384,113 @@ struct btdev *btdev_create(uint16_t id)
btdev->features[7] |= 0x02; /* Inquiry TX Power Level */
btdev->features[7] |= 0x80; /* Extended features */
+ btdev->feat_page_2[0] |= 0x01; /* CSB - Master Operation */
+ btdev->feat_page_2[0] |= 0x02; /* CSB - Slave Operation */
+ btdev->feat_page_2[0] |= 0x04; /* Synchronization Train */
+ btdev->feat_page_2[0] |= 0x08; /* Synchronization Scan */
+ btdev->feat_page_2[0] |= 0x10; /* Inquiry Response Notification */
+
+ btdev->max_page = 2;
+}
+
+static void set_bredr_features(struct btdev *btdev)
+{
+ btdev->features[0] |= 0x04; /* Encryption */
+ btdev->features[0] |= 0x20; /* Role switch */
+ btdev->features[0] |= 0x80; /* Sniff mode */
+ btdev->features[1] |= 0x08; /* SCO link */
+ btdev->features[3] |= 0x40; /* RSSI with inquiry results */
+ btdev->features[3] |= 0x80; /* Extended SCO link */
+ btdev->features[4] |= 0x08; /* AFH capable slave */
+ btdev->features[4] |= 0x10; /* AFH classification slave */
+ btdev->features[5] |= 0x02; /* Sniff subrating */
+ btdev->features[5] |= 0x04; /* Pause encryption */
+ btdev->features[5] |= 0x08; /* AFH capable master */
+ btdev->features[5] |= 0x10; /* AFH classification master */
+ btdev->features[6] |= 0x01; /* Extended Inquiry Response */
+ btdev->features[6] |= 0x08; /* Secure Simple Pairing */
+ btdev->features[6] |= 0x10; /* Encapsulated PDU */
+ btdev->features[6] |= 0x20; /* Erroneous Data Reporting */
+ btdev->features[6] |= 0x40; /* Non-flushable Packet Boundary Flag */
+ btdev->features[7] |= 0x01; /* Link Supervision Timeout Event */
+ btdev->features[7] |= 0x02; /* Inquiry TX Power Level */
+ btdev->features[7] |= 0x80; /* Extended features */
+
+ btdev->max_page = 1;
+}
+
+static void set_le_features(struct btdev *btdev)
+{
+ btdev->features[4] |= 0x20; /* BR/EDR Not Supported */
+ btdev->features[4] |= 0x40; /* LE Supported */
+
+ btdev->max_page = 1;
+}
+
+static void set_amp_features(struct btdev *btdev)
+{
+}
+
+struct btdev *btdev_create(enum btdev_type type, uint16_t id)
+{
+ struct btdev *btdev;
+ int index;
+
+ btdev = malloc(sizeof(*btdev));
+ if (!btdev)
+ return NULL;
+
+ memset(btdev, 0, sizeof(*btdev));
+ btdev->type = type;
+
+ btdev->manufacturer = 63;
+
+ if (type == BTDEV_TYPE_BREDR)
+ btdev->version = 0x05;
+ else
+ btdev->version = 0x06;
+
+ btdev->revision = 0x0000;
+
+ switch (btdev->type) {
+ case BTDEV_TYPE_BREDRLE:
+ set_bredrle_features(btdev);
+ set_bredrle_commands(btdev);
+ break;
+ case BTDEV_TYPE_BREDR:
+ set_bredr_features(btdev);
+ set_bredr_commands(btdev);
+ break;
+ case BTDEV_TYPE_LE:
+ set_le_features(btdev);
+ set_le_commands(btdev);
+ break;
+ case BTDEV_TYPE_AMP:
+ set_amp_features(btdev);
+ set_amp_commands(btdev);
+ break;
+ }
+
+ btdev->page_scan_interval = 0x0800;
+ btdev->page_scan_window = 0x0012;
+ btdev->page_scan_type = 0x00;
+
+ btdev->sync_train_interval = 0x0080;
+ btdev->sync_train_timeout = 0x0002ee00;
+ btdev->sync_train_service_data = 0x00;
+
btdev->acl_mtu = 192;
btdev->acl_max_pkt = 1;
btdev->country_code = 0x00;
- get_bdaddr(id, btdev->bdaddr);
+ index = add_btdev(btdev);
+ if (index < 0) {
+ free(btdev);
+ return NULL;
+ }
- add_btdev(btdev);
+ get_bdaddr(id, index, btdev->bdaddr);
return btdev;
}
@@ -229,6 +505,26 @@ void btdev_destroy(struct btdev *btdev)
free(btdev);
}
+const uint8_t *btdev_get_bdaddr(struct btdev *btdev)
+{
+ return btdev->bdaddr;
+}
+
+uint8_t *btdev_get_features(struct btdev *btdev)
+{
+ return btdev->features;
+}
+
+void btdev_set_command_handler(struct btdev *btdev, btdev_command_func handler,
+ void *user_data)
+{
+ if (!btdev)
+ return;
+
+ btdev->command_handler = handler;
+ btdev->command_data = user_data;
+}
+
void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
void *user_data)
{
@@ -269,7 +565,8 @@ static void send_event(struct btdev *btdev, uint8_t event,
if (len > 0)
memcpy(pkt_data + 1 + sizeof(*hdr), data, len);
- send_packet(btdev, pkt_data, pkt_len);
+ if (run_hooks(btdev, BTDEV_HOOK_POST_EVT, event, pkt_data, pkt_len))
+ send_packet(btdev, pkt_data, pkt_len);
free(pkt_data);
}
@@ -301,20 +598,40 @@ static void cmd_complete(struct btdev *btdev, uint16_t opcode,
if (len > 0)
memcpy(pkt_data + 1 + sizeof(*hdr) + sizeof(*cc), data, len);
- send_packet(btdev, pkt_data, pkt_len);
+ if (run_hooks(btdev, BTDEV_HOOK_POST_CMD, opcode, pkt_data, pkt_len))
+ send_packet(btdev, pkt_data, pkt_len);
free(pkt_data);
}
static void cmd_status(struct btdev *btdev, uint8_t status, uint16_t opcode)
{
- struct bt_hci_evt_cmd_status cs;
+ struct bt_hci_evt_hdr *hdr;
+ struct bt_hci_evt_cmd_status *cs;
+ uint16_t pkt_len;
+ void *pkt_data;
+
+ pkt_len = 1 + sizeof(*hdr) + sizeof(*cs);
+
+ pkt_data = malloc(pkt_len);
+ if (!pkt_data)
+ return;
+
+ ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
- cs.status = status;
- cs.ncmd = 0x01;
- cs.opcode = cpu_to_le16(opcode);
+ hdr = pkt_data + 1;
+ hdr->evt = BT_HCI_EVT_CMD_STATUS;
+ hdr->plen = sizeof(*cs);
+
+ cs = pkt_data + 1 + sizeof(*hdr);
+ cs->status = status;
+ cs->ncmd = 0x01;
+ cs->opcode = cpu_to_le16(opcode);
- send_event(btdev, BT_HCI_EVT_CMD_STATUS, &cs, sizeof(cs));
+ if (run_hooks(btdev, BTDEV_HOOK_POST_CMD, opcode, pkt_data, pkt_len))
+ send_packet(btdev, pkt_data, pkt_len);
+
+ free(pkt_data);
}
static void num_completed_packets(struct btdev *btdev)
@@ -349,7 +666,10 @@ static void inquiry_complete(struct btdev *btdev, uint8_t status)
ir.num_resp = 0x01;
memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
+ ir.pscan_rep_mode = 0x00;
+ ir.pscan_period_mode = 0x00;
memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
+ ir.clock_offset = 0x0000;
ir.rssi = -60;
memcpy(ir.data, btdev_list[i]->ext_inquiry_rsp, 240);
@@ -363,7 +683,10 @@ static void inquiry_complete(struct btdev *btdev, uint8_t status)
ir.num_resp = 0x01;
memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
+ ir.pscan_rep_mode = 0x00;
+ ir.pscan_period_mode = 0x00;
memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
+ ir.clock_offset = 0x0000;
ir.rssi = -60;
send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT_WITH_RSSI,
@@ -373,7 +696,11 @@ static void inquiry_complete(struct btdev *btdev, uint8_t status)
ir.num_resp = 0x01;
memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
+ ir.pscan_rep_mode = 0x00;
+ ir.pscan_period_mode = 0x00;
+ ir.pscan_mode = 0x00;
memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
+ ir.clock_offset = 0x0000;
send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT,
&ir, sizeof(ir));
@@ -419,12 +746,95 @@ static void conn_complete(struct btdev *btdev,
send_event(btdev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
}
+static void sync_conn_complete(struct btdev *btdev, uint16_t voice_setting,
+ uint8_t status)
+{
+ struct bt_hci_evt_sync_conn_complete cc;
+
+ if (!btdev->conn)
+ return;
+
+ cc.status = status;
+ memcpy(cc.bdaddr, btdev->conn->bdaddr, 6);
+
+ cc.handle = cpu_to_le16(status == BT_HCI_ERR_SUCCESS ? 257 : 0);
+ cc.link_type = 0x02;
+ cc.tx_interval = 0x000c;
+ cc.retrans_window = 0x06;
+ cc.rx_pkt_len = 60;
+ cc.tx_pkt_len = 60;
+ cc.air_mode = (voice_setting == 0x0060) ? 0x02 : 0x03;
+
+ send_event(btdev, BT_HCI_EVT_SYNC_CONN_COMPLETE, &cc, sizeof(cc));
+}
+
+static void sco_conn_complete(struct btdev *btdev, uint8_t status)
+{
+ struct bt_hci_evt_conn_complete cc;
+
+ if (!btdev->conn)
+ return;
+
+ cc.status = status;
+ memcpy(cc.bdaddr, btdev->conn->bdaddr, 6);
+ cc.handle = cpu_to_le16(status == BT_HCI_ERR_SUCCESS ? 257 : 0);
+ cc.link_type = 0x00;
+ cc.encr_mode = 0x00;
+
+ send_event(btdev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
+}
+
+static void le_conn_complete(struct btdev *btdev,
+ const uint8_t *bdaddr, uint8_t status)
+{
+ char buf[1 + sizeof(struct bt_hci_evt_le_conn_complete)];
+ struct bt_hci_evt_le_conn_complete *cc = (void *) &buf[1];
+
+ memset(buf, 0, sizeof(buf));
+
+ buf[0] = BT_HCI_EVT_LE_CONN_COMPLETE;
+
+ if (!status) {
+ struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
+
+ btdev->conn = remote;
+ remote->conn = btdev;
+
+ cc->status = status;
+ memcpy(cc->peer_addr, btdev->bdaddr, 6);
+
+ cc->role = 0x01;
+ cc->handle = cpu_to_le16(42);
+
+ send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+
+ cc->handle = cpu_to_le16(42);
+ }
+
+ cc->status = status;
+ memcpy(cc->peer_addr, bdaddr, 6);
+ cc->role = 0x00;
+
+ send_event(btdev, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+}
+
+static void le_conn_request(struct btdev *btdev, const uint8_t *bdaddr)
+{
+ struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
+
+ if (remote && remote->le_adv_enable)
+ le_conn_complete(btdev, bdaddr, 0);
+ else
+ le_conn_complete(btdev, bdaddr,
+ BT_HCI_ERR_CONN_FAILED_TO_ESTABLISH);
+}
+
static void conn_request(struct btdev *btdev, const uint8_t *bdaddr)
{
struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
if (remote) {
- if (remote->scan_enable & 0x01) {
+ if (remote->scan_enable & 0x02) {
struct bt_hci_evt_conn_request cr;
memcpy(cr.bdaddr, btdev->bdaddr, 6);
@@ -471,7 +881,7 @@ static void disconnect_complete(struct btdev *btdev, uint16_t handle,
static void name_request_complete(struct btdev *btdev,
const uint8_t *bdaddr, uint8_t status)
{
- struct bt_hci_evt_remote_name_req_complete nc;
+ struct bt_hci_evt_remote_name_request_complete nc;
nc.status = status;
memcpy(nc.bdaddr, bdaddr, 6);
@@ -566,19 +976,64 @@ static void remote_version_complete(struct btdev *btdev, uint16_t handle)
&rvc, sizeof(rvc));
}
-static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
+static void le_send_adv_report(struct btdev *btdev, const struct btdev *remote)
+{
+ struct __packed {
+ uint8_t subevent;
+ union {
+ struct bt_hci_evt_le_adv_report lar;
+ uint8_t raw[10 + 31 + 1];
+ };
+ } meta_event;
+
+ meta_event.subevent = BT_HCI_EVT_LE_ADV_REPORT;
+
+ memset(&meta_event.lar, 0, sizeof(meta_event.lar));
+ meta_event.lar.num_reports = 1;
+ memcpy(meta_event.lar.addr, remote->bdaddr, 6);
+ meta_event.lar.data_len = remote->le_adv_data_len;
+ memcpy(meta_event.lar.data, remote->le_adv_data,
+ meta_event.lar.data_len);
+ /* Not available */
+ meta_event.raw[10 + meta_event.lar.data_len] = 127;
+ send_event(btdev, BT_HCI_EVT_LE_META_EVENT, &meta_event,
+ 1 + 10 + meta_event.lar.data_len + 1);
+}
+
+static void le_set_adv_enable_complete(struct btdev *btdev)
+{
+ int i;
+
+ for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
+ if (!btdev_list[i] || btdev_list[i] == btdev)
+ continue;
+
+ if (!btdev_list[i]->le_scan_enable)
+ continue;
+
+ le_send_adv_report(btdev_list[i], btdev);
+ }
+}
+
+static void le_set_scan_enable_complete(struct btdev *btdev)
+{
+ int i;
+
+ for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
+ if (!btdev_list[i] || btdev_list[i] == btdev)
+ continue;
+
+ if (!btdev_list[i]->le_adv_enable)
+ continue;
+
+ le_send_adv_report(btdev, btdev_list[i]);
+ }
+}
+
+static void default_cmd(struct btdev *btdev, uint16_t opcode,
+ const void *data, uint8_t len)
{
- const struct bt_hci_cmd_hdr *hdr = data;
- const struct bt_hci_cmd_create_conn *cc;
- const struct bt_hci_cmd_disconnect *dc;
- const struct bt_hci_cmd_create_conn_cancel *ccc;
- const struct bt_hci_cmd_accept_conn_request *acr;
- const struct bt_hci_cmd_reject_conn_request *rcr;
- const struct bt_hci_cmd_remote_name_request *rnr;
const struct bt_hci_cmd_remote_name_request_cancel *rnrc;
- const struct bt_hci_cmd_read_remote_features *rrf;
- const struct bt_hci_cmd_read_remote_ext_features *rref;
- const struct bt_hci_cmd_read_remote_version *rrv;
const struct bt_hci_cmd_write_default_link_policy *wdlp;
const struct bt_hci_cmd_set_event_mask *sem;
const struct bt_hci_cmd_set_event_filter *sef;
@@ -586,15 +1041,24 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
const struct bt_hci_cmd_write_conn_accept_timeout *wcat;
const struct bt_hci_cmd_write_page_timeout *wpt;
const struct bt_hci_cmd_write_scan_enable *wse;
+ const struct bt_hci_cmd_write_page_scan_activity *wpsa;
+ const struct bt_hci_cmd_write_inquiry_scan_activity *wisa;
+ const struct bt_hci_cmd_write_page_scan_type *wpst;
const struct bt_hci_cmd_write_auth_enable *wae;
const struct bt_hci_cmd_write_class_of_dev *wcod;
const struct bt_hci_cmd_write_voice_setting *wvs;
const struct bt_hci_cmd_write_inquiry_mode *wim;
- const struct bt_hci_cmd_write_afh_assess_mode *waam;
- const struct bt_hci_cmd_write_ext_inquiry_rsp *weir;
+ const struct bt_hci_cmd_write_afh_assessment_mode *waam;
+ const struct bt_hci_cmd_write_ext_inquiry_response *weir;
const struct bt_hci_cmd_write_simple_pairing_mode *wspm;
const struct bt_hci_cmd_write_le_host_supported *wlhs;
+ const struct bt_hci_cmd_set_event_mask_page2 *semp2;
const struct bt_hci_cmd_le_set_event_mask *lsem;
+ const struct bt_hci_cmd_le_set_adv_data *lsad;
+ const struct bt_hci_cmd_setup_sync_conn *ssc;
+ const struct bt_hci_cmd_le_set_adv_enable *lsae;
+ const struct bt_hci_cmd_le_set_scan_enable *lsse;
+ const struct bt_hci_cmd_read_local_amp_assoc *rlaa_cmd;
struct bt_hci_rsp_read_default_link_policy rdlp;
struct bt_hci_rsp_read_stored_link_key rslk;
struct bt_hci_rsp_write_stored_link_key wslk;
@@ -603,15 +1067,22 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
struct bt_hci_rsp_read_conn_accept_timeout rcat;
struct bt_hci_rsp_read_page_timeout rpt;
struct bt_hci_rsp_read_scan_enable rse;
+ struct bt_hci_rsp_read_page_scan_activity rpsa;
+ struct bt_hci_rsp_read_inquiry_scan_activity risa;
+ struct bt_hci_rsp_read_page_scan_type rpst;
struct bt_hci_rsp_read_auth_enable rae;
struct bt_hci_rsp_read_class_of_dev rcod;
struct bt_hci_rsp_read_voice_setting rvs;
+ struct bt_hci_rsp_read_num_supported_iac rnsi;
+ struct bt_hci_rsp_read_current_iac_lap *rcil;
struct bt_hci_rsp_read_inquiry_mode rim;
- struct bt_hci_rsp_read_afh_assess_mode raam;
- struct bt_hci_rsp_read_ext_inquiry_rsp reir;
+ struct bt_hci_rsp_read_afh_assessment_mode raam;
+ struct bt_hci_rsp_read_ext_inquiry_response reir;
struct bt_hci_rsp_read_simple_pairing_mode rspm;
- struct bt_hci_rsp_read_inquiry_rsp_tx_power rirtp;
+ struct bt_hci_rsp_read_local_oob_data rlod;
+ struct bt_hci_rsp_read_inquiry_resp_tx_power rirtp;
struct bt_hci_rsp_read_le_host_supported rlhs;
+ struct bt_hci_rsp_read_sync_train_params rstp;
struct bt_hci_rsp_read_local_version rlv;
struct bt_hci_rsp_read_local_commands rlc;
struct bt_hci_rsp_read_local_features rlf;
@@ -620,106 +1091,110 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
struct bt_hci_rsp_read_country_code rcc;
struct bt_hci_rsp_read_bd_addr rba;
struct bt_hci_rsp_read_data_block_size rdbs;
+ struct bt_hci_rsp_read_local_amp_info rlai;
+ struct bt_hci_rsp_read_local_amp_assoc rlaa_rsp;
struct bt_hci_rsp_le_read_buffer_size lrbs;
struct bt_hci_rsp_le_read_local_features lrlf;
+ struct bt_hci_rsp_le_read_adv_tx_power lratp;
struct bt_hci_rsp_le_read_supported_states lrss;
- uint16_t opcode;
+ struct bt_hci_rsp_le_read_white_list_size lrwls;
+ struct bt_hci_rsp_le_rand lr;
+ struct bt_hci_rsp_le_test_end lte;
+ struct bt_hci_rsp_remote_name_request_cancel rnrc_rsp;
uint8_t status, page;
- if (len < sizeof(*hdr))
- return;
-
- opcode = le16_to_cpu(hdr->opcode);
-
switch (opcode) {
case BT_HCI_CMD_INQUIRY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- inquiry_complete(btdev, BT_HCI_ERR_SUCCESS);
break;
case BT_HCI_CMD_INQUIRY_CANCEL:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_CREATE_CONN:
- cc = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- conn_request(btdev, cc->bdaddr);
break;
case BT_HCI_CMD_DISCONNECT:
- dc = data + sizeof(*hdr);
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- disconnect_complete(btdev, le16_to_cpu(dc->handle), dc->reason);
break;
case BT_HCI_CMD_CREATE_CONN_CANCEL:
- ccc = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- conn_complete(btdev, ccc->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
break;
case BT_HCI_CMD_ACCEPT_CONN_REQUEST:
- acr = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- conn_complete(btdev, acr->bdaddr, BT_HCI_ERR_SUCCESS);
break;
case BT_HCI_CMD_REJECT_CONN_REQUEST:
- rcr = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- conn_complete(btdev, rcr->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
break;
case BT_HCI_CMD_REMOTE_NAME_REQUEST:
- rnr = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- name_request_complete(btdev, rnr->bdaddr, BT_HCI_ERR_SUCCESS);
break;
case BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL:
- rnrc = data + sizeof(*hdr);
- status = BT_HCI_ERR_SUCCESS;
- cmd_complete(btdev, opcode, &status, sizeof(status));
- name_request_complete(btdev, rnrc->bdaddr,
- BT_HCI_ERR_UNKNOWN_CONN_ID);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ rnrc = data;
+ rnrc_rsp.status = BT_HCI_ERR_SUCCESS;
+ memcpy(rnrc_rsp.bdaddr, rnrc->bdaddr, 6);
+ cmd_complete(btdev, opcode, &rnrc_rsp, sizeof(rnrc_rsp));
break;
case BT_HCI_CMD_READ_REMOTE_FEATURES:
- rrf = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- remote_features_complete(btdev, le16_to_cpu(rrf->handle));
break;
case BT_HCI_CMD_READ_REMOTE_EXT_FEATURES:
- rref = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- remote_ext_features_complete(btdev, le16_to_cpu(rref->handle),
- rref->page);
break;
case BT_HCI_CMD_READ_REMOTE_VERSION:
- rrv = data + sizeof(*hdr);
cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
- remote_version_complete(btdev, le16_to_cpu(rrv->handle));
break;
case BT_HCI_CMD_READ_DEFAULT_LINK_POLICY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rdlp.status = BT_HCI_ERR_SUCCESS;
rdlp.policy = cpu_to_le16(btdev->default_link_policy);
cmd_complete(btdev, opcode, &rdlp, sizeof(rdlp));
break;
case BT_HCI_CMD_WRITE_DEFAULT_LINK_POLICY:
- wdlp = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wdlp = data;
btdev->default_link_policy = le16_to_cpu(wdlp->policy);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_SET_EVENT_MASK:
- sem = data + sizeof(*hdr);
+ sem = data;
memcpy(btdev->event_mask, sem->mask, 8);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
@@ -731,13 +1206,17 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_SET_EVENT_FILTER:
- sef = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ sef = data;
btdev->event_filter = sef->type;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_READ_STORED_LINK_KEY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rslk.status = BT_HCI_ERR_SUCCESS;
rslk.max_num_keys = cpu_to_le16(0);
rslk.num_keys = cpu_to_le16(0);
@@ -745,143 +1224,274 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_WRITE_STORED_LINK_KEY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
wslk.status = BT_HCI_ERR_SUCCESS;
wslk.num_keys = 0;
cmd_complete(btdev, opcode, &wslk, sizeof(wslk));
break;
case BT_HCI_CMD_DELETE_STORED_LINK_KEY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
dslk.status = BT_HCI_ERR_SUCCESS;
dslk.num_keys = cpu_to_le16(0);
cmd_complete(btdev, opcode, &dslk, sizeof(dslk));
break;
case BT_HCI_CMD_WRITE_LOCAL_NAME:
- wln = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wln = data;
memcpy(btdev->name, wln->name, 248);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_READ_LOCAL_NAME:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rln.status = BT_HCI_ERR_SUCCESS;
memcpy(rln.name, btdev->name, 248);
cmd_complete(btdev, opcode, &rln, sizeof(rln));
break;
case BT_HCI_CMD_READ_CONN_ACCEPT_TIMEOUT:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rcat.status = BT_HCI_ERR_SUCCESS;
rcat.timeout = cpu_to_le16(btdev->conn_accept_timeout);
cmd_complete(btdev, opcode, &rcat, sizeof(rcat));
break;
case BT_HCI_CMD_WRITE_CONN_ACCEPT_TIMEOUT:
- wcat = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wcat = data;
btdev->conn_accept_timeout = le16_to_cpu(wcat->timeout);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_READ_PAGE_TIMEOUT:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rpt.status = BT_HCI_ERR_SUCCESS;
rpt.timeout = cpu_to_le16(btdev->page_timeout);
cmd_complete(btdev, opcode, &rpt, sizeof(rpt));
break;
case BT_HCI_CMD_WRITE_PAGE_TIMEOUT:
- wpt = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wpt = data;
btdev->page_timeout = le16_to_cpu(wpt->timeout);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_READ_SCAN_ENABLE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rse.status = BT_HCI_ERR_SUCCESS;
rse.enable = btdev->scan_enable;
cmd_complete(btdev, opcode, &rse, sizeof(rse));
break;
case BT_HCI_CMD_WRITE_SCAN_ENABLE:
- wse = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wse = data;
btdev->scan_enable = wse->enable;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
+ case BT_HCI_CMD_READ_PAGE_SCAN_ACTIVITY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ rpsa.status = BT_HCI_ERR_SUCCESS;
+ rpsa.interval = cpu_to_le16(btdev->page_scan_interval);
+ rpsa.window = cpu_to_le16(btdev->page_scan_window);
+ cmd_complete(btdev, opcode, &rpsa, sizeof(rpsa));
+ break;
+
+ case BT_HCI_CMD_WRITE_PAGE_SCAN_ACTIVITY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wpsa = data;
+ btdev->page_scan_interval = le16_to_cpu(wpsa->interval);
+ btdev->page_scan_window = le16_to_cpu(wpsa->window);
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_READ_INQUIRY_SCAN_ACTIVITY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ risa.status = BT_HCI_ERR_SUCCESS;
+ risa.interval = cpu_to_le16(btdev->inquiry_scan_interval);
+ risa.window = cpu_to_le16(btdev->inquiry_scan_window);
+ cmd_complete(btdev, opcode, &risa, sizeof(risa));
+ break;
+
+ case BT_HCI_CMD_WRITE_INQUIRY_SCAN_ACTIVITY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wisa = data;
+ btdev->inquiry_scan_interval = le16_to_cpu(wisa->interval);
+ btdev->inquiry_scan_window = le16_to_cpu(wisa->window);
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_READ_PAGE_SCAN_TYPE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ rpst.status = BT_HCI_ERR_SUCCESS;
+ rpst.type = btdev->page_scan_type;
+ cmd_complete(btdev, opcode, &rpst, sizeof(rpst));
+ break;
+
+ case BT_HCI_CMD_WRITE_PAGE_SCAN_TYPE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wpst = data;
+ btdev->page_scan_type = wpst->type;
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
case BT_HCI_CMD_READ_AUTH_ENABLE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rae.status = BT_HCI_ERR_SUCCESS;
rae.enable = btdev->auth_enable;
cmd_complete(btdev, opcode, &rae, sizeof(rae));
break;
case BT_HCI_CMD_WRITE_AUTH_ENABLE:
- wae = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wae = data;
btdev->auth_enable = wae->enable;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_READ_CLASS_OF_DEV:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rcod.status = BT_HCI_ERR_SUCCESS;
memcpy(rcod.dev_class, btdev->dev_class, 3);
cmd_complete(btdev, opcode, &rcod, sizeof(rcod));
break;
case BT_HCI_CMD_WRITE_CLASS_OF_DEV:
- wcod = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wcod = data;
memcpy(btdev->dev_class, wcod->dev_class, 3);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_READ_VOICE_SETTING:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rvs.status = BT_HCI_ERR_SUCCESS;
rvs.setting = cpu_to_le16(btdev->voice_setting);
cmd_complete(btdev, opcode, &rvs, sizeof(rvs));
break;
case BT_HCI_CMD_WRITE_VOICE_SETTING:
- wvs = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wvs = data;
btdev->voice_setting = le16_to_cpu(wvs->setting);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
+ case BT_HCI_CMD_HOST_BUFFER_SIZE:
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_READ_NUM_SUPPORTED_IAC:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ rnsi.status = BT_HCI_ERR_SUCCESS;
+ rnsi.num_iac = 0x01;
+ cmd_complete(btdev, opcode, &rnsi, sizeof(rnsi));
+ break;
+
+ case BT_HCI_CMD_READ_CURRENT_IAC_LAP:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ rcil = alloca(sizeof(*rcil) + 3);
+ rcil->status = BT_HCI_ERR_SUCCESS;
+ rcil->num_iac = 0x01;
+ rcil->iac_lap[0] = 0x33;
+ rcil->iac_lap[1] = 0x8b;
+ rcil->iac_lap[2] = 0x9e;
+ cmd_complete(btdev, opcode, rcil, sizeof(*rcil) + 3);
+ break;
+
+ case BT_HCI_CMD_WRITE_CURRENT_IAC_LAP:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
case BT_HCI_CMD_READ_INQUIRY_MODE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rim.status = BT_HCI_ERR_SUCCESS;
rim.mode = btdev->inquiry_mode;
cmd_complete(btdev, opcode, &rim, sizeof(rim));
break;
case BT_HCI_CMD_WRITE_INQUIRY_MODE:
- wim = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wim = data;
btdev->inquiry_mode = wim->mode;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
- case BT_HCI_CMD_READ_AFH_ASSESS_MODE:
+ case BT_HCI_CMD_READ_AFH_ASSESSMENT_MODE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
raam.status = BT_HCI_ERR_SUCCESS;
- raam.mode = btdev->afh_assess_mode;
+ raam.mode = btdev->afh_assessment_mode;
cmd_complete(btdev, opcode, &raam, sizeof(raam));
break;
- case BT_HCI_CMD_WRITE_AFH_ASSESS_MODE:
- waam = data + sizeof(*hdr);
- btdev->afh_assess_mode = waam->mode;
+ case BT_HCI_CMD_WRITE_AFH_ASSESSMENT_MODE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ waam = data;
+ btdev->afh_assessment_mode = waam->mode;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
- case BT_HCI_CMD_READ_EXT_INQUIRY_RSP:
+ case BT_HCI_CMD_READ_EXT_INQUIRY_RESPONSE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
reir.status = BT_HCI_ERR_SUCCESS;
reir.fec = btdev->ext_inquiry_fec;
memcpy(reir.data, btdev->ext_inquiry_rsp, 240);
cmd_complete(btdev, opcode, &reir, sizeof(reir));
break;
- case BT_HCI_CMD_WRITE_EXT_INQUIRY_RSP:
- weir = data + sizeof(*hdr);
+ case BT_HCI_CMD_WRITE_EXT_INQUIRY_RESPONSE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ weir = data;
btdev->ext_inquiry_fec = weir->fec;
memcpy(btdev->ext_inquiry_rsp, weir->data, 240);
status = BT_HCI_ERR_SUCCESS;
@@ -889,25 +1499,40 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_READ_SIMPLE_PAIRING_MODE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rspm.status = BT_HCI_ERR_SUCCESS;
rspm.mode = btdev->simple_pairing_mode;
cmd_complete(btdev, opcode, &rspm, sizeof(rspm));
break;
case BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE:
- wspm = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ wspm = data;
btdev->simple_pairing_mode = wspm->mode;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
- case BT_HCI_CMD_READ_INQUIRY_RSP_TX_POWER:
+ case BT_HCI_CMD_READ_LOCAL_OOB_DATA:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ rlod.status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &rlod, sizeof(rlod));
+ break;
+
+ case BT_HCI_CMD_READ_INQUIRY_RESP_TX_POWER:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rirtp.status = BT_HCI_ERR_SUCCESS;
rirtp.level = 0;
cmd_complete(btdev, opcode, &rirtp, sizeof(rirtp));
break;
case BT_HCI_CMD_READ_LE_HOST_SUPPORTED:
+ if (btdev->type != BTDEV_TYPE_BREDRLE)
+ goto unsupported;
rlhs.status = BT_HCI_ERR_SUCCESS;
rlhs.supported = btdev->le_supported;
rlhs.simultaneous = btdev->le_simultaneous;
@@ -915,13 +1540,25 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED:
- wlhs = data + sizeof(*hdr);
+ if (btdev->type != BTDEV_TYPE_BREDRLE)
+ goto unsupported;
+ wlhs = data;
btdev->le_supported = wlhs->supported;
btdev->le_simultaneous = wlhs->simultaneous;
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
+ case BT_HCI_CMD_READ_SYNC_TRAIN_PARAMS:
+ if (btdev->type != BTDEV_TYPE_BREDRLE)
+ goto unsupported;
+ rstp.status = BT_HCI_ERR_SUCCESS;
+ rstp.interval = cpu_to_le16(btdev->sync_train_interval);
+ rstp.timeout = cpu_to_le32(btdev->sync_train_timeout);
+ rstp.service_data = btdev->sync_train_service_data;
+ cmd_complete(btdev, opcode, &rstp, sizeof(rstp));
+ break;
+
case BT_HCI_CMD_READ_LOCAL_VERSION:
rlv.status = BT_HCI_ERR_SUCCESS;
rlv.hci_ver = btdev->version;
@@ -945,18 +1582,28 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_READ_LOCAL_EXT_FEATURES:
- page = ((const uint8_t *) data)[sizeof(*hdr)];
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+
+ page = ((const uint8_t *) data)[0];
+
+ rlef.page = page;
+ rlef.max_page = btdev->max_page;
+
+ if (page > btdev->max_page) {
+ rlef.status = BT_HCI_ERR_INVALID_PARAMETERS;
+ memset(rlef.features, 0, 8);
+ cmd_complete(btdev, opcode, &rlef, sizeof(rlef));
+ break;
+ }
+
switch (page) {
case 0x00:
rlef.status = BT_HCI_ERR_SUCCESS;
- rlef.page = 0x00;
- rlef.max_page = 0x01;
memcpy(rlef.features, btdev->features, 8);
break;
case 0x01:
rlef.status = BT_HCI_ERR_SUCCESS;
- rlef.page = 0x01;
- rlef.max_page = 0x01;
memset(rlef.features, 0, 8);
if (btdev->simple_pairing_mode)
rlef.features[0] |= 0x01;
@@ -965,10 +1612,12 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
if (btdev->le_simultaneous)
rlef.features[0] |= 0x04;
break;
+ case 0x02:
+ rlef.status = BT_HCI_ERR_SUCCESS;
+ memcpy(rlef.features, btdev->feat_page_2, 8);
+ break;
default:
rlef.status = BT_HCI_ERR_INVALID_PARAMETERS;
- rlef.page = page;
- rlef.max_page = 0x01;
memset(rlef.features, 0, 8);
break;
}
@@ -997,6 +1646,8 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_READ_DATA_BLOCK_SIZE:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
rdbs.status = BT_HCI_ERR_SUCCESS;
rdbs.max_acl_len = cpu_to_le16(btdev->acl_mtu);
rdbs.block_len = cpu_to_le16(btdev->acl_mtu);
@@ -1004,14 +1655,57 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
cmd_complete(btdev, opcode, &rdbs, sizeof(rdbs));
break;
+ case BT_HCI_CMD_READ_LOCAL_AMP_INFO:
+ if (btdev->type != BTDEV_TYPE_AMP)
+ goto unsupported;
+ rlai.status = BT_HCI_ERR_SUCCESS;
+ rlai.amp_status = 0x01; /* Used for Bluetooth only */
+ rlai.total_bw = cpu_to_le32(0);
+ rlai.max_bw = cpu_to_le32(0);
+ rlai.min_latency = cpu_to_le32(0);
+ rlai.max_pdu = cpu_to_le32(672);
+ rlai.amp_type = 0x01; /* 802.11 AMP Controller */
+ rlai.pal_cap = cpu_to_le16(0x0000);
+ rlai.max_assoc_len = cpu_to_le16(672);
+ rlai.max_flush_to = cpu_to_le32(0xffffffff);
+ rlai.be_flush_to = cpu_to_le32(0xffffffff);
+ cmd_complete(btdev, opcode, &rlai, sizeof(rlai));
+ break;
+
+ case BT_HCI_CMD_READ_LOCAL_AMP_ASSOC:
+ if (btdev->type != BTDEV_TYPE_AMP)
+ goto unsupported;
+ rlaa_cmd = data;
+ rlaa_rsp.status = BT_HCI_ERR_SUCCESS;
+ rlaa_rsp.phy_handle = rlaa_cmd->phy_handle;
+ rlaa_rsp.remain_assoc_len = cpu_to_le16(1);
+ rlaa_rsp.assoc_fragment[0] = 0x42;
+ memset(rlaa_rsp.assoc_fragment + 1, 0,
+ sizeof(rlaa_rsp.assoc_fragment) - 1);
+ cmd_complete(btdev, opcode, &rlaa_rsp, sizeof(rlaa_rsp));
+ break;
+
+ case BT_HCI_CMD_SET_EVENT_MASK_PAGE2:
+ if (btdev->type != BTDEV_TYPE_BREDRLE)
+ goto unsupported;
+ semp2 = data;
+ memcpy(btdev->event_mask_page2, semp2->mask, 8);
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
case BT_HCI_CMD_LE_SET_EVENT_MASK:
- lsem = data + sizeof(*hdr);
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lsem = data;
memcpy(btdev->le_event_mask, lsem->mask, 8);
status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_LE_READ_BUFFER_SIZE:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
lrbs.status = BT_HCI_ERR_SUCCESS;
lrbs.le_mtu = cpu_to_le16(btdev->acl_mtu);
lrbs.le_max_pkt = btdev->acl_max_pkt;
@@ -1019,35 +1713,353 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
break;
case BT_HCI_CMD_LE_READ_LOCAL_FEATURES:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
lrlf.status = BT_HCI_ERR_SUCCESS;
memcpy(lrlf.features, btdev->le_features, 8);
cmd_complete(btdev, opcode, &lrlf, sizeof(lrlf));
break;
+ case BT_HCI_CMD_LE_SET_ADV_PARAMETERS:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ if (btdev->le_adv_enable)
+ status = BT_HCI_ERR_COMMAND_DISALLOWED;
+ else
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_LE_READ_ADV_TX_POWER:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lratp.status = BT_HCI_ERR_SUCCESS;
+ lratp.level = 0;
+ cmd_complete(btdev, opcode, &lratp, sizeof(lratp));
+ break;
+
+ case BT_HCI_CMD_LE_SET_ADV_ENABLE:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lsae = data;
+ if (btdev->le_adv_enable == lsae->enable)
+ status = BT_HCI_ERR_COMMAND_DISALLOWED;
+ else {
+ btdev->le_adv_enable = lsae->enable;
+ status = BT_HCI_ERR_SUCCESS;
+ }
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ if (status == BT_HCI_ERR_SUCCESS && btdev->le_adv_enable)
+ le_set_adv_enable_complete(btdev);
+ break;
+
case BT_HCI_CMD_LE_SET_SCAN_PARAMETERS:
- status = BT_HCI_ERR_SUCCESS;
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ if (btdev->le_scan_enable)
+ status = BT_HCI_ERR_COMMAND_DISALLOWED;
+ else
+ status = BT_HCI_ERR_SUCCESS;
cmd_complete(btdev, opcode, &status, sizeof(status));
break;
case BT_HCI_CMD_LE_SET_SCAN_ENABLE:
- status = BT_HCI_ERR_SUCCESS;
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lsse = data;
+ if (btdev->le_scan_enable == lsse->enable)
+ status = BT_HCI_ERR_COMMAND_DISALLOWED;
+ else {
+ btdev->le_scan_enable = lsse->enable;
+ btdev->le_filter_dup = lsse->filter_dup;
+ status = BT_HCI_ERR_SUCCESS;
+ }
cmd_complete(btdev, opcode, &status, sizeof(status));
+ if (status == BT_HCI_ERR_SUCCESS && btdev->le_scan_enable)
+ le_set_scan_enable_complete(btdev);
+ break;
+
+ case BT_HCI_CMD_LE_CREATE_CONN:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
+ break;
+
+ case BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lrwls.status = BT_HCI_ERR_SUCCESS;
+ lrwls.size = 0;
+ cmd_complete(btdev, opcode, &lrwls, sizeof(lrwls));
break;
case BT_HCI_CMD_LE_READ_SUPPORTED_STATES:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
lrss.status = BT_HCI_ERR_SUCCESS;
memcpy(lrss.states, btdev->le_states, 8);
cmd_complete(btdev, opcode, &lrss, sizeof(lrss));
break;
+ case BT_HCI_CMD_LE_SET_ADV_DATA:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lsad = data;
+ btdev->le_adv_data_len = lsad->len;
+ memcpy(btdev->le_adv_data, lsad->data, 31);
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_LE_RAND:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lr.status = BT_HCI_ERR_SUCCESS;
+ lr.number[0] = rand();
+ lr.number[1] = rand();
+ lr.number[2] = rand();
+ lr.number[3] = rand();
+ lr.number[4] = rand();
+ lr.number[5] = rand();
+ lr.number[6] = rand();
+ lr.number[7] = rand();
+ cmd_complete(btdev, opcode, &lr, sizeof(lr));
+ break;
+
+ case BT_HCI_CMD_SETUP_SYNC_CONN:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ ssc = data;
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
+ sync_conn_complete(btdev, ssc->voice_setting,
+ BT_HCI_ERR_SUCCESS);
+ break;
+
+ case BT_HCI_CMD_ADD_SCO_CONN:
+ if (btdev->type == BTDEV_TYPE_LE)
+ goto unsupported;
+ sco_conn_complete(btdev, BT_HCI_ERR_SUCCESS);
+ break;
+
+ case BT_HCI_CMD_ENABLE_DUT_MODE:
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_LE_RECEIVER_TEST:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_LE_TRANSMITTER_TEST:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(btdev, opcode, &status, sizeof(status));
+ break;
+
+ case BT_HCI_CMD_LE_TEST_END:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ goto unsupported;
+ lte.status = BT_HCI_ERR_SUCCESS;
+ lte.num_packets = 0;
+ cmd_complete(btdev, opcode, &lte, sizeof(lte));
+ break;
+
default:
- printf("Unsupported command 0x%4.4x\n", opcode);
- hexdump(data, len);
- cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
+ goto unsupported;
+ }
+
+ return;
+
+unsupported:
+ printf("Unsupported command 0x%4.4x\n", opcode);
+ hexdump(data, len);
+ cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
+}
+
+static void default_cmd_completion(struct btdev *btdev, uint16_t opcode,
+ const void *data, uint8_t len)
+{
+ const struct bt_hci_cmd_create_conn *cc;
+ const struct bt_hci_cmd_disconnect *dc;
+ const struct bt_hci_cmd_create_conn_cancel *ccc;
+ const struct bt_hci_cmd_accept_conn_request *acr;
+ const struct bt_hci_cmd_reject_conn_request *rcr;
+ const struct bt_hci_cmd_remote_name_request *rnr;
+ const struct bt_hci_cmd_remote_name_request_cancel *rnrc;
+ const struct bt_hci_cmd_read_remote_features *rrf;
+ const struct bt_hci_cmd_read_remote_ext_features *rref;
+ const struct bt_hci_cmd_read_remote_version *rrv;
+ const struct bt_hci_cmd_le_create_conn *lecc;
+
+ switch (opcode) {
+ case BT_HCI_CMD_INQUIRY:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ inquiry_complete(btdev, BT_HCI_ERR_SUCCESS);
+ break;
+
+ case BT_HCI_CMD_CREATE_CONN:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ cc = data;
+ conn_request(btdev, cc->bdaddr);
+ break;
+
+ case BT_HCI_CMD_DISCONNECT:
+ dc = data;
+ disconnect_complete(btdev, le16_to_cpu(dc->handle), dc->reason);
+ break;
+
+ case BT_HCI_CMD_CREATE_CONN_CANCEL:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ ccc = data;
+ conn_complete(btdev, ccc->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
+ break;
+
+ case BT_HCI_CMD_ACCEPT_CONN_REQUEST:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ acr = data;
+ conn_complete(btdev, acr->bdaddr, BT_HCI_ERR_SUCCESS);
+ break;
+
+ case BT_HCI_CMD_REJECT_CONN_REQUEST:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ rcr = data;
+ conn_complete(btdev, rcr->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
+ break;
+
+ case BT_HCI_CMD_REMOTE_NAME_REQUEST:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ rnr = data;
+ name_request_complete(btdev, rnr->bdaddr, BT_HCI_ERR_SUCCESS);
+ break;
+
+ case BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ rnrc = data;
+ name_request_complete(btdev, rnrc->bdaddr,
+ BT_HCI_ERR_UNKNOWN_CONN_ID);
+ break;
+
+ case BT_HCI_CMD_READ_REMOTE_FEATURES:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ rrf = data;
+ remote_features_complete(btdev, le16_to_cpu(rrf->handle));
+ break;
+
+ case BT_HCI_CMD_READ_REMOTE_EXT_FEATURES:
+ if (btdev->type == BTDEV_TYPE_LE)
+ return;
+ rref = data;
+ remote_ext_features_complete(btdev, le16_to_cpu(rref->handle),
+ rref->page);
+ break;
+
+ case BT_HCI_CMD_READ_REMOTE_VERSION:
+ rrv = data;
+ remote_version_complete(btdev, le16_to_cpu(rrv->handle));
+ break;
+
+ case BT_HCI_CMD_LE_CREATE_CONN:
+ if (btdev->type == BTDEV_TYPE_BREDR)
+ return;
+ lecc = data;
+ le_conn_request(btdev, lecc->peer_addr);
+ break;
+ }
+}
+
+struct btdev_callback {
+ void (*function)(btdev_callback callback, uint8_t response,
+ uint8_t status, const void *data, uint8_t len);
+ void *user_data;
+ uint16_t opcode;
+ const void *data;
+ uint8_t len;
+};
+
+void btdev_command_response(btdev_callback callback, uint8_t response,
+ uint8_t status, const void *data, uint8_t len)
+{
+ callback->function(callback, response, status, data, len);
+}
+
+static void handler_callback(btdev_callback callback, uint8_t response,
+ uint8_t status, const void *data, uint8_t len)
+{
+ struct btdev *btdev = callback->user_data;
+
+ switch (response) {
+ case BTDEV_RESPONSE_DEFAULT:
+ if (!run_hooks(btdev, BTDEV_HOOK_PRE_CMD, callback->opcode,
+ callback->data, callback->len))
+ return;
+ default_cmd(btdev, callback->opcode,
+ callback->data, callback->len);
+
+ if (!run_hooks(btdev, BTDEV_HOOK_PRE_EVT, callback->opcode,
+ callback->data, callback->len))
+ return;
+ default_cmd_completion(btdev, callback->opcode,
+ callback->data, callback->len);
+ break;
+ case BTDEV_RESPONSE_COMMAND_STATUS:
+ cmd_status(btdev, status, callback->opcode);
+ break;
+ case BTDEV_RESPONSE_COMMAND_COMPLETE:
+ cmd_complete(btdev, callback->opcode, data, len);
+ break;
+ default:
+ cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND,
+ callback->opcode);
break;
}
}
+static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
+{
+ struct btdev_callback callback;
+ const struct bt_hci_cmd_hdr *hdr = data;
+
+ if (len < sizeof(*hdr))
+ return;
+
+ callback.function = handler_callback;
+ callback.user_data = btdev;
+ callback.opcode = le16_to_cpu(hdr->opcode);
+ callback.data = data + sizeof(*hdr);
+ callback.len = hdr->plen;
+
+ if (btdev->command_handler)
+ btdev->command_handler(callback.opcode,
+ callback.data, callback.len,
+ &callback, btdev->command_data);
+ else {
+ if (!run_hooks(btdev, BTDEV_HOOK_PRE_CMD, callback.opcode,
+ callback.data, callback.len))
+ return;
+ default_cmd(btdev, callback.opcode,
+ callback.data, callback.len);
+
+ if (!run_hooks(btdev, BTDEV_HOOK_PRE_EVT, callback.opcode,
+ callback.data, callback.len))
+ return;
+ default_cmd_completion(btdev, callback.opcode,
+ callback.data, callback.len);
+ }
+}
+
void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
{
uint8_t pkt_type;
@@ -1074,3 +2086,57 @@ void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
break;
}
}
+
+int btdev_add_hook(struct btdev *btdev, enum btdev_hook_type type,
+ uint16_t opcode, btdev_hook_func handler,
+ void *user_data)
+{
+ int i;
+
+ if (!btdev)
+ return -1;
+
+ if (get_hook_index(btdev, type, opcode) > 0)
+ return -1;
+
+ for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
+ if (btdev->hook_list[i] == NULL) {
+ btdev->hook_list[i] = malloc(sizeof(struct hook));
+ if (btdev->hook_list[i] == NULL)
+ return -1;
+
+ btdev->hook_list[i]->handler = handler;
+ btdev->hook_list[i]->user_data = user_data;
+ btdev->hook_list[i]->opcode = opcode;
+ btdev->hook_list[i]->type = type;
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+bool btdev_del_hook(struct btdev *btdev, enum btdev_hook_type type,
+ uint16_t opcode)
+{
+ int i;
+
+ if (!btdev)
+ return false;
+
+ for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
+ if (btdev->hook_list[i] == NULL)
+ continue;
+
+ if (btdev->hook_list[i]->type != type ||
+ btdev->hook_list[i]->opcode != opcode)
+ continue;
+
+ free(btdev->hook_list[i]);
+ btdev->hook_list[i] = NULL;
+
+ return true;
+ }
+
+ return false;
+}
diff --git a/emulator/btdev.h b/emulator/btdev.h
index 7b211a2c..1e623f49 100644
--- a/emulator/btdev.h
+++ b/emulator/btdev.h
@@ -6,33 +6,90 @@
* Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
*
- * This program is distributed in the hope that it will be useful,
+ * This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
*
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdint.h>
+#include <stdbool.h>
+
+#define BTDEV_RESPONSE_DEFAULT 0
+#define BTDEV_RESPONSE_COMMAND_STATUS 1
+#define BTDEV_RESPONSE_COMMAND_COMPLETE 2
+
+typedef struct btdev_callback * btdev_callback;
+
+void btdev_command_response(btdev_callback callback, uint8_t response,
+ uint8_t status, const void *data, uint8_t len);
+
+#define btdev_command_default(callback) \
+ btdev_command_response(callback, \
+ BTDEV_RESPONSE_DEFAULT, 0x00, NULL, 0);
+
+#define btdev_command_status(callback, status) \
+ btdev_command_response(callback, \
+ BTDEV_RESPONSE_COMMAND_STATUS, status, NULL, 0);
+
+#define btdev_command_complete(callback, data, len) \
+ btdev_command_response(callback, \
+ BTDEV_RESPONSE_COMMAND_COMPLETE, 0x00, data, len);
+
+
+typedef void (*btdev_command_func) (uint16_t opcode,
+ const void *data, uint8_t len,
+ btdev_callback callback, void *user_data);
typedef void (*btdev_send_func) (const void *data, uint16_t len,
void *user_data);
+typedef bool (*btdev_hook_func) (const void *data, uint16_t len,
+ void *user_data);
+
+enum btdev_type {
+ BTDEV_TYPE_BREDRLE,
+ BTDEV_TYPE_BREDR,
+ BTDEV_TYPE_LE,
+ BTDEV_TYPE_AMP,
+};
+
+enum btdev_hook_type {
+ BTDEV_HOOK_PRE_CMD,
+ BTDEV_HOOK_POST_CMD,
+ BTDEV_HOOK_PRE_EVT,
+ BTDEV_HOOK_POST_EVT,
+};
+
struct btdev;
-struct btdev *btdev_create(uint16_t id);
+struct btdev *btdev_create(enum btdev_type type, uint16_t id);
void btdev_destroy(struct btdev *btdev);
+const uint8_t *btdev_get_bdaddr(struct btdev *btdev);
+uint8_t *btdev_get_features(struct btdev *btdev);
+
+void btdev_set_command_handler(struct btdev *btdev, btdev_command_func handler,
+ void *user_data);
+
void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
void *user_data);
void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len);
+
+int btdev_add_hook(struct btdev *btdev, enum btdev_hook_type type,
+ uint16_t opcode, btdev_hook_func handler,
+ void *user_data);
+
+bool btdev_del_hook(struct btdev *btdev, enum btdev_hook_type type,
+ uint16_t opcode);
diff --git a/emulator/bthost.c b/emulator/bthost.c
new file mode 100644
index 00000000..da56b5cb
--- /dev/null
+++ b/emulator/bthost.c
@@ -0,0 +1,1111 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011-2012 Intel Corporation
+ * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <ctype.h>
+#include <stdlib.h>
+#include <string.h>
+#include <endian.h>
+#include <stdbool.h>
+
+#include "bluetooth/bluetooth.h"
+
+#include "monitor/bt.h"
+#include "bthost.h"
+
+/* ACL handle and flags pack/unpack */
+#define acl_handle_pack(h, f) (uint16_t)((h & 0x0fff)|(f << 12))
+#define acl_handle(h) (h & 0x0fff)
+#define acl_flags(h) (h >> 12)
+
+#define le16_to_cpu(val) (val)
+#define le32_to_cpu(val) (val)
+#define cpu_to_le16(val) (val)
+#define cpu_to_le32(val) (val)
+
+struct cmd {
+ struct cmd *next;
+ struct cmd *prev;
+ uint8_t data[256 + sizeof(struct bt_hci_cmd_hdr)];
+ uint16_t len;
+};
+
+struct cmd_queue {
+ struct cmd *head;
+ struct cmd *tail;
+};
+
+struct btconn {
+ uint16_t handle;
+ uint8_t addr_type;
+ uint16_t next_cid;
+ struct l2conn *l2conns;
+ struct btconn *next;
+};
+
+struct l2conn {
+ uint16_t scid;
+ uint16_t dcid;
+ struct l2conn *next;
+};
+
+struct l2cap_pending_req {
+ uint8_t ident;
+ bthost_l2cap_rsp_cb cb;
+ void *user_data;
+ struct l2cap_pending_req *next;
+};
+
+struct bthost {
+ uint8_t bdaddr[6];
+ bthost_send_func send_handler;
+ void *send_data;
+ struct cmd_queue cmd_q;
+ uint8_t ncmd;
+ struct btconn *conns;
+ bthost_cmd_complete_cb cmd_complete_cb;
+ void *cmd_complete_data;
+ bthost_new_conn_cb new_conn_cb;
+ void *new_conn_data;
+ uint16_t server_psm;
+ struct l2cap_pending_req *l2reqs;
+};
+
+struct bthost *bthost_create(void)
+{
+ struct bthost *bthost;
+
+ bthost = malloc(sizeof(*bthost));
+ if (!bthost)
+ return NULL;
+
+ memset(bthost, 0, sizeof(*bthost));
+
+ return bthost;
+}
+
+static void l2conn_free(struct l2conn *conn)
+{
+ free(conn);
+}
+
+static void btconn_free(struct btconn *conn)
+{
+ while (conn->l2conns) {
+ struct l2conn *l2conn = conn->l2conns;
+
+ conn->l2conns = l2conn->next;
+ l2conn_free(l2conn);
+ }
+
+ free(conn);
+}
+
+static struct btconn *bthost_find_conn(struct bthost *bthost, uint16_t handle)
+{
+ struct btconn *conn;
+
+ for (conn = bthost->conns; conn != NULL; conn = conn->next) {
+ if (conn->handle == handle)
+ return conn;
+ }
+
+ return NULL;
+}
+
+static void bthost_add_l2cap_conn(struct bthost *bthost, struct btconn *conn,
+ uint16_t scid, uint16_t dcid)
+{
+ struct l2conn *l2conn;
+
+ l2conn = malloc(sizeof(*l2conn));
+ if (!l2conn)
+ return;
+
+ memset(l2conn, 0, sizeof(*l2conn));
+
+ l2conn->scid = scid;
+ l2conn->dcid = dcid;
+
+ l2conn->next = conn->l2conns;
+ conn->l2conns = l2conn;
+}
+
+static struct l2conn *btconn_find_l2cap_conn_by_scid(struct btconn *conn,
+ uint16_t scid)
+{
+ struct l2conn *l2conn;
+
+ for (l2conn = conn->l2conns; l2conn != NULL; l2conn = l2conn->next) {
+ if (l2conn->scid == scid)
+ return l2conn;
+ }
+
+ return NULL;
+}
+
+void bthost_destroy(struct bthost *bthost)
+{
+ struct cmd *cmd;
+
+ if (!bthost)
+ return;
+
+ for (cmd = bthost->cmd_q.tail; cmd != NULL; cmd = cmd->next)
+ free(cmd);
+
+ while (bthost->conns) {
+ struct btconn *conn = bthost->conns;
+
+ bthost->conns = conn->next;
+ btconn_free(conn);
+ }
+
+ while (bthost->l2reqs) {
+ struct l2cap_pending_req *req = bthost->l2reqs;
+
+ bthost->l2reqs = req->next;
+ req->cb(0, NULL, 0, req->user_data);
+ free(req);
+ }
+
+ free(bthost);
+}
+
+void bthost_set_send_handler(struct bthost *bthost, bthost_send_func handler,
+ void *user_data)
+{
+ if (!bthost)
+ return;
+
+ bthost->send_handler = handler;
+ bthost->send_data = user_data;
+}
+
+static void queue_command(struct bthost *bthost, const void *data,
+ uint16_t len)
+{
+ struct cmd_queue *cmd_q = &bthost->cmd_q;
+ struct cmd *cmd;
+
+ cmd = malloc(sizeof(*cmd));
+ if (!cmd)
+ return;
+
+ memset(cmd, 0, sizeof(*cmd));
+
+ memcpy(cmd->data, data, len);
+ cmd->len = len;
+
+ if (cmd_q->tail)
+ cmd_q->tail->next = cmd;
+
+ cmd->prev = cmd_q->tail;
+ cmd_q->tail = cmd;
+}
+
+static void send_packet(struct bthost *bthost, const void *data, uint16_t len)
+{
+ if (!bthost->send_handler)
+ return;
+
+ bthost->send_handler(data, len, bthost->send_data);
+}
+
+static void send_acl(struct bthost *bthost, uint16_t handle, uint16_t cid,
+ const void *data, uint16_t len)
+{
+ struct bt_hci_acl_hdr *acl_hdr;
+ struct bt_l2cap_hdr *l2_hdr;
+ uint16_t pkt_len;
+ void *pkt_data;
+
+ pkt_len = 1 + sizeof(*acl_hdr) + sizeof(*l2_hdr) + len;
+
+ pkt_data = malloc(pkt_len);
+ if (!pkt_data)
+ return;
+
+ ((uint8_t *) pkt_data)[0] = BT_H4_ACL_PKT;
+
+ acl_hdr = pkt_data + 1;
+ acl_hdr->handle = acl_handle_pack(handle, 0);
+ acl_hdr->dlen = cpu_to_le16(len + sizeof(*l2_hdr));
+
+ l2_hdr = pkt_data + 1 + sizeof(*acl_hdr);
+ l2_hdr->cid = cpu_to_le16(cid);
+ l2_hdr->len = cpu_to_le16(len);
+
+ if (len > 0)
+ memcpy(pkt_data + 1 + sizeof(*acl_hdr) + sizeof(*l2_hdr),
+ data, len);
+
+ send_packet(bthost, pkt_data, pkt_len);
+
+ free(pkt_data);
+}
+
+static uint8_t l2cap_sig_send(struct bthost *bthost, struct btconn *conn,
+ uint8_t code, uint8_t ident,
+ const void *data, uint16_t len)
+{
+ static uint8_t next_ident = 1;
+ struct bt_l2cap_hdr_sig *hdr;
+ uint16_t pkt_len, cid;
+ void *pkt_data;
+
+ pkt_len = sizeof(*hdr) + len;
+
+ pkt_data = malloc(pkt_len);
+ if (!pkt_data)
+ return 0;
+
+ if (!ident) {
+ ident = next_ident++;
+ if (!ident)
+ ident = next_ident++;
+ }
+
+ hdr = pkt_data;
+ hdr->code = code;
+ hdr->ident = ident;
+ hdr->len = cpu_to_le16(len);
+
+ if (len > 0)
+ memcpy(pkt_data + sizeof(*hdr), data, len);
+
+ if (conn->addr_type == BDADDR_BREDR)
+ cid = 0x0001;
+ else
+ cid = 0x0005;
+
+ send_acl(bthost, conn->handle, cid, pkt_data, pkt_len);
+
+ free(pkt_data);
+
+ return ident;
+}
+
+bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
+ const void *data, uint16_t len,
+ bthost_l2cap_rsp_cb cb, void *user_data)
+{
+ struct l2cap_pending_req *req;
+ struct btconn *conn;
+ uint8_t ident;
+
+ conn = bthost_find_conn(bthost, handle);
+ if (!conn)
+ return false;
+
+ ident = l2cap_sig_send(bthost, conn, code, 0, data, len);
+ if (!ident)
+ return false;
+
+ if (!cb)
+ return true;
+
+ req = malloc(sizeof(*req));
+ if (!req)
+ return false;
+
+ memset(req, 0, sizeof(*req));
+ req->ident = ident;
+ req->cb = cb;
+ req->user_data = user_data;
+
+ req->next = bthost->l2reqs;
+ bthost->l2reqs = req;
+
+ return true;
+}
+
+static void send_command(struct bthost *bthost, uint16_t opcode,
+ const void *data, uint8_t len)
+{
+ struct bt_hci_cmd_hdr *hdr;
+ uint16_t pkt_len;
+ void *pkt_data;
+
+ pkt_len = 1 + sizeof(*hdr) + len;
+
+ pkt_data = malloc(pkt_len);
+ if (!pkt_data)
+ return;
+
+ ((uint8_t *) pkt_data)[0] = BT_H4_CMD_PKT;
+
+ hdr = pkt_data + 1;
+ hdr->opcode = cpu_to_le16(opcode);
+ hdr->plen = len;
+
+ if (len > 0)
+ memcpy(pkt_data + 1 + sizeof(*hdr), data, len);
+
+ if (bthost->ncmd) {
+ send_packet(bthost, pkt_data, pkt_len);
+ bthost->ncmd--;
+ } else {
+ queue_command(bthost, pkt_data, pkt_len);
+ }
+
+ free(pkt_data);
+}
+
+static void next_cmd(struct bthost *bthost)
+{
+ struct cmd_queue *cmd_q = &bthost->cmd_q;
+ struct cmd *cmd = cmd_q->tail;
+ struct cmd *next;
+
+ if (!cmd)
+ return;
+
+ next = cmd->next;
+
+ if (!bthost->ncmd)
+ return;
+
+ send_packet(bthost, cmd->data, cmd->len);
+ bthost->ncmd--;
+
+ if (next)
+ next->prev = NULL;
+
+ cmd_q->tail = next;
+
+ free(cmd);
+}
+
+static void read_bd_addr_complete(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_rsp_read_bd_addr *ev = data;
+
+ if (len < sizeof(*ev))
+ return;
+
+ if (ev->status)
+ return;
+
+ memcpy(bthost->bdaddr, ev->bdaddr, 6);
+}
+
+static void evt_cmd_complete(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_cmd_complete *ev = data;
+ const void *param;
+ uint16_t opcode;
+
+ if (len < sizeof(*ev))
+ return;
+
+ param = data + sizeof(*ev);
+
+ bthost->ncmd = ev->ncmd;
+
+ opcode = le16toh(ev->opcode);
+
+ switch (opcode) {
+ case BT_HCI_CMD_RESET:
+ break;
+ case BT_HCI_CMD_READ_BD_ADDR:
+ read_bd_addr_complete(bthost, param, len - sizeof(*ev));
+ break;
+ case BT_HCI_CMD_WRITE_SCAN_ENABLE:
+ break;
+ case BT_HCI_CMD_LE_SET_ADV_ENABLE:
+ break;
+ default:
+ printf("Unhandled cmd_complete opcode 0x%04x\n", opcode);
+ break;
+ }
+
+ if (bthost->cmd_complete_cb)
+ bthost->cmd_complete_cb(opcode, 0, param, len - sizeof(*ev),
+ bthost->cmd_complete_data);
+
+ next_cmd(bthost);
+}
+
+static void evt_cmd_status(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_cmd_status *ev = data;
+ uint16_t opcode;
+
+ if (len < sizeof(*ev))
+ return;
+
+ bthost->ncmd = ev->ncmd;
+
+ opcode = le16toh(ev->opcode);
+
+ if (ev->status && bthost->cmd_complete_cb)
+ bthost->cmd_complete_cb(opcode, ev->status, NULL, 0,
+ bthost->cmd_complete_data);
+
+ next_cmd(bthost);
+}
+
+static void evt_conn_request(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_conn_request *ev = data;
+ struct bt_hci_cmd_accept_conn_request cmd;
+
+ if (len < sizeof(*ev))
+ return;
+
+ memset(&cmd, 0, sizeof(cmd));
+ memcpy(cmd.bdaddr, ev->bdaddr, sizeof(ev->bdaddr));
+
+ send_command(bthost, BT_HCI_CMD_ACCEPT_CONN_REQUEST, &cmd,
+ sizeof(cmd));
+}
+
+static void init_conn(struct bthost *bthost, uint16_t handle, uint8_t addr_type)
+{
+ struct btconn *conn;
+
+ conn = malloc(sizeof(*conn));
+ if (!conn)
+ return;
+
+ memset(conn, 0, sizeof(*conn));
+ conn->handle = handle;
+ conn->addr_type = addr_type;
+ conn->next_cid = 0x0040;
+
+ conn->next = bthost->conns;
+ bthost->conns = conn;
+
+ if (bthost->new_conn_cb)
+ bthost->new_conn_cb(conn->handle, bthost->new_conn_data);
+}
+
+static void evt_conn_complete(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_conn_complete *ev = data;
+
+ if (len < sizeof(*ev))
+ return;
+
+ if (ev->status)
+ return;
+
+ init_conn(bthost, le16_to_cpu(ev->handle), BDADDR_BREDR);
+}
+
+static void evt_disconn_complete(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_disconnect_complete *ev = data;
+ struct btconn **curr;
+ uint16_t handle;
+
+ if (len < sizeof(*ev))
+ return;
+
+ if (ev->status)
+ return;
+
+ handle = le16_to_cpu(ev->handle);
+
+ for (curr = &bthost->conns; *curr;) {
+ struct btconn *conn = *curr;
+
+ if (conn->handle == handle) {
+ *curr = conn->next;
+ btconn_free(conn);
+ } else {
+ curr = &conn->next;
+ }
+ }
+}
+
+static void evt_num_completed_packets(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_num_completed_packets *ev = data;
+
+ if (len < sizeof(*ev))
+ return;
+}
+
+static void evt_le_conn_complete(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const struct bt_hci_evt_le_conn_complete *ev = data;
+ uint8_t addr_type;
+
+ if (len < sizeof(*ev))
+ return;
+
+ if (ev->status)
+ return;
+
+ if (ev->peer_addr_type == 0x00)
+ addr_type = BDADDR_LE_PUBLIC;
+ else
+ addr_type = BDADDR_LE_RANDOM;
+
+ init_conn(bthost, le16_to_cpu(ev->handle), addr_type);
+}
+
+static void evt_le_meta_event(struct bthost *bthost, const void *data,
+ uint8_t len)
+{
+ const uint8_t *event = data;
+ const void *evt_data = data + 1;
+
+ if (len < 1)
+ return;
+
+ switch (*event) {
+ case BT_HCI_EVT_LE_CONN_COMPLETE:
+ evt_le_conn_complete(bthost, evt_data, len - 1);
+ break;
+ default:
+ break;
+ }
+}
+
+static void process_evt(struct bthost *bthost, const void *data, uint16_t len)
+{
+ const struct bt_hci_evt_hdr *hdr = data;
+ const void *param;
+
+ if (len < sizeof(*hdr))
+ return;
+
+ if (sizeof(*hdr) + hdr->plen != len)
+ return;
+
+ param = data + sizeof(*hdr);
+
+ switch (hdr->evt) {
+ case BT_HCI_EVT_CMD_COMPLETE:
+ evt_cmd_complete(bthost, param, hdr->plen);
+ break;
+
+ case BT_HCI_EVT_CMD_STATUS:
+ evt_cmd_status(bthost, param, hdr->plen);
+ break;
+
+ case BT_HCI_EVT_CONN_REQUEST:
+ evt_conn_request(bthost, param, hdr->plen);
+ break;
+
+ case BT_HCI_EVT_CONN_COMPLETE:
+ evt_conn_complete(bthost, param, hdr->plen);
+ break;
+
+ case BT_HCI_EVT_DISCONNECT_COMPLETE:
+ evt_disconn_complete(bthost, param, hdr->plen);
+ break;
+
+ case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
+ evt_num_completed_packets(bthost, param, hdr->plen);
+ break;
+
+ case BT_HCI_EVT_LE_META_EVENT:
+ evt_le_meta_event(bthost, param, hdr->plen);
+ break;
+
+ default:
+ printf("Unsupported event 0x%2.2x\n", hdr->evt);
+ break;
+ }
+}
+
+static bool l2cap_cmd_rej(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_cmd_reject *rsp = data;
+
+ if (len < sizeof(*rsp))
+ return false;
+
+ return true;
+}
+
+static bool l2cap_conn_req(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_conn_req *req = data;
+ struct bt_l2cap_pdu_conn_rsp rsp;
+ uint16_t psm;
+
+ if (len < sizeof(*req))
+ return false;
+
+ psm = le16_to_cpu(req->psm);
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.scid = req->scid;
+
+ if (bthost->server_psm && bthost->server_psm == psm)
+ rsp.dcid = cpu_to_le16(conn->next_cid++);
+ else
+ rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */
+
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_RSP, ident, &rsp,
+ sizeof(rsp));
+
+ if (!rsp.result) {
+ struct bt_l2cap_pdu_config_req conf_req;
+
+ bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(rsp.dcid),
+ le16_to_cpu(rsp.scid));
+
+ memset(&conf_req, 0, sizeof(conf_req));
+ conf_req.dcid = rsp.dcid;
+
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
+ &conf_req, sizeof(conf_req));
+ }
+
+ return true;
+}
+
+static bool l2cap_conn_rsp(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_conn_rsp *rsp = data;
+
+ if (len < sizeof(*rsp))
+ return false;
+
+ bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(rsp->scid),
+ le16_to_cpu(rsp->dcid));
+
+ if (le16_to_cpu(rsp->result) == 0x0001) {
+ struct bt_l2cap_pdu_config_req req;
+
+ memset(&req, 0, sizeof(req));
+ req.dcid = rsp->dcid;
+
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
+ &req, sizeof(req));
+ }
+
+ return true;
+}
+
+static bool l2cap_config_req(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_config_req *req = data;
+ struct bt_l2cap_pdu_config_rsp rsp;
+ struct l2conn *l2conn;
+ uint16_t dcid;
+
+ if (len < sizeof(*req))
+ return false;
+
+ dcid = le16_to_cpu(req->dcid);
+
+ l2conn = btconn_find_l2cap_conn_by_scid(conn, dcid);
+ if (!l2conn)
+ return false;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.scid = cpu_to_le16(l2conn->dcid);
+ rsp.flags = req->flags;
+
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_RSP, ident, &rsp,
+ sizeof(rsp));
+
+ return true;
+}
+
+static bool l2cap_config_rsp(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_config_rsp *rsp = data;
+
+ if (len < sizeof(*rsp))
+ return false;
+
+ return true;
+}
+
+static bool l2cap_disconn_req(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_disconn_req *req = data;
+ struct bt_l2cap_pdu_disconn_rsp rsp;
+
+ if (len < sizeof(*req))
+ return false;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.dcid = req->dcid;
+ rsp.scid = req->scid;
+
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_DISCONN_RSP, ident, &rsp,
+ sizeof(rsp));
+
+ return true;
+}
+
+static bool l2cap_info_req(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_info_req *req = data;
+ struct bt_l2cap_pdu_info_rsp rsp;
+
+ if (len < sizeof(*req))
+ return false;
+
+ rsp.type = req->type;
+ rsp.result = cpu_to_le16(0x0001); /* Not Supported */
+
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_RSP, ident, &rsp,
+ sizeof(rsp));
+
+ return true;
+}
+
+static void handle_pending_l2reqs(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, uint8_t code,
+ const void *data, uint16_t len)
+{
+ struct l2cap_pending_req **curr;
+
+ for (curr = &bthost->l2reqs; *curr != NULL;) {
+ struct l2cap_pending_req *req = *curr;
+
+ if (req->ident != ident) {
+ curr = &req->next;
+ continue;
+ }
+
+ *curr = req->next;
+ req->cb(code, data, len, req->user_data);
+ free(req);
+ }
+}
+
+static void l2cap_sig(struct bthost *bthost, struct btconn *conn,
+ const void *data, uint16_t len)
+{
+ const struct bt_l2cap_hdr_sig *hdr = data;
+ struct bt_l2cap_pdu_cmd_reject rej;
+ uint16_t hdr_len;
+ bool ret;
+
+ if (len < sizeof(*hdr))
+ goto reject;
+
+ hdr_len = le16_to_cpu(hdr->len);
+
+ if (sizeof(*hdr) + hdr_len != len)
+ goto reject;
+
+ switch (hdr->code) {
+ case BT_L2CAP_PDU_CMD_REJECT:
+ ret = l2cap_cmd_rej(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_CONN_REQ:
+ ret = l2cap_conn_req(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_CONN_RSP:
+ ret = l2cap_conn_rsp(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_CONFIG_REQ:
+ ret = l2cap_config_req(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_CONFIG_RSP:
+ ret = l2cap_config_rsp(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_DISCONN_REQ:
+ ret = l2cap_disconn_req(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_INFO_REQ:
+ ret = l2cap_info_req(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ default:
+ printf("Unknown L2CAP code 0x%02x\n", hdr->code);
+ ret = false;
+ }
+
+ handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
+ data + sizeof(*hdr), hdr_len);
+
+ if (ret)
+ return;
+
+reject:
+ memset(&rej, 0, sizeof(rej));
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
+ &rej, sizeof(rej));
+}
+
+static bool l2cap_conn_param_req(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_conn_param_req *req = data;
+ struct bt_l2cap_pdu_conn_param_rsp rsp;
+ struct bt_hci_cmd_le_conn_update hci_cmd;
+
+ if (len < sizeof(*req))
+ return false;
+
+ memset(&hci_cmd, 0, sizeof(hci_cmd));
+ hci_cmd.handle = cpu_to_le16(conn->handle);
+ hci_cmd.min_interval = req->min_interval;
+ hci_cmd.max_interval = req->max_interval;
+ hci_cmd.latency = req->latency;
+ hci_cmd.supv_timeout = req->timeout;
+ hci_cmd.min_length = cpu_to_le16(0x0001);
+ hci_cmd.max_length = cpu_to_le16(0x0001);
+
+ send_command(bthost, BT_HCI_CMD_LE_CONN_UPDATE,
+ &hci_cmd, sizeof(hci_cmd));
+
+ memset(&rsp, 0, sizeof(rsp));
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_PARAM_RSP, ident,
+ &rsp, sizeof(rsp));
+
+ return true;
+}
+
+static bool l2cap_conn_param_rsp(struct bthost *bthost, struct btconn *conn,
+ uint8_t ident, const void *data, uint16_t len)
+{
+ const struct bt_l2cap_pdu_conn_param_req *rsp = data;
+
+ if (len < sizeof(*rsp))
+ return false;
+
+ return true;
+}
+
+static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn,
+ const void *data, uint16_t len)
+{
+ const struct bt_l2cap_hdr_sig *hdr = data;
+ struct bt_l2cap_pdu_cmd_reject rej;
+ uint16_t hdr_len;
+ bool ret;
+
+ if (len < sizeof(*hdr))
+ goto reject;
+
+ hdr_len = le16_to_cpu(hdr->len);
+
+ if (sizeof(*hdr) + hdr_len != len)
+ goto reject;
+
+ switch (hdr->code) {
+ case BT_L2CAP_PDU_CMD_REJECT:
+ ret = l2cap_cmd_rej(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_CONN_PARAM_REQ:
+ ret = l2cap_conn_param_req(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ case BT_L2CAP_PDU_CONN_PARAM_RSP:
+ ret = l2cap_conn_param_rsp(bthost, conn, hdr->ident,
+ data + sizeof(*hdr), hdr_len);
+ break;
+
+ default:
+ printf("Unknown L2CAP code 0x%02x\n", hdr->code);
+ ret = false;
+ }
+
+ handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
+ data + sizeof(*hdr), hdr_len);
+
+ if (ret)
+ return;
+
+reject:
+ memset(&rej, 0, sizeof(rej));
+ l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
+ &rej, sizeof(rej));
+}
+
+static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
+{
+ const struct bt_hci_acl_hdr *acl_hdr = data;
+ const struct bt_l2cap_hdr *l2_hdr = data + sizeof(*acl_hdr);
+ uint16_t handle, cid, acl_len, l2_len;
+ struct btconn *conn;
+ const void *l2_data;
+
+ if (len < sizeof(*acl_hdr) + sizeof(*l2_hdr))
+ return;
+
+ acl_len = le16_to_cpu(acl_hdr->dlen);
+ if (len != sizeof(*acl_hdr) + acl_len)
+ return;
+
+ handle = acl_handle(acl_hdr->handle);
+ conn = bthost_find_conn(bthost, handle);
+ if (!conn) {
+ printf("ACL data for unknown handle 0x%04x\n", handle);
+ return;
+ }
+
+ l2_len = le16_to_cpu(l2_hdr->len);
+ if (len - sizeof(*acl_hdr) != sizeof(*l2_hdr) + l2_len)
+ return;
+
+ l2_data = data + sizeof(*acl_hdr) + sizeof(*l2_hdr);
+
+ cid = le16_to_cpu(l2_hdr->cid);
+
+ switch (cid) {
+ case 0x0001:
+ l2cap_sig(bthost, conn, l2_data, l2_len);
+ break;
+ case 0x0005:
+ l2cap_le_sig(bthost, conn, l2_data, l2_len);
+ break;
+ default:
+ printf("Packet for unknown CID 0x%04x (%u)\n", cid, cid);
+ break;
+ }
+}
+
+void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
+{
+ uint8_t pkt_type;
+
+ if (!bthost)
+ return;
+
+ if (len < 1)
+ return;
+
+ pkt_type = ((const uint8_t *) data)[0];
+
+ switch (pkt_type) {
+ case BT_H4_EVT_PKT:
+ process_evt(bthost, data + 1, len - 1);
+ break;
+ case BT_H4_ACL_PKT:
+ process_acl(bthost, data + 1, len - 1);
+ break;
+ default:
+ printf("Unsupported packet 0x%2.2x\n", pkt_type);
+ break;
+ }
+}
+
+void bthost_set_cmd_complete_cb(struct bthost *bthost,
+ bthost_cmd_complete_cb cb, void *user_data)
+{
+ bthost->cmd_complete_cb = cb;
+ bthost->cmd_complete_data = user_data;
+}
+
+void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
+ void *user_data)
+{
+ bthost->new_conn_cb = cb;
+ bthost->new_conn_data = user_data;
+}
+
+void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
+ uint8_t addr_type)
+{
+ if (addr_type == BDADDR_BREDR) {
+ struct bt_hci_cmd_create_conn cc;
+
+ memset(&cc, 0, sizeof(cc));
+ memcpy(cc.bdaddr, bdaddr, sizeof(cc.bdaddr));
+
+ send_command(bthost, BT_HCI_CMD_CREATE_CONN, &cc, sizeof(cc));
+ } else {
+ struct bt_hci_cmd_le_create_conn cc;
+
+ memset(&cc, 0, sizeof(cc));
+ memcpy(cc.peer_addr, bdaddr, sizeof(cc.peer_addr));
+
+ if (addr_type == BDADDR_LE_RANDOM)
+ cc.peer_addr_type = 0x01;
+
+ send_command(bthost, BT_HCI_CMD_LE_CREATE_CONN,
+ &cc, sizeof(cc));
+ }
+}
+
+void bthost_write_scan_enable(struct bthost *bthost, uint8_t scan)
+{
+ send_command(bthost, BT_HCI_CMD_WRITE_SCAN_ENABLE, &scan, 1);
+}
+
+void bthost_set_adv_enable(struct bthost *bthost, uint8_t enable)
+{
+ send_command(bthost, BT_HCI_CMD_LE_SET_ADV_ENABLE, &enable, 1);
+}
+
+void bthost_set_server_psm(struct bthost *bthost, uint16_t psm)
+{
+ bthost->server_psm = psm;
+}
+
+void bthost_start(struct bthost *bthost)
+{
+ if (!bthost)
+ return;
+
+ bthost->ncmd = 1;
+
+ send_command(bthost, BT_HCI_CMD_RESET, NULL, 0);
+
+ send_command(bthost, BT_HCI_CMD_READ_BD_ADDR, NULL, 0);
+}
+
+void bthost_stop(struct bthost *bthost)
+{
+}
diff --git a/emulator/bthost.h b/emulator/bthost.h
new file mode 100644
index 00000000..cd5bf1c9
--- /dev/null
+++ b/emulator/bthost.h
@@ -0,0 +1,69 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011-2012 Intel Corporation
+ * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#include <stdint.h>
+
+typedef void (*bthost_send_func) (const void *data, uint16_t len,
+ void *user_data);
+
+struct bthost;
+
+struct bthost *bthost_create(void);
+void bthost_destroy(struct bthost *bthost);
+
+void bthost_set_send_handler(struct bthost *bthost, bthost_send_func handler,
+ void *user_data);
+
+void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len);
+
+typedef void (*bthost_cmd_complete_cb) (uint16_t opcode, uint8_t status,
+ const void *param, uint8_t len,
+ void *user_data);
+
+void bthost_set_cmd_complete_cb(struct bthost *bthost,
+ bthost_cmd_complete_cb cb, void *user_data);
+
+typedef void (*bthost_new_conn_cb) (uint16_t handle, void *user_data);
+
+void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
+ void *user_data);
+
+void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
+ uint8_t addr_type);
+
+typedef void (*bthost_l2cap_rsp_cb) (uint8_t code, const void *data,
+ uint16_t len, void *user_data);
+
+bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t req,
+ const void *data, uint16_t len,
+ bthost_l2cap_rsp_cb cb, void *user_data);
+
+void bthost_write_scan_enable(struct bthost *bthost, uint8_t scan);
+
+void bthost_set_adv_enable(struct bthost *bthost, uint8_t enable);
+
+void bthost_set_server_psm(struct bthost *bthost, uint16_t psm);
+
+void bthost_start(struct bthost *bthost);
+void bthost_stop(struct bthost *bthost);
diff --git a/emulator/main.c b/emulator/main.c
index 125460d5..85b10f1f 100644
--- a/emulator/main.c
+++ b/emulator/main.c
@@ -27,10 +27,14 @@
#endif
#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <getopt.h>
-#include "mainloop.h"
+#include "monitor/mainloop.h"
#include "server.h"
#include "vhci.h"
+#include "amp.h"
static void signal_callback(int signum, void *user_data)
{
@@ -42,31 +46,149 @@ static void signal_callback(int signum, void *user_data)
}
}
+static void usage(void)
+{
+ printf("btvirt - Bluetooth emulator\n"
+ "Usage:\n");
+ printf("\tbtvirt [options]\n");
+ printf("options:\n"
+ "\t-s Create local server sockets\n"
+ "\t-l [num] Number of local controllers\n"
+ "\t-L Create LE only controller\n"
+ "\t-B Create BR/EDR only controller\n"
+ "\t-A Create AMP controller\n"
+ "\t-h, --help Show help options\n");
+}
+
+static const struct option main_options[] = {
+ { "server", no_argument, NULL, 's' },
+ { "local", optional_argument, NULL, 'l' },
+ { "le", no_argument, NULL, 'L' },
+ { "bredr", no_argument, NULL, 'B' },
+ { "amp", no_argument, NULL, 'A' },
+ { "amptest", optional_argument, NULL, 'T' },
+ { "version", no_argument, NULL, 'v' },
+ { "help", no_argument, NULL, 'h' },
+ { }
+};
+
int main(int argc, char *argv[])
{
- struct vhci *vhci;
- struct server *server;
+ struct server *server1;
+ struct server *server2;
+ struct server *server3;
+ struct server *server4;
+ struct server *server5;
+ bool server_enabled = false;
+ int amptest_count = 0;
+ int vhci_count = 0;
+ enum vhci_type vhci_type = VHCI_TYPE_BREDRLE;
sigset_t mask;
+ int i;
mainloop_init();
+ for (;;) {
+ int opt;
+
+ opt = getopt_long(argc, argv, "sl::LBATvh", main_options, NULL);
+ if (opt < 0)
+ break;
+
+ switch (opt) {
+ case 's':
+ server_enabled = true;
+ break;
+ case 'l':
+ if (optarg)
+ vhci_count = atoi(optarg);
+ else
+ vhci_count = 1;
+ break;
+ case 'L':
+ vhci_type = VHCI_TYPE_LE;
+ break;
+ case 'B':
+ vhci_type = VHCI_TYPE_BREDR;
+ break;
+ case 'A':
+ vhci_type = VHCI_TYPE_AMP;
+ break;
+ case 'T':
+ if (optarg)
+ amptest_count = atoi(optarg);
+ else
+ amptest_count = 1;
+ break;
+ case 'v':
+ printf("%s\n", VERSION);
+ return EXIT_SUCCESS;
+ case 'h':
+ usage();
+ return EXIT_SUCCESS;
+ default:
+ return EXIT_FAILURE;
+ }
+ }
+
+ if (amptest_count < 1 && vhci_count < 1 && !server_enabled) {
+ fprintf(stderr, "No emulator specified\n");
+ return EXIT_FAILURE;
+ }
+
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGTERM);
mainloop_set_signal(&mask, signal_callback, NULL, NULL);
- vhci = vhci_open(VHCI_TYPE_BREDR, 0x23);
- if (!vhci) {
- fprintf(stderr, "Failed to open Virtual HCI device\n");
- return 1;
+ printf("Bluetooth emulator ver %s\n", VERSION);
+
+ for (i = 0; i < amptest_count; i++) {
+ struct bt_amp *amp;
+
+ amp = bt_amp_new();
+ if (!amp) {
+ fprintf(stderr, "Failed to create AMP controller\n");
+ return EXIT_FAILURE;
+ }
+ }
+
+ for (i = 0; i < vhci_count; i++) {
+ struct vhci *vhci;
+
+ vhci = vhci_open(vhci_type);
+ if (!vhci) {
+ fprintf(stderr, "Failed to open Virtual HCI device\n");
+ return EXIT_FAILURE;
+ }
}
- server = server_open_unix("/tmp/bt-server-bredr", 0x42);
- if (!server) {
- fprintf(stderr, "Failed to open server channel\n");
- vhci_close(vhci);
- return 1;
+ if (server_enabled) {
+ server1 = server_open_unix(SERVER_TYPE_BREDRLE,
+ "/tmp/bt-server-bredrle");
+ if (!server1)
+ fprintf(stderr, "Failed to open BR/EDR/LE server\n");
+
+ server2 = server_open_unix(SERVER_TYPE_BREDR,
+ "/tmp/bt-server-bredr");
+ if (!server2)
+ fprintf(stderr, "Failed to open BR/EDR server\n");
+
+ server3 = server_open_unix(SERVER_TYPE_AMP,
+ "/tmp/bt-server-amp");
+ if (!server3)
+ fprintf(stderr, "Failed to open AMP server\n");
+
+ server4 = server_open_unix(SERVER_TYPE_LE,
+ "/tmp/bt-server-le");
+ if (!server4)
+ fprintf(stderr, "Failed to open LE server\n");
+
+ server5 = server_open_unix(SERVER_TYPE_MONITOR,
+ "/tmp/bt-server-mon");
+ if (!server5)
+ fprintf(stderr, "Failed to open monitor server\n");
}
return mainloop_run();
diff --git a/emulator/server.c b/emulator/server.c
index 1ff9904e..b2a4b02d 100644
--- a/emulator/server.c
+++ b/emulator/server.c
@@ -36,15 +36,20 @@
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/un.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
-#include "mainloop.h"
+#include "monitor/mainloop.h"
#include "btdev.h"
#include "server.h"
+#define uninitialized_var(x) x = x
+
struct server {
+ enum server_type type;
uint16_t id;
int fd;
};
@@ -98,8 +103,10 @@ static void client_read_callback(int fd, uint32_t events, void *user_data)
ssize_t len;
uint16_t count;
- if (events & (EPOLLERR | EPOLLHUP))
+ if (events & (EPOLLERR | EPOLLHUP)) {
+ mainloop_remove_fd(client->fd);
return;
+ }
again:
len = recv(fd, buf + client->pkt_offset,
@@ -110,6 +117,9 @@ again:
return;
}
+ if (!client->btdev)
+ return;
+
count = client->pkt_offset + len;
while (count > 0) {
@@ -187,9 +197,12 @@ static void server_accept_callback(int fd, uint32_t events, void *user_data)
{
struct server *server = user_data;
struct client *client;
+ enum btdev_type uninitialized_var(type);
- if (events & (EPOLLERR | EPOLLHUP))
+ if (events & (EPOLLERR | EPOLLHUP)) {
+ mainloop_remove_fd(server->fd);
return;
+ }
client = malloc(sizeof(*client));
if (!client)
@@ -203,7 +216,24 @@ static void server_accept_callback(int fd, uint32_t events, void *user_data)
return;
}
- client->btdev = btdev_create(server->id);
+ switch (server->type) {
+ case SERVER_TYPE_BREDRLE:
+ type = BTDEV_TYPE_BREDRLE;
+ break;
+ case SERVER_TYPE_BREDR:
+ type = BTDEV_TYPE_BREDR;
+ break;
+ case SERVER_TYPE_LE:
+ type = BTDEV_TYPE_LE;
+ break;
+ case SERVER_TYPE_AMP:
+ type = BTDEV_TYPE_AMP;
+ break;
+ case SERVER_TYPE_MONITOR:
+ goto done;
+ }
+
+ client->btdev = btdev_create(type, server->id);
if (!client->btdev) {
close(client->fd);
free(client);
@@ -212,6 +242,7 @@ static void server_accept_callback(int fd, uint32_t events, void *user_data)
btdev_set_send_handler(client->btdev, client_write_callback, client);
+done:
if (mainloop_add_fd(client->fd, EPOLLIN, client_read_callback,
client, client_destroy) < 0) {
btdev_destroy(client->btdev);
@@ -220,7 +251,7 @@ static void server_accept_callback(int fd, uint32_t events, void *user_data)
}
}
-static int open_server(const char *path)
+static int open_unix(const char *path)
{
struct sockaddr_un addr;
int fd;
@@ -252,7 +283,7 @@ static int open_server(const char *path)
return fd;
}
-struct server *server_open_unix(const char *path, uint16_t id)
+struct server *server_open_unix(enum server_type type, const char *path)
{
struct server *server;
@@ -261,9 +292,72 @@ struct server *server_open_unix(const char *path, uint16_t id)
return NULL;
memset(server, 0, sizeof(*server));
- server->id = id;
+ server->type = type;
+ server->id = 0x42;
+
+ server->fd = open_unix(path);
+ if (server->fd < 0) {
+ free(server);
+ return NULL;
+ }
+
+ if (mainloop_add_fd(server->fd, EPOLLIN, server_accept_callback,
+ server, server_destroy) < 0) {
+ close(server->fd);
+ free(server);
+ return NULL;
+ }
+
+ return server;
+}
+
+static int open_tcp(void)
+{
+ struct sockaddr_in addr;
+ int fd, opt = 1;
+
+ fd = socket(PF_INET, SOCK_STREAM, 0);
+ if (fd < 0) {
+ perror("Failed to open server socket");
+ return -1;
+ }
+
+ setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+
+ memset(&addr, 0, sizeof(addr));
+ addr.sin_family = AF_INET;
+ addr.sin_addr.s_addr = INADDR_ANY;
+ addr.sin_addr.s_addr = inet_addr("127.0.0.1");
+ addr.sin_port = htons(45550);
+
+ if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+ perror("Failed to bind server socket");
+ close(fd);
+ return -1;
+ }
+
+ if (listen(fd, 5) < 0) {
+ perror("Failed to listen server socket");
+ close(fd);
+ return -1;
+ }
+
+ return fd;
+}
+
+struct server *server_open_tcp(enum server_type type)
+{
+ struct server *server;
+
+ server = malloc(sizeof(*server));
+ if (!server)
+ return server;
+
+ memset(server, 0, sizeof(*server));
+ server->type = type;
+ server->id = 0x43;
- server->fd = open_server(path);
+ server->fd = open_tcp();
if (server->fd < 0) {
free(server);
return NULL;
diff --git a/emulator/server.h b/emulator/server.h
index 836db5f9..f0b37270 100644
--- a/emulator/server.h
+++ b/emulator/server.h
@@ -24,7 +24,16 @@
#include <stdint.h>
+enum server_type {
+ SERVER_TYPE_BREDRLE,
+ SERVER_TYPE_BREDR,
+ SERVER_TYPE_LE,
+ SERVER_TYPE_AMP,
+ SERVER_TYPE_MONITOR,
+};
+
struct server;
-struct server *server_open_unix(const char *path, uint16_t id);
+struct server *server_open_unix(enum server_type type, const char *path);
+struct server *server_open_tcp(enum server_type type);
void server_close(struct server *server);
diff --git a/emulator/vhci.c b/emulator/vhci.c
index 940e562b..d32d5e57 100644
--- a/emulator/vhci.c
+++ b/emulator/vhci.c
@@ -33,10 +33,16 @@
#include <stdlib.h>
#include <string.h>
-#include "mainloop.h"
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+
+#include "monitor/mainloop.h"
+#include "monitor/bt.h"
#include "btdev.h"
#include "vhci.h"
+#define uninitialized_var(x) x = x
+
struct vhci {
enum vhci_type type;
int fd;
@@ -74,21 +80,43 @@ static void vhci_read_callback(int fd, uint32_t events, void *user_data)
return;
len = read(vhci->fd, buf, sizeof(buf));
- if (len < 0)
+ if (len < 1)
return;
- btdev_receive_h4(vhci->btdev, buf, len);
+ switch (buf[0]) {
+ case BT_H4_CMD_PKT:
+ case BT_H4_ACL_PKT:
+ case BT_H4_SCO_PKT:
+ btdev_receive_h4(vhci->btdev, buf, len);
+ break;
+ }
}
-struct vhci *vhci_open(enum vhci_type type, uint16_t id)
+struct vhci *vhci_open(enum vhci_type type)
{
struct vhci *vhci;
+ enum btdev_type uninitialized_var(btdev_type);
+ unsigned char uninitialized_var(ctrl_type);
+ unsigned char setup_cmd[2];
+ static uint8_t id = 0x23;
switch (type) {
+ case VHCI_TYPE_BREDRLE:
+ btdev_type = BTDEV_TYPE_BREDRLE;
+ ctrl_type = HCI_BREDR;
+ break;
case VHCI_TYPE_BREDR:
+ btdev_type = BTDEV_TYPE_BREDR;
+ ctrl_type = HCI_BREDR;
+ break;
+ case VHCI_TYPE_LE:
+ btdev_type = BTDEV_TYPE_LE;
+ ctrl_type = HCI_BREDR;
break;
case VHCI_TYPE_AMP:
- return NULL;
+ btdev_type = BTDEV_TYPE_AMP;
+ ctrl_type = HCI_AMP;
+ break;
}
vhci = malloc(sizeof(*vhci));
@@ -104,7 +132,16 @@ struct vhci *vhci_open(enum vhci_type type, uint16_t id)
return NULL;
}
- vhci->btdev = btdev_create(id);
+ setup_cmd[0] = HCI_VENDOR_PKT;
+ setup_cmd[1] = ctrl_type;
+
+ if (write(vhci->fd, setup_cmd, sizeof(setup_cmd)) < 0) {
+ close(vhci->fd);
+ free(vhci);
+ return NULL;
+ }
+
+ vhci->btdev = btdev_create(btdev_type, id++);
if (!vhci->btdev) {
close(vhci->fd);
free(vhci);
diff --git a/emulator/vhci.h b/emulator/vhci.h
index 4abb1830..b9ae63fc 100644
--- a/emulator/vhci.h
+++ b/emulator/vhci.h
@@ -25,11 +25,13 @@
#include <stdint.h>
enum vhci_type {
- VHCI_TYPE_BREDR = 0,
- VHCI_TYPE_AMP = 1,
+ VHCI_TYPE_BREDRLE,
+ VHCI_TYPE_BREDR,
+ VHCI_TYPE_LE,
+ VHCI_TYPE_AMP,
};
struct vhci;
-struct vhci *vhci_open(enum vhci_type type, uint16_t id);
+struct vhci *vhci_open(enum vhci_type type);
void vhci_close(struct vhci *vhci);
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-02 22:44:43 +0000