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