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