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__ "sdp_server.c" 39 40 /* 41 * Implementation of the Service Discovery Protocol Server 42 */ 43 44 #include <stdio.h> 45 #include <string.h> 46 47 #include "bluetooth.h" 48 #include "bluetooth_sdp.h" 49 #include "btstack_debug.h" 50 #include "btstack_event.h" 51 #include "btstack_memory.h" 52 #include "classic/core.h" 53 #include "classic/sdp_server.h" 54 #include "classic/sdp_util.h" 55 #include "hci_dump.h" 56 #include "hci.h" 57 #include "l2cap.h" 58 59 // max reserved ServiceRecordHandle 60 #define maxReservedServiceRecordHandle 0xffff 61 62 // max SDP response matches L2CAP PDU -- allow to use smaller buffer 63 #ifndef SDP_RESPONSE_BUFFER_SIZE 64 #define SDP_RESPONSE_BUFFER_SIZE (HCI_ACL_PAYLOAD_SIZE-L2CAP_HEADER_SIZE) 65 #endif 66 67 static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 68 69 // registered service records 70 static btstack_linked_list_t sdp_service_records = NULL; 71 72 // our handles start after the reserved range 73 static uint32_t sdp_next_service_record_handle = ((uint32_t) maxReservedServiceRecordHandle) + 2; 74 75 static uint8_t sdp_response_buffer[SDP_RESPONSE_BUFFER_SIZE]; 76 77 static uint16_t l2cap_cid = 0; 78 static uint16_t sdp_response_size = 0; 79 80 void sdp_init(void){ 81 // register with l2cap psm sevices - max MTU 82 l2cap_register_service(sdp_packet_handler, BLUETOOTH_PROTOCOL_SDP, 0xffff, LEVEL_0); 83 } 84 85 uint32_t sdp_get_service_record_handle(const uint8_t * record){ 86 // TODO: make sdp_get_attribute_value_for_attribute_id accept const data to remove cast 87 uint8_t * serviceRecordHandleAttribute = sdp_get_attribute_value_for_attribute_id((uint8_t *)record, BLUETOOTH_ATTRIBUTE_SERVICE_RECORD_HANDLE); 88 if (!serviceRecordHandleAttribute) return 0; 89 if (de_get_element_type(serviceRecordHandleAttribute) != DE_UINT) return 0; 90 if (de_get_size_type(serviceRecordHandleAttribute) != DE_SIZE_32) return 0; 91 return big_endian_read_32(serviceRecordHandleAttribute, 1); 92 } 93 94 static service_record_item_t * sdp_get_record_item_for_handle(uint32_t handle){ 95 btstack_linked_item_t *it; 96 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next){ 97 service_record_item_t * item = (service_record_item_t *) it; 98 if (item->service_record_handle == handle){ 99 return item; 100 } 101 } 102 return NULL; 103 } 104 105 uint8_t * sdp_get_record_for_handle(uint32_t handle){ 106 service_record_item_t * record_item = sdp_get_record_item_for_handle(handle); 107 if (!record_item) return 0; 108 return record_item->service_record; 109 } 110 111 // get next free, unregistered service record handle 112 uint32_t sdp_create_service_record_handle(void){ 113 uint32_t handle = 0; 114 do { 115 handle = sdp_next_service_record_handle++; 116 if (sdp_get_record_item_for_handle(handle)) handle = 0; 117 } while (handle == 0); 118 return handle; 119 } 120 121 /** 122 * @brief Register Service Record with database using ServiceRecordHandle stored in record 123 * @pre AttributeIDs are in ascending order 124 * @pre ServiceRecordHandle is first attribute and valid 125 * @param record is not copied! 126 * @result status 127 */ 128 uint8_t sdp_register_service(const uint8_t * record){ 129 130 // validate service record handle. it must: exist, be in valid range, not have been already used 131 uint32_t record_handle = sdp_get_service_record_handle(record); 132 if (!record_handle) return SDP_HANDLE_INVALID; 133 if (record_handle <= maxReservedServiceRecordHandle) return SDP_HANDLE_INVALID; 134 if (sdp_get_record_item_for_handle(record_handle)) return SDP_HANDLE_ALREADY_REGISTERED; 135 136 // alloc memory for new service_record_item 137 service_record_item_t * newRecordItem = btstack_memory_service_record_item_get(); 138 if (!newRecordItem) return BTSTACK_MEMORY_ALLOC_FAILED; 139 140 // set handle and record 141 newRecordItem->service_record_handle = record_handle; 142 newRecordItem->service_record = (uint8_t*) record; 143 144 // add to linked list 145 btstack_linked_list_add(&sdp_service_records, (btstack_linked_item_t *) newRecordItem); 146 147 return 0; 148 } 149 150 // 151 // unregister service record 152 // 153 void sdp_unregister_service(uint32_t service_record_handle){ 154 service_record_item_t * record_item = sdp_get_record_item_for_handle(service_record_handle); 155 if (!record_item) return; 156 btstack_linked_list_remove(&sdp_service_records, (btstack_linked_item_t *) record_item); 157 btstack_memory_service_record_item_free(record_item); 158 } 159 160 // PDU 161 // PDU ID (1), Transaction ID (2), Param Length (2), Param 1, Param 2, .. 162 163 static int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code){ 164 sdp_response_buffer[0] = SDP_ErrorResponse; 165 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 166 big_endian_store_16(sdp_response_buffer, 3, 2); 167 big_endian_store_16(sdp_response_buffer, 5, error_code); // invalid syntax 168 return 7; 169 } 170 171 int sdp_handle_service_search_request(uint8_t * packet, uint16_t remote_mtu){ 172 173 // get request details 174 uint16_t transaction_id = big_endian_read_16(packet, 1); 175 uint16_t param_len = big_endian_read_16(packet, 3); 176 uint8_t * serviceSearchPattern = &packet[5]; 177 uint16_t serviceSearchPatternLen = de_get_len_safe(serviceSearchPattern, param_len); 178 // assert service search pattern is contained 179 if (!serviceSearchPatternLen) return 0; 180 param_len -= serviceSearchPatternLen; 181 // assert max record count is contained 182 if (param_len < 2) return 0; 183 uint16_t maximumServiceRecordCount = big_endian_read_16(packet, 5 + serviceSearchPatternLen); 184 param_len -= 2; 185 // assert continuation state len is contained in param_len 186 if (param_len < 1) return 0; 187 uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2]; 188 // assert continuation state is contained in param_len 189 if (1 + continuationState[0] > param_len) return 0; 190 191 // calc maximumServiceRecordCount based on remote MTU 192 uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4; 193 194 // continuation state contains index of next service record to examine 195 int continuation = 0; 196 uint16_t continuation_index = 0; 197 if (continuationState[0] == 2){ 198 continuation_index = big_endian_read_16(continuationState, 1); 199 } 200 201 // get and limit total count 202 btstack_linked_item_t *it; 203 uint16_t total_service_count = 0; 204 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next){ 205 service_record_item_t * item = (service_record_item_t *) it; 206 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 207 total_service_count++; 208 } 209 if (total_service_count > maximumServiceRecordCount){ 210 total_service_count = maximumServiceRecordCount; 211 } 212 213 // ServiceRecordHandleList at 9 214 uint16_t pos = 9; 215 uint16_t current_service_count = 0; 216 uint16_t current_service_index = 0; 217 uint16_t matching_service_count = 0; 218 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next, ++current_service_index){ 219 service_record_item_t * item = (service_record_item_t *) it; 220 221 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 222 matching_service_count++; 223 224 if (current_service_index < continuation_index) continue; 225 226 big_endian_store_32(sdp_response_buffer, pos, item->service_record_handle); 227 pos += 4; 228 current_service_count++; 229 230 if (matching_service_count >= total_service_count) break; 231 232 if (current_service_count >= maxNrServiceRecordsPerResponse){ 233 continuation = 1; 234 continuation_index = current_service_index + 1; 235 break; 236 } 237 } 238 239 // Store continuation state 240 if (continuation) { 241 sdp_response_buffer[pos++] = 2; 242 big_endian_store_16(sdp_response_buffer, pos, continuation_index); 243 pos += 2; 244 } else { 245 sdp_response_buffer[pos++] = 0; 246 } 247 248 // header 249 sdp_response_buffer[0] = SDP_ServiceSearchResponse; 250 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 251 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload 252 big_endian_store_16(sdp_response_buffer, 5, total_service_count); 253 big_endian_store_16(sdp_response_buffer, 7, current_service_count); 254 255 return pos; 256 } 257 258 int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu){ 259 260 // get request details 261 uint16_t transaction_id = big_endian_read_16(packet, 1); 262 uint16_t param_len = big_endian_read_16(packet, 3); 263 // assert serviceRecordHandle and maximumAttributeByteCount are in param_len 264 if (param_len < 6) return 0; 265 uint32_t serviceRecordHandle = big_endian_read_32(packet, 5); 266 uint16_t maximumAttributeByteCount = big_endian_read_16(packet, 9); 267 param_len -= 6; 268 uint8_t * attributeIDList = &packet[11]; 269 uint16_t attributeIDListLen = de_get_len_safe(attributeIDList, param_len); 270 // assert attributeIDList are in param_len 271 if (!attributeIDListLen) return 0; 272 param_len -= attributeIDListLen; 273 // assert continuation state len is contained in param_len 274 if (param_len < 1) return 0; 275 uint8_t * continuationState = &packet[11+attributeIDListLen]; 276 // assert continuation state is contained in param_len 277 if (1 + continuationState[0] > param_len) return 0; 278 279 // calc maximumAttributeByteCount based on remote MTU 280 uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3); 281 if (maximumAttributeByteCount2 < maximumAttributeByteCount) { 282 maximumAttributeByteCount = maximumAttributeByteCount2; 283 } 284 285 // continuation state contains the offset into the complete response 286 uint16_t continuation_offset = 0; 287 if (continuationState[0] == 2){ 288 continuation_offset = big_endian_read_16(continuationState, 1); 289 } 290 291 // get service record 292 service_record_item_t * item = sdp_get_record_item_for_handle(serviceRecordHandle); 293 if (!item){ 294 // service record handle doesn't exist 295 return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle 296 } 297 298 299 // AttributeList - starts at offset 7 300 uint16_t pos = 7; 301 302 if (continuation_offset == 0){ 303 304 // get size of this record 305 uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList); 306 307 // store DES 308 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size); 309 maximumAttributeByteCount -= 3; 310 pos += 3; 311 } 312 313 // copy maximumAttributeByteCount from record 314 uint16_t bytes_used; 315 int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]); 316 pos += bytes_used; 317 318 uint16_t attributeListByteCount = pos - 7; 319 320 if (complete) { 321 sdp_response_buffer[pos++] = 0; 322 } else { 323 continuation_offset += bytes_used; 324 sdp_response_buffer[pos++] = 2; 325 big_endian_store_16(sdp_response_buffer, pos, continuation_offset); 326 pos += 2; 327 } 328 329 // header 330 sdp_response_buffer[0] = SDP_ServiceAttributeResponse; 331 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 332 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload 333 big_endian_store_16(sdp_response_buffer, 5, attributeListByteCount); 334 335 return pos; 336 } 337 338 static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList){ 339 uint16_t total_response_size = 0; 340 btstack_linked_item_t *it; 341 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next){ 342 service_record_item_t * item = (service_record_item_t *) it; 343 344 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 345 346 // for all service records that match 347 total_response_size += 3 + spd_get_filtered_size(item->service_record, attributeIDList); 348 } 349 return total_response_size; 350 } 351 352 int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu){ 353 354 // SDP header before attribute sevice list: 7 355 // Continuation, worst case: 5 356 357 // get request details 358 uint16_t transaction_id = big_endian_read_16(packet, 1); 359 uint16_t param_len = big_endian_read_16(packet, 3); 360 uint8_t * serviceSearchPattern = &packet[5]; 361 uint16_t serviceSearchPatternLen = de_get_len_safe(serviceSearchPattern, param_len); 362 // assert serviceSearchPattern header is contained in param_len 363 if (!serviceSearchPatternLen) return 0; 364 param_len -= serviceSearchPatternLen; 365 // assert maximumAttributeByteCount contained in param_len 366 if (param_len < 2) return 0; 367 uint16_t maximumAttributeByteCount = big_endian_read_16(packet, 5 + serviceSearchPatternLen); 368 param_len -= 2; 369 uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2]; 370 uint16_t attributeIDListLen = de_get_len_safe(attributeIDList, param_len); 371 // assert attributeIDList is contained in param_len 372 if (!attributeIDListLen) return 0; 373 // assert continuation state len is contained in param_len 374 if (param_len < 1) return 0; 375 uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen]; 376 // assert continuation state is contained in param_len 377 if (1 + continuationState[0] > param_len) return 0; 378 379 // calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block 380 uint16_t maximumAttributeByteCount2 = remote_mtu - 12; 381 if (maximumAttributeByteCount2 < maximumAttributeByteCount) { 382 maximumAttributeByteCount = maximumAttributeByteCount2; 383 } 384 385 // continuation state contains: index of next service record to examine 386 // continuation state contains: byte offset into this service record 387 uint16_t continuation_service_index = 0; 388 uint16_t continuation_offset = 0; 389 if (continuationState[0] == 4){ 390 continuation_service_index = big_endian_read_16(continuationState, 1); 391 continuation_offset = big_endian_read_16(continuationState, 3); 392 } 393 394 // log_info("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u", continuation_service_index, continuation_offset, maximumAttributeByteCount); 395 396 // AttributeLists - starts at offset 7 397 uint16_t pos = 7; 398 399 // add DES with total size for first request 400 if (continuation_service_index == 0 && continuation_offset == 0){ 401 uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList); 402 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size); 403 // log_info("total response size %u", total_response_size); 404 pos += 3; 405 maximumAttributeByteCount -= 3; 406 } 407 408 // create attribute list 409 int first_answer = 1; 410 int continuation = 0; 411 uint16_t current_service_index = 0; 412 btstack_linked_item_t *it = (btstack_linked_item_t *) sdp_service_records; 413 for ( ; it ; it = it->next, ++current_service_index){ 414 service_record_item_t * item = (service_record_item_t *) it; 415 416 if (current_service_index < continuation_service_index ) continue; 417 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 418 419 if (continuation_offset == 0){ 420 421 // get size of this record 422 uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList); 423 424 // stop if complete record doesn't fits into response but we already have a partial response 425 if ((filtered_attributes_size + 3 > maximumAttributeByteCount) && !first_answer) { 426 continuation = 1; 427 break; 428 } 429 430 // store DES 431 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size); 432 pos += 3; 433 maximumAttributeByteCount -= 3; 434 } 435 436 first_answer = 0; 437 438 // copy maximumAttributeByteCount from record 439 uint16_t bytes_used; 440 int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]); 441 pos += bytes_used; 442 maximumAttributeByteCount -= bytes_used; 443 444 if (complete) { 445 continuation_offset = 0; 446 continue; 447 } 448 449 continuation = 1; 450 continuation_offset += bytes_used; 451 break; 452 } 453 454 uint16_t attributeListsByteCount = pos - 7; 455 456 // Continuation State 457 if (continuation){ 458 sdp_response_buffer[pos++] = 4; 459 big_endian_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index); 460 pos += 2; 461 big_endian_store_16(sdp_response_buffer, pos, continuation_offset); 462 pos += 2; 463 } else { 464 // complete 465 sdp_response_buffer[pos++] = 0; 466 } 467 468 // create SDP header 469 sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse; 470 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 471 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload 472 big_endian_store_16(sdp_response_buffer, 5, attributeListsByteCount); 473 474 return pos; 475 } 476 477 static void sdp_respond(void){ 478 if (!sdp_response_size ) return; 479 if (!l2cap_cid) return; 480 481 // update state before sending packet (avoid getting called when new l2cap credit gets emitted) 482 uint16_t size = sdp_response_size; 483 sdp_response_size = 0; 484 l2cap_send(l2cap_cid, sdp_response_buffer, size); 485 } 486 487 // we assume that we don't get two requests in a row 488 static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 489 uint16_t transaction_id; 490 SDP_PDU_ID_t pdu_id; 491 uint16_t remote_mtu; 492 uint16_t param_len; 493 494 switch (packet_type) { 495 496 case L2CAP_DATA_PACKET: 497 pdu_id = (SDP_PDU_ID_t) packet[0]; 498 transaction_id = big_endian_read_16(packet, 1); 499 param_len = big_endian_read_16(packet, 3); 500 remote_mtu = l2cap_get_remote_mtu_for_local_cid(channel); 501 // account for our buffer 502 if (remote_mtu > SDP_RESPONSE_BUFFER_SIZE){ 503 remote_mtu = SDP_RESPONSE_BUFFER_SIZE; 504 } 505 // validate parm_len against packet size 506 if (param_len + 5 > size) { 507 // just clear pdu_id 508 pdu_id = SDP_ErrorResponse; 509 } 510 511 // log_info("SDP Request: type %u, transaction id %u, len %u, mtu %u", pdu_id, transaction_id, param_len, remote_mtu); 512 switch (pdu_id){ 513 514 case SDP_ServiceSearchRequest: 515 sdp_response_size = sdp_handle_service_search_request(packet, remote_mtu); 516 break; 517 518 case SDP_ServiceAttributeRequest: 519 sdp_response_size = sdp_handle_service_attribute_request(packet, remote_mtu); 520 break; 521 522 case SDP_ServiceSearchAttributeRequest: 523 sdp_response_size = sdp_handle_service_search_attribute_request(packet, remote_mtu); 524 break; 525 526 default: 527 sdp_response_size = sdp_create_error_response(transaction_id, 0x0003); // invalid syntax 528 break; 529 } 530 if (!sdp_response_size) break; 531 l2cap_request_can_send_now_event(l2cap_cid); 532 break; 533 534 case HCI_EVENT_PACKET: 535 536 switch (hci_event_packet_get_type(packet)) { 537 538 case L2CAP_EVENT_INCOMING_CONNECTION: 539 if (l2cap_cid) { 540 // CONNECTION REJECTED DUE TO LIMITED RESOURCES 541 l2cap_decline_connection(channel); 542 break; 543 } 544 // accept 545 l2cap_cid = channel; 546 sdp_response_size = 0; 547 l2cap_accept_connection(channel); 548 break; 549 550 case L2CAP_EVENT_CHANNEL_OPENED: 551 if (packet[2]) { 552 // open failed -> reset 553 l2cap_cid = 0; 554 } 555 break; 556 557 case L2CAP_EVENT_CAN_SEND_NOW: 558 sdp_respond(); 559 break; 560 561 case L2CAP_EVENT_CHANNEL_CLOSED: 562 if (channel == l2cap_cid){ 563 // reset 564 l2cap_cid = 0; 565 } 566 break; 567 568 default: 569 // other event 570 break; 571 } 572 break; 573 574 default: 575 // other packet type 576 break; 577 } 578 } 579 580