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 BLUEKITCHEN
24 * GMBH 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 <string.h>
45
46 #include "bluetooth.h"
47 #include "bluetooth_psm.h"
48 #include "bluetooth_sdp.h"
49 #include "btstack_debug.h"
50 #include "btstack_event.h"
51 #include "btstack_memory.h"
52 #include "classic/core.h"
53 #include "classic/sdp_server.h"
54 #include "classic/sdp_util.h"
55 #include "hci.h"
56 #include "hci_dump.h"
57 #include "l2cap.h"
58
59 // max number of incoming l2cap connections that can be queued instead of getting rejected
60 #ifndef SDP_WAITING_LIST_MAX_COUNT
61 #define SDP_WAITING_LIST_MAX_COUNT 8
62 #endif
63
64 // max reserved ServiceRecordHandle
65 #define MAX_RESERVED_SERVICE_RECORD_HANDLE 0xffff
66
67 // max SDP response matches L2CAP PDU -- allow to use smaller buffer
68 #ifndef SDP_RESPONSE_BUFFER_SIZE
69 #define SDP_RESPONSE_BUFFER_SIZE (HCI_ACL_PAYLOAD_SIZE-L2CAP_HEADER_SIZE)
70 #endif
71
72 static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
73
74 // registered service records
75 static btstack_linked_list_t sdp_server_service_records;
76
77 // our handles start after the reserved range
78 static uint32_t sdp_server_next_service_record_handle;
79
80 static uint8_t sdp_response_buffer[SDP_RESPONSE_BUFFER_SIZE];
81
82 static uint16_t sdp_server_l2cap_cid;
83 static uint16_t sdp_server_response_size;
84 static uint16_t sdp_server_l2cap_waiting_list_cids[SDP_WAITING_LIST_MAX_COUNT];
85 static int sdp_server_l2cap_waiting_list_count;
86
87 #ifdef ENABLE_TESTING_SUPPORT
88 static bool sdp_server_testing_single_record_reponse = false;
89 #endif
90
sdp_init(void)91 void sdp_init(void){
92 sdp_server_next_service_record_handle = ((uint32_t) MAX_RESERVED_SERVICE_RECORD_HANDLE) + 2;
93 // register with l2cap psm sevices - max MTU
94 l2cap_register_service(sdp_packet_handler, BLUETOOTH_PSM_SDP, 0xffff, LEVEL_0);
95 }
96
sdp_deinit(void)97 void sdp_deinit(void){
98 sdp_server_service_records = NULL;
99 sdp_server_l2cap_cid = 0;
100 sdp_server_response_size = 0;
101 sdp_server_l2cap_waiting_list_count = 0;
102 }
103
sdp_get_service_record_handle(const uint8_t * record)104 uint32_t sdp_get_service_record_handle(const uint8_t * record){
105 // TODO: make sdp_get_attribute_value_for_attribute_id accept const data to remove cast
106 uint8_t * serviceRecordHandleAttribute = sdp_get_attribute_value_for_attribute_id((uint8_t *)record, BLUETOOTH_ATTRIBUTE_SERVICE_RECORD_HANDLE);
107 if (!serviceRecordHandleAttribute) return 0;
108 if (de_get_element_type(serviceRecordHandleAttribute) != DE_UINT) return 0;
109 if (de_get_size_type(serviceRecordHandleAttribute) != DE_SIZE_32) return 0;
110 return big_endian_read_32(serviceRecordHandleAttribute, 1);
111 }
112
sdp_get_record_item_for_handle(uint32_t handle)113 static service_record_item_t * sdp_get_record_item_for_handle(uint32_t handle){
114 btstack_linked_item_t *it;
115 for (it = (btstack_linked_item_t *) sdp_server_service_records; it ; it = it->next){
116 service_record_item_t * item = (service_record_item_t *) it;
117 if (item->service_record_handle == handle){
118 return item;
119 }
120 }
121 return NULL;
122 }
123
sdp_get_record_for_handle(uint32_t handle)124 uint8_t * sdp_get_record_for_handle(uint32_t handle){
125 service_record_item_t * record_item = sdp_get_record_item_for_handle(handle);
126 if (!record_item) return NULL;
127 return record_item->service_record;
128 }
129
130 // get next free, unregistered service record handle
sdp_create_service_record_handle(void)131 uint32_t sdp_create_service_record_handle(void){
132 uint32_t handle = 0;
133 do {
134 handle = sdp_server_next_service_record_handle++;
135 if (sdp_get_record_item_for_handle(handle)) handle = 0;
136 } while (handle == 0);
137 return handle;
138 }
139
140 /**
141 * @brief Register Service Record with database using ServiceRecordHandle stored in record
142 * @pre AttributeIDs are in ascending order
143 * @pre ServiceRecordHandle is first attribute and valid
144 * @param record is not copied!
145 * @result status
146 */
sdp_register_service(const uint8_t * record)147 uint8_t sdp_register_service(const uint8_t * record){
148
149 // validate service record handle. it must: exist, be in valid range, not have been already used
150 uint32_t record_handle = sdp_get_service_record_handle(record);
151 if (!record_handle) return SDP_HANDLE_INVALID;
152 if (record_handle <= MAX_RESERVED_SERVICE_RECORD_HANDLE) return SDP_HANDLE_INVALID;
153 if (sdp_get_record_item_for_handle(record_handle)) return SDP_HANDLE_ALREADY_REGISTERED;
154
155 // alloc memory for new service_record_item
156 service_record_item_t * newRecordItem = btstack_memory_service_record_item_get();
157 if (!newRecordItem) return BTSTACK_MEMORY_ALLOC_FAILED;
158
159 // set handle and record
160 newRecordItem->service_record_handle = record_handle;
161 newRecordItem->service_record = (uint8_t*) record;
162
163 // add to linked list
164 btstack_linked_list_add(&sdp_server_service_records, (btstack_linked_item_t *) newRecordItem);
165
166 return 0;
167 }
168
169 //
170 // unregister service record
171 //
sdp_unregister_service(uint32_t service_record_handle)172 void sdp_unregister_service(uint32_t service_record_handle){
173 service_record_item_t * record_item = sdp_get_record_item_for_handle(service_record_handle);
174 if (!record_item) return;
175 btstack_linked_list_remove(&sdp_server_service_records, (btstack_linked_item_t *) record_item);
176 btstack_memory_service_record_item_free(record_item);
177 }
178
179 // PDU
180 // PDU ID (1), Transaction ID (2), Param Length (2), Param 1, Param 2, ..
181
sdp_create_error_response(uint16_t transaction_id,uint16_t error_code)182 static int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code){
183 sdp_response_buffer[0] = SDP_ErrorResponse;
184 big_endian_store_16(sdp_response_buffer, 1, transaction_id);
185 big_endian_store_16(sdp_response_buffer, 3, 2);
186 big_endian_store_16(sdp_response_buffer, 5, error_code); // invalid syntax
187 return 7;
188 }
189
sdp_handle_service_search_request(uint8_t * packet,uint16_t remote_mtu)190 int sdp_handle_service_search_request(uint8_t * packet, uint16_t remote_mtu){
191
192 // get request details
193 uint16_t transaction_id = big_endian_read_16(packet, 1);
194 uint16_t param_len = big_endian_read_16(packet, 3);
195 uint8_t * serviceSearchPattern = &packet[5];
196 uint16_t serviceSearchPatternLen = de_get_len_safe(serviceSearchPattern, param_len);
197 if (sdp_valid_service_search_pattern(serviceSearchPattern) == false){
198 return sdp_create_error_response(transaction_id, 0x0003); /// invalid request syntax
199 }
200 // assert service search pattern is contained
201 if (!serviceSearchPatternLen) return 0;
202 param_len -= serviceSearchPatternLen;
203 // assert max record count is contained
204 if (param_len < 2) return 0;
205 uint16_t maximumServiceRecordCount = big_endian_read_16(packet, 5 + serviceSearchPatternLen);
206 param_len -= 2;
207 // assert continuation state len is contained in param_len
208 if (param_len < 1) return 0;
209 uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2];
210 // assert continuation state is contained in param_len
211 if ((1 + continuationState[0]) > param_len) return 0;
212
213 // calc maximumServiceRecordCount based on remote MTU
214 uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4;
215 #ifdef ENABLE_TESTING_SUPPORT
216 if (sdp_server_testing_single_record_reponse){
217 maxNrServiceRecordsPerResponse = 1;
218 }
219 #endif
220
221 // continuation state contains index of next service record to examine
222 int continuation = 0;
223 uint16_t continuation_index = 0;
224 if (continuationState[0] == 2){
225 continuation_index = big_endian_read_16(continuationState, 1);
226 }
227
228 // get and limit total count
229 btstack_linked_item_t *it;
230 uint16_t total_service_count = 0;
231 for (it = (btstack_linked_item_t *) sdp_server_service_records; it ; it = it->next){
232 service_record_item_t * item = (service_record_item_t *) it;
233 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
234 total_service_count++;
235 }
236 if (total_service_count > maximumServiceRecordCount){
237 total_service_count = maximumServiceRecordCount;
238 }
239
240 // ServiceRecordHandleList at 9
241 uint16_t pos = 9;
242 uint16_t current_service_count = 0;
243 uint16_t current_service_index = 0;
244 uint16_t matching_service_count = 0;
245 for (it = (btstack_linked_item_t *) sdp_server_service_records; it ; it = it->next, ++current_service_index){
246 service_record_item_t * item = (service_record_item_t *) it;
247
248 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
249 matching_service_count++;
250
251 if (current_service_index < continuation_index) continue;
252
253 big_endian_store_32(sdp_response_buffer, pos, item->service_record_handle);
254 pos += 4;
255 current_service_count++;
256
257 if (matching_service_count >= total_service_count) break;
258
259 if (current_service_count >= maxNrServiceRecordsPerResponse){
260 continuation = 1;
261 continuation_index = current_service_index + 1;
262 break;
263 }
264 }
265
266 // Store continuation state
267 if (continuation) {
268 sdp_response_buffer[pos++] = 2;
269 big_endian_store_16(sdp_response_buffer, pos, continuation_index);
270 pos += 2;
271 } else {
272 sdp_response_buffer[pos++] = 0;
273 }
274
275 // header
276 sdp_response_buffer[0] = SDP_ServiceSearchResponse;
277 big_endian_store_16(sdp_response_buffer, 1, transaction_id);
278 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
279 big_endian_store_16(sdp_response_buffer, 5, total_service_count);
280 big_endian_store_16(sdp_response_buffer, 7, current_service_count);
281
282 return pos;
283 }
284
sdp_handle_service_attribute_request(uint8_t * packet,uint16_t remote_mtu)285 int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu){
286
287 // get request details
288 uint16_t transaction_id = big_endian_read_16(packet, 1);
289 uint16_t param_len = big_endian_read_16(packet, 3);
290 // assert serviceRecordHandle and maximumAttributeByteCount are in param_len
291 if (param_len < 6) return 0;
292 uint32_t serviceRecordHandle = big_endian_read_32(packet, 5);
293 uint16_t maximumAttributeByteCount = big_endian_read_16(packet, 9);
294 param_len -= 6;
295 uint8_t * attributeIDList = &packet[11];
296 uint16_t attributeIDListLen = de_get_len_safe(attributeIDList, param_len);
297 if (!sdp_attribute_list_valid(attributeIDList) == false){
298 return sdp_create_error_response(transaction_id, 0x0003); /// invalid request syntax
299 }
300 // assert attributeIDList are in param_len
301 if (!attributeIDListLen) return 0;
302 param_len -= attributeIDListLen;
303 // assert continuation state len is contained in param_len
304 if (param_len < 1) return 0;
305 uint8_t * continuationState = &packet[11+attributeIDListLen];
306 // assert continuation state is contained in param_len
307 if ((1 + continuationState[0]) > param_len) return 0;
308
309 // calc maximumAttributeByteCount based on remote MTU
310 uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3);
311 if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
312 maximumAttributeByteCount = maximumAttributeByteCount2;
313 }
314
315 // continuation state contains the offset into the complete response
316 uint16_t continuation_offset = 0;
317 if (continuationState[0] == 2){
318 continuation_offset = big_endian_read_16(continuationState, 1);
319 }
320
321 // get service record
322 service_record_item_t * item = sdp_get_record_item_for_handle(serviceRecordHandle);
323 if (!item){
324 // service record handle doesn't exist
325 return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle
326 }
327
328
329 // AttributeList - starts at offset 7
330 uint16_t pos = 7;
331
332 if (continuation_offset == 0){
333
334 // get size of this record
335 uint16_t filtered_attributes_size = sdp_get_filtered_size(item->service_record, attributeIDList);
336
337 // store DES
338 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
339 maximumAttributeByteCount -= 3;
340 pos += 3;
341 }
342
343 // copy maximumAttributeByteCount from record
344 uint16_t bytes_used;
345 int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
346 pos += bytes_used;
347
348 uint16_t attributeListByteCount = pos - 7;
349
350 if (complete) {
351 sdp_response_buffer[pos++] = 0;
352 } else {
353 continuation_offset += bytes_used;
354 sdp_response_buffer[pos++] = 2;
355 big_endian_store_16(sdp_response_buffer, pos, continuation_offset);
356 pos += 2;
357 }
358
359 // header
360 sdp_response_buffer[0] = SDP_ServiceAttributeResponse;
361 big_endian_store_16(sdp_response_buffer, 1, transaction_id);
362 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
363 big_endian_store_16(sdp_response_buffer, 5, attributeListByteCount);
364
365 return pos;
366 }
367
sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern,uint8_t * attributeIDList)368 static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList){
369 uint16_t total_response_size = 0;
370 btstack_linked_item_t *it;
371 for (it = (btstack_linked_item_t *) sdp_server_service_records; it ; it = it->next){
372 service_record_item_t * item = (service_record_item_t *) it;
373
374 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
375
376 // for all service records that match, ignoring empty attribute lists
377 uint16_t filtered_size = sdp_get_filtered_size(item->service_record, attributeIDList);
378 if (filtered_size > 0){
379 total_response_size += 3 + filtered_size;
380 }
381 }
382 return total_response_size;
383 }
384
sdp_handle_service_search_attribute_request(uint8_t * packet,uint16_t remote_mtu)385 int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu){
386
387 // SDP header before attribute service list: 7
388 // Continuation, worst case: 5
389
390 // get request details
391 uint16_t transaction_id = big_endian_read_16(packet, 1);
392 uint16_t param_len = big_endian_read_16(packet, 3);
393 uint8_t * serviceSearchPattern = &packet[5];
394 uint16_t serviceSearchPatternLen = de_get_len_safe(serviceSearchPattern, param_len);
395 if (sdp_valid_service_search_pattern(serviceSearchPattern) == false){
396 return sdp_create_error_response(transaction_id, 0x0003); /// invalid request syntax
397 }
398 // assert serviceSearchPattern header is contained in param_len
399 if (!serviceSearchPatternLen) return 0;
400 param_len -= serviceSearchPatternLen;
401 // assert maximumAttributeByteCount contained in param_len
402 if (param_len < 2) return 0;
403 uint16_t maximumAttributeByteCount = big_endian_read_16(packet, 5 + serviceSearchPatternLen);
404 param_len -= 2;
405 uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2];
406 uint16_t attributeIDListLen = de_get_len_safe(attributeIDList, param_len);
407 if (!sdp_attribute_list_valid(attributeIDList) == false){
408 return sdp_create_error_response(transaction_id, 0x0003); /// invalid request syntax
409 }
410 // assert attributeIDList is contained in param_len
411 if (!attributeIDListLen) return 0;
412 // assert continuation state len is contained in param_len
413 if (param_len < 1) return 0;
414 uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen];
415 // assert continuation state is contained in param_len
416 if ((1 + continuationState[0]) > param_len) return 0;
417
418 // calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block
419 uint16_t maximumAttributeByteCount2 = remote_mtu - 12;
420 if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
421 maximumAttributeByteCount = maximumAttributeByteCount2;
422 }
423
424 // continuation state contains: index of next service record to examine
425 // continuation state contains: byte offset into this service record
426 uint16_t continuation_service_index = 0;
427 uint16_t continuation_offset = 0;
428 if (continuationState[0] == 4){
429 continuation_service_index = big_endian_read_16(continuationState, 1);
430 continuation_offset = big_endian_read_16(continuationState, 3);
431 }
432
433 // log_info("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u", continuation_service_index, continuation_offset, maximumAttributeByteCount);
434
435 // AttributeLists - starts at offset 7
436 uint16_t pos = 7;
437
438 // add DES with total size for first request
439 if ((continuation_service_index == 0) && (continuation_offset == 0)){
440 uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList);
441 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size);
442 // log_info("total response size %u", total_response_size);
443 pos += 3;
444 maximumAttributeByteCount -= 3;
445 }
446
447 // create attribute list
448 int first_answer = 1;
449 int continuation = 0;
450 uint16_t current_service_index = 0;
451 btstack_linked_item_t *it = (btstack_linked_item_t *) sdp_server_service_records;
452 for ( ; it ; it = it->next, ++current_service_index){
453 service_record_item_t * item = (service_record_item_t *) it;
454
455 if (current_service_index < continuation_service_index ) continue;
456 if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
457
458 if (continuation_offset == 0){
459
460 // get size of this record
461 uint16_t filtered_attributes_size = sdp_get_filtered_size(item->service_record, attributeIDList);
462
463 // ignore empty lists
464 if (filtered_attributes_size == 0) continue;
465
466 // stop if complete record doesn't fits into response but we already have a partial response
467 if (((filtered_attributes_size + 3) > maximumAttributeByteCount) && !first_answer) {
468 continuation = 1;
469 break;
470 }
471
472 // store DES
473 de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
474 pos += 3;
475 maximumAttributeByteCount -= 3;
476 }
477
478 first_answer = 0;
479
480 // copy maximumAttributeByteCount from record
481 uint16_t bytes_used;
482 int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
483 pos += bytes_used;
484 maximumAttributeByteCount -= bytes_used;
485
486 if (complete) {
487 continuation_offset = 0;
488 continue;
489 }
490
491 continuation = 1;
492 continuation_offset += bytes_used;
493 break;
494 }
495
496 uint16_t attributeListsByteCount = pos - 7;
497
498 // Continuation State
499 if (continuation){
500 sdp_response_buffer[pos++] = 4;
501 big_endian_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index);
502 pos += 2;
503 big_endian_store_16(sdp_response_buffer, pos, continuation_offset);
504 pos += 2;
505 } else {
506 // complete
507 sdp_response_buffer[pos++] = 0;
508 }
509
510 // create SDP header
511 sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse;
512 big_endian_store_16(sdp_response_buffer, 1, transaction_id);
513 big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
514 big_endian_store_16(sdp_response_buffer, 5, attributeListsByteCount);
515
516 return pos;
517 }
518
sdp_respond(void)519 static void sdp_respond(void){
520 if (!sdp_server_response_size ) return;
521 if (!sdp_server_l2cap_cid) return;
522
523 // update state before sending packet (avoid getting called when new l2cap credit gets emitted)
524 uint16_t size = sdp_server_response_size;
525 sdp_server_response_size = 0;
526 l2cap_send(sdp_server_l2cap_cid, sdp_response_buffer, size);
527 }
528
529 // @pre space in list
sdp_waiting_list_add(uint16_t cid)530 static void sdp_waiting_list_add(uint16_t cid){
531 sdp_server_l2cap_waiting_list_cids[sdp_server_l2cap_waiting_list_count++] = cid;
532 }
533
534 // @pre at least one item in list
sdp_waiting_list_get(void)535 static uint16_t sdp_waiting_list_get(void){
536 uint16_t cid = sdp_server_l2cap_waiting_list_cids[0];
537 sdp_server_l2cap_waiting_list_count--;
538 if (sdp_server_l2cap_waiting_list_count){
539 memmove(&sdp_server_l2cap_waiting_list_cids[0], &sdp_server_l2cap_waiting_list_cids[1], sdp_server_l2cap_waiting_list_count * sizeof(uint16_t));
540 }
541 return cid;
542 }
543
544 // we assume that we don't get two requests in a row
sdp_packet_handler(uint8_t packet_type,uint16_t channel,uint8_t * packet,uint16_t size)545 static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
546 uint16_t transaction_id;
547 sdp_pdu_id_t pdu_id;
548 uint16_t remote_mtu;
549 uint16_t param_len;
550
551 switch (packet_type) {
552
553 case L2CAP_DATA_PACKET:
554 pdu_id = (sdp_pdu_id_t) packet[0];
555 transaction_id = big_endian_read_16(packet, 1);
556 param_len = big_endian_read_16(packet, 3);
557 remote_mtu = l2cap_get_remote_mtu_for_local_cid(channel);
558 // account for our buffer
559 if (remote_mtu > SDP_RESPONSE_BUFFER_SIZE){
560 remote_mtu = SDP_RESPONSE_BUFFER_SIZE;
561 }
562 // validate parm_len against packet size
563 if ((param_len + 5) > size) {
564 // just clear pdu_id
565 pdu_id = SDP_ErrorResponse;
566 }
567
568 // log_info("SDP Request: type %u, transaction id %u, len %u, mtu %u", pdu_id, transaction_id, param_len, remote_mtu);
569 switch (pdu_id){
570
571 case SDP_ServiceSearchRequest:
572 sdp_server_response_size = sdp_handle_service_search_request(packet, remote_mtu);
573 break;
574
575 case SDP_ServiceAttributeRequest:
576 sdp_server_response_size = sdp_handle_service_attribute_request(packet, remote_mtu);
577 break;
578
579 case SDP_ServiceSearchAttributeRequest:
580 sdp_server_response_size = sdp_handle_service_search_attribute_request(packet, remote_mtu);
581 break;
582
583 default:
584 sdp_server_response_size = sdp_create_error_response(transaction_id, 0x0004); // invalid PDU size
585 break;
586 }
587 if (!sdp_server_response_size) break;
588 l2cap_request_can_send_now_event(sdp_server_l2cap_cid);
589 break;
590
591 case HCI_EVENT_PACKET:
592
593 switch (hci_event_packet_get_type(packet)) {
594
595 case L2CAP_EVENT_INCOMING_CONNECTION:
596 if (sdp_server_l2cap_cid) {
597 // try to queue up
598 if (sdp_server_l2cap_waiting_list_count < SDP_WAITING_LIST_MAX_COUNT){
599 sdp_waiting_list_add(channel);
600 log_info("busy, queing incoming cid 0x%04x, now %u waiting", channel, sdp_server_l2cap_waiting_list_count);
601 break;
602 }
603
604 // CONNECTION REJECTED DUE TO LIMITED RESOURCES
605 l2cap_decline_connection(channel);
606 break;
607 }
608 // accept
609 sdp_server_l2cap_cid = channel;
610 sdp_server_response_size = 0;
611 l2cap_accept_connection(sdp_server_l2cap_cid);
612 break;
613
614 case L2CAP_EVENT_CHANNEL_OPENED:
615 if (packet[2]) {
616 // open failed -> reset
617 sdp_server_l2cap_cid = 0;
618 }
619 break;
620
621 case L2CAP_EVENT_CAN_SEND_NOW:
622 sdp_respond();
623 break;
624
625 case L2CAP_EVENT_CHANNEL_CLOSED:
626 if (channel == sdp_server_l2cap_cid){
627 // reset
628 sdp_server_l2cap_cid = 0;
629
630 // other request queued?
631 if (!sdp_server_l2cap_waiting_list_count) break;
632
633 // get first item
634 sdp_server_l2cap_cid = sdp_waiting_list_get();
635
636 log_info("disconnect, accept queued cid 0x%04x, now %u waiting", sdp_server_l2cap_cid, sdp_server_l2cap_waiting_list_count);
637
638 // accept connection
639 sdp_server_response_size = 0;
640 l2cap_accept_connection(sdp_server_l2cap_cid);
641 }
642 break;
643
644 default:
645 // other event
646 break;
647 }
648 break;
649
650 default:
651 // other packet type
652 break;
653 }
654 }
655
656 #ifdef ENABLE_TESTING_SUPPORT
sdp_server_set_single_record_response(bool enable)657 void sdp_server_set_single_record_response(bool enable){
658 sdp_server_testing_single_record_reponse = enable;
659 }
660 #endif