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, 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(¶ms->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, ¶ms, 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 = NULL; 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 = NULL; 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] = NULL; 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, NULL, 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 } 1365
