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