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