1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define BTSTACK_FILE__ "hid_host.c" 39 40 #include <string.h> 41 42 #include "bluetooth.h" 43 #include "bluetooth_psm.h" 44 #include "bluetooth_sdp.h" 45 #include "btstack_debug.h" 46 #include "btstack_event.h" 47 #include "btstack_hid_parser.h" 48 #include "btstack_memory.h" 49 #include "classic/hid.h" 50 #include "classic/hid_host.h" 51 #include "classic/sdp_util.h" 52 #include "classic/sdp_client.h" 53 #include "l2cap.h" 54 55 #define MAX_ATTRIBUTE_VALUE_SIZE 300 56 57 static btstack_packet_handler_t hid_callback; 58 59 static uint8_t * hid_host_descriptor_storage; 60 static uint16_t hid_host_descriptor_storage_len; 61 62 static btstack_linked_list_t connections; 63 static uint16_t hid_host_cid_counter = 0; 64 65 // SDP 66 static uint8_t attribute_value[MAX_ATTRIBUTE_VALUE_SIZE]; 67 static const unsigned int attribute_value_buffer_size = MAX_ATTRIBUTE_VALUE_SIZE; 68 69 static uint16_t sdp_query_context_hid_host_control_cid = 0; 70 71 static btstack_context_callback_registration_t hid_host_handle_sdp_client_query_request; 72 static void hid_host_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 73 74 static uint16_t hid_descriptor_storage_get_available_space(void){ 75 // assumes all descriptors are back to back 76 uint16_t free_space = hid_host_descriptor_storage_len; 77 78 btstack_linked_list_iterator_t it; 79 btstack_linked_list_iterator_init(&it, &connections); 80 while (btstack_linked_list_iterator_has_next(&it)){ 81 hid_host_connection_t * connection = (hid_host_connection_t *)btstack_linked_list_iterator_next(&it); 82 free_space -= connection->hid_descriptor_len; 83 } 84 return free_space; 85 } 86 87 static void hid_descriptor_storage_init(hid_host_connection_t * connection){ 88 connection->hid_descriptor_len = 0; 89 connection->hid_descriptor_max_len = hid_descriptor_storage_get_available_space(); 90 connection->hid_descriptor_offset = hid_host_descriptor_storage_len - connection->hid_descriptor_max_len; 91 } 92 93 static bool hid_descriptor_storage_store(hid_host_connection_t * connection, uint8_t byte){ 94 if (connection->hid_descriptor_len >= connection->hid_descriptor_max_len) return false; 95 96 hid_host_descriptor_storage[connection->hid_descriptor_offset + connection->hid_descriptor_len] = byte; 97 connection->hid_descriptor_len++; 98 return true; 99 } 100 101 102 static void hid_descriptor_storage_delete(hid_host_connection_t * connection){ 103 uint16_t next_offset = connection->hid_descriptor_offset + connection->hid_descriptor_len; 104 105 memmove(&hid_host_descriptor_storage[connection->hid_descriptor_offset], 106 &hid_host_descriptor_storage[next_offset], 107 hid_host_descriptor_storage_len - next_offset); 108 109 connection->hid_descriptor_len = 0; 110 connection->hid_descriptor_offset = 0; 111 112 btstack_linked_list_iterator_t it; 113 btstack_linked_list_iterator_init(&it, &connections); 114 while (btstack_linked_list_iterator_has_next(&it)){ 115 hid_host_connection_t * conn = (hid_host_connection_t *)btstack_linked_list_iterator_next(&it); 116 if (conn->hid_descriptor_offset >= next_offset){ 117 conn->hid_descriptor_offset = next_offset; 118 next_offset += conn->hid_descriptor_len; 119 } 120 } 121 } 122 123 // HID Util 124 static inline void hid_emit_connected_event(hid_host_connection_t * context, uint8_t status){ 125 uint8_t event[15]; 126 int pos = 0; 127 event[pos++] = HCI_EVENT_HID_META; 128 pos++; // skip len 129 event[pos++] = HID_SUBEVENT_CONNECTION_OPENED; 130 little_endian_store_16(event, pos, context->hid_cid); 131 pos+=2; 132 event[pos++] = status; 133 reverse_bd_addr(context->remote_addr, &event[pos]); 134 pos += 6; 135 little_endian_store_16(event,pos,context->con_handle); 136 pos += 2; 137 event[pos++] = context->incoming; 138 event[1] = pos - 2; 139 hid_callback(HCI_EVENT_PACKET, context->hid_cid, &event[0], pos); 140 } 141 142 // HID Host 143 144 static uint16_t hid_host_get_next_cid(void){ 145 if (hid_host_cid_counter == 0xffff) { 146 hid_host_cid_counter = 1; 147 } else { 148 hid_host_cid_counter++; 149 } 150 return hid_host_cid_counter; 151 } 152 153 static hid_host_connection_t * hid_host_create_connection(bd_addr_t remote_addr){ 154 hid_host_connection_t * connection = btstack_memory_hid_host_connection_get(); 155 if (!connection){ 156 log_error("Not enough memory to create connection"); 157 return NULL; 158 } 159 connection->state = HID_HOST_IDLE; 160 connection->hid_cid = hid_host_get_next_cid(); 161 (void)memcpy(connection->remote_addr, remote_addr, 6); 162 printf("hid_host_create_connection, cid 0x%02x, %s \n", connection->hid_cid, bd_addr_to_str(connection->remote_addr)); 163 164 btstack_linked_list_add(&connections, (btstack_linked_item_t *) connection); 165 return connection; 166 } 167 168 static hid_host_connection_t * hid_host_get_connection_for_bd_addr(bd_addr_t addr){ 169 btstack_linked_list_iterator_t it; 170 btstack_linked_list_iterator_init(&it, &connections); 171 while (btstack_linked_list_iterator_has_next(&it)){ 172 hid_host_connection_t * connection = (hid_host_connection_t *)btstack_linked_list_iterator_next(&it); 173 if (memcmp(addr, connection->remote_addr, 6) != 0) continue; 174 return connection; 175 } 176 return NULL; 177 } 178 179 static hid_host_connection_t * hid_host_connection_for_hid_cid(uint16_t hid_cid){ 180 btstack_linked_list_iterator_t it; 181 btstack_linked_list_iterator_init(&it, &connections); 182 while (btstack_linked_list_iterator_has_next(&it)){ 183 hid_host_connection_t * connection = (hid_host_connection_t *)btstack_linked_list_iterator_next(&it); 184 if (connection->hid_cid != hid_cid) continue; 185 return connection; 186 } 187 return NULL; 188 } 189 190 static void hid_host_finalize_connection(hid_host_connection_t * connection){ 191 btstack_linked_list_remove(&connections, (btstack_linked_item_t*) connection); 192 btstack_memory_hid_host_connection_free(connection); 193 } 194 195 static void hid_host_handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { 196 UNUSED(packet_type); 197 UNUSED(channel); 198 UNUSED(size); 199 200 des_iterator_t attribute_list_it; 201 des_iterator_t additional_des_it; 202 des_iterator_t prot_it; 203 uint8_t *des_element; 204 uint8_t *element; 205 uint32_t uuid; 206 uint8_t status = ERROR_CODE_SUCCESS; 207 208 hid_host_connection_t * connection = hid_host_connection_for_hid_cid(sdp_query_context_hid_host_control_cid); 209 if (!connection) { 210 log_error("SDP query, connection with 0x%02x cid not found", sdp_query_context_hid_host_control_cid); 211 return; 212 } 213 214 switch (hci_event_packet_get_type(packet)){ 215 case SDP_EVENT_QUERY_ATTRIBUTE_VALUE: 216 217 if (sdp_event_query_attribute_byte_get_attribute_length(packet) <= attribute_value_buffer_size) { 218 219 attribute_value[sdp_event_query_attribute_byte_get_data_offset(packet)] = sdp_event_query_attribute_byte_get_data(packet); 220 221 if ((uint16_t)(sdp_event_query_attribute_byte_get_data_offset(packet)+1) == sdp_event_query_attribute_byte_get_attribute_length(packet)) { 222 switch(sdp_event_query_attribute_byte_get_attribute_id(packet)) { 223 224 case BLUETOOTH_ATTRIBUTE_PROTOCOL_DESCRIPTOR_LIST: 225 for (des_iterator_init(&attribute_list_it, attribute_value); des_iterator_has_more(&attribute_list_it); des_iterator_next(&attribute_list_it)) { 226 if (des_iterator_get_type(&attribute_list_it) != DE_DES) continue; 227 des_element = des_iterator_get_element(&attribute_list_it); 228 des_iterator_init(&prot_it, des_element); 229 element = des_iterator_get_element(&prot_it); 230 if (de_get_element_type(element) != DE_UUID) continue; 231 uuid = de_get_uuid32(element); 232 switch (uuid){ 233 case BLUETOOTH_PROTOCOL_L2CAP: 234 if (!des_iterator_has_more(&prot_it)) continue; 235 des_iterator_next(&prot_it); 236 de_element_get_uint16(des_iterator_get_element(&prot_it), &connection->control_psm); 237 log_info("HID Control PSM: 0x%04x", (int) &connection->control_psm); 238 break; 239 default: 240 break; 241 } 242 } 243 break; 244 case BLUETOOTH_ATTRIBUTE_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS: 245 for (des_iterator_init(&attribute_list_it, attribute_value); des_iterator_has_more(&attribute_list_it); des_iterator_next(&attribute_list_it)) { 246 if (des_iterator_get_type(&attribute_list_it) != DE_DES) continue; 247 des_element = des_iterator_get_element(&attribute_list_it); 248 for (des_iterator_init(&additional_des_it, des_element); des_iterator_has_more(&additional_des_it); des_iterator_next(&additional_des_it)) { 249 if (des_iterator_get_type(&additional_des_it) != DE_DES) continue; 250 des_element = des_iterator_get_element(&additional_des_it); 251 des_iterator_init(&prot_it, des_element); 252 element = des_iterator_get_element(&prot_it); 253 if (de_get_element_type(element) != DE_UUID) continue; 254 uuid = de_get_uuid32(element); 255 switch (uuid){ 256 case BLUETOOTH_PROTOCOL_L2CAP: 257 if (!des_iterator_has_more(&prot_it)) continue; 258 des_iterator_next(&prot_it); 259 de_element_get_uint16(des_iterator_get_element(&prot_it), &connection->interrupt_psm); 260 log_info("HID Interrupt PSM: 0x%04x", (int) &connection->interrupt_psm); 261 break; 262 default: 263 break; 264 } 265 } 266 } 267 break; 268 269 case BLUETOOTH_ATTRIBUTE_HID_DESCRIPTOR_LIST: 270 for (des_iterator_init(&attribute_list_it, attribute_value); des_iterator_has_more(&attribute_list_it); des_iterator_next(&attribute_list_it)) { 271 if (des_iterator_get_type(&attribute_list_it) != DE_DES) continue; 272 des_element = des_iterator_get_element(&attribute_list_it); 273 for (des_iterator_init(&additional_des_it, des_element); des_iterator_has_more(&additional_des_it); des_iterator_next(&additional_des_it)) { 274 if (des_iterator_get_type(&additional_des_it) != DE_STRING) continue; 275 element = des_iterator_get_element(&additional_des_it); 276 277 const uint8_t * descriptor = de_get_string(element); 278 uint16_t descriptor_len = de_get_data_size(element); 279 280 uint16_t i; 281 for (i = 0; i < descriptor_len; i++){ 282 hid_descriptor_storage_store(connection, descriptor[i]); 283 } 284 printf("HID Descriptor:\n"); 285 printf_hexdump(descriptor, descriptor_len); 286 } 287 } 288 break; 289 default: 290 break; 291 } 292 } 293 } else { 294 fprintf(stderr, "SDP attribute value buffer size exceeded: available %d, required %d\n", attribute_value_buffer_size, sdp_event_query_attribute_byte_get_attribute_length(packet)); 295 } 296 break; 297 298 case SDP_EVENT_QUERY_COMPLETE: 299 if (!connection->control_psm) { 300 printf("HID Control PSM missing\n"); 301 status = ERROR_CODE_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE; 302 break; 303 } 304 if (!connection->interrupt_psm) { 305 printf("HID Interrupt PSM missing\n"); 306 status = ERROR_CODE_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE; 307 break; 308 } 309 310 printf("Setup HID\n"); 311 status = l2cap_create_channel(hid_host_packet_handler, connection->remote_addr, connection->control_psm, 48, &connection->control_cid); 312 if (status){ 313 printf("Connecting to HID Control failed: 0x%02x\n", status); 314 } 315 break; 316 317 default: 318 // bail out, we must have had an incoming connection in the meantime; just trigger next sdp query on complete 319 if (hci_event_packet_get_type(packet) == SDP_EVENT_QUERY_COMPLETE){ 320 (void) sdp_client_register_query_callback(&hid_host_handle_sdp_client_query_request); 321 } 322 return; 323 } 324 325 if (status != ERROR_CODE_SUCCESS){ 326 hid_emit_connected_event(connection, status); 327 hid_host_finalize_connection(connection); 328 sdp_query_context_hid_host_control_cid = 0; 329 } 330 } 331 332 static void hid_host_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 333 UNUSED(channel); 334 UNUSED(size); 335 336 uint8_t event; 337 bd_addr_t address; 338 uint8_t status; 339 uint16_t cid; 340 hid_host_connection_t * connection; 341 342 // uint8_t param; 343 // hid_message_type_t message_type; 344 // hid_handshake_param_type_t message_status; 345 346 switch (packet_type) { 347 case HCI_EVENT_PACKET: 348 event = hci_event_packet_get_type(packet); 349 switch (event) { 350 case L2CAP_EVENT_INCOMING_CONNECTION: 351 l2cap_event_incoming_connection_get_address(packet, address); 352 connection = hid_host_get_connection_for_bd_addr(address); 353 354 if (connection && connection->unplugged){ 355 log_info("Decline connection for %s, host is unplugged", bd_addr_to_str(address)); 356 l2cap_decline_connection(channel); 357 break; 358 } 359 360 switch (l2cap_event_incoming_connection_get_psm(packet)){ 361 case PSM_HID_CONTROL: 362 if (connection){ 363 log_error("Connection already exists %s", bd_addr_to_str(address)); 364 l2cap_decline_connection(channel); 365 break; 366 } 367 368 connection = hid_host_create_connection(address); 369 if (!connection) { 370 log_error("Cannot create connection for %s", bd_addr_to_str(address)); 371 l2cap_decline_connection(channel); 372 break; 373 } 374 375 connection->state = HID_HOST_W4_CONTROL_CONNECTION_ESTABLISHED; 376 connection->con_handle = l2cap_event_incoming_connection_get_handle(packet); 377 connection->control_cid = l2cap_event_incoming_connection_get_local_cid(packet); 378 connection->incoming = true; 379 log_info("Accept connection on Control channel %s", bd_addr_to_str(address)); 380 l2cap_accept_connection(channel); 381 break; 382 383 case PSM_HID_INTERRUPT: 384 if (!connection || (connection->interrupt_cid != 0) || (l2cap_event_incoming_connection_get_handle(packet) != connection->con_handle)){ 385 log_error("Decline connection for %s", bd_addr_to_str(address)); 386 l2cap_decline_connection(channel); 387 break; 388 } 389 390 connection->state = HID_HOST_W4_INTERRUPT_CONNECTION_ESTABLISHED; 391 connection->interrupt_cid = l2cap_event_incoming_connection_get_local_cid(packet); 392 log_info("Accept connection on Interrupt channel %s", bd_addr_to_str(address)); 393 l2cap_accept_connection(channel); 394 break; 395 396 default: 397 log_info("Decline connection for %s", bd_addr_to_str(address)); 398 l2cap_decline_connection(channel); 399 break; 400 } 401 break; 402 403 case L2CAP_EVENT_CHANNEL_OPENED: 404 l2cap_event_channel_opened_get_address(packet, address); 405 406 status = l2cap_event_channel_opened_get_status(packet); 407 if (status){ 408 log_info("L2CAP connection %s failed: 0x%02xn", bd_addr_to_str(address), status); 409 break; 410 } 411 412 connection = hid_host_get_connection_for_bd_addr(address); 413 if (!connection){ 414 log_error("Connection does not exist %s", bd_addr_to_str(address)); 415 break; 416 } 417 418 cid = l2cap_event_channel_opened_get_local_cid(packet); 419 420 switch (l2cap_event_channel_opened_get_psm(packet)){ 421 case PSM_HID_CONTROL: 422 if (connection->state != HID_HOST_W4_CONTROL_CONNECTION_ESTABLISHED) break; 423 connection->state = HID_HOST_CONTROL_CONNECTION_ESTABLISHED; 424 425 if (connection->boot_mode){ 426 break; 427 } 428 429 if (!connection->incoming){ 430 status = l2cap_create_channel(hid_host_packet_handler, address, connection->interrupt_psm, 48, &connection->interrupt_cid); 431 if (status){ 432 log_info("Connecting to HID Interrupt failed: 0x%02x", status); 433 break; 434 } 435 connection->con_handle = l2cap_event_channel_opened_get_handle(packet); 436 connection->state = HID_HOST_W4_INTERRUPT_CONNECTION_ESTABLISHED; 437 } 438 break; 439 440 case PSM_HID_INTERRUPT: 441 if (connection->state != HID_HOST_W4_INTERRUPT_CONNECTION_ESTABLISHED) break; 442 if (connection->con_handle != l2cap_event_channel_opened_get_handle(packet)) break; 443 444 connection->state = HID_HOST_CONNECTION_ESTABLISHED; 445 hid_emit_connected_event(connection, ERROR_CODE_SUCCESS); 446 447 log_info("Connection on interrupt channel established, interrupt_cid 0x%02x", connection->interrupt_cid); 448 break; 449 450 default: 451 break; 452 } 453 // disconnect? 454 break; 455 456 default: 457 break; 458 } 459 default: 460 break; 461 } 462 } 463 464 465 void hid_host_init(uint8_t * hid_descriptor_storage, uint16_t hid_descriptor_storage_len){ 466 hid_host_descriptor_storage = hid_descriptor_storage; 467 hid_host_descriptor_storage_len = hid_descriptor_storage_len; 468 469 // register L2CAP Services for reconnections 470 l2cap_register_service(hid_host_packet_handler, PSM_HID_INTERRUPT, 0xffff, gap_get_security_level()); 471 l2cap_register_service(hid_host_packet_handler, PSM_HID_CONTROL, 0xffff, gap_get_security_level()); 472 } 473 474 void hid_host_register_packet_handler(btstack_packet_handler_t callback){ 475 hid_callback = callback; 476 } 477 478 479 static void hid_host_handle_start_sdp_client_query(void * context){ 480 UNUSED(context); 481 482 btstack_linked_list_iterator_t it; 483 btstack_linked_list_iterator_init(&it, &connections); 484 while (btstack_linked_list_iterator_has_next(&it)){ 485 hid_host_connection_t * connection = (hid_host_connection_t *)btstack_linked_list_iterator_next(&it); 486 487 switch (connection->state){ 488 case HID_HOST_W2_SEND_SDP_QUERY: 489 connection->state = HID_HOST_W4_SDP_QUERY_RESULT; 490 break; 491 default: 492 continue; 493 } 494 printf("hid_descriptor_storage_init, start query, cid 0x%02x, %s \n", connection->hid_cid, bd_addr_to_str(connection->remote_addr)); 495 hid_descriptor_storage_init(connection); 496 sdp_query_context_hid_host_control_cid = connection->hid_cid; 497 sdp_client_query_uuid16(&hid_host_handle_sdp_client_query_result, (uint8_t *) connection->remote_addr, BLUETOOTH_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE_SERVICE); 498 return; 499 } 500 } 501 502 uint8_t hid_host_connect(bd_addr_t remote_addr, hid_protocol_mode_t protocol_mode, uint16_t * hid_cid){ 503 UNUSED(protocol_mode); 504 505 if (hid_cid == NULL) { 506 return ERROR_CODE_COMMAND_DISALLOWED; 507 } 508 509 hid_host_connection_t * connection = hid_host_get_connection_for_bd_addr(remote_addr); 510 if (connection){ 511 return ERROR_CODE_COMMAND_DISALLOWED; 512 } 513 514 connection = hid_host_create_connection(remote_addr); 515 if (!connection) return BTSTACK_MEMORY_ALLOC_FAILED; 516 517 518 *hid_cid = connection->hid_cid; 519 520 connection->state = HID_HOST_W2_SEND_SDP_QUERY; 521 connection->incoming = false; 522 connection->control_cid = 0; 523 connection->control_psm = 0; 524 connection->interrupt_cid = 0; 525 connection->interrupt_psm = 0; 526 printf("hid_host_connect, cid 0x%02x, %s \n", connection->hid_cid, bd_addr_to_str(connection->remote_addr)); 527 528 hid_host_handle_sdp_client_query_request.callback = &hid_host_handle_start_sdp_client_query; 529 // ignore ERROR_CODE_COMMAND_DISALLOWED because in that case, we already have requested an SDP callback 530 (void) sdp_client_register_query_callback(&hid_host_handle_sdp_client_query_request); 531 532 return ERROR_CODE_SUCCESS; 533 } 534 535 536 void hid_host_disconnect(uint16_t hid_cid){ 537 UNUSED(hid_cid); 538 } 539 540 void hid_host_request_can_send_now_event(uint16_t hid_cid){ 541 UNUSED(hid_cid); 542 } 543 544 void hid_host_send_interrupt_message(uint16_t hid_cid, const uint8_t * message, uint16_t message_len){ 545 UNUSED(hid_cid); 546 UNUSED(message); 547 UNUSED(message_len); 548 } 549 550 void hid_host_send_control_message(uint16_t hid_cid, const uint8_t * message, uint16_t message_len){ 551 UNUSED(hid_cid); 552 UNUSED(message); 553 UNUSED(message_len); 554 }