gwj / at91sam9263 · Commits

Commit 4d2d181cdb8936b55712a81be798c288fa8b5f59

Authored by balrog
1 parent 4e38eb54

Add L2CAP logic and a virtual SDP server for use in emulated devices. 

Note that the L2CAP flow-controlled mode is not fully supported.


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@5346 c046a42c-6fe2-441c-8c8c-71466251a162
Inline Side-by-side
Showing 4 changed files with 2886 additions and 1 deletions
Makefile
  View file @4d2d181
@@ -81,7 +81,7 @@ OBJS+=scsi-generic.o @@ -81,7 +81,7 @@ OBJS+=scsi-generic.o
81 OBJS+=usb.o usb-hub.o usb-linux.o usb-hid.o usb-msd.o usb-wacom.o 81 OBJS+=usb.o usb-hub.o usb-linux.o usb-hid.o usb-msd.o usb-wacom.o
82 OBJS+=usb-serial.o usb-net.o 82 OBJS+=usb-serial.o usb-net.o
83 OBJS+=sd.o ssi-sd.o 83 OBJS+=sd.o ssi-sd.o
84 -OBJS+=bt.o bt-host.o bt-hci.o 84 +OBJS+=bt.o bt-host.o bt-l2cap.o bt-sdp.o bt-hci.o
85 85
86 ifdef CONFIG_BRLAPI 86 ifdef CONFIG_BRLAPI
87 OBJS+= baum.o 87 OBJS+= baum.o
hw/bt-l2cap.c 0 → 100644
  View file @4d2d181
  1 +/*
  2 + * QEMU Bluetooth L2CAP logic.
  3 + *
  4 + * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
  5 + *
  6 + * This program is free software; you can redistribute it and/or
  7 + * modify it under the terms of the GNU General Public License as
  8 + * published by the Free Software Foundation; either version 2 of
  9 + * the License, or (at your option) any later version.
  10 + *
  11 + * This program is distributed in the hope that it will be useful,
  12 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14 + * GNU General Public License for more details.
  15 + *
  16 + * You should have received a copy of the GNU General Public License
  17 + * along with this program; if not, write to the Free Software
  18 + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
  19 + * MA 02110-1301 USA
  20 + */
  21 +
  22 +#include "qemu-common.h"
  23 +#include "qemu-timer.h"
  24 +#include "bt.h"
  25 +
  26 +#define L2CAP_CID_MAX 0x100 /* Between 0x40 and 0x10000 */
  27 +
  28 +struct l2cap_instance_s {
  29 + struct bt_link_s *link;
  30 + struct bt_l2cap_device_s *dev;
  31 + int role;
  32 +
  33 + uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
  34 + int frame_in_len;
  35 +
  36 + uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
  37 + int frame_out_len;
  38 +
  39 + /* Signalling channel timers. They exist per-request but we can make
  40 + * sure we have no more than one outstanding request at any time. */
  41 + QEMUTimer *rtx;
  42 + QEMUTimer *ertx;
  43 +
  44 + int last_id;
  45 + int next_id;
  46 +
  47 + struct l2cap_chan_s {
  48 + struct bt_l2cap_conn_params_s params;
  49 +
  50 + void (*frame_in)(struct l2cap_chan_s *chan, uint16_t cid,
  51 + const l2cap_hdr *hdr, int len);
  52 + int mps;
  53 + int min_mtu;
  54 +
  55 + struct l2cap_instance_s *l2cap;
  56 +
  57 + /* Only allocated channels */
  58 + uint16_t remote_cid;
  59 +#define L2CAP_CFG_INIT 2
  60 +#define L2CAP_CFG_ACC 1
  61 + int config_req_id; /* TODO: handle outgoing requests generically */
  62 + int config;
  63 +
  64 + /* Only connection-oriented channels. Note: if we allow the tx and
  65 + * rx traffic to be in different modes at any time, we need two. */
  66 + int mode;
  67 +
  68 + /* Only flow-controlled, connection-oriented channels */
  69 + uint8_t sdu[65536]; /* TODO: dynamically allocate */
  70 + int len_cur, len_total;
  71 + int rexmit;
  72 + int monitor_timeout;
  73 + QEMUTimer *monitor_timer;
  74 + QEMUTimer *retransmission_timer;
  75 + } *cid[L2CAP_CID_MAX];
  76 + /* The channel state machine states map as following:
  77 + * CLOSED -> !cid[N]
  78 + * WAIT_CONNECT -> never occurs
  79 + * WAIT_CONNECT_RSP -> never occurs
  80 + * CONFIG -> cid[N] && config < 3
  81 + * WAIT_CONFIG -> never occurs, cid[N] && config == 0 && !config_r
  82 + * WAIT_SEND_CONFIG -> never occurs, cid[N] && config == 1 && !config_r
  83 + * WAIT_CONFIG_REQ_RSP -> cid[N] && config == 0 && config_req_id
  84 + * WAIT_CONFIG_RSP -> cid[N] && config == 1 && config_req_id
  85 + * WAIT_CONFIG_REQ -> cid[N] && config == 2
  86 + * OPEN -> cid[N] && config == 3
  87 + * WAIT_DISCONNECT -> never occurs
  88 + */
  89 +
  90 + struct l2cap_chan_s signalling_ch;
  91 + struct l2cap_chan_s group_ch;
  92 +};
  93 +
  94 +struct slave_l2cap_instance_s {
  95 + struct bt_link_s link; /* Underlying logical link (ACL) */
  96 + struct l2cap_instance_s l2cap;
  97 +};
  98 +
  99 +struct bt_l2cap_psm_s {
  100 + int psm;
  101 + int min_mtu;
  102 + int (*new_channel)(struct bt_l2cap_device_s *device,
  103 + struct bt_l2cap_conn_params_s *params);
  104 + struct bt_l2cap_psm_s *next;
  105 +};
  106 +
  107 +static const uint16_t l2cap_fcs16_table[256] = {
  108 + 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
  109 + 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
  110 + 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
  111 + 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
  112 + 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
  113 + 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
  114 + 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
  115 + 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
  116 + 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
  117 + 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
  118 + 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
  119 + 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
  120 + 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
  121 + 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
  122 + 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
  123 + 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
  124 + 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
  125 + 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
  126 + 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
  127 + 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
  128 + 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
  129 + 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
  130 + 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
  131 + 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
  132 + 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
  133 + 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
  134 + 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
  135 + 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
  136 + 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
  137 + 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
  138 + 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
  139 + 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040,
  140 +};
  141 +
  142 +static uint16_t l2cap_fcs16(const uint8_t *message, int len)
  143 +{
  144 + uint16_t fcs = 0x0000;
  145 +
  146 + while (len --)
  147 +#if 0
  148 + {
  149 + int i;
  150 +
  151 + fcs ^= *message ++;
  152 + for (i = 8; i; -- i)
  153 + if (fcs & 1)
  154 + fcs = (fcs >> 1) ^ 0xa001;
  155 + else
  156 + fcs = (fcs >> 1);
  157 + }
  158 +#else
  159 + fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff];
  160 +#endif
  161 +
  162 + return fcs;
  163 +}
  164 +
  165 +/* L2CAP layer logic (protocol) */
  166 +
  167 +static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch)
  168 +{
  169 +#if 0
  170 + if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit)
  171 + qemu_mod_timer(ch->retransmission_timer);
  172 + else
  173 + qemu_del_timer(ch->retransmission_timer);
  174 +#endif
  175 +}
  176 +
  177 +static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch)
  178 +{
  179 +#if 0
  180 + if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit)
  181 + qemu_mod_timer(ch->monitor_timer);
  182 + else
  183 + qemu_del_timer(ch->monitor_timer);
  184 +#endif
  185 +}
  186 +
  187 +static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id,
  188 + uint16_t reason, const void *data, int plen)
  189 +{
  190 + uint8_t *pkt;
  191 + l2cap_cmd_hdr *hdr;
  192 + l2cap_cmd_rej *params;
  193 + uint16_t len;
  194 +
  195 + reason = cpu_to_le16(reason);
  196 + len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen);
  197 +
  198 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  199 + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE + plen);
  200 + hdr = (void *) (pkt + 0);
  201 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  202 +
  203 + hdr->code = L2CAP_COMMAND_REJ;
  204 + hdr->ident = id;
  205 + memcpy(&hdr->len, &len, sizeof(hdr->len));
  206 + memcpy(&params->reason, &reason, sizeof(reason));
  207 + if (plen)
  208 + memcpy(pkt + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE, data, plen);
  209 +
  210 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  211 +}
  212 +
  213 +static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id,
  214 + uint16_t reason, uint16_t dcid, uint16_t scid)
  215 +{
  216 + l2cap_cmd_rej_cid params = {
  217 + .dcid = dcid,
  218 + .scid = scid,
  219 + };
  220 +
  221 + l2cap_command_reject(l2cap, id, reason, &params, L2CAP_CMD_REJ_CID_SIZE);
  222 +}
  223 +
  224 +static void l2cap_connection_response(struct l2cap_instance_s *l2cap,
  225 + int dcid, int scid, int result, int status)
  226 +{
  227 + uint8_t *pkt;
  228 + l2cap_cmd_hdr *hdr;
  229 + l2cap_conn_rsp *params;
  230 +
  231 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  232 + L2CAP_CMD_HDR_SIZE + L2CAP_CONN_RSP_SIZE);
  233 + hdr = (void *) (pkt + 0);
  234 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  235 +
  236 + hdr->code = L2CAP_CONN_RSP;
  237 + hdr->ident = l2cap->last_id;
  238 + hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE);
  239 +
  240 + params->dcid = cpu_to_le16(dcid);
  241 + params->scid = cpu_to_le16(scid);
  242 + params->result = cpu_to_le16(result);
  243 + params->status = cpu_to_le16(status);
  244 +
  245 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  246 +}
  247 +
  248 +static void l2cap_configuration_request(struct l2cap_instance_s *l2cap,
  249 + int dcid, int flag, const uint8_t *data, int len)
  250 +{
  251 + uint8_t *pkt;
  252 + l2cap_cmd_hdr *hdr;
  253 + l2cap_conf_req *params;
  254 +
  255 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  256 + L2CAP_CMD_HDR_SIZE + L2CAP_CONF_REQ_SIZE(len));
  257 + hdr = (void *) (pkt + 0);
  258 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  259 +
  260 + /* TODO: unify the id sequencing */
  261 + l2cap->last_id = l2cap->next_id;
  262 + l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
  263 +
  264 + hdr->code = L2CAP_CONF_REQ;
  265 + hdr->ident = l2cap->last_id;
  266 + hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len));
  267 +
  268 + params->dcid = cpu_to_le16(dcid);
  269 + params->flags = cpu_to_le16(flag);
  270 + if (len)
  271 + memcpy(params->data, data, len);
  272 +
  273 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  274 +}
  275 +
  276 +static void l2cap_configuration_response(struct l2cap_instance_s *l2cap,
  277 + int scid, int flag, int result, const uint8_t *data, int len)
  278 +{
  279 + uint8_t *pkt;
  280 + l2cap_cmd_hdr *hdr;
  281 + l2cap_conf_rsp *params;
  282 +
  283 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  284 + L2CAP_CMD_HDR_SIZE + L2CAP_CONF_RSP_SIZE(len));
  285 + hdr = (void *) (pkt + 0);
  286 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  287 +
  288 + hdr->code = L2CAP_CONF_RSP;
  289 + hdr->ident = l2cap->last_id;
  290 + hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len));
  291 +
  292 + params->scid = cpu_to_le16(scid);
  293 + params->flags = cpu_to_le16(flag);
  294 + params->result = cpu_to_le16(result);
  295 + if (len)
  296 + memcpy(params->data, data, len);
  297 +
  298 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  299 +}
  300 +
  301 +static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap,
  302 + int dcid, int scid)
  303 +{
  304 + uint8_t *pkt;
  305 + l2cap_cmd_hdr *hdr;
  306 + l2cap_disconn_rsp *params;
  307 +
  308 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  309 + L2CAP_CMD_HDR_SIZE + L2CAP_DISCONN_RSP_SIZE);
  310 + hdr = (void *) (pkt + 0);
  311 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  312 +
  313 + hdr->code = L2CAP_DISCONN_RSP;
  314 + hdr->ident = l2cap->last_id;
  315 + hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE);
  316 +
  317 + params->dcid = cpu_to_le16(dcid);
  318 + params->scid = cpu_to_le16(scid);
  319 +
  320 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  321 +}
  322 +
  323 +static void l2cap_echo_response(struct l2cap_instance_s *l2cap,
  324 + const uint8_t *data, int len)
  325 +{
  326 + uint8_t *pkt;
  327 + l2cap_cmd_hdr *hdr;
  328 + uint8_t *params;
  329 +
  330 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  331 + L2CAP_CMD_HDR_SIZE + len);
  332 + hdr = (void *) (pkt + 0);
  333 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  334 +
  335 + hdr->code = L2CAP_ECHO_RSP;
  336 + hdr->ident = l2cap->last_id;
  337 + hdr->len = cpu_to_le16(len);
  338 +
  339 + memcpy(params, data, len);
  340 +
  341 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  342 +}
  343 +
  344 +static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type,
  345 + int result, const uint8_t *data, int len)
  346 +{
  347 + uint8_t *pkt;
  348 + l2cap_cmd_hdr *hdr;
  349 + l2cap_info_rsp *params;
  350 +
  351 + pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
  352 + L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + len);
  353 + hdr = (void *) (pkt + 0);
  354 + params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
  355 +
  356 + hdr->code = L2CAP_INFO_RSP;
  357 + hdr->ident = l2cap->last_id;
  358 + hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len);
  359 +
  360 + params->type = cpu_to_le16(type);
  361 + params->result = cpu_to_le16(result);
  362 + if (len)
  363 + memcpy(params->data, data, len);
  364 +
  365 + l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
  366 +}
  367 +
  368 +static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len);
  369 +static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms);
  370 +#if 0
  371 +static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len);
  372 +static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm);
  373 +#endif
  374 +static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
  375 + const l2cap_hdr *hdr, int len);
  376 +static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
  377 + const l2cap_hdr *hdr, int len);
  378 +
  379 +static int l2cap_cid_new(struct l2cap_instance_s *l2cap)
  380 +{
  381 + int i;
  382 +
  383 + for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++)
  384 + if (!l2cap->cid[i])
  385 + return i;
  386 +
  387 + return L2CAP_CID_INVALID;
  388 +}
  389 +
  390 +static inline struct bt_l2cap_psm_s *l2cap_psm(
  391 + struct bt_l2cap_device_s *device, int psm)
  392 +{
  393 + struct bt_l2cap_psm_s *ret = device->first_psm;
  394 +
  395 + while (ret && ret->psm != psm)
  396 + ret = ret->next;
  397 +
  398 + return ret;
  399 +}
  400 +
  401 +static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap,
  402 + int psm, int source_cid)
  403 +{
  404 + struct l2cap_chan_s *ch = 0;
  405 + struct bt_l2cap_psm_s *psm_info;
  406 + int result, status;
  407 + int cid = l2cap_cid_new(l2cap);
  408 +
  409 + if (cid) {
  410 + /* See what the channel is to be used for.. */
  411 + psm_info = l2cap_psm(l2cap->dev, psm);
  412 +
  413 + if (psm_info) {
  414 + /* Device supports this use-case. */
  415 + ch = qemu_mallocz(sizeof(*ch));
  416 + ch->params.sdu_out = l2cap_bframe_out;
  417 + ch->params.sdu_submit = l2cap_bframe_submit;
  418 + ch->frame_in = l2cap_bframe_in;
  419 + ch->mps = 65536;
  420 + ch->min_mtu = MAX(48, psm_info->min_mtu);
  421 + ch->params.remote_mtu = MAX(672, ch->min_mtu);
  422 + ch->remote_cid = source_cid;
  423 + ch->mode = L2CAP_MODE_BASIC;
  424 + ch->l2cap = l2cap;
  425 +
  426 + /* Does it feel like opening yet another channel though? */
  427 + if (!psm_info->new_channel(l2cap->dev, &ch->params)) {
  428 + l2cap->cid[cid] = ch;
  429 +
  430 + result = L2CAP_CR_SUCCESS;
  431 + status = L2CAP_CS_NO_INFO;
  432 + } else {
  433 + qemu_free(ch);
  434 +
  435 + result = L2CAP_CR_NO_MEM;
  436 + status = L2CAP_CS_NO_INFO;
  437 + }
  438 + } else {
  439 + result = L2CAP_CR_BAD_PSM;
  440 + status = L2CAP_CS_NO_INFO;
  441 + }
  442 + } else {
  443 + result = L2CAP_CR_NO_MEM;
  444 + status = L2CAP_CS_NO_INFO;
  445 + }
  446 +
  447 + l2cap_connection_response(l2cap, cid, source_cid, result, status);
  448 +
  449 + return ch;
  450 +}
  451 +
  452 +static void l2cap_channel_close(struct l2cap_instance_s *l2cap,
  453 + int cid, int source_cid)
  454 +{
  455 + struct l2cap_chan_s *ch = 0;
  456 +
  457 + /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a
  458 + * connection in CLOSED state still responds with a L2CAP_DisconnectRsp
  459 + * message on an L2CAP_DisconnectReq event. */
  460 + if (unlikely(cid < L2CAP_CID_ALLOC)) {
  461 + l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
  462 + cid, source_cid);
  463 + return;
  464 + }
  465 + if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX))
  466 + ch = l2cap->cid[cid];
  467 +
  468 + if (likely(ch)) {
  469 + if (ch->remote_cid != source_cid) {
  470 + fprintf(stderr, "%s: Ignoring a Disconnection Request with the "
  471 + "invalid SCID %04x.\n", __FUNCTION__, source_cid);
  472 + return;
  473 + }
  474 +
  475 + l2cap->cid[cid] = 0;
  476 +
  477 + ch->params.close(ch->params.opaque);
  478 + qemu_free(ch);
  479 + }
  480 +
  481 + l2cap_disconnection_response(l2cap, cid, source_cid);
  482 +}
  483 +
  484 +static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap,
  485 + struct l2cap_chan_s *ch)
  486 +{
  487 + l2cap_configuration_request(l2cap, ch->remote_cid, 0, 0, 0);
  488 + ch->config_req_id = l2cap->last_id;
  489 + ch->config &= ~L2CAP_CFG_INIT;
  490 +}
  491 +
  492 +static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap,
  493 + struct l2cap_chan_s *ch)
  494 +{
  495 + /* Use all default channel options and terminate negotiation. */
  496 + l2cap_channel_config_null(l2cap, ch);
  497 +}
  498 +
  499 +static int l2cap_channel_config(struct l2cap_instance_s *l2cap,
  500 + struct l2cap_chan_s *ch, int flag,
  501 + const uint8_t *data, int len)
  502 +{
  503 + l2cap_conf_opt *opt;
  504 + l2cap_conf_opt_qos *qos;
  505 + uint32_t val;
  506 + uint8_t rsp[len];
  507 + int result = L2CAP_CONF_SUCCESS;
  508 +
  509 + data = memcpy(rsp, data, len);
  510 + while (len) {
  511 + opt = (void *) data;
  512 +
  513 + if (len < L2CAP_CONF_OPT_SIZE ||
  514 + len < L2CAP_CONF_OPT_SIZE + opt->len) {
  515 + result = L2CAP_CONF_REJECT;
  516 + break;
  517 + }
  518 + data += L2CAP_CONF_OPT_SIZE + opt->len;
  519 + len -= L2CAP_CONF_OPT_SIZE + opt->len;
  520 +
  521 + switch (opt->type & 0x7f) {
  522 + case L2CAP_CONF_MTU:
  523 + if (opt->len != 2) {
  524 + result = L2CAP_CONF_REJECT;
  525 + break;
  526 + }
  527 +
  528 + /* MTU */
  529 + val = le16_to_cpup((void *) opt->val);
  530 + if (val < ch->min_mtu) {
  531 + cpu_to_le16w((void *) opt->val, ch->min_mtu);
  532 + result = L2CAP_CONF_UNACCEPT;
  533 + break;
  534 + }
  535 +
  536 + ch->params.remote_mtu = val;
  537 + break;
  538 +
  539 + case L2CAP_CONF_FLUSH_TO:
  540 + if (opt->len != 2) {
  541 + result = L2CAP_CONF_REJECT;
  542 + break;
  543 + }
  544 +
  545 + /* Flush Timeout */
  546 + val = le16_to_cpup((void *) opt->val);
  547 + if (val < 0x0001) {
  548 + opt->val[0] = 0xff;
  549 + opt->val[1] = 0xff;
  550 + result = L2CAP_CONF_UNACCEPT;
  551 + break;
  552 + }
  553 + break;
  554 +
  555 + case L2CAP_CONF_QOS:
  556 + if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) {
  557 + result = L2CAP_CONF_REJECT;
  558 + break;
  559 + }
  560 + qos = (void *) opt->val;
  561 +
  562 + /* Flags */
  563 + val = qos->flags;
  564 + if (val) {
  565 + qos->flags = 0;
  566 + result = L2CAP_CONF_UNACCEPT;
  567 + }
  568 +
  569 + /* Service type */
  570 + val = qos->service_type;
  571 + if (val != L2CAP_CONF_QOS_BEST_EFFORT &&
  572 + val != L2CAP_CONF_QOS_NO_TRAFFIC) {
  573 + qos->service_type = L2CAP_CONF_QOS_BEST_EFFORT;
  574 + result = L2CAP_CONF_UNACCEPT;
  575 + }
  576 +
  577 + if (val != L2CAP_CONF_QOS_NO_TRAFFIC) {
  578 + /* XXX: These values should possibly be calculated
  579 + * based on LM / baseband properties also. */
  580 +
  581 + /* Token rate */
  582 + val = le32_to_cpu(qos->token_rate);
  583 + if (val == L2CAP_CONF_QOS_WILDCARD)
  584 + qos->token_rate = cpu_to_le32(0x100000);
  585 +
  586 + /* Token bucket size */
  587 + val = le32_to_cpu(qos->token_bucket_size);
  588 + if (val == L2CAP_CONF_QOS_WILDCARD)
  589 + qos->token_bucket_size = cpu_to_le32(65500);
  590 +
  591 + /* Any Peak bandwidth value is correct to return as-is */
  592 + /* Any Access latency value is correct to return as-is */
  593 + /* Any Delay variation value is correct to return as-is */
  594 + }
  595 + break;
  596 +
  597 + case L2CAP_CONF_RFC:
  598 + if (opt->len != 9) {
  599 + result = L2CAP_CONF_REJECT;
  600 + break;
  601 + }
  602 +
  603 + /* Mode */
  604 + val = opt->val[0];
  605 + switch (val) {
  606 + case L2CAP_MODE_BASIC:
  607 + ch->mode = val;
  608 + ch->frame_in = l2cap_bframe_in;
  609 +
  610 + /* All other parameters shall be ignored */
  611 + break;
  612 +
  613 + case L2CAP_MODE_RETRANS:
  614 + case L2CAP_MODE_FLOWCTL:
  615 + ch->mode = val;
  616 + ch->frame_in = l2cap_iframe_in;
  617 + /* Note: most of these parameters refer to incoming traffic
  618 + * so we don't need to save them as long as we can accept
  619 + * incoming PDUs at any values of the parameters. */
  620 +
  621 + /* TxWindow size */
  622 + val = opt->val[1];
  623 + if (val < 1 || val > 32) {
  624 + opt->val[1] = 32;
  625 + result = L2CAP_CONF_UNACCEPT;
  626 + break;
  627 + }
  628 +
  629 + /* MaxTransmit */
  630 + val = opt->val[2];
  631 + if (val < 1) {
  632 + opt->val[2] = 1;
  633 + result = L2CAP_CONF_UNACCEPT;
  634 + break;
  635 + }
  636 +
  637 + /* Remote Retransmission time-out shouldn't affect local
  638 + * operation (?) */
  639 +
  640 + /* The Monitor time-out drives the local Monitor timer (?),
  641 + * so save the value. */
  642 + val = (opt->val[6] << 8) | opt->val[5];
  643 + if (val < 30) {
  644 + opt->val[5] = 100 & 0xff;
  645 + opt->val[6] = 100 >> 8;
  646 + result = L2CAP_CONF_UNACCEPT;
  647 + break;
  648 + }
  649 + ch->monitor_timeout = val;
  650 + l2cap_monitor_timer_update(ch);
  651 +
  652 + /* MPS */
  653 + val = (opt->val[8] << 8) | opt->val[7];
  654 + if (val < ch->min_mtu) {
  655 + opt->val[7] = ch->min_mtu & 0xff;
  656 + opt->val[8] = ch->min_mtu >> 8;
  657 + result = L2CAP_CONF_UNACCEPT;
  658 + break;
  659 + }
  660 + ch->mps = val;
  661 + break;
  662 +
  663 + default:
  664 + result = L2CAP_CONF_UNACCEPT;
  665 + break;
  666 + }
  667 + break;
  668 +
  669 + default:
  670 + if (!(opt->type >> 7))
  671 + result = L2CAP_CONF_UNKNOWN;
  672 + break;
  673 + }
  674 +
  675 + if (result != L2CAP_CONF_SUCCESS)
  676 + break; /* XXX: should continue? */
  677 + }
  678 +
  679 + l2cap_configuration_response(l2cap, ch->remote_cid,
  680 + flag, result, rsp, len);
  681 +
  682 + return result == L2CAP_CONF_SUCCESS && !flag;
  683 +}
  684 +
  685 +static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap,
  686 + int flag, int cid, const uint8_t *data, int len)
  687 +{
  688 + struct l2cap_chan_s *ch;
  689 +
  690 + if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
  691 + l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
  692 + cid, 0x0000);
  693 + return;
  694 + }
  695 + ch = l2cap->cid[cid];
  696 +
  697 + /* From OPEN go to WAIT_CONFIG_REQ and from WAIT_CONFIG_REQ_RSP to
  698 + * WAIT_CONFIG_REQ_RSP. This is assuming the transition chart for OPEN
  699 + * on pg 1053, section 6.1.5, volume 3 of BT Core V2.0 has a mistake
  700 + * and on options-acceptable we go back to OPEN and otherwise to
  701 + * WAIT_CONFIG_REQ and not the other way. */
  702 + ch->config &= ~L2CAP_CFG_ACC;
  703 +
  704 + if (l2cap_channel_config(l2cap, ch, flag, data, len))
  705 + /* Go to OPEN or WAIT_CONFIG_RSP */
  706 + ch->config |= L2CAP_CFG_ACC;
  707 +
  708 + /* TODO: if the incoming traffic flow control or retransmission mode
  709 + * changed then we probably need to also generate the
  710 + * ConfigureChannel_Req event and set the outgoing traffic to the same
  711 + * mode. */
  712 + if (!(ch->config & L2CAP_CFG_INIT) && (ch->config & L2CAP_CFG_ACC) &&
  713 + !ch->config_req_id)
  714 + l2cap_channel_config_req_event(l2cap, ch);
  715 +}
  716 +
  717 +static int l2cap_channel_config_rsp_msg(struct l2cap_instance_s *l2cap,
  718 + int result, int flag, int cid, const uint8_t *data, int len)
  719 +{
  720 + struct l2cap_chan_s *ch;
  721 +
  722 + if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
  723 + l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
  724 + cid, 0x0000);
  725 + return 0;
  726 + }
  727 + ch = l2cap->cid[cid];
  728 +
  729 + if (ch->config_req_id != l2cap->last_id)
  730 + return 1;
  731 + ch->config_req_id = 0;
  732 +
  733 + if (result == L2CAP_CONF_SUCCESS) {
  734 + if (!flag)
  735 + ch->config |= L2CAP_CFG_INIT;
  736 + else
  737 + l2cap_channel_config_null(l2cap, ch);
  738 + } else
  739 + /* Retry until we succeed */
  740 + l2cap_channel_config_req_event(l2cap, ch);
  741 +
  742 + return 0;
  743 +}
  744 +
  745 +static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap,
  746 + int psm, int source_cid)
  747 +{
  748 + struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid);
  749 +
  750 + if (!ch)
  751 + return;
  752 +
  753 + /* Optional */
  754 + if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id)
  755 + l2cap_channel_config_req_event(l2cap, ch);
  756 +}
  757 +
  758 +static void l2cap_info(struct l2cap_instance_s *l2cap, int type)
  759 +{
  760 + uint8_t data[4];
  761 + int len = 0;
  762 + int result = L2CAP_IR_SUCCESS;
  763 +
  764 + switch (type) {
  765 + case L2CAP_IT_CL_MTU:
  766 + data[len ++] = l2cap->group_ch.mps & 0xff;
  767 + data[len ++] = l2cap->group_ch.mps >> 8;
  768 + break;
  769 +
  770 + case L2CAP_IT_FEAT_MASK:
  771 + /* (Prematurely) report Flow control and Retransmission modes. */
  772 + data[len ++] = 0x03;
  773 + data[len ++] = 0x00;
  774 + data[len ++] = 0x00;
  775 + data[len ++] = 0x00;
  776 + break;
  777 +
  778 + default:
  779 + result = L2CAP_IR_NOTSUPP;
  780 + }
  781 +
  782 + l2cap_info_response(l2cap, type, result, data, len);
  783 +}
  784 +
  785 +static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id,
  786 + const uint8_t *params, int len)
  787 +{
  788 + int err;
  789 +
  790 +#if 0
  791 + /* TODO: do the IDs really have to be in sequence? */
  792 + if (!id || (id != l2cap->last_id && id != l2cap->next_id)) {
  793 + fprintf(stderr, "%s: out of sequence command packet ignored.\n",
  794 + __FUNCTION__);
  795 + return;
  796 + }
  797 +#else
  798 + l2cap->next_id = id;
  799 +#endif
  800 + if (id == l2cap->next_id) {
  801 + l2cap->last_id = l2cap->next_id;
  802 + l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
  803 + } else {
  804 + /* TODO: Need to re-send the same response, without re-executing
  805 + * the corresponding command! */
  806 + }
  807 +
  808 + switch (code) {
  809 + case L2CAP_COMMAND_REJ:
  810 + if (unlikely(len != 2 && len != 4 && len != 6)) {
  811 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  812 + goto reject;
  813 + }
  814 +
  815 + /* We never issue commands other than Command Reject currently. */
  816 + fprintf(stderr, "%s: stray Command Reject (%02x, %04x) "
  817 + "packet, ignoring.\n", __FUNCTION__, id,
  818 + le16_to_cpu(((l2cap_cmd_rej *) params)->reason));
  819 + break;
  820 +
  821 + case L2CAP_CONN_REQ:
  822 + if (unlikely(len != L2CAP_CONN_REQ_SIZE)) {
  823 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  824 + goto reject;
  825 + }
  826 +
  827 + l2cap_channel_open_req_msg(l2cap,
  828 + le16_to_cpu(((l2cap_conn_req *) params)->psm),
  829 + le16_to_cpu(((l2cap_conn_req *) params)->scid));
  830 + break;
  831 +
  832 + case L2CAP_CONN_RSP:
  833 + if (unlikely(len != L2CAP_CONN_RSP_SIZE)) {
  834 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  835 + goto reject;
  836 + }
  837 +
  838 + /* We never issue Connection Requests currently. TODO */
  839 + fprintf(stderr, "%s: unexpected Connection Response (%02x) "
  840 + "packet, ignoring.\n", __FUNCTION__, id);
  841 + break;
  842 +
  843 + case L2CAP_CONF_REQ:
  844 + if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) {
  845 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  846 + goto reject;
  847 + }
  848 +
  849 + l2cap_channel_config_req_msg(l2cap,
  850 + le16_to_cpu(((l2cap_conf_req *) params)->flags) & 1,
  851 + le16_to_cpu(((l2cap_conf_req *) params)->dcid),
  852 + ((l2cap_conf_req *) params)->data,
  853 + len - L2CAP_CONF_REQ_SIZE(0));
  854 + break;
  855 +
  856 + case L2CAP_CONF_RSP:
  857 + if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) {
  858 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  859 + goto reject;
  860 + }
  861 +
  862 + if (l2cap_channel_config_rsp_msg(l2cap,
  863 + le16_to_cpu(((l2cap_conf_rsp *) params)->result),
  864 + le16_to_cpu(((l2cap_conf_rsp *) params)->flags) & 1,
  865 + le16_to_cpu(((l2cap_conf_rsp *) params)->scid),
  866 + ((l2cap_conf_rsp *) params)->data,
  867 + len - L2CAP_CONF_RSP_SIZE(0)))
  868 + fprintf(stderr, "%s: unexpected Configure Response (%02x) "
  869 + "packet, ignoring.\n", __FUNCTION__, id);
  870 + break;
  871 +
  872 + case L2CAP_DISCONN_REQ:
  873 + if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) {
  874 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  875 + goto reject;
  876 + }
  877 +
  878 + l2cap_channel_close(l2cap,
  879 + le16_to_cpu(((l2cap_disconn_req *) params)->dcid),
  880 + le16_to_cpu(((l2cap_disconn_req *) params)->scid));
  881 + break;
  882 +
  883 + case L2CAP_DISCONN_RSP:
  884 + if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) {
  885 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  886 + goto reject;
  887 + }
  888 +
  889 + /* We never issue Disconnection Requests currently. TODO */
  890 + fprintf(stderr, "%s: unexpected Disconnection Response (%02x) "
  891 + "packet, ignoring.\n", __FUNCTION__, id);
  892 + break;
  893 +
  894 + case L2CAP_ECHO_REQ:
  895 + l2cap_echo_response(l2cap, params, len);
  896 + break;
  897 +
  898 + case L2CAP_ECHO_RSP:
  899 + /* We never issue Echo Requests currently. TODO */
  900 + fprintf(stderr, "%s: unexpected Echo Response (%02x) "
  901 + "packet, ignoring.\n", __FUNCTION__, id);
  902 + break;
  903 +
  904 + case L2CAP_INFO_REQ:
  905 + if (unlikely(len != L2CAP_INFO_REQ_SIZE)) {
  906 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  907 + goto reject;
  908 + }
  909 +
  910 + l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type));
  911 + break;
  912 +
  913 + case L2CAP_INFO_RSP:
  914 + if (unlikely(len != L2CAP_INFO_RSP_SIZE)) {
  915 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  916 + goto reject;
  917 + }
  918 +
  919 + /* We never issue Information Requests currently. TODO */
  920 + fprintf(stderr, "%s: unexpected Information Response (%02x) "
  921 + "packet, ignoring.\n", __FUNCTION__, id);
  922 + break;
  923 +
  924 + default:
  925 + err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
  926 + reject:
  927 + l2cap_command_reject(l2cap, id, err, 0, 0);
  928 + break;
  929 + }
  930 +}
  931 +
  932 +static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable)
  933 +{
  934 + ch->rexmit = enable;
  935 +
  936 + l2cap_retransmission_timer_update(ch);
  937 + l2cap_monitor_timer_update(ch);
  938 +}
  939 +
  940 +/* Command frame SDU */
  941 +static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len)
  942 +{
  943 + struct l2cap_instance_s *l2cap = opaque;
  944 + const l2cap_cmd_hdr *hdr;
  945 + int clen;
  946 +
  947 + while (len) {
  948 + hdr = (void *) data;
  949 + if (len < L2CAP_CMD_HDR_SIZE)
  950 + /* TODO: signal an error */
  951 + return;
  952 + len -= L2CAP_CMD_HDR_SIZE;
  953 + data += L2CAP_CMD_HDR_SIZE;
  954 +
  955 + clen = le16_to_cpu(hdr->len);
  956 + if (len < clen) {
  957 + l2cap_command_reject(l2cap, hdr->ident,
  958 + L2CAP_REJ_CMD_NOT_UNDERSTOOD, 0, 0);
  959 + break;
  960 + }
  961 +
  962 + l2cap_command(l2cap, hdr->code, hdr->ident, data, clen);
  963 + len -= clen;
  964 + data += clen;
  965 + }
  966 +}
  967 +
  968 +/* Group frame SDU */
  969 +static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
  970 +{
  971 +}
  972 +
  973 +/* Supervisory frame */
  974 +static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
  975 +{
  976 +}
  977 +
  978 +/* Basic L2CAP mode Information frame */
  979 +static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
  980 + const l2cap_hdr *hdr, int len)
  981 +{
  982 + /* We have a full SDU, no further processing */
  983 + ch->params.sdu_in(ch->params.opaque, hdr->data, len);
  984 +}
  985 +
  986 +/* Flow Control and Retransmission mode frame */
  987 +static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
  988 + const l2cap_hdr *hdr, int len)
  989 +{
  990 + uint16_t fcs = le16_to_cpup((void *) (hdr->data + len - 2));
  991 +
  992 + if (len < 4)
  993 + goto len_error;
  994 + if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs)
  995 + goto fcs_error;
  996 +
  997 + if ((hdr->data[0] >> 7) == ch->rexmit)
  998 + l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7));
  999 +
  1000 + if (hdr->data[0] & 1) {
  1001 + if (len != 4)
  1002 + /* TODO: Signal an error? */;
  1003 + return;
  1004 +
  1005 + return l2cap_sframe_in(ch, le16_to_cpup((void *) hdr->data));
  1006 + }
  1007 +
  1008 + switch (hdr->data[1] >> 6) { /* SAR */
  1009 + case L2CAP_SAR_NO_SEG:
  1010 + if (ch->len_total)
  1011 + goto seg_error;
  1012 + if (len - 4 > ch->mps)
  1013 + goto len_error;
  1014 +
  1015 + return ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4);
  1016 +
  1017 + case L2CAP_SAR_START:
  1018 + if (ch->len_total || len < 6)
  1019 + goto seg_error;
  1020 + if (len - 6 > ch->mps)
  1021 + goto len_error;
  1022 +
  1023 + ch->len_total = le16_to_cpup((void *) (hdr->data + 2));
  1024 + if (len >= 6 + ch->len_total)
  1025 + goto seg_error;
  1026 +
  1027 + ch->len_cur = len - 6;
  1028 + memcpy(ch->sdu, hdr->data + 4, ch->len_cur);
  1029 + break;
  1030 +
  1031 + case L2CAP_SAR_END:
  1032 + if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total)
  1033 + goto seg_error;
  1034 + if (len - 4 > ch->mps)
  1035 + goto len_error;
  1036 +
  1037 + memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
  1038 + return ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total);
  1039 +
  1040 + case L2CAP_SAR_CONT:
  1041 + if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total)
  1042 + goto seg_error;
  1043 + if (len - 4 > ch->mps)
  1044 + goto len_error;
  1045 +
  1046 + memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
  1047 + ch->len_cur += len - 4;
  1048 + break;
  1049 +
  1050 + seg_error:
  1051 + len_error: /* TODO */
  1052 + fcs_error: /* TODO */
  1053 + ch->len_cur = 0;
  1054 + ch->len_total = 0;
  1055 + break;
  1056 + }
  1057 +}
  1058 +
  1059 +static void l2cap_frame_in(struct l2cap_instance_s *l2cap,
  1060 + const l2cap_hdr *frame)
  1061 +{
  1062 + uint16_t cid = le16_to_cpu(frame->cid);
  1063 + uint16_t len = le16_to_cpu(frame->len);
  1064 +
  1065 + if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
  1066 + fprintf(stderr, "%s: frame addressed to a non-existent L2CAP "
  1067 + "channel %04x received.\n", __FUNCTION__, cid);
  1068 + return;
  1069 + }
  1070 +
  1071 + l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len);
  1072 +}
  1073 +
  1074 +/* "Recombination" */
  1075 +static void l2cap_pdu_in(struct l2cap_instance_s *l2cap,
  1076 + const uint8_t *data, int len)
  1077 +{
  1078 + const l2cap_hdr *hdr = (void *) l2cap->frame_in;
  1079 +
  1080 + if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) {
  1081 + if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) {
  1082 + memcpy(l2cap->frame_in + l2cap->frame_in_len, data,
  1083 + sizeof(l2cap->frame_in) - l2cap->frame_in_len);
  1084 + l2cap->frame_in_len = sizeof(l2cap->frame_in);
  1085 + /* TODO: truncate */
  1086 + l2cap_frame_in(l2cap, hdr);
  1087 + }
  1088 +
  1089 + return;
  1090 + }
  1091 +
  1092 + memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len);
  1093 + l2cap->frame_in_len += len;
  1094 +
  1095 + if (len >= L2CAP_HDR_SIZE)
  1096 + if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len))
  1097 + l2cap_frame_in(l2cap, hdr);
  1098 + /* There is never a start of a new PDU in the same ACL packet, so
  1099 + * no need to memmove the remaining payload and loop. */
  1100 +}
  1101 +
  1102 +static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap,
  1103 + uint16_t cid, uint16_t len)
  1104 +{
  1105 + l2cap_hdr *hdr = (void *) l2cap->frame_out;
  1106 +
  1107 + l2cap->frame_out_len = len + L2CAP_HDR_SIZE;
  1108 +
  1109 + hdr->cid = cpu_to_le16(cid);
  1110 + hdr->len = cpu_to_le16(len);
  1111 +
  1112 + return l2cap->frame_out + L2CAP_HDR_SIZE;
  1113 +}
  1114 +
  1115 +static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap)
  1116 +{
  1117 + /* TODO: Fragmentation */
  1118 + (l2cap->role ?
  1119 + l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp)
  1120 + (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len);
  1121 +}
  1122 +
  1123 +static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len)
  1124 +{
  1125 + struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
  1126 +
  1127 + if (len > chan->params.remote_mtu) {
  1128 + fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n",
  1129 + __FUNCTION__,
  1130 + chan->remote_cid, chan->params.remote_mtu);
  1131 + exit(-1);
  1132 + }
  1133 +
  1134 + return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len);
  1135 +}
  1136 +
  1137 +static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms)
  1138 +{
  1139 + struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms;
  1140 +
  1141 + return l2cap_pdu_submit(chan->l2cap);
  1142 +}
  1143 +
  1144 +#if 0
  1145 +/* Stub: Only used if an emulated device requests outgoing flow control */
  1146 +static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len)
  1147 +{
  1148 + struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
  1149 +
  1150 + if (len > chan->params.remote_mtu) {
  1151 + /* TODO: slice into segments and queue each segment as a separate
  1152 + * I-Frame in a FIFO of I-Frames, local to the CID. */
  1153 + } else {
  1154 + /* TODO: add to the FIFO of I-Frames, local to the CID. */
  1155 + /* Possibly we need to return a pointer to a contiguous buffer
  1156 + * for now and then memcpy from it into FIFOs in l2cap_iframe_submit
  1157 + * while segmenting at the same time. */
  1158 + }
  1159 + return 0;
  1160 +}
  1161 +
  1162 +static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm)
  1163 +{
  1164 + /* TODO: If flow control indicates clear to send, start submitting the
  1165 + * invidual I-Frames from the FIFO, but don't remove them from there.
  1166 + * Kick the appropriate timer until we get an S-Frame, and only then
  1167 + * remove from FIFO or resubmit and re-kick the timer if the timer
  1168 + * expired. */
  1169 +}
  1170 +#endif
  1171 +
  1172 +static void l2cap_init(struct l2cap_instance_s *l2cap,
  1173 + struct bt_link_s *link, int role)
  1174 +{
  1175 + l2cap->link = link;
  1176 + l2cap->role = role;
  1177 + l2cap->dev = (struct bt_l2cap_device_s *)
  1178 + (role ? link->host : link->slave);
  1179 +
  1180 + l2cap->next_id = 1;
  1181 +
  1182 + /* Establish the signalling channel */
  1183 + l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in;
  1184 + l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out;
  1185 + l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit;
  1186 + l2cap->signalling_ch.params.opaque = l2cap;
  1187 + l2cap->signalling_ch.params.remote_mtu = 48;
  1188 + l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING;
  1189 + l2cap->signalling_ch.frame_in = l2cap_bframe_in;
  1190 + l2cap->signalling_ch.mps = 65536;
  1191 + l2cap->signalling_ch.min_mtu = 48;
  1192 + l2cap->signalling_ch.mode = L2CAP_MODE_BASIC;
  1193 + l2cap->signalling_ch.l2cap = l2cap;
  1194 + l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch;
  1195 +
  1196 + /* Establish the connection-less data channel */
  1197 + l2cap->group_ch.params.sdu_in = l2cap_gframe_in;
  1198 + l2cap->group_ch.params.opaque = l2cap;
  1199 + l2cap->group_ch.frame_in = l2cap_bframe_in;
  1200 + l2cap->group_ch.mps = 65533;
  1201 + l2cap->group_ch.l2cap = l2cap;
  1202 + l2cap->group_ch.remote_cid = L2CAP_CID_INVALID;
  1203 + l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch;
  1204 +}
  1205 +
  1206 +static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect)
  1207 +{
  1208 + int cid;
  1209 +
  1210 + /* Don't send DISCONNECT if we are currently handling a DISCONNECT
  1211 + * sent from the other side. */
  1212 + if (send_disconnect) {
  1213 + if (l2cap->role)
  1214 + l2cap->dev->device.lmp_disconnect_slave(l2cap->link);
  1215 + /* l2cap->link is invalid from now on. */
  1216 + else
  1217 + l2cap->dev->device.lmp_disconnect_master(l2cap->link);
  1218 + }
  1219 +
  1220 + for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++)
  1221 + if (l2cap->cid[cid]) {
  1222 + l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque);
  1223 + free(l2cap->cid[cid]);
  1224 + }
  1225 +
  1226 + if (l2cap->role)
  1227 + qemu_free(l2cap);
  1228 + else
  1229 + qemu_free(l2cap->link);
  1230 +}
  1231 +
  1232 +/* L2CAP glue to lower layers in bluetooth stack (LMP) */
  1233 +
  1234 +static void l2cap_lmp_connection_request(struct bt_link_s *link)
  1235 +{
  1236 + struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave;
  1237 + struct slave_l2cap_instance_s *l2cap;
  1238 +
  1239 + /* Always accept - we only get called if (dev->device->page_scan). */
  1240 +
  1241 + l2cap = qemu_mallocz(sizeof(struct slave_l2cap_instance_s));
  1242 + l2cap->link.slave = &dev->device;
  1243 + l2cap->link.host = link->host;
  1244 + l2cap_init(&l2cap->l2cap, &l2cap->link, 0);
  1245 +
  1246 + /* Always at the end */
  1247 + link->host->reject_reason = 0;
  1248 + link->host->lmp_connection_complete(&l2cap->link);
  1249 +}
  1250 +
  1251 +/* Stub */
  1252 +static void l2cap_lmp_connection_complete(struct bt_link_s *link)
  1253 +{
  1254 + struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
  1255 + struct l2cap_instance_s *l2cap;
  1256 +
  1257 + if (dev->device.reject_reason) {
  1258 + /* Signal to upper layer */
  1259 + return;
  1260 + }
  1261 +
  1262 + l2cap = qemu_mallocz(sizeof(struct l2cap_instance_s));
  1263 + l2cap_init(l2cap, link, 1);
  1264 +
  1265 + link->acl_mode = acl_active;
  1266 +
  1267 + /* Signal to upper layer */
  1268 +}
  1269 +
  1270 +/* Stub */
  1271 +static void l2cap_lmp_disconnect_host(struct bt_link_s *link)
  1272 +{
  1273 + struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
  1274 + struct l2cap_instance_s *l2cap =
  1275 + /* TODO: Retrieve from upper layer */ (void *) dev;
  1276 +
  1277 + /* Signal to upper layer */
  1278 +
  1279 + l2cap_teardown(l2cap, 0);
  1280 +}
  1281 +
  1282 +static void l2cap_lmp_disconnect_slave(struct bt_link_s *link)
  1283 +{
  1284 + struct slave_l2cap_instance_s *l2cap =
  1285 + (struct slave_l2cap_instance_s *) link;
  1286 +
  1287 + l2cap_teardown(&l2cap->l2cap, 0);
  1288 +}
  1289 +
  1290 +static void l2cap_lmp_acl_data_slave(struct bt_link_s *link,
  1291 + const uint8_t *data, int start, int len)
  1292 +{
  1293 + struct slave_l2cap_instance_s *l2cap =
  1294 + (struct slave_l2cap_instance_s *) link;
  1295 +
  1296 + if (start)
  1297 + l2cap->l2cap.frame_in_len = 0;
  1298 +
  1299 + l2cap_pdu_in(&l2cap->l2cap, data, len);
  1300 +}
  1301 +
  1302 +/* Stub */
  1303 +static void l2cap_lmp_acl_data_host(struct bt_link_s *link,
  1304 + const uint8_t *data, int start, int len)
  1305 +{
  1306 + struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
  1307 + struct l2cap_instance_s *l2cap =
  1308 + /* TODO: Retrieve from upper layer */ (void *) dev;
  1309 +
  1310 + if (start)
  1311 + l2cap->frame_in_len = 0;
  1312 +
  1313 + l2cap_pdu_in(l2cap, data, len);
  1314 +}
  1315 +
  1316 +static void l2cap_dummy_destroy(struct bt_device_s *dev)
  1317 +{
  1318 + struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev;
  1319 +
  1320 + bt_l2cap_device_done(l2cap_dev);
  1321 +}
  1322 +
  1323 +void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
  1324 + struct bt_scatternet_s *net)
  1325 +{
  1326 + bt_device_init(&dev->device, net);
  1327 +
  1328 + dev->device.lmp_connection_request = l2cap_lmp_connection_request;
  1329 + dev->device.lmp_connection_complete = l2cap_lmp_connection_complete;
  1330 + dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host;
  1331 + dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave;
  1332 + dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave;
  1333 + dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host;
  1334 +
  1335 + dev->device.handle_destroy = l2cap_dummy_destroy;
  1336 +}
  1337 +
  1338 +void bt_l2cap_device_done(struct bt_l2cap_device_s *dev)
  1339 +{
  1340 + bt_device_done(&dev->device);
  1341 +
  1342 + /* Should keep a list of all instances and go through it and
  1343 + * invoke l2cap_teardown() for each. */
  1344 +}
  1345 +
  1346 +void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu,
  1347 + int (*new_channel)(struct bt_l2cap_device_s *dev,
  1348 + struct bt_l2cap_conn_params_s *params))
  1349 +{
  1350 + struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm);
  1351 +
  1352 + if (new_psm) {
  1353 + fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n",
  1354 + __FUNCTION__, psm, dev->device.lmp_name);
  1355 + exit(-1);
  1356 + }
  1357 +
  1358 + new_psm = qemu_mallocz(sizeof(*new_psm));
  1359 + new_psm->psm = psm;
  1360 + new_psm->min_mtu = min_mtu;
  1361 + new_psm->new_channel = new_channel;
  1362 + new_psm->next = dev->first_psm;
  1363 + dev->first_psm = new_psm;
  1364 +}
hw/bt-sdp.c 0 → 100644
  View file @4d2d181
  1 +/*
  2 + * Service Discover Protocol server for QEMU L2CAP devices
  3 + *
  4 + * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
  5 + *
  6 + * This program is free software; you can redistribute it and/or
  7 + * modify it under the terms of the GNU General Public License as
  8 + * published by the Free Software Foundation; either version 2 of
  9 + * the License, or (at your option) any later version.
  10 + *
  11 + * This program is distributed in the hope that it will be useful,
  12 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14 + * GNU General Public License for more details.
  15 + *
  16 + * You should have received a copy of the GNU General Public License
  17 + * along with this program; if not, write to the Free Software
  18 + * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  19 + * MA 02111-1307 USA
  20 + */
  21 +
  22 +#include "qemu-common.h"
  23 +#include "bt.h"
  24 +
  25 +struct bt_l2cap_sdp_state_s {
  26 + struct bt_l2cap_conn_params_s *channel;
  27 +
  28 + struct sdp_service_record_s {
  29 + int match;
  30 +
  31 + int *uuid;
  32 + int uuids;
  33 + struct sdp_service_attribute_s {
  34 + int match;
  35 +
  36 + int attribute_id;
  37 + int len;
  38 + void *pair;
  39 + } *attribute_list;
  40 + int attributes;
  41 + } *service_list;
  42 + int services;
  43 +};
  44 +
  45 +static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left)
  46 +{
  47 + size_t len = *(*element) ++ & SDP_DSIZE_MASK;
  48 +
  49 + if (!*left)
  50 + return -1;
  51 + (*left) --;
  52 +
  53 + if (len < SDP_DSIZE_NEXT1)
  54 + return 1 << len;
  55 + else if (len == SDP_DSIZE_NEXT1) {
  56 + if (*left < 1)
  57 + return -1;
  58 + (*left) --;
  59 +
  60 + return *(*element) ++;
  61 + } else if (len == SDP_DSIZE_NEXT2) {
  62 + if (*left < 2)
  63 + return -1;
  64 + (*left) -= 2;
  65 +
  66 + len = (*(*element) ++) << 8;
  67 + return len | (*(*element) ++);
  68 + } else {
  69 + if (*left < 4)
  70 + return -1;
  71 + (*left) -= 4;
  72 +
  73 + len = (*(*element) ++) << 24;
  74 + len |= (*(*element) ++) << 16;
  75 + len |= (*(*element) ++) << 8;
  76 + return len | (*(*element) ++);
  77 + }
  78 +}
  79 +
  80 +static const uint8_t bt_base_uuid[12] = {
  81 + 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
  82 +};
  83 +
  84 +static int sdp_uuid_match(struct sdp_service_record_s *record,
  85 + const uint8_t *uuid, ssize_t datalen)
  86 +{
  87 + int *lo, hi, val;
  88 +
  89 + if (datalen == 16 || datalen == 4) {
  90 + if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12))
  91 + return 0;
  92 +
  93 + if (uuid[0] | uuid[1])
  94 + return 0;
  95 + uuid += 2;
  96 + }
  97 +
  98 + val = (uuid[0] << 8) | uuid[1];
  99 + lo = record->uuid;
  100 + hi = record->uuids;
  101 + while (hi >>= 1)
  102 + if (lo[hi] <= val)
  103 + lo += hi;
  104 +
  105 + return *lo == val;
  106 +}
  107 +
  108 +#define CONTINUATION_PARAM_SIZE (1 + sizeof(int))
  109 +#define MAX_PDU_OUT_SIZE 96 /* Arbitrary */
  110 +#define PDU_HEADER_SIZE 5
  111 +#define MAX_RSP_PARAM_SIZE (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \
  112 + CONTINUATION_PARAM_SIZE)
  113 +
  114 +static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp,
  115 + const uint8_t **req, ssize_t *len)
  116 +{
  117 + size_t datalen;
  118 + int i;
  119 +
  120 + if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)
  121 + return 1;
  122 +
  123 + datalen = sdp_datalen(req, len);
  124 + if (datalen != 2 && datalen != 4 && datalen != 16)
  125 + return 1;
  126 +
  127 + for (i = 0; i < sdp->services; i ++)
  128 + if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))
  129 + sdp->service_list[i].match = 1;
  130 +
  131 + (*req) += datalen;
  132 + (*len) -= datalen;
  133 +
  134 + return 0;
  135 +}
  136 +
  137 +static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp,
  138 + uint8_t *rsp, const uint8_t *req, ssize_t len)
  139 +{
  140 + ssize_t seqlen;
  141 + int i, count, start, end, max;
  142 + int32_t handle;
  143 +
  144 + /* Perform the search */
  145 + for (i = 0; i < sdp->services; i ++)
  146 + sdp->service_list[i].match = 0;
  147 +
  148 + if (len < 1)
  149 + return -SDP_INVALID_SYNTAX;
  150 + if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
  151 + seqlen = sdp_datalen(&req, &len);
  152 + if (seqlen < 3 || len < seqlen)
  153 + return -SDP_INVALID_SYNTAX;
  154 + len -= seqlen;
  155 +
  156 + while (seqlen)
  157 + if (sdp_svc_match(sdp, &req, &seqlen))
  158 + return -SDP_INVALID_SYNTAX;
  159 + } else if (sdp_svc_match(sdp, &req, &seqlen))
  160 + return -SDP_INVALID_SYNTAX;
  161 +
  162 + if (len < 3)
  163 + return -SDP_INVALID_SYNTAX;
  164 + end = (req[0] << 8) | req[1];
  165 + req += 2;
  166 + len -= 2;
  167 +
  168 + if (*req) {
  169 + if (len <= sizeof(int))
  170 + return -SDP_INVALID_SYNTAX;
  171 + len -= sizeof(int);
  172 + memcpy(&start, req + 1, sizeof(int));
  173 + } else
  174 + start = 0;
  175 +
  176 + if (len > 1);
  177 + return -SDP_INVALID_SYNTAX;
  178 +
  179 + /* Output the results */
  180 + len = 4;
  181 + count = 0;
  182 + end = start;
  183 + for (i = 0; i < sdp->services; i ++)
  184 + if (sdp->service_list[i].match) {
  185 + if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) {
  186 + handle = i;
  187 + memcpy(rsp + len, &handle, 4);
  188 + len += 4;
  189 + end = count + 1;
  190 + }
  191 +
  192 + count ++;
  193 + }
  194 +
  195 + rsp[0] = count >> 8;
  196 + rsp[1] = count & 0xff;
  197 + rsp[2] = (end - start) >> 8;
  198 + rsp[3] = (end - start) & 0xff;
  199 +
  200 + if (end < count) {
  201 + rsp[len ++] = sizeof(int);
  202 + memcpy(rsp + len, &end, sizeof(int));
  203 + len += 4;
  204 + } else
  205 + rsp[len ++] = 0;
  206 +
  207 + return len;
  208 +}
  209 +
  210 +static int sdp_attr_match(struct sdp_service_record_s *record,
  211 + const uint8_t **req, ssize_t *len)
  212 +{
  213 + int i, start, end;
  214 +
  215 + if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
  216 + (*req) ++;
  217 + if (*len < 3)
  218 + return 1;
  219 +
  220 + start = (*(*req) ++) << 8;
  221 + start |= *(*req) ++;
  222 + end = start;
  223 + *len -= 3;
  224 + } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
  225 + (*req) ++;
  226 + if (*len < 5)
  227 + return 1;
  228 +
  229 + start = (*(*req) ++) << 8;
  230 + start |= *(*req) ++;
  231 + end = (*(*req) ++) << 8;
  232 + end |= *(*req) ++;
  233 + *len -= 5;
  234 + } else
  235 + return 1;
  236 +
  237 + for (i = 0; i < record->attributes; i ++)
  238 + if (record->attribute_list[i].attribute_id >= start &&
  239 + record->attribute_list[i].attribute_id <= end)
  240 + record->attribute_list[i].match = 1;
  241 +
  242 + return 0;
  243 +}
  244 +
  245 +static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp,
  246 + uint8_t *rsp, const uint8_t *req, ssize_t len)
  247 +{
  248 + ssize_t seqlen;
  249 + int i, start, end, max;
  250 + int32_t handle;
  251 + struct sdp_service_record_s *record;
  252 + uint8_t *lst;
  253 +
  254 + /* Perform the search */
  255 + if (len < 7)
  256 + return -SDP_INVALID_SYNTAX;
  257 + memcpy(&handle, req, 6);
  258 + req += 4;
  259 + len -= 4;
  260 +
  261 + if (handle < 0 || handle > sdp->services)
  262 + return -SDP_INVALID_RECORD_HANDLE;
  263 + record = &sdp->service_list[handle];
  264 +
  265 + for (i = 0; i < record->attributes; i ++)
  266 + record->attribute_list[i].match = 0;
  267 +
  268 + max = (req[0] << 8) | req[1];
  269 + req += 2;
  270 + len -= 2;
  271 + if (max < 0x0007)
  272 + return -SDP_INVALID_SYNTAX;
  273 +
  274 + if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
  275 + seqlen = sdp_datalen(&req, &len);
  276 + if (seqlen < 3 || len < seqlen)
  277 + return -SDP_INVALID_SYNTAX;
  278 + len -= seqlen;
  279 +
  280 + while (seqlen)
  281 + if (sdp_attr_match(record, &req, &seqlen))
  282 + return -SDP_INVALID_SYNTAX;
  283 + } else if (sdp_attr_match(record, &req, &seqlen))
  284 + return -SDP_INVALID_SYNTAX;
  285 +
  286 + if (len < 1)
  287 + return -SDP_INVALID_SYNTAX;
  288 +
  289 + if (*req) {
  290 + if (len <= sizeof(int))
  291 + return -SDP_INVALID_SYNTAX;
  292 + len -= sizeof(int);
  293 + memcpy(&start, req + 1, sizeof(int));
  294 + } else
  295 + start = 0;
  296 +
  297 + if (len > 1)
  298 + return -SDP_INVALID_SYNTAX;
  299 +
  300 + /* Output the results */
  301 + lst = rsp + 2;
  302 + max = MIN(max, MAX_RSP_PARAM_SIZE);
  303 + len = 3 - start;
  304 + end = 0;
  305 + for (i = 0; i < record->attributes; i ++)
  306 + if (record->attribute_list[i].match) {
  307 + if (len >= 0 && len + record->attribute_list[i].len < max) {
  308 + memcpy(lst + len, record->attribute_list[i].pair,
  309 + record->attribute_list[i].len);
  310 + end = len + record->attribute_list[i].len;
  311 + }
  312 + len += record->attribute_list[i].len;
  313 + }
  314 + if (0 >= start) {
  315 + lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
  316 + lst[1] = (len + start - 3) >> 8;
  317 + lst[2] = (len + start - 3) & 0xff;
  318 + }
  319 +
  320 + rsp[0] = end >> 8;
  321 + rsp[1] = end & 0xff;
  322 +
  323 + if (end < len) {
  324 + len = end + start;
  325 + lst[end ++] = sizeof(int);
  326 + memcpy(lst + end, &len, sizeof(int));
  327 + end += sizeof(int);
  328 + } else
  329 + lst[end ++] = 0;
  330 +
  331 + return end + 2;
  332 +}
  333 +
  334 +static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp,
  335 + const uint8_t **req, ssize_t *len)
  336 +{
  337 + int i, j, start, end;
  338 + struct sdp_service_record_s *record;
  339 +
  340 + if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
  341 + (*req) ++;
  342 + if (*len < 3)
  343 + return 1;
  344 +
  345 + start = (*(*req) ++) << 8;
  346 + start |= *(*req) ++;
  347 + end = start;
  348 + *len -= 3;
  349 + } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
  350 + (*req) ++;
  351 + if (*len < 5)
  352 + return 1;
  353 +
  354 + start = (*(*req) ++) << 8;
  355 + start |= *(*req) ++;
  356 + end = (*(*req) ++) << 8;
  357 + end |= *(*req) ++;
  358 + *len -= 5;
  359 + } else
  360 + return 1;
  361 +
  362 + for (i = 0; i < sdp->services; i ++)
  363 + if ((record = &sdp->service_list[i])->match)
  364 + for (j = 0; j < record->attributes; j ++)
  365 + if (record->attribute_list[j].attribute_id >= start &&
  366 + record->attribute_list[j].attribute_id <= end)
  367 + record->attribute_list[j].match = 1;
  368 +
  369 + return 0;
  370 +}
  371 +
  372 +static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp,
  373 + uint8_t *rsp, const uint8_t *req, ssize_t len)
  374 +{
  375 + ssize_t seqlen;
  376 + int i, j, start, end, max;
  377 + struct sdp_service_record_s *record;
  378 + uint8_t *lst;
  379 +
  380 + /* Perform the search */
  381 + for (i = 0; i < sdp->services; i ++) {
  382 + sdp->service_list[i].match = 0;
  383 + for (j = 0; j < sdp->service_list[i].attributes; j ++)
  384 + sdp->service_list[i].attribute_list[j].match = 0;
  385 + }
  386 +
  387 + if (len < 1)
  388 + return -SDP_INVALID_SYNTAX;
  389 + if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
  390 + seqlen = sdp_datalen(&req, &len);
  391 + if (seqlen < 3 || len < seqlen)
  392 + return -SDP_INVALID_SYNTAX;
  393 + len -= seqlen;
  394 +
  395 + while (seqlen)
  396 + if (sdp_svc_match(sdp, &req, &seqlen))
  397 + return -SDP_INVALID_SYNTAX;
  398 + } else if (sdp_svc_match(sdp, &req, &seqlen))
  399 + return -SDP_INVALID_SYNTAX;
  400 +
  401 + if (len < 3)
  402 + return -SDP_INVALID_SYNTAX;
  403 + max = (req[0] << 8) | req[1];
  404 + req += 2;
  405 + len -= 2;
  406 + if (max < 0x0007)
  407 + return -SDP_INVALID_SYNTAX;
  408 +
  409 + if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
  410 + seqlen = sdp_datalen(&req, &len);
  411 + if (seqlen < 3 || len < seqlen)
  412 + return -SDP_INVALID_SYNTAX;
  413 + len -= seqlen;
  414 +
  415 + while (seqlen)
  416 + if (sdp_svc_attr_match(sdp, &req, &seqlen))
  417 + return -SDP_INVALID_SYNTAX;
  418 + } else if (sdp_svc_attr_match(sdp, &req, &seqlen))
  419 + return -SDP_INVALID_SYNTAX;
  420 +
  421 + if (len < 1)
  422 + return -SDP_INVALID_SYNTAX;
  423 +
  424 + if (*req) {
  425 + if (len <= sizeof(int))
  426 + return -SDP_INVALID_SYNTAX;
  427 + len -= sizeof(int);
  428 + memcpy(&start, req + 1, sizeof(int));
  429 + } else
  430 + start = 0;
  431 +
  432 + if (len > 1)
  433 + return -SDP_INVALID_SYNTAX;
  434 +
  435 + /* Output the results */
  436 + /* This assumes empty attribute lists are never to be returned even
  437 + * for matching Service Records. In practice this shouldn't happen
  438 + * as the requestor will usually include the always present
  439 + * ServiceRecordHandle AttributeID in AttributeIDList. */
  440 + lst = rsp + 2;
  441 + max = MIN(max, MAX_RSP_PARAM_SIZE);
  442 + len = 3 - start;
  443 + end = 0;
  444 + for (i = 0; i < sdp->services; i ++)
  445 + if ((record = &sdp->service_list[i])->match) {
  446 + len += 3;
  447 + seqlen = len;
  448 + for (j = 0; j < record->attributes; j ++)
  449 + if (record->attribute_list[j].match) {
  450 + if (len >= 0)
  451 + if (len + record->attribute_list[j].len < max) {
  452 + memcpy(lst + len, record->attribute_list[j].pair,
  453 + record->attribute_list[j].len);
  454 + end = len + record->attribute_list[j].len;
  455 + }
  456 + len += record->attribute_list[j].len;
  457 + }
  458 + if (seqlen == len)
  459 + len -= 3;
  460 + else if (seqlen >= 3 && seqlen < max) {
  461 + lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
  462 + lst[seqlen - 2] = (len - seqlen) >> 8;
  463 + lst[seqlen - 1] = (len - seqlen) & 0xff;
  464 + }
  465 + }
  466 + if (len == 3 - start)
  467 + len -= 3;
  468 + else if (0 >= start) {
  469 + lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
  470 + lst[1] = (len + start - 3) >> 8;
  471 + lst[2] = (len + start - 3) & 0xff;
  472 + }
  473 +
  474 + rsp[0] = end >> 8;
  475 + rsp[1] = end & 0xff;
  476 +
  477 + if (end < len) {
  478 + len = end + start;
  479 + lst[end ++] = sizeof(int);
  480 + memcpy(lst + end, &len, sizeof(int));
  481 + end += sizeof(int);
  482 + } else
  483 + lst[end ++] = 0;
  484 +
  485 + return end + 2;
  486 +}
  487 +
  488 +static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len)
  489 +{
  490 + struct bt_l2cap_sdp_state_s *sdp = opaque;
  491 + enum bt_sdp_cmd pdu_id;
  492 + uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out;
  493 + int transaction_id, plen;
  494 + int err = 0;
  495 + int rsp_len = 0;
  496 +
  497 + if (len < 5) {
  498 + fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);
  499 + return;
  500 + }
  501 +
  502 + pdu_id = *data ++;
  503 + transaction_id = (data[0] << 8) | data[1];
  504 + plen = (data[2] << 8) | data[3];
  505 + data += 4;
  506 + len -= 5;
  507 +
  508 + if (len != plen) {
  509 + fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",
  510 + __FUNCTION__, plen, len);
  511 + err = SDP_INVALID_PDU_SIZE;
  512 + goto respond;
  513 + }
  514 +
  515 + switch (pdu_id) {
  516 + case SDP_SVC_SEARCH_REQ:
  517 + rsp_len = sdp_svc_search(sdp, rsp, data, len);
  518 + pdu_id = SDP_SVC_SEARCH_RSP;
  519 + break;
  520 +
  521 + case SDP_SVC_ATTR_REQ:
  522 + rsp_len = sdp_attr_get(sdp, rsp, data, len);
  523 + pdu_id = SDP_SVC_ATTR_RSP;
  524 + break;
  525 +
  526 + case SDP_SVC_SEARCH_ATTR_REQ:
  527 + rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len);
  528 + pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
  529 + break;
  530 +
  531 + case SDP_ERROR_RSP:
  532 + case SDP_SVC_ATTR_RSP:
  533 + case SDP_SVC_SEARCH_RSP:
  534 + case SDP_SVC_SEARCH_ATTR_RSP:
  535 + default:
  536 + fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n",
  537 + __FUNCTION__, pdu_id);
  538 + err = SDP_INVALID_SYNTAX;
  539 + break;
  540 + }
  541 +
  542 + if (rsp_len < 0) {
  543 + err = -rsp_len;
  544 + rsp_len = 0;
  545 + }
  546 +
  547 +respond:
  548 + if (err) {
  549 + pdu_id = SDP_ERROR_RSP;
  550 + rsp[rsp_len ++] = err >> 8;
  551 + rsp[rsp_len ++] = err & 0xff;
  552 + }
  553 +
  554 + sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE);
  555 +
  556 + sdu_out[0] = pdu_id;
  557 + sdu_out[1] = transaction_id >> 8;
  558 + sdu_out[2] = transaction_id & 0xff;
  559 + sdu_out[3] = rsp_len >> 8;
  560 + sdu_out[4] = rsp_len & 0xff;
  561 + memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len);
  562 +
  563 + sdp->channel->sdu_submit(sdp->channel);
  564 +}
  565 +
  566 +static void bt_l2cap_sdp_close_ch(void *opaque)
  567 +{
  568 + struct bt_l2cap_sdp_state_s *sdp = opaque;
  569 + int i;
  570 +
  571 + for (i = 0; i < sdp->services; i ++) {
  572 + qemu_free(sdp->service_list[i].attribute_list->pair);
  573 + qemu_free(sdp->service_list[i].attribute_list);
  574 + qemu_free(sdp->service_list[i].uuid);
  575 + }
  576 + qemu_free(sdp->service_list);
  577 + qemu_free(sdp);
  578 +}
  579 +
  580 +struct sdp_def_service_s {
  581 + uint16_t class_uuid;
  582 + struct sdp_def_attribute_s {
  583 + uint16_t id;
  584 + struct sdp_def_data_element_s {
  585 + uint8_t type;
  586 + union {
  587 + uint32_t uint;
  588 + const char *str;
  589 + struct sdp_def_data_element_s *list;
  590 + } value;
  591 + } data;
  592 + } attributes[];
  593 +};
  594 +
  595 +/* Calculate a safe byte count to allocate that will store the given
  596 + * element, at the same time count elements of a UUID type. */
  597 +static int sdp_attr_max_size(struct sdp_def_data_element_s *element,
  598 + int *uuids)
  599 +{
  600 + int type = element->type & ~SDP_DSIZE_MASK;
  601 + int len;
  602 +
  603 + if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID ||
  604 + type == SDP_DTYPE_BOOL) {
  605 + if (type == SDP_DTYPE_UUID)
  606 + (*uuids) ++;
  607 + return 1 + (1 << (element->type & SDP_DSIZE_MASK));
  608 + }
  609 +
  610 + if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
  611 + if (element->type & SDP_DSIZE_MASK) {
  612 + for (len = 0; element->value.str[len] |
  613 + element->value.str[len + 1]; len ++);
  614 + return len;
  615 + } else
  616 + return 2 + strlen(element->value.str);
  617 + }
  618 +
  619 + if (type != SDP_DTYPE_SEQ)
  620 + exit(-1);
  621 + len = 2;
  622 + element = element->value.list;
  623 + while (element->type)
  624 + len += sdp_attr_max_size(element ++, uuids);
  625 + if (len > 255)
  626 + exit (-1);
  627 +
  628 + return len;
  629 +}
  630 +
  631 +static int sdp_attr_write(uint8_t *data,
  632 + struct sdp_def_data_element_s *element, int **uuid)
  633 +{
  634 + int type = element->type & ~SDP_DSIZE_MASK;
  635 + int len = 0;
  636 +
  637 + if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) {
  638 + data[len ++] = element->type;
  639 + if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1)
  640 + data[len ++] = (element->value.uint >> 0) & 0xff;
  641 + else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) {
  642 + data[len ++] = (element->value.uint >> 8) & 0xff;
  643 + data[len ++] = (element->value.uint >> 0) & 0xff;
  644 + } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) {
  645 + data[len ++] = (element->value.uint >> 24) & 0xff;
  646 + data[len ++] = (element->value.uint >> 16) & 0xff;
  647 + data[len ++] = (element->value.uint >> 8) & 0xff;
  648 + data[len ++] = (element->value.uint >> 0) & 0xff;
  649 + }
  650 +
  651 + return len;
  652 + }
  653 +
  654 + if (type == SDP_DTYPE_UUID) {
  655 + *(*uuid) ++ = element->value.uint;
  656 +
  657 + data[len ++] = element->type;
  658 + data[len ++] = (element->value.uint >> 24) & 0xff;
  659 + data[len ++] = (element->value.uint >> 16) & 0xff;
  660 + data[len ++] = (element->value.uint >> 8) & 0xff;
  661 + data[len ++] = (element->value.uint >> 0) & 0xff;
  662 + memcpy(data + len, bt_base_uuid, 12);
  663 +
  664 + return len + 12;
  665 + }
  666 +
  667 + data[0] = type | SDP_DSIZE_NEXT1;
  668 + if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
  669 + if (element->type & SDP_DSIZE_MASK)
  670 + for (len = 0; element->value.str[len] |
  671 + element->value.str[len + 1]; len ++);
  672 + else
  673 + len = strlen(element->value.str);
  674 + memcpy(data + 2, element->value.str, data[1] = len);
  675 +
  676 + return len + 2;
  677 + }
  678 +
  679 + len = 2;
  680 + element = element->value.list;
  681 + while (element->type)
  682 + len += sdp_attr_write(data + len, element ++, uuid);
  683 + data[1] = len - 2;
  684 +
  685 + return len;
  686 +}
  687 +
  688 +static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a,
  689 + const struct sdp_service_attribute_s *b)
  690 +{
  691 + return (int) b->attribute_id - a->attribute_id;
  692 +}
  693 +
  694 +static int sdp_uuid_compare(const int *a, const int *b)
  695 +{
  696 + return *a - *b;
  697 +}
  698 +
  699 +static void sdp_service_record_build(struct sdp_service_record_s *record,
  700 + struct sdp_def_service_s *def, int handle)
  701 +{
  702 + int len = 0;
  703 + uint8_t *data;
  704 + int *uuid;
  705 +
  706 + record->uuids = 0;
  707 + while (def->attributes[record->attributes].data.type) {
  708 + len += 3;
  709 + len += sdp_attr_max_size(&def->attributes[record->attributes ++].data,
  710 + &record->uuids);
  711 + }
  712 + record->uuids = 1 << ffs(record->uuids - 1);
  713 + record->attribute_list =
  714 + qemu_mallocz(record->attributes * sizeof(*record->attribute_list));
  715 + record->uuid =
  716 + qemu_mallocz(record->uuids * sizeof(*record->uuid));
  717 + data = qemu_malloc(len);
  718 +
  719 + record->attributes = 0;
  720 + uuid = record->uuid;
  721 + while (def->attributes[record->attributes].data.type) {
  722 + record->attribute_list[record->attributes].pair = data;
  723 +
  724 + len = 0;
  725 + data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2;
  726 + data[len ++] = def->attributes[record->attributes].id >> 8;
  727 + data[len ++] = def->attributes[record->attributes].id & 0xff;
  728 + len += sdp_attr_write(data + len,
  729 + &def->attributes[record->attributes].data, &uuid);
  730 +
  731 + /* Special case: assign a ServiceRecordHandle in sequence */
  732 + if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE)
  733 + def->attributes[record->attributes].data.value.uint = handle;
  734 + /* Note: we could also assign a ServiceDescription based on
  735 + * sdp->device.device->lmp_name. */
  736 +
  737 + record->attribute_list[record->attributes ++].len = len;
  738 + data += len;
  739 + }
  740 +
  741 + /* Sort the attribute list by the AttributeID */
  742 + qsort(record->attribute_list, record->attributes,
  743 + sizeof(*record->attribute_list),
  744 + (void *) sdp_attributeid_compare);
  745 + /* Sort the searchable UUIDs list for bisection */
  746 + qsort(record->uuid, record->uuids,
  747 + sizeof(*record->uuid),
  748 + (void *) sdp_uuid_compare);
  749 +}
  750 +
  751 +static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp,
  752 + struct sdp_def_service_s **service)
  753 +{
  754 + sdp->services = 0;
  755 + while (service[sdp->services])
  756 + sdp->services ++;
  757 + sdp->service_list =
  758 + qemu_mallocz(sdp->services * sizeof(*sdp->service_list));
  759 +
  760 + sdp->services = 0;
  761 + while (*service) {
  762 + sdp_service_record_build(&sdp->service_list[sdp->services],
  763 + *service, sdp->services);
  764 + service ++;
  765 + sdp->services ++;
  766 + }
  767 +}
  768 +
  769 +#define LAST { .type = 0 }
  770 +#define SERVICE(name, attrs) \
  771 + static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \
  772 + .attributes = { attrs { .data = LAST } }, \
  773 + };
  774 +#define ATTRIBUTE(attrid, val) { .id = glue(SDP_ATTR_, attrid), .data = val },
  775 +#define UINT8(val) { \
  776 + .type = SDP_DTYPE_UINT | SDP_DSIZE_1, \
  777 + .value.uint = val, \
  778 + },
  779 +#define UINT16(val) { \
  780 + .type = SDP_DTYPE_UINT | SDP_DSIZE_2, \
  781 + .value.uint = val, \
  782 + },
  783 +#define UINT32(val) { \
  784 + .type = SDP_DTYPE_UINT | SDP_DSIZE_4, \
  785 + .value.uint = val, \
  786 + },
  787 +#define UUID128(val) { \
  788 + .type = SDP_DTYPE_UUID | SDP_DSIZE_16, \
  789 + .value.uint = val, \
  790 + },
  791 +#define TRUE { \
  792 + .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \
  793 + .value.uint = 1, \
  794 + },
  795 +#define FALSE { \
  796 + .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \
  797 + .value.uint = 0, \
  798 + },
  799 +#define STRING(val) { \
  800 + .type = SDP_DTYPE_STRING, \
  801 + .value.str = val, \
  802 + },
  803 +#define ARRAY(...) { \
  804 + .type = SDP_DTYPE_STRING | SDP_DSIZE_2, \
  805 + .value.str = (char []) { __VA_ARGS__, 0, 0 }, \
  806 + },
  807 +#define URL(val) { \
  808 + .type = SDP_DTYPE_URL, \
  809 + .value.str = val, \
  810 + },
  811 +#if 1
  812 +#define LIST(val) { \
  813 + .type = SDP_DTYPE_SEQ, \
  814 + .value.list = (struct sdp_def_data_element_s []) { val LAST }, \
  815 + },
  816 +#endif
  817 +
  818 +/* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes
  819 + * in resulting SDP data representation size. */
  820 +
  821 +SERVICE(hid,
  822 + ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */
  823 + ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID)))
  824 + ATTRIBUTE(RECORD_STATE, UINT32(1))
  825 + ATTRIBUTE(PROTO_DESC_LIST, LIST(
  826 + LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL))
  827 + LIST(UUID128(HIDP_UUID))
  828 + ))
  829 + ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
  830 + ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
  831 + UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
  832 + ))
  833 + ATTRIBUTE(PFILE_DESC_LIST, LIST(
  834 + LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100))
  835 + ))
  836 + ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
  837 + ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID"))
  838 + ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse"))
  839 + ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
  840 +
  841 + /* Profile specific */
  842 + ATTRIBUTE(DEVICE_RELEASE_NUMBER, UINT16(0x0091)) /* Deprecated, remove */
  843 + ATTRIBUTE(PARSER_VERSION, UINT16(0x0111))
  844 + /* TODO: extract from l2cap_device->device.class[0] */
  845 + ATTRIBUTE(DEVICE_SUBCLASS, UINT8(0x40))
  846 + ATTRIBUTE(COUNTRY_CODE, UINT8(0x15))
  847 + ATTRIBUTE(VIRTUAL_CABLE, TRUE)
  848 + ATTRIBUTE(RECONNECT_INITIATE, FALSE)
  849 + /* TODO: extract from hid->usbdev->report_desc */
  850 + ATTRIBUTE(DESCRIPTOR_LIST, LIST(
  851 + LIST(UINT8(0x22) ARRAY(
  852 + 0x05, 0x01, /* Usage Page (Generic Desktop) */
  853 + 0x09, 0x06, /* Usage (Keyboard) */
  854 + 0xa1, 0x01, /* Collection (Application) */
  855 + 0x75, 0x01, /* Report Size (1) */
  856 + 0x95, 0x08, /* Report Count (8) */
  857 + 0x05, 0x07, /* Usage Page (Key Codes) */
  858 + 0x19, 0xe0, /* Usage Minimum (224) */
  859 + 0x29, 0xe7, /* Usage Maximum (231) */
  860 + 0x15, 0x00, /* Logical Minimum (0) */
  861 + 0x25, 0x01, /* Logical Maximum (1) */
  862 + 0x81, 0x02, /* Input (Data, Variable, Absolute) */
  863 + 0x95, 0x01, /* Report Count (1) */
  864 + 0x75, 0x08, /* Report Size (8) */
  865 + 0x81, 0x01, /* Input (Constant) */
  866 + 0x95, 0x05, /* Report Count (5) */
  867 + 0x75, 0x01, /* Report Size (1) */
  868 + 0x05, 0x08, /* Usage Page (LEDs) */
  869 + 0x19, 0x01, /* Usage Minimum (1) */
  870 + 0x29, 0x05, /* Usage Maximum (5) */
  871 + 0x91, 0x02, /* Output (Data, Variable, Absolute) */
  872 + 0x95, 0x01, /* Report Count (1) */
  873 + 0x75, 0x03, /* Report Size (3) */
  874 + 0x91, 0x01, /* Output (Constant) */
  875 + 0x95, 0x06, /* Report Count (6) */
  876 + 0x75, 0x08, /* Report Size (8) */
  877 + 0x15, 0x00, /* Logical Minimum (0) */
  878 + 0x25, 0xff, /* Logical Maximum (255) */
  879 + 0x05, 0x07, /* Usage Page (Key Codes) */
  880 + 0x19, 0x00, /* Usage Minimum (0) */
  881 + 0x29, 0xff, /* Usage Maximum (255) */
  882 + 0x81, 0x00, /* Input (Data, Array) */
  883 + 0xc0 /* End Collection */
  884 + ))))
  885 + ATTRIBUTE(LANG_ID_BASE_LIST, LIST(
  886 + LIST(UINT16(0x0409) UINT16(0x0100))
  887 + ))
  888 + ATTRIBUTE(SDP_DISABLE, FALSE)
  889 + ATTRIBUTE(BATTERY_POWER, TRUE)
  890 + ATTRIBUTE(REMOTE_WAKEUP, TRUE)
  891 + ATTRIBUTE(BOOT_DEVICE, TRUE) /* XXX: untested */
  892 + ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80))
  893 + ATTRIBUTE(NORMALLY_CONNECTABLE, TRUE)
  894 + ATTRIBUTE(PROFILE_VERSION, UINT16(0x0100))
  895 +)
  896 +
  897 +SERVICE(sdp,
  898 + ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */
  899 + ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID)))
  900 + ATTRIBUTE(RECORD_STATE, UINT32(1))
  901 + ATTRIBUTE(PROTO_DESC_LIST, LIST(
  902 + LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
  903 + LIST(UUID128(SDP_UUID))
  904 + ))
  905 + ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
  906 + ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
  907 + UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
  908 + ))
  909 + ATTRIBUTE(PFILE_DESC_LIST, LIST(
  910 + LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100))
  911 + ))
  912 + ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
  913 + ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
  914 +
  915 + /* Profile specific */
  916 + ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100)))
  917 + ATTRIBUTE(SVCDB_STATE , UINT32(1))
  918 +)
  919 +
  920 +SERVICE(pnp,
  921 + ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */
  922 + ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID)))
  923 + ATTRIBUTE(RECORD_STATE, UINT32(1))
  924 + ATTRIBUTE(PROTO_DESC_LIST, LIST(
  925 + LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
  926 + LIST(UUID128(SDP_UUID))
  927 + ))
  928 + ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
  929 + ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
  930 + UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
  931 + ))
  932 + ATTRIBUTE(PFILE_DESC_LIST, LIST(
  933 + LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100))
  934 + ))
  935 + ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
  936 + ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
  937 +
  938 + /* Profile specific */
  939 + ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100))
  940 + ATTRIBUTE(VERSION, UINT16(0x0100))
  941 + ATTRIBUTE(PRIMARY_RECORD, TRUE)
  942 +)
  943 +
  944 +static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev,
  945 + struct bt_l2cap_conn_params_s *params)
  946 +{
  947 + struct bt_l2cap_sdp_state_s *sdp = qemu_mallocz(sizeof(*sdp));
  948 + struct sdp_def_service_s *services[] = {
  949 + &sdp_service_sdp_s,
  950 + &sdp_service_hid_s,
  951 + &sdp_service_pnp_s,
  952 + 0,
  953 + };
  954 +
  955 + sdp->channel = params;
  956 + sdp->channel->opaque = sdp;
  957 + sdp->channel->close = bt_l2cap_sdp_close_ch;
  958 + sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in;
  959 +
  960 + sdp_service_db_build(sdp, services);
  961 +
  962 + return 0;
  963 +}
  964 +
  965 +void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev)
  966 +{
  967 + bt_l2cap_psm_register(dev, BT_PSM_SDP,
  968 + MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch);
  969 +}
@@ -124,6 +124,49 @@ enum { @@ -124,6 +124,49 @@ enum {
124 qemu_irq *csrhci_pins_get(CharDriverState *chr); 124 qemu_irq *csrhci_pins_get(CharDriverState *chr);
125 CharDriverState *uart_hci_init(qemu_irq wakeup); 125 CharDriverState *uart_hci_init(qemu_irq wakeup);
126 126
  127 +/* bt-l2cap.c */
  128 +struct bt_l2cap_device_s;
  129 +struct bt_l2cap_conn_params_s;
  130 +struct bt_l2cap_psm_s;
  131 +void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
  132 + struct bt_scatternet_s *net);
  133 +void bt_l2cap_device_done(struct bt_l2cap_device_s *dev);
  134 +void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm,
  135 + int min_mtu, int (*new_channel)(struct bt_l2cap_device_s *dev,
  136 + struct bt_l2cap_conn_params_s *params));
  137 +
  138 +struct bt_l2cap_device_s {
  139 + struct bt_device_s device;
  140 + struct bt_l2cap_psm_s *first_psm;
  141 +};
  142 +
  143 +struct bt_l2cap_conn_params_s {
  144 + /* Input */
  145 + uint8_t *(*sdu_out)(struct bt_l2cap_conn_params_s *chan, int len);
  146 + void (*sdu_submit)(struct bt_l2cap_conn_params_s *chan);
  147 + int remote_mtu;
  148 + /* Output */
  149 + void *opaque;
  150 + void (*sdu_in)(void *opaque, const uint8_t *data, int len);
  151 + void (*close)(void *opaque);
  152 +};
  153 +
  154 +enum bt_l2cap_psm_predef {
  155 + BT_PSM_SDP = 0x0001,
  156 + BT_PSM_RFCOMM = 0x0003,
  157 + BT_PSM_TELEPHONY = 0x0005,
  158 + BT_PSM_TCS = 0x0007,
  159 + BT_PSM_BNEP = 0x000f,
  160 + BT_PSM_HID_CTRL = 0x0011,
  161 + BT_PSM_HID_INTR = 0x0013,
  162 + BT_PSM_UPNP = 0x0015,
  163 + BT_PSM_AVCTP = 0x0017,
  164 + BT_PSM_AVDTP = 0x0019,
  165 +};
  166 +
  167 +/* bt-sdp.c */
  168 +void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev);
  169 +
127 /* Link Management Protocol layer defines */ 170 /* Link Management Protocol layer defines */
128 171
129 #define LLID_ACLU_CONT 0x1 172 #define LLID_ACLU_CONT 0x1
@@ -1626,3 +1669,512 @@ struct hci_sco_hdr { @@ -1626,3 +1669,512 @@ struct hci_sco_hdr {
1626 uint16_t handle; 1669 uint16_t handle;
1627 uint8_t dlen; 1670 uint8_t dlen;
1628 } __attribute__ ((packed)); 1671 } __attribute__ ((packed));
  1672 +
  1673 +/* L2CAP layer defines */
  1674 +
  1675 +enum bt_l2cap_lm_bits {
  1676 + L2CAP_LM_MASTER = 1 << 0,
  1677 + L2CAP_LM_AUTH = 1 << 1,
  1678 + L2CAP_LM_ENCRYPT = 1 << 2,
  1679 + L2CAP_LM_TRUSTED = 1 << 3,
  1680 + L2CAP_LM_RELIABLE = 1 << 4,
  1681 + L2CAP_LM_SECURE = 1 << 5,
  1682 +};
  1683 +
  1684 +enum bt_l2cap_cid_predef {
  1685 + L2CAP_CID_INVALID = 0x0000,
  1686 + L2CAP_CID_SIGNALLING= 0x0001,
  1687 + L2CAP_CID_GROUP = 0x0002,
  1688 + L2CAP_CID_ALLOC = 0x0040,
  1689 +};
  1690 +
  1691 +/* L2CAP command codes */
  1692 +enum bt_l2cap_cmd {
  1693 + L2CAP_COMMAND_REJ = 1,
  1694 + L2CAP_CONN_REQ,
  1695 + L2CAP_CONN_RSP,
  1696 + L2CAP_CONF_REQ,
  1697 + L2CAP_CONF_RSP,
  1698 + L2CAP_DISCONN_REQ,
  1699 + L2CAP_DISCONN_RSP,
  1700 + L2CAP_ECHO_REQ,
  1701 + L2CAP_ECHO_RSP,
  1702 + L2CAP_INFO_REQ,
  1703 + L2CAP_INFO_RSP,
  1704 +};
  1705 +
  1706 +enum bt_l2cap_sar_bits {
  1707 + L2CAP_SAR_NO_SEG = 0,
  1708 + L2CAP_SAR_START,
  1709 + L2CAP_SAR_END,
  1710 + L2CAP_SAR_CONT,
  1711 +};
  1712 +
  1713 +/* L2CAP structures */
  1714 +typedef struct {
  1715 + uint16_t len;
  1716 + uint16_t cid;
  1717 + uint8_t data[0];
  1718 +} __attribute__ ((packed)) l2cap_hdr;
  1719 +#define L2CAP_HDR_SIZE 4
  1720 +
  1721 +typedef struct {
  1722 + uint8_t code;
  1723 + uint8_t ident;
  1724 + uint16_t len;
  1725 +} __attribute__ ((packed)) l2cap_cmd_hdr;
  1726 +#define L2CAP_CMD_HDR_SIZE 4
  1727 +
  1728 +typedef struct {
  1729 + uint16_t reason;
  1730 +} __attribute__ ((packed)) l2cap_cmd_rej;
  1731 +#define L2CAP_CMD_REJ_SIZE 2
  1732 +
  1733 +typedef struct {
  1734 + uint16_t dcid;
  1735 + uint16_t scid;
  1736 +} __attribute__ ((packed)) l2cap_cmd_rej_cid;
  1737 +#define L2CAP_CMD_REJ_CID_SIZE 4
  1738 +
  1739 +/* reject reason */
  1740 +enum bt_l2cap_rej_reason {
  1741 + L2CAP_REJ_CMD_NOT_UNDERSTOOD = 0,
  1742 + L2CAP_REJ_SIG_TOOBIG,
  1743 + L2CAP_REJ_CID_INVAL,
  1744 +};
  1745 +
  1746 +typedef struct {
  1747 + uint16_t psm;
  1748 + uint16_t scid;
  1749 +} __attribute__ ((packed)) l2cap_conn_req;
  1750 +#define L2CAP_CONN_REQ_SIZE 4
  1751 +
  1752 +typedef struct {
  1753 + uint16_t dcid;
  1754 + uint16_t scid;
  1755 + uint16_t result;
  1756 + uint16_t status;
  1757 +} __attribute__ ((packed)) l2cap_conn_rsp;
  1758 +#define L2CAP_CONN_RSP_SIZE 8
  1759 +
  1760 +/* connect result */
  1761 +enum bt_l2cap_conn_res {
  1762 + L2CAP_CR_SUCCESS = 0,
  1763 + L2CAP_CR_PEND,
  1764 + L2CAP_CR_BAD_PSM,
  1765 + L2CAP_CR_SEC_BLOCK,
  1766 + L2CAP_CR_NO_MEM,
  1767 +};
  1768 +
  1769 +/* connect status */
  1770 +enum bt_l2cap_conn_stat {
  1771 + L2CAP_CS_NO_INFO = 0,
  1772 + L2CAP_CS_AUTHEN_PEND,
  1773 + L2CAP_CS_AUTHOR_PEND,
  1774 +};
  1775 +
  1776 +typedef struct {
  1777 + uint16_t dcid;
  1778 + uint16_t flags;
  1779 + uint8_t data[0];
  1780 +} __attribute__ ((packed)) l2cap_conf_req;
  1781 +#define L2CAP_CONF_REQ_SIZE(datalen) (4 + (datalen))
  1782 +
  1783 +typedef struct {
  1784 + uint16_t scid;
  1785 + uint16_t flags;
  1786 + uint16_t result;
  1787 + uint8_t data[0];
  1788 +} __attribute__ ((packed)) l2cap_conf_rsp;
  1789 +#define L2CAP_CONF_RSP_SIZE(datalen) (6 + datalen)
  1790 +
  1791 +enum bt_l2cap_conf_res {
  1792 + L2CAP_CONF_SUCCESS = 0,
  1793 + L2CAP_CONF_UNACCEPT,
  1794 + L2CAP_CONF_REJECT,
  1795 + L2CAP_CONF_UNKNOWN,
  1796 +};
  1797 +
  1798 +typedef struct {
  1799 + uint8_t type;
  1800 + uint8_t len;
  1801 + uint8_t val[0];
  1802 +} __attribute__ ((packed)) l2cap_conf_opt;
  1803 +#define L2CAP_CONF_OPT_SIZE 2
  1804 +
  1805 +enum bt_l2cap_conf_val {
  1806 + L2CAP_CONF_MTU = 1,
  1807 + L2CAP_CONF_FLUSH_TO,
  1808 + L2CAP_CONF_QOS,
  1809 + L2CAP_CONF_RFC,
  1810 + L2CAP_CONF_RFC_MODE = L2CAP_CONF_RFC,
  1811 +};
  1812 +
  1813 +typedef struct {
  1814 + uint8_t flags;
  1815 + uint8_t service_type;
  1816 + uint32_t token_rate;
  1817 + uint32_t token_bucket_size;
  1818 + uint32_t peak_bandwidth;
  1819 + uint32_t latency;
  1820 + uint32_t delay_variation;
  1821 +} __attribute__ ((packed)) l2cap_conf_opt_qos;
  1822 +#define L2CAP_CONF_OPT_QOS_SIZE 22
  1823 +
  1824 +enum bt_l2cap_conf_opt_qos_st {
  1825 + L2CAP_CONF_QOS_NO_TRAFFIC = 0x00,
  1826 + L2CAP_CONF_QOS_BEST_EFFORT,
  1827 + L2CAP_CONF_QOS_GUARANTEED,
  1828 +};
  1829 +
  1830 +#define L2CAP_CONF_QOS_WILDCARD 0xffffffff
  1831 +
  1832 +enum bt_l2cap_mode {
  1833 + L2CAP_MODE_BASIC = 0,
  1834 + L2CAP_MODE_RETRANS = 1,
  1835 + L2CAP_MODE_FLOWCTL = 2,
  1836 +};
  1837 +
  1838 +typedef struct {
  1839 + uint16_t dcid;
  1840 + uint16_t scid;
  1841 +} __attribute__ ((packed)) l2cap_disconn_req;
  1842 +#define L2CAP_DISCONN_REQ_SIZE 4
  1843 +
  1844 +typedef struct {
  1845 + uint16_t dcid;
  1846 + uint16_t scid;
  1847 +} __attribute__ ((packed)) l2cap_disconn_rsp;
  1848 +#define L2CAP_DISCONN_RSP_SIZE 4
  1849 +
  1850 +typedef struct {
  1851 + uint16_t type;
  1852 +} __attribute__ ((packed)) l2cap_info_req;
  1853 +#define L2CAP_INFO_REQ_SIZE 2
  1854 +
  1855 +typedef struct {
  1856 + uint16_t type;
  1857 + uint16_t result;
  1858 + uint8_t data[0];
  1859 +} __attribute__ ((packed)) l2cap_info_rsp;
  1860 +#define L2CAP_INFO_RSP_SIZE 4
  1861 +
  1862 +/* info type */
  1863 +enum bt_l2cap_info_type {
  1864 + L2CAP_IT_CL_MTU = 1,
  1865 + L2CAP_IT_FEAT_MASK,
  1866 +};
  1867 +
  1868 +/* info result */
  1869 +enum bt_l2cap_info_result {
  1870 + L2CAP_IR_SUCCESS = 0,
  1871 + L2CAP_IR_NOTSUPP,
  1872 +};
  1873 +
  1874 +/* Service Discovery Protocol defines */
  1875 +/* Note that all multibyte values in lower layer protocols (above in this file)
  1876 + * are little-endian while SDP is big-endian. */
  1877 +
  1878 +/* Protocol UUIDs */
  1879 +enum sdp_proto_uuid {
  1880 + SDP_UUID = 0x0001,
  1881 + UDP_UUID = 0x0002,
  1882 + RFCOMM_UUID = 0x0003,
  1883 + TCP_UUID = 0x0004,
  1884 + TCS_BIN_UUID = 0x0005,
  1885 + TCS_AT_UUID = 0x0006,
  1886 + OBEX_UUID = 0x0008,
  1887 + IP_UUID = 0x0009,
  1888 + FTP_UUID = 0x000a,
  1889 + HTTP_UUID = 0x000c,
  1890 + WSP_UUID = 0x000e,
  1891 + BNEP_UUID = 0x000f,
  1892 + UPNP_UUID = 0x0010,
  1893 + HIDP_UUID = 0x0011,
  1894 + HCRP_CTRL_UUID = 0x0012,
  1895 + HCRP_DATA_UUID = 0x0014,
  1896 + HCRP_NOTE_UUID = 0x0016,
  1897 + AVCTP_UUID = 0x0017,
  1898 + AVDTP_UUID = 0x0019,
  1899 + CMTP_UUID = 0x001b,
  1900 + UDI_UUID = 0x001d,
  1901 + MCAP_CTRL_UUID = 0x001e,
  1902 + MCAP_DATA_UUID = 0x001f,
  1903 + L2CAP_UUID = 0x0100,
  1904 +};
  1905 +
  1906 +/*
  1907 + * Service class identifiers of standard services and service groups
  1908 + */
  1909 +enum service_class_id {
  1910 + SDP_SERVER_SVCLASS_ID = 0x1000,
  1911 + BROWSE_GRP_DESC_SVCLASS_ID = 0x1001,
  1912 + PUBLIC_BROWSE_GROUP = 0x1002,
  1913 + SERIAL_PORT_SVCLASS_ID = 0x1101,
  1914 + LAN_ACCESS_SVCLASS_ID = 0x1102,
  1915 + DIALUP_NET_SVCLASS_ID = 0x1103,
  1916 + IRMC_SYNC_SVCLASS_ID = 0x1104,
  1917 + OBEX_OBJPUSH_SVCLASS_ID = 0x1105,
  1918 + OBEX_FILETRANS_SVCLASS_ID = 0x1106,
  1919 + IRMC_SYNC_CMD_SVCLASS_ID = 0x1107,
  1920 + HEADSET_SVCLASS_ID = 0x1108,
  1921 + CORDLESS_TELEPHONY_SVCLASS_ID = 0x1109,
  1922 + AUDIO_SOURCE_SVCLASS_ID = 0x110a,
  1923 + AUDIO_SINK_SVCLASS_ID = 0x110b,
  1924 + AV_REMOTE_TARGET_SVCLASS_ID = 0x110c,
  1925 + ADVANCED_AUDIO_SVCLASS_ID = 0x110d,
  1926 + AV_REMOTE_SVCLASS_ID = 0x110e,
  1927 + VIDEO_CONF_SVCLASS_ID = 0x110f,
  1928 + INTERCOM_SVCLASS_ID = 0x1110,
  1929 + FAX_SVCLASS_ID = 0x1111,
  1930 + HEADSET_AGW_SVCLASS_ID = 0x1112,
  1931 + WAP_SVCLASS_ID = 0x1113,
  1932 + WAP_CLIENT_SVCLASS_ID = 0x1114,
  1933 + PANU_SVCLASS_ID = 0x1115,
  1934 + NAP_SVCLASS_ID = 0x1116,
  1935 + GN_SVCLASS_ID = 0x1117,
  1936 + DIRECT_PRINTING_SVCLASS_ID = 0x1118,
  1937 + REFERENCE_PRINTING_SVCLASS_ID = 0x1119,
  1938 + IMAGING_SVCLASS_ID = 0x111a,
  1939 + IMAGING_RESPONDER_SVCLASS_ID = 0x111b,
  1940 + IMAGING_ARCHIVE_SVCLASS_ID = 0x111c,
  1941 + IMAGING_REFOBJS_SVCLASS_ID = 0x111d,
  1942 + HANDSFREE_SVCLASS_ID = 0x111e,
  1943 + HANDSFREE_AGW_SVCLASS_ID = 0x111f,
  1944 + DIRECT_PRT_REFOBJS_SVCLASS_ID = 0x1120,
  1945 + REFLECTED_UI_SVCLASS_ID = 0x1121,
  1946 + BASIC_PRINTING_SVCLASS_ID = 0x1122,
  1947 + PRINTING_STATUS_SVCLASS_ID = 0x1123,
  1948 + HID_SVCLASS_ID = 0x1124,
  1949 + HCR_SVCLASS_ID = 0x1125,
  1950 + HCR_PRINT_SVCLASS_ID = 0x1126,
  1951 + HCR_SCAN_SVCLASS_ID = 0x1127,
  1952 + CIP_SVCLASS_ID = 0x1128,
  1953 + VIDEO_CONF_GW_SVCLASS_ID = 0x1129,
  1954 + UDI_MT_SVCLASS_ID = 0x112a,
  1955 + UDI_TA_SVCLASS_ID = 0x112b,
  1956 + AV_SVCLASS_ID = 0x112c,
  1957 + SAP_SVCLASS_ID = 0x112d,
  1958 + PBAP_PCE_SVCLASS_ID = 0x112e,
  1959 + PBAP_PSE_SVCLASS_ID = 0x112f,
  1960 + PBAP_SVCLASS_ID = 0x1130,
  1961 + PNP_INFO_SVCLASS_ID = 0x1200,
  1962 + GENERIC_NETWORKING_SVCLASS_ID = 0x1201,
  1963 + GENERIC_FILETRANS_SVCLASS_ID = 0x1202,
  1964 + GENERIC_AUDIO_SVCLASS_ID = 0x1203,
  1965 + GENERIC_TELEPHONY_SVCLASS_ID = 0x1204,
  1966 + UPNP_SVCLASS_ID = 0x1205,
  1967 + UPNP_IP_SVCLASS_ID = 0x1206,
  1968 + UPNP_PAN_SVCLASS_ID = 0x1300,
  1969 + UPNP_LAP_SVCLASS_ID = 0x1301,
  1970 + UPNP_L2CAP_SVCLASS_ID = 0x1302,
  1971 + VIDEO_SOURCE_SVCLASS_ID = 0x1303,
  1972 + VIDEO_SINK_SVCLASS_ID = 0x1304,
  1973 + VIDEO_DISTRIBUTION_SVCLASS_ID = 0x1305,
  1974 + MDP_SVCLASS_ID = 0x1400,
  1975 + MDP_SOURCE_SVCLASS_ID = 0x1401,
  1976 + MDP_SINK_SVCLASS_ID = 0x1402,
  1977 + APPLE_AGENT_SVCLASS_ID = 0x2112,
  1978 +};
  1979 +
  1980 +/*
  1981 + * Standard profile descriptor identifiers; note these
  1982 + * may be identical to some of the service classes defined above
  1983 + */
  1984 +#define SDP_SERVER_PROFILE_ID SDP_SERVER_SVCLASS_ID
  1985 +#define BROWSE_GRP_DESC_PROFILE_ID BROWSE_GRP_DESC_SVCLASS_ID
  1986 +#define SERIAL_PORT_PROFILE_ID SERIAL_PORT_SVCLASS_ID
  1987 +#define LAN_ACCESS_PROFILE_ID LAN_ACCESS_SVCLASS_ID
  1988 +#define DIALUP_NET_PROFILE_ID DIALUP_NET_SVCLASS_ID
  1989 +#define IRMC_SYNC_PROFILE_ID IRMC_SYNC_SVCLASS_ID
  1990 +#define OBEX_OBJPUSH_PROFILE_ID OBEX_OBJPUSH_SVCLASS_ID
  1991 +#define OBEX_FILETRANS_PROFILE_ID OBEX_FILETRANS_SVCLASS_ID
  1992 +#define IRMC_SYNC_CMD_PROFILE_ID IRMC_SYNC_CMD_SVCLASS_ID
  1993 +#define HEADSET_PROFILE_ID HEADSET_SVCLASS_ID
  1994 +#define CORDLESS_TELEPHONY_PROFILE_ID CORDLESS_TELEPHONY_SVCLASS_ID
  1995 +#define AUDIO_SOURCE_PROFILE_ID AUDIO_SOURCE_SVCLASS_ID
  1996 +#define AUDIO_SINK_PROFILE_ID AUDIO_SINK_SVCLASS_ID
  1997 +#define AV_REMOTE_TARGET_PROFILE_ID AV_REMOTE_TARGET_SVCLASS_ID
  1998 +#define ADVANCED_AUDIO_PROFILE_ID ADVANCED_AUDIO_SVCLASS_ID
  1999 +#define AV_REMOTE_PROFILE_ID AV_REMOTE_SVCLASS_ID
  2000 +#define VIDEO_CONF_PROFILE_ID VIDEO_CONF_SVCLASS_ID
  2001 +#define INTERCOM_PROFILE_ID INTERCOM_SVCLASS_ID
  2002 +#define FAX_PROFILE_ID FAX_SVCLASS_ID
  2003 +#define HEADSET_AGW_PROFILE_ID HEADSET_AGW_SVCLASS_ID
  2004 +#define WAP_PROFILE_ID WAP_SVCLASS_ID
  2005 +#define WAP_CLIENT_PROFILE_ID WAP_CLIENT_SVCLASS_ID
  2006 +#define PANU_PROFILE_ID PANU_SVCLASS_ID
  2007 +#define NAP_PROFILE_ID NAP_SVCLASS_ID
  2008 +#define GN_PROFILE_ID GN_SVCLASS_ID
  2009 +#define DIRECT_PRINTING_PROFILE_ID DIRECT_PRINTING_SVCLASS_ID
  2010 +#define REFERENCE_PRINTING_PROFILE_ID REFERENCE_PRINTING_SVCLASS_ID
  2011 +#define IMAGING_PROFILE_ID IMAGING_SVCLASS_ID
  2012 +#define IMAGING_RESPONDER_PROFILE_ID IMAGING_RESPONDER_SVCLASS_ID
  2013 +#define IMAGING_ARCHIVE_PROFILE_ID IMAGING_ARCHIVE_SVCLASS_ID
  2014 +#define IMAGING_REFOBJS_PROFILE_ID IMAGING_REFOBJS_SVCLASS_ID
  2015 +#define HANDSFREE_PROFILE_ID HANDSFREE_SVCLASS_ID
  2016 +#define HANDSFREE_AGW_PROFILE_ID HANDSFREE_AGW_SVCLASS_ID
  2017 +#define DIRECT_PRT_REFOBJS_PROFILE_ID DIRECT_PRT_REFOBJS_SVCLASS_ID
  2018 +#define REFLECTED_UI_PROFILE_ID REFLECTED_UI_SVCLASS_ID
  2019 +#define BASIC_PRINTING_PROFILE_ID BASIC_PRINTING_SVCLASS_ID
  2020 +#define PRINTING_STATUS_PROFILE_ID PRINTING_STATUS_SVCLASS_ID
  2021 +#define HID_PROFILE_ID HID_SVCLASS_ID
  2022 +#define HCR_PROFILE_ID HCR_SCAN_SVCLASS_ID
  2023 +#define HCR_PRINT_PROFILE_ID HCR_PRINT_SVCLASS_ID
  2024 +#define HCR_SCAN_PROFILE_ID HCR_SCAN_SVCLASS_ID
  2025 +#define CIP_PROFILE_ID CIP_SVCLASS_ID
  2026 +#define VIDEO_CONF_GW_PROFILE_ID VIDEO_CONF_GW_SVCLASS_ID
  2027 +#define UDI_MT_PROFILE_ID UDI_MT_SVCLASS_ID
  2028 +#define UDI_TA_PROFILE_ID UDI_TA_SVCLASS_ID
  2029 +#define AV_PROFILE_ID AV_SVCLASS_ID
  2030 +#define SAP_PROFILE_ID SAP_SVCLASS_ID
  2031 +#define PBAP_PCE_PROFILE_ID PBAP_PCE_SVCLASS_ID
  2032 +#define PBAP_PSE_PROFILE_ID PBAP_PSE_SVCLASS_ID
  2033 +#define PBAP_PROFILE_ID PBAP_SVCLASS_ID
  2034 +#define PNP_INFO_PROFILE_ID PNP_INFO_SVCLASS_ID
  2035 +#define GENERIC_NETWORKING_PROFILE_ID GENERIC_NETWORKING_SVCLASS_ID
  2036 +#define GENERIC_FILETRANS_PROFILE_ID GENERIC_FILETRANS_SVCLASS_ID
  2037 +#define GENERIC_AUDIO_PROFILE_ID GENERIC_AUDIO_SVCLASS_ID
  2038 +#define GENERIC_TELEPHONY_PROFILE_ID GENERIC_TELEPHONY_SVCLASS_ID
  2039 +#define UPNP_PROFILE_ID UPNP_SVCLASS_ID
  2040 +#define UPNP_IP_PROFILE_ID UPNP_IP_SVCLASS_ID
  2041 +#define UPNP_PAN_PROFILE_ID UPNP_PAN_SVCLASS_ID
  2042 +#define UPNP_LAP_PROFILE_ID UPNP_LAP_SVCLASS_ID
  2043 +#define UPNP_L2CAP_PROFILE_ID UPNP_L2CAP_SVCLASS_ID
  2044 +#define VIDEO_SOURCE_PROFILE_ID VIDEO_SOURCE_SVCLASS_ID
  2045 +#define VIDEO_SINK_PROFILE_ID VIDEO_SINK_SVCLASS_ID
  2046 +#define VIDEO_DISTRIBUTION_PROFILE_ID VIDEO_DISTRIBUTION_SVCLASS_ID
  2047 +#define MDP_PROFILE_ID MDP_SVCLASS_ID
  2048 +#define MDP_SOURCE_PROFILE_ID MDP_SROUCE_SVCLASS_ID
  2049 +#define MDP_SINK_PROFILE_ID MDP_SINK_SVCLASS_ID
  2050 +#define APPLE_AGENT_PROFILE_ID APPLE_AGENT_SVCLASS_ID
  2051 +
  2052 +/* Data Representation */
  2053 +enum bt_sdp_data_type {
  2054 + SDP_DTYPE_NIL = 0 << 3,
  2055 + SDP_DTYPE_UINT = 1 << 3,
  2056 + SDP_DTYPE_SINT = 2 << 3,
  2057 + SDP_DTYPE_UUID = 3 << 3,
  2058 + SDP_DTYPE_STRING = 4 << 3,
  2059 + SDP_DTYPE_BOOL = 5 << 3,
  2060 + SDP_DTYPE_SEQ = 6 << 3,
  2061 + SDP_DTYPE_ALT = 7 << 3,
  2062 + SDP_DTYPE_URL = 8 << 3,
  2063 +};
  2064 +
  2065 +enum bt_sdp_data_size {
  2066 + SDP_DSIZE_1 = 0,
  2067 + SDP_DSIZE_2,
  2068 + SDP_DSIZE_4,
  2069 + SDP_DSIZE_8,
  2070 + SDP_DSIZE_16,
  2071 + SDP_DSIZE_NEXT1,
  2072 + SDP_DSIZE_NEXT2,
  2073 + SDP_DSIZE_NEXT4,
  2074 + SDP_DSIZE_MASK = SDP_DSIZE_NEXT4,
  2075 +};
  2076 +
  2077 +enum bt_sdp_cmd {
  2078 + SDP_ERROR_RSP = 0x01,
  2079 + SDP_SVC_SEARCH_REQ = 0x02,
  2080 + SDP_SVC_SEARCH_RSP = 0x03,
  2081 + SDP_SVC_ATTR_REQ = 0x04,
  2082 + SDP_SVC_ATTR_RSP = 0x05,
  2083 + SDP_SVC_SEARCH_ATTR_REQ = 0x06,
  2084 + SDP_SVC_SEARCH_ATTR_RSP = 0x07,
  2085 +};
  2086 +
  2087 +enum bt_sdp_errorcode {
  2088 + SDP_INVALID_VERSION = 0x0001,
  2089 + SDP_INVALID_RECORD_HANDLE = 0x0002,
  2090 + SDP_INVALID_SYNTAX = 0x0003,
  2091 + SDP_INVALID_PDU_SIZE = 0x0004,
  2092 + SDP_INVALID_CSTATE = 0x0005,
  2093 +};
  2094 +
  2095 +/*
  2096 + * String identifiers are based on the SDP spec stating that
  2097 + * "base attribute id of the primary (universal) language must be 0x0100"
  2098 + *
  2099 + * Other languages should have their own offset; e.g.:
  2100 + * #define XXXLangBase yyyy
  2101 + * #define AttrServiceName_XXX 0x0000+XXXLangBase
  2102 + */
  2103 +#define SDP_PRIMARY_LANG_BASE 0x0100
  2104 +
  2105 +enum bt_sdp_attribute_id {
  2106 + SDP_ATTR_RECORD_HANDLE = 0x0000,
  2107 + SDP_ATTR_SVCLASS_ID_LIST = 0x0001,
  2108 + SDP_ATTR_RECORD_STATE = 0x0002,
  2109 + SDP_ATTR_SERVICE_ID = 0x0003,
  2110 + SDP_ATTR_PROTO_DESC_LIST = 0x0004,
  2111 + SDP_ATTR_BROWSE_GRP_LIST = 0x0005,
  2112 + SDP_ATTR_LANG_BASE_ATTR_ID_LIST = 0x0006,
  2113 + SDP_ATTR_SVCINFO_TTL = 0x0007,
  2114 + SDP_ATTR_SERVICE_AVAILABILITY = 0x0008,
  2115 + SDP_ATTR_PFILE_DESC_LIST = 0x0009,
  2116 + SDP_ATTR_DOC_URL = 0x000a,
  2117 + SDP_ATTR_CLNT_EXEC_URL = 0x000b,
  2118 + SDP_ATTR_ICON_URL = 0x000c,
  2119 + SDP_ATTR_ADD_PROTO_DESC_LIST = 0x000d,
  2120 +
  2121 + SDP_ATTR_SVCNAME_PRIMARY = SDP_PRIMARY_LANG_BASE + 0,
  2122 + SDP_ATTR_SVCDESC_PRIMARY = SDP_PRIMARY_LANG_BASE + 1,
  2123 + SDP_ATTR_SVCPROV_PRIMARY = SDP_PRIMARY_LANG_BASE + 2,
  2124 +
  2125 + SDP_ATTR_GROUP_ID = 0x0200,
  2126 + SDP_ATTR_IP_SUBNET = 0x0200,
  2127 +
  2128 + /* SDP */
  2129 + SDP_ATTR_VERSION_NUM_LIST = 0x0200,
  2130 + SDP_ATTR_SVCDB_STATE = 0x0201,
  2131 +
  2132 + SDP_ATTR_SERVICE_VERSION = 0x0300,
  2133 + SDP_ATTR_EXTERNAL_NETWORK = 0x0301,
  2134 + SDP_ATTR_SUPPORTED_DATA_STORES_LIST = 0x0301,
  2135 + SDP_ATTR_FAX_CLASS1_SUPPORT = 0x0302,
  2136 + SDP_ATTR_REMOTE_AUDIO_VOLUME_CONTROL = 0x0302,
  2137 + SDP_ATTR_FAX_CLASS20_SUPPORT = 0x0303,
  2138 + SDP_ATTR_SUPPORTED_FORMATS_LIST = 0x0303,
  2139 + SDP_ATTR_FAX_CLASS2_SUPPORT = 0x0304,
  2140 + SDP_ATTR_AUDIO_FEEDBACK_SUPPORT = 0x0305,
  2141 + SDP_ATTR_NETWORK_ADDRESS = 0x0306,
  2142 + SDP_ATTR_WAP_GATEWAY = 0x0307,
  2143 + SDP_ATTR_HOMEPAGE_URL = 0x0308,
  2144 + SDP_ATTR_WAP_STACK_TYPE = 0x0309,
  2145 + SDP_ATTR_SECURITY_DESC = 0x030a,
  2146 + SDP_ATTR_NET_ACCESS_TYPE = 0x030b,
  2147 + SDP_ATTR_MAX_NET_ACCESSRATE = 0x030c,
  2148 + SDP_ATTR_IP4_SUBNET = 0x030d,
  2149 + SDP_ATTR_IP6_SUBNET = 0x030e,
  2150 + SDP_ATTR_SUPPORTED_CAPABILITIES = 0x0310,
  2151 + SDP_ATTR_SUPPORTED_FEATURES = 0x0311,
  2152 + SDP_ATTR_SUPPORTED_FUNCTIONS = 0x0312,
  2153 + SDP_ATTR_TOTAL_IMAGING_DATA_CAPACITY = 0x0313,
  2154 + SDP_ATTR_SUPPORTED_REPOSITORIES = 0x0314,
  2155 +
  2156 + /* PnP Information */
  2157 + SDP_ATTR_SPECIFICATION_ID = 0x0200,
  2158 + SDP_ATTR_VENDOR_ID = 0x0201,
  2159 + SDP_ATTR_PRODUCT_ID = 0x0202,
  2160 + SDP_ATTR_VERSION = 0x0203,
  2161 + SDP_ATTR_PRIMARY_RECORD = 0x0204,
  2162 + SDP_ATTR_VENDOR_ID_SOURCE = 0x0205,
  2163 +
  2164 + /* BT HID */
  2165 + SDP_ATTR_DEVICE_RELEASE_NUMBER = 0x0200,
  2166 + SDP_ATTR_PARSER_VERSION = 0x0201,
  2167 + SDP_ATTR_DEVICE_SUBCLASS = 0x0202,
  2168 + SDP_ATTR_COUNTRY_CODE = 0x0203,
  2169 + SDP_ATTR_VIRTUAL_CABLE = 0x0204,
  2170 + SDP_ATTR_RECONNECT_INITIATE = 0x0205,
  2171 + SDP_ATTR_DESCRIPTOR_LIST = 0x0206,
  2172 + SDP_ATTR_LANG_ID_BASE_LIST = 0x0207,
  2173 + SDP_ATTR_SDP_DISABLE = 0x0208,
  2174 + SDP_ATTR_BATTERY_POWER = 0x0209,
  2175 + SDP_ATTR_REMOTE_WAKEUP = 0x020a,
  2176 + SDP_ATTR_PROFILE_VERSION = 0x020b,
  2177 + SDP_ATTR_SUPERVISION_TIMEOUT = 0x020c,
  2178 + SDP_ATTR_NORMALLY_CONNECTABLE = 0x020d,
  2179 + SDP_ATTR_BOOT_DEVICE = 0x020e,
  2180 +};