diff options
author | 2017-04-25 03:31:15 -0700 | |
---|---|---|
committer | 2017-05-22 06:48:08 +0000 | |
commit | bb756eebdac6fd24e8919e2c43f7d2c8c4091f59 (patch) | |
tree | ca11e03542edf2d8f631efeca5e1626d211107e3 /qemu/hw/bt | |
parent | a14b48d18a9ed03ec191cf16b162206998a895ce (diff) |
Adding qemu as a submodule of KVMFORNFV
This Patch includes the changes to add qemu as a submodule to
kvmfornfv repo and make use of the updated latest qemu for the
execution of all testcase
Change-Id: I1280af507a857675c7f81d30c95255635667bdd7
Signed-off-by:RajithaY<rajithax.yerrumsetty@intel.com>
Diffstat (limited to 'qemu/hw/bt')
-rw-r--r-- | qemu/hw/bt/Makefile.objs | 3 | ||||
-rw-r--r-- | qemu/hw/bt/core.c | 145 | ||||
-rw-r--r-- | qemu/hw/bt/hci-csr.c | 455 | ||||
-rw-r--r-- | qemu/hw/bt/hci.c | 2272 | ||||
-rw-r--r-- | qemu/hw/bt/hid.c | 554 | ||||
-rw-r--r-- | qemu/hw/bt/l2cap.c | 1366 | ||||
-rw-r--r-- | qemu/hw/bt/sdp.c | 979 |
7 files changed, 0 insertions, 5774 deletions
diff --git a/qemu/hw/bt/Makefile.objs b/qemu/hw/bt/Makefile.objs deleted file mode 100644 index 867a7d2e8..000000000 --- a/qemu/hw/bt/Makefile.objs +++ /dev/null @@ -1,3 +0,0 @@ -common-obj-y += core.o l2cap.o sdp.o hci.o hid.o -common-obj-y += hci-csr.o - diff --git a/qemu/hw/bt/core.c b/qemu/hw/bt/core.c deleted file mode 100644 index 615f0af07..000000000 --- a/qemu/hw/bt/core.c +++ /dev/null @@ -1,145 +0,0 @@ -/* - * Convenience functions for bluetooth. - * - * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.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 or - * (at your option) version 3 of the License. - * - * 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, see <http://www.gnu.org/licenses/>. - */ - -#include "qemu/osdep.h" -#include "qemu-common.h" -#include "sysemu/bt.h" -#include "hw/bt.h" - -/* Slave implementations can ignore this */ -static void bt_dummy_lmp_mode_change(struct bt_link_s *link) -{ -} - -/* Slaves should never receive these PDUs */ -static void bt_dummy_lmp_connection_complete(struct bt_link_s *link) -{ - if (link->slave->reject_reason) - fprintf(stderr, "%s: stray LMP_not_accepted received, fixme\n", - __FUNCTION__); - else - fprintf(stderr, "%s: stray LMP_accepted received, fixme\n", - __FUNCTION__); - exit(-1); -} - -static void bt_dummy_lmp_disconnect_master(struct bt_link_s *link) -{ - fprintf(stderr, "%s: stray LMP_detach received, fixme\n", __FUNCTION__); - exit(-1); -} - -static void bt_dummy_lmp_acl_resp(struct bt_link_s *link, - const uint8_t *data, int start, int len) -{ - fprintf(stderr, "%s: stray ACL response PDU, fixme\n", __FUNCTION__); - exit(-1); -} - -/* Slaves that don't hold any additional per link state can use these */ -static void bt_dummy_lmp_connection_request(struct bt_link_s *req) -{ - struct bt_link_s *link = g_malloc0(sizeof(struct bt_link_s)); - - link->slave = req->slave; - link->host = req->host; - - req->host->reject_reason = 0; - req->host->lmp_connection_complete(link); -} - -static void bt_dummy_lmp_disconnect_slave(struct bt_link_s *link) -{ - g_free(link); -} - -static void bt_dummy_destroy(struct bt_device_s *device) -{ - bt_device_done(device); - g_free(device); -} - -static int bt_dev_idx = 0; - -void bt_device_init(struct bt_device_s *dev, struct bt_scatternet_s *net) -{ - memset(dev, 0, sizeof(*dev)); - dev->inquiry_scan = 1; - dev->page_scan = 1; - - dev->bd_addr.b[0] = bt_dev_idx & 0xff; - dev->bd_addr.b[1] = bt_dev_idx >> 8; - dev->bd_addr.b[2] = 0xd0; - dev->bd_addr.b[3] = 0xba; - dev->bd_addr.b[4] = 0xbe; - dev->bd_addr.b[5] = 0xba; - bt_dev_idx ++; - - /* Simple slave-only devices need to implement only .lmp_acl_data */ - dev->lmp_connection_complete = bt_dummy_lmp_connection_complete; - dev->lmp_disconnect_master = bt_dummy_lmp_disconnect_master; - dev->lmp_acl_resp = bt_dummy_lmp_acl_resp; - dev->lmp_mode_change = bt_dummy_lmp_mode_change; - dev->lmp_connection_request = bt_dummy_lmp_connection_request; - dev->lmp_disconnect_slave = bt_dummy_lmp_disconnect_slave; - - dev->handle_destroy = bt_dummy_destroy; - - dev->net = net; - dev->next = net->slave; - net->slave = dev; -} - -void bt_device_done(struct bt_device_s *dev) -{ - struct bt_device_s **p = &dev->net->slave; - - while (*p && *p != dev) - p = &(*p)->next; - if (*p != dev) { - fprintf(stderr, "%s: bad bt device \"%s\"\n", __FUNCTION__, - dev->lmp_name ?: "(null)"); - exit(-1); - } - - *p = dev->next; -} - -static struct bt_vlan_s { - struct bt_scatternet_s net; - int id; - struct bt_vlan_s *next; -} *first_bt_vlan; - -/* find or alloc a new bluetooth "VLAN" */ -struct bt_scatternet_s *qemu_find_bt_vlan(int id) -{ - struct bt_vlan_s **pvlan, *vlan; - for (vlan = first_bt_vlan; vlan != NULL; vlan = vlan->next) { - if (vlan->id == id) - return &vlan->net; - } - vlan = g_malloc0(sizeof(struct bt_vlan_s)); - vlan->id = id; - pvlan = &first_bt_vlan; - while (*pvlan != NULL) - pvlan = &(*pvlan)->next; - *pvlan = vlan; - return &vlan->net; -} diff --git a/qemu/hw/bt/hci-csr.c b/qemu/hw/bt/hci-csr.c deleted file mode 100644 index 2e970b656..000000000 --- a/qemu/hw/bt/hci-csr.c +++ /dev/null @@ -1,455 +0,0 @@ -/* - * Bluetooth serial HCI transport. - * CSR41814 HCI with H4p vendor extensions. - * - * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.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 or - * (at your option) version 3 of the License. - * - * 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, see <http://www.gnu.org/licenses/>. - */ - -#include "qemu/osdep.h" -#include "qemu-common.h" -#include "sysemu/char.h" -#include "qemu/timer.h" -#include "hw/irq.h" -#include "sysemu/bt.h" -#include "hw/bt.h" - -struct csrhci_s { - int enable; - qemu_irq *pins; - int pin_state; - int modem_state; - CharDriverState chr; -#define FIFO_LEN 4096 - int out_start; - int out_len; - int out_size; - uint8_t outfifo[FIFO_LEN * 2]; - uint8_t inpkt[FIFO_LEN]; - int in_len; - int in_hdr; - int in_data; - QEMUTimer *out_tm; - int64_t baud_delay; - - bdaddr_t bd_addr; - struct HCIInfo *hci; -}; - -/* H4+ packet types */ -enum { - H4_CMD_PKT = 1, - H4_ACL_PKT = 2, - H4_SCO_PKT = 3, - H4_EVT_PKT = 4, - H4_NEG_PKT = 6, - H4_ALIVE_PKT = 7, -}; - -/* CSR41814 negotiation start magic packet */ -static const uint8_t csrhci_neg_packet[] = { - H4_NEG_PKT, 10, - 0x00, 0xa0, 0x01, 0x00, 0x00, - 0x4c, 0x00, 0x96, 0x00, 0x00, -}; - -/* CSR41814 vendor-specific command OCFs */ -enum { - OCF_CSR_SEND_FIRMWARE = 0x000, -}; - -static inline void csrhci_fifo_wake(struct csrhci_s *s) -{ - if (!s->enable || !s->out_len) - return; - - /* XXX: Should wait for s->modem_state & CHR_TIOCM_RTS? */ - if (s->chr.chr_can_read && s->chr.chr_can_read(s->chr.handler_opaque) && - s->chr.chr_read) { - s->chr.chr_read(s->chr.handler_opaque, - s->outfifo + s->out_start ++, 1); - s->out_len --; - if (s->out_start >= s->out_size) { - s->out_start = 0; - s->out_size = FIFO_LEN; - } - } - - if (s->out_len) - timer_mod(s->out_tm, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) + s->baud_delay); -} - -#define csrhci_out_packetz(s, len) memset(csrhci_out_packet(s, len), 0, len) -static uint8_t *csrhci_out_packet(struct csrhci_s *s, int len) -{ - int off = s->out_start + s->out_len; - - /* TODO: do the padding here, i.e. align len */ - s->out_len += len; - - if (off < FIFO_LEN) { - if (off + len > FIFO_LEN && (s->out_size = off + len) > FIFO_LEN * 2) { - fprintf(stderr, "%s: can't alloc %i bytes\n", __FUNCTION__, len); - exit(-1); - } - return s->outfifo + off; - } - - if (s->out_len > s->out_size) { - fprintf(stderr, "%s: can't alloc %i bytes\n", __FUNCTION__, len); - exit(-1); - } - - return s->outfifo + off - s->out_size; -} - -static inline uint8_t *csrhci_out_packet_csr(struct csrhci_s *s, - int type, int len) -{ - uint8_t *ret = csrhci_out_packetz(s, len + 2); - - *ret ++ = type; - *ret ++ = len; - - return ret; -} - -static inline uint8_t *csrhci_out_packet_event(struct csrhci_s *s, - int evt, int len) -{ - uint8_t *ret = csrhci_out_packetz(s, - len + 1 + sizeof(struct hci_event_hdr)); - - *ret ++ = H4_EVT_PKT; - ((struct hci_event_hdr *) ret)->evt = evt; - ((struct hci_event_hdr *) ret)->plen = len; - - return ret + sizeof(struct hci_event_hdr); -} - -static void csrhci_in_packet_vendor(struct csrhci_s *s, int ocf, - uint8_t *data, int len) -{ - int offset; - uint8_t *rpkt; - - switch (ocf) { - case OCF_CSR_SEND_FIRMWARE: - /* Check if this is the bd_address packet */ - if (len >= 18 + 8 && data[12] == 0x01 && data[13] == 0x00) { - offset = 18; - s->bd_addr.b[0] = data[offset + 7]; /* Beyond cmd packet end(!?) */ - s->bd_addr.b[1] = data[offset + 6]; - s->bd_addr.b[2] = data[offset + 4]; - s->bd_addr.b[3] = data[offset + 0]; - s->bd_addr.b[4] = data[offset + 3]; - s->bd_addr.b[5] = data[offset + 2]; - - s->hci->bdaddr_set(s->hci, s->bd_addr.b); - fprintf(stderr, "%s: bd_address loaded from firmware: " - "%02x:%02x:%02x:%02x:%02x:%02x\n", __FUNCTION__, - s->bd_addr.b[0], s->bd_addr.b[1], s->bd_addr.b[2], - s->bd_addr.b[3], s->bd_addr.b[4], s->bd_addr.b[5]); - } - - rpkt = csrhci_out_packet_event(s, EVT_VENDOR, 11); - /* Status bytes: no error */ - rpkt[9] = 0x00; - rpkt[10] = 0x00; - break; - - default: - fprintf(stderr, "%s: got a bad CMD packet\n", __FUNCTION__); - return; - } - - csrhci_fifo_wake(s); -} - -static void csrhci_in_packet(struct csrhci_s *s, uint8_t *pkt) -{ - uint8_t *rpkt; - int opc; - - switch (*pkt ++) { - case H4_CMD_PKT: - opc = le16_to_cpu(((struct hci_command_hdr *) pkt)->opcode); - if (cmd_opcode_ogf(opc) == OGF_VENDOR_CMD) { - csrhci_in_packet_vendor(s, cmd_opcode_ocf(opc), - pkt + sizeof(struct hci_command_hdr), - s->in_len - sizeof(struct hci_command_hdr) - 1); - return; - } - - /* TODO: if the command is OCF_READ_LOCAL_COMMANDS or the likes, - * we need to send it to the HCI layer and then add our supported - * commands to the returned mask (such as OGF_VENDOR_CMD). With - * bt-hci.c we could just have hooks for this kind of commands but - * we can't with bt-host.c. */ - - s->hci->cmd_send(s->hci, pkt, s->in_len - 1); - break; - - case H4_EVT_PKT: - goto bad_pkt; - - case H4_ACL_PKT: - s->hci->acl_send(s->hci, pkt, s->in_len - 1); - break; - - case H4_SCO_PKT: - s->hci->sco_send(s->hci, pkt, s->in_len - 1); - break; - - case H4_NEG_PKT: - if (s->in_hdr != sizeof(csrhci_neg_packet) || - memcmp(pkt - 1, csrhci_neg_packet, s->in_hdr)) { - fprintf(stderr, "%s: got a bad NEG packet\n", __FUNCTION__); - return; - } - pkt += 2; - - rpkt = csrhci_out_packet_csr(s, H4_NEG_PKT, 10); - - *rpkt ++ = 0x20; /* Operational settings negotiation Ok */ - memcpy(rpkt, pkt, 7); rpkt += 7; - *rpkt ++ = 0xff; - *rpkt = 0xff; - break; - - case H4_ALIVE_PKT: - if (s->in_hdr != 4 || pkt[1] != 0x55 || pkt[2] != 0x00) { - fprintf(stderr, "%s: got a bad ALIVE packet\n", __FUNCTION__); - return; - } - - rpkt = csrhci_out_packet_csr(s, H4_ALIVE_PKT, 2); - - *rpkt ++ = 0xcc; - *rpkt = 0x00; - break; - - default: - bad_pkt: - /* TODO: error out */ - fprintf(stderr, "%s: got a bad packet\n", __FUNCTION__); - break; - } - - csrhci_fifo_wake(s); -} - -static int csrhci_header_len(const uint8_t *pkt) -{ - switch (pkt[0]) { - case H4_CMD_PKT: - return HCI_COMMAND_HDR_SIZE; - case H4_EVT_PKT: - return HCI_EVENT_HDR_SIZE; - case H4_ACL_PKT: - return HCI_ACL_HDR_SIZE; - case H4_SCO_PKT: - return HCI_SCO_HDR_SIZE; - case H4_NEG_PKT: - return pkt[1] + 1; - case H4_ALIVE_PKT: - return 3; - } - - exit(-1); -} - -static int csrhci_data_len(const uint8_t *pkt) -{ - switch (*pkt ++) { - case H4_CMD_PKT: - /* It seems that vendor-specific command packets for H4+ are all - * one byte longer than indicated in the standard header. */ - if (le16_to_cpu(((struct hci_command_hdr *) pkt)->opcode) == 0xfc00) - return (((struct hci_command_hdr *) pkt)->plen + 1) & ~1; - - return ((struct hci_command_hdr *) pkt)->plen; - case H4_EVT_PKT: - return ((struct hci_event_hdr *) pkt)->plen; - case H4_ACL_PKT: - return le16_to_cpu(((struct hci_acl_hdr *) pkt)->dlen); - case H4_SCO_PKT: - return ((struct hci_sco_hdr *) pkt)->dlen; - case H4_NEG_PKT: - case H4_ALIVE_PKT: - return 0; - } - - exit(-1); -} - -static int csrhci_write(struct CharDriverState *chr, - const uint8_t *buf, int len) -{ - struct csrhci_s *s = (struct csrhci_s *) chr->opaque; - int plen = s->in_len; - - if (!s->enable) - return 0; - - s->in_len += len; - memcpy(s->inpkt + plen, buf, len); - - while (1) { - if (s->in_len >= 2 && plen < 2) - s->in_hdr = csrhci_header_len(s->inpkt) + 1; - - if (s->in_len >= s->in_hdr && plen < s->in_hdr) - s->in_data = csrhci_data_len(s->inpkt) + s->in_hdr; - - if (s->in_len >= s->in_data) { - csrhci_in_packet(s, s->inpkt); - - memmove(s->inpkt, s->inpkt + s->in_len, s->in_len - s->in_data); - s->in_len -= s->in_data; - s->in_hdr = INT_MAX; - s->in_data = INT_MAX; - plen = 0; - } else - break; - } - - return len; -} - -static void csrhci_out_hci_packet_event(void *opaque, - const uint8_t *data, int len) -{ - struct csrhci_s *s = (struct csrhci_s *) opaque; - uint8_t *pkt = csrhci_out_packet(s, (len + 2) & ~1); /* Align */ - - *pkt ++ = H4_EVT_PKT; - memcpy(pkt, data, len); - - csrhci_fifo_wake(s); -} - -static void csrhci_out_hci_packet_acl(void *opaque, - const uint8_t *data, int len) -{ - struct csrhci_s *s = (struct csrhci_s *) opaque; - uint8_t *pkt = csrhci_out_packet(s, (len + 2) & ~1); /* Align */ - - *pkt ++ = H4_ACL_PKT; - pkt[len & ~1] = 0; - memcpy(pkt, data, len); - - csrhci_fifo_wake(s); -} - -static int csrhci_ioctl(struct CharDriverState *chr, int cmd, void *arg) -{ - QEMUSerialSetParams *ssp; - struct csrhci_s *s = (struct csrhci_s *) chr->opaque; - int prev_state = s->modem_state; - - switch (cmd) { - case CHR_IOCTL_SERIAL_SET_PARAMS: - ssp = (QEMUSerialSetParams *) arg; - s->baud_delay = NANOSECONDS_PER_SECOND / ssp->speed; - /* Moments later... (but shorter than 100ms) */ - s->modem_state |= CHR_TIOCM_CTS; - break; - - case CHR_IOCTL_SERIAL_GET_TIOCM: - *(int *) arg = s->modem_state; - break; - - case CHR_IOCTL_SERIAL_SET_TIOCM: - s->modem_state = *(int *) arg; - if (~s->modem_state & prev_state & CHR_TIOCM_RTS) - s->modem_state &= ~CHR_TIOCM_CTS; - break; - - default: - return -ENOTSUP; - } - return 0; -} - -static void csrhci_reset(struct csrhci_s *s) -{ - s->out_len = 0; - s->out_size = FIFO_LEN; - s->in_len = 0; - s->baud_delay = NANOSECONDS_PER_SECOND; - s->enable = 0; - s->in_hdr = INT_MAX; - s->in_data = INT_MAX; - - s->modem_state = 0; - /* After a while... (but sooner than 10ms) */ - s->modem_state |= CHR_TIOCM_CTS; - - memset(&s->bd_addr, 0, sizeof(bdaddr_t)); -} - -static void csrhci_out_tick(void *opaque) -{ - csrhci_fifo_wake((struct csrhci_s *) opaque); -} - -static void csrhci_pins(void *opaque, int line, int level) -{ - struct csrhci_s *s = (struct csrhci_s *) opaque; - int state = s->pin_state; - - s->pin_state &= ~(1 << line); - s->pin_state |= (!!level) << line; - - if ((state & ~s->pin_state) & (1 << csrhci_pin_reset)) { - /* TODO: Disappear from lower layers */ - csrhci_reset(s); - } - - if (s->pin_state == 3 && state != 3) { - s->enable = 1; - /* TODO: Wake lower layers up */ - } -} - -qemu_irq *csrhci_pins_get(CharDriverState *chr) -{ - struct csrhci_s *s = (struct csrhci_s *) chr->opaque; - - return s->pins; -} - -CharDriverState *uart_hci_init(qemu_irq wakeup) -{ - struct csrhci_s *s = (struct csrhci_s *) - g_malloc0(sizeof(struct csrhci_s)); - - s->chr.opaque = s; - s->chr.chr_write = csrhci_write; - s->chr.chr_ioctl = csrhci_ioctl; - s->chr.avail_connections = 1; - - s->hci = qemu_next_hci(); - s->hci->opaque = s; - s->hci->evt_recv = csrhci_out_hci_packet_event; - s->hci->acl_recv = csrhci_out_hci_packet_acl; - - s->out_tm = timer_new_ns(QEMU_CLOCK_VIRTUAL, csrhci_out_tick, s); - s->pins = qemu_allocate_irqs(csrhci_pins, s, __csrhci_pins); - csrhci_reset(s); - - return &s->chr; -} diff --git a/qemu/hw/bt/hci.c b/qemu/hw/bt/hci.c deleted file mode 100644 index 7d5220509..000000000 --- a/qemu/hw/bt/hci.c +++ /dev/null @@ -1,2272 +0,0 @@ -/* - * QEMU Bluetooth HCI logic. - * - * Copyright (C) 2007 OpenMoko, Inc. - * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.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, see <http://www.gnu.org/licenses/>. - */ - -#include "qemu/osdep.h" -#include "qapi/error.h" -#include "qemu-common.h" -#include "qemu/timer.h" -#include "hw/usb.h" -#include "sysemu/bt.h" -#include "hw/bt.h" -#include "qapi/qmp/qerror.h" -#include "sysemu/replay.h" -#include "qemu/cutils.h" - -struct bt_hci_s { - uint8_t *(*evt_packet)(void *opaque); - void (*evt_submit)(void *opaque, int len); - void *opaque; - uint8_t evt_buf[256]; - - uint8_t acl_buf[4096]; - int acl_len; - - uint16_t asb_handle; - uint16_t psb_handle; - - int last_cmd; /* Note: Always little-endian */ - - struct bt_device_s *conn_req_host; - - struct { - int inquire; - int periodic; - int responses_left; - int responses; - QEMUTimer *inquiry_done; - QEMUTimer *inquiry_next; - int inquiry_length; - int inquiry_period; - int inquiry_mode; - -#define HCI_HANDLE_OFFSET 0x20 -#define HCI_HANDLES_MAX 0x10 - struct bt_hci_master_link_s { - struct bt_link_s *link; - void (*lmp_acl_data)(struct bt_link_s *link, - const uint8_t *data, int start, int len); - QEMUTimer *acl_mode_timer; - } handle[HCI_HANDLES_MAX]; - uint32_t role_bmp; - int last_handle; - int connecting; - bdaddr_t awaiting_bdaddr[HCI_HANDLES_MAX]; - } lm; - - uint8_t event_mask[8]; - uint16_t voice_setting; /* Notw: Always little-endian */ - uint16_t conn_accept_tout; - QEMUTimer *conn_accept_timer; - - struct HCIInfo info; - struct bt_device_s device; - - Error *replay_blocker; -}; - -#define DEFAULT_RSSI_DBM 20 - -#define hci_from_info(ptr) container_of((ptr), struct bt_hci_s, info) -#define hci_from_device(ptr) container_of((ptr), struct bt_hci_s, device) - -struct bt_hci_link_s { - struct bt_link_s btlink; - uint16_t handle; /* Local */ -}; - -/* LMP layer emulation */ -#if 0 -static void bt_submit_lmp(struct bt_device_s *bt, int length, uint8_t *data) -{ - int resp, resplen, error, op, tr; - uint8_t respdata[17]; - - if (length < 1) - return; - - tr = *data & 1; - op = *(data ++) >> 1; - resp = LMP_ACCEPTED; - resplen = 2; - respdata[1] = op; - error = 0; - length --; - - if (op >= 0x7c) { /* Extended opcode */ - op |= *(data ++) << 8; - resp = LMP_ACCEPTED_EXT; - resplen = 4; - respdata[0] = op >> 8; - respdata[1] = op & 0xff; - length --; - } - - switch (op) { - case LMP_ACCEPTED: - /* data[0] Op code - */ - if (length < 1) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = 0; - break; - - case LMP_ACCEPTED_EXT: - /* data[0] Escape op code - * data[1] Extended op code - */ - if (length < 2) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = 0; - break; - - case LMP_NOT_ACCEPTED: - /* data[0] Op code - * data[1] Error code - */ - if (length < 2) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = 0; - break; - - case LMP_NOT_ACCEPTED_EXT: - /* data[0] Op code - * data[1] Extended op code - * data[2] Error code - */ - if (length < 3) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = 0; - break; - - case LMP_HOST_CONNECTION_REQ: - break; - - case LMP_SETUP_COMPLETE: - resp = LMP_SETUP_COMPLETE; - resplen = 1; - bt->setup = 1; - break; - - case LMP_DETACH: - /* data[0] Error code - */ - if (length < 1) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - bt->setup = 0; - resp = 0; - break; - - case LMP_SUPERVISION_TIMEOUT: - /* data[0,1] Supervision timeout - */ - if (length < 2) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = 0; - break; - - case LMP_QUALITY_OF_SERVICE: - resp = 0; - /* Fall through */ - case LMP_QOS_REQ: - /* data[0,1] Poll interval - * data[2] N(BC) - */ - if (length < 3) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - break; - - case LMP_MAX_SLOT: - resp = 0; - /* Fall through */ - case LMP_MAX_SLOT_REQ: - /* data[0] Max slots - */ - if (length < 1) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - break; - - case LMP_AU_RAND: - case LMP_IN_RAND: - case LMP_COMB_KEY: - /* data[0-15] Random number - */ - if (length < 16) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - if (op == LMP_AU_RAND) { - if (bt->key_present) { - resp = LMP_SRES; - resplen = 5; - /* XXX: [Part H] Section 6.1 on page 801 */ - } else { - error = HCI_PIN_OR_KEY_MISSING; - goto not_accepted; - } - } else if (op == LMP_IN_RAND) { - error = HCI_PAIRING_NOT_ALLOWED; - goto not_accepted; - } else { - /* XXX: [Part H] Section 3.2 on page 779 */ - resp = LMP_UNIT_KEY; - resplen = 17; - memcpy(respdata + 1, bt->key, 16); - - error = HCI_UNIT_LINK_KEY_USED; - goto not_accepted; - } - break; - - case LMP_UNIT_KEY: - /* data[0-15] Key - */ - if (length < 16) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - memcpy(bt->key, data, 16); - bt->key_present = 1; - break; - - case LMP_SRES: - /* data[0-3] Authentication response - */ - if (length < 4) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - break; - - case LMP_CLKOFFSET_REQ: - resp = LMP_CLKOFFSET_RES; - resplen = 3; - respdata[1] = 0x33; - respdata[2] = 0x33; - break; - - case LMP_CLKOFFSET_RES: - /* data[0,1] Clock offset - * (Slave to master only) - */ - if (length < 2) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - break; - - case LMP_VERSION_REQ: - case LMP_VERSION_RES: - /* data[0] VersNr - * data[1,2] CompId - * data[3,4] SubVersNr - */ - if (length < 5) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - if (op == LMP_VERSION_REQ) { - resp = LMP_VERSION_RES; - resplen = 6; - respdata[1] = 0x20; - respdata[2] = 0xff; - respdata[3] = 0xff; - respdata[4] = 0xff; - respdata[5] = 0xff; - } else - resp = 0; - break; - - case LMP_FEATURES_REQ: - case LMP_FEATURES_RES: - /* data[0-7] Features - */ - if (length < 8) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - if (op == LMP_FEATURES_REQ) { - resp = LMP_FEATURES_RES; - resplen = 9; - respdata[1] = (bt->lmp_caps >> 0) & 0xff; - respdata[2] = (bt->lmp_caps >> 8) & 0xff; - respdata[3] = (bt->lmp_caps >> 16) & 0xff; - respdata[4] = (bt->lmp_caps >> 24) & 0xff; - respdata[5] = (bt->lmp_caps >> 32) & 0xff; - respdata[6] = (bt->lmp_caps >> 40) & 0xff; - respdata[7] = (bt->lmp_caps >> 48) & 0xff; - respdata[8] = (bt->lmp_caps >> 56) & 0xff; - } else - resp = 0; - break; - - case LMP_NAME_REQ: - /* data[0] Name offset - */ - if (length < 1) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = LMP_NAME_RES; - resplen = 17; - respdata[1] = data[0]; - respdata[2] = strlen(bt->lmp_name); - memset(respdata + 3, 0x00, 14); - if (respdata[2] > respdata[1]) - memcpy(respdata + 3, bt->lmp_name + respdata[1], - respdata[2] - respdata[1]); - break; - - case LMP_NAME_RES: - /* data[0] Name offset - * data[1] Name length - * data[2-15] Name fragment - */ - if (length < 16) { - error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE; - goto not_accepted; - } - resp = 0; - break; - - default: - error = HCI_UNKNOWN_LMP_PDU; - /* Fall through */ - not_accepted: - if (op >> 8) { - resp = LMP_NOT_ACCEPTED_EXT; - resplen = 5; - respdata[0] = op >> 8; - respdata[1] = op & 0xff; - respdata[2] = error; - } else { - resp = LMP_NOT_ACCEPTED; - resplen = 3; - respdata[0] = op & 0xff; - respdata[1] = error; - } - } - - if (resp == 0) - return; - - if (resp >> 8) { - respdata[0] = resp >> 8; - respdata[1] = resp & 0xff; - } else - respdata[0] = resp & 0xff; - - respdata[0] <<= 1; - respdata[0] |= tr; -} - -static void bt_submit_raw_acl(struct bt_piconet_s *net, int length, uint8_t *data) -{ - struct bt_device_s *slave; - if (length < 1) - return; - - slave = 0; -#if 0 - slave = net->slave; -#endif - - switch (data[0] & 3) { - case LLID_ACLC: - bt_submit_lmp(slave, length - 1, data + 1); - break; - case LLID_ACLU_START: -#if 0 - bt_sumbit_l2cap(slave, length - 1, data + 1, (data[0] >> 2) & 1); - breka; -#endif - default: - case LLID_ACLU_CONT: - break; - } -} -#endif - -/* HCI layer emulation */ - -/* Note: we could ignore endiannes because unswapped handles will still - * be valid as connection identifiers for the guest - they don't have to - * be continuously allocated. We do it though, to preserve similar - * behaviour between hosts. Some things, like the BD_ADDR cannot be - * preserved though (for example if a real hci is used). */ -#ifdef HOST_WORDS_BIGENDIAN -# define HNDL(raw) bswap16(raw) -#else -# define HNDL(raw) (raw) -#endif - -static const uint8_t bt_event_reserved_mask[8] = { - 0xff, 0x9f, 0xfb, 0xff, 0x07, 0x18, 0x00, 0x00, -}; - - -static void null_hci_send(struct HCIInfo *hci, const uint8_t *data, int len) -{ -} - -static int null_hci_addr_set(struct HCIInfo *hci, const uint8_t *bd_addr) -{ - return -ENOTSUP; -} - -struct HCIInfo null_hci = { - .cmd_send = null_hci_send, - .sco_send = null_hci_send, - .acl_send = null_hci_send, - .bdaddr_set = null_hci_addr_set, -}; - - -static inline uint8_t *bt_hci_event_start(struct bt_hci_s *hci, - int evt, int len) -{ - uint8_t *packet, mask; - int mask_byte; - - if (len > 255) { - fprintf(stderr, "%s: HCI event params too long (%ib)\n", - __FUNCTION__, len); - exit(-1); - } - - mask_byte = (evt - 1) >> 3; - mask = 1 << ((evt - 1) & 3); - if (mask & bt_event_reserved_mask[mask_byte] & ~hci->event_mask[mask_byte]) - return NULL; - - packet = hci->evt_packet(hci->opaque); - packet[0] = evt; - packet[1] = len; - - return &packet[2]; -} - -static inline void bt_hci_event(struct bt_hci_s *hci, int evt, - void *params, int len) -{ - uint8_t *packet = bt_hci_event_start(hci, evt, len); - - if (!packet) - return; - - if (len) - memcpy(packet, params, len); - - hci->evt_submit(hci->opaque, len + 2); -} - -static inline void bt_hci_event_status(struct bt_hci_s *hci, int status) -{ - evt_cmd_status params = { - .status = status, - .ncmd = 1, - .opcode = hci->last_cmd, - }; - - bt_hci_event(hci, EVT_CMD_STATUS, ¶ms, EVT_CMD_STATUS_SIZE); -} - -static inline void bt_hci_event_complete(struct bt_hci_s *hci, - void *ret, int len) -{ - uint8_t *packet = bt_hci_event_start(hci, EVT_CMD_COMPLETE, - len + EVT_CMD_COMPLETE_SIZE); - evt_cmd_complete *params = (evt_cmd_complete *) packet; - - if (!packet) - return; - - params->ncmd = 1; - params->opcode = hci->last_cmd; - if (len) - memcpy(&packet[EVT_CMD_COMPLETE_SIZE], ret, len); - - hci->evt_submit(hci->opaque, len + EVT_CMD_COMPLETE_SIZE + 2); -} - -static void bt_hci_inquiry_done(void *opaque) -{ - struct bt_hci_s *hci = (struct bt_hci_s *) opaque; - uint8_t status = HCI_SUCCESS; - - if (!hci->lm.periodic) - hci->lm.inquire = 0; - - /* The specification is inconsistent about this one. Page 565 reads - * "The event parameters of Inquiry Complete event will have a summary - * of the result from the Inquiry process, which reports the number of - * nearby Bluetooth devices that responded [so hci->responses].", but - * Event Parameters (see page 729) has only Status. */ - bt_hci_event(hci, EVT_INQUIRY_COMPLETE, &status, 1); -} - -static void bt_hci_inquiry_result_standard(struct bt_hci_s *hci, - struct bt_device_s *slave) -{ - inquiry_info params = { - .num_responses = 1, - .bdaddr = BAINIT(&slave->bd_addr), - .pscan_rep_mode = 0x00, /* R0 */ - .pscan_period_mode = 0x00, /* P0 - deprecated */ - .pscan_mode = 0x00, /* Standard scan - deprecated */ - .dev_class[0] = slave->class[0], - .dev_class[1] = slave->class[1], - .dev_class[2] = slave->class[2], - /* TODO: return the clkoff *differenece* */ - .clock_offset = slave->clkoff, /* Note: no swapping */ - }; - - bt_hci_event(hci, EVT_INQUIRY_RESULT, ¶ms, INQUIRY_INFO_SIZE); -} - -static void bt_hci_inquiry_result_with_rssi(struct bt_hci_s *hci, - struct bt_device_s *slave) -{ - inquiry_info_with_rssi params = { - .num_responses = 1, - .bdaddr = BAINIT(&slave->bd_addr), - .pscan_rep_mode = 0x00, /* R0 */ - .pscan_period_mode = 0x00, /* P0 - deprecated */ - .dev_class[0] = slave->class[0], - .dev_class[1] = slave->class[1], - .dev_class[2] = slave->class[2], - /* TODO: return the clkoff *differenece* */ - .clock_offset = slave->clkoff, /* Note: no swapping */ - .rssi = DEFAULT_RSSI_DBM, - }; - - bt_hci_event(hci, EVT_INQUIRY_RESULT_WITH_RSSI, - ¶ms, INQUIRY_INFO_WITH_RSSI_SIZE); -} - -static void bt_hci_inquiry_result(struct bt_hci_s *hci, - struct bt_device_s *slave) -{ - if (!slave->inquiry_scan || !hci->lm.responses_left) - return; - - hci->lm.responses_left --; - hci->lm.responses ++; - - switch (hci->lm.inquiry_mode) { - case 0x00: - bt_hci_inquiry_result_standard(hci, slave); - return; - case 0x01: - bt_hci_inquiry_result_with_rssi(hci, slave); - return; - default: - fprintf(stderr, "%s: bad inquiry mode %02x\n", __FUNCTION__, - hci->lm.inquiry_mode); - exit(-1); - } -} - -static void bt_hci_mod_timer_1280ms(QEMUTimer *timer, int period) -{ - timer_mod(timer, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) + - (uint64_t)(period << 7) * 10000000); -} - -static void bt_hci_inquiry_start(struct bt_hci_s *hci, int length) -{ - struct bt_device_s *slave; - - hci->lm.inquiry_length = length; - for (slave = hci->device.net->slave; slave; slave = slave->next) - /* Don't uncover ourselves. */ - if (slave != &hci->device) - bt_hci_inquiry_result(hci, slave); - - /* TODO: register for a callback on a new device's addition to the - * scatternet so that if it's added before inquiry_length expires, - * an Inquiry Result is generated immediately. Alternatively re-loop - * through the devices on the inquiry_length expiration and report - * devices not seen before. */ - if (hci->lm.responses_left) - bt_hci_mod_timer_1280ms(hci->lm.inquiry_done, hci->lm.inquiry_length); - else - bt_hci_inquiry_done(hci); - - if (hci->lm.periodic) - bt_hci_mod_timer_1280ms(hci->lm.inquiry_next, hci->lm.inquiry_period); -} - -static void bt_hci_inquiry_next(void *opaque) -{ - struct bt_hci_s *hci = (struct bt_hci_s *) opaque; - - hci->lm.responses_left += hci->lm.responses; - hci->lm.responses = 0; - bt_hci_inquiry_start(hci, hci->lm.inquiry_length); -} - -static inline int bt_hci_handle_bad(struct bt_hci_s *hci, uint16_t handle) -{ - return !(handle & HCI_HANDLE_OFFSET) || - handle >= (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX) || - !hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link; -} - -static inline int bt_hci_role_master(struct bt_hci_s *hci, uint16_t handle) -{ - return !!(hci->lm.role_bmp & (1 << (handle & ~HCI_HANDLE_OFFSET))); -} - -static inline struct bt_device_s *bt_hci_remote_dev(struct bt_hci_s *hci, - uint16_t handle) -{ - struct bt_link_s *link = hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link; - - return bt_hci_role_master(hci, handle) ? link->slave : link->host; -} - -static void bt_hci_mode_tick(void *opaque); -static void bt_hci_lmp_link_establish(struct bt_hci_s *hci, - struct bt_link_s *link, int master) -{ - hci->lm.handle[hci->lm.last_handle].link = link; - - if (master) { - /* We are the master side of an ACL link */ - hci->lm.role_bmp |= 1 << hci->lm.last_handle; - - hci->lm.handle[hci->lm.last_handle].lmp_acl_data = - link->slave->lmp_acl_data; - } else { - /* We are the slave side of an ACL link */ - hci->lm.role_bmp &= ~(1 << hci->lm.last_handle); - - hci->lm.handle[hci->lm.last_handle].lmp_acl_data = - link->host->lmp_acl_resp; - } - - /* Mode */ - if (master) { - link->acl_mode = acl_active; - hci->lm.handle[hci->lm.last_handle].acl_mode_timer = - timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_mode_tick, link); - } -} - -static void bt_hci_lmp_link_teardown(struct bt_hci_s *hci, uint16_t handle) -{ - handle &= ~HCI_HANDLE_OFFSET; - hci->lm.handle[handle].link = NULL; - - if (bt_hci_role_master(hci, handle)) { - timer_del(hci->lm.handle[handle].acl_mode_timer); - timer_free(hci->lm.handle[handle].acl_mode_timer); - } -} - -static int bt_hci_connect(struct bt_hci_s *hci, bdaddr_t *bdaddr) -{ - struct bt_device_s *slave; - struct bt_link_s link; - - for (slave = hci->device.net->slave; slave; slave = slave->next) - if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr)) - break; - if (!slave || slave == &hci->device) - return -ENODEV; - - bacpy(&hci->lm.awaiting_bdaddr[hci->lm.connecting ++], &slave->bd_addr); - - link.slave = slave; - link.host = &hci->device; - link.slave->lmp_connection_request(&link); /* Always last */ - - return 0; -} - -static void bt_hci_connection_reject(struct bt_hci_s *hci, - struct bt_device_s *host, uint8_t because) -{ - struct bt_link_s link = { - .slave = &hci->device, - .host = host, - /* Rest uninitialised */ - }; - - host->reject_reason = because; - host->lmp_connection_complete(&link); -} - -static void bt_hci_connection_reject_event(struct bt_hci_s *hci, - bdaddr_t *bdaddr) -{ - evt_conn_complete params; - - params.status = HCI_NO_CONNECTION; - params.handle = 0; - bacpy(¶ms.bdaddr, bdaddr); - params.link_type = ACL_LINK; - params.encr_mode = 0x00; /* Encryption not required */ - bt_hci_event(hci, EVT_CONN_COMPLETE, ¶ms, EVT_CONN_COMPLETE_SIZE); -} - -static void bt_hci_connection_accept(struct bt_hci_s *hci, - struct bt_device_s *host) -{ - struct bt_hci_link_s *link = g_malloc0(sizeof(struct bt_hci_link_s)); - evt_conn_complete params; - uint16_t handle; - uint8_t status = HCI_SUCCESS; - int tries = HCI_HANDLES_MAX; - - /* Make a connection handle */ - do { - while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries) - hci->lm.last_handle &= HCI_HANDLES_MAX - 1; - handle = hci->lm.last_handle | HCI_HANDLE_OFFSET; - } while ((handle == hci->asb_handle || handle == hci->psb_handle) && - tries); - - if (!tries) { - g_free(link); - bt_hci_connection_reject(hci, host, HCI_REJECTED_LIMITED_RESOURCES); - status = HCI_NO_CONNECTION; - goto complete; - } - - link->btlink.slave = &hci->device; - link->btlink.host = host; - link->handle = handle; - - /* Link established */ - bt_hci_lmp_link_establish(hci, &link->btlink, 0); - -complete: - params.status = status; - params.handle = HNDL(handle); - bacpy(¶ms.bdaddr, &host->bd_addr); - params.link_type = ACL_LINK; - params.encr_mode = 0x00; /* Encryption not required */ - bt_hci_event(hci, EVT_CONN_COMPLETE, ¶ms, EVT_CONN_COMPLETE_SIZE); - - /* Neets to be done at the very end because it can trigger a (nested) - * disconnected, in case the other and had cancelled the request - * locally. */ - if (status == HCI_SUCCESS) { - host->reject_reason = 0; - host->lmp_connection_complete(&link->btlink); - } -} - -static void bt_hci_lmp_connection_request(struct bt_link_s *link) -{ - struct bt_hci_s *hci = hci_from_device(link->slave); - evt_conn_request params; - - if (hci->conn_req_host) { - bt_hci_connection_reject(hci, link->host, - HCI_REJECTED_LIMITED_RESOURCES); - return; - } - hci->conn_req_host = link->host; - /* TODO: if masked and auto-accept, then auto-accept, - * if masked and not auto-accept, then auto-reject */ - /* TODO: kick the hci->conn_accept_timer, timeout after - * hci->conn_accept_tout * 0.625 msec */ - - bacpy(¶ms.bdaddr, &link->host->bd_addr); - memcpy(¶ms.dev_class, &link->host->class, sizeof(params.dev_class)); - params.link_type = ACL_LINK; - bt_hci_event(hci, EVT_CONN_REQUEST, ¶ms, EVT_CONN_REQUEST_SIZE); -} - -static void bt_hci_conn_accept_timeout(void *opaque) -{ - struct bt_hci_s *hci = (struct bt_hci_s *) opaque; - - if (!hci->conn_req_host) - /* Already accepted or rejected. If the other end cancelled the - * connection request then we still have to reject or accept it - * and then we'll get a disconnect. */ - return; - - /* TODO */ -} - -/* Remove from the list of devices which we wanted to connect to and - * are awaiting a response from. If the callback sees a response from - * a device which is not on the list it will assume it's a connection - * that's been cancelled by the host in the meantime and immediately - * try to detach the link and send a Connection Complete. */ -static int bt_hci_lmp_connection_ready(struct bt_hci_s *hci, - bdaddr_t *bdaddr) -{ - int i; - - for (i = 0; i < hci->lm.connecting; i ++) - if (!bacmp(&hci->lm.awaiting_bdaddr[i], bdaddr)) { - if (i < -- hci->lm.connecting) - bacpy(&hci->lm.awaiting_bdaddr[i], - &hci->lm.awaiting_bdaddr[hci->lm.connecting]); - return 0; - } - - return 1; -} - -static void bt_hci_lmp_connection_complete(struct bt_link_s *link) -{ - struct bt_hci_s *hci = hci_from_device(link->host); - evt_conn_complete params; - uint16_t handle; - uint8_t status = HCI_SUCCESS; - int tries = HCI_HANDLES_MAX; - - if (bt_hci_lmp_connection_ready(hci, &link->slave->bd_addr)) { - if (!hci->device.reject_reason) - link->slave->lmp_disconnect_slave(link); - handle = 0; - status = HCI_NO_CONNECTION; - goto complete; - } - - if (hci->device.reject_reason) { - handle = 0; - status = hci->device.reject_reason; - goto complete; - } - - /* Make a connection handle */ - do { - while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries) - hci->lm.last_handle &= HCI_HANDLES_MAX - 1; - handle = hci->lm.last_handle | HCI_HANDLE_OFFSET; - } while ((handle == hci->asb_handle || handle == hci->psb_handle) && - tries); - - if (!tries) { - link->slave->lmp_disconnect_slave(link); - status = HCI_NO_CONNECTION; - goto complete; - } - - /* Link established */ - link->handle = handle; - bt_hci_lmp_link_establish(hci, link, 1); - -complete: - params.status = status; - params.handle = HNDL(handle); - params.link_type = ACL_LINK; - bacpy(¶ms.bdaddr, &link->slave->bd_addr); - params.encr_mode = 0x00; /* Encryption not required */ - bt_hci_event(hci, EVT_CONN_COMPLETE, ¶ms, EVT_CONN_COMPLETE_SIZE); -} - -static void bt_hci_disconnect(struct bt_hci_s *hci, - uint16_t handle, int reason) -{ - struct bt_link_s *btlink = - hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link; - struct bt_hci_link_s *link; - evt_disconn_complete params; - - if (bt_hci_role_master(hci, handle)) { - btlink->slave->reject_reason = reason; - btlink->slave->lmp_disconnect_slave(btlink); - /* The link pointer is invalid from now on */ - - goto complete; - } - - btlink->host->reject_reason = reason; - btlink->host->lmp_disconnect_master(btlink); - - /* We are the slave, we get to clean this burden */ - link = (struct bt_hci_link_s *) btlink; - g_free(link); - -complete: - bt_hci_lmp_link_teardown(hci, handle); - - params.status = HCI_SUCCESS; - params.handle = HNDL(handle); - params.reason = HCI_CONNECTION_TERMINATED; - bt_hci_event(hci, EVT_DISCONN_COMPLETE, - ¶ms, EVT_DISCONN_COMPLETE_SIZE); -} - -/* TODO: use only one function */ -static void bt_hci_lmp_disconnect_host(struct bt_link_s *link) -{ - struct bt_hci_s *hci = hci_from_device(link->host); - uint16_t handle = link->handle; - evt_disconn_complete params; - - bt_hci_lmp_link_teardown(hci, handle); - - params.status = HCI_SUCCESS; - params.handle = HNDL(handle); - params.reason = hci->device.reject_reason; - bt_hci_event(hci, EVT_DISCONN_COMPLETE, - ¶ms, EVT_DISCONN_COMPLETE_SIZE); -} - -static void bt_hci_lmp_disconnect_slave(struct bt_link_s *btlink) -{ - struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink; - struct bt_hci_s *hci = hci_from_device(btlink->slave); - uint16_t handle = link->handle; - evt_disconn_complete params; - - g_free(link); - - bt_hci_lmp_link_teardown(hci, handle); - - params.status = HCI_SUCCESS; - params.handle = HNDL(handle); - params.reason = hci->device.reject_reason; - bt_hci_event(hci, EVT_DISCONN_COMPLETE, - ¶ms, EVT_DISCONN_COMPLETE_SIZE); -} - -static int bt_hci_name_req(struct bt_hci_s *hci, bdaddr_t *bdaddr) -{ - struct bt_device_s *slave; - evt_remote_name_req_complete params; - - for (slave = hci->device.net->slave; slave; slave = slave->next) - if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr)) - break; - if (!slave) - return -ENODEV; - - bt_hci_event_status(hci, HCI_SUCCESS); - - params.status = HCI_SUCCESS; - bacpy(¶ms.bdaddr, &slave->bd_addr); - pstrcpy(params.name, sizeof(params.name), slave->lmp_name ?: ""); - bt_hci_event(hci, EVT_REMOTE_NAME_REQ_COMPLETE, - ¶ms, EVT_REMOTE_NAME_REQ_COMPLETE_SIZE); - - return 0; -} - -static int bt_hci_features_req(struct bt_hci_s *hci, uint16_t handle) -{ - struct bt_device_s *slave; - evt_read_remote_features_complete params; - - if (bt_hci_handle_bad(hci, handle)) - return -ENODEV; - - slave = bt_hci_remote_dev(hci, handle); - - bt_hci_event_status(hci, HCI_SUCCESS); - - params.status = HCI_SUCCESS; - params.handle = HNDL(handle); - params.features[0] = (slave->lmp_caps >> 0) & 0xff; - params.features[1] = (slave->lmp_caps >> 8) & 0xff; - params.features[2] = (slave->lmp_caps >> 16) & 0xff; - params.features[3] = (slave->lmp_caps >> 24) & 0xff; - params.features[4] = (slave->lmp_caps >> 32) & 0xff; - params.features[5] = (slave->lmp_caps >> 40) & 0xff; - params.features[6] = (slave->lmp_caps >> 48) & 0xff; - params.features[7] = (slave->lmp_caps >> 56) & 0xff; - bt_hci_event(hci, EVT_READ_REMOTE_FEATURES_COMPLETE, - ¶ms, EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE); - - return 0; -} - -static int bt_hci_version_req(struct bt_hci_s *hci, uint16_t handle) -{ - evt_read_remote_version_complete params; - - if (bt_hci_handle_bad(hci, handle)) - return -ENODEV; - - bt_hci_remote_dev(hci, handle); - - bt_hci_event_status(hci, HCI_SUCCESS); - - params.status = HCI_SUCCESS; - params.handle = HNDL(handle); - params.lmp_ver = 0x03; - params.manufacturer = cpu_to_le16(0xa000); - params.lmp_subver = cpu_to_le16(0xa607); - bt_hci_event(hci, EVT_READ_REMOTE_VERSION_COMPLETE, - ¶ms, EVT_READ_REMOTE_VERSION_COMPLETE_SIZE); - - return 0; -} - -static int bt_hci_clkoffset_req(struct bt_hci_s *hci, uint16_t handle) -{ - struct bt_device_s *slave; - evt_read_clock_offset_complete params; - - if (bt_hci_handle_bad(hci, handle)) - return -ENODEV; - - slave = bt_hci_remote_dev(hci, handle); - - bt_hci_event_status(hci, HCI_SUCCESS); - - params.status = HCI_SUCCESS; - params.handle = HNDL(handle); - /* TODO: return the clkoff *differenece* */ - params.clock_offset = slave->clkoff; /* Note: no swapping */ - bt_hci_event(hci, EVT_READ_CLOCK_OFFSET_COMPLETE, - ¶ms, EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE); - - return 0; -} - -static void bt_hci_event_mode(struct bt_hci_s *hci, struct bt_link_s *link, - uint16_t handle) -{ - evt_mode_change params = { - .status = HCI_SUCCESS, - .handle = HNDL(handle), - .mode = link->acl_mode, - .interval = cpu_to_le16(link->acl_interval), - }; - - bt_hci_event(hci, EVT_MODE_CHANGE, ¶ms, EVT_MODE_CHANGE_SIZE); -} - -static void bt_hci_lmp_mode_change_master(struct bt_hci_s *hci, - struct bt_link_s *link, int mode, uint16_t interval) -{ - link->acl_mode = mode; - link->acl_interval = interval; - - bt_hci_event_mode(hci, link, link->handle); - - link->slave->lmp_mode_change(link); -} - -static void bt_hci_lmp_mode_change_slave(struct bt_link_s *btlink) -{ - struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink; - struct bt_hci_s *hci = hci_from_device(btlink->slave); - - bt_hci_event_mode(hci, btlink, link->handle); -} - -static int bt_hci_mode_change(struct bt_hci_s *hci, uint16_t handle, - int interval, int mode) -{ - struct bt_hci_master_link_s *link; - - if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle)) - return -ENODEV; - - link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET]; - if (link->link->acl_mode != acl_active) { - bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED); - return 0; - } - - bt_hci_event_status(hci, HCI_SUCCESS); - - timer_mod(link->acl_mode_timer, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) + - ((uint64_t)interval * 625) * 1000); - bt_hci_lmp_mode_change_master(hci, link->link, mode, interval); - - return 0; -} - -static int bt_hci_mode_cancel(struct bt_hci_s *hci, uint16_t handle, int mode) -{ - struct bt_hci_master_link_s *link; - - if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle)) - return -ENODEV; - - link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET]; - if (link->link->acl_mode != mode) { - bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED); - - return 0; - } - - bt_hci_event_status(hci, HCI_SUCCESS); - - timer_del(link->acl_mode_timer); - bt_hci_lmp_mode_change_master(hci, link->link, acl_active, 0); - - return 0; -} - -static void bt_hci_mode_tick(void *opaque) -{ - struct bt_link_s *link = opaque; - struct bt_hci_s *hci = hci_from_device(link->host); - - bt_hci_lmp_mode_change_master(hci, link, acl_active, 0); -} - -static void bt_hci_reset(struct bt_hci_s *hci) -{ - hci->acl_len = 0; - hci->last_cmd = 0; - hci->lm.connecting = 0; - - hci->event_mask[0] = 0xff; - hci->event_mask[1] = 0xff; - hci->event_mask[2] = 0xff; - hci->event_mask[3] = 0xff; - hci->event_mask[4] = 0xff; - hci->event_mask[5] = 0x1f; - hci->event_mask[6] = 0x00; - hci->event_mask[7] = 0x00; - hci->device.inquiry_scan = 0; - hci->device.page_scan = 0; - g_free((void *) hci->device.lmp_name); - hci->device.lmp_name = NULL; - hci->device.class[0] = 0x00; - hci->device.class[1] = 0x00; - hci->device.class[2] = 0x00; - hci->voice_setting = 0x0000; - hci->conn_accept_tout = 0x1f40; - hci->lm.inquiry_mode = 0x00; - - hci->psb_handle = 0x000; - hci->asb_handle = 0x000; - - /* XXX: timer_del(sl->acl_mode_timer); for all links */ - timer_del(hci->lm.inquiry_done); - timer_del(hci->lm.inquiry_next); - timer_del(hci->conn_accept_timer); -} - -static void bt_hci_read_local_version_rp(struct bt_hci_s *hci) -{ - read_local_version_rp lv = { - .status = HCI_SUCCESS, - .hci_ver = 0x03, - .hci_rev = cpu_to_le16(0xa607), - .lmp_ver = 0x03, - .manufacturer = cpu_to_le16(0xa000), - .lmp_subver = cpu_to_le16(0xa607), - }; - - bt_hci_event_complete(hci, &lv, READ_LOCAL_VERSION_RP_SIZE); -} - -static void bt_hci_read_local_commands_rp(struct bt_hci_s *hci) -{ - read_local_commands_rp lc = { - .status = HCI_SUCCESS, - .commands = { - /* Keep updated! */ - /* Also, keep in sync with hci->device.lmp_caps in bt_new_hci */ - 0xbf, 0x80, 0xf9, 0x03, 0xb2, 0xc0, 0x03, 0xc3, - 0x00, 0x0f, 0x80, 0x00, 0xc0, 0x00, 0xe8, 0x13, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - }, - }; - - bt_hci_event_complete(hci, &lc, READ_LOCAL_COMMANDS_RP_SIZE); -} - -static void bt_hci_read_local_features_rp(struct bt_hci_s *hci) -{ - read_local_features_rp lf = { - .status = HCI_SUCCESS, - .features = { - (hci->device.lmp_caps >> 0) & 0xff, - (hci->device.lmp_caps >> 8) & 0xff, - (hci->device.lmp_caps >> 16) & 0xff, - (hci->device.lmp_caps >> 24) & 0xff, - (hci->device.lmp_caps >> 32) & 0xff, - (hci->device.lmp_caps >> 40) & 0xff, - (hci->device.lmp_caps >> 48) & 0xff, - (hci->device.lmp_caps >> 56) & 0xff, - }, - }; - - bt_hci_event_complete(hci, &lf, READ_LOCAL_FEATURES_RP_SIZE); -} - -static void bt_hci_read_local_ext_features_rp(struct bt_hci_s *hci, int page) -{ - read_local_ext_features_rp lef = { - .status = HCI_SUCCESS, - .page_num = page, - .max_page_num = 0x00, - .features = { - /* Keep updated! */ - 0x5f, 0x35, 0x85, 0x7e, 0x9b, 0x19, 0x00, 0x80, - }, - }; - if (page) - memset(lef.features, 0, sizeof(lef.features)); - - bt_hci_event_complete(hci, &lef, READ_LOCAL_EXT_FEATURES_RP_SIZE); -} - -static void bt_hci_read_buffer_size_rp(struct bt_hci_s *hci) -{ - read_buffer_size_rp bs = { - /* This can be made configurable, for one standard USB dongle HCI - * the four values are cpu_to_le16(0x0180), 0x40, - * cpu_to_le16(0x0008), cpu_to_le16(0x0008). */ - .status = HCI_SUCCESS, - .acl_mtu = cpu_to_le16(0x0200), - .sco_mtu = 0, - .acl_max_pkt = cpu_to_le16(0x0001), - .sco_max_pkt = cpu_to_le16(0x0000), - }; - - bt_hci_event_complete(hci, &bs, READ_BUFFER_SIZE_RP_SIZE); -} - -/* Deprecated in V2.0 (page 661) */ -static void bt_hci_read_country_code_rp(struct bt_hci_s *hci) -{ - read_country_code_rp cc ={ - .status = HCI_SUCCESS, - .country_code = 0x00, /* North America & Europe^1 and Japan */ - }; - - bt_hci_event_complete(hci, &cc, READ_COUNTRY_CODE_RP_SIZE); - - /* ^1. Except France, sorry */ -} - -static void bt_hci_read_bd_addr_rp(struct bt_hci_s *hci) -{ - read_bd_addr_rp ba = { - .status = HCI_SUCCESS, - .bdaddr = BAINIT(&hci->device.bd_addr), - }; - - bt_hci_event_complete(hci, &ba, READ_BD_ADDR_RP_SIZE); -} - -static int bt_hci_link_quality_rp(struct bt_hci_s *hci, uint16_t handle) -{ - read_link_quality_rp lq = { - .status = HCI_SUCCESS, - .handle = HNDL(handle), - .link_quality = 0xff, - }; - - if (bt_hci_handle_bad(hci, handle)) - lq.status = HCI_NO_CONNECTION; - - bt_hci_event_complete(hci, &lq, READ_LINK_QUALITY_RP_SIZE); - return 0; -} - -/* Generate a Command Complete event with only the Status parameter */ -static inline void bt_hci_event_complete_status(struct bt_hci_s *hci, - uint8_t status) -{ - bt_hci_event_complete(hci, &status, 1); -} - -static inline void bt_hci_event_complete_conn_cancel(struct bt_hci_s *hci, - uint8_t status, bdaddr_t *bd_addr) -{ - create_conn_cancel_rp params = { - .status = status, - .bdaddr = BAINIT(bd_addr), - }; - - bt_hci_event_complete(hci, ¶ms, CREATE_CONN_CANCEL_RP_SIZE); -} - -static inline void bt_hci_event_auth_complete(struct bt_hci_s *hci, - uint16_t handle) -{ - evt_auth_complete params = { - .status = HCI_SUCCESS, - .handle = HNDL(handle), - }; - - bt_hci_event(hci, EVT_AUTH_COMPLETE, ¶ms, EVT_AUTH_COMPLETE_SIZE); -} - -static inline void bt_hci_event_encrypt_change(struct bt_hci_s *hci, - uint16_t handle, uint8_t mode) -{ - evt_encrypt_change params = { - .status = HCI_SUCCESS, - .handle = HNDL(handle), - .encrypt = mode, - }; - - bt_hci_event(hci, EVT_ENCRYPT_CHANGE, ¶ms, EVT_ENCRYPT_CHANGE_SIZE); -} - -static inline void bt_hci_event_complete_name_cancel(struct bt_hci_s *hci, - bdaddr_t *bd_addr) -{ - remote_name_req_cancel_rp params = { - .status = HCI_INVALID_PARAMETERS, - .bdaddr = BAINIT(bd_addr), - }; - - bt_hci_event_complete(hci, ¶ms, REMOTE_NAME_REQ_CANCEL_RP_SIZE); -} - -static inline void bt_hci_event_read_remote_ext_features(struct bt_hci_s *hci, - uint16_t handle) -{ - evt_read_remote_ext_features_complete params = { - .status = HCI_UNSUPPORTED_FEATURE, - .handle = HNDL(handle), - /* Rest uninitialised */ - }; - - bt_hci_event(hci, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE, - ¶ms, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE); -} - -static inline void bt_hci_event_complete_lmp_handle(struct bt_hci_s *hci, - uint16_t handle) -{ - read_lmp_handle_rp params = { - .status = HCI_NO_CONNECTION, - .handle = HNDL(handle), - .reserved = 0, - /* Rest uninitialised */ - }; - - bt_hci_event_complete(hci, ¶ms, READ_LMP_HANDLE_RP_SIZE); -} - -static inline void bt_hci_event_complete_role_discovery(struct bt_hci_s *hci, - int status, uint16_t handle, int master) -{ - role_discovery_rp params = { - .status = status, - .handle = HNDL(handle), - .role = master ? 0x00 : 0x01, - }; - - bt_hci_event_complete(hci, ¶ms, ROLE_DISCOVERY_RP_SIZE); -} - -static inline void bt_hci_event_complete_flush(struct bt_hci_s *hci, - int status, uint16_t handle) -{ - flush_rp params = { - .status = status, - .handle = HNDL(handle), - }; - - bt_hci_event_complete(hci, ¶ms, FLUSH_RP_SIZE); -} - -static inline void bt_hci_event_complete_read_local_name(struct bt_hci_s *hci) -{ - read_local_name_rp params; - params.status = HCI_SUCCESS; - memset(params.name, 0, sizeof(params.name)); - if (hci->device.lmp_name) - pstrcpy(params.name, sizeof(params.name), hci->device.lmp_name); - - bt_hci_event_complete(hci, ¶ms, READ_LOCAL_NAME_RP_SIZE); -} - -static inline void bt_hci_event_complete_read_conn_accept_timeout( - struct bt_hci_s *hci) -{ - read_conn_accept_timeout_rp params = { - .status = HCI_SUCCESS, - .timeout = cpu_to_le16(hci->conn_accept_tout), - }; - - bt_hci_event_complete(hci, ¶ms, READ_CONN_ACCEPT_TIMEOUT_RP_SIZE); -} - -static inline void bt_hci_event_complete_read_scan_enable(struct bt_hci_s *hci) -{ - read_scan_enable_rp params = { - .status = HCI_SUCCESS, - .enable = - (hci->device.inquiry_scan ? SCAN_INQUIRY : 0) | - (hci->device.page_scan ? SCAN_PAGE : 0), - }; - - bt_hci_event_complete(hci, ¶ms, READ_SCAN_ENABLE_RP_SIZE); -} - -static inline void bt_hci_event_complete_read_local_class(struct bt_hci_s *hci) -{ - read_class_of_dev_rp params; - - params.status = HCI_SUCCESS; - memcpy(params.dev_class, hci->device.class, sizeof(params.dev_class)); - - bt_hci_event_complete(hci, ¶ms, READ_CLASS_OF_DEV_RP_SIZE); -} - -static inline void bt_hci_event_complete_voice_setting(struct bt_hci_s *hci) -{ - read_voice_setting_rp params = { - .status = HCI_SUCCESS, - .voice_setting = hci->voice_setting, /* Note: no swapping */ - }; - - bt_hci_event_complete(hci, ¶ms, READ_VOICE_SETTING_RP_SIZE); -} - -static inline void bt_hci_event_complete_read_inquiry_mode( - struct bt_hci_s *hci) -{ - read_inquiry_mode_rp params = { - .status = HCI_SUCCESS, - .mode = hci->lm.inquiry_mode, - }; - - bt_hci_event_complete(hci, ¶ms, READ_INQUIRY_MODE_RP_SIZE); -} - -static inline void bt_hci_event_num_comp_pkts(struct bt_hci_s *hci, - uint16_t handle, int packets) -{ - uint16_t buf[EVT_NUM_COMP_PKTS_SIZE(1) / 2 + 1]; - evt_num_comp_pkts *params = (void *) ((uint8_t *) buf + 1); - - params->num_hndl = 1; - params->connection->handle = HNDL(handle); - params->connection->num_packets = cpu_to_le16(packets); - - bt_hci_event(hci, EVT_NUM_COMP_PKTS, params, EVT_NUM_COMP_PKTS_SIZE(1)); -} - -static void bt_submit_hci(struct HCIInfo *info, - const uint8_t *data, int length) -{ - struct bt_hci_s *hci = hci_from_info(info); - uint16_t cmd; - int paramlen, i; - - if (length < HCI_COMMAND_HDR_SIZE) - goto short_hci; - - memcpy(&hci->last_cmd, data, 2); - - cmd = (data[1] << 8) | data[0]; - paramlen = data[2]; - if (cmd_opcode_ogf(cmd) == 0 || cmd_opcode_ocf(cmd) == 0) /* NOP */ - return; - - data += HCI_COMMAND_HDR_SIZE; - length -= HCI_COMMAND_HDR_SIZE; - - if (paramlen > length) - return; - -#define PARAM(cmd, param) (((cmd##_cp *) data)->param) -#define PARAM16(cmd, param) le16_to_cpup(&PARAM(cmd, param)) -#define PARAMHANDLE(cmd) HNDL(PARAM(cmd, handle)) -#define LENGTH_CHECK(cmd) if (length < sizeof(cmd##_cp)) goto short_hci - /* Note: the supported commands bitmask in bt_hci_read_local_commands_rp - * needs to be updated every time a command is implemented here! */ - switch (cmd) { - case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY): - LENGTH_CHECK(inquiry); - - if (PARAM(inquiry, length) < 1) { - bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - hci->lm.inquire = 1; - hci->lm.periodic = 0; - hci->lm.responses_left = PARAM(inquiry, num_rsp) ?: INT_MAX; - hci->lm.responses = 0; - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_inquiry_start(hci, PARAM(inquiry, length)); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY_CANCEL): - if (!hci->lm.inquire || hci->lm.periodic) { - fprintf(stderr, "%s: Inquiry Cancel should only be issued after " - "the Inquiry command has been issued, a Command " - "Status event has been received for the Inquiry " - "command, and before the Inquiry Complete event " - "occurs", __FUNCTION__); - bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED); - break; - } - - hci->lm.inquire = 0; - timer_del(hci->lm.inquiry_done); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_PERIODIC_INQUIRY): - LENGTH_CHECK(periodic_inquiry); - - if (!(PARAM(periodic_inquiry, length) < - PARAM16(periodic_inquiry, min_period) && - PARAM16(periodic_inquiry, min_period) < - PARAM16(periodic_inquiry, max_period)) || - PARAM(periodic_inquiry, length) < 1 || - PARAM16(periodic_inquiry, min_period) < 2 || - PARAM16(periodic_inquiry, max_period) < 3) { - bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - hci->lm.inquire = 1; - hci->lm.periodic = 1; - hci->lm.responses_left = PARAM(periodic_inquiry, num_rsp); - hci->lm.responses = 0; - hci->lm.inquiry_period = PARAM16(periodic_inquiry, max_period); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - bt_hci_inquiry_start(hci, PARAM(periodic_inquiry, length)); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_EXIT_PERIODIC_INQUIRY): - if (!hci->lm.inquire || !hci->lm.periodic) { - fprintf(stderr, "%s: Inquiry Cancel should only be issued after " - "the Inquiry command has been issued, a Command " - "Status event has been received for the Inquiry " - "command, and before the Inquiry Complete event " - "occurs", __FUNCTION__); - bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED); - break; - } - hci->lm.inquire = 0; - timer_del(hci->lm.inquiry_done); - timer_del(hci->lm.inquiry_next); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN): - LENGTH_CHECK(create_conn); - - if (hci->lm.connecting >= HCI_HANDLES_MAX) { - bt_hci_event_status(hci, HCI_REJECTED_LIMITED_RESOURCES); - break; - } - bt_hci_event_status(hci, HCI_SUCCESS); - - if (bt_hci_connect(hci, &PARAM(create_conn, bdaddr))) - bt_hci_connection_reject_event(hci, &PARAM(create_conn, bdaddr)); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_DISCONNECT): - LENGTH_CHECK(disconnect); - - if (bt_hci_handle_bad(hci, PARAMHANDLE(disconnect))) { - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - } - - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_disconnect(hci, PARAMHANDLE(disconnect), - PARAM(disconnect, reason)); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN_CANCEL): - LENGTH_CHECK(create_conn_cancel); - - if (bt_hci_lmp_connection_ready(hci, - &PARAM(create_conn_cancel, bdaddr))) { - for (i = 0; i < HCI_HANDLES_MAX; i ++) - if (bt_hci_role_master(hci, i) && hci->lm.handle[i].link && - !bacmp(&hci->lm.handle[i].link->slave->bd_addr, - &PARAM(create_conn_cancel, bdaddr))) - break; - - bt_hci_event_complete_conn_cancel(hci, i < HCI_HANDLES_MAX ? - HCI_ACL_CONNECTION_EXISTS : HCI_NO_CONNECTION, - &PARAM(create_conn_cancel, bdaddr)); - } else - bt_hci_event_complete_conn_cancel(hci, HCI_SUCCESS, - &PARAM(create_conn_cancel, bdaddr)); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ): - LENGTH_CHECK(accept_conn_req); - - if (!hci->conn_req_host || - bacmp(&PARAM(accept_conn_req, bdaddr), - &hci->conn_req_host->bd_addr)) { - bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_connection_accept(hci, hci->conn_req_host); - hci->conn_req_host = NULL; - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_REJECT_CONN_REQ): - LENGTH_CHECK(reject_conn_req); - - if (!hci->conn_req_host || - bacmp(&PARAM(reject_conn_req, bdaddr), - &hci->conn_req_host->bd_addr)) { - bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_connection_reject(hci, hci->conn_req_host, - PARAM(reject_conn_req, reason)); - bt_hci_connection_reject_event(hci, &hci->conn_req_host->bd_addr); - hci->conn_req_host = NULL; - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_AUTH_REQUESTED): - LENGTH_CHECK(auth_requested); - - if (bt_hci_handle_bad(hci, PARAMHANDLE(auth_requested))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - else { - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_event_auth_complete(hci, PARAMHANDLE(auth_requested)); - } - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_SET_CONN_ENCRYPT): - LENGTH_CHECK(set_conn_encrypt); - - if (bt_hci_handle_bad(hci, PARAMHANDLE(set_conn_encrypt))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - else { - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_event_encrypt_change(hci, - PARAMHANDLE(set_conn_encrypt), - PARAM(set_conn_encrypt, encrypt)); - } - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ): - LENGTH_CHECK(remote_name_req); - - if (bt_hci_name_req(hci, &PARAM(remote_name_req, bdaddr))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ_CANCEL): - LENGTH_CHECK(remote_name_req_cancel); - - bt_hci_event_complete_name_cancel(hci, - &PARAM(remote_name_req_cancel, bdaddr)); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_FEATURES): - LENGTH_CHECK(read_remote_features); - - if (bt_hci_features_req(hci, PARAMHANDLE(read_remote_features))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_EXT_FEATURES): - LENGTH_CHECK(read_remote_ext_features); - - if (bt_hci_handle_bad(hci, PARAMHANDLE(read_remote_ext_features))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - else { - bt_hci_event_status(hci, HCI_SUCCESS); - bt_hci_event_read_remote_ext_features(hci, - PARAMHANDLE(read_remote_ext_features)); - } - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_VERSION): - LENGTH_CHECK(read_remote_version); - - if (bt_hci_version_req(hci, PARAMHANDLE(read_remote_version))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_CLOCK_OFFSET): - LENGTH_CHECK(read_clock_offset); - - if (bt_hci_clkoffset_req(hci, PARAMHANDLE(read_clock_offset))) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_LMP_HANDLE): - LENGTH_CHECK(read_lmp_handle); - - /* TODO: */ - bt_hci_event_complete_lmp_handle(hci, PARAMHANDLE(read_lmp_handle)); - break; - - case cmd_opcode_pack(OGF_LINK_POLICY, OCF_HOLD_MODE): - LENGTH_CHECK(hold_mode); - - if (PARAM16(hold_mode, min_interval) > - PARAM16(hold_mode, max_interval) || - PARAM16(hold_mode, min_interval) < 0x0002 || - PARAM16(hold_mode, max_interval) > 0xff00 || - (PARAM16(hold_mode, min_interval) & 1) || - (PARAM16(hold_mode, max_interval) & 1)) { - bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - if (bt_hci_mode_change(hci, PARAMHANDLE(hold_mode), - PARAM16(hold_mode, max_interval), - acl_hold)) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_POLICY, OCF_PARK_MODE): - LENGTH_CHECK(park_mode); - - if (PARAM16(park_mode, min_interval) > - PARAM16(park_mode, max_interval) || - PARAM16(park_mode, min_interval) < 0x000e || - (PARAM16(park_mode, min_interval) & 1) || - (PARAM16(park_mode, max_interval) & 1)) { - bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - if (bt_hci_mode_change(hci, PARAMHANDLE(park_mode), - PARAM16(park_mode, max_interval), - acl_parked)) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_POLICY, OCF_EXIT_PARK_MODE): - LENGTH_CHECK(exit_park_mode); - - if (bt_hci_mode_cancel(hci, PARAMHANDLE(exit_park_mode), - acl_parked)) - bt_hci_event_status(hci, HCI_NO_CONNECTION); - break; - - case cmd_opcode_pack(OGF_LINK_POLICY, OCF_ROLE_DISCOVERY): - LENGTH_CHECK(role_discovery); - - if (bt_hci_handle_bad(hci, PARAMHANDLE(role_discovery))) - bt_hci_event_complete_role_discovery(hci, - HCI_NO_CONNECTION, PARAMHANDLE(role_discovery), 0); - else - bt_hci_event_complete_role_discovery(hci, - HCI_SUCCESS, PARAMHANDLE(role_discovery), - bt_hci_role_master(hci, - PARAMHANDLE(role_discovery))); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_MASK): - LENGTH_CHECK(set_event_mask); - - memcpy(hci->event_mask, PARAM(set_event_mask, mask), 8); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_RESET): - bt_hci_reset(hci); - bt_hci_event_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_FLT): - if (length >= 1 && PARAM(set_event_flt, flt_type) == FLT_CLEAR_ALL) - /* No length check */; - else - LENGTH_CHECK(set_event_flt); - - /* Filters are not implemented */ - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_FLUSH): - LENGTH_CHECK(flush); - - if (bt_hci_handle_bad(hci, PARAMHANDLE(flush))) - bt_hci_event_complete_flush(hci, - HCI_NO_CONNECTION, PARAMHANDLE(flush)); - else { - /* TODO: ordering? */ - bt_hci_event(hci, EVT_FLUSH_OCCURRED, - &PARAM(flush, handle), - EVT_FLUSH_OCCURRED_SIZE); - bt_hci_event_complete_flush(hci, - HCI_SUCCESS, PARAMHANDLE(flush)); - } - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME): - LENGTH_CHECK(change_local_name); - - g_free((void *) hci->device.lmp_name); - hci->device.lmp_name = g_strndup(PARAM(change_local_name, name), - sizeof(PARAM(change_local_name, name))); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_LOCAL_NAME): - bt_hci_event_complete_read_local_name(hci); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CONN_ACCEPT_TIMEOUT): - bt_hci_event_complete_read_conn_accept_timeout(hci); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CONN_ACCEPT_TIMEOUT): - /* TODO */ - LENGTH_CHECK(write_conn_accept_timeout); - - if (PARAM16(write_conn_accept_timeout, timeout) < 0x0001 || - PARAM16(write_conn_accept_timeout, timeout) > 0xb540) { - bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - hci->conn_accept_tout = PARAM16(write_conn_accept_timeout, timeout); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_SCAN_ENABLE): - bt_hci_event_complete_read_scan_enable(hci); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE): - LENGTH_CHECK(write_scan_enable); - - /* TODO: check that the remaining bits are all 0 */ - hci->device.inquiry_scan = - !!(PARAM(write_scan_enable, scan_enable) & SCAN_INQUIRY); - hci->device.page_scan = - !!(PARAM(write_scan_enable, scan_enable) & SCAN_PAGE); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CLASS_OF_DEV): - bt_hci_event_complete_read_local_class(hci); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV): - LENGTH_CHECK(write_class_of_dev); - - memcpy(hci->device.class, PARAM(write_class_of_dev, dev_class), - sizeof(PARAM(write_class_of_dev, dev_class))); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_VOICE_SETTING): - bt_hci_event_complete_voice_setting(hci); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_VOICE_SETTING): - LENGTH_CHECK(write_voice_setting); - - hci->voice_setting = PARAM(write_voice_setting, voice_setting); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_HOST_NUMBER_OF_COMPLETED_PACKETS): - if (length < data[0] * 2 + 1) - goto short_hci; - - for (i = 0; i < data[0]; i ++) - if (bt_hci_handle_bad(hci, - data[i * 2 + 1] | (data[i * 2 + 2] << 8))) - bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_INQUIRY_MODE): - /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x40) - * else - * goto unknown_command */ - bt_hci_event_complete_read_inquiry_mode(hci); - break; - - case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_INQUIRY_MODE): - /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x80) - * else - * goto unknown_command */ - LENGTH_CHECK(write_inquiry_mode); - - if (PARAM(write_inquiry_mode, mode) > 0x01) { - bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); - break; - } - - hci->lm.inquiry_mode = PARAM(write_inquiry_mode, mode); - bt_hci_event_complete_status(hci, HCI_SUCCESS); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_VERSION): - bt_hci_read_local_version_rp(hci); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_COMMANDS): - bt_hci_read_local_commands_rp(hci); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_FEATURES): - bt_hci_read_local_features_rp(hci); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_EXT_FEATURES): - LENGTH_CHECK(read_local_ext_features); - - bt_hci_read_local_ext_features_rp(hci, - PARAM(read_local_ext_features, page_num)); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BUFFER_SIZE): - bt_hci_read_buffer_size_rp(hci); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_COUNTRY_CODE): - bt_hci_read_country_code_rp(hci); - break; - - case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BD_ADDR): - bt_hci_read_bd_addr_rp(hci); - break; - - case cmd_opcode_pack(OGF_STATUS_PARAM, OCF_READ_LINK_QUALITY): - LENGTH_CHECK(read_link_quality); - - bt_hci_link_quality_rp(hci, PARAMHANDLE(read_link_quality)); - break; - - default: - bt_hci_event_status(hci, HCI_UNKNOWN_COMMAND); - break; - - short_hci: - fprintf(stderr, "%s: HCI packet too short (%iB)\n", - __FUNCTION__, length); - bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); - break; - } -} - -/* We could perform fragmentation here, we can't do "recombination" because - * at this layer the length of the payload is not know ahead, so we only - * know that a packet contained the last fragment of the SDU when the next - * SDU starts. */ -static inline void bt_hci_lmp_acl_data(struct bt_hci_s *hci, uint16_t handle, - const uint8_t *data, int start, int len) -{ - struct hci_acl_hdr *pkt = (void *) hci->acl_buf; - - /* TODO: packet flags */ - /* TODO: avoid memcpy'ing */ - - if (len + HCI_ACL_HDR_SIZE > sizeof(hci->acl_buf)) { - fprintf(stderr, "%s: can't take ACL packets %i bytes long\n", - __FUNCTION__, len); - return; - } - memcpy(hci->acl_buf + HCI_ACL_HDR_SIZE, data, len); - - pkt->handle = cpu_to_le16( - acl_handle_pack(handle, start ? ACL_START : ACL_CONT)); - pkt->dlen = cpu_to_le16(len); - hci->info.acl_recv(hci->info.opaque, - hci->acl_buf, len + HCI_ACL_HDR_SIZE); -} - -static void bt_hci_lmp_acl_data_slave(struct bt_link_s *btlink, - const uint8_t *data, int start, int len) -{ - struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink; - - bt_hci_lmp_acl_data(hci_from_device(btlink->slave), - link->handle, data, start, len); -} - -static void bt_hci_lmp_acl_data_host(struct bt_link_s *link, - const uint8_t *data, int start, int len) -{ - bt_hci_lmp_acl_data(hci_from_device(link->host), - link->handle, data, start, len); -} - -static void bt_submit_acl(struct HCIInfo *info, - const uint8_t *data, int length) -{ - struct bt_hci_s *hci = hci_from_info(info); - uint16_t handle; - int datalen, flags; - struct bt_link_s *link; - - if (length < HCI_ACL_HDR_SIZE) { - fprintf(stderr, "%s: ACL packet too short (%iB)\n", - __FUNCTION__, length); - return; - } - - handle = acl_handle((data[1] << 8) | data[0]); - flags = acl_flags((data[1] << 8) | data[0]); - datalen = (data[3] << 8) | data[2]; - data += HCI_ACL_HDR_SIZE; - length -= HCI_ACL_HDR_SIZE; - - if (bt_hci_handle_bad(hci, handle)) { - fprintf(stderr, "%s: invalid ACL handle %03x\n", - __FUNCTION__, handle); - /* TODO: signal an error */ - return; - } - handle &= ~HCI_HANDLE_OFFSET; - - if (datalen > length) { - fprintf(stderr, "%s: ACL packet too short (%iB < %iB)\n", - __FUNCTION__, length, datalen); - return; - } - - link = hci->lm.handle[handle].link; - - if ((flags & ~3) == ACL_ACTIVE_BCAST) { - if (!hci->asb_handle) - hci->asb_handle = handle; - else if (handle != hci->asb_handle) { - fprintf(stderr, "%s: Bad handle %03x in Active Slave Broadcast\n", - __FUNCTION__, handle); - /* TODO: signal an error */ - return; - } - - /* TODO */ - } - - if ((flags & ~3) == ACL_PICO_BCAST) { - if (!hci->psb_handle) - hci->psb_handle = handle; - else if (handle != hci->psb_handle) { - fprintf(stderr, "%s: Bad handle %03x in Parked Slave Broadcast\n", - __FUNCTION__, handle); - /* TODO: signal an error */ - return; - } - - /* TODO */ - } - - /* TODO: increase counter and send EVT_NUM_COMP_PKTS */ - bt_hci_event_num_comp_pkts(hci, handle | HCI_HANDLE_OFFSET, 1); - - /* Do this last as it can trigger further events even in this HCI */ - hci->lm.handle[handle].lmp_acl_data(link, data, - (flags & 3) == ACL_START, length); -} - -static void bt_submit_sco(struct HCIInfo *info, - const uint8_t *data, int length) -{ - struct bt_hci_s *hci = hci_from_info(info); - uint16_t handle; - int datalen; - - if (length < 3) - return; - - handle = acl_handle((data[1] << 8) | data[0]); - datalen = data[2]; - length -= 3; - - if (bt_hci_handle_bad(hci, handle)) { - fprintf(stderr, "%s: invalid SCO handle %03x\n", - __FUNCTION__, handle); - return; - } - - if (datalen > length) { - fprintf(stderr, "%s: SCO packet too short (%iB < %iB)\n", - __FUNCTION__, length, datalen); - return; - } - - /* TODO */ - - /* TODO: increase counter and send EVT_NUM_COMP_PKTS if synchronous - * Flow Control is enabled. - * (See Read/Write_Synchronous_Flow_Control_Enable on page 513 and - * page 514.) */ -} - -static uint8_t *bt_hci_evt_packet(void *opaque) -{ - /* TODO: allocate a packet from upper layer */ - struct bt_hci_s *s = opaque; - - return s->evt_buf; -} - -static void bt_hci_evt_submit(void *opaque, int len) -{ - /* TODO: notify upper layer */ - struct bt_hci_s *s = opaque; - - s->info.evt_recv(s->info.opaque, s->evt_buf, len); -} - -static int bt_hci_bdaddr_set(struct HCIInfo *info, const uint8_t *bd_addr) -{ - struct bt_hci_s *hci = hci_from_info(info); - - bacpy(&hci->device.bd_addr, (const bdaddr_t *) bd_addr); - return 0; -} - -static void bt_hci_done(struct HCIInfo *info); -static void bt_hci_destroy(struct bt_device_s *dev) -{ - struct bt_hci_s *hci = hci_from_device(dev); - - bt_hci_done(&hci->info); -} - -struct HCIInfo *bt_new_hci(struct bt_scatternet_s *net) -{ - struct bt_hci_s *s = g_malloc0(sizeof(struct bt_hci_s)); - - s->lm.inquiry_done = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_done, s); - s->lm.inquiry_next = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_next, s); - s->conn_accept_timer = - timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_conn_accept_timeout, s); - - s->evt_packet = bt_hci_evt_packet; - s->evt_submit = bt_hci_evt_submit; - s->opaque = s; - - bt_device_init(&s->device, net); - s->device.lmp_connection_request = bt_hci_lmp_connection_request; - s->device.lmp_connection_complete = bt_hci_lmp_connection_complete; - s->device.lmp_disconnect_master = bt_hci_lmp_disconnect_host; - s->device.lmp_disconnect_slave = bt_hci_lmp_disconnect_slave; - s->device.lmp_acl_data = bt_hci_lmp_acl_data_slave; - s->device.lmp_acl_resp = bt_hci_lmp_acl_data_host; - s->device.lmp_mode_change = bt_hci_lmp_mode_change_slave; - - /* Keep updated! */ - /* Also keep in sync with supported commands bitmask in - * bt_hci_read_local_commands_rp */ - s->device.lmp_caps = 0x8000199b7e85355fll; - - bt_hci_reset(s); - - s->info.cmd_send = bt_submit_hci; - s->info.sco_send = bt_submit_sco; - s->info.acl_send = bt_submit_acl; - s->info.bdaddr_set = bt_hci_bdaddr_set; - - s->device.handle_destroy = bt_hci_destroy; - - error_setg(&s->replay_blocker, QERR_REPLAY_NOT_SUPPORTED, "-bt hci"); - replay_add_blocker(s->replay_blocker); - - return &s->info; -} - -struct HCIInfo *hci_init(const char *str) -{ - char *endp; - struct bt_scatternet_s *vlan = 0; - - if (!strcmp(str, "null")) - /* null */ - return &null_hci; - else if (!strncmp(str, "host", 4) && (str[4] == '\0' || str[4] == ':')) - /* host[:hciN] */ - return bt_host_hci(str[4] ? str + 5 : "hci0"); - else if (!strncmp(str, "hci", 3)) { - /* hci[,vlan=n] */ - if (str[3]) { - if (!strncmp(str + 3, ",vlan=", 6)) { - vlan = qemu_find_bt_vlan(strtol(str + 9, &endp, 0)); - if (*endp) - vlan = 0; - } - } else - vlan = qemu_find_bt_vlan(0); - if (vlan) - return bt_new_hci(vlan); - } - - fprintf(stderr, "qemu: Unknown bluetooth HCI `%s'.\n", str); - - return 0; -} - -static void bt_hci_done(struct HCIInfo *info) -{ - struct bt_hci_s *hci = hci_from_info(info); - int handle; - - bt_device_done(&hci->device); - - g_free((void *) hci->device.lmp_name); - - /* Be gentle and send DISCONNECT to all connected peers and those - * currently waiting for us to accept or reject a connection request. - * This frees the links. */ - if (hci->conn_req_host) { - bt_hci_connection_reject(hci, - hci->conn_req_host, HCI_OE_POWER_OFF); - return; - } - - for (handle = HCI_HANDLE_OFFSET; - handle < (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX); handle ++) - if (!bt_hci_handle_bad(hci, handle)) - bt_hci_disconnect(hci, handle, HCI_OE_POWER_OFF); - - /* TODO: this is not enough actually, there may be slaves from whom - * we have requested a connection who will soon (or not) respond with - * an accept or a reject, so we should also check if hci->lm.connecting - * is non-zero and if so, avoid freeing the hci but otherwise disappear - * from all qemu social life (e.g. stop scanning and request to be - * removed from s->device.net) and arrange for - * s->device.lmp_connection_complete to free the remaining bits once - * hci->lm.awaiting_bdaddr[] is empty. */ - - timer_free(hci->lm.inquiry_done); - timer_free(hci->lm.inquiry_next); - timer_free(hci->conn_accept_timer); - - g_free(hci); -} diff --git a/qemu/hw/bt/hid.c b/qemu/hw/bt/hid.c deleted file mode 100644 index f6affbbb4..000000000 --- a/qemu/hw/bt/hid.c +++ /dev/null @@ -1,554 +0,0 @@ -/* - * QEMU Bluetooth HID Profile wrapper for USB HID. - * - * Copyright (C) 2007-2008 OpenMoko, Inc. - * Written by Andrzej Zaborowski <andrew@openedhand.com> - * - * 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 or - * (at your option) version 3 of the License. - * - * 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, if not, see <http://www.gnu.org/licenses/>. - */ - -#include "qemu/osdep.h" -#include "qemu-common.h" -#include "qemu/timer.h" -#include "ui/console.h" -#include "hw/input/hid.h" -#include "hw/bt.h" - -enum hid_transaction_req { - BT_HANDSHAKE = 0x0, - BT_HID_CONTROL = 0x1, - BT_GET_REPORT = 0x4, - BT_SET_REPORT = 0x5, - BT_GET_PROTOCOL = 0x6, - BT_SET_PROTOCOL = 0x7, - BT_GET_IDLE = 0x8, - BT_SET_IDLE = 0x9, - BT_DATA = 0xa, - BT_DATC = 0xb, -}; - -enum hid_transaction_handshake { - BT_HS_SUCCESSFUL = 0x0, - BT_HS_NOT_READY = 0x1, - BT_HS_ERR_INVALID_REPORT_ID = 0x2, - BT_HS_ERR_UNSUPPORTED_REQUEST = 0x3, - BT_HS_ERR_INVALID_PARAMETER = 0x4, - BT_HS_ERR_UNKNOWN = 0xe, - BT_HS_ERR_FATAL = 0xf, -}; - -enum hid_transaction_control { - BT_HC_NOP = 0x0, - BT_HC_HARD_RESET = 0x1, - BT_HC_SOFT_RESET = 0x2, - BT_HC_SUSPEND = 0x3, - BT_HC_EXIT_SUSPEND = 0x4, - BT_HC_VIRTUAL_CABLE_UNPLUG = 0x5, -}; - -enum hid_protocol { - BT_HID_PROTO_BOOT = 0, - BT_HID_PROTO_REPORT = 1, -}; - -enum hid_boot_reportid { - BT_HID_BOOT_INVALID = 0, - BT_HID_BOOT_KEYBOARD, - BT_HID_BOOT_MOUSE, -}; - -enum hid_data_pkt { - BT_DATA_OTHER = 0, - BT_DATA_INPUT, - BT_DATA_OUTPUT, - BT_DATA_FEATURE, -}; - -#define BT_HID_MTU 48 - -/* HID interface requests */ -#define GET_REPORT 0xa101 -#define GET_IDLE 0xa102 -#define GET_PROTOCOL 0xa103 -#define SET_REPORT 0x2109 -#define SET_IDLE 0x210a -#define SET_PROTOCOL 0x210b - -struct bt_hid_device_s { - struct bt_l2cap_device_s btdev; - struct bt_l2cap_conn_params_s *control; - struct bt_l2cap_conn_params_s *interrupt; - HIDState hid; - - int proto; - int connected; - int data_type; - int intr_state; - struct { - int len; - uint8_t buffer[1024]; - } dataother, datain, dataout, feature, intrdataout; - enum { - bt_state_ready, - bt_state_transaction, - bt_state_suspend, - } state; -}; - -static void bt_hid_reset(struct bt_hid_device_s *s) -{ - struct bt_scatternet_s *net = s->btdev.device.net; - - /* Go as far as... */ - bt_l2cap_device_done(&s->btdev); - bt_l2cap_device_init(&s->btdev, net); - - hid_reset(&s->hid); - s->proto = BT_HID_PROTO_REPORT; - s->state = bt_state_ready; - s->dataother.len = 0; - s->datain.len = 0; - s->dataout.len = 0; - s->feature.len = 0; - s->intrdataout.len = 0; - s->intr_state = 0; -} - -static int bt_hid_out(struct bt_hid_device_s *s) -{ - if (s->data_type == BT_DATA_OUTPUT) { - /* nothing */ - ; - } - - if (s->data_type == BT_DATA_FEATURE) { - /* XXX: - * does this send a USB_REQ_CLEAR_FEATURE/USB_REQ_SET_FEATURE - * or a SET_REPORT? */ - ; - } - - return -1; -} - -static int bt_hid_in(struct bt_hid_device_s *s) -{ - s->datain.len = hid_keyboard_poll(&s->hid, s->datain.buffer, - sizeof(s->datain.buffer)); - return s->datain.len; -} - -static void bt_hid_send_handshake(struct bt_hid_device_s *s, int result) -{ - *s->control->sdu_out(s->control, 1) = - (BT_HANDSHAKE << 4) | result; - s->control->sdu_submit(s->control); -} - -static void bt_hid_send_control(struct bt_hid_device_s *s, int operation) -{ - *s->control->sdu_out(s->control, 1) = - (BT_HID_CONTROL << 4) | operation; - s->control->sdu_submit(s->control); -} - -static void bt_hid_disconnect(struct bt_hid_device_s *s) -{ - /* Disconnect s->control and s->interrupt */ -} - -static void bt_hid_send_data(struct bt_l2cap_conn_params_s *ch, int type, - const uint8_t *data, int len) -{ - uint8_t *pkt, hdr = (BT_DATA << 4) | type; - int plen; - - do { - plen = MIN(len, ch->remote_mtu - 1); - pkt = ch->sdu_out(ch, plen + 1); - - pkt[0] = hdr; - if (plen) - memcpy(pkt + 1, data, plen); - ch->sdu_submit(ch); - - len -= plen; - data += plen; - hdr = (BT_DATC << 4) | type; - } while (plen == ch->remote_mtu - 1); -} - -static void bt_hid_control_transaction(struct bt_hid_device_s *s, - const uint8_t *data, int len) -{ - uint8_t type, parameter; - int rlen, ret = -1; - if (len < 1) - return; - - type = data[0] >> 4; - parameter = data[0] & 0xf; - - switch (type) { - case BT_HANDSHAKE: - case BT_DATA: - switch (parameter) { - default: - /* These are not expected to be sent this direction. */ - ret = BT_HS_ERR_INVALID_PARAMETER; - } - break; - - case BT_HID_CONTROL: - if (len != 1 || (parameter != BT_HC_VIRTUAL_CABLE_UNPLUG && - s->state == bt_state_transaction)) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - switch (parameter) { - case BT_HC_NOP: - break; - case BT_HC_HARD_RESET: - case BT_HC_SOFT_RESET: - bt_hid_reset(s); - break; - case BT_HC_SUSPEND: - if (s->state == bt_state_ready) - s->state = bt_state_suspend; - else - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - case BT_HC_EXIT_SUSPEND: - if (s->state == bt_state_suspend) - s->state = bt_state_ready; - else - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - case BT_HC_VIRTUAL_CABLE_UNPLUG: - bt_hid_disconnect(s); - break; - default: - ret = BT_HS_ERR_INVALID_PARAMETER; - } - break; - - case BT_GET_REPORT: - /* No ReportIDs declared. */ - if (((parameter & 8) && len != 3) || - (!(parameter & 8) && len != 1) || - s->state != bt_state_ready) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - if (parameter & 8) - rlen = data[2] | (data[3] << 8); - else - rlen = INT_MAX; - switch (parameter & 3) { - case BT_DATA_OTHER: - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - case BT_DATA_INPUT: - /* Here we can as well poll s->usbdev */ - bt_hid_send_data(s->control, BT_DATA_INPUT, - s->datain.buffer, MIN(rlen, s->datain.len)); - break; - case BT_DATA_OUTPUT: - bt_hid_send_data(s->control, BT_DATA_OUTPUT, - s->dataout.buffer, MIN(rlen, s->dataout.len)); - break; - case BT_DATA_FEATURE: - bt_hid_send_data(s->control, BT_DATA_FEATURE, - s->feature.buffer, MIN(rlen, s->feature.len)); - break; - } - break; - - case BT_SET_REPORT: - if (len < 2 || len > BT_HID_MTU || s->state != bt_state_ready || - (parameter & 3) == BT_DATA_OTHER || - (parameter & 3) == BT_DATA_INPUT) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - s->data_type = parameter & 3; - if (s->data_type == BT_DATA_OUTPUT) { - s->dataout.len = len - 1; - memcpy(s->dataout.buffer, data + 1, s->dataout.len); - } else { - s->feature.len = len - 1; - memcpy(s->feature.buffer, data + 1, s->feature.len); - } - if (len == BT_HID_MTU) - s->state = bt_state_transaction; - else - bt_hid_out(s); - break; - - case BT_GET_PROTOCOL: - if (len != 1 || s->state == bt_state_transaction) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - *s->control->sdu_out(s->control, 1) = s->proto; - s->control->sdu_submit(s->control); - break; - - case BT_SET_PROTOCOL: - if (len != 1 || s->state == bt_state_transaction || - (parameter != BT_HID_PROTO_BOOT && - parameter != BT_HID_PROTO_REPORT)) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - s->proto = parameter; - s->hid.protocol = parameter; - ret = BT_HS_SUCCESSFUL; - break; - - case BT_GET_IDLE: - if (len != 1 || s->state == bt_state_transaction) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - *s->control->sdu_out(s->control, 1) = s->hid.idle; - s->control->sdu_submit(s->control); - break; - - case BT_SET_IDLE: - if (len != 2 || s->state == bt_state_transaction) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - - s->hid.idle = data[1]; - /* XXX: Does this generate a handshake? */ - break; - - case BT_DATC: - if (len > BT_HID_MTU || s->state != bt_state_transaction) { - ret = BT_HS_ERR_INVALID_PARAMETER; - break; - } - if (s->data_type == BT_DATA_OUTPUT) { - memcpy(s->dataout.buffer + s->dataout.len, data + 1, len - 1); - s->dataout.len += len - 1; - } else { - memcpy(s->feature.buffer + s->feature.len, data + 1, len - 1); - s->feature.len += len - 1; - } - if (len < BT_HID_MTU) { - bt_hid_out(s); - s->state = bt_state_ready; - } - break; - - default: - ret = BT_HS_ERR_UNSUPPORTED_REQUEST; - } - - if (ret != -1) - bt_hid_send_handshake(s, ret); -} - -static void bt_hid_control_sdu(void *opaque, const uint8_t *data, int len) -{ - struct bt_hid_device_s *hid = opaque; - - bt_hid_control_transaction(hid, data, len); -} - -static void bt_hid_datain(HIDState *hs) -{ - struct bt_hid_device_s *hid = - container_of(hs, struct bt_hid_device_s, hid); - - /* If suspended, wake-up and send a wake-up event first. We might - * want to also inspect the input report and ignore event like - * mouse movements until a button event occurs. */ - if (hid->state == bt_state_suspend) { - hid->state = bt_state_ready; - } - - if (bt_hid_in(hid) > 0) - /* TODO: when in boot-mode precede any Input reports with the ReportID - * byte, here and in GetReport/SetReport on the Control channel. */ - bt_hid_send_data(hid->interrupt, BT_DATA_INPUT, - hid->datain.buffer, hid->datain.len); -} - -static void bt_hid_interrupt_sdu(void *opaque, const uint8_t *data, int len) -{ - struct bt_hid_device_s *hid = opaque; - - if (len > BT_HID_MTU || len < 1) - goto bad; - if ((data[0] & 3) != BT_DATA_OUTPUT) - goto bad; - if ((data[0] >> 4) == BT_DATA) { - if (hid->intr_state) - goto bad; - - hid->data_type = BT_DATA_OUTPUT; - hid->intrdataout.len = 0; - } else if ((data[0] >> 4) == BT_DATC) { - if (!hid->intr_state) - goto bad; - } else - goto bad; - - memcpy(hid->intrdataout.buffer + hid->intrdataout.len, data + 1, len - 1); - hid->intrdataout.len += len - 1; - hid->intr_state = (len == BT_HID_MTU); - if (!hid->intr_state) { - memcpy(hid->dataout.buffer, hid->intrdataout.buffer, - hid->dataout.len = hid->intrdataout.len); - bt_hid_out(hid); - } - - return; -bad: - fprintf(stderr, "%s: bad transaction on Interrupt channel.\n", - __FUNCTION__); -} - -/* "Virtual cable" plug/unplug event. */ -static void bt_hid_connected_update(struct bt_hid_device_s *hid) -{ - int prev = hid->connected; - - hid->connected = hid->control && hid->interrupt; - - /* Stop page-/inquiry-scanning when a host is connected. */ - hid->btdev.device.page_scan = !hid->connected; - hid->btdev.device.inquiry_scan = !hid->connected; - - if (hid->connected && !prev) { - hid_reset(&hid->hid); - hid->proto = BT_HID_PROTO_REPORT; - } - - /* Should set HIDVirtualCable in SDP (possibly need to check that SDP - * isn't destroyed yet, in case we're being called from handle_destroy) */ -} - -static void bt_hid_close_control(void *opaque) -{ - struct bt_hid_device_s *hid = opaque; - - hid->control = NULL; - bt_hid_connected_update(hid); -} - -static void bt_hid_close_interrupt(void *opaque) -{ - struct bt_hid_device_s *hid = opaque; - - hid->interrupt = NULL; - bt_hid_connected_update(hid); -} - -static int bt_hid_new_control_ch(struct bt_l2cap_device_s *dev, - struct bt_l2cap_conn_params_s *params) -{ - struct bt_hid_device_s *hid = (struct bt_hid_device_s *) dev; - - if (hid->control) - return 1; - - hid->control = params; - hid->control->opaque = hid; - hid->control->close = bt_hid_close_control; - hid->control->sdu_in = bt_hid_control_sdu; - - bt_hid_connected_update(hid); - - return 0; -} - -static int bt_hid_new_interrupt_ch(struct bt_l2cap_device_s *dev, - struct bt_l2cap_conn_params_s *params) -{ - struct bt_hid_device_s *hid = (struct bt_hid_device_s *) dev; - - if (hid->interrupt) - return 1; - - hid->interrupt = params; - hid->interrupt->opaque = hid; - hid->interrupt->close = bt_hid_close_interrupt; - hid->interrupt->sdu_in = bt_hid_interrupt_sdu; - - bt_hid_connected_update(hid); - - return 0; -} - -static void bt_hid_destroy(struct bt_device_s *dev) -{ - struct bt_hid_device_s *hid = (struct bt_hid_device_s *) dev; - - if (hid->connected) - bt_hid_send_control(hid, BT_HC_VIRTUAL_CABLE_UNPLUG); - bt_l2cap_device_done(&hid->btdev); - - hid_free(&hid->hid); - - g_free(hid); -} - -enum peripheral_minor_class { - class_other = 0 << 4, - class_keyboard = 1 << 4, - class_pointing = 2 << 4, - class_combo = 3 << 4, -}; - -static struct bt_device_s *bt_hid_init(struct bt_scatternet_s *net, - enum peripheral_minor_class minor) -{ - struct bt_hid_device_s *s = g_malloc0(sizeof(*s)); - uint32_t class = - /* Format type */ - (0 << 0) | - /* Device class */ - (minor << 2) | - (5 << 8) | /* "Peripheral" */ - /* Service classes */ - (1 << 13) | /* Limited discoverable mode */ - (1 << 19); /* Capturing device (?) */ - - bt_l2cap_device_init(&s->btdev, net); - bt_l2cap_sdp_init(&s->btdev); - bt_l2cap_psm_register(&s->btdev, BT_PSM_HID_CTRL, - BT_HID_MTU, bt_hid_new_control_ch); - bt_l2cap_psm_register(&s->btdev, BT_PSM_HID_INTR, - BT_HID_MTU, bt_hid_new_interrupt_ch); - - hid_init(&s->hid, HID_KEYBOARD, bt_hid_datain); - s->btdev.device.lmp_name = "BT Keyboard"; - - s->btdev.device.handle_destroy = bt_hid_destroy; - - s->btdev.device.class[0] = (class >> 0) & 0xff; - s->btdev.device.class[1] = (class >> 8) & 0xff; - s->btdev.device.class[2] = (class >> 16) & 0xff; - - return &s->btdev.device; -} - -struct bt_device_s *bt_keyboard_init(struct bt_scatternet_s *net) -{ - return bt_hid_init(net, class_keyboard); -} diff --git a/qemu/hw/bt/l2cap.c b/qemu/hw/bt/l2cap.c deleted file mode 100644 index 806525194..000000000 --- a/qemu/hw/bt/l2cap.c +++ /dev/null @@ -1,1366 +0,0 @@ -/* - * QEMU Bluetooth L2CAP logic. - * - * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.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, see <http://www.gnu.org/licenses/>. - */ - -#include "qemu/osdep.h" -#include "qemu-common.h" -#include "qemu/timer.h" -#include "hw/bt.h" - -#define L2CAP_CID_MAX 0x100 /* Between 0x40 and 0x10000 */ - -struct l2cap_instance_s { - struct bt_link_s *link; - struct bt_l2cap_device_s *dev; - int role; - - uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4))); - int frame_in_len; - - uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4))); - int frame_out_len; - - /* Signalling channel timers. They exist per-request but we can make - * sure we have no more than one outstanding request at any time. */ - QEMUTimer *rtx; - QEMUTimer *ertx; - - int last_id; - int next_id; - - struct l2cap_chan_s { - struct bt_l2cap_conn_params_s params; - - void (*frame_in)(struct l2cap_chan_s *chan, uint16_t cid, - const l2cap_hdr *hdr, int len); - int mps; - int min_mtu; - - struct l2cap_instance_s *l2cap; - - /* Only allocated channels */ - uint16_t remote_cid; -#define L2CAP_CFG_INIT 2 -#define L2CAP_CFG_ACC 1 - int config_req_id; /* TODO: handle outgoing requests generically */ - int config; - - /* Only connection-oriented channels. Note: if we allow the tx and - * rx traffic to be in different modes at any time, we need two. */ - int mode; - - /* Only flow-controlled, connection-oriented channels */ - uint8_t sdu[65536]; /* TODO: dynamically allocate */ - int len_cur, len_total; - int rexmit; - int monitor_timeout; - QEMUTimer *monitor_timer; - QEMUTimer *retransmission_timer; - } *cid[L2CAP_CID_MAX]; - /* The channel state machine states map as following: - * CLOSED -> !cid[N] - * WAIT_CONNECT -> never occurs - * WAIT_CONNECT_RSP -> never occurs - * CONFIG -> cid[N] && config < 3 - * WAIT_CONFIG -> never occurs, cid[N] && config == 0 && !config_r - * WAIT_SEND_CONFIG -> never occurs, cid[N] && config == 1 && !config_r - * WAIT_CONFIG_REQ_RSP -> cid[N] && config == 0 && config_req_id - * WAIT_CONFIG_RSP -> cid[N] && config == 1 && config_req_id - * WAIT_CONFIG_REQ -> cid[N] && config == 2 - * OPEN -> cid[N] && config == 3 - * WAIT_DISCONNECT -> never occurs - */ - - struct l2cap_chan_s signalling_ch; - struct l2cap_chan_s group_ch; -}; - -struct slave_l2cap_instance_s { - struct bt_link_s link; /* Underlying logical link (ACL) */ - struct l2cap_instance_s l2cap; -}; - -struct bt_l2cap_psm_s { - int psm; - int min_mtu; - int (*new_channel)(struct bt_l2cap_device_s *device, - struct bt_l2cap_conn_params_s *params); - struct bt_l2cap_psm_s *next; -}; - -static const uint16_t l2cap_fcs16_table[256] = { - 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, - 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, - 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, - 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, - 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, - 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, - 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, - 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, - 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, - 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, - 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, - 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, - 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, - 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, - 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, - 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, - 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, - 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, - 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, - 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, - 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, - 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, - 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, - 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, - 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, - 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, - 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, - 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, - 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, - 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, - 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, - 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, -}; - -static uint16_t l2cap_fcs16(const uint8_t *message, int len) -{ - uint16_t fcs = 0x0000; - - while (len --) -#if 0 - { - int i; - - fcs ^= *message ++; - for (i = 8; i; -- i) - if (fcs & 1) - fcs = (fcs >> 1) ^ 0xa001; - else - fcs = (fcs >> 1); - } -#else - fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff]; -#endif - - return fcs; -} - -/* L2CAP layer logic (protocol) */ - -static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch) -{ -#if 0 - if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit) - timer_mod(ch->retransmission_timer); - else - timer_del(ch->retransmission_timer); -#endif -} - -static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch) -{ -#if 0 - if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit) - timer_mod(ch->monitor_timer); - else - timer_del(ch->monitor_timer); -#endif -} - -static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id, - uint16_t reason, const void *data, int plen) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - l2cap_cmd_rej *params; - uint16_t len; - - reason = cpu_to_le16(reason); - len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen); - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE + plen); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - hdr->code = L2CAP_COMMAND_REJ; - hdr->ident = id; - memcpy(&hdr->len, &len, sizeof(hdr->len)); - memcpy(¶ms->reason, &reason, sizeof(reason)); - if (plen) - memcpy(pkt + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE, data, plen); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id, - uint16_t reason, uint16_t dcid, uint16_t scid) -{ - l2cap_cmd_rej_cid params = { - .dcid = dcid, - .scid = scid, - }; - - l2cap_command_reject(l2cap, id, reason, ¶ms, L2CAP_CMD_REJ_CID_SIZE); -} - -static void l2cap_connection_response(struct l2cap_instance_s *l2cap, - int dcid, int scid, int result, int status) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - l2cap_conn_rsp *params; - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + L2CAP_CONN_RSP_SIZE); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - hdr->code = L2CAP_CONN_RSP; - hdr->ident = l2cap->last_id; - hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE); - - params->dcid = cpu_to_le16(dcid); - params->scid = cpu_to_le16(scid); - params->result = cpu_to_le16(result); - params->status = cpu_to_le16(status); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static void l2cap_configuration_request(struct l2cap_instance_s *l2cap, - int dcid, int flag, const uint8_t *data, int len) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - l2cap_conf_req *params; - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + L2CAP_CONF_REQ_SIZE(len)); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - /* TODO: unify the id sequencing */ - l2cap->last_id = l2cap->next_id; - l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1; - - hdr->code = L2CAP_CONF_REQ; - hdr->ident = l2cap->last_id; - hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len)); - - params->dcid = cpu_to_le16(dcid); - params->flags = cpu_to_le16(flag); - if (len) - memcpy(params->data, data, len); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static void l2cap_configuration_response(struct l2cap_instance_s *l2cap, - int scid, int flag, int result, const uint8_t *data, int len) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - l2cap_conf_rsp *params; - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + L2CAP_CONF_RSP_SIZE(len)); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - hdr->code = L2CAP_CONF_RSP; - hdr->ident = l2cap->last_id; - hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len)); - - params->scid = cpu_to_le16(scid); - params->flags = cpu_to_le16(flag); - params->result = cpu_to_le16(result); - if (len) - memcpy(params->data, data, len); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap, - int dcid, int scid) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - l2cap_disconn_rsp *params; - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + L2CAP_DISCONN_RSP_SIZE); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - hdr->code = L2CAP_DISCONN_RSP; - hdr->ident = l2cap->last_id; - hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE); - - params->dcid = cpu_to_le16(dcid); - params->scid = cpu_to_le16(scid); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static void l2cap_echo_response(struct l2cap_instance_s *l2cap, - const uint8_t *data, int len) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - uint8_t *params; - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + len); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - hdr->code = L2CAP_ECHO_RSP; - hdr->ident = l2cap->last_id; - hdr->len = cpu_to_le16(len); - - memcpy(params, data, len); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type, - int result, const uint8_t *data, int len) -{ - uint8_t *pkt; - l2cap_cmd_hdr *hdr; - l2cap_info_rsp *params; - - pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, - L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + len); - hdr = (void *) (pkt + 0); - params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); - - hdr->code = L2CAP_INFO_RSP; - hdr->ident = l2cap->last_id; - hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len); - - params->type = cpu_to_le16(type); - params->result = cpu_to_le16(result); - if (len) - memcpy(params->data, data, len); - - l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); -} - -static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len); -static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms); -#if 0 -static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len); -static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm); -#endif -static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid, - const l2cap_hdr *hdr, int len); -static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid, - const l2cap_hdr *hdr, int len); - -static int l2cap_cid_new(struct l2cap_instance_s *l2cap) -{ - int i; - - for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++) - if (!l2cap->cid[i]) - return i; - - return L2CAP_CID_INVALID; -} - -static inline struct bt_l2cap_psm_s *l2cap_psm( - struct bt_l2cap_device_s *device, int psm) -{ - struct bt_l2cap_psm_s *ret = device->first_psm; - - while (ret && ret->psm != psm) - ret = ret->next; - - return ret; -} - -static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap, - int psm, int source_cid) -{ - struct l2cap_chan_s *ch = NULL; - struct bt_l2cap_psm_s *psm_info; - int result, status; - int cid = l2cap_cid_new(l2cap); - - if (cid) { - /* See what the channel is to be used for.. */ - psm_info = l2cap_psm(l2cap->dev, psm); - - if (psm_info) { - /* Device supports this use-case. */ - ch = g_malloc0(sizeof(*ch)); - ch->params.sdu_out = l2cap_bframe_out; - ch->params.sdu_submit = l2cap_bframe_submit; - ch->frame_in = l2cap_bframe_in; - ch->mps = 65536; - ch->min_mtu = MAX(48, psm_info->min_mtu); - ch->params.remote_mtu = MAX(672, ch->min_mtu); - ch->remote_cid = source_cid; - ch->mode = L2CAP_MODE_BASIC; - ch->l2cap = l2cap; - - /* Does it feel like opening yet another channel though? */ - if (!psm_info->new_channel(l2cap->dev, &ch->params)) { - l2cap->cid[cid] = ch; - - result = L2CAP_CR_SUCCESS; - status = L2CAP_CS_NO_INFO; - } else { - g_free(ch); - ch = NULL; - result = L2CAP_CR_NO_MEM; - status = L2CAP_CS_NO_INFO; - } - } else { - result = L2CAP_CR_BAD_PSM; - status = L2CAP_CS_NO_INFO; - } - } else { - result = L2CAP_CR_NO_MEM; - status = L2CAP_CS_NO_INFO; - } - - l2cap_connection_response(l2cap, cid, source_cid, result, status); - - return ch; -} - -static void l2cap_channel_close(struct l2cap_instance_s *l2cap, - int cid, int source_cid) -{ - struct l2cap_chan_s *ch = NULL; - - /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a - * connection in CLOSED state still responds with a L2CAP_DisconnectRsp - * message on an L2CAP_DisconnectReq event. */ - if (unlikely(cid < L2CAP_CID_ALLOC)) { - l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL, - cid, source_cid); - return; - } - if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX)) - ch = l2cap->cid[cid]; - - if (likely(ch)) { - if (ch->remote_cid != source_cid) { - fprintf(stderr, "%s: Ignoring a Disconnection Request with the " - "invalid SCID %04x.\n", __FUNCTION__, source_cid); - return; - } - - l2cap->cid[cid] = NULL; - - ch->params.close(ch->params.opaque); - g_free(ch); - } - - l2cap_disconnection_response(l2cap, cid, source_cid); -} - -static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap, - struct l2cap_chan_s *ch) -{ - l2cap_configuration_request(l2cap, ch->remote_cid, 0, NULL, 0); - ch->config_req_id = l2cap->last_id; - ch->config &= ~L2CAP_CFG_INIT; -} - -static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap, - struct l2cap_chan_s *ch) -{ - /* Use all default channel options and terminate negotiation. */ - l2cap_channel_config_null(l2cap, ch); -} - -static int l2cap_channel_config(struct l2cap_instance_s *l2cap, - struct l2cap_chan_s *ch, int flag, - const uint8_t *data, int len) -{ - l2cap_conf_opt *opt; - l2cap_conf_opt_qos *qos; - uint32_t val; - uint8_t rsp[len]; - int result = L2CAP_CONF_SUCCESS; - - data = memcpy(rsp, data, len); - while (len) { - opt = (void *) data; - - if (len < L2CAP_CONF_OPT_SIZE || - len < L2CAP_CONF_OPT_SIZE + opt->len) { - result = L2CAP_CONF_REJECT; - break; - } - data += L2CAP_CONF_OPT_SIZE + opt->len; - len -= L2CAP_CONF_OPT_SIZE + opt->len; - - switch (opt->type & 0x7f) { - case L2CAP_CONF_MTU: - if (opt->len != 2) { - result = L2CAP_CONF_REJECT; - break; - } - - /* MTU */ - val = le16_to_cpup((void *) opt->val); - if (val < ch->min_mtu) { - cpu_to_le16w((void *) opt->val, ch->min_mtu); - result = L2CAP_CONF_UNACCEPT; - break; - } - - ch->params.remote_mtu = val; - break; - - case L2CAP_CONF_FLUSH_TO: - if (opt->len != 2) { - result = L2CAP_CONF_REJECT; - break; - } - - /* Flush Timeout */ - val = le16_to_cpup((void *) opt->val); - if (val < 0x0001) { - opt->val[0] = 0xff; - opt->val[1] = 0xff; - result = L2CAP_CONF_UNACCEPT; - break; - } - break; - - case L2CAP_CONF_QOS: - if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) { - result = L2CAP_CONF_REJECT; - break; - } - qos = (void *) opt->val; - - /* Flags */ - val = qos->flags; - if (val) { - qos->flags = 0; - result = L2CAP_CONF_UNACCEPT; - } - - /* Service type */ - val = qos->service_type; - if (val != L2CAP_CONF_QOS_BEST_EFFORT && - val != L2CAP_CONF_QOS_NO_TRAFFIC) { - qos->service_type = L2CAP_CONF_QOS_BEST_EFFORT; - result = L2CAP_CONF_UNACCEPT; - } - - if (val != L2CAP_CONF_QOS_NO_TRAFFIC) { - /* XXX: These values should possibly be calculated - * based on LM / baseband properties also. */ - - /* Token rate */ - val = le32_to_cpu(qos->token_rate); - if (val == L2CAP_CONF_QOS_WILDCARD) - qos->token_rate = cpu_to_le32(0x100000); - - /* Token bucket size */ - val = le32_to_cpu(qos->token_bucket_size); - if (val == L2CAP_CONF_QOS_WILDCARD) - qos->token_bucket_size = cpu_to_le32(65500); - - /* Any Peak bandwidth value is correct to return as-is */ - /* Any Access latency value is correct to return as-is */ - /* Any Delay variation value is correct to return as-is */ - } - break; - - case L2CAP_CONF_RFC: - if (opt->len != 9) { - result = L2CAP_CONF_REJECT; - break; - } - - /* Mode */ - val = opt->val[0]; - switch (val) { - case L2CAP_MODE_BASIC: - ch->mode = val; - ch->frame_in = l2cap_bframe_in; - - /* All other parameters shall be ignored */ - break; - - case L2CAP_MODE_RETRANS: - case L2CAP_MODE_FLOWCTL: - ch->mode = val; - ch->frame_in = l2cap_iframe_in; - /* Note: most of these parameters refer to incoming traffic - * so we don't need to save them as long as we can accept - * incoming PDUs at any values of the parameters. */ - - /* TxWindow size */ - val = opt->val[1]; - if (val < 1 || val > 32) { - opt->val[1] = 32; - result = L2CAP_CONF_UNACCEPT; - break; - } - - /* MaxTransmit */ - val = opt->val[2]; - if (val < 1) { - opt->val[2] = 1; - result = L2CAP_CONF_UNACCEPT; - break; - } - - /* Remote Retransmission time-out shouldn't affect local - * operation (?) */ - - /* The Monitor time-out drives the local Monitor timer (?), - * so save the value. */ - val = (opt->val[6] << 8) | opt->val[5]; - if (val < 30) { - opt->val[5] = 100 & 0xff; - opt->val[6] = 100 >> 8; - result = L2CAP_CONF_UNACCEPT; - break; - } - ch->monitor_timeout = val; - l2cap_monitor_timer_update(ch); - - /* MPS */ - val = (opt->val[8] << 8) | opt->val[7]; - if (val < ch->min_mtu) { - opt->val[7] = ch->min_mtu & 0xff; - opt->val[8] = ch->min_mtu >> 8; - result = L2CAP_CONF_UNACCEPT; - break; - } - ch->mps = val; - break; - - default: - result = L2CAP_CONF_UNACCEPT; - break; - } - break; - - default: - if (!(opt->type >> 7)) - result = L2CAP_CONF_UNKNOWN; - break; - } - - if (result != L2CAP_CONF_SUCCESS) - break; /* XXX: should continue? */ - } - - l2cap_configuration_response(l2cap, ch->remote_cid, - flag, result, rsp, len); - - return result == L2CAP_CONF_SUCCESS && !flag; -} - -static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap, - int flag, int cid, const uint8_t *data, int len) -{ - struct l2cap_chan_s *ch; - - if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { - l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL, - cid, 0x0000); - return; - } - ch = l2cap->cid[cid]; - - /* From OPEN go to WAIT_CONFIG_REQ and from WAIT_CONFIG_REQ_RSP to - * WAIT_CONFIG_REQ_RSP. This is assuming the transition chart for OPEN - * on pg 1053, section 6.1.5, volume 3 of BT Core V2.0 has a mistake - * and on options-acceptable we go back to OPEN and otherwise to - * WAIT_CONFIG_REQ and not the other way. */ - ch->config &= ~L2CAP_CFG_ACC; - - if (l2cap_channel_config(l2cap, ch, flag, data, len)) - /* Go to OPEN or WAIT_CONFIG_RSP */ - ch->config |= L2CAP_CFG_ACC; - - /* TODO: if the incoming traffic flow control or retransmission mode - * changed then we probably need to also generate the - * ConfigureChannel_Req event and set the outgoing traffic to the same - * mode. */ - if (!(ch->config & L2CAP_CFG_INIT) && (ch->config & L2CAP_CFG_ACC) && - !ch->config_req_id) - l2cap_channel_config_req_event(l2cap, ch); -} - -static int l2cap_channel_config_rsp_msg(struct l2cap_instance_s *l2cap, - int result, int flag, int cid, const uint8_t *data, int len) -{ - struct l2cap_chan_s *ch; - - if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { - l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL, - cid, 0x0000); - return 0; - } - ch = l2cap->cid[cid]; - - if (ch->config_req_id != l2cap->last_id) - return 1; - ch->config_req_id = 0; - - if (result == L2CAP_CONF_SUCCESS) { - if (!flag) - ch->config |= L2CAP_CFG_INIT; - else - l2cap_channel_config_null(l2cap, ch); - } else - /* Retry until we succeed */ - l2cap_channel_config_req_event(l2cap, ch); - - return 0; -} - -static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap, - int psm, int source_cid) -{ - struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid); - - if (!ch) - return; - - /* Optional */ - if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id) - l2cap_channel_config_req_event(l2cap, ch); -} - -static void l2cap_info(struct l2cap_instance_s *l2cap, int type) -{ - uint8_t data[4]; - int len = 0; - int result = L2CAP_IR_SUCCESS; - - switch (type) { - case L2CAP_IT_CL_MTU: - data[len ++] = l2cap->group_ch.mps & 0xff; - data[len ++] = l2cap->group_ch.mps >> 8; - break; - - case L2CAP_IT_FEAT_MASK: - /* (Prematurely) report Flow control and Retransmission modes. */ - data[len ++] = 0x03; - data[len ++] = 0x00; - data[len ++] = 0x00; - data[len ++] = 0x00; - break; - - default: - result = L2CAP_IR_NOTSUPP; - } - - l2cap_info_response(l2cap, type, result, data, len); -} - -static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id, - const uint8_t *params, int len) -{ - int err; - -#if 0 - /* TODO: do the IDs really have to be in sequence? */ - if (!id || (id != l2cap->last_id && id != l2cap->next_id)) { - fprintf(stderr, "%s: out of sequence command packet ignored.\n", - __FUNCTION__); - return; - } -#else - l2cap->next_id = id; -#endif - if (id == l2cap->next_id) { - l2cap->last_id = l2cap->next_id; - l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1; - } else { - /* TODO: Need to re-send the same response, without re-executing - * the corresponding command! */ - } - - switch (code) { - case L2CAP_COMMAND_REJ: - if (unlikely(len != 2 && len != 4 && len != 6)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - /* We never issue commands other than Command Reject currently. */ - fprintf(stderr, "%s: stray Command Reject (%02x, %04x) " - "packet, ignoring.\n", __FUNCTION__, id, - le16_to_cpu(((l2cap_cmd_rej *) params)->reason)); - break; - - case L2CAP_CONN_REQ: - if (unlikely(len != L2CAP_CONN_REQ_SIZE)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - l2cap_channel_open_req_msg(l2cap, - le16_to_cpu(((l2cap_conn_req *) params)->psm), - le16_to_cpu(((l2cap_conn_req *) params)->scid)); - break; - - case L2CAP_CONN_RSP: - if (unlikely(len != L2CAP_CONN_RSP_SIZE)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - /* We never issue Connection Requests currently. TODO */ - fprintf(stderr, "%s: unexpected Connection Response (%02x) " - "packet, ignoring.\n", __FUNCTION__, id); - break; - - case L2CAP_CONF_REQ: - if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - l2cap_channel_config_req_msg(l2cap, - le16_to_cpu(((l2cap_conf_req *) params)->flags) & 1, - le16_to_cpu(((l2cap_conf_req *) params)->dcid), - ((l2cap_conf_req *) params)->data, - len - L2CAP_CONF_REQ_SIZE(0)); - break; - - case L2CAP_CONF_RSP: - if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - if (l2cap_channel_config_rsp_msg(l2cap, - le16_to_cpu(((l2cap_conf_rsp *) params)->result), - le16_to_cpu(((l2cap_conf_rsp *) params)->flags) & 1, - le16_to_cpu(((l2cap_conf_rsp *) params)->scid), - ((l2cap_conf_rsp *) params)->data, - len - L2CAP_CONF_RSP_SIZE(0))) - fprintf(stderr, "%s: unexpected Configure Response (%02x) " - "packet, ignoring.\n", __FUNCTION__, id); - break; - - case L2CAP_DISCONN_REQ: - if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - l2cap_channel_close(l2cap, - le16_to_cpu(((l2cap_disconn_req *) params)->dcid), - le16_to_cpu(((l2cap_disconn_req *) params)->scid)); - break; - - case L2CAP_DISCONN_RSP: - if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - /* We never issue Disconnection Requests currently. TODO */ - fprintf(stderr, "%s: unexpected Disconnection Response (%02x) " - "packet, ignoring.\n", __FUNCTION__, id); - break; - - case L2CAP_ECHO_REQ: - l2cap_echo_response(l2cap, params, len); - break; - - case L2CAP_ECHO_RSP: - /* We never issue Echo Requests currently. TODO */ - fprintf(stderr, "%s: unexpected Echo Response (%02x) " - "packet, ignoring.\n", __FUNCTION__, id); - break; - - case L2CAP_INFO_REQ: - if (unlikely(len != L2CAP_INFO_REQ_SIZE)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type)); - break; - - case L2CAP_INFO_RSP: - if (unlikely(len != L2CAP_INFO_RSP_SIZE)) { - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - goto reject; - } - - /* We never issue Information Requests currently. TODO */ - fprintf(stderr, "%s: unexpected Information Response (%02x) " - "packet, ignoring.\n", __FUNCTION__, id); - break; - - default: - err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; - reject: - l2cap_command_reject(l2cap, id, err, 0, 0); - break; - } -} - -static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable) -{ - ch->rexmit = enable; - - l2cap_retransmission_timer_update(ch); - l2cap_monitor_timer_update(ch); -} - -/* Command frame SDU */ -static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len) -{ - struct l2cap_instance_s *l2cap = opaque; - const l2cap_cmd_hdr *hdr; - int clen; - - while (len) { - hdr = (void *) data; - if (len < L2CAP_CMD_HDR_SIZE) - /* TODO: signal an error */ - return; - len -= L2CAP_CMD_HDR_SIZE; - data += L2CAP_CMD_HDR_SIZE; - - clen = le16_to_cpu(hdr->len); - if (len < clen) { - l2cap_command_reject(l2cap, hdr->ident, - L2CAP_REJ_CMD_NOT_UNDERSTOOD, 0, 0); - break; - } - - l2cap_command(l2cap, hdr->code, hdr->ident, data, clen); - len -= clen; - data += clen; - } -} - -/* Group frame SDU */ -static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len) -{ -} - -/* Supervisory frame */ -static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl) -{ -} - -/* Basic L2CAP mode Information frame */ -static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid, - const l2cap_hdr *hdr, int len) -{ - /* We have a full SDU, no further processing */ - ch->params.sdu_in(ch->params.opaque, hdr->data, len); -} - -/* Flow Control and Retransmission mode frame */ -static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid, - const l2cap_hdr *hdr, int len) -{ - uint16_t fcs = le16_to_cpup((void *) (hdr->data + len - 2)); - - if (len < 4) - goto len_error; - if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs) - goto fcs_error; - - if ((hdr->data[0] >> 7) == ch->rexmit) - l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7)); - - if (hdr->data[0] & 1) { - if (len != 4) { - /* TODO: Signal an error? */ - return; - } - l2cap_sframe_in(ch, le16_to_cpup((void *) hdr->data)); - return; - } - - switch (hdr->data[1] >> 6) { /* SAR */ - case L2CAP_SAR_NO_SEG: - if (ch->len_total) - goto seg_error; - if (len - 4 > ch->mps) - goto len_error; - - ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4); - break; - - case L2CAP_SAR_START: - if (ch->len_total || len < 6) - goto seg_error; - if (len - 6 > ch->mps) - goto len_error; - - ch->len_total = le16_to_cpup((void *) (hdr->data + 2)); - if (len >= 6 + ch->len_total) - goto seg_error; - - ch->len_cur = len - 6; - memcpy(ch->sdu, hdr->data + 4, ch->len_cur); - break; - - case L2CAP_SAR_END: - if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total) - goto seg_error; - if (len - 4 > ch->mps) - goto len_error; - - memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4); - ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total); - break; - - case L2CAP_SAR_CONT: - if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total) - goto seg_error; - if (len - 4 > ch->mps) - goto len_error; - - memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4); - ch->len_cur += len - 4; - break; - - seg_error: - len_error: /* TODO */ - fcs_error: /* TODO */ - ch->len_cur = 0; - ch->len_total = 0; - break; - } -} - -static void l2cap_frame_in(struct l2cap_instance_s *l2cap, - const l2cap_hdr *frame) -{ - uint16_t cid = le16_to_cpu(frame->cid); - uint16_t len = le16_to_cpu(frame->len); - - if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { - fprintf(stderr, "%s: frame addressed to a non-existent L2CAP " - "channel %04x received.\n", __FUNCTION__, cid); - return; - } - - l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len); -} - -/* "Recombination" */ -static void l2cap_pdu_in(struct l2cap_instance_s *l2cap, - const uint8_t *data, int len) -{ - const l2cap_hdr *hdr = (void *) l2cap->frame_in; - - if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) { - if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) { - memcpy(l2cap->frame_in + l2cap->frame_in_len, data, - sizeof(l2cap->frame_in) - l2cap->frame_in_len); - l2cap->frame_in_len = sizeof(l2cap->frame_in); - /* TODO: truncate */ - l2cap_frame_in(l2cap, hdr); - } - - return; - } - - memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len); - l2cap->frame_in_len += len; - - if (len >= L2CAP_HDR_SIZE) - if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len)) - l2cap_frame_in(l2cap, hdr); - /* There is never a start of a new PDU in the same ACL packet, so - * no need to memmove the remaining payload and loop. */ -} - -static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap, - uint16_t cid, uint16_t len) -{ - l2cap_hdr *hdr = (void *) l2cap->frame_out; - - l2cap->frame_out_len = len + L2CAP_HDR_SIZE; - - hdr->cid = cpu_to_le16(cid); - hdr->len = cpu_to_le16(len); - - return l2cap->frame_out + L2CAP_HDR_SIZE; -} - -static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap) -{ - /* TODO: Fragmentation */ - (l2cap->role ? - l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp) - (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len); -} - -static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len) -{ - struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm; - - if (len > chan->params.remote_mtu) { - fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n", - __FUNCTION__, - chan->remote_cid, chan->params.remote_mtu); - exit(-1); - } - - return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len); -} - -static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms) -{ - struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms; - - l2cap_pdu_submit(chan->l2cap); -} - -#if 0 -/* Stub: Only used if an emulated device requests outgoing flow control */ -static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len) -{ - struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm; - - if (len > chan->params.remote_mtu) { - /* TODO: slice into segments and queue each segment as a separate - * I-Frame in a FIFO of I-Frames, local to the CID. */ - } else { - /* TODO: add to the FIFO of I-Frames, local to the CID. */ - /* Possibly we need to return a pointer to a contiguous buffer - * for now and then memcpy from it into FIFOs in l2cap_iframe_submit - * while segmenting at the same time. */ - } - return 0; -} - -static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm) -{ - /* TODO: If flow control indicates clear to send, start submitting the - * invidual I-Frames from the FIFO, but don't remove them from there. - * Kick the appropriate timer until we get an S-Frame, and only then - * remove from FIFO or resubmit and re-kick the timer if the timer - * expired. */ -} -#endif - -static void l2cap_init(struct l2cap_instance_s *l2cap, - struct bt_link_s *link, int role) -{ - l2cap->link = link; - l2cap->role = role; - l2cap->dev = (struct bt_l2cap_device_s *) - (role ? link->host : link->slave); - - l2cap->next_id = 1; - - /* Establish the signalling channel */ - l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in; - l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out; - l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit; - l2cap->signalling_ch.params.opaque = l2cap; - l2cap->signalling_ch.params.remote_mtu = 48; - l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING; - l2cap->signalling_ch.frame_in = l2cap_bframe_in; - l2cap->signalling_ch.mps = 65536; - l2cap->signalling_ch.min_mtu = 48; - l2cap->signalling_ch.mode = L2CAP_MODE_BASIC; - l2cap->signalling_ch.l2cap = l2cap; - l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch; - - /* Establish the connection-less data channel */ - l2cap->group_ch.params.sdu_in = l2cap_gframe_in; - l2cap->group_ch.params.opaque = l2cap; - l2cap->group_ch.frame_in = l2cap_bframe_in; - l2cap->group_ch.mps = 65533; - l2cap->group_ch.l2cap = l2cap; - l2cap->group_ch.remote_cid = L2CAP_CID_INVALID; - l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch; -} - -static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect) -{ - int cid; - - /* Don't send DISCONNECT if we are currently handling a DISCONNECT - * sent from the other side. */ - if (send_disconnect) { - if (l2cap->role) - l2cap->dev->device.lmp_disconnect_slave(l2cap->link); - /* l2cap->link is invalid from now on. */ - else - l2cap->dev->device.lmp_disconnect_master(l2cap->link); - } - - for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++) - if (l2cap->cid[cid]) { - l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque); - g_free(l2cap->cid[cid]); - } - - if (l2cap->role) - g_free(l2cap); - else - g_free(l2cap->link); -} - -/* L2CAP glue to lower layers in bluetooth stack (LMP) */ - -static void l2cap_lmp_connection_request(struct bt_link_s *link) -{ - struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave; - struct slave_l2cap_instance_s *l2cap; - - /* Always accept - we only get called if (dev->device->page_scan). */ - - l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s)); - l2cap->link.slave = &dev->device; - l2cap->link.host = link->host; - l2cap_init(&l2cap->l2cap, &l2cap->link, 0); - - /* Always at the end */ - link->host->reject_reason = 0; - link->host->lmp_connection_complete(&l2cap->link); -} - -/* Stub */ -static void l2cap_lmp_connection_complete(struct bt_link_s *link) -{ - struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; - struct l2cap_instance_s *l2cap; - - if (dev->device.reject_reason) { - /* Signal to upper layer */ - return; - } - - l2cap = g_malloc0(sizeof(struct l2cap_instance_s)); - l2cap_init(l2cap, link, 1); - - link->acl_mode = acl_active; - - /* Signal to upper layer */ -} - -/* Stub */ -static void l2cap_lmp_disconnect_host(struct bt_link_s *link) -{ - struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; - struct l2cap_instance_s *l2cap = - /* TODO: Retrieve from upper layer */ (void *) dev; - - /* Signal to upper layer */ - - l2cap_teardown(l2cap, 0); -} - -static void l2cap_lmp_disconnect_slave(struct bt_link_s *link) -{ - struct slave_l2cap_instance_s *l2cap = - (struct slave_l2cap_instance_s *) link; - - l2cap_teardown(&l2cap->l2cap, 0); -} - -static void l2cap_lmp_acl_data_slave(struct bt_link_s *link, - const uint8_t *data, int start, int len) -{ - struct slave_l2cap_instance_s *l2cap = - (struct slave_l2cap_instance_s *) link; - - if (start) - l2cap->l2cap.frame_in_len = 0; - - l2cap_pdu_in(&l2cap->l2cap, data, len); -} - -/* Stub */ -static void l2cap_lmp_acl_data_host(struct bt_link_s *link, - const uint8_t *data, int start, int len) -{ - struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; - struct l2cap_instance_s *l2cap = - /* TODO: Retrieve from upper layer */ (void *) dev; - - if (start) - l2cap->frame_in_len = 0; - - l2cap_pdu_in(l2cap, data, len); -} - -static void l2cap_dummy_destroy(struct bt_device_s *dev) -{ - struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev; - - bt_l2cap_device_done(l2cap_dev); -} - -void bt_l2cap_device_init(struct bt_l2cap_device_s *dev, - struct bt_scatternet_s *net) -{ - bt_device_init(&dev->device, net); - - dev->device.lmp_connection_request = l2cap_lmp_connection_request; - dev->device.lmp_connection_complete = l2cap_lmp_connection_complete; - dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host; - dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave; - dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave; - dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host; - - dev->device.handle_destroy = l2cap_dummy_destroy; -} - -void bt_l2cap_device_done(struct bt_l2cap_device_s *dev) -{ - bt_device_done(&dev->device); - - /* Should keep a list of all instances and go through it and - * invoke l2cap_teardown() for each. */ -} - -void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu, - int (*new_channel)(struct bt_l2cap_device_s *dev, - struct bt_l2cap_conn_params_s *params)) -{ - struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm); - - if (new_psm) { - fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n", - __FUNCTION__, psm, dev->device.lmp_name); - exit(-1); - } - - new_psm = g_malloc0(sizeof(*new_psm)); - new_psm->psm = psm; - new_psm->min_mtu = min_mtu; - new_psm->new_channel = new_channel; - new_psm->next = dev->first_psm; - dev->first_psm = new_psm; -} diff --git a/qemu/hw/bt/sdp.c b/qemu/hw/bt/sdp.c deleted file mode 100644 index be26009b0..000000000 --- a/qemu/hw/bt/sdp.c +++ /dev/null @@ -1,979 +0,0 @@ -/* - * Service Discover Protocol server for QEMU L2CAP devices - * - * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.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, see <http://www.gnu.org/licenses/>. - */ - -#include "qemu/osdep.h" -#include "qemu-common.h" -#include "hw/bt.h" - -struct bt_l2cap_sdp_state_s { - struct bt_l2cap_conn_params_s *channel; - - struct sdp_service_record_s { - int match; - - int *uuid; - int uuids; - struct sdp_service_attribute_s { - int match; - - int attribute_id; - int len; - void *pair; - } *attribute_list; - int attributes; - } *service_list; - int services; -}; - -static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left) -{ - uint32_t len = *(*element) ++ & SDP_DSIZE_MASK; - - if (!*left) - return -1; - (*left) --; - - if (len < SDP_DSIZE_NEXT1) - return 1 << len; - else if (len == SDP_DSIZE_NEXT1) { - if (*left < 1) - return -1; - (*left) --; - - return *(*element) ++; - } else if (len == SDP_DSIZE_NEXT2) { - if (*left < 2) - return -1; - (*left) -= 2; - - len = (*(*element) ++) << 8; - return len | (*(*element) ++); - } else { - if (*left < 4) - return -1; - (*left) -= 4; - - len = (*(*element) ++) << 24; - len |= (*(*element) ++) << 16; - len |= (*(*element) ++) << 8; - return len | (*(*element) ++); - } -} - -static const uint8_t bt_base_uuid[12] = { - 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb, -}; - -static int sdp_uuid_match(struct sdp_service_record_s *record, - const uint8_t *uuid, ssize_t datalen) -{ - int *lo, hi, val; - - if (datalen == 16 || datalen == 4) { - if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12)) - return 0; - - if (uuid[0] | uuid[1]) - return 0; - uuid += 2; - } - - val = (uuid[0] << 8) | uuid[1]; - lo = record->uuid; - hi = record->uuids; - while (hi >>= 1) - if (lo[hi] <= val) - lo += hi; - - return *lo == val; -} - -#define CONTINUATION_PARAM_SIZE (1 + sizeof(int)) -#define MAX_PDU_OUT_SIZE 96 /* Arbitrary */ -#define PDU_HEADER_SIZE 5 -#define MAX_RSP_PARAM_SIZE (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \ - CONTINUATION_PARAM_SIZE) - -static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp, - const uint8_t **req, ssize_t *len) -{ - size_t datalen; - int i; - - if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID) - return 1; - - datalen = sdp_datalen(req, len); - if (datalen != 2 && datalen != 4 && datalen != 16) - return 1; - - for (i = 0; i < sdp->services; i ++) - if (sdp_uuid_match(&sdp->service_list[i], *req, datalen)) - sdp->service_list[i].match = 1; - - (*req) += datalen; - (*len) -= datalen; - - return 0; -} - -static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp, - uint8_t *rsp, const uint8_t *req, ssize_t len) -{ - ssize_t seqlen; - int i, count, start, end, max; - int32_t handle; - - /* Perform the search */ - for (i = 0; i < sdp->services; i ++) - sdp->service_list[i].match = 0; - - if (len < 1) - return -SDP_INVALID_SYNTAX; - if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { - seqlen = sdp_datalen(&req, &len); - if (seqlen < 3 || len < seqlen) - return -SDP_INVALID_SYNTAX; - len -= seqlen; - while (seqlen) - if (sdp_svc_match(sdp, &req, &seqlen)) - return -SDP_INVALID_SYNTAX; - } else { - if (sdp_svc_match(sdp, &req, &len)) { - return -SDP_INVALID_SYNTAX; - } - } - - if (len < 3) - return -SDP_INVALID_SYNTAX; - max = (req[0] << 8) | req[1]; - req += 2; - len -= 2; - - if (*req) { - if (len <= sizeof(int)) - return -SDP_INVALID_SYNTAX; - len -= sizeof(int); - memcpy(&start, req + 1, sizeof(int)); - } else - start = 0; - - if (len > 1) - return -SDP_INVALID_SYNTAX; - - /* Output the results */ - len = 4; - count = 0; - end = start; - for (i = 0; i < sdp->services; i ++) - if (sdp->service_list[i].match) { - if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) { - handle = i; - memcpy(rsp + len, &handle, 4); - len += 4; - end = count + 1; - } - - count ++; - } - - rsp[0] = count >> 8; - rsp[1] = count & 0xff; - rsp[2] = (end - start) >> 8; - rsp[3] = (end - start) & 0xff; - - if (end < count) { - rsp[len ++] = sizeof(int); - memcpy(rsp + len, &end, sizeof(int)); - len += 4; - } else - rsp[len ++] = 0; - - return len; -} - -static int sdp_attr_match(struct sdp_service_record_s *record, - const uint8_t **req, ssize_t *len) -{ - int i, start, end; - - if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) { - (*req) ++; - if (*len < 3) - return 1; - - start = (*(*req) ++) << 8; - start |= *(*req) ++; - end = start; - *len -= 3; - } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) { - (*req) ++; - if (*len < 5) - return 1; - - start = (*(*req) ++) << 8; - start |= *(*req) ++; - end = (*(*req) ++) << 8; - end |= *(*req) ++; - *len -= 5; - } else - return 1; - - for (i = 0; i < record->attributes; i ++) - if (record->attribute_list[i].attribute_id >= start && - record->attribute_list[i].attribute_id <= end) - record->attribute_list[i].match = 1; - - return 0; -} - -static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp, - uint8_t *rsp, const uint8_t *req, ssize_t len) -{ - ssize_t seqlen; - int i, start, end, max; - int32_t handle; - struct sdp_service_record_s *record; - uint8_t *lst; - - /* Perform the search */ - if (len < 7) - return -SDP_INVALID_SYNTAX; - memcpy(&handle, req, 4); - req += 4; - len -= 4; - - if (handle < 0 || handle > sdp->services) - return -SDP_INVALID_RECORD_HANDLE; - record = &sdp->service_list[handle]; - - for (i = 0; i < record->attributes; i ++) - record->attribute_list[i].match = 0; - - max = (req[0] << 8) | req[1]; - req += 2; - len -= 2; - if (max < 0x0007) - return -SDP_INVALID_SYNTAX; - - if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { - seqlen = sdp_datalen(&req, &len); - if (seqlen < 3 || len < seqlen) - return -SDP_INVALID_SYNTAX; - len -= seqlen; - - while (seqlen) - if (sdp_attr_match(record, &req, &seqlen)) - return -SDP_INVALID_SYNTAX; - } else { - if (sdp_attr_match(record, &req, &len)) { - return -SDP_INVALID_SYNTAX; - } - } - - if (len < 1) - return -SDP_INVALID_SYNTAX; - - if (*req) { - if (len <= sizeof(int)) - return -SDP_INVALID_SYNTAX; - len -= sizeof(int); - memcpy(&start, req + 1, sizeof(int)); - } else - start = 0; - - if (len > 1) - return -SDP_INVALID_SYNTAX; - - /* Output the results */ - lst = rsp + 2; - max = MIN(max, MAX_RSP_PARAM_SIZE); - len = 3 - start; - end = 0; - for (i = 0; i < record->attributes; i ++) - if (record->attribute_list[i].match) { - if (len >= 0 && len + record->attribute_list[i].len < max) { - memcpy(lst + len, record->attribute_list[i].pair, - record->attribute_list[i].len); - end = len + record->attribute_list[i].len; - } - len += record->attribute_list[i].len; - } - if (0 >= start) { - lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2; - lst[1] = (len + start - 3) >> 8; - lst[2] = (len + start - 3) & 0xff; - } - - rsp[0] = end >> 8; - rsp[1] = end & 0xff; - - if (end < len) { - len = end + start; - lst[end ++] = sizeof(int); - memcpy(lst + end, &len, sizeof(int)); - end += sizeof(int); - } else - lst[end ++] = 0; - - return end + 2; -} - -static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp, - const uint8_t **req, ssize_t *len) -{ - int i, j, start, end; - struct sdp_service_record_s *record; - - if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) { - (*req) ++; - if (*len < 3) - return 1; - - start = (*(*req) ++) << 8; - start |= *(*req) ++; - end = start; - *len -= 3; - } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) { - (*req) ++; - if (*len < 5) - return 1; - - start = (*(*req) ++) << 8; - start |= *(*req) ++; - end = (*(*req) ++) << 8; - end |= *(*req) ++; - *len -= 5; - } else - return 1; - - for (i = 0; i < sdp->services; i ++) - if ((record = &sdp->service_list[i])->match) - for (j = 0; j < record->attributes; j ++) - if (record->attribute_list[j].attribute_id >= start && - record->attribute_list[j].attribute_id <= end) - record->attribute_list[j].match = 1; - - return 0; -} - -static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp, - uint8_t *rsp, const uint8_t *req, ssize_t len) -{ - ssize_t seqlen; - int i, j, start, end, max; - struct sdp_service_record_s *record; - uint8_t *lst; - - /* Perform the search */ - for (i = 0; i < sdp->services; i ++) { - sdp->service_list[i].match = 0; - for (j = 0; j < sdp->service_list[i].attributes; j ++) - sdp->service_list[i].attribute_list[j].match = 0; - } - - if (len < 1) - return -SDP_INVALID_SYNTAX; - if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { - seqlen = sdp_datalen(&req, &len); - if (seqlen < 3 || len < seqlen) - return -SDP_INVALID_SYNTAX; - len -= seqlen; - - while (seqlen) - if (sdp_svc_match(sdp, &req, &seqlen)) - return -SDP_INVALID_SYNTAX; - } else { - if (sdp_svc_match(sdp, &req, &len)) { - return -SDP_INVALID_SYNTAX; - } - } - - if (len < 3) - return -SDP_INVALID_SYNTAX; - max = (req[0] << 8) | req[1]; - req += 2; - len -= 2; - if (max < 0x0007) - return -SDP_INVALID_SYNTAX; - - if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { - seqlen = sdp_datalen(&req, &len); - if (seqlen < 3 || len < seqlen) - return -SDP_INVALID_SYNTAX; - len -= seqlen; - - while (seqlen) - if (sdp_svc_attr_match(sdp, &req, &seqlen)) - return -SDP_INVALID_SYNTAX; - } else { - if (sdp_svc_attr_match(sdp, &req, &len)) { - return -SDP_INVALID_SYNTAX; - } - } - - if (len < 1) - return -SDP_INVALID_SYNTAX; - - if (*req) { - if (len <= sizeof(int)) - return -SDP_INVALID_SYNTAX; - len -= sizeof(int); - memcpy(&start, req + 1, sizeof(int)); - } else - start = 0; - - if (len > 1) - return -SDP_INVALID_SYNTAX; - - /* Output the results */ - /* This assumes empty attribute lists are never to be returned even - * for matching Service Records. In practice this shouldn't happen - * as the requestor will usually include the always present - * ServiceRecordHandle AttributeID in AttributeIDList. */ - lst = rsp + 2; - max = MIN(max, MAX_RSP_PARAM_SIZE); - len = 3 - start; - end = 0; - for (i = 0; i < sdp->services; i ++) - if ((record = &sdp->service_list[i])->match) { - len += 3; - seqlen = len; - for (j = 0; j < record->attributes; j ++) - if (record->attribute_list[j].match) { - if (len >= 0) - if (len + record->attribute_list[j].len < max) { - memcpy(lst + len, record->attribute_list[j].pair, - record->attribute_list[j].len); - end = len + record->attribute_list[j].len; - } - len += record->attribute_list[j].len; - } - if (seqlen == len) - len -= 3; - else if (seqlen >= 3 && seqlen < max) { - lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2; - lst[seqlen - 2] = (len - seqlen) >> 8; - lst[seqlen - 1] = (len - seqlen) & 0xff; - } - } - if (len == 3 - start) - len -= 3; - else if (0 >= start) { - lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2; - lst[1] = (len + start - 3) >> 8; - lst[2] = (len + start - 3) & 0xff; - } - - rsp[0] = end >> 8; - rsp[1] = end & 0xff; - - if (end < len) { - len = end + start; - lst[end ++] = sizeof(int); - memcpy(lst + end, &len, sizeof(int)); - end += sizeof(int); - } else - lst[end ++] = 0; - - return end + 2; -} - -static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len) -{ - struct bt_l2cap_sdp_state_s *sdp = opaque; - enum bt_sdp_cmd pdu_id; - uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out; - int transaction_id, plen; - int err = 0; - int rsp_len = 0; - - if (len < 5) { - fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len); - return; - } - - pdu_id = *data ++; - transaction_id = (data[0] << 8) | data[1]; - plen = (data[2] << 8) | data[3]; - data += 4; - len -= 5; - - if (len != plen) { - fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n", - __FUNCTION__, plen, len); - err = SDP_INVALID_PDU_SIZE; - goto respond; - } - - switch (pdu_id) { - case SDP_SVC_SEARCH_REQ: - rsp_len = sdp_svc_search(sdp, rsp, data, len); - pdu_id = SDP_SVC_SEARCH_RSP; - break; - - case SDP_SVC_ATTR_REQ: - rsp_len = sdp_attr_get(sdp, rsp, data, len); - pdu_id = SDP_SVC_ATTR_RSP; - break; - - case SDP_SVC_SEARCH_ATTR_REQ: - rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len); - pdu_id = SDP_SVC_SEARCH_ATTR_RSP; - break; - - case SDP_ERROR_RSP: - case SDP_SVC_ATTR_RSP: - case SDP_SVC_SEARCH_RSP: - case SDP_SVC_SEARCH_ATTR_RSP: - default: - fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n", - __FUNCTION__, pdu_id); - err = SDP_INVALID_SYNTAX; - break; - } - - if (rsp_len < 0) { - err = -rsp_len; - rsp_len = 0; - } - -respond: - if (err) { - pdu_id = SDP_ERROR_RSP; - rsp[rsp_len ++] = err >> 8; - rsp[rsp_len ++] = err & 0xff; - } - - sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE); - - sdu_out[0] = pdu_id; - sdu_out[1] = transaction_id >> 8; - sdu_out[2] = transaction_id & 0xff; - sdu_out[3] = rsp_len >> 8; - sdu_out[4] = rsp_len & 0xff; - memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len); - - sdp->channel->sdu_submit(sdp->channel); -} - -static void bt_l2cap_sdp_close_ch(void *opaque) -{ - struct bt_l2cap_sdp_state_s *sdp = opaque; - int i; - - for (i = 0; i < sdp->services; i ++) { - g_free(sdp->service_list[i].attribute_list->pair); - g_free(sdp->service_list[i].attribute_list); - g_free(sdp->service_list[i].uuid); - } - g_free(sdp->service_list); - g_free(sdp); -} - -struct sdp_def_service_s { - uint16_t class_uuid; - struct sdp_def_attribute_s { - uint16_t id; - struct sdp_def_data_element_s { - uint8_t type; - union { - uint32_t uint; - const char *str; - struct sdp_def_data_element_s *list; - } value; - } data; - } attributes[]; -}; - -/* Calculate a safe byte count to allocate that will store the given - * element, at the same time count elements of a UUID type. */ -static int sdp_attr_max_size(struct sdp_def_data_element_s *element, - int *uuids) -{ - int type = element->type & ~SDP_DSIZE_MASK; - int len; - - if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID || - type == SDP_DTYPE_BOOL) { - if (type == SDP_DTYPE_UUID) - (*uuids) ++; - return 1 + (1 << (element->type & SDP_DSIZE_MASK)); - } - - if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) { - if (element->type & SDP_DSIZE_MASK) { - for (len = 0; element->value.str[len] | - element->value.str[len + 1]; len ++); - return len; - } else - return 2 + strlen(element->value.str); - } - - if (type != SDP_DTYPE_SEQ) - exit(-1); - len = 2; - element = element->value.list; - while (element->type) - len += sdp_attr_max_size(element ++, uuids); - if (len > 255) - exit (-1); - - return len; -} - -static int sdp_attr_write(uint8_t *data, - struct sdp_def_data_element_s *element, int **uuid) -{ - int type = element->type & ~SDP_DSIZE_MASK; - int len = 0; - - if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) { - data[len ++] = element->type; - if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1) - data[len ++] = (element->value.uint >> 0) & 0xff; - else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) { - data[len ++] = (element->value.uint >> 8) & 0xff; - data[len ++] = (element->value.uint >> 0) & 0xff; - } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) { - data[len ++] = (element->value.uint >> 24) & 0xff; - data[len ++] = (element->value.uint >> 16) & 0xff; - data[len ++] = (element->value.uint >> 8) & 0xff; - data[len ++] = (element->value.uint >> 0) & 0xff; - } - - return len; - } - - if (type == SDP_DTYPE_UUID) { - *(*uuid) ++ = element->value.uint; - - data[len ++] = element->type; - data[len ++] = (element->value.uint >> 24) & 0xff; - data[len ++] = (element->value.uint >> 16) & 0xff; - data[len ++] = (element->value.uint >> 8) & 0xff; - data[len ++] = (element->value.uint >> 0) & 0xff; - memcpy(data + len, bt_base_uuid, 12); - - return len + 12; - } - - data[0] = type | SDP_DSIZE_NEXT1; - if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) { - if (element->type & SDP_DSIZE_MASK) - for (len = 0; element->value.str[len] | - element->value.str[len + 1]; len ++); - else - len = strlen(element->value.str); - memcpy(data + 2, element->value.str, data[1] = len); - - return len + 2; - } - - len = 2; - element = element->value.list; - while (element->type) - len += sdp_attr_write(data + len, element ++, uuid); - data[1] = len - 2; - - return len; -} - -static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a, - const struct sdp_service_attribute_s *b) -{ - return (int) b->attribute_id - a->attribute_id; -} - -static int sdp_uuid_compare(const int *a, const int *b) -{ - return *a - *b; -} - -static void sdp_service_record_build(struct sdp_service_record_s *record, - struct sdp_def_service_s *def, int handle) -{ - int len = 0; - uint8_t *data; - int *uuid; - - record->uuids = 0; - while (def->attributes[record->attributes].data.type) { - len += 3; - len += sdp_attr_max_size(&def->attributes[record->attributes ++].data, - &record->uuids); - } - record->uuids = pow2ceil(record->uuids); - record->attribute_list = - g_malloc0(record->attributes * sizeof(*record->attribute_list)); - record->uuid = - g_malloc0(record->uuids * sizeof(*record->uuid)); - data = g_malloc(len); - - record->attributes = 0; - uuid = record->uuid; - while (def->attributes[record->attributes].data.type) { - record->attribute_list[record->attributes].pair = data; - - len = 0; - data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2; - data[len ++] = def->attributes[record->attributes].id >> 8; - data[len ++] = def->attributes[record->attributes].id & 0xff; - len += sdp_attr_write(data + len, - &def->attributes[record->attributes].data, &uuid); - - /* Special case: assign a ServiceRecordHandle in sequence */ - if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE) - def->attributes[record->attributes].data.value.uint = handle; - /* Note: we could also assign a ServiceDescription based on - * sdp->device.device->lmp_name. */ - - record->attribute_list[record->attributes ++].len = len; - data += len; - } - - /* Sort the attribute list by the AttributeID */ - qsort(record->attribute_list, record->attributes, - sizeof(*record->attribute_list), - (void *) sdp_attributeid_compare); - /* Sort the searchable UUIDs list for bisection */ - qsort(record->uuid, record->uuids, - sizeof(*record->uuid), - (void *) sdp_uuid_compare); -} - -static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp, - struct sdp_def_service_s **service) -{ - sdp->services = 0; - while (service[sdp->services]) - sdp->services ++; - sdp->service_list = - g_malloc0(sdp->services * sizeof(*sdp->service_list)); - - sdp->services = 0; - while (*service) { - sdp_service_record_build(&sdp->service_list[sdp->services], - *service, sdp->services); - service ++; - sdp->services ++; - } -} - -#define LAST { .type = 0 } -#define SERVICE(name, attrs) \ - static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \ - .attributes = { attrs { .data = LAST } }, \ - }; -#define ATTRIBUTE(attrid, val) { .id = glue(SDP_ATTR_, attrid), .data = val }, -#define UINT8(val) { \ - .type = SDP_DTYPE_UINT | SDP_DSIZE_1, \ - .value.uint = val, \ - }, -#define UINT16(val) { \ - .type = SDP_DTYPE_UINT | SDP_DSIZE_2, \ - .value.uint = val, \ - }, -#define UINT32(val) { \ - .type = SDP_DTYPE_UINT | SDP_DSIZE_4, \ - .value.uint = val, \ - }, -#define UUID128(val) { \ - .type = SDP_DTYPE_UUID | SDP_DSIZE_16, \ - .value.uint = val, \ - }, -#define SDP_TRUE { \ - .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ - .value.uint = 1, \ - }, -#define SDP_FALSE { \ - .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ - .value.uint = 0, \ - }, -#define STRING(val) { \ - .type = SDP_DTYPE_STRING, \ - .value.str = val, \ - }, -#define ARRAY(...) { \ - .type = SDP_DTYPE_STRING | SDP_DSIZE_2, \ - .value.str = (char []) { __VA_ARGS__, 0, 0 }, \ - }, -#define URL(val) { \ - .type = SDP_DTYPE_URL, \ - .value.str = val, \ - }, -#if 1 -#define LIST(val) { \ - .type = SDP_DTYPE_SEQ, \ - .value.list = (struct sdp_def_data_element_s []) { val LAST }, \ - }, -#endif - -/* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes - * in resulting SDP data representation size. */ - -SERVICE(hid, - ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ - ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID))) - ATTRIBUTE(RECORD_STATE, UINT32(1)) - ATTRIBUTE(PROTO_DESC_LIST, LIST( - LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL)) - LIST(UUID128(HIDP_UUID)) - )) - ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) - ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( - UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) - )) - ATTRIBUTE(PFILE_DESC_LIST, LIST( - LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100)) - )) - ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html")) - ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID")) - ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse")) - ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU")) - - /* Profile specific */ - ATTRIBUTE(DEVICE_RELEASE_NUMBER, UINT16(0x0091)) /* Deprecated, remove */ - ATTRIBUTE(PARSER_VERSION, UINT16(0x0111)) - /* TODO: extract from l2cap_device->device.class[0] */ - ATTRIBUTE(DEVICE_SUBCLASS, UINT8(0x40)) - ATTRIBUTE(COUNTRY_CODE, UINT8(0x15)) - ATTRIBUTE(VIRTUAL_CABLE, SDP_TRUE) - ATTRIBUTE(RECONNECT_INITIATE, SDP_FALSE) - /* TODO: extract from hid->usbdev->report_desc */ - ATTRIBUTE(DESCRIPTOR_LIST, LIST( - LIST(UINT8(0x22) ARRAY( - 0x05, 0x01, /* Usage Page (Generic Desktop) */ - 0x09, 0x06, /* Usage (Keyboard) */ - 0xa1, 0x01, /* Collection (Application) */ - 0x75, 0x01, /* Report Size (1) */ - 0x95, 0x08, /* Report Count (8) */ - 0x05, 0x07, /* Usage Page (Key Codes) */ - 0x19, 0xe0, /* Usage Minimum (224) */ - 0x29, 0xe7, /* Usage Maximum (231) */ - 0x15, 0x00, /* Logical Minimum (0) */ - 0x25, 0x01, /* Logical Maximum (1) */ - 0x81, 0x02, /* Input (Data, Variable, Absolute) */ - 0x95, 0x01, /* Report Count (1) */ - 0x75, 0x08, /* Report Size (8) */ - 0x81, 0x01, /* Input (Constant) */ - 0x95, 0x05, /* Report Count (5) */ - 0x75, 0x01, /* Report Size (1) */ - 0x05, 0x08, /* Usage Page (LEDs) */ - 0x19, 0x01, /* Usage Minimum (1) */ - 0x29, 0x05, /* Usage Maximum (5) */ - 0x91, 0x02, /* Output (Data, Variable, Absolute) */ - 0x95, 0x01, /* Report Count (1) */ - 0x75, 0x03, /* Report Size (3) */ - 0x91, 0x01, /* Output (Constant) */ - 0x95, 0x06, /* Report Count (6) */ - 0x75, 0x08, /* Report Size (8) */ - 0x15, 0x00, /* Logical Minimum (0) */ - 0x25, 0xff, /* Logical Maximum (255) */ - 0x05, 0x07, /* Usage Page (Key Codes) */ - 0x19, 0x00, /* Usage Minimum (0) */ - 0x29, 0xff, /* Usage Maximum (255) */ - 0x81, 0x00, /* Input (Data, Array) */ - 0xc0 /* End Collection */ - )))) - ATTRIBUTE(LANG_ID_BASE_LIST, LIST( - LIST(UINT16(0x0409) UINT16(0x0100)) - )) - ATTRIBUTE(SDP_DISABLE, SDP_FALSE) - ATTRIBUTE(BATTERY_POWER, SDP_TRUE) - ATTRIBUTE(REMOTE_WAKEUP, SDP_TRUE) - ATTRIBUTE(BOOT_DEVICE, SDP_TRUE) /* XXX: untested */ - ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80)) - ATTRIBUTE(NORMALLY_CONNECTABLE, SDP_TRUE) - ATTRIBUTE(PROFILE_VERSION, UINT16(0x0100)) -) - -SERVICE(sdp, - ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ - ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID))) - ATTRIBUTE(RECORD_STATE, UINT32(1)) - ATTRIBUTE(PROTO_DESC_LIST, LIST( - LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) - LIST(UUID128(SDP_UUID)) - )) - ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) - ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( - UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) - )) - ATTRIBUTE(PFILE_DESC_LIST, LIST( - LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100)) - )) - ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html")) - ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU")) - - /* Profile specific */ - ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100))) - ATTRIBUTE(SVCDB_STATE , UINT32(1)) -) - -SERVICE(pnp, - ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ - ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID))) - ATTRIBUTE(RECORD_STATE, UINT32(1)) - ATTRIBUTE(PROTO_DESC_LIST, LIST( - LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) - LIST(UUID128(SDP_UUID)) - )) - ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) - ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( - UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) - )) - ATTRIBUTE(PFILE_DESC_LIST, LIST( - LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100)) - )) - ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html")) - ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU")) - - /* Profile specific */ - ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100)) - ATTRIBUTE(VERSION, UINT16(0x0100)) - ATTRIBUTE(PRIMARY_RECORD, SDP_TRUE) -) - -static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev, - struct bt_l2cap_conn_params_s *params) -{ - struct bt_l2cap_sdp_state_s *sdp = g_malloc0(sizeof(*sdp)); - struct sdp_def_service_s *services[] = { - &sdp_service_sdp_s, - &sdp_service_hid_s, - &sdp_service_pnp_s, - NULL, - }; - - sdp->channel = params; - sdp->channel->opaque = sdp; - sdp->channel->close = bt_l2cap_sdp_close_ch; - sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in; - - sdp_service_db_build(sdp, services); - - return 0; -} - -void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev) -{ - bt_l2cap_psm_register(dev, BT_PSM_SDP, - MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch); -} |