1 /****************************************************************************** 2 * 3 * Copyright 1999-2012 Broadcom Corporation 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at: 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 * 17 ******************************************************************************/ 18 19 /****************************************************************************** 20 * 21 * This file contains the main L2CAP entry points 22 * 23 ******************************************************************************/ 24 25 #define LOG_TAG "bt_l2c_main" 26 27 #include <stdio.h> 28 #include <stdlib.h> 29 #include <string.h> 30 31 #include "bt_common.h" 32 #include "bt_target.h" 33 #include "btm_int.h" 34 #include "btu.h" 35 #include "device/include/controller.h" 36 #include "hcimsgs.h" 37 #include "l2c_api.h" 38 #include "l2c_int.h" 39 #include "l2cdefs.h" 40 #include "osi/include/log.h" 41 #include "osi/include/osi.h" 42 43 /******************************************************************************/ 44 /* L O C A L F U N C T I O N P R O T O T Y P E S */ 45 /******************************************************************************/ 46 static void process_l2cap_cmd(tL2C_LCB* p_lcb, uint8_t* p, uint16_t pkt_len); 47 48 /******************************************************************************/ 49 /* G L O B A L L 2 C A P D A T A */ 50 /******************************************************************************/ 51 tL2C_CB l2cb; 52 53 /******************************************************************************* 54 * 55 * Function l2c_rcv_acl_data 56 * 57 * Description This function is called from the HCI Interface when an ACL 58 * data packet is received. 59 * 60 * Returns void 61 * 62 ******************************************************************************/ 63 void l2c_rcv_acl_data(BT_HDR* p_msg) { 64 uint8_t* p = (uint8_t*)(p_msg + 1) + p_msg->offset; 65 66 /* Extract the handle */ 67 uint16_t handle; 68 STREAM_TO_UINT16(handle, p); 69 uint8_t pkt_type = HCID_GET_EVENT(handle); 70 handle = HCID_GET_HANDLE(handle); 71 72 /* Since the HCI Transport is putting segmented packets back together, we */ 73 /* should never get a valid packet with the type set to "continuation" */ 74 if (pkt_type == L2CAP_PKT_CONTINUE) { 75 L2CAP_TRACE_WARNING("L2CAP - received packet continuation"); 76 osi_free(p_msg); 77 return; 78 } 79 80 uint16_t hci_len; 81 STREAM_TO_UINT16(hci_len, p); 82 if (hci_len < L2CAP_PKT_OVERHEAD) { 83 /* Must receive at least the L2CAP length and CID */ 84 L2CAP_TRACE_WARNING("L2CAP - got incorrect hci header"); 85 osi_free(p_msg); 86 return; 87 } 88 89 uint16_t l2cap_len, rcv_cid; 90 STREAM_TO_UINT16(l2cap_len, p); 91 STREAM_TO_UINT16(rcv_cid, p); 92 93 /* Find the LCB based on the handle */ 94 tL2C_LCB* p_lcb = l2cu_find_lcb_by_handle(handle); 95 if (!p_lcb) { 96 /* There is a slight possibility (specifically with USB) that we get an */ 97 /* L2CAP connection request before we get the HCI connection complete. */ 98 /* So for these types of messages, hold them for up to 2 seconds. */ 99 uint8_t cmd_code; 100 STREAM_TO_UINT8(cmd_code, p); 101 102 if ((p_msg->layer_specific != 0) || (rcv_cid != L2CAP_SIGNALLING_CID) || 103 (cmd_code != L2CAP_CMD_INFO_REQ && cmd_code != L2CAP_CMD_CONN_REQ)) { 104 bool qcom_debug_log = 105 (handle == 3804 && cmd_code == 10 && p_msg->layer_specific == 0); 106 107 if (!qcom_debug_log) { 108 L2CAP_TRACE_ERROR( 109 "L2CAP - rcvd ACL for unknown handle:%d ls:%d cid:%d opcode:%d cur " 110 "count:%d", 111 handle, p_msg->layer_specific, rcv_cid, cmd_code, 112 list_length(l2cb.rcv_pending_q)); 113 } 114 115 osi_free(p_msg); 116 return; 117 } 118 119 L2CAP_TRACE_WARNING( 120 "L2CAP - holding ACL for unknown handle:%d ls:%d cid:%d opcode:%d cur " 121 "count:%d", 122 handle, p_msg->layer_specific, rcv_cid, cmd_code, 123 list_length(l2cb.rcv_pending_q)); 124 p_msg->layer_specific = 2; 125 list_append(l2cb.rcv_pending_q, p_msg); 126 127 if (list_length(l2cb.rcv_pending_q) == 1) { 128 alarm_set_on_mloop(l2cb.receive_hold_timer, BT_1SEC_TIMEOUT_MS, 129 l2c_receive_hold_timer_timeout, NULL); 130 } 131 return; 132 } 133 134 /* Update the buffer header */ 135 p_msg->offset += 4; 136 137 /* for BLE channel, always notify connection when ACL data received on the 138 * link */ 139 if (p_lcb && p_lcb->transport == BT_TRANSPORT_LE && 140 p_lcb->link_state != LST_DISCONNECTING) { 141 /* only process fixed channel data as channel open indication when link is 142 * not in disconnecting mode */ 143 l2cble_notify_le_connection(p_lcb->remote_bd_addr); 144 } 145 146 /* Find the CCB for this CID */ 147 tL2C_CCB* p_ccb = NULL; 148 if (rcv_cid >= L2CAP_BASE_APPL_CID) { 149 p_ccb = l2cu_find_ccb_by_cid(p_lcb, rcv_cid); 150 if (!p_ccb) { 151 L2CAP_TRACE_WARNING("L2CAP - unknown CID: 0x%04x", rcv_cid); 152 osi_free(p_msg); 153 return; 154 } 155 } 156 157 p_msg->len = hci_len - L2CAP_PKT_OVERHEAD; 158 p_msg->offset += L2CAP_PKT_OVERHEAD; 159 160 if (l2cap_len != p_msg->len) { 161 L2CAP_TRACE_WARNING("L2CAP - bad length in pkt. Exp: %d Act: %d", 162 l2cap_len, p_msg->len); 163 osi_free(p_msg); 164 return; 165 } 166 167 /* Send the data through the channel state machine */ 168 if (rcv_cid == L2CAP_SIGNALLING_CID) { 169 process_l2cap_cmd(p_lcb, p, l2cap_len); 170 osi_free(p_msg); 171 return; 172 } 173 174 if (rcv_cid == L2CAP_CONNECTIONLESS_CID) { 175 /* process_connectionless_data (p_lcb); */ 176 uint16_t psm; 177 STREAM_TO_UINT16(psm, p); 178 L2CAP_TRACE_DEBUG("GOT CONNECTIONLESS DATA PSM:%d", psm); 179 osi_free(p_msg); 180 return; 181 } 182 183 if (rcv_cid == L2CAP_BLE_SIGNALLING_CID) { 184 l2cble_process_sig_cmd(p_lcb, p, l2cap_len); 185 osi_free(p_msg); 186 return; 187 } 188 189 #if (L2CAP_NUM_FIXED_CHNLS > 0) 190 if ((rcv_cid >= L2CAP_FIRST_FIXED_CHNL) && 191 (rcv_cid <= L2CAP_LAST_FIXED_CHNL) && 192 (l2cb.fixed_reg[rcv_cid - L2CAP_FIRST_FIXED_CHNL].pL2CA_FixedData_Cb != 193 NULL)) { 194 /* only process fixed channel data when link is open or wait for data 195 * indication */ 196 if (!p_lcb || p_lcb->link_state == LST_DISCONNECTING || 197 !l2cu_initialize_fixed_ccb( 198 p_lcb, rcv_cid, 199 &l2cb.fixed_reg[rcv_cid - L2CAP_FIRST_FIXED_CHNL] 200 .fixed_chnl_opts)) { 201 osi_free(p_msg); 202 return; 203 } 204 205 /* If no CCB for this channel, allocate one */ 206 p_ccb = p_lcb->p_fixed_ccbs[rcv_cid - L2CAP_FIRST_FIXED_CHNL]; 207 208 if (p_ccb->peer_cfg.fcr.mode != L2CAP_FCR_BASIC_MODE) 209 l2c_fcr_proc_pdu(p_ccb, p_msg); 210 else 211 (*l2cb.fixed_reg[rcv_cid - L2CAP_FIRST_FIXED_CHNL].pL2CA_FixedData_Cb)( 212 rcv_cid, p_lcb->remote_bd_addr, p_msg); 213 return; 214 } 215 #endif 216 217 if (!p_ccb) { 218 osi_free(p_msg); 219 return; 220 } 221 222 if (p_lcb->transport == BT_TRANSPORT_LE) { 223 l2c_lcc_proc_pdu(p_ccb, p_msg); 224 225 /* The remote device has one less credit left */ 226 --p_ccb->remote_credit_count; 227 228 /* If the credits left on the remote device are getting low, send some */ 229 if (p_ccb->remote_credit_count <= L2CAP_LE_CREDIT_THRESHOLD) { 230 uint16_t credits = L2CAP_LE_CREDIT_DEFAULT - p_ccb->remote_credit_count; 231 p_ccb->remote_credit_count = L2CAP_LE_CREDIT_DEFAULT; 232 233 /* Return back credits */ 234 l2c_csm_execute(p_ccb, L2CEVT_L2CA_SEND_FLOW_CONTROL_CREDIT, &credits); 235 } 236 } else { 237 /* Basic mode packets go straight to the state machine */ 238 if (p_ccb->peer_cfg.fcr.mode == L2CAP_FCR_BASIC_MODE) 239 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_DATA, p_msg); 240 else { 241 /* eRTM or streaming mode, so we need to validate states first */ 242 if ((p_ccb->chnl_state == CST_OPEN) || (p_ccb->chnl_state == CST_CONFIG)) 243 l2c_fcr_proc_pdu(p_ccb, p_msg); 244 else 245 osi_free(p_msg); 246 } 247 } 248 } 249 250 /******************************************************************************* 251 * 252 * Function process_l2cap_cmd 253 * 254 * Description This function is called when a packet is received on the 255 * L2CAP signalling CID 256 * 257 * Returns void 258 * 259 ******************************************************************************/ 260 static void process_l2cap_cmd(tL2C_LCB* p_lcb, uint8_t* p, uint16_t pkt_len) { 261 tL2C_CONN_INFO con_info; 262 263 /* if l2cap command received in CID 1 on top of an LE link, ignore this 264 * command */ 265 if (p_lcb->transport == BT_TRANSPORT_LE) return; 266 267 /* Reject the packet if it exceeds the default Signalling Channel MTU */ 268 bool pkt_size_rej = false; 269 if (pkt_len > L2CAP_DEFAULT_MTU) { 270 /* Core Spec requires a single response to the first command found in a 271 * multi-command L2cap packet. If only responses in the packet, then it 272 * will be ignored. Here we simply mark the bad packet and decide which cmd 273 * ID to reject later */ 274 pkt_size_rej = true; 275 L2CAP_TRACE_ERROR("L2CAP SIG MTU pkt_len=%d Exceeded 672", pkt_len); 276 } 277 278 uint8_t* p_next_cmd = p; 279 uint8_t* p_pkt_end = p + pkt_len; 280 281 tL2CAP_CFG_INFO cfg_info; 282 memset(&cfg_info, 0, sizeof(cfg_info)); 283 284 /* An L2CAP packet may contain multiple commands */ 285 while (true) { 286 /* Smallest command is 4 bytes */ 287 p = p_next_cmd; 288 if (p > (p_pkt_end - 4)) break; 289 290 uint8_t cmd_code, id; 291 uint16_t cmd_len; 292 STREAM_TO_UINT8(cmd_code, p); 293 STREAM_TO_UINT8(id, p); 294 STREAM_TO_UINT16(cmd_len, p); 295 296 if (cmd_len > BT_SMALL_BUFFER_SIZE) { 297 L2CAP_TRACE_WARNING("L2CAP - Invalid MTU Size"); 298 l2cu_send_peer_cmd_reject(p_lcb, L2CAP_CMD_REJ_MTU_EXCEEDED, id, 0, 0); 299 return; 300 } 301 302 /* Check command length does not exceed packet length */ 303 p_next_cmd = p + cmd_len; 304 if (p_next_cmd > p_pkt_end) { 305 L2CAP_TRACE_WARNING("Command len bad pkt_len: %d cmd_len: %d code: %d", 306 pkt_len, cmd_len, cmd_code); 307 break; 308 } 309 310 L2CAP_TRACE_DEBUG("cmd_code: %d, id:%d, cmd_len:%d", cmd_code, id, cmd_len); 311 312 /* Bad L2CAP packet length, look for cmd to reject */ 313 if (pkt_size_rej) { 314 /* If command found rejected it and we're done, otherwise keep looking */ 315 if (l2c_is_cmd_rejected(cmd_code, id, p_lcb)) 316 return; 317 else 318 continue; /* Look for next cmd/response in current packet */ 319 } 320 321 switch (cmd_code) { 322 case L2CAP_CMD_REJECT: 323 uint16_t rej_reason; 324 if (p + 2 > p_next_cmd) return; 325 STREAM_TO_UINT16(rej_reason, p); 326 if (rej_reason == L2CAP_CMD_REJ_MTU_EXCEEDED) { 327 uint16_t rej_mtu; 328 if (p + 2 > p_next_cmd) return; 329 STREAM_TO_UINT16(rej_mtu, p); 330 /* What to do with the MTU reject ? We have negotiated an MTU. For now 331 * we will ignore it and let a higher protocol timeout take care of it 332 */ 333 L2CAP_TRACE_WARNING("L2CAP - MTU rej Handle: %d MTU: %d", 334 p_lcb->handle, rej_mtu); 335 } 336 if (rej_reason == L2CAP_CMD_REJ_INVALID_CID) { 337 uint16_t lcid, rcid; 338 if (p + 4 > p_next_cmd) return; 339 STREAM_TO_UINT16(rcid, p); 340 STREAM_TO_UINT16(lcid, p); 341 342 L2CAP_TRACE_WARNING( 343 "L2CAP - rej with CID invalid, LCID: 0x%04x RCID: 0x%04x", lcid, 344 rcid); 345 346 /* Remote CID invalid. Treat as a disconnect */ 347 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, lcid); 348 if ((p_ccb != NULL) && (p_ccb->remote_cid == rcid)) { 349 /* Fake link disconnect - no reply is generated */ 350 l2c_csm_execute(p_ccb, L2CEVT_LP_DISCONNECT_IND, NULL); 351 } 352 } 353 354 /* SonyEricsson Info request Bug workaround (Continue connection) */ 355 else if (rej_reason == L2CAP_CMD_REJ_NOT_UNDERSTOOD && 356 p_lcb->w4_info_rsp) { 357 alarm_cancel(p_lcb->info_resp_timer); 358 359 p_lcb->w4_info_rsp = false; 360 tL2C_CONN_INFO ci; 361 ci.status = HCI_SUCCESS; 362 ci.bd_addr = p_lcb->remote_bd_addr; 363 364 /* For all channels, send the event through their FSMs */ 365 for (tL2C_CCB* p_ccb = p_lcb->ccb_queue.p_first_ccb; p_ccb; 366 p_ccb = p_ccb->p_next_ccb) { 367 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_INFO_RSP, &ci); 368 } 369 } 370 break; 371 372 case L2CAP_CMD_CONN_REQ: { 373 uint16_t rcid; 374 if (p + 4 > p_next_cmd) return; 375 STREAM_TO_UINT16(con_info.psm, p); 376 STREAM_TO_UINT16(rcid, p); 377 tL2C_RCB* p_rcb = l2cu_find_rcb_by_psm(con_info.psm); 378 if (!p_rcb) { 379 L2CAP_TRACE_WARNING("L2CAP - rcvd conn req for unknown PSM: %d", 380 con_info.psm); 381 l2cu_reject_connection(p_lcb, rcid, id, L2CAP_CONN_NO_PSM); 382 break; 383 } else { 384 if (!p_rcb->api.pL2CA_ConnectInd_Cb) { 385 L2CAP_TRACE_WARNING( 386 "L2CAP - rcvd conn req for outgoing-only connection PSM: %d", 387 con_info.psm); 388 l2cu_reject_connection(p_lcb, rcid, id, L2CAP_CONN_NO_PSM); 389 break; 390 } 391 } 392 tL2C_CCB* p_ccb = l2cu_allocate_ccb(p_lcb, 0); 393 if (!p_ccb) { 394 L2CAP_TRACE_ERROR("L2CAP - unable to allocate CCB"); 395 l2cu_reject_connection(p_lcb, rcid, id, L2CAP_CONN_NO_RESOURCES); 396 break; 397 } 398 p_ccb->remote_id = id; 399 p_ccb->p_rcb = p_rcb; 400 p_ccb->remote_cid = rcid; 401 402 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONNECT_REQ, &con_info); 403 break; 404 } 405 406 case L2CAP_CMD_CONN_RSP: { 407 uint16_t lcid; 408 if (p + 8 > p_next_cmd) return; 409 STREAM_TO_UINT16(con_info.remote_cid, p); 410 STREAM_TO_UINT16(lcid, p); 411 STREAM_TO_UINT16(con_info.l2cap_result, p); 412 STREAM_TO_UINT16(con_info.l2cap_status, p); 413 414 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, lcid); 415 if (!p_ccb) { 416 L2CAP_TRACE_WARNING("L2CAP - no CCB for conn rsp, LCID: %d RCID: %d", 417 lcid, con_info.remote_cid); 418 break; 419 } 420 if (p_ccb->local_id != id) { 421 L2CAP_TRACE_WARNING("L2CAP - con rsp - bad ID. Exp: %d Got: %d", 422 p_ccb->local_id, id); 423 break; 424 } 425 426 if (con_info.l2cap_result == L2CAP_CONN_OK) 427 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONNECT_RSP, &con_info); 428 else if (con_info.l2cap_result == L2CAP_CONN_PENDING) 429 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONNECT_RSP_PND, &con_info); 430 else 431 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONNECT_RSP_NEG, &con_info); 432 433 break; 434 } 435 436 case L2CAP_CMD_CONFIG_REQ: { 437 uint8_t* p_cfg_end = p + cmd_len; 438 bool cfg_rej = false; 439 uint16_t cfg_rej_len = 0; 440 441 uint16_t lcid; 442 if (p + 4 > p_next_cmd) return; 443 STREAM_TO_UINT16(lcid, p); 444 STREAM_TO_UINT16(cfg_info.flags, p); 445 446 uint8_t* p_cfg_start = p; 447 448 cfg_info.flush_to_present = cfg_info.mtu_present = 449 cfg_info.qos_present = cfg_info.fcr_present = cfg_info.fcs_present = 450 false; 451 452 while (p < p_cfg_end) { 453 uint8_t cfg_code, cfg_len; 454 if (p + 2 > p_next_cmd) return; 455 STREAM_TO_UINT8(cfg_code, p); 456 STREAM_TO_UINT8(cfg_len, p); 457 458 switch (cfg_code & 0x7F) { 459 case L2CAP_CFG_TYPE_MTU: 460 cfg_info.mtu_present = true; 461 if (p + 2 > p_next_cmd) return; 462 STREAM_TO_UINT16(cfg_info.mtu, p); 463 break; 464 465 case L2CAP_CFG_TYPE_FLUSH_TOUT: 466 cfg_info.flush_to_present = true; 467 if (p + 2 > p_next_cmd) return; 468 STREAM_TO_UINT16(cfg_info.flush_to, p); 469 break; 470 471 case L2CAP_CFG_TYPE_QOS: 472 cfg_info.qos_present = true; 473 if (p + 2 + 5 * 4 > p_next_cmd) return; 474 STREAM_TO_UINT8(cfg_info.qos.qos_flags, p); 475 STREAM_TO_UINT8(cfg_info.qos.service_type, p); 476 STREAM_TO_UINT32(cfg_info.qos.token_rate, p); 477 STREAM_TO_UINT32(cfg_info.qos.token_bucket_size, p); 478 STREAM_TO_UINT32(cfg_info.qos.peak_bandwidth, p); 479 STREAM_TO_UINT32(cfg_info.qos.latency, p); 480 STREAM_TO_UINT32(cfg_info.qos.delay_variation, p); 481 break; 482 483 case L2CAP_CFG_TYPE_FCR: 484 cfg_info.fcr_present = true; 485 if (p + 3 + 3 * 2 > p_next_cmd) return; 486 STREAM_TO_UINT8(cfg_info.fcr.mode, p); 487 STREAM_TO_UINT8(cfg_info.fcr.tx_win_sz, p); 488 STREAM_TO_UINT8(cfg_info.fcr.max_transmit, p); 489 STREAM_TO_UINT16(cfg_info.fcr.rtrans_tout, p); 490 STREAM_TO_UINT16(cfg_info.fcr.mon_tout, p); 491 STREAM_TO_UINT16(cfg_info.fcr.mps, p); 492 break; 493 494 case L2CAP_CFG_TYPE_FCS: 495 cfg_info.fcs_present = true; 496 if (p + 1 > p_next_cmd) return; 497 STREAM_TO_UINT8(cfg_info.fcs, p); 498 break; 499 500 case L2CAP_CFG_TYPE_EXT_FLOW: 501 cfg_info.ext_flow_spec_present = true; 502 if (p + 2 + 2 + 3 * 4 > p_next_cmd) return; 503 STREAM_TO_UINT8(cfg_info.ext_flow_spec.id, p); 504 STREAM_TO_UINT8(cfg_info.ext_flow_spec.stype, p); 505 STREAM_TO_UINT16(cfg_info.ext_flow_spec.max_sdu_size, p); 506 STREAM_TO_UINT32(cfg_info.ext_flow_spec.sdu_inter_time, p); 507 STREAM_TO_UINT32(cfg_info.ext_flow_spec.access_latency, p); 508 STREAM_TO_UINT32(cfg_info.ext_flow_spec.flush_timeout, p); 509 break; 510 511 default: 512 /* sanity check option length */ 513 if ((cfg_len + L2CAP_CFG_OPTION_OVERHEAD) <= cmd_len) { 514 p += cfg_len; 515 if ((cfg_code & 0x80) == 0) { 516 cfg_rej_len += cfg_len + L2CAP_CFG_OPTION_OVERHEAD; 517 cfg_rej = true; 518 } 519 } 520 /* bad length; force loop exit */ 521 else { 522 p = p_cfg_end; 523 cfg_rej = true; 524 } 525 break; 526 } 527 } 528 529 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, lcid); 530 if (p_ccb) { 531 p_ccb->remote_id = id; 532 if (cfg_rej) { 533 l2cu_send_peer_config_rej( 534 p_ccb, p_cfg_start, (uint16_t)(cmd_len - L2CAP_CONFIG_REQ_LEN), 535 cfg_rej_len); 536 } else { 537 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONFIG_REQ, &cfg_info); 538 } 539 } else { 540 /* updated spec says send command reject on invalid cid */ 541 l2cu_send_peer_cmd_reject(p_lcb, L2CAP_CMD_REJ_INVALID_CID, id, 0, 0); 542 } 543 break; 544 } 545 546 case L2CAP_CMD_CONFIG_RSP: { 547 uint8_t* p_cfg_end = p + cmd_len; 548 uint16_t lcid; 549 if (p + 6 > p_next_cmd) return; 550 STREAM_TO_UINT16(lcid, p); 551 STREAM_TO_UINT16(cfg_info.flags, p); 552 STREAM_TO_UINT16(cfg_info.result, p); 553 554 cfg_info.flush_to_present = cfg_info.mtu_present = 555 cfg_info.qos_present = cfg_info.fcr_present = cfg_info.fcs_present = 556 false; 557 558 while (p < p_cfg_end) { 559 uint8_t cfg_code, cfg_len; 560 if (p + 2 > p_next_cmd) return; 561 STREAM_TO_UINT8(cfg_code, p); 562 STREAM_TO_UINT8(cfg_len, p); 563 564 switch (cfg_code & 0x7F) { 565 case L2CAP_CFG_TYPE_MTU: 566 cfg_info.mtu_present = true; 567 if (p + 2 > p_next_cmd) return; 568 STREAM_TO_UINT16(cfg_info.mtu, p); 569 break; 570 571 case L2CAP_CFG_TYPE_FLUSH_TOUT: 572 cfg_info.flush_to_present = true; 573 if (p + 2 > p_next_cmd) return; 574 STREAM_TO_UINT16(cfg_info.flush_to, p); 575 break; 576 577 case L2CAP_CFG_TYPE_QOS: 578 cfg_info.qos_present = true; 579 if (p + 2 + 5 * 4 > p_next_cmd) return; 580 STREAM_TO_UINT8(cfg_info.qos.qos_flags, p); 581 STREAM_TO_UINT8(cfg_info.qos.service_type, p); 582 STREAM_TO_UINT32(cfg_info.qos.token_rate, p); 583 STREAM_TO_UINT32(cfg_info.qos.token_bucket_size, p); 584 STREAM_TO_UINT32(cfg_info.qos.peak_bandwidth, p); 585 STREAM_TO_UINT32(cfg_info.qos.latency, p); 586 STREAM_TO_UINT32(cfg_info.qos.delay_variation, p); 587 break; 588 589 case L2CAP_CFG_TYPE_FCR: 590 cfg_info.fcr_present = true; 591 if (p + 3 + 3 * 2 > p_next_cmd) return; 592 STREAM_TO_UINT8(cfg_info.fcr.mode, p); 593 STREAM_TO_UINT8(cfg_info.fcr.tx_win_sz, p); 594 STREAM_TO_UINT8(cfg_info.fcr.max_transmit, p); 595 STREAM_TO_UINT16(cfg_info.fcr.rtrans_tout, p); 596 STREAM_TO_UINT16(cfg_info.fcr.mon_tout, p); 597 STREAM_TO_UINT16(cfg_info.fcr.mps, p); 598 break; 599 600 case L2CAP_CFG_TYPE_FCS: 601 cfg_info.fcs_present = true; 602 if (p + 1 > p_next_cmd) return; 603 STREAM_TO_UINT8(cfg_info.fcs, p); 604 break; 605 606 case L2CAP_CFG_TYPE_EXT_FLOW: 607 cfg_info.ext_flow_spec_present = true; 608 if (p + 2 + 2 + 3 * 4 > p_next_cmd) return; 609 STREAM_TO_UINT8(cfg_info.ext_flow_spec.id, p); 610 STREAM_TO_UINT8(cfg_info.ext_flow_spec.stype, p); 611 STREAM_TO_UINT16(cfg_info.ext_flow_spec.max_sdu_size, p); 612 STREAM_TO_UINT32(cfg_info.ext_flow_spec.sdu_inter_time, p); 613 STREAM_TO_UINT32(cfg_info.ext_flow_spec.access_latency, p); 614 STREAM_TO_UINT32(cfg_info.ext_flow_spec.flush_timeout, p); 615 break; 616 } 617 } 618 619 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, lcid); 620 if (p_ccb) { 621 if (p_ccb->local_id != id) { 622 L2CAP_TRACE_WARNING("L2CAP - cfg rsp - bad ID. Exp: %d Got: %d", 623 p_ccb->local_id, id); 624 break; 625 } 626 if ((cfg_info.result == L2CAP_CFG_OK) || 627 (cfg_info.result == L2CAP_CFG_PENDING)) 628 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONFIG_RSP, &cfg_info); 629 else 630 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONFIG_RSP_NEG, &cfg_info); 631 } else { 632 L2CAP_TRACE_WARNING("L2CAP - rcvd cfg rsp for unknown CID: 0x%04x", 633 lcid); 634 } 635 break; 636 } 637 638 case L2CAP_CMD_DISC_REQ: { 639 uint16_t lcid, rcid; 640 if (p + 4 > p_next_cmd) return; 641 STREAM_TO_UINT16(lcid, p); 642 STREAM_TO_UINT16(rcid, p); 643 644 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, lcid); 645 if (p_ccb) { 646 if (p_ccb->remote_cid == rcid) { 647 p_ccb->remote_id = id; 648 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_DISCONNECT_REQ, &con_info); 649 } 650 } else 651 l2cu_send_peer_disc_rsp(p_lcb, id, lcid, rcid); 652 653 break; 654 } 655 656 case L2CAP_CMD_DISC_RSP: { 657 uint16_t lcid, rcid; 658 if (p + 4 > p_next_cmd) return; 659 STREAM_TO_UINT16(rcid, p); 660 STREAM_TO_UINT16(lcid, p); 661 662 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, lcid); 663 if (p_ccb) { 664 if ((p_ccb->remote_cid == rcid) && (p_ccb->local_id == id)) { 665 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_DISCONNECT_RSP, &con_info); 666 } 667 } 668 break; 669 } 670 671 case L2CAP_CMD_ECHO_REQ: 672 l2cu_send_peer_echo_rsp(p_lcb, id, p, cmd_len); 673 break; 674 675 case L2CAP_CMD_ECHO_RSP: 676 if (p_lcb->p_echo_rsp_cb) { 677 tL2CA_ECHO_RSP_CB* p_cb = p_lcb->p_echo_rsp_cb; 678 679 /* Zero out the callback in case app immediately calls us again */ 680 p_lcb->p_echo_rsp_cb = NULL; 681 682 (*p_cb)(L2CAP_PING_RESULT_OK); 683 } 684 break; 685 686 case L2CAP_CMD_INFO_REQ: { 687 uint16_t info_type; 688 if (p + 2 > p_next_cmd) return; 689 STREAM_TO_UINT16(info_type, p); 690 l2cu_send_peer_info_rsp(p_lcb, id, info_type); 691 break; 692 } 693 694 case L2CAP_CMD_INFO_RSP: 695 /* Stop the link connect timer if sent before L2CAP connection is up */ 696 if (p_lcb->w4_info_rsp) { 697 alarm_cancel(p_lcb->info_resp_timer); 698 p_lcb->w4_info_rsp = false; 699 } 700 701 uint16_t info_type, result; 702 if (p + 4 > p_next_cmd) return; 703 STREAM_TO_UINT16(info_type, p); 704 STREAM_TO_UINT16(result, p); 705 706 p_lcb->info_rx_bits |= (1 << info_type); 707 708 if ((info_type == L2CAP_EXTENDED_FEATURES_INFO_TYPE) && 709 (result == L2CAP_INFO_RESP_RESULT_SUCCESS)) { 710 if (p + 4 > p_next_cmd) return; 711 STREAM_TO_UINT32(p_lcb->peer_ext_fea, p); 712 713 #if (L2CAP_NUM_FIXED_CHNLS > 0) 714 if (p_lcb->peer_ext_fea & L2CAP_EXTFEA_FIXED_CHNLS) { 715 l2cu_send_peer_info_req(p_lcb, L2CAP_FIXED_CHANNELS_INFO_TYPE); 716 break; 717 } else { 718 l2cu_process_fixed_chnl_resp(p_lcb); 719 } 720 #endif 721 } 722 723 #if (L2CAP_NUM_FIXED_CHNLS > 0) 724 if (info_type == L2CAP_FIXED_CHANNELS_INFO_TYPE) { 725 if (result == L2CAP_INFO_RESP_RESULT_SUCCESS) { 726 memcpy(p_lcb->peer_chnl_mask, p, L2CAP_FIXED_CHNL_ARRAY_SIZE); 727 } 728 729 l2cu_process_fixed_chnl_resp(p_lcb); 730 } 731 #endif 732 tL2C_CONN_INFO ci; 733 ci.status = HCI_SUCCESS; 734 ci.bd_addr = p_lcb->remote_bd_addr; 735 for (tL2C_CCB* p_ccb = p_lcb->ccb_queue.p_first_ccb; p_ccb; 736 p_ccb = p_ccb->p_next_ccb) { 737 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_INFO_RSP, &ci); 738 } 739 break; 740 741 default: 742 L2CAP_TRACE_WARNING("L2CAP - bad cmd code: %d", cmd_code); 743 l2cu_send_peer_cmd_reject(p_lcb, L2CAP_CMD_REJ_NOT_UNDERSTOOD, id, 0, 744 0); 745 return; 746 } 747 } 748 } 749 750 /******************************************************************************* 751 * 752 * Function l2c_process_held_packets 753 * 754 * Description This function processes any L2CAP packets that arrived 755 * before the HCI connection complete arrived. It is a work 756 * around for badly behaved controllers. 757 * 758 * Returns void 759 * 760 ******************************************************************************/ 761 void l2c_process_held_packets(bool timed_out) { 762 if (list_is_empty(l2cb.rcv_pending_q)) return; 763 764 if (!timed_out) { 765 alarm_cancel(l2cb.receive_hold_timer); 766 L2CAP_TRACE_WARNING("L2CAP HOLD CONTINUE"); 767 } else { 768 L2CAP_TRACE_WARNING("L2CAP HOLD TIMEOUT"); 769 } 770 771 for (const list_node_t* node = list_begin(l2cb.rcv_pending_q); 772 node != list_end(l2cb.rcv_pending_q);) { 773 BT_HDR* p_buf = static_cast<BT_HDR*>(list_node(node)); 774 node = list_next(node); 775 if (!timed_out || (!p_buf->layer_specific) || 776 (--p_buf->layer_specific == 0)) { 777 list_remove(l2cb.rcv_pending_q, p_buf); 778 p_buf->layer_specific = 0xFFFF; 779 l2c_rcv_acl_data(p_buf); 780 } 781 } 782 783 /* If anyone still in the queue, restart the timeout */ 784 if (!list_is_empty(l2cb.rcv_pending_q)) { 785 alarm_set_on_mloop(l2cb.receive_hold_timer, BT_1SEC_TIMEOUT_MS, 786 l2c_receive_hold_timer_timeout, NULL); 787 } 788 } 789 790 /******************************************************************************* 791 * 792 * Function l2c_init 793 * 794 * Description This function is called once at startup to initialize 795 * all the L2CAP structures 796 * 797 * Returns void 798 * 799 ******************************************************************************/ 800 void l2c_init(void) { 801 int16_t xx; 802 803 memset(&l2cb, 0, sizeof(tL2C_CB)); 804 /* the psm is increased by 2 before being used */ 805 l2cb.dyn_psm = 0xFFF; 806 807 /* the LE PSM is increased by 1 before being used */ 808 l2cb.le_dyn_psm = LE_DYNAMIC_PSM_START - 1; 809 810 /* Put all the channel control blocks on the free queue */ 811 for (xx = 0; xx < MAX_L2CAP_CHANNELS - 1; xx++) { 812 l2cb.ccb_pool[xx].p_next_ccb = &l2cb.ccb_pool[xx + 1]; 813 } 814 815 #if (L2CAP_NON_FLUSHABLE_PB_INCLUDED == TRUE) 816 /* it will be set to L2CAP_PKT_START_NON_FLUSHABLE if controller supports */ 817 l2cb.non_flushable_pbf = L2CAP_PKT_START << L2CAP_PKT_TYPE_SHIFT; 818 #endif 819 820 l2cb.p_free_ccb_first = &l2cb.ccb_pool[0]; 821 l2cb.p_free_ccb_last = &l2cb.ccb_pool[MAX_L2CAP_CHANNELS - 1]; 822 823 #ifdef L2CAP_DESIRED_LINK_ROLE 824 l2cb.desire_role = L2CAP_DESIRED_LINK_ROLE; 825 #else 826 l2cb.desire_role = HCI_ROLE_SLAVE; 827 #endif 828 829 /* Set the default idle timeout */ 830 l2cb.idle_timeout = L2CAP_LINK_INACTIVITY_TOUT; 831 832 #if defined(L2CAP_INITIAL_TRACE_LEVEL) 833 l2cb.l2cap_trace_level = L2CAP_INITIAL_TRACE_LEVEL; 834 #else 835 l2cb.l2cap_trace_level = BT_TRACE_LEVEL_NONE; /* No traces */ 836 #endif 837 838 #if (L2CAP_CONFORMANCE_TESTING == TRUE) 839 /* Conformance testing needs a dynamic response */ 840 l2cb.test_info_resp = L2CAP_EXTFEA_SUPPORTED_MASK; 841 #endif 842 843 /* Number of ACL buffers to use for high priority channel */ 844 #if (L2CAP_HIGH_PRI_CHAN_QUOTA_IS_CONFIGURABLE == TRUE) 845 l2cb.high_pri_min_xmit_quota = L2CAP_HIGH_PRI_MIN_XMIT_QUOTA; 846 #endif 847 848 l2cb.l2c_ble_fixed_chnls_mask = L2CAP_FIXED_CHNL_ATT_BIT | 849 L2CAP_FIXED_CHNL_BLE_SIG_BIT | 850 L2CAP_FIXED_CHNL_SMP_BIT; 851 852 l2cb.rcv_pending_q = list_new(NULL); 853 CHECK(l2cb.rcv_pending_q != NULL); 854 855 l2cb.receive_hold_timer = alarm_new("l2c.receive_hold_timer"); 856 } 857 858 void l2c_free(void) { 859 list_free(l2cb.rcv_pending_q); 860 l2cb.rcv_pending_q = NULL; 861 } 862 863 void l2c_receive_hold_timer_timeout(UNUSED_ATTR void* data) { 864 /* Update the timeouts in the hold queue */ 865 l2c_process_held_packets(true); 866 } 867 868 void l2c_ccb_timer_timeout(void* data) { 869 tL2C_CCB* p_ccb = (tL2C_CCB*)data; 870 871 l2c_csm_execute(p_ccb, L2CEVT_TIMEOUT, NULL); 872 } 873 874 void l2c_fcrb_ack_timer_timeout(void* data) { 875 tL2C_CCB* p_ccb = (tL2C_CCB*)data; 876 877 l2c_csm_execute(p_ccb, L2CEVT_ACK_TIMEOUT, NULL); 878 } 879 880 void l2c_lcb_timer_timeout(void* data) { 881 tL2C_LCB* p_lcb = (tL2C_LCB*)data; 882 883 l2c_link_timeout(p_lcb); 884 } 885 886 /******************************************************************************* 887 * 888 * Function l2c_data_write 889 * 890 * Description API functions call this function to write data. 891 * 892 * Returns L2CAP_DW_SUCCESS, if data accepted, else false 893 * L2CAP_DW_CONGESTED, if data accepted and the channel is 894 * congested 895 * L2CAP_DW_FAILED, if error 896 * 897 ******************************************************************************/ 898 uint8_t l2c_data_write(uint16_t cid, BT_HDR* p_data, uint16_t flags) { 899 900 /* Find the channel control block. We don't know the link it is on. */ 901 tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(NULL, cid); 902 if (!p_ccb) { 903 L2CAP_TRACE_WARNING("L2CAP - no CCB for L2CA_DataWrite, CID: %d", cid); 904 osi_free(p_data); 905 return (L2CAP_DW_FAILED); 906 } 907 908 #ifndef TESTER 909 /* Tester may send any amount of data. otherwise sending message bigger than 910 * mtu size of peer is a violation of protocol */ 911 uint16_t mtu; 912 913 if (p_ccb->p_lcb->transport == BT_TRANSPORT_LE) 914 mtu = p_ccb->peer_conn_cfg.mtu; 915 else 916 mtu = p_ccb->peer_cfg.mtu; 917 918 if (p_data->len > mtu) { 919 L2CAP_TRACE_WARNING( 920 "L2CAP - CID: 0x%04x cannot send message bigger than peer's mtu size: " 921 "len=%u mtu=%u", 922 cid, p_data->len, mtu); 923 osi_free(p_data); 924 return (L2CAP_DW_FAILED); 925 } 926 #endif 927 928 /* channel based, packet based flushable or non-flushable */ 929 p_data->layer_specific = flags; 930 931 /* If already congested, do not accept any more packets */ 932 if (p_ccb->cong_sent) { 933 L2CAP_TRACE_ERROR( 934 "L2CAP - CID: 0x%04x cannot send, already congested " 935 "xmit_hold_q.count: %u buff_quota: %u", 936 p_ccb->local_cid, fixed_queue_length(p_ccb->xmit_hold_q), 937 p_ccb->buff_quota); 938 939 osi_free(p_data); 940 return (L2CAP_DW_FAILED); 941 } 942 943 l2c_csm_execute(p_ccb, L2CEVT_L2CA_DATA_WRITE, p_data); 944 945 if (p_ccb->cong_sent) return (L2CAP_DW_CONGESTED); 946 947 return (L2CAP_DW_SUCCESS); 948 } 949
