1 /* 2 * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com 3 * All rights reserved. Distributed under the terms of the MIT License. 4 */ 5 #include <KernelExport.h> 6 #include <string.h> 7 8 #include <net/if_dl.h> 9 #include <net_datalink.h> 10 #include <NetBufferUtilities.h> 11 12 #include <l2cap.h> 13 #include "L2capEndpoint.h" 14 #include "L2capEndpointManager.h" 15 #include "l2cap_internal.h" 16 #include "l2cap_signal.h" 17 #include "l2cap_command.h" 18 19 #include <btDebug.h> 20 21 #include <bluetooth/L2CAP/btL2CAP.h> 22 #include <bluetooth/HCI/btHCI_command.h> 23 24 25 struct l2cap_config_options { 26 uint16 mtu; 27 bool mtu_set; 28 uint16 flush_timeout; 29 bool flush_timeout_set; 30 l2cap_qos qos; 31 bool qos_set; 32 net_buffer* rejected; 33 }; 34 35 36 // #pragma mark - incoming signals 37 38 39 static status_t 40 l2cap_handle_connection_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 41 { 42 NetBufferHeaderReader<l2cap_connection_req> command(buffer); 43 if (command.Status() != B_OK) 44 return ENOBUFS; 45 46 const uint16 psm = le16toh(command->psm); 47 const uint16 scid = le16toh(command->scid); 48 49 L2capEndpoint* endpoint = gL2capEndpointManager.ForPSM(psm); 50 if (endpoint == NULL) { 51 // Refuse connection. 52 send_l2cap_connection_rsp(conn, ident, 0, scid, 53 l2cap_connection_rsp::RESULT_PSM_NOT_SUPPORTED, 0); 54 return B_OK; 55 } 56 57 endpoint->_HandleConnectionReq(conn, ident, psm, scid); 58 return B_OK; 59 } 60 61 62 static void 63 l2cap_handle_connection_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer) 64 { 65 NetBufferHeaderReader<l2cap_connection_rsp> command(buffer); 66 if (command.Status() != B_OK) 67 return; 68 69 l2cap_connection_rsp response; 70 response.dcid = le16toh(command->dcid); 71 response.scid = le16toh(command->scid); 72 response.result = le16toh(command->result); 73 response.status = le16toh(command->status); 74 75 TRACE("%s: dcid=%d scid=%d result=%d status%d\n", 76 __func__, response.dcid, response.scid, response.result, response.status); 77 78 endpoint->_HandleConnectionRsp(ident, response); 79 } 80 81 82 static void 83 parse_configuration_options(net_buffer* buffer, size_t offset, uint16 length, 84 l2cap_config_options& options) 85 { 86 while (offset < length) { 87 l2cap_configuration_option option; 88 if (gBufferModule->read(buffer, offset, &option, sizeof(option)) != B_OK) 89 break; 90 91 l2cap_configuration_option_value value; 92 if (gBufferModule->read(buffer, offset + sizeof(option), &value, 93 min_c(sizeof(value), option.length)) != B_OK) { 94 break; 95 } 96 97 const bool hint = (option.type & l2cap_configuration_option::OPTION_HINT_BIT); 98 option.type &= ~l2cap_configuration_option::OPTION_HINT_BIT; 99 100 switch (option.type) { 101 case l2cap_configuration_option::OPTION_MTU: 102 options.mtu = le16toh(value.mtu); 103 options.mtu_set = true; 104 break; 105 106 case l2cap_configuration_option::OPTION_FLUSH_TIMEOUT: 107 options.flush_timeout = le16toh(value.flush_timeout); 108 options.flush_timeout_set = true; 109 break; 110 111 case l2cap_configuration_option::OPTION_QOS: 112 options.qos.flags = value.qos.flags; 113 options.qos.service_type = value.qos.service_type; 114 options.qos.token_rate = le32toh(value.qos.token_rate); 115 options.qos.token_bucket_size = le32toh(value.qos.token_bucket_size); 116 options.qos.peak_bandwidth = le32toh(value.qos.peak_bandwidth); 117 options.qos.access_latency = le32toh(value.qos.access_latency); 118 options.qos.delay_variation = le32toh(value.qos.delay_variation); 119 options.qos_set = true; 120 break; 121 122 default: { 123 if (hint) 124 break; 125 126 // Unknown option: we need to reject it. 127 if (options.rejected == NULL) 128 options.rejected = gBufferModule->create(128); 129 gBufferModule->append_cloned(options.rejected, buffer, offset, 130 sizeof(option) + option.length); 131 } 132 } 133 134 offset += sizeof(option) + option.length; 135 } 136 } 137 138 139 static status_t 140 l2cap_handle_configuration_req(HciConnection* conn, uint8 ident, net_buffer* buffer, uint16 length) 141 { 142 NetBufferHeaderReader<l2cap_configuration_req> command(buffer); 143 if (command.Status() != B_OK) 144 return ENOBUFS; 145 146 const uint16 dcid = le16toh(command->dcid); 147 L2capEndpoint* endpoint = gL2capEndpointManager.ForChannel(dcid); 148 if (endpoint == NULL) { 149 ERROR("l2cap: unexpected configuration req: channel does not exist (cid=%d)\n", dcid); 150 send_l2cap_command_reject(conn, ident, 151 l2cap_command_reject::REJECTED_INVALID_CID, 0, 0, 0); 152 return B_ERROR; 153 } 154 155 const uint16 flags = le16toh(command->flags); 156 157 // Read options (if any). 158 l2cap_config_options options = {}; 159 parse_configuration_options(buffer, sizeof(l2cap_configuration_req), length, options); 160 161 if (options.rejected != NULL) { 162 // Reject without doing anything else. 163 send_l2cap_configuration_rsp(conn, ident, dcid, 0, 164 l2cap_configuration_rsp::RESULT_UNKNOWN_OPTION, options.rejected); 165 return B_OK; 166 } 167 168 endpoint->_HandleConfigurationReq(ident, flags, 169 options.mtu_set ? &options.mtu : NULL, 170 options.flush_timeout_set ? &options.flush_timeout : NULL, 171 options.qos_set ? &options.qos : NULL); 172 return B_OK; 173 } 174 175 176 static status_t 177 l2cap_handle_configuration_rsp(HciConnection* conn, L2capEndpoint* endpoint, 178 uint8 ident, net_buffer* buffer, uint16 length, bool& releaseIdent) 179 { 180 if (endpoint == NULL) { 181 // The endpoint seems to be gone. 182 return B_ERROR; 183 } 184 185 NetBufferHeaderReader<l2cap_configuration_rsp> command(buffer); 186 if (command.Status() != B_OK) 187 return ENOBUFS; 188 189 const uint16 scid = le16toh(command->scid); 190 const uint16 flags = le16toh(command->flags); 191 const uint16 result = le16toh(command->result); 192 releaseIdent = (flags & L2CAP_CFG_FLAG_CONTINUATION) == 0; 193 194 // Read options (if any). 195 l2cap_config_options options = {}; 196 parse_configuration_options(buffer, sizeof(l2cap_configuration_rsp), length, options); 197 198 if (options.rejected != NULL) { 199 // Reject without doing anything else. 200 send_l2cap_command_reject(conn, ident, 201 l2cap_command_reject::REJECTED_NOT_UNDERSTOOD, 0, 0, 0); 202 return B_OK; 203 } 204 205 endpoint->_HandleConfigurationRsp(ident, scid, flags, result, 206 options.mtu_set ? &options.mtu : NULL, 207 options.flush_timeout_set ? &options.flush_timeout : NULL, 208 options.qos_set ? &options.qos : NULL); 209 return B_OK; 210 } 211 212 213 static status_t 214 l2cap_handle_disconnection_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 215 { 216 NetBufferHeaderReader<l2cap_disconnection_req> command(buffer); 217 if (command.Status() != B_OK) 218 return ENOBUFS; 219 220 const uint16 dcid = le16toh(command->dcid); 221 L2capEndpoint* endpoint = gL2capEndpointManager.ForChannel(dcid); 222 if (endpoint == NULL) { 223 ERROR("l2cap: unexpected disconnection req: channel does not exist (cid=%d)\n", dcid); 224 send_l2cap_command_reject(conn, ident, 225 l2cap_command_reject::REJECTED_INVALID_CID, 0, 0, 0); 226 return B_ERROR; 227 } 228 229 const uint16 scid = le16toh(command->scid); 230 231 endpoint->_HandleDisconnectionReq(ident, scid); 232 return B_OK; 233 } 234 235 236 static status_t 237 l2cap_handle_disconnection_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer) 238 { 239 NetBufferHeaderReader<l2cap_disconnection_rsp> command(buffer); 240 if (command.Status() != B_OK) 241 return ENOBUFS; 242 243 const uint16 dcid = le16toh(command->dcid); 244 const uint16 scid = le16toh(command->scid); 245 246 endpoint->_HandleDisconnectionRsp(ident, dcid, scid); 247 return B_OK; 248 } 249 250 251 static status_t 252 l2cap_handle_echo_req(HciConnection *conn, uint8 ident, net_buffer* buffer, uint16 length) 253 { 254 net_buffer* reply = gBufferModule->clone(buffer, false); 255 if (reply == NULL) 256 return ENOMEM; 257 258 gBufferModule->trim(reply, length); 259 return send_l2cap_command(conn, L2CAP_ECHO_RSP, ident, reply); 260 } 261 262 263 static status_t 264 l2cap_handle_echo_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer, uint16 length) 265 { 266 // Not currently triggered by this module. 267 return B_OK; 268 } 269 270 271 static status_t 272 l2cap_handle_info_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 273 { 274 NetBufferHeaderReader<l2cap_information_req> command(buffer); 275 if (command.Status() != B_OK) 276 return ENOBUFS; 277 278 const uint16 type = le16toh(command->type); 279 280 net_buffer* reply = NULL; 281 uint8 replyCode = 0; 282 switch (type) { 283 case l2cap_information_req::TYPE_CONNECTIONLESS_MTU: 284 reply = make_l2cap_information_rsp(replyCode, type, 285 l2cap_information_rsp::RESULT_SUCCESS, L2CAP_MTU_DEFAULT); 286 break; 287 288 default: 289 ERROR("l2cap: unhandled information request type %d\n", type); 290 reply = make_l2cap_information_rsp(replyCode, type, 291 l2cap_information_rsp::RESULT_NOT_SUPPORTED, 0); 292 break; 293 } 294 295 return send_l2cap_command(conn, replyCode, ident, reply); 296 } 297 298 299 static status_t 300 l2cap_handle_info_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer) 301 { 302 // Not currently triggered by this module. 303 return B_OK; 304 } 305 306 307 static status_t 308 l2cap_handle_command_reject(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer) 309 { 310 if (endpoint == NULL) { 311 ERROR("l2cap: unexpected command rejected: ident %d unknown\n", ident); 312 return B_ERROR; 313 } 314 315 NetBufferHeaderReader<l2cap_command_reject> command(buffer); 316 if (command.Status() != B_OK) 317 return ENOBUFS; 318 319 const uint16 reason = le16toh(command->reason); 320 TRACE("%s: reason=%d\n", __func__, command->reason); 321 322 l2cap_command_reject_data data = {}; 323 gBufferModule->read(buffer, sizeof(l2cap_command_reject), 324 &data, min_c(buffer->size, sizeof(l2cap_command_reject_data))); 325 326 endpoint->_HandleCommandRejected(ident, reason, data); 327 328 return B_OK; 329 } 330 331 332 // #pragma mark - outgoing signals 333 334 335 status_t 336 send_l2cap_command(HciConnection* conn, uint8 code, uint8 ident, net_buffer* command) 337 { 338 // TODO: Command timeouts (probably in L2capEndpoint.) 339 { 340 NetBufferPrepend<l2cap_command_header> header(command); 341 header->code = code; 342 header->ident = ident; 343 header->length = htole16(command->size - sizeof(l2cap_command_header)); 344 } { 345 NetBufferPrepend<l2cap_basic_header> basicHeader(command); 346 basicHeader->length = htole16(command->size - sizeof(l2cap_basic_header)); 347 basicHeader->dcid = htole16(L2CAP_SIGNALING_CID); 348 } 349 command->type = conn->handle; 350 status_t status = btDevices->PostACL(conn->Hid, command); 351 if (status != B_OK) 352 gBufferModule->free(command); 353 return status; 354 } 355 356 357 status_t 358 send_l2cap_command_reject(HciConnection* conn, uint8 ident, uint16 reason, 359 uint16 mtu, uint16 scid, uint16 dcid) 360 { 361 uint8 code = 0; 362 net_buffer* buffer = make_l2cap_command_reject(code, reason, mtu, scid, dcid); 363 if (buffer == NULL) 364 return ENOMEM; 365 366 return send_l2cap_command(conn, code, ident, buffer); 367 } 368 369 370 status_t 371 send_l2cap_configuration_req(HciConnection* conn, uint8 ident, uint16 dcid, uint16 flags, 372 uint16* mtu, uint16* flush_timeout, l2cap_qos* flow) 373 { 374 uint8 code = 0; 375 net_buffer* buffer = make_l2cap_configuration_req(code, dcid, flags, mtu, flush_timeout, flow); 376 if (buffer == NULL) 377 return ENOMEM; 378 379 return send_l2cap_command(conn, code, ident, buffer); 380 } 381 382 383 status_t 384 send_l2cap_connection_req(HciConnection* conn, uint8 ident, uint16 psm, uint16 scid) 385 { 386 uint8 code = 0; 387 net_buffer* buffer = make_l2cap_connection_req(code, psm, scid); 388 if (buffer == NULL) 389 return ENOMEM; 390 391 return send_l2cap_command(conn, code, ident, buffer); 392 } 393 394 395 status_t 396 send_l2cap_connection_rsp(HciConnection* conn, uint8 ident, uint16 dcid, uint16 scid, 397 uint16 result, uint16 status) 398 { 399 uint8 code = 0; 400 net_buffer* buffer = make_l2cap_connection_rsp(code, dcid, scid, result, status); 401 if (buffer == NULL) 402 return ENOMEM; 403 404 return send_l2cap_command(conn, code, ident, buffer); 405 } 406 407 408 status_t 409 send_l2cap_configuration_rsp(HciConnection* conn, uint8 ident, uint16 scid, uint16 flags, 410 uint16 result, net_buffer* opt) 411 { 412 uint8 code = 0; 413 net_buffer* buffer = make_l2cap_configuration_rsp(code, scid, flags, result, opt); 414 if (buffer == NULL) 415 return ENOMEM; 416 417 return send_l2cap_command(conn, code, ident, buffer); 418 } 419 420 421 status_t 422 send_l2cap_disconnection_req(HciConnection* conn, uint8 ident, uint16 dcid, uint16 scid) 423 { 424 uint8 code = 0; 425 net_buffer* buffer = make_l2cap_disconnection_req(code, dcid, scid); 426 if (buffer == NULL) 427 return ENOMEM; 428 429 return send_l2cap_command(conn, code, ident, buffer); 430 } 431 432 433 status_t 434 send_l2cap_disconnection_rsp(HciConnection* conn, uint8 ident, uint16 dcid, uint16 scid) 435 { 436 uint8 code = 0; 437 net_buffer* buffer = make_l2cap_disconnection_rsp(code, dcid, scid); 438 if (buffer == NULL) 439 return ENOMEM; 440 441 return send_l2cap_command(conn, code, ident, buffer); 442 } 443 444 445 // #pragma mark - dispatcher 446 447 448 status_t 449 l2cap_handle_signaling_command(HciConnection* connection, net_buffer* buffer) 450 { 451 TRACE("%s: signal size=%" B_PRIu32 "\n", __func__, buffer->size); 452 453 // TODO: If there are multiple commands in a packet, we could accumulate the 454 // responses into a single packet also... 455 456 while (buffer->size != 0) { 457 NetBufferHeaderReader<l2cap_command_header> commandHeader(buffer); 458 if (commandHeader.Status() != B_OK) 459 return ENOBUFS; 460 461 const uint8 code = commandHeader->code; 462 const uint8 ident = commandHeader->ident; 463 const uint16 length = le16toh(commandHeader->length); 464 465 L2capEndpoint* endpoint = NULL; 466 bool releaseIdent = false; 467 if (L2CAP_IS_SIGNAL_RSP(code)) { 468 endpoint = static_cast<L2capEndpoint*>( 469 btCoreData->lookup_command_ident(connection, ident)); 470 releaseIdent = true; 471 } 472 473 if (buffer->size < length) { 474 ERROR("%s: invalid L2CAP signaling command packet, code=%#x, " 475 "ident=%d, length=%d, buffer size=%" B_PRIu32 "\n", __func__, 476 code, ident, length, buffer->size); 477 return EMSGSIZE; 478 } 479 480 commandHeader.Remove(); 481 482 switch (code) { 483 case L2CAP_COMMAND_REJECT_RSP: 484 l2cap_handle_command_reject(endpoint, ident, buffer); 485 break; 486 487 case L2CAP_CONNECTION_REQ: 488 l2cap_handle_connection_req(connection, ident, buffer); 489 break; 490 491 case L2CAP_CONNECTION_RSP: 492 l2cap_handle_connection_rsp(endpoint, ident, buffer); 493 break; 494 495 case L2CAP_CONFIGURATION_REQ: 496 l2cap_handle_configuration_req(connection, ident, 497 buffer, length); 498 break; 499 500 case L2CAP_CONFIGURATION_RSP: 501 l2cap_handle_configuration_rsp(connection, endpoint, ident, 502 buffer, length, releaseIdent); 503 break; 504 505 case L2CAP_DISCONNECTION_REQ: 506 l2cap_handle_disconnection_req(connection, ident, buffer); 507 break; 508 509 case L2CAP_DISCONNECTION_RSP: 510 l2cap_handle_disconnection_rsp(endpoint, ident, buffer); 511 break; 512 513 case L2CAP_ECHO_REQ: 514 l2cap_handle_echo_req(connection, ident, buffer, length); 515 break; 516 517 case L2CAP_ECHO_RSP: 518 l2cap_handle_echo_rsp(endpoint, ident, buffer, length); 519 break; 520 521 case L2CAP_INFORMATION_REQ: 522 l2cap_handle_info_req(connection, ident, buffer); 523 break; 524 525 case L2CAP_INFORMATION_RSP: 526 l2cap_handle_info_rsp(endpoint, ident, buffer); 527 break; 528 529 default: 530 ERROR("l2cap: unknown L2CAP signaling command, " 531 "code=%#x\n", code); 532 send_l2cap_command_reject(connection, ident, 533 l2cap_command_reject::REJECTED_NOT_UNDERSTOOD, 0, 0, 0); 534 break; 535 } 536 537 // Only release the ident if no more signals with it are expected. 538 if (releaseIdent) 539 btCoreData->free_command_ident(connection, ident); 540 541 // Advance to the next command (if any.) 542 gBufferModule->remove_header(buffer, length); 543 } 544 545 gBufferModule->free(buffer); 546 return B_OK; 547 } 548