1 /* 2 * QEMU Bluetooth L2CAP logic. 3 * 4 * Copyright (C) 2008 Andrzej Zaborowski <balrog (at) 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, see <http://www.gnu.org/licenses/>. 18 */ 19 20 #include "qemu-common.h" 21 #include "qemu-timer.h" 22 #include "bt.h" 23 24 #define L2CAP_CID_MAX 0x100 /* Between 0x40 and 0x10000 */ 25 26 struct l2cap_instance_s { 27 struct bt_link_s *link; 28 struct bt_l2cap_device_s *dev; 29 int role; 30 31 uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4))); 32 int frame_in_len; 33 34 uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4))); 35 int frame_out_len; 36 37 /* Signalling channel timers. They exist per-request but we can make 38 * sure we have no more than one outstanding request at any time. */ 39 QEMUTimer *rtx; 40 QEMUTimer *ertx; 41 42 int last_id; 43 int next_id; 44 45 struct l2cap_chan_s { 46 struct bt_l2cap_conn_params_s params; 47 48 void (*frame_in)(struct l2cap_chan_s *chan, uint16_t cid, 49 const l2cap_hdr *hdr, int len); 50 int mps; 51 int min_mtu; 52 53 struct l2cap_instance_s *l2cap; 54 55 /* Only allocated channels */ 56 uint16_t remote_cid; 57 #define L2CAP_CFG_INIT 2 58 #define L2CAP_CFG_ACC 1 59 int config_req_id; /* TODO: handle outgoing requests generically */ 60 int config; 61 62 /* Only connection-oriented channels. Note: if we allow the tx and 63 * rx traffic to be in different modes at any time, we need two. */ 64 int mode; 65 66 /* Only flow-controlled, connection-oriented channels */ 67 uint8_t sdu[65536]; /* TODO: dynamically allocate */ 68 int len_cur, len_total; 69 int rexmit; 70 int monitor_timeout; 71 QEMUTimer *monitor_timer; 72 QEMUTimer *retransmission_timer; 73 } *cid[L2CAP_CID_MAX]; 74 /* The channel state machine states map as following: 75 * CLOSED -> !cid[N] 76 * WAIT_CONNECT -> never occurs 77 * WAIT_CONNECT_RSP -> never occurs 78 * CONFIG -> cid[N] && config < 3 79 * WAIT_CONFIG -> never occurs, cid[N] && config == 0 && !config_r 80 * WAIT_SEND_CONFIG -> never occurs, cid[N] && config == 1 && !config_r 81 * WAIT_CONFIG_REQ_RSP -> cid[N] && config == 0 && config_req_id 82 * WAIT_CONFIG_RSP -> cid[N] && config == 1 && config_req_id 83 * WAIT_CONFIG_REQ -> cid[N] && config == 2 84 * OPEN -> cid[N] && config == 3 85 * WAIT_DISCONNECT -> never occurs 86 */ 87 88 struct l2cap_chan_s signalling_ch; 89 struct l2cap_chan_s group_ch; 90 }; 91 92 struct slave_l2cap_instance_s { 93 struct bt_link_s link; /* Underlying logical link (ACL) */ 94 struct l2cap_instance_s l2cap; 95 }; 96 97 struct bt_l2cap_psm_s { 98 int psm; 99 int min_mtu; 100 int (*new_channel)(struct bt_l2cap_device_s *device, 101 struct bt_l2cap_conn_params_s *params); 102 struct bt_l2cap_psm_s *next; 103 }; 104 105 static const uint16_t l2cap_fcs16_table[256] = { 106 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, 107 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, 108 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, 109 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, 110 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, 111 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, 112 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, 113 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, 114 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, 115 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, 116 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, 117 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 118 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 119 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, 120 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, 121 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, 122 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, 123 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, 124 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 125 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 126 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, 127 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, 128 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, 129 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, 130 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, 131 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, 132 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, 133 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, 134 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, 135 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 136 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 137 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, 138 }; 139 140 static uint16_t l2cap_fcs16(const uint8_t *message, int len) 141 { 142 uint16_t fcs = 0x0000; 143 144 while (len --) 145 #if 0 146 { 147 int i; 148 149 fcs ^= *message ++; 150 for (i = 8; i; -- i) 151 if (fcs & 1) 152 fcs = (fcs >> 1) ^ 0xa001; 153 else 154 fcs = (fcs >> 1); 155 } 156 #else 157 fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff]; 158 #endif 159 160 return fcs; 161 } 162 163 /* L2CAP layer logic (protocol) */ 164 165 static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch) 166 { 167 #if 0 168 if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit) 169 qemu_mod_timer(ch->retransmission_timer); 170 else 171 qemu_del_timer(ch->retransmission_timer); 172 #endif 173 } 174 175 static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch) 176 { 177 #if 0 178 if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit) 179 qemu_mod_timer(ch->monitor_timer); 180 else 181 qemu_del_timer(ch->monitor_timer); 182 #endif 183 } 184 185 static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id, 186 uint16_t reason, const void *data, int plen) 187 { 188 uint8_t *pkt; 189 l2cap_cmd_hdr *hdr; 190 l2cap_cmd_rej *params; 191 uint16_t len; 192 193 reason = cpu_to_le16(reason); 194 len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen); 195 196 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 197 L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE + plen); 198 hdr = (void *) (pkt + 0); 199 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 200 201 hdr->code = L2CAP_COMMAND_REJ; 202 hdr->ident = id; 203 memcpy(&hdr->len, &len, sizeof(hdr->len)); 204 memcpy(¶ms->reason, &reason, sizeof(reason)); 205 if (plen) 206 memcpy(pkt + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE, data, plen); 207 208 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 209 } 210 211 static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id, 212 uint16_t reason, uint16_t dcid, uint16_t scid) 213 { 214 l2cap_cmd_rej_cid params = { 215 .dcid = dcid, 216 .scid = scid, 217 }; 218 219 l2cap_command_reject(l2cap, id, reason, ¶ms, L2CAP_CMD_REJ_CID_SIZE); 220 } 221 222 static void l2cap_connection_response(struct l2cap_instance_s *l2cap, 223 int dcid, int scid, int result, int status) 224 { 225 uint8_t *pkt; 226 l2cap_cmd_hdr *hdr; 227 l2cap_conn_rsp *params; 228 229 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 230 L2CAP_CMD_HDR_SIZE + L2CAP_CONN_RSP_SIZE); 231 hdr = (void *) (pkt + 0); 232 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 233 234 hdr->code = L2CAP_CONN_RSP; 235 hdr->ident = l2cap->last_id; 236 hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE); 237 238 params->dcid = cpu_to_le16(dcid); 239 params->scid = cpu_to_le16(scid); 240 params->result = cpu_to_le16(result); 241 params->status = cpu_to_le16(status); 242 243 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 244 } 245 246 static void l2cap_configuration_request(struct l2cap_instance_s *l2cap, 247 int dcid, int flag, const uint8_t *data, int len) 248 { 249 uint8_t *pkt; 250 l2cap_cmd_hdr *hdr; 251 l2cap_conf_req *params; 252 253 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 254 L2CAP_CMD_HDR_SIZE + L2CAP_CONF_REQ_SIZE(len)); 255 hdr = (void *) (pkt + 0); 256 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 257 258 /* TODO: unify the id sequencing */ 259 l2cap->last_id = l2cap->next_id; 260 l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1; 261 262 hdr->code = L2CAP_CONF_REQ; 263 hdr->ident = l2cap->last_id; 264 hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len)); 265 266 params->dcid = cpu_to_le16(dcid); 267 params->flags = cpu_to_le16(flag); 268 if (len) 269 memcpy(params->data, data, len); 270 271 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 272 } 273 274 static void l2cap_configuration_response(struct l2cap_instance_s *l2cap, 275 int scid, int flag, int result, const uint8_t *data, int len) 276 { 277 uint8_t *pkt; 278 l2cap_cmd_hdr *hdr; 279 l2cap_conf_rsp *params; 280 281 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 282 L2CAP_CMD_HDR_SIZE + L2CAP_CONF_RSP_SIZE(len)); 283 hdr = (void *) (pkt + 0); 284 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 285 286 hdr->code = L2CAP_CONF_RSP; 287 hdr->ident = l2cap->last_id; 288 hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len)); 289 290 params->scid = cpu_to_le16(scid); 291 params->flags = cpu_to_le16(flag); 292 params->result = cpu_to_le16(result); 293 if (len) 294 memcpy(params->data, data, len); 295 296 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 297 } 298 299 static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap, 300 int dcid, int scid) 301 { 302 uint8_t *pkt; 303 l2cap_cmd_hdr *hdr; 304 l2cap_disconn_rsp *params; 305 306 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 307 L2CAP_CMD_HDR_SIZE + L2CAP_DISCONN_RSP_SIZE); 308 hdr = (void *) (pkt + 0); 309 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 310 311 hdr->code = L2CAP_DISCONN_RSP; 312 hdr->ident = l2cap->last_id; 313 hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE); 314 315 params->dcid = cpu_to_le16(dcid); 316 params->scid = cpu_to_le16(scid); 317 318 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 319 } 320 321 static void l2cap_echo_response(struct l2cap_instance_s *l2cap, 322 const uint8_t *data, int len) 323 { 324 uint8_t *pkt; 325 l2cap_cmd_hdr *hdr; 326 uint8_t *params; 327 328 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 329 L2CAP_CMD_HDR_SIZE + len); 330 hdr = (void *) (pkt + 0); 331 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 332 333 hdr->code = L2CAP_ECHO_RSP; 334 hdr->ident = l2cap->last_id; 335 hdr->len = cpu_to_le16(len); 336 337 memcpy(params, data, len); 338 339 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 340 } 341 342 static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type, 343 int result, const uint8_t *data, int len) 344 { 345 uint8_t *pkt; 346 l2cap_cmd_hdr *hdr; 347 l2cap_info_rsp *params; 348 349 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params, 350 L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + len); 351 hdr = (void *) (pkt + 0); 352 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE); 353 354 hdr->code = L2CAP_INFO_RSP; 355 hdr->ident = l2cap->last_id; 356 hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len); 357 358 params->type = cpu_to_le16(type); 359 params->result = cpu_to_le16(result); 360 if (len) 361 memcpy(params->data, data, len); 362 363 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params); 364 } 365 366 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len); 367 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms); 368 #if 0 369 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len); 370 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm); 371 #endif 372 static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid, 373 const l2cap_hdr *hdr, int len); 374 static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid, 375 const l2cap_hdr *hdr, int len); 376 377 static int l2cap_cid_new(struct l2cap_instance_s *l2cap) 378 { 379 int i; 380 381 for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++) 382 if (!l2cap->cid[i]) 383 return i; 384 385 return L2CAP_CID_INVALID; 386 } 387 388 static inline struct bt_l2cap_psm_s *l2cap_psm( 389 struct bt_l2cap_device_s *device, int psm) 390 { 391 struct bt_l2cap_psm_s *ret = device->first_psm; 392 393 while (ret && ret->psm != psm) 394 ret = ret->next; 395 396 return ret; 397 } 398 399 static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap, 400 int psm, int source_cid) 401 { 402 struct l2cap_chan_s *ch = NULL; 403 struct bt_l2cap_psm_s *psm_info; 404 int result, status; 405 int cid = l2cap_cid_new(l2cap); 406 407 if (cid) { 408 /* See what the channel is to be used for.. */ 409 psm_info = l2cap_psm(l2cap->dev, psm); 410 411 if (psm_info) { 412 /* Device supports this use-case. */ 413 ch = qemu_mallocz(sizeof(*ch)); 414 ch->params.sdu_out = l2cap_bframe_out; 415 ch->params.sdu_submit = l2cap_bframe_submit; 416 ch->frame_in = l2cap_bframe_in; 417 ch->mps = 65536; 418 ch->min_mtu = MAX(48, psm_info->min_mtu); 419 ch->params.remote_mtu = MAX(672, ch->min_mtu); 420 ch->remote_cid = source_cid; 421 ch->mode = L2CAP_MODE_BASIC; 422 ch->l2cap = l2cap; 423 424 /* Does it feel like opening yet another channel though? */ 425 if (!psm_info->new_channel(l2cap->dev, &ch->params)) { 426 l2cap->cid[cid] = ch; 427 428 result = L2CAP_CR_SUCCESS; 429 status = L2CAP_CS_NO_INFO; 430 } else { 431 qemu_free(ch); 432 433 result = L2CAP_CR_NO_MEM; 434 status = L2CAP_CS_NO_INFO; 435 } 436 } else { 437 result = L2CAP_CR_BAD_PSM; 438 status = L2CAP_CS_NO_INFO; 439 } 440 } else { 441 result = L2CAP_CR_NO_MEM; 442 status = L2CAP_CS_NO_INFO; 443 } 444 445 l2cap_connection_response(l2cap, cid, source_cid, result, status); 446 447 return ch; 448 } 449 450 static void l2cap_channel_close(struct l2cap_instance_s *l2cap, 451 int cid, int source_cid) 452 { 453 struct l2cap_chan_s *ch = NULL; 454 455 /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a 456 * connection in CLOSED state still responds with a L2CAP_DisconnectRsp 457 * message on an L2CAP_DisconnectReq event. */ 458 if (unlikely(cid < L2CAP_CID_ALLOC)) { 459 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL, 460 cid, source_cid); 461 return; 462 } 463 if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX)) 464 ch = l2cap->cid[cid]; 465 466 if (likely(ch)) { 467 if (ch->remote_cid != source_cid) { 468 fprintf(stderr, "%s: Ignoring a Disconnection Request with the " 469 "invalid SCID %04x.\n", __FUNCTION__, source_cid); 470 return; 471 } 472 473 l2cap->cid[cid] = NULL; 474 475 ch->params.close(ch->params.opaque); 476 qemu_free(ch); 477 } 478 479 l2cap_disconnection_response(l2cap, cid, source_cid); 480 } 481 482 static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap, 483 struct l2cap_chan_s *ch) 484 { 485 l2cap_configuration_request(l2cap, ch->remote_cid, 0, NULL, 0); 486 ch->config_req_id = l2cap->last_id; 487 ch->config &= ~L2CAP_CFG_INIT; 488 } 489 490 static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap, 491 struct l2cap_chan_s *ch) 492 { 493 /* Use all default channel options and terminate negotiation. */ 494 l2cap_channel_config_null(l2cap, ch); 495 } 496 497 static int l2cap_channel_config(struct l2cap_instance_s *l2cap, 498 struct l2cap_chan_s *ch, int flag, 499 const uint8_t *data, int len) 500 { 501 l2cap_conf_opt *opt; 502 l2cap_conf_opt_qos *qos; 503 uint32_t val; 504 uint8_t rsp[len]; 505 int result = L2CAP_CONF_SUCCESS; 506 507 data = memcpy(rsp, data, len); 508 while (len) { 509 opt = (void *) data; 510 511 if (len < L2CAP_CONF_OPT_SIZE || 512 len < L2CAP_CONF_OPT_SIZE + opt->len) { 513 result = L2CAP_CONF_REJECT; 514 break; 515 } 516 data += L2CAP_CONF_OPT_SIZE + opt->len; 517 len -= L2CAP_CONF_OPT_SIZE + opt->len; 518 519 switch (opt->type & 0x7f) { 520 case L2CAP_CONF_MTU: 521 if (opt->len != 2) { 522 result = L2CAP_CONF_REJECT; 523 break; 524 } 525 526 /* MTU */ 527 val = le16_to_cpup((void *) opt->val); 528 if (val < ch->min_mtu) { 529 cpu_to_le16w((void *) opt->val, ch->min_mtu); 530 result = L2CAP_CONF_UNACCEPT; 531 break; 532 } 533 534 ch->params.remote_mtu = val; 535 break; 536 537 case L2CAP_CONF_FLUSH_TO: 538 if (opt->len != 2) { 539 result = L2CAP_CONF_REJECT; 540 break; 541 } 542 543 /* Flush Timeout */ 544 val = le16_to_cpup((void *) opt->val); 545 if (val < 0x0001) { 546 opt->val[0] = 0xff; 547 opt->val[1] = 0xff; 548 result = L2CAP_CONF_UNACCEPT; 549 break; 550 } 551 break; 552 553 case L2CAP_CONF_QOS: 554 if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) { 555 result = L2CAP_CONF_REJECT; 556 break; 557 } 558 qos = (void *) opt->val; 559 560 /* Flags */ 561 val = qos->flags; 562 if (val) { 563 qos->flags = 0; 564 result = L2CAP_CONF_UNACCEPT; 565 } 566 567 /* Service type */ 568 val = qos->service_type; 569 if (val != L2CAP_CONF_QOS_BEST_EFFORT && 570 val != L2CAP_CONF_QOS_NO_TRAFFIC) { 571 qos->service_type = L2CAP_CONF_QOS_BEST_EFFORT; 572 result = L2CAP_CONF_UNACCEPT; 573 } 574 575 if (val != L2CAP_CONF_QOS_NO_TRAFFIC) { 576 /* XXX: These values should possibly be calculated 577 * based on LM / baseband properties also. */ 578 579 /* Token rate */ 580 val = le32_to_cpu(qos->token_rate); 581 if (val == L2CAP_CONF_QOS_WILDCARD) 582 qos->token_rate = cpu_to_le32(0x100000); 583 584 /* Token bucket size */ 585 val = le32_to_cpu(qos->token_bucket_size); 586 if (val == L2CAP_CONF_QOS_WILDCARD) 587 qos->token_bucket_size = cpu_to_le32(65500); 588 589 /* Any Peak bandwidth value is correct to return as-is */ 590 /* Any Access latency value is correct to return as-is */ 591 /* Any Delay variation value is correct to return as-is */ 592 } 593 break; 594 595 case L2CAP_CONF_RFC: 596 if (opt->len != 9) { 597 result = L2CAP_CONF_REJECT; 598 break; 599 } 600 601 /* Mode */ 602 val = opt->val[0]; 603 switch (val) { 604 case L2CAP_MODE_BASIC: 605 ch->mode = val; 606 ch->frame_in = l2cap_bframe_in; 607 608 /* All other parameters shall be ignored */ 609 break; 610 611 case L2CAP_MODE_RETRANS: 612 case L2CAP_MODE_FLOWCTL: 613 ch->mode = val; 614 ch->frame_in = l2cap_iframe_in; 615 /* Note: most of these parameters refer to incoming traffic 616 * so we don't need to save them as long as we can accept 617 * incoming PDUs at any values of the parameters. */ 618 619 /* TxWindow size */ 620 val = opt->val[1]; 621 if (val < 1 || val > 32) { 622 opt->val[1] = 32; 623 result = L2CAP_CONF_UNACCEPT; 624 break; 625 } 626 627 /* MaxTransmit */ 628 val = opt->val[2]; 629 if (val < 1) { 630 opt->val[2] = 1; 631 result = L2CAP_CONF_UNACCEPT; 632 break; 633 } 634 635 /* Remote Retransmission time-out shouldn't affect local 636 * operation (?) */ 637 638 /* The Monitor time-out drives the local Monitor timer (?), 639 * so save the value. */ 640 val = (opt->val[6] << 8) | opt->val[5]; 641 if (val < 30) { 642 opt->val[5] = 100 & 0xff; 643 opt->val[6] = 100 >> 8; 644 result = L2CAP_CONF_UNACCEPT; 645 break; 646 } 647 ch->monitor_timeout = val; 648 l2cap_monitor_timer_update(ch); 649 650 /* MPS */ 651 val = (opt->val[8] << 8) | opt->val[7]; 652 if (val < ch->min_mtu) { 653 opt->val[7] = ch->min_mtu & 0xff; 654 opt->val[8] = ch->min_mtu >> 8; 655 result = L2CAP_CONF_UNACCEPT; 656 break; 657 } 658 ch->mps = val; 659 break; 660 661 default: 662 result = L2CAP_CONF_UNACCEPT; 663 break; 664 } 665 break; 666 667 default: 668 if (!(opt->type >> 7)) 669 result = L2CAP_CONF_UNKNOWN; 670 break; 671 } 672 673 if (result != L2CAP_CONF_SUCCESS) 674 break; /* XXX: should continue? */ 675 } 676 677 l2cap_configuration_response(l2cap, ch->remote_cid, 678 flag, result, rsp, len); 679 680 return result == L2CAP_CONF_SUCCESS && !flag; 681 } 682 683 static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap, 684 int flag, int cid, const uint8_t *data, int len) 685 { 686 struct l2cap_chan_s *ch; 687 688 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { 689 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL, 690 cid, 0x0000); 691 return; 692 } 693 ch = l2cap->cid[cid]; 694 695 /* From OPEN go to WAIT_CONFIG_REQ and from WAIT_CONFIG_REQ_RSP to 696 * WAIT_CONFIG_REQ_RSP. This is assuming the transition chart for OPEN 697 * on pg 1053, section 6.1.5, volume 3 of BT Core V2.0 has a mistake 698 * and on options-acceptable we go back to OPEN and otherwise to 699 * WAIT_CONFIG_REQ and not the other way. */ 700 ch->config &= ~L2CAP_CFG_ACC; 701 702 if (l2cap_channel_config(l2cap, ch, flag, data, len)) 703 /* Go to OPEN or WAIT_CONFIG_RSP */ 704 ch->config |= L2CAP_CFG_ACC; 705 706 /* TODO: if the incoming traffic flow control or retransmission mode 707 * changed then we probably need to also generate the 708 * ConfigureChannel_Req event and set the outgoing traffic to the same 709 * mode. */ 710 if (!(ch->config & L2CAP_CFG_INIT) && (ch->config & L2CAP_CFG_ACC) && 711 !ch->config_req_id) 712 l2cap_channel_config_req_event(l2cap, ch); 713 } 714 715 static int l2cap_channel_config_rsp_msg(struct l2cap_instance_s *l2cap, 716 int result, int flag, int cid, const uint8_t *data, int len) 717 { 718 struct l2cap_chan_s *ch; 719 720 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { 721 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL, 722 cid, 0x0000); 723 return 0; 724 } 725 ch = l2cap->cid[cid]; 726 727 if (ch->config_req_id != l2cap->last_id) 728 return 1; 729 ch->config_req_id = 0; 730 731 if (result == L2CAP_CONF_SUCCESS) { 732 if (!flag) 733 ch->config |= L2CAP_CFG_INIT; 734 else 735 l2cap_channel_config_null(l2cap, ch); 736 } else 737 /* Retry until we succeed */ 738 l2cap_channel_config_req_event(l2cap, ch); 739 740 return 0; 741 } 742 743 static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap, 744 int psm, int source_cid) 745 { 746 struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid); 747 748 if (!ch) 749 return; 750 751 /* Optional */ 752 if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id) 753 l2cap_channel_config_req_event(l2cap, ch); 754 } 755 756 static void l2cap_info(struct l2cap_instance_s *l2cap, int type) 757 { 758 uint8_t data[4]; 759 int len = 0; 760 int result = L2CAP_IR_SUCCESS; 761 762 switch (type) { 763 case L2CAP_IT_CL_MTU: 764 data[len ++] = l2cap->group_ch.mps & 0xff; 765 data[len ++] = l2cap->group_ch.mps >> 8; 766 break; 767 768 case L2CAP_IT_FEAT_MASK: 769 /* (Prematurely) report Flow control and Retransmission modes. */ 770 data[len ++] = 0x03; 771 data[len ++] = 0x00; 772 data[len ++] = 0x00; 773 data[len ++] = 0x00; 774 break; 775 776 default: 777 result = L2CAP_IR_NOTSUPP; 778 } 779 780 l2cap_info_response(l2cap, type, result, data, len); 781 } 782 783 static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id, 784 const uint8_t *params, int len) 785 { 786 int err; 787 788 #if 0 789 /* TODO: do the IDs really have to be in sequence? */ 790 if (!id || (id != l2cap->last_id && id != l2cap->next_id)) { 791 fprintf(stderr, "%s: out of sequence command packet ignored.\n", 792 __FUNCTION__); 793 return; 794 } 795 #else 796 l2cap->next_id = id; 797 #endif 798 if (id == l2cap->next_id) { 799 l2cap->last_id = l2cap->next_id; 800 l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1; 801 } else { 802 /* TODO: Need to re-send the same response, without re-executing 803 * the corresponding command! */ 804 } 805 806 switch (code) { 807 case L2CAP_COMMAND_REJ: 808 if (unlikely(len != 2 && len != 4 && len != 6)) { 809 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 810 goto reject; 811 } 812 813 /* We never issue commands other than Command Reject currently. */ 814 fprintf(stderr, "%s: stray Command Reject (%02x, %04x) " 815 "packet, ignoring.\n", __FUNCTION__, id, 816 le16_to_cpu(((l2cap_cmd_rej *) params)->reason)); 817 break; 818 819 case L2CAP_CONN_REQ: 820 if (unlikely(len != L2CAP_CONN_REQ_SIZE)) { 821 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 822 goto reject; 823 } 824 825 l2cap_channel_open_req_msg(l2cap, 826 le16_to_cpu(((l2cap_conn_req *) params)->psm), 827 le16_to_cpu(((l2cap_conn_req *) params)->scid)); 828 break; 829 830 case L2CAP_CONN_RSP: 831 if (unlikely(len != L2CAP_CONN_RSP_SIZE)) { 832 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 833 goto reject; 834 } 835 836 /* We never issue Connection Requests currently. TODO */ 837 fprintf(stderr, "%s: unexpected Connection Response (%02x) " 838 "packet, ignoring.\n", __FUNCTION__, id); 839 break; 840 841 case L2CAP_CONF_REQ: 842 if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) { 843 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 844 goto reject; 845 } 846 847 l2cap_channel_config_req_msg(l2cap, 848 le16_to_cpu(((l2cap_conf_req *) params)->flags) & 1, 849 le16_to_cpu(((l2cap_conf_req *) params)->dcid), 850 ((l2cap_conf_req *) params)->data, 851 len - L2CAP_CONF_REQ_SIZE(0)); 852 break; 853 854 case L2CAP_CONF_RSP: 855 if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) { 856 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 857 goto reject; 858 } 859 860 if (l2cap_channel_config_rsp_msg(l2cap, 861 le16_to_cpu(((l2cap_conf_rsp *) params)->result), 862 le16_to_cpu(((l2cap_conf_rsp *) params)->flags) & 1, 863 le16_to_cpu(((l2cap_conf_rsp *) params)->scid), 864 ((l2cap_conf_rsp *) params)->data, 865 len - L2CAP_CONF_RSP_SIZE(0))) 866 fprintf(stderr, "%s: unexpected Configure Response (%02x) " 867 "packet, ignoring.\n", __FUNCTION__, id); 868 break; 869 870 case L2CAP_DISCONN_REQ: 871 if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) { 872 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 873 goto reject; 874 } 875 876 l2cap_channel_close(l2cap, 877 le16_to_cpu(((l2cap_disconn_req *) params)->dcid), 878 le16_to_cpu(((l2cap_disconn_req *) params)->scid)); 879 break; 880 881 case L2CAP_DISCONN_RSP: 882 if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) { 883 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 884 goto reject; 885 } 886 887 /* We never issue Disconnection Requests currently. TODO */ 888 fprintf(stderr, "%s: unexpected Disconnection Response (%02x) " 889 "packet, ignoring.\n", __FUNCTION__, id); 890 break; 891 892 case L2CAP_ECHO_REQ: 893 l2cap_echo_response(l2cap, params, len); 894 break; 895 896 case L2CAP_ECHO_RSP: 897 /* We never issue Echo Requests currently. TODO */ 898 fprintf(stderr, "%s: unexpected Echo Response (%02x) " 899 "packet, ignoring.\n", __FUNCTION__, id); 900 break; 901 902 case L2CAP_INFO_REQ: 903 if (unlikely(len != L2CAP_INFO_REQ_SIZE)) { 904 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 905 goto reject; 906 } 907 908 l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type)); 909 break; 910 911 case L2CAP_INFO_RSP: 912 if (unlikely(len != L2CAP_INFO_RSP_SIZE)) { 913 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 914 goto reject; 915 } 916 917 /* We never issue Information Requests currently. TODO */ 918 fprintf(stderr, "%s: unexpected Information Response (%02x) " 919 "packet, ignoring.\n", __FUNCTION__, id); 920 break; 921 922 default: 923 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD; 924 reject: 925 l2cap_command_reject(l2cap, id, err, 0, 0); 926 break; 927 } 928 } 929 930 static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable) 931 { 932 ch->rexmit = enable; 933 934 l2cap_retransmission_timer_update(ch); 935 l2cap_monitor_timer_update(ch); 936 } 937 938 /* Command frame SDU */ 939 static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len) 940 { 941 struct l2cap_instance_s *l2cap = opaque; 942 const l2cap_cmd_hdr *hdr; 943 int clen; 944 945 while (len) { 946 hdr = (void *) data; 947 if (len < L2CAP_CMD_HDR_SIZE) 948 /* TODO: signal an error */ 949 return; 950 len -= L2CAP_CMD_HDR_SIZE; 951 data += L2CAP_CMD_HDR_SIZE; 952 953 clen = le16_to_cpu(hdr->len); 954 if (len < clen) { 955 l2cap_command_reject(l2cap, hdr->ident, 956 L2CAP_REJ_CMD_NOT_UNDERSTOOD, 0, 0); 957 break; 958 } 959 960 l2cap_command(l2cap, hdr->code, hdr->ident, data, clen); 961 len -= clen; 962 data += clen; 963 } 964 } 965 966 /* Group frame SDU */ 967 static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len) 968 { 969 } 970 971 /* Supervisory frame */ 972 static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl) 973 { 974 } 975 976 /* Basic L2CAP mode Information frame */ 977 static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid, 978 const l2cap_hdr *hdr, int len) 979 { 980 /* We have a full SDU, no further processing */ 981 ch->params.sdu_in(ch->params.opaque, hdr->data, len); 982 } 983 984 /* Flow Control and Retransmission mode frame */ 985 static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid, 986 const l2cap_hdr *hdr, int len) 987 { 988 uint16_t fcs = le16_to_cpup((void *) (hdr->data + len - 2)); 989 990 if (len < 4) 991 goto len_error; 992 if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs) 993 goto fcs_error; 994 995 if ((hdr->data[0] >> 7) == ch->rexmit) 996 l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7)); 997 998 if (hdr->data[0] & 1) { 999 if (len != 4) { 1000 /* TODO: Signal an error? */ 1001 return; 1002 } 1003 return l2cap_sframe_in(ch, le16_to_cpup((void *) hdr->data)); 1004 } 1005 1006 switch (hdr->data[1] >> 6) { /* SAR */ 1007 case L2CAP_SAR_NO_SEG: 1008 if (ch->len_total) 1009 goto seg_error; 1010 if (len - 4 > ch->mps) 1011 goto len_error; 1012 1013 return ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4); 1014 1015 case L2CAP_SAR_START: 1016 if (ch->len_total || len < 6) 1017 goto seg_error; 1018 if (len - 6 > ch->mps) 1019 goto len_error; 1020 1021 ch->len_total = le16_to_cpup((void *) (hdr->data + 2)); 1022 if (len >= 6 + ch->len_total) 1023 goto seg_error; 1024 1025 ch->len_cur = len - 6; 1026 memcpy(ch->sdu, hdr->data + 4, ch->len_cur); 1027 break; 1028 1029 case L2CAP_SAR_END: 1030 if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total) 1031 goto seg_error; 1032 if (len - 4 > ch->mps) 1033 goto len_error; 1034 1035 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4); 1036 return ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total); 1037 1038 case L2CAP_SAR_CONT: 1039 if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total) 1040 goto seg_error; 1041 if (len - 4 > ch->mps) 1042 goto len_error; 1043 1044 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4); 1045 ch->len_cur += len - 4; 1046 break; 1047 1048 seg_error: 1049 len_error: /* TODO */ 1050 fcs_error: /* TODO */ 1051 ch->len_cur = 0; 1052 ch->len_total = 0; 1053 break; 1054 } 1055 } 1056 1057 static void l2cap_frame_in(struct l2cap_instance_s *l2cap, 1058 const l2cap_hdr *frame) 1059 { 1060 uint16_t cid = le16_to_cpu(frame->cid); 1061 uint16_t len = le16_to_cpu(frame->len); 1062 1063 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { 1064 fprintf(stderr, "%s: frame addressed to a non-existent L2CAP " 1065 "channel %04x received.\n", __FUNCTION__, cid); 1066 return; 1067 } 1068 1069 l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len); 1070 } 1071 1072 /* "Recombination" */ 1073 static void l2cap_pdu_in(struct l2cap_instance_s *l2cap, 1074 const uint8_t *data, int len) 1075 { 1076 const l2cap_hdr *hdr = (void *) l2cap->frame_in; 1077 1078 if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) { 1079 if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) { 1080 memcpy(l2cap->frame_in + l2cap->frame_in_len, data, 1081 sizeof(l2cap->frame_in) - l2cap->frame_in_len); 1082 l2cap->frame_in_len = sizeof(l2cap->frame_in); 1083 /* TODO: truncate */ 1084 l2cap_frame_in(l2cap, hdr); 1085 } 1086 1087 return; 1088 } 1089 1090 memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len); 1091 l2cap->frame_in_len += len; 1092 1093 if (len >= L2CAP_HDR_SIZE) 1094 if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len)) 1095 l2cap_frame_in(l2cap, hdr); 1096 /* There is never a start of a new PDU in the same ACL packet, so 1097 * no need to memmove the remaining payload and loop. */ 1098 } 1099 1100 static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap, 1101 uint16_t cid, uint16_t len) 1102 { 1103 l2cap_hdr *hdr = (void *) l2cap->frame_out; 1104 1105 l2cap->frame_out_len = len + L2CAP_HDR_SIZE; 1106 1107 hdr->cid = cpu_to_le16(cid); 1108 hdr->len = cpu_to_le16(len); 1109 1110 return l2cap->frame_out + L2CAP_HDR_SIZE; 1111 } 1112 1113 static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap) 1114 { 1115 /* TODO: Fragmentation */ 1116 (l2cap->role ? 1117 l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp) 1118 (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len); 1119 } 1120 1121 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len) 1122 { 1123 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm; 1124 1125 if (len > chan->params.remote_mtu) { 1126 fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n", 1127 __FUNCTION__, 1128 chan->remote_cid, chan->params.remote_mtu); 1129 exit(-1); 1130 } 1131 1132 return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len); 1133 } 1134 1135 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms) 1136 { 1137 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms; 1138 1139 return l2cap_pdu_submit(chan->l2cap); 1140 } 1141 1142 #if 0 1143 /* Stub: Only used if an emulated device requests outgoing flow control */ 1144 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len) 1145 { 1146 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm; 1147 1148 if (len > chan->params.remote_mtu) { 1149 /* TODO: slice into segments and queue each segment as a separate 1150 * I-Frame in a FIFO of I-Frames, local to the CID. */ 1151 } else { 1152 /* TODO: add to the FIFO of I-Frames, local to the CID. */ 1153 /* Possibly we need to return a pointer to a contiguous buffer 1154 * for now and then memcpy from it into FIFOs in l2cap_iframe_submit 1155 * while segmenting at the same time. */ 1156 } 1157 return 0; 1158 } 1159 1160 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm) 1161 { 1162 /* TODO: If flow control indicates clear to send, start submitting the 1163 * invidual I-Frames from the FIFO, but don't remove them from there. 1164 * Kick the appropriate timer until we get an S-Frame, and only then 1165 * remove from FIFO or resubmit and re-kick the timer if the timer 1166 * expired. */ 1167 } 1168 #endif 1169 1170 static void l2cap_init(struct l2cap_instance_s *l2cap, 1171 struct bt_link_s *link, int role) 1172 { 1173 l2cap->link = link; 1174 l2cap->role = role; 1175 l2cap->dev = (struct bt_l2cap_device_s *) 1176 (role ? link->host : link->slave); 1177 1178 l2cap->next_id = 1; 1179 1180 /* Establish the signalling channel */ 1181 l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in; 1182 l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out; 1183 l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit; 1184 l2cap->signalling_ch.params.opaque = l2cap; 1185 l2cap->signalling_ch.params.remote_mtu = 48; 1186 l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING; 1187 l2cap->signalling_ch.frame_in = l2cap_bframe_in; 1188 l2cap->signalling_ch.mps = 65536; 1189 l2cap->signalling_ch.min_mtu = 48; 1190 l2cap->signalling_ch.mode = L2CAP_MODE_BASIC; 1191 l2cap->signalling_ch.l2cap = l2cap; 1192 l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch; 1193 1194 /* Establish the connection-less data channel */ 1195 l2cap->group_ch.params.sdu_in = l2cap_gframe_in; 1196 l2cap->group_ch.params.opaque = l2cap; 1197 l2cap->group_ch.frame_in = l2cap_bframe_in; 1198 l2cap->group_ch.mps = 65533; 1199 l2cap->group_ch.l2cap = l2cap; 1200 l2cap->group_ch.remote_cid = L2CAP_CID_INVALID; 1201 l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch; 1202 } 1203 1204 static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect) 1205 { 1206 int cid; 1207 1208 /* Don't send DISCONNECT if we are currently handling a DISCONNECT 1209 * sent from the other side. */ 1210 if (send_disconnect) { 1211 if (l2cap->role) 1212 l2cap->dev->device.lmp_disconnect_slave(l2cap->link); 1213 /* l2cap->link is invalid from now on. */ 1214 else 1215 l2cap->dev->device.lmp_disconnect_master(l2cap->link); 1216 } 1217 1218 for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++) 1219 if (l2cap->cid[cid]) { 1220 l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque); 1221 qemu_free(l2cap->cid[cid]); 1222 } 1223 1224 if (l2cap->role) 1225 qemu_free(l2cap); 1226 else 1227 qemu_free(l2cap->link); 1228 } 1229 1230 /* L2CAP glue to lower layers in bluetooth stack (LMP) */ 1231 1232 static void l2cap_lmp_connection_request(struct bt_link_s *link) 1233 { 1234 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave; 1235 struct slave_l2cap_instance_s *l2cap; 1236 1237 /* Always accept - we only get called if (dev->device->page_scan). */ 1238 1239 l2cap = qemu_mallocz(sizeof(struct slave_l2cap_instance_s)); 1240 l2cap->link.slave = &dev->device; 1241 l2cap->link.host = link->host; 1242 l2cap_init(&l2cap->l2cap, &l2cap->link, 0); 1243 1244 /* Always at the end */ 1245 link->host->reject_reason = 0; 1246 link->host->lmp_connection_complete(&l2cap->link); 1247 } 1248 1249 /* Stub */ 1250 static void l2cap_lmp_connection_complete(struct bt_link_s *link) 1251 { 1252 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; 1253 struct l2cap_instance_s *l2cap; 1254 1255 if (dev->device.reject_reason) { 1256 /* Signal to upper layer */ 1257 return; 1258 } 1259 1260 l2cap = qemu_mallocz(sizeof(struct l2cap_instance_s)); 1261 l2cap_init(l2cap, link, 1); 1262 1263 link->acl_mode = acl_active; 1264 1265 /* Signal to upper layer */ 1266 } 1267 1268 /* Stub */ 1269 static void l2cap_lmp_disconnect_host(struct bt_link_s *link) 1270 { 1271 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; 1272 struct l2cap_instance_s *l2cap = 1273 /* TODO: Retrieve from upper layer */ (void *) dev; 1274 1275 /* Signal to upper layer */ 1276 1277 l2cap_teardown(l2cap, 0); 1278 } 1279 1280 static void l2cap_lmp_disconnect_slave(struct bt_link_s *link) 1281 { 1282 struct slave_l2cap_instance_s *l2cap = 1283 (struct slave_l2cap_instance_s *) link; 1284 1285 l2cap_teardown(&l2cap->l2cap, 0); 1286 } 1287 1288 static void l2cap_lmp_acl_data_slave(struct bt_link_s *link, 1289 const uint8_t *data, int start, int len) 1290 { 1291 struct slave_l2cap_instance_s *l2cap = 1292 (struct slave_l2cap_instance_s *) link; 1293 1294 if (start) 1295 l2cap->l2cap.frame_in_len = 0; 1296 1297 l2cap_pdu_in(&l2cap->l2cap, data, len); 1298 } 1299 1300 /* Stub */ 1301 static void l2cap_lmp_acl_data_host(struct bt_link_s *link, 1302 const uint8_t *data, int start, int len) 1303 { 1304 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; 1305 struct l2cap_instance_s *l2cap = 1306 /* TODO: Retrieve from upper layer */ (void *) dev; 1307 1308 if (start) 1309 l2cap->frame_in_len = 0; 1310 1311 l2cap_pdu_in(l2cap, data, len); 1312 } 1313 1314 static void l2cap_dummy_destroy(struct bt_device_s *dev) 1315 { 1316 struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev; 1317 1318 bt_l2cap_device_done(l2cap_dev); 1319 } 1320 1321 void bt_l2cap_device_init(struct bt_l2cap_device_s *dev, 1322 struct bt_scatternet_s *net) 1323 { 1324 bt_device_init(&dev->device, net); 1325 1326 dev->device.lmp_connection_request = l2cap_lmp_connection_request; 1327 dev->device.lmp_connection_complete = l2cap_lmp_connection_complete; 1328 dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host; 1329 dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave; 1330 dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave; 1331 dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host; 1332 1333 dev->device.handle_destroy = l2cap_dummy_destroy; 1334 } 1335 1336 void bt_l2cap_device_done(struct bt_l2cap_device_s *dev) 1337 { 1338 bt_device_done(&dev->device); 1339 1340 /* Should keep a list of all instances and go through it and 1341 * invoke l2cap_teardown() for each. */ 1342 } 1343 1344 void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu, 1345 int (*new_channel)(struct bt_l2cap_device_s *dev, 1346 struct bt_l2cap_conn_params_s *params)) 1347 { 1348 struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm); 1349 1350 if (new_psm) { 1351 fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n", 1352 __FUNCTION__, psm, dev->device.lmp_name); 1353 exit(-1); 1354 } 1355 1356 new_psm = qemu_mallocz(sizeof(*new_psm)); 1357 new_psm->psm = psm; 1358 new_psm->min_mtu = min_mtu; 1359 new_psm->new_channel = new_channel; 1360 new_psm->next = dev->first_psm; 1361 dev->first_psm = new_psm; 1362 } 1363
