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