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 btstack_memory_service_record_item_free(record_item); 156 } 157 158 // PDU 159 // PDU ID (1), Transaction ID (2), Param Length (2), Param 1, Param 2, .. 160 161 static int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code){ 162 sdp_response_buffer[0] = SDP_ErrorResponse; 163 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 164 big_endian_store_16(sdp_response_buffer, 3, 2); 165 big_endian_store_16(sdp_response_buffer, 5, error_code); // invalid syntax 166 return 7; 167 } 168 169 int sdp_handle_service_search_request(uint8_t * packet, uint16_t remote_mtu){ 170 171 // get request details 172 uint16_t transaction_id = big_endian_read_16(packet, 1); 173 uint16_t param_len = big_endian_read_16(packet, 3); 174 uint8_t * serviceSearchPattern = &packet[5]; 175 uint16_t serviceSearchPatternLen = de_get_len_safe(serviceSearchPattern, param_len); 176 // assert service search pattern is contained 177 if (!serviceSearchPatternLen) return 0; 178 param_len -= serviceSearchPatternLen; 179 // assert max record count is contained 180 if (param_len < 2) return 0; 181 uint16_t maximumServiceRecordCount = big_endian_read_16(packet, 5 + serviceSearchPatternLen); 182 param_len -= 2; 183 // assert continuation state len is contained in param_len 184 if (param_len < 1) return 0; 185 uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2]; 186 // assert continuation state is contained in param_len 187 if (1 + continuationState[0] > param_len) return 0; 188 189 // calc maximumServiceRecordCount based on remote MTU 190 uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4; 191 192 // continuation state contains index of next service record to examine 193 int continuation = 0; 194 uint16_t continuation_index = 0; 195 if (continuationState[0] == 2){ 196 continuation_index = big_endian_read_16(continuationState, 1); 197 } 198 199 // get and limit total count 200 btstack_linked_item_t *it; 201 uint16_t total_service_count = 0; 202 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next){ 203 service_record_item_t * item = (service_record_item_t *) it; 204 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 205 total_service_count++; 206 } 207 if (total_service_count > maximumServiceRecordCount){ 208 total_service_count = maximumServiceRecordCount; 209 } 210 211 // ServiceRecordHandleList at 9 212 uint16_t pos = 9; 213 uint16_t current_service_count = 0; 214 uint16_t current_service_index = 0; 215 uint16_t matching_service_count = 0; 216 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next, ++current_service_index){ 217 service_record_item_t * item = (service_record_item_t *) it; 218 219 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 220 matching_service_count++; 221 222 if (current_service_index < continuation_index) continue; 223 224 big_endian_store_32(sdp_response_buffer, pos, item->service_record_handle); 225 pos += 4; 226 current_service_count++; 227 228 if (matching_service_count >= total_service_count) break; 229 230 if (current_service_count >= maxNrServiceRecordsPerResponse){ 231 continuation = 1; 232 continuation_index = current_service_index + 1; 233 break; 234 } 235 } 236 237 // Store continuation state 238 if (continuation) { 239 sdp_response_buffer[pos++] = 2; 240 big_endian_store_16(sdp_response_buffer, pos, continuation_index); 241 pos += 2; 242 } else { 243 sdp_response_buffer[pos++] = 0; 244 } 245 246 // header 247 sdp_response_buffer[0] = SDP_ServiceSearchResponse; 248 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 249 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload 250 big_endian_store_16(sdp_response_buffer, 5, total_service_count); 251 big_endian_store_16(sdp_response_buffer, 7, current_service_count); 252 253 return pos; 254 } 255 256 int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu){ 257 258 // get request details 259 uint16_t transaction_id = big_endian_read_16(packet, 1); 260 uint16_t param_len = big_endian_read_16(packet, 3); 261 // assert serviceRecordHandle and maximumAttributeByteCount are in param_len 262 if (param_len < 6) return 0; 263 uint32_t serviceRecordHandle = big_endian_read_32(packet, 5); 264 uint16_t maximumAttributeByteCount = big_endian_read_16(packet, 9); 265 param_len -= 6; 266 uint8_t * attributeIDList = &packet[11]; 267 uint16_t attributeIDListLen = de_get_len_safe(attributeIDList, param_len); 268 // assert attributeIDList are in param_len 269 if (!attributeIDListLen) return 0; 270 param_len -= attributeIDListLen; 271 // assert continuation state len is contained in param_len 272 if (param_len < 1) return 0; 273 uint8_t * continuationState = &packet[11+attributeIDListLen]; 274 // assert continuation state is contained in param_len 275 if (1 + continuationState[0] > param_len) return 0; 276 277 // calc maximumAttributeByteCount based on remote MTU 278 uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3); 279 if (maximumAttributeByteCount2 < maximumAttributeByteCount) { 280 maximumAttributeByteCount = maximumAttributeByteCount2; 281 } 282 283 // continuation state contains the offset into the complete response 284 uint16_t continuation_offset = 0; 285 if (continuationState[0] == 2){ 286 continuation_offset = big_endian_read_16(continuationState, 1); 287 } 288 289 // get service record 290 service_record_item_t * item = sdp_get_record_item_for_handle(serviceRecordHandle); 291 if (!item){ 292 // service record handle doesn't exist 293 return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle 294 } 295 296 297 // AttributeList - starts at offset 7 298 uint16_t pos = 7; 299 300 if (continuation_offset == 0){ 301 302 // get size of this record 303 uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList); 304 305 // store DES 306 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size); 307 maximumAttributeByteCount -= 3; 308 pos += 3; 309 } 310 311 // copy maximumAttributeByteCount from record 312 uint16_t bytes_used; 313 int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]); 314 pos += bytes_used; 315 316 uint16_t attributeListByteCount = pos - 7; 317 318 if (complete) { 319 sdp_response_buffer[pos++] = 0; 320 } else { 321 continuation_offset += bytes_used; 322 sdp_response_buffer[pos++] = 2; 323 big_endian_store_16(sdp_response_buffer, pos, continuation_offset); 324 pos += 2; 325 } 326 327 // header 328 sdp_response_buffer[0] = SDP_ServiceAttributeResponse; 329 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 330 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload 331 big_endian_store_16(sdp_response_buffer, 5, attributeListByteCount); 332 333 return pos; 334 } 335 336 static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList){ 337 uint16_t total_response_size = 0; 338 btstack_linked_item_t *it; 339 for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next){ 340 service_record_item_t * item = (service_record_item_t *) it; 341 342 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 343 344 // for all service records that match 345 total_response_size += 3 + spd_get_filtered_size(item->service_record, attributeIDList); 346 } 347 return total_response_size; 348 } 349 350 int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu){ 351 352 // SDP header before attribute sevice list: 7 353 // Continuation, worst case: 5 354 355 // get request details 356 uint16_t transaction_id = big_endian_read_16(packet, 1); 357 uint16_t param_len = big_endian_read_16(packet, 3); 358 uint8_t * serviceSearchPattern = &packet[5]; 359 uint16_t serviceSearchPatternLen = de_get_len_safe(serviceSearchPattern, param_len); 360 // assert serviceSearchPattern header is contained in param_len 361 if (!serviceSearchPatternLen) return 0; 362 param_len -= serviceSearchPatternLen; 363 // assert maximumAttributeByteCount contained in param_len 364 if (param_len < 2) return 0; 365 uint16_t maximumAttributeByteCount = big_endian_read_16(packet, 5 + serviceSearchPatternLen); 366 param_len -= 2; 367 uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2]; 368 uint16_t attributeIDListLen = de_get_len_safe(attributeIDList, param_len); 369 // assert attributeIDList is contained in param_len 370 if (!attributeIDListLen) return 0; 371 // assert continuation state len is contained in param_len 372 if (param_len < 1) return 0; 373 uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen]; 374 // assert continuation state is contained in param_len 375 if (1 + continuationState[0] > param_len) return 0; 376 377 // calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block 378 uint16_t maximumAttributeByteCount2 = remote_mtu - 12; 379 if (maximumAttributeByteCount2 < maximumAttributeByteCount) { 380 maximumAttributeByteCount = maximumAttributeByteCount2; 381 } 382 383 // continuation state contains: index of next service record to examine 384 // continuation state contains: byte offset into this service record 385 uint16_t continuation_service_index = 0; 386 uint16_t continuation_offset = 0; 387 if (continuationState[0] == 4){ 388 continuation_service_index = big_endian_read_16(continuationState, 1); 389 continuation_offset = big_endian_read_16(continuationState, 3); 390 } 391 392 // log_info("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u", continuation_service_index, continuation_offset, maximumAttributeByteCount); 393 394 // AttributeLists - starts at offset 7 395 uint16_t pos = 7; 396 397 // add DES with total size for first request 398 if (continuation_service_index == 0 && continuation_offset == 0){ 399 uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList); 400 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size); 401 // log_info("total response size %u", total_response_size); 402 pos += 3; 403 maximumAttributeByteCount -= 3; 404 } 405 406 // create attribute list 407 int first_answer = 1; 408 int continuation = 0; 409 uint16_t current_service_index = 0; 410 btstack_linked_item_t *it = (btstack_linked_item_t *) sdp_service_records; 411 for ( ; it ; it = it->next, ++current_service_index){ 412 service_record_item_t * item = (service_record_item_t *) it; 413 414 if (current_service_index < continuation_service_index ) continue; 415 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; 416 417 if (continuation_offset == 0){ 418 419 // get size of this record 420 uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList); 421 422 // stop if complete record doesn't fits into response but we already have a partial response 423 if ((filtered_attributes_size + 3 > maximumAttributeByteCount) && !first_answer) { 424 continuation = 1; 425 break; 426 } 427 428 // store DES 429 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size); 430 pos += 3; 431 maximumAttributeByteCount -= 3; 432 } 433 434 first_answer = 0; 435 436 // copy maximumAttributeByteCount from record 437 uint16_t bytes_used; 438 int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]); 439 pos += bytes_used; 440 maximumAttributeByteCount -= bytes_used; 441 442 if (complete) { 443 continuation_offset = 0; 444 continue; 445 } 446 447 continuation = 1; 448 continuation_offset += bytes_used; 449 break; 450 } 451 452 uint16_t attributeListsByteCount = pos - 7; 453 454 // Continuation State 455 if (continuation){ 456 sdp_response_buffer[pos++] = 4; 457 big_endian_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index); 458 pos += 2; 459 big_endian_store_16(sdp_response_buffer, pos, continuation_offset); 460 pos += 2; 461 } else { 462 // complete 463 sdp_response_buffer[pos++] = 0; 464 } 465 466 // create SDP header 467 sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse; 468 big_endian_store_16(sdp_response_buffer, 1, transaction_id); 469 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload 470 big_endian_store_16(sdp_response_buffer, 5, attributeListsByteCount); 471 472 return pos; 473 } 474 475 static void sdp_respond(void){ 476 if (!sdp_response_size ) return; 477 if (!l2cap_cid) return; 478 479 // update state before sending packet (avoid getting called when new l2cap credit gets emitted) 480 uint16_t size = sdp_response_size; 481 sdp_response_size = 0; 482 l2cap_send(l2cap_cid, sdp_response_buffer, size); 483 } 484 485 // we assume that we don't get two requests in a row 486 static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 487 uint16_t transaction_id; 488 SDP_PDU_ID_t pdu_id; 489 uint16_t remote_mtu; 490 uint16_t param_len; 491 492 switch (packet_type) { 493 494 case L2CAP_DATA_PACKET: 495 pdu_id = (SDP_PDU_ID_t) packet[0]; 496 transaction_id = big_endian_read_16(packet, 1); 497 param_len = big_endian_read_16(packet, 3); 498 remote_mtu = l2cap_get_remote_mtu_for_local_cid(channel); 499 // account for our buffer 500 if (remote_mtu > SDP_RESPONSE_BUFFER_SIZE){ 501 remote_mtu = SDP_RESPONSE_BUFFER_SIZE; 502 } 503 // validate parm_len against packet size 504 if (param_len + 5 > size) { 505 // just clear pdu_id 506 pdu_id = SDP_ErrorResponse; 507 } 508 509 // log_info("SDP Request: type %u, transaction id %u, len %u, mtu %u", pdu_id, transaction_id, param_len, remote_mtu); 510 switch (pdu_id){ 511 512 case SDP_ServiceSearchRequest: 513 sdp_response_size = sdp_handle_service_search_request(packet, remote_mtu); 514 break; 515 516 case SDP_ServiceAttributeRequest: 517 sdp_response_size = sdp_handle_service_attribute_request(packet, remote_mtu); 518 break; 519 520 case SDP_ServiceSearchAttributeRequest: 521 sdp_response_size = sdp_handle_service_search_attribute_request(packet, remote_mtu); 522 break; 523 524 default: 525 sdp_response_size = sdp_create_error_response(transaction_id, 0x0003); // invalid syntax 526 break; 527 } 528 if (!sdp_response_size) break; 529 l2cap_request_can_send_now_event(l2cap_cid); 530 break; 531 532 case HCI_EVENT_PACKET: 533 534 switch (hci_event_packet_get_type(packet)) { 535 536 case L2CAP_EVENT_INCOMING_CONNECTION: 537 if (l2cap_cid) { 538 // CONNECTION REJECTED DUE TO LIMITED RESOURCES 539 l2cap_decline_connection(channel); 540 break; 541 } 542 // accept 543 l2cap_cid = channel; 544 sdp_response_size = 0; 545 l2cap_accept_connection(channel); 546 break; 547 548 case L2CAP_EVENT_CHANNEL_OPENED: 549 if (packet[2]) { 550 // open failed -> reset 551 l2cap_cid = 0; 552 } 553 break; 554 555 case L2CAP_EVENT_CAN_SEND_NOW: 556 sdp_respond(); 557 break; 558 559 case L2CAP_EVENT_CHANNEL_CLOSED: 560 if (channel == l2cap_cid){ 561 // reset 562 l2cap_cid = 0; 563 } 564 break; 565 566 default: 567 // other event 568 break; 569 } 570 break; 571 572 default: 573 // other packet type 574 break; 575 } 576 } 577 578