1 /** @file
2 * @brief Service Discovery Protocol handling.
3 */
4
5 /*
6 * Copyright (c) 2016 Intel Corporation
7 *
8 * SPDX-License-Identifier: Apache-2.0
9 */
10
11 #include <errno.h>
12 #include <sys/types.h>
13 #include <sys/byteorder.h>
14 #include <sys/__assert.h>
15
16 #include <bluetooth/buf.h>
17 #include <bluetooth/sdp.h>
18
19 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_SDP)
20 #define LOG_MODULE_NAME bt_sdp
21 #include "common/log.h"
22
23 #include "hci_core.h"
24 #include "conn_internal.h"
25 #include "l2cap_internal.h"
26 #include "sdp_internal.h"
27
28 #define SDP_PSM 0x0001
29
30 #define SDP_CHAN(_ch) CONTAINER_OF(_ch, struct bt_sdp, chan.chan)
31
32 #define IN_RANGE(val, min, max) (val >= min && val <= max)
33
34 #define SDP_DATA_MTU 200
35
36 #define SDP_MTU (SDP_DATA_MTU + sizeof(struct bt_sdp_hdr))
37
38 #define MAX_NUM_ATT_ID_FILTER 10
39
40 #define SDP_SERVICE_HANDLE_BASE 0x10000
41
42 #define SDP_DATA_ELEM_NEST_LEVEL_MAX 5
43
44 /* Size of Cont state length */
45 #define SDP_CONT_STATE_LEN_SIZE 1
46
47 /* 1 byte for the no. of services searched till this response */
48 /* 2 bytes for the total no. of matching records */
49 #define SDP_SS_CONT_STATE_SIZE 3
50
51 /* 1 byte for the no. of attributes searched till this response */
52 #define SDP_SA_CONT_STATE_SIZE 1
53
54 /* 1 byte for the no. of services searched till this response */
55 /* 1 byte for the no. of attributes searched till this response */
56 #define SDP_SSA_CONT_STATE_SIZE 2
57
58 #define SDP_INVALID 0xff
59
60 struct bt_sdp {
61 struct bt_l2cap_br_chan chan;
62 struct k_fifo partial_resp_queue;
63 /* TODO: Allow more than one pending request */
64 };
65
66 static struct bt_sdp_record *db;
67 static uint8_t num_services;
68
69 static struct bt_sdp bt_sdp_pool[CONFIG_BT_MAX_CONN];
70
71 /* Pool for outgoing SDP packets */
72 NET_BUF_POOL_FIXED_DEFINE(sdp_pool, CONFIG_BT_MAX_CONN,
73 BT_L2CAP_BUF_SIZE(SDP_MTU), NULL);
74
75 #define SDP_CLIENT_CHAN(_ch) CONTAINER_OF(_ch, struct bt_sdp_client, chan.chan)
76
77 #define SDP_CLIENT_MTU 64
78
79 struct bt_sdp_client {
80 struct bt_l2cap_br_chan chan;
81 /* list of waiting to be resolved UUID params */
82 sys_slist_t reqs;
83 /* required SDP transaction ID */
84 uint16_t tid;
85 /* UUID params holder being now resolved */
86 const struct bt_sdp_discover_params *param;
87 /* PDU continuation state object */
88 struct bt_sdp_pdu_cstate cstate;
89 /* buffer for collecting record data */
90 struct net_buf *rec_buf;
91 };
92
93 static struct bt_sdp_client bt_sdp_client_pool[CONFIG_BT_MAX_CONN];
94
95 enum {
96 BT_SDP_ITER_STOP,
97 BT_SDP_ITER_CONTINUE,
98 };
99
100 struct search_state {
101 uint16_t att_list_size;
102 uint8_t current_svc;
103 uint8_t last_att;
104 bool pkt_full;
105 };
106
107 struct select_attrs_data {
108 struct bt_sdp_record *rec;
109 struct net_buf *rsp_buf;
110 struct bt_sdp *sdp;
111 struct bt_sdp_data_elem_seq *seq;
112 struct search_state *state;
113 uint32_t *filter;
114 uint16_t max_att_len;
115 uint16_t att_list_len;
116 uint8_t cont_state_size;
117 uint8_t num_filters;
118 bool new_service;
119 };
120
121 /* @typedef bt_sdp_attr_func_t
122 * @brief SDP attribute iterator callback.
123 *
124 * @param attr Attribute found.
125 * @param att_idx Index of the found attribute in the attribute database.
126 * @param user_data Data given.
127 *
128 * @return BT_SDP_ITER_CONTINUE if should continue to the next attribute
129 * or BT_SDP_ITER_STOP to stop.
130 */
131 typedef uint8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr,
132 uint8_t att_idx, void *user_data);
133
134 /* @typedef bt_sdp_svc_func_t
135 * @brief SDP service record iterator callback.
136 *
137 * @param rec Service record found.
138 * @param user_data Data given.
139 *
140 * @return BT_SDP_ITER_CONTINUE if should continue to the next service record
141 * or BT_SDP_ITER_STOP to stop.
142 */
143 typedef uint8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec,
144 void *user_data);
145
146 /* @brief Callback for SDP connection
147 *
148 * Gets called when an SDP connection is established
149 *
150 * @param chan L2CAP channel
151 *
152 * @return None
153 */
bt_sdp_connected(struct bt_l2cap_chan * chan)154 static void bt_sdp_connected(struct bt_l2cap_chan *chan)
155 {
156 struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan,
157 struct bt_l2cap_br_chan,
158 chan);
159
160 struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan);
161
162 BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid);
163
164 k_fifo_init(&sdp->partial_resp_queue);
165 }
166
167 /** @brief Callback for SDP disconnection
168 *
169 * Gets called when an SDP connection is terminated
170 *
171 * @param chan L2CAP channel
172 *
173 * @return None
174 */
bt_sdp_disconnected(struct bt_l2cap_chan * chan)175 static void bt_sdp_disconnected(struct bt_l2cap_chan *chan)
176 {
177 struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan,
178 struct bt_l2cap_br_chan,
179 chan);
180
181 struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan);
182
183 BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid);
184
185 (void)memset(sdp, 0, sizeof(*sdp));
186 }
187
188 /* @brief Creates an SDP PDU
189 *
190 * Creates an empty SDP PDU and returns the buffer
191 *
192 * @param None
193 *
194 * @return Pointer to the net_buf buffer
195 */
bt_sdp_create_pdu(void)196 static struct net_buf *bt_sdp_create_pdu(void)
197 {
198 return bt_l2cap_create_pdu(&sdp_pool, sizeof(struct bt_sdp_hdr));
199 }
200
201 /* @brief Sends out an SDP PDU
202 *
203 * Sends out an SDP PDU after adding the relevant header
204 *
205 * @param chan L2CAP channel
206 * @param buf Buffer to be sent out
207 * @param op Opcode to be used in the packet header
208 * @param tid Transaction ID to be used in the packet header
209 *
210 * @return None
211 */
bt_sdp_send(struct bt_l2cap_chan * chan,struct net_buf * buf,uint8_t op,uint16_t tid)212 static int bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf,
213 uint8_t op, uint16_t tid)
214 {
215 struct bt_sdp_hdr *hdr;
216 uint16_t param_len = buf->len;
217 int err;
218
219 hdr = net_buf_push(buf, sizeof(struct bt_sdp_hdr));
220 hdr->op_code = op;
221 hdr->tid = tid;
222 hdr->param_len = sys_cpu_to_be16(param_len);
223
224 err = bt_l2cap_chan_send(chan, buf);
225 if (err < 0) {
226 net_buf_unref(buf);
227 }
228
229 return err;
230 }
231
232 /* @brief Sends an error response PDU
233 *
234 * Creates and sends an error response PDU
235 *
236 * @param chan L2CAP channel
237 * @param err Error code to be sent in the packet
238 * @param tid Transaction ID to be used in the packet header
239 *
240 * @return None
241 */
send_err_rsp(struct bt_l2cap_chan * chan,uint16_t err,uint16_t tid)242 static void send_err_rsp(struct bt_l2cap_chan *chan, uint16_t err,
243 uint16_t tid)
244 {
245 struct net_buf *buf;
246
247 BT_DBG("tid %u, error %u", tid, err);
248
249 buf = bt_sdp_create_pdu();
250
251 net_buf_add_be16(buf, err);
252
253 bt_sdp_send(chan, buf, BT_SDP_ERROR_RSP, tid);
254 }
255
256 /* @brief Parses data elements from a net_buf
257 *
258 * Parses the first data element from a buffer and splits it into type, size,
259 * data. Used for parsing incoming requests. Net buf is advanced to the data
260 * part of the element.
261 *
262 * @param buf Buffer to be advanced
263 * @param data_elem Pointer to the parsed data element structure
264 *
265 * @return 0 for success, or relevant error code
266 */
parse_data_elem(struct net_buf * buf,struct bt_sdp_data_elem * data_elem)267 static uint16_t parse_data_elem(struct net_buf *buf,
268 struct bt_sdp_data_elem *data_elem)
269 {
270 uint8_t size_field_len = 0U; /* Space used to accommodate the size */
271
272 if (buf->len < 1) {
273 BT_WARN("Malformed packet");
274 return BT_SDP_INVALID_SYNTAX;
275 }
276
277 data_elem->type = net_buf_pull_u8(buf);
278
279 switch (data_elem->type & BT_SDP_TYPE_DESC_MASK) {
280 case BT_SDP_UINT8:
281 case BT_SDP_INT8:
282 case BT_SDP_UUID_UNSPEC:
283 case BT_SDP_BOOL:
284 data_elem->data_size = BIT(data_elem->type &
285 BT_SDP_SIZE_DESC_MASK);
286 break;
287 case BT_SDP_TEXT_STR_UNSPEC:
288 case BT_SDP_SEQ_UNSPEC:
289 case BT_SDP_ALT_UNSPEC:
290 case BT_SDP_URL_STR_UNSPEC:
291 size_field_len = BIT((data_elem->type & BT_SDP_SIZE_DESC_MASK) -
292 BT_SDP_SIZE_INDEX_OFFSET);
293 if (buf->len < size_field_len) {
294 BT_WARN("Malformed packet");
295 return BT_SDP_INVALID_SYNTAX;
296 }
297 switch (size_field_len) {
298 case 1:
299 data_elem->data_size = net_buf_pull_u8(buf);
300 break;
301 case 2:
302 data_elem->data_size = net_buf_pull_be16(buf);
303 break;
304 case 4:
305 data_elem->data_size = net_buf_pull_be32(buf);
306 break;
307 default:
308 BT_WARN("Invalid size in remote request");
309 return BT_SDP_INVALID_SYNTAX;
310 }
311 break;
312 default:
313 BT_WARN("Invalid type in remote request");
314 return BT_SDP_INVALID_SYNTAX;
315 }
316
317 if (buf->len < data_elem->data_size) {
318 BT_WARN("Malformed packet");
319 return BT_SDP_INVALID_SYNTAX;
320 }
321
322 data_elem->total_size = data_elem->data_size + size_field_len + 1;
323 data_elem->data = buf->data;
324
325 return 0;
326 }
327
328 /* @brief Searches for an UUID within an attribute
329 *
330 * Searches for an UUID within an attribute. If the attribute has data element
331 * sequences, it recursively searches within them as well. On finding a match
332 * with the UUID, it sets the found flag.
333 *
334 * @param elem Attribute to be used as the search space (haystack)
335 * @param uuid UUID to be looked for (needle)
336 * @param found Flag set to true if the UUID is found (to be returned)
337 * @param nest_level Used to limit the extent of recursion into nested data
338 * elements, to avoid potential stack overflows
339 *
340 * @return Size of the last data element that has been searched
341 * (used in recursion)
342 */
search_uuid(struct bt_sdp_data_elem * elem,struct bt_uuid * uuid,bool * found,uint8_t nest_level)343 static uint32_t search_uuid(struct bt_sdp_data_elem *elem, struct bt_uuid *uuid,
344 bool *found, uint8_t nest_level)
345 {
346 const uint8_t *cur_elem;
347 uint32_t seq_size, size;
348 union {
349 struct bt_uuid uuid;
350 struct bt_uuid_16 u16;
351 struct bt_uuid_32 u32;
352 struct bt_uuid_128 u128;
353 } u;
354
355 if (*found) {
356 return 0;
357 }
358
359 /* Limit recursion depth to avoid stack overflows */
360 if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) {
361 return 0;
362 }
363
364 seq_size = elem->data_size;
365 cur_elem = elem->data;
366
367 if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) {
368 if (seq_size == 2U) {
369 u.uuid.type = BT_UUID_TYPE_16;
370 u.u16.val = *((uint16_t *)cur_elem);
371 if (!bt_uuid_cmp(&u.uuid, uuid)) {
372 *found = true;
373 }
374 } else if (seq_size == 4U) {
375 u.uuid.type = BT_UUID_TYPE_32;
376 u.u32.val = *((uint32_t *)cur_elem);
377 if (!bt_uuid_cmp(&u.uuid, uuid)) {
378 *found = true;
379 }
380 } else if (seq_size == 16U) {
381 u.uuid.type = BT_UUID_TYPE_128;
382 memcpy(u.u128.val, cur_elem, seq_size);
383 if (!bt_uuid_cmp(&u.uuid, uuid)) {
384 *found = true;
385 }
386 } else {
387 BT_WARN("Invalid UUID size in local database");
388 BT_ASSERT(0);
389 }
390 }
391
392 if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC ||
393 (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) {
394 do {
395 /* Recursively parse data elements */
396 size = search_uuid((struct bt_sdp_data_elem *)cur_elem,
397 uuid, found, nest_level + 1);
398 if (*found) {
399 return 0;
400 }
401 cur_elem += sizeof(struct bt_sdp_data_elem);
402 seq_size -= size;
403 } while (seq_size);
404 }
405
406 return elem->total_size;
407 }
408
409 /* @brief SDP service record iterator.
410 *
411 * Iterate over service records from a starting point.
412 *
413 * @param func Callback function.
414 * @param user_data Data to pass to the callback.
415 *
416 * @return Pointer to the record where the iterator stopped, or NULL if all
417 * records are covered
418 */
bt_sdp_foreach_svc(bt_sdp_svc_func_t func,void * user_data)419 static struct bt_sdp_record *bt_sdp_foreach_svc(bt_sdp_svc_func_t func,
420 void *user_data)
421 {
422 struct bt_sdp_record *rec = db;
423
424 while (rec) {
425 if (func(rec, user_data) == BT_SDP_ITER_STOP) {
426 break;
427 }
428
429 rec = rec->next;
430 }
431 return rec;
432 }
433
434 /* @brief Inserts a service record into a record pointer list
435 *
436 * Inserts a service record into a record pointer list
437 *
438 * @param rec The current service record.
439 * @param user_data Pointer to the destination record list.
440 *
441 * @return BT_SDP_ITER_CONTINUE to move on to the next record.
442 */
insert_record(struct bt_sdp_record * rec,void * user_data)443 static uint8_t insert_record(struct bt_sdp_record *rec, void *user_data)
444 {
445 struct bt_sdp_record **rec_list = user_data;
446
447 rec_list[rec->index] = rec;
448
449 return BT_SDP_ITER_CONTINUE;
450 }
451
452 /* @brief Looks for matching UUIDs in a list of service records
453 *
454 * Parses out a sequence of UUIDs from an input buffer, and checks if a record
455 * in the list contains all the UUIDs. If it doesn't, the record is removed
456 * from the list, so the list contains only the records which has all the
457 * input UUIDs in them.
458 *
459 * @param buf Incoming buffer containing all the UUIDs to be matched
460 * @param matching_recs List of service records to use for storing matching
461 * records
462 *
463 * @return 0 for success, or relevant error code
464 */
find_services(struct net_buf * buf,struct bt_sdp_record ** matching_recs)465 static uint16_t find_services(struct net_buf *buf,
466 struct bt_sdp_record **matching_recs)
467 {
468 struct bt_sdp_data_elem data_elem;
469 struct bt_sdp_record *record;
470 uint32_t uuid_list_size;
471 uint16_t res;
472 uint8_t att_idx, rec_idx = 0U;
473 bool found;
474 union {
475 struct bt_uuid uuid;
476 struct bt_uuid_16 u16;
477 struct bt_uuid_32 u32;
478 struct bt_uuid_128 u128;
479 } u;
480
481 res = parse_data_elem(buf, &data_elem);
482 if (res) {
483 return res;
484 }
485
486 if (((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_SEQ_UNSPEC) &&
487 ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_ALT_UNSPEC)) {
488 BT_WARN("Invalid type %x in service search pattern",
489 data_elem.type);
490 return BT_SDP_INVALID_SYNTAX;
491 }
492
493 uuid_list_size = data_elem.data_size;
494
495 bt_sdp_foreach_svc(insert_record, matching_recs);
496
497 /* Go over the sequence of UUIDs, and match one UUID at a time */
498 while (uuid_list_size) {
499 res = parse_data_elem(buf, &data_elem);
500 if (res) {
501 return res;
502 }
503
504 if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) !=
505 BT_SDP_UUID_UNSPEC) {
506 BT_WARN("Invalid type %u in service search pattern",
507 data_elem.type);
508 return BT_SDP_INVALID_SYNTAX;
509 }
510
511 if (buf->len < data_elem.data_size) {
512 BT_WARN("Malformed packet");
513 return BT_SDP_INVALID_SYNTAX;
514 }
515
516 if (data_elem.data_size == 2U) {
517 u.uuid.type = BT_UUID_TYPE_16;
518 u.u16.val = net_buf_pull_be16(buf);
519 } else if (data_elem.data_size == 4U) {
520 u.uuid.type = BT_UUID_TYPE_32;
521 u.u32.val = net_buf_pull_be32(buf);
522 } else if (data_elem.data_size == 16U) {
523 u.uuid.type = BT_UUID_TYPE_128;
524 sys_memcpy_swap(u.u128.val, buf->data,
525 data_elem.data_size);
526 net_buf_pull(buf, data_elem.data_size);
527 } else {
528 BT_WARN("Invalid UUID len %u in service search pattern",
529 data_elem.data_size);
530 net_buf_pull(buf, data_elem.data_size);
531 }
532
533 uuid_list_size -= data_elem.total_size;
534
535 /* Go over the list of services, and look for a service which
536 * doesn't have this UUID
537 */
538 for (rec_idx = 0U; rec_idx < num_services; rec_idx++) {
539 record = matching_recs[rec_idx];
540
541 if (!record) {
542 continue;
543 }
544
545 found = false;
546
547 /* Search for the UUID in all the attrs of the svc */
548 for (att_idx = 0U; att_idx < record->attr_count;
549 att_idx++) {
550 search_uuid(&record->attrs[att_idx].val,
551 &u.uuid, &found, 1);
552 if (found) {
553 break;
554 }
555 }
556
557 /* Remove the record from the list if it doesn't have
558 * the UUID
559 */
560 if (!found) {
561 matching_recs[rec_idx] = NULL;
562 }
563 }
564 }
565
566 return 0;
567 }
568
569 /* @brief Handler for Service Search Request
570 *
571 * Parses, processes and responds to a Service Search Request
572 *
573 * @param sdp Pointer to the SDP structure
574 * @param buf Request net buf
575 * @param tid Transaction ID
576 *
577 * @return 0 for success, or relevant error code
578 */
sdp_svc_search_req(struct bt_sdp * sdp,struct net_buf * buf,uint16_t tid)579 static uint16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf,
580 uint16_t tid)
581 {
582 struct bt_sdp_svc_rsp *rsp;
583 struct net_buf *resp_buf;
584 struct bt_sdp_record *record;
585 struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES];
586 uint16_t max_rec_count, total_recs = 0U, current_recs = 0U, res;
587 uint8_t cont_state_size, cont_state = 0U, idx = 0U, count = 0U;
588 bool pkt_full = false;
589
590 res = find_services(buf, matching_recs);
591 if (res) {
592 /* Error in parsing */
593 return res;
594 }
595
596 if (buf->len < 3) {
597 BT_WARN("Malformed packet");
598 return BT_SDP_INVALID_SYNTAX;
599 }
600
601 max_rec_count = net_buf_pull_be16(buf);
602 cont_state_size = net_buf_pull_u8(buf);
603
604 /* Zero out the matching services beyond max_rec_count */
605 for (idx = 0U; idx < num_services; idx++) {
606 if (count == max_rec_count) {
607 matching_recs[idx] = NULL;
608 continue;
609 }
610
611 if (matching_recs[idx]) {
612 count++;
613 }
614 }
615
616 /* We send out only SDP_SS_CONT_STATE_SIZE bytes continuation state in
617 * responses, so expect only SDP_SS_CONT_STATE_SIZE bytes in requests
618 */
619 if (cont_state_size) {
620 if (cont_state_size != SDP_SS_CONT_STATE_SIZE) {
621 BT_WARN("Invalid cont state size %u", cont_state_size);
622 return BT_SDP_INVALID_CSTATE;
623 }
624
625 if (buf->len < cont_state_size) {
626 BT_WARN("Malformed packet");
627 return BT_SDP_INVALID_SYNTAX;
628 }
629
630 cont_state = net_buf_pull_u8(buf);
631 /* We include total_recs in the continuation state. We calculate
632 * it once and preserve it across all the partial responses
633 */
634 total_recs = net_buf_pull_be16(buf);
635 }
636
637 BT_DBG("max_rec_count %u, cont_state %u", max_rec_count, cont_state);
638
639 resp_buf = bt_sdp_create_pdu();
640 rsp = net_buf_add(resp_buf, sizeof(*rsp));
641
642 for (; cont_state < num_services; cont_state++) {
643 record = matching_recs[cont_state];
644
645 if (!record) {
646 continue;
647 }
648
649 /* Calculate total recs only if it is first packet */
650 if (!cont_state_size) {
651 total_recs++;
652 }
653
654 if (pkt_full) {
655 continue;
656 }
657
658 /* 4 bytes per Service Record Handle */
659 /* 4 bytes for ContinuationState */
660 if ((MIN(SDP_MTU, sdp->chan.tx.mtu) - resp_buf->len) <
661 (4 + 4 + sizeof(struct bt_sdp_hdr))) {
662 pkt_full = true;
663 }
664
665 if (pkt_full) {
666 /* Packet exhausted: Add continuation state and break */
667 BT_DBG("Packet full, num_services_covered %u",
668 cont_state);
669 net_buf_add_u8(resp_buf, SDP_SS_CONT_STATE_SIZE);
670 net_buf_add_u8(resp_buf, cont_state);
671
672 /* If it is the first packet of a partial response,
673 * continue dry-running to calculate total_recs.
674 * Else break
675 */
676 if (cont_state_size) {
677 break;
678 }
679
680 continue;
681 }
682
683 /* Add the service record handle to the packet */
684 net_buf_add_be32(resp_buf, record->handle);
685 current_recs++;
686 }
687
688 /* Add 0 continuation state if packet is exhausted */
689 if (!pkt_full) {
690 net_buf_add_u8(resp_buf, 0);
691 } else {
692 net_buf_add_be16(resp_buf, total_recs);
693 }
694
695 rsp->total_recs = sys_cpu_to_be16(total_recs);
696 rsp->current_recs = sys_cpu_to_be16(current_recs);
697
698 BT_DBG("Sending response, len %u", resp_buf->len);
699 bt_sdp_send(&sdp->chan.chan, resp_buf, BT_SDP_SVC_SEARCH_RSP, tid);
700
701 return 0;
702 }
703
704 /* @brief Copies an attribute into an outgoing buffer
705 *
706 * Copies an attribute into a buffer. Recursively calls itself for complex
707 * attributes.
708 *
709 * @param elem Attribute to be copied to the buffer
710 * @param buf Buffer where the attribute is to be copied
711 *
712 * @return Size of the last data element that has been searched
713 * (used in recursion)
714 */
copy_attribute(struct bt_sdp_data_elem * elem,struct net_buf * buf,uint8_t nest_level)715 static uint32_t copy_attribute(struct bt_sdp_data_elem *elem,
716 struct net_buf *buf, uint8_t nest_level)
717 {
718 const uint8_t *cur_elem;
719 uint32_t size, seq_size, total_size;
720
721 /* Limit recursion depth to avoid stack overflows */
722 if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) {
723 return 0;
724 }
725
726 seq_size = elem->data_size;
727 total_size = elem->total_size;
728 cur_elem = elem->data;
729
730 /* Copy the header */
731 net_buf_add_u8(buf, elem->type);
732
733 switch (total_size - (seq_size + 1U)) {
734 case 1:
735 net_buf_add_u8(buf, elem->data_size);
736 break;
737 case 2:
738 net_buf_add_be16(buf, elem->data_size);
739 break;
740 case 4:
741 net_buf_add_be32(buf, elem->data_size);
742 break;
743 }
744
745 /* Recursively parse (till the last element is not another data element)
746 * and then fill the elements
747 */
748 if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC ||
749 (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) {
750 do {
751 size = copy_attribute((struct bt_sdp_data_elem *)
752 cur_elem, buf, nest_level + 1);
753 cur_elem += sizeof(struct bt_sdp_data_elem);
754 seq_size -= size;
755 } while (seq_size);
756 } else if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UINT8 ||
757 (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_INT8 ||
758 (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) {
759 if (seq_size == 1U) {
760 net_buf_add_u8(buf, *((uint8_t *)elem->data));
761 } else if (seq_size == 2U) {
762 net_buf_add_be16(buf, *((uint16_t *)elem->data));
763 } else if (seq_size == 4U) {
764 net_buf_add_be32(buf, *((uint32_t *)elem->data));
765 } else {
766 /* TODO: Convert 32bit and 128bit values to big-endian*/
767 net_buf_add_mem(buf, elem->data, seq_size);
768 }
769 } else {
770 net_buf_add_mem(buf, elem->data, seq_size);
771 }
772
773 return total_size;
774 }
775
776 /* @brief SDP attribute iterator.
777 *
778 * Iterate over attributes of a service record from a starting index.
779 *
780 * @param record Service record whose attributes are to be iterated over.
781 * @param idx Index in the attribute list from where to start.
782 * @param func Callback function.
783 * @param user_data Data to pass to the callback.
784 *
785 * @return Index of the attribute where the iterator stopped
786 */
bt_sdp_foreach_attr(struct bt_sdp_record * record,uint8_t idx,bt_sdp_attr_func_t func,void * user_data)787 static uint8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, uint8_t idx,
788 bt_sdp_attr_func_t func, void *user_data)
789 {
790 for (; idx < record->attr_count; idx++) {
791 if (func(&record->attrs[idx], idx, user_data) ==
792 BT_SDP_ITER_STOP) {
793 break;
794 }
795 }
796
797 return idx;
798 }
799
800 /* @brief Check if an attribute matches a range, and include it in the response
801 *
802 * Checks if an attribute matches a given attribute ID or range, and if so,
803 * includes it in the response packet
804 *
805 * @param attr The current attribute
806 * @param att_idx Index of the current attribute in the database
807 * @param user_data Pointer to the structure containing response packet, byte
808 * count, states, etc
809 *
810 * @return BT_SDP_ITER_CONTINUE if should continue to the next attribute
811 * or BT_SDP_ITER_STOP to stop.
812 */
select_attrs(struct bt_sdp_attribute * attr,uint8_t att_idx,void * user_data)813 static uint8_t select_attrs(struct bt_sdp_attribute *attr, uint8_t att_idx,
814 void *user_data)
815 {
816 struct select_attrs_data *sad = user_data;
817 uint16_t att_id_lower, att_id_upper, att_id_cur, space;
818 uint32_t attr_size, seq_size;
819 uint8_t idx_filter;
820
821 for (idx_filter = 0U; idx_filter < sad->num_filters; idx_filter++) {
822
823 att_id_lower = (sad->filter[idx_filter] >> 16);
824 att_id_upper = (sad->filter[idx_filter]);
825 att_id_cur = attr->id;
826
827 /* Check for range values */
828 if (att_id_lower != 0xffff &&
829 (!IN_RANGE(att_id_cur, att_id_lower, att_id_upper))) {
830 continue;
831 }
832
833 /* Check for match values */
834 if (att_id_lower == 0xffff && att_id_cur != att_id_upper) {
835 continue;
836 }
837
838 /* Attribute ID matches */
839
840 /* 3 bytes for Attribute ID */
841 attr_size = 3 + attr->val.total_size;
842
843 /* If this is the first attribute of the service, then we need
844 * to account for the space required to add the per-service
845 * data element sequence header as well.
846 */
847 if ((sad->state->current_svc != sad->rec->index) &&
848 sad->new_service) {
849 /* 3 bytes for Per-Service Data Elem Seq declaration */
850 seq_size = attr_size + 3;
851 } else {
852 seq_size = attr_size;
853 }
854
855 if (sad->rsp_buf) {
856 space = MIN(SDP_MTU, sad->sdp->chan.tx.mtu) -
857 sad->rsp_buf->len - sizeof(struct bt_sdp_hdr);
858
859 if ((!sad->state->pkt_full) &&
860 ((seq_size > sad->max_att_len) ||
861 (space < seq_size + sad->cont_state_size))) {
862 /* Packet exhausted */
863 sad->state->pkt_full = true;
864 }
865 }
866
867 /* Keep filling data only if packet is not exhausted */
868 if (!sad->state->pkt_full && sad->rsp_buf) {
869 /* Add Per-Service Data Element Seq declaration once
870 * only when we are starting from the first attribute
871 */
872 if (!sad->seq &&
873 (sad->state->current_svc != sad->rec->index)) {
874 sad->seq = net_buf_add(sad->rsp_buf,
875 sizeof(*sad->seq));
876 sad->seq->type = BT_SDP_SEQ16;
877 sad->seq->size = 0U;
878 }
879
880 /* Add attribute ID */
881 net_buf_add_u8(sad->rsp_buf, BT_SDP_UINT16);
882 net_buf_add_be16(sad->rsp_buf, att_id_cur);
883
884 /* Add attribute value */
885 copy_attribute(&attr->val, sad->rsp_buf, 1);
886
887 sad->max_att_len -= seq_size;
888 sad->att_list_len += seq_size;
889 sad->state->last_att = att_idx;
890 sad->state->current_svc = sad->rec->index;
891 }
892
893 if (sad->seq) {
894 /* Keep adding the sequence size if this packet contains
895 * the Per-Service Data Element Seq declaration header
896 */
897 sad->seq->size += attr_size;
898 sad->state->att_list_size += seq_size;
899 } else {
900 /* Keep adding the total attr lists size if:
901 * It's a dry-run, calculating the total attr lists size
902 */
903 sad->state->att_list_size += seq_size;
904 }
905
906 sad->new_service = false;
907 break;
908 }
909
910 /* End the search if:
911 * 1. We have exhausted the packet
912 * AND
913 * 2. This packet doesn't contain the service element declaration header
914 * AND
915 * 3. This is not a dry-run (then we look for other attrs that match)
916 */
917 if (sad->state->pkt_full && !sad->seq && sad->rsp_buf) {
918 return BT_SDP_ITER_STOP;
919 }
920
921 return BT_SDP_ITER_CONTINUE;
922 }
923
924 /* @brief Creates attribute list in the given buffer
925 *
926 * Populates the attribute list of a service record in the buffer. To be used
927 * for responding to Service Attribute and Service Search Attribute requests
928 *
929 * @param sdp Pointer to the SDP structure
930 * @param record Service record whose attributes are to be included in the
931 * response
932 * @param filter Attribute values/ranges to be used as a filter
933 * @param num_filters Number of elements in the attribute filter
934 * @param max_att_len Maximum size of attributes to be included in the response
935 * @param cont_state_size No. of additional continuation state bytes to keep
936 * space for in the packet. This will vary based on the type of the request
937 * @param next_att Starting position of the search in the service's attr list
938 * @param state State of the overall search
939 * @param rsp_buf Response buffer which is filled in
940 *
941 * @return len Length of the attribute list created
942 */
create_attr_list(struct bt_sdp * sdp,struct bt_sdp_record * record,uint32_t * filter,uint8_t num_filters,uint16_t max_att_len,uint8_t cont_state_size,uint8_t next_att,struct search_state * state,struct net_buf * rsp_buf)943 static uint16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *record,
944 uint32_t *filter, uint8_t num_filters,
945 uint16_t max_att_len, uint8_t cont_state_size,
946 uint8_t next_att, struct search_state *state,
947 struct net_buf *rsp_buf)
948 {
949 struct select_attrs_data sad;
950 uint8_t idx_att;
951
952 sad.num_filters = num_filters;
953 sad.rec = record;
954 sad.rsp_buf = rsp_buf;
955 sad.sdp = sdp;
956 sad.max_att_len = max_att_len;
957 sad.cont_state_size = cont_state_size;
958 sad.seq = NULL;
959 sad.filter = filter;
960 sad.state = state;
961 sad.att_list_len = 0U;
962 sad.new_service = true;
963
964 idx_att = bt_sdp_foreach_attr(sad.rec, next_att, select_attrs, &sad);
965
966 if (sad.seq) {
967 sad.seq->size = sys_cpu_to_be16(sad.seq->size);
968 }
969
970 return sad.att_list_len;
971 }
972
973 /* @brief Extracts the attribute search list from a buffer
974 *
975 * Parses a buffer to extract the attribute search list (list of attribute IDs
976 * and ranges) which are to be used to filter attributes.
977 *
978 * @param buf Buffer to be parsed for extracting the attribute search list
979 * @param filter Empty list of 4byte filters that are filled in. For attribute
980 * IDs, the lower 2 bytes contain the ID and the upper 2 bytes are set to
981 * 0xFFFF. For attribute ranges, the lower 2bytes indicate the start ID and
982 * the upper 2bytes indicate the end ID
983 * @param num_filters No. of filter elements filled in (to be returned)
984 *
985 * @return 0 for success, or relevant error code
986 */
get_att_search_list(struct net_buf * buf,uint32_t * filter,uint8_t * num_filters)987 static uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter,
988 uint8_t *num_filters)
989 {
990 struct bt_sdp_data_elem data_elem;
991 uint16_t res;
992 uint32_t size;
993
994 *num_filters = 0U;
995 res = parse_data_elem(buf, &data_elem);
996 if (res) {
997 return res;
998 }
999
1000 size = data_elem.data_size;
1001
1002 while (size) {
1003 res = parse_data_elem(buf, &data_elem);
1004 if (res) {
1005 return res;
1006 }
1007
1008 if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UINT8) {
1009 BT_WARN("Invalid type %u in attribute ID list",
1010 data_elem.type);
1011 return BT_SDP_INVALID_SYNTAX;
1012 }
1013
1014 if (buf->len < data_elem.data_size) {
1015 BT_WARN("Malformed packet");
1016 return BT_SDP_INVALID_SYNTAX;
1017 }
1018
1019 /* This is an attribute ID */
1020 if (data_elem.data_size == 2U) {
1021 filter[(*num_filters)++] = 0xffff0000 |
1022 net_buf_pull_be16(buf);
1023 }
1024
1025 /* This is an attribute ID range */
1026 if (data_elem.data_size == 4U) {
1027 filter[(*num_filters)++] = net_buf_pull_be32(buf);
1028 }
1029
1030 size -= data_elem.total_size;
1031 }
1032
1033 return 0;
1034 }
1035
1036 /* @brief Check if a given handle matches that of the current service
1037 *
1038 * Checks if a given handle matches that of the current service
1039 *
1040 * @param rec The current service record
1041 * @param user_data Pointer to the service record handle to be matched
1042 *
1043 * @return BT_SDP_ITER_CONTINUE if should continue to the next record
1044 * or BT_SDP_ITER_STOP to stop.
1045 */
find_handle(struct bt_sdp_record * rec,void * user_data)1046 static uint8_t find_handle(struct bt_sdp_record *rec, void *user_data)
1047 {
1048 uint32_t *svc_rec_hdl = user_data;
1049
1050 if (rec->handle == *svc_rec_hdl) {
1051 return BT_SDP_ITER_STOP;
1052 }
1053
1054 return BT_SDP_ITER_CONTINUE;
1055 }
1056
1057 /* @brief Handler for Service Attribute Request
1058 *
1059 * Parses, processes and responds to a Service Attribute Request
1060 *
1061 * @param sdp Pointer to the SDP structure
1062 * @param buf Request buffer
1063 * @param tid Transaction ID
1064 *
1065 * @return 0 for success, or relevant error code
1066 */
sdp_svc_att_req(struct bt_sdp * sdp,struct net_buf * buf,uint16_t tid)1067 static uint16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf,
1068 uint16_t tid)
1069 {
1070 uint32_t filter[MAX_NUM_ATT_ID_FILTER];
1071 struct search_state state = {
1072 .current_svc = SDP_INVALID,
1073 .last_att = SDP_INVALID,
1074 .pkt_full = false
1075 };
1076 struct bt_sdp_record *record;
1077 struct bt_sdp_att_rsp *rsp;
1078 struct net_buf *rsp_buf;
1079 uint32_t svc_rec_hdl;
1080 uint16_t max_att_len, res, att_list_len;
1081 uint8_t num_filters, cont_state_size, next_att = 0U;
1082
1083 if (buf->len < 6) {
1084 BT_WARN("Malformed packet");
1085 return BT_SDP_INVALID_SYNTAX;
1086 }
1087
1088 svc_rec_hdl = net_buf_pull_be32(buf);
1089 max_att_len = net_buf_pull_be16(buf);
1090
1091 /* Set up the filters */
1092 res = get_att_search_list(buf, filter, &num_filters);
1093 if (res) {
1094 /* Error in parsing */
1095 return res;
1096 }
1097
1098 if (buf->len < 1) {
1099 BT_WARN("Malformed packet");
1100 return BT_SDP_INVALID_SYNTAX;
1101 }
1102
1103 cont_state_size = net_buf_pull_u8(buf);
1104
1105 /* We only send out 1 byte continuation state in responses,
1106 * so expect only 1 byte in requests
1107 */
1108 if (cont_state_size) {
1109 if (cont_state_size != SDP_SA_CONT_STATE_SIZE) {
1110 BT_WARN("Invalid cont state size %u", cont_state_size);
1111 return BT_SDP_INVALID_CSTATE;
1112 }
1113
1114 if (buf->len < cont_state_size) {
1115 BT_WARN("Malformed packet");
1116 return BT_SDP_INVALID_SYNTAX;
1117 }
1118
1119 state.last_att = net_buf_pull_u8(buf) + 1;
1120 next_att = state.last_att;
1121 }
1122
1123 BT_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u", svc_rec_hdl,
1124 max_att_len, next_att);
1125
1126 /* Find the service */
1127 record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl);
1128
1129 if (!record) {
1130 BT_WARN("Handle %u not found", svc_rec_hdl);
1131 return BT_SDP_INVALID_RECORD_HANDLE;
1132 }
1133
1134 /* For partial responses, restore the search state */
1135 if (cont_state_size) {
1136 state.current_svc = record->index;
1137 }
1138
1139 rsp_buf = bt_sdp_create_pdu();
1140 rsp = net_buf_add(rsp_buf, sizeof(*rsp));
1141
1142 /* cont_state_size should include 1 byte header */
1143 att_list_len = create_attr_list(sdp, record, filter, num_filters,
1144 max_att_len, SDP_SA_CONT_STATE_SIZE + 1,
1145 next_att, &state, rsp_buf);
1146
1147 if (!att_list_len) {
1148 /* For empty responses, add an empty data element sequence */
1149 net_buf_add_u8(rsp_buf, BT_SDP_SEQ8);
1150 net_buf_add_u8(rsp_buf, 0);
1151 att_list_len = 2U;
1152 }
1153
1154 /* Add continuation state */
1155 if (state.pkt_full) {
1156 BT_DBG("Packet full, state.last_att %u", state.last_att);
1157 net_buf_add_u8(rsp_buf, 1);
1158 net_buf_add_u8(rsp_buf, state.last_att);
1159 } else {
1160 net_buf_add_u8(rsp_buf, 0);
1161 }
1162
1163 rsp->att_list_len = sys_cpu_to_be16(att_list_len);
1164
1165 BT_DBG("Sending response, len %u", rsp_buf->len);
1166 bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_ATTR_RSP, tid);
1167
1168 return 0;
1169 }
1170
1171 /* @brief Handler for Service Search Attribute Request
1172 *
1173 * Parses, processes and responds to a Service Search Attribute Request
1174 *
1175 * @param sdp Pointer to the SDP structure
1176 * @param buf Request buffer
1177 * @param tid Transaction ID
1178 *
1179 * @return 0 for success, or relevant error code
1180 */
sdp_svc_search_att_req(struct bt_sdp * sdp,struct net_buf * buf,uint16_t tid)1181 static uint16_t sdp_svc_search_att_req(struct bt_sdp *sdp, struct net_buf *buf,
1182 uint16_t tid)
1183 {
1184 uint32_t filter[MAX_NUM_ATT_ID_FILTER];
1185 struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES];
1186 struct search_state state = {
1187 .att_list_size = 0,
1188 .current_svc = SDP_INVALID,
1189 .last_att = SDP_INVALID,
1190 .pkt_full = false
1191 };
1192 struct net_buf *rsp_buf, *rsp_buf_cpy;
1193 struct bt_sdp_record *record;
1194 struct bt_sdp_att_rsp *rsp;
1195 struct bt_sdp_data_elem_seq *seq = NULL;
1196 uint16_t max_att_len, res, att_list_len = 0U;
1197 uint8_t num_filters, cont_state_size, next_svc = 0U, next_att = 0U;
1198 bool dry_run = false;
1199
1200 res = find_services(buf, matching_recs);
1201 if (res) {
1202 return res;
1203 }
1204
1205 if (buf->len < 2) {
1206 BT_WARN("Malformed packet");
1207 return BT_SDP_INVALID_SYNTAX;
1208 }
1209
1210 max_att_len = net_buf_pull_be16(buf);
1211
1212 /* Set up the filters */
1213 res = get_att_search_list(buf, filter, &num_filters);
1214
1215 if (res) {
1216 /* Error in parsing */
1217 return res;
1218 }
1219
1220 if (buf->len < 1) {
1221 BT_WARN("Malformed packet");
1222 return BT_SDP_INVALID_SYNTAX;
1223 }
1224
1225 cont_state_size = net_buf_pull_u8(buf);
1226
1227 /* We only send out 2 bytes continuation state in responses,
1228 * so expect only 2 bytes in requests
1229 */
1230 if (cont_state_size) {
1231 if (cont_state_size != SDP_SSA_CONT_STATE_SIZE) {
1232 BT_WARN("Invalid cont state size %u", cont_state_size);
1233 return BT_SDP_INVALID_CSTATE;
1234 }
1235
1236 if (buf->len < cont_state_size) {
1237 BT_WARN("Malformed packet");
1238 return BT_SDP_INVALID_SYNTAX;
1239 }
1240
1241 state.current_svc = net_buf_pull_u8(buf);
1242 state.last_att = net_buf_pull_u8(buf) + 1;
1243 next_svc = state.current_svc;
1244 next_att = state.last_att;
1245 }
1246
1247 BT_DBG("max_att_len 0x%04x, state.current_svc %u, state.last_att %u",
1248 max_att_len, state.current_svc, state.last_att);
1249
1250 rsp_buf = bt_sdp_create_pdu();
1251
1252 rsp = net_buf_add(rsp_buf, sizeof(*rsp));
1253
1254 /* Add headers only if this is not a partial response */
1255 if (!cont_state_size) {
1256 seq = net_buf_add(rsp_buf, sizeof(*seq));
1257 seq->type = BT_SDP_SEQ16;
1258 seq->size = 0U;
1259
1260 /* 3 bytes for Outer Data Element Sequence declaration */
1261 att_list_len = 3U;
1262 }
1263
1264 rsp_buf_cpy = rsp_buf;
1265
1266 for (; next_svc < num_services; next_svc++) {
1267 record = matching_recs[next_svc];
1268
1269 if (!record) {
1270 continue;
1271 }
1272
1273 att_list_len += create_attr_list(sdp, record, filter,
1274 num_filters, max_att_len,
1275 SDP_SSA_CONT_STATE_SIZE + 1,
1276 next_att, &state, rsp_buf_cpy);
1277
1278 /* Check if packet is full and not dry run */
1279 if (state.pkt_full && !dry_run) {
1280 BT_DBG("Packet full, state.last_att %u",
1281 state.last_att);
1282 dry_run = true;
1283
1284 /* Add continuation state */
1285 net_buf_add_u8(rsp_buf, 2);
1286 net_buf_add_u8(rsp_buf, state.current_svc);
1287 net_buf_add_u8(rsp_buf, state.last_att);
1288
1289 /* Break if it's not a partial response, else dry-run
1290 * Dry run: Look for other services that match
1291 */
1292 if (cont_state_size) {
1293 break;
1294 }
1295
1296 rsp_buf_cpy = NULL;
1297 }
1298
1299 next_att = 0U;
1300 }
1301
1302 if (!dry_run) {
1303 if (!att_list_len) {
1304 /* For empty responses, add an empty data elem seq */
1305 net_buf_add_u8(rsp_buf, BT_SDP_SEQ8);
1306 net_buf_add_u8(rsp_buf, 0);
1307 att_list_len = 2U;
1308 }
1309 /* Search exhausted */
1310 net_buf_add_u8(rsp_buf, 0);
1311 }
1312
1313 rsp->att_list_len = sys_cpu_to_be16(att_list_len);
1314 if (seq) {
1315 seq->size = sys_cpu_to_be16(state.att_list_size);
1316 }
1317
1318 BT_DBG("Sending response, len %u", rsp_buf->len);
1319 bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_SEARCH_ATTR_RSP,
1320 tid);
1321
1322 return 0;
1323 }
1324
1325 static const struct {
1326 uint8_t op_code;
1327 uint16_t (*func)(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid);
1328 } handlers[] = {
1329 { BT_SDP_SVC_SEARCH_REQ, sdp_svc_search_req },
1330 { BT_SDP_SVC_ATTR_REQ, sdp_svc_att_req },
1331 { BT_SDP_SVC_SEARCH_ATTR_REQ, sdp_svc_search_att_req },
1332 };
1333
1334 /* @brief Callback for SDP data receive
1335 *
1336 * Gets called when an SDP PDU is received. Calls the corresponding handler
1337 * based on the op code of the PDU.
1338 *
1339 * @param chan L2CAP channel
1340 * @param buf Received PDU
1341 *
1342 * @return None
1343 */
bt_sdp_recv(struct bt_l2cap_chan * chan,struct net_buf * buf)1344 static int bt_sdp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
1345 {
1346 struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan,
1347 struct bt_l2cap_br_chan, chan);
1348 struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan);
1349 struct bt_sdp_hdr *hdr;
1350 uint16_t err = BT_SDP_INVALID_SYNTAX;
1351 size_t i;
1352
1353 BT_DBG("chan %p, ch %p, cid 0x%04x", chan, ch, ch->tx.cid);
1354
1355 BT_ASSERT(sdp);
1356
1357 if (buf->len < sizeof(*hdr)) {
1358 BT_ERR("Too small SDP PDU received");
1359 return 0;
1360 }
1361
1362 hdr = net_buf_pull_mem(buf, sizeof(*hdr));
1363 BT_DBG("Received SDP code 0x%02x len %u", hdr->op_code, buf->len);
1364
1365 if (sys_cpu_to_be16(hdr->param_len) != buf->len) {
1366 err = BT_SDP_INVALID_PDU_SIZE;
1367 } else {
1368 for (i = 0; i < ARRAY_SIZE(handlers); i++) {
1369 if (hdr->op_code != handlers[i].op_code) {
1370 continue;
1371 }
1372
1373 err = handlers[i].func(sdp, buf, hdr->tid);
1374 break;
1375 }
1376 }
1377
1378 if (err) {
1379 BT_WARN("SDP error 0x%02x", err);
1380 send_err_rsp(chan, err, hdr->tid);
1381 }
1382
1383 return 0;
1384 }
1385
1386 /* @brief Callback for SDP connection accept
1387 *
1388 * Gets called when an incoming SDP connection needs to be authorized.
1389 * Registers the L2CAP callbacks and allocates an SDP context to the connection
1390 *
1391 * @param conn BT connection object
1392 * @param chan L2CAP channel structure (to be returned)
1393 *
1394 * @return 0 for success, or relevant error code
1395 */
bt_sdp_accept(struct bt_conn * conn,struct bt_l2cap_chan ** chan)1396 static int bt_sdp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan)
1397 {
1398 static const struct bt_l2cap_chan_ops ops = {
1399 .connected = bt_sdp_connected,
1400 .disconnected = bt_sdp_disconnected,
1401 .recv = bt_sdp_recv,
1402 };
1403 int i;
1404
1405 BT_DBG("conn %p", conn);
1406
1407 for (i = 0; i < ARRAY_SIZE(bt_sdp_pool); i++) {
1408 struct bt_sdp *sdp = &bt_sdp_pool[i];
1409
1410 if (sdp->chan.chan.conn) {
1411 continue;
1412 }
1413
1414 sdp->chan.chan.ops = &ops;
1415 sdp->chan.rx.mtu = SDP_MTU;
1416
1417 *chan = &sdp->chan.chan;
1418
1419 return 0;
1420 }
1421
1422 BT_ERR("No available SDP context for conn %p", conn);
1423
1424 return -ENOMEM;
1425 }
1426
bt_sdp_init(void)1427 void bt_sdp_init(void)
1428 {
1429 static struct bt_l2cap_server server = {
1430 .psm = SDP_PSM,
1431 .accept = bt_sdp_accept,
1432 .sec_level = BT_SECURITY_L0,
1433 };
1434 int res;
1435
1436 res = bt_l2cap_br_server_register(&server);
1437 if (res) {
1438 BT_ERR("L2CAP server registration failed with error %d", res);
1439 }
1440 }
1441
bt_sdp_register_service(struct bt_sdp_record * service)1442 int bt_sdp_register_service(struct bt_sdp_record *service)
1443 {
1444 uint32_t handle = SDP_SERVICE_HANDLE_BASE;
1445
1446 if (!service) {
1447 BT_ERR("No service record specified");
1448 return 0;
1449 }
1450
1451 if (num_services == BT_SDP_MAX_SERVICES) {
1452 BT_ERR("Reached max allowed registrations");
1453 return -ENOMEM;
1454 }
1455
1456 if (db) {
1457 handle = db->handle + 1;
1458 }
1459
1460 service->next = db;
1461 service->index = num_services++;
1462 service->handle = handle;
1463 *((uint32_t *)(service->attrs[0].val.data)) = handle;
1464 db = service;
1465
1466 BT_DBG("Service registered at %u", handle);
1467
1468 return 0;
1469 }
1470
1471 #define GET_PARAM(__node) \
1472 CONTAINER_OF(__node, struct bt_sdp_discover_params, _node)
1473
1474 /* ServiceSearchAttribute PDU, ref to BT Core 4.2, Vol 3, part B, 4.7.1 */
sdp_client_ssa_search(struct bt_sdp_client * session)1475 static int sdp_client_ssa_search(struct bt_sdp_client *session)
1476 {
1477 const struct bt_sdp_discover_params *param;
1478 struct net_buf *buf;
1479
1480 /*
1481 * Select proper user params, if session->param is invalid it means
1482 * getting new UUID from top of to be resolved params list. Otherwise
1483 * the context is in a middle of partial SDP PDU responses and cached
1484 * value from context can be used.
1485 */
1486 if (!session->param) {
1487 param = GET_PARAM(sys_slist_peek_head(&session->reqs));
1488 } else {
1489 param = session->param;
1490 }
1491
1492 if (!param) {
1493 BT_WARN("No UUIDs to be resolved on remote");
1494 return -EINVAL;
1495 }
1496
1497 buf = bt_sdp_create_pdu();
1498
1499 /* BT_SDP_SEQ8 means length of sequence is on additional next byte */
1500 net_buf_add_u8(buf, BT_SDP_SEQ8);
1501
1502 switch (param->uuid->type) {
1503 case BT_UUID_TYPE_16:
1504 /* Seq length */
1505 net_buf_add_u8(buf, 0x03);
1506 /* Seq type */
1507 net_buf_add_u8(buf, BT_SDP_UUID16);
1508 /* Seq value */
1509 net_buf_add_be16(buf, BT_UUID_16(param->uuid)->val);
1510 break;
1511 case BT_UUID_TYPE_32:
1512 net_buf_add_u8(buf, 0x05);
1513 net_buf_add_u8(buf, BT_SDP_UUID32);
1514 net_buf_add_be32(buf, BT_UUID_32(param->uuid)->val);
1515 break;
1516 case BT_UUID_TYPE_128:
1517 net_buf_add_u8(buf, 0x11);
1518 net_buf_add_u8(buf, BT_SDP_UUID128);
1519 net_buf_add_mem(buf, BT_UUID_128(param->uuid)->val,
1520 ARRAY_SIZE(BT_UUID_128(param->uuid)->val));
1521 break;
1522 default:
1523 BT_ERR("Unknown UUID type %u", param->uuid->type);
1524 return -EINVAL;
1525 }
1526
1527 /* Set attribute max bytes count to be returned from server */
1528 net_buf_add_be16(buf, BT_SDP_MAX_ATTR_LEN);
1529 /*
1530 * Sequence definition where data is sequence of elements and where
1531 * additional next byte points the size of elements within
1532 */
1533 net_buf_add_u8(buf, BT_SDP_SEQ8);
1534 net_buf_add_u8(buf, 0x05);
1535 /* Data element definition for two following 16bits range elements */
1536 net_buf_add_u8(buf, BT_SDP_UINT32);
1537 /* Get all attributes. It enables filter out wanted only attributes */
1538 net_buf_add_be16(buf, 0x0000);
1539 net_buf_add_be16(buf, 0xffff);
1540
1541 /*
1542 * Update and validate PDU ContinuationState. Initial SSA Request has
1543 * zero length continuation state since no interaction has place with
1544 * server so far, otherwise use the original state taken from remote's
1545 * last response PDU that is cached by SDP client context.
1546 */
1547 if (session->cstate.length == 0U) {
1548 net_buf_add_u8(buf, 0x00);
1549 } else {
1550 net_buf_add_u8(buf, session->cstate.length);
1551 net_buf_add_mem(buf, session->cstate.data,
1552 session->cstate.length);
1553 }
1554
1555 /* Update context param to the one being resolving now */
1556 session->param = param;
1557 session->tid++;
1558
1559 return bt_sdp_send(&session->chan.chan, buf, BT_SDP_SVC_SEARCH_ATTR_REQ,
1560 session->tid);
1561 }
1562
sdp_client_params_iterator(struct bt_sdp_client * session)1563 static void sdp_client_params_iterator(struct bt_sdp_client *session)
1564 {
1565 struct bt_l2cap_chan *chan = &session->chan.chan;
1566 struct bt_sdp_discover_params *param, *tmp;
1567
1568 SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) {
1569 if (param != session->param) {
1570 continue;
1571 }
1572
1573 BT_DBG("");
1574
1575 /* Remove already checked UUID node */
1576 sys_slist_remove(&session->reqs, NULL, ¶m->_node);
1577 /* Invalidate cached param in context */
1578 session->param = NULL;
1579 /* Reset continuation state in current context */
1580 (void)memset(&session->cstate, 0, sizeof(session->cstate));
1581
1582 /* Check if there's valid next UUID */
1583 if (!sys_slist_is_empty(&session->reqs)) {
1584 sdp_client_ssa_search(session);
1585 return;
1586 }
1587
1588 /* No UUID items, disconnect channel */
1589 bt_l2cap_chan_disconnect(chan);
1590 break;
1591 }
1592 }
1593
sdp_client_get_total(struct bt_sdp_client * session,struct net_buf * buf,uint16_t * total)1594 static uint16_t sdp_client_get_total(struct bt_sdp_client *session,
1595 struct net_buf *buf, uint16_t *total)
1596 {
1597 uint16_t pulled;
1598 uint8_t seq;
1599
1600 /*
1601 * Pull value of total octets of all attributes available to be
1602 * collected when response gets completed for given UUID. Such info can
1603 * be get from the very first response frame after initial SSA request
1604 * was sent. For subsequent calls related to the same SSA request input
1605 * buf and in/out function parameters stays neutral.
1606 */
1607 if (session->cstate.length == 0U) {
1608 seq = net_buf_pull_u8(buf);
1609 pulled = 1U;
1610 switch (seq) {
1611 case BT_SDP_SEQ8:
1612 *total = net_buf_pull_u8(buf);
1613 pulled += 1U;
1614 break;
1615 case BT_SDP_SEQ16:
1616 *total = net_buf_pull_be16(buf);
1617 pulled += 2U;
1618 break;
1619 default:
1620 BT_WARN("Sequence type 0x%02x not handled", seq);
1621 *total = 0U;
1622 break;
1623 }
1624
1625 BT_DBG("Total %u octets of all attributes", *total);
1626 } else {
1627 pulled = 0U;
1628 *total = 0U;
1629 }
1630
1631 return pulled;
1632 }
1633
get_record_len(struct net_buf * buf)1634 static uint16_t get_record_len(struct net_buf *buf)
1635 {
1636 uint16_t len;
1637 uint8_t seq;
1638
1639 seq = net_buf_pull_u8(buf);
1640
1641 switch (seq) {
1642 case BT_SDP_SEQ8:
1643 len = net_buf_pull_u8(buf);
1644 break;
1645 case BT_SDP_SEQ16:
1646 len = net_buf_pull_be16(buf);
1647 break;
1648 default:
1649 BT_WARN("Sequence type 0x%02x not handled", seq);
1650 len = 0U;
1651 break;
1652 }
1653
1654 BT_DBG("Record len %u", len);
1655
1656 return len;
1657 }
1658
1659 enum uuid_state {
1660 UUID_NOT_RESOLVED,
1661 UUID_RESOLVED,
1662 };
1663
sdp_client_notify_result(struct bt_sdp_client * session,enum uuid_state state)1664 static void sdp_client_notify_result(struct bt_sdp_client *session,
1665 enum uuid_state state)
1666 {
1667 struct bt_conn *conn = session->chan.chan.conn;
1668 struct bt_sdp_client_result result;
1669 uint16_t rec_len;
1670 uint8_t user_ret;
1671
1672 result.uuid = session->param->uuid;
1673
1674 if (state == UUID_NOT_RESOLVED) {
1675 result.resp_buf = NULL;
1676 result.next_record_hint = false;
1677 session->param->func(conn, &result);
1678 return;
1679 }
1680
1681 while (session->rec_buf->len) {
1682 struct net_buf_simple_state buf_state;
1683
1684 rec_len = get_record_len(session->rec_buf);
1685 /* tell the user about multi record resolution */
1686 if (session->rec_buf->len > rec_len) {
1687 result.next_record_hint = true;
1688 } else {
1689 result.next_record_hint = false;
1690 }
1691
1692 /* save the original session buffer */
1693 net_buf_simple_save(&session->rec_buf->b, &buf_state);
1694 /* initialize internal result buffer instead of memcpy */
1695 result.resp_buf = session->rec_buf;
1696 /*
1697 * Set user internal result buffer length as same as record
1698 * length to fake user. User will see the individual record
1699 * length as rec_len insted of whole session rec_buf length.
1700 */
1701 result.resp_buf->len = rec_len;
1702
1703 user_ret = session->param->func(conn, &result);
1704
1705 /* restore original session buffer */
1706 net_buf_simple_restore(&session->rec_buf->b, &buf_state);
1707 /*
1708 * sync session buffer data length with next record chunk not
1709 * send to user so far
1710 */
1711 net_buf_pull(session->rec_buf, rec_len);
1712 if (user_ret == BT_SDP_DISCOVER_UUID_STOP) {
1713 break;
1714 }
1715 }
1716 }
1717
sdp_client_receive(struct bt_l2cap_chan * chan,struct net_buf * buf)1718 static int sdp_client_receive(struct bt_l2cap_chan *chan, struct net_buf *buf)
1719 {
1720 struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
1721 struct bt_sdp_hdr *hdr;
1722 struct bt_sdp_pdu_cstate *cstate;
1723 uint16_t len, tid, frame_len;
1724 uint16_t total;
1725
1726 BT_DBG("session %p buf %p", session, buf);
1727
1728 if (buf->len < sizeof(*hdr)) {
1729 BT_ERR("Too small SDP PDU");
1730 return 0;
1731 }
1732
1733 hdr = net_buf_pull_mem(buf, sizeof(*hdr));
1734 if (hdr->op_code == BT_SDP_ERROR_RSP) {
1735 BT_INFO("Error SDP PDU response");
1736 return 0;
1737 }
1738
1739 len = sys_be16_to_cpu(hdr->param_len);
1740 tid = sys_be16_to_cpu(hdr->tid);
1741
1742 BT_DBG("SDP PDU tid %u len %u", tid, len);
1743
1744 if (buf->len != len) {
1745 BT_ERR("SDP PDU length mismatch (%u != %u)", buf->len, len);
1746 return 0;
1747 }
1748
1749 if (tid != session->tid) {
1750 BT_ERR("Mismatch transaction ID value in SDP PDU");
1751 return 0;
1752 }
1753
1754 switch (hdr->op_code) {
1755 case BT_SDP_SVC_SEARCH_ATTR_RSP:
1756 /* Get number of attributes in this frame. */
1757 frame_len = net_buf_pull_be16(buf);
1758 /* Check valid buf len for attribute list and cont state */
1759 if (buf->len < frame_len + SDP_CONT_STATE_LEN_SIZE) {
1760 BT_ERR("Invalid frame payload length");
1761 return 0;
1762 }
1763 /* Check valid range of attributes length */
1764 if (frame_len < 2) {
1765 BT_ERR("Invalid attributes data length");
1766 return 0;
1767 }
1768
1769 /* Get PDU continuation state */
1770 cstate = (struct bt_sdp_pdu_cstate *)(buf->data + frame_len);
1771
1772 if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) {
1773 BT_ERR("Invalid SDP PDU Continuation State length %u",
1774 cstate->length);
1775 return 0;
1776 }
1777
1778 if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) >
1779 buf->len) {
1780 BT_ERR("Invalid frame payload length");
1781 return 0;
1782 }
1783
1784 /*
1785 * No record found for given UUID. The check catches case when
1786 * current response frame has Continuation State shortest and
1787 * valid and this is the first response frame as well.
1788 */
1789 if (frame_len == 2U && cstate->length == 0U &&
1790 session->cstate.length == 0U) {
1791 BT_DBG("record for UUID 0x%s not found",
1792 bt_uuid_str(session->param->uuid));
1793 /* Call user UUID handler */
1794 sdp_client_notify_result(session, UUID_NOT_RESOLVED);
1795 net_buf_pull(buf, frame_len + sizeof(cstate->length));
1796 goto iterate;
1797 }
1798
1799 /* Get total value of all attributes to be collected */
1800 frame_len -= sdp_client_get_total(session, buf, &total);
1801
1802 if (total > net_buf_tailroom(session->rec_buf)) {
1803 BT_WARN("Not enough room for getting records data");
1804 goto iterate;
1805 }
1806
1807 net_buf_add_mem(session->rec_buf, buf->data, frame_len);
1808 net_buf_pull(buf, frame_len);
1809
1810 /*
1811 * check if current response says there's next portion to be
1812 * fetched
1813 */
1814 if (cstate->length) {
1815 /* Cache original Continuation State in context */
1816 memcpy(&session->cstate, cstate,
1817 sizeof(struct bt_sdp_pdu_cstate));
1818
1819 net_buf_pull(buf, cstate->length +
1820 sizeof(cstate->length));
1821
1822 /* Request for next portion of attributes data */
1823 sdp_client_ssa_search(session);
1824 break;
1825 }
1826
1827 net_buf_pull(buf, sizeof(cstate->length));
1828
1829 BT_DBG("UUID 0x%s resolved", bt_uuid_str(session->param->uuid));
1830 sdp_client_notify_result(session, UUID_RESOLVED);
1831 iterate:
1832 /* Get next UUID and start resolving it */
1833 sdp_client_params_iterator(session);
1834 break;
1835 default:
1836 BT_DBG("PDU 0x%0x response not handled", hdr->op_code);
1837 break;
1838 }
1839
1840 return 0;
1841 }
1842
sdp_client_chan_connect(struct bt_sdp_client * session)1843 static int sdp_client_chan_connect(struct bt_sdp_client *session)
1844 {
1845 return bt_l2cap_br_chan_connect(session->chan.chan.conn,
1846 &session->chan.chan, SDP_PSM);
1847 }
1848
sdp_client_alloc_buf(struct bt_l2cap_chan * chan)1849 static struct net_buf *sdp_client_alloc_buf(struct bt_l2cap_chan *chan)
1850 {
1851 struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
1852 struct net_buf *buf;
1853
1854 BT_DBG("session %p chan %p", session, chan);
1855
1856 session->param = GET_PARAM(sys_slist_peek_head(&session->reqs));
1857
1858 buf = net_buf_alloc(session->param->pool, K_FOREVER);
1859 __ASSERT_NO_MSG(buf);
1860
1861 return buf;
1862 }
1863
sdp_client_connected(struct bt_l2cap_chan * chan)1864 static void sdp_client_connected(struct bt_l2cap_chan *chan)
1865 {
1866 struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
1867
1868 BT_DBG("session %p chan %p connected", session, chan);
1869
1870 session->rec_buf = chan->ops->alloc_buf(chan);
1871
1872 sdp_client_ssa_search(session);
1873 }
1874
sdp_client_disconnected(struct bt_l2cap_chan * chan)1875 static void sdp_client_disconnected(struct bt_l2cap_chan *chan)
1876 {
1877 struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
1878
1879 BT_DBG("session %p chan %p disconnected", session, chan);
1880
1881 net_buf_unref(session->rec_buf);
1882
1883 /*
1884 * Reset session excluding L2CAP channel member. Let's the channel
1885 * resets autonomous.
1886 */
1887 (void)memset(&session->reqs, 0,
1888 sizeof(*session) - sizeof(session->chan));
1889 }
1890
1891 static const struct bt_l2cap_chan_ops sdp_client_chan_ops = {
1892 .connected = sdp_client_connected,
1893 .disconnected = sdp_client_disconnected,
1894 .recv = sdp_client_receive,
1895 .alloc_buf = sdp_client_alloc_buf,
1896 };
1897
sdp_client_new_session(struct bt_conn * conn)1898 static struct bt_sdp_client *sdp_client_new_session(struct bt_conn *conn)
1899 {
1900 int i;
1901
1902 for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) {
1903 struct bt_sdp_client *session = &bt_sdp_client_pool[i];
1904 int err;
1905
1906 if (session->chan.chan.conn) {
1907 continue;
1908 }
1909
1910 sys_slist_init(&session->reqs);
1911
1912 session->chan.chan.ops = &sdp_client_chan_ops;
1913 session->chan.chan.conn = conn;
1914 session->chan.rx.mtu = SDP_CLIENT_MTU;
1915
1916 err = sdp_client_chan_connect(session);
1917 if (err) {
1918 (void)memset(session, 0, sizeof(*session));
1919 BT_ERR("Cannot connect %d", err);
1920 return NULL;
1921 }
1922
1923 return session;
1924 }
1925
1926 BT_ERR("No available SDP client context");
1927
1928 return NULL;
1929 }
1930
sdp_client_get_session(struct bt_conn * conn)1931 static struct bt_sdp_client *sdp_client_get_session(struct bt_conn *conn)
1932 {
1933 int i;
1934
1935 for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) {
1936 if (bt_sdp_client_pool[i].chan.chan.conn == conn) {
1937 return &bt_sdp_client_pool[i];
1938 }
1939 }
1940
1941 /*
1942 * Try to allocate session context since not found in pool and attempt
1943 * connect to remote SDP endpoint.
1944 */
1945 return sdp_client_new_session(conn);
1946 }
1947
bt_sdp_discover(struct bt_conn * conn,const struct bt_sdp_discover_params * params)1948 int bt_sdp_discover(struct bt_conn *conn,
1949 const struct bt_sdp_discover_params *params)
1950 {
1951 struct bt_sdp_client *session;
1952
1953 if (!params || !params->uuid || !params->func || !params->pool) {
1954 BT_WARN("Invalid user params");
1955 return -EINVAL;
1956 }
1957
1958 session = sdp_client_get_session(conn);
1959 if (!session) {
1960 return -ENOMEM;
1961 }
1962
1963 sys_slist_append(&session->reqs, (sys_snode_t *)¶ms->_node);
1964
1965 return 0;
1966 }
1967
1968 /* Helper getting length of data determined by DTD for integers */
sdp_get_int_len(const uint8_t * data,size_t len)1969 static inline ssize_t sdp_get_int_len(const uint8_t *data, size_t len)
1970 {
1971 BT_ASSERT(data);
1972
1973 switch (data[0]) {
1974 case BT_SDP_DATA_NIL:
1975 return 1;
1976 case BT_SDP_BOOL:
1977 case BT_SDP_INT8:
1978 case BT_SDP_UINT8:
1979 if (len < 2) {
1980 break;
1981 }
1982
1983 return 2;
1984 case BT_SDP_INT16:
1985 case BT_SDP_UINT16:
1986 if (len < 3) {
1987 break;
1988 }
1989
1990 return 3;
1991 case BT_SDP_INT32:
1992 case BT_SDP_UINT32:
1993 if (len < 5) {
1994 break;
1995 }
1996
1997 return 5;
1998 case BT_SDP_INT64:
1999 case BT_SDP_UINT64:
2000 if (len < 9) {
2001 break;
2002 }
2003
2004 return 9;
2005 case BT_SDP_INT128:
2006 case BT_SDP_UINT128:
2007 default:
2008 BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
2009 return -EINVAL;
2010 }
2011
2012 BT_ERR("Too short buffer length %zu", len);
2013 return -EMSGSIZE;
2014 }
2015
2016 /* Helper getting length of data determined by DTD for UUID */
sdp_get_uuid_len(const uint8_t * data,size_t len)2017 static inline ssize_t sdp_get_uuid_len(const uint8_t *data, size_t len)
2018 {
2019 BT_ASSERT(data);
2020
2021 switch (data[0]) {
2022 case BT_SDP_UUID16:
2023 if (len < 3) {
2024 break;
2025 }
2026
2027 return 3;
2028 case BT_SDP_UUID32:
2029 if (len < 5) {
2030 break;
2031 }
2032
2033 return 5;
2034 case BT_SDP_UUID128:
2035 default:
2036 BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
2037 return -EINVAL;
2038 }
2039
2040 BT_ERR("Too short buffer length %zu", len);
2041 return -EMSGSIZE;
2042 }
2043
2044 /* Helper getting length of data determined by DTD for strings */
sdp_get_str_len(const uint8_t * data,size_t len)2045 static inline ssize_t sdp_get_str_len(const uint8_t *data, size_t len)
2046 {
2047 const uint8_t *pnext;
2048
2049 BT_ASSERT(data);
2050
2051 /* validate len for pnext safe use to read next 8bit value */
2052 if (len < 2) {
2053 goto err;
2054 }
2055
2056 pnext = data + sizeof(uint8_t);
2057
2058 switch (data[0]) {
2059 case BT_SDP_TEXT_STR8:
2060 case BT_SDP_URL_STR8:
2061 if (len < (2 + pnext[0])) {
2062 break;
2063 }
2064
2065 return 2 + pnext[0];
2066 case BT_SDP_TEXT_STR16:
2067 case BT_SDP_URL_STR16:
2068 /* validate len for pnext safe use to read 16bit value */
2069 if (len < 3) {
2070 break;
2071 }
2072
2073 if (len < (3 + sys_get_be16(pnext))) {
2074 break;
2075 }
2076
2077 return 3 + sys_get_be16(pnext);
2078 case BT_SDP_TEXT_STR32:
2079 case BT_SDP_URL_STR32:
2080 default:
2081 BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
2082 return -EINVAL;
2083 }
2084 err:
2085 BT_ERR("Too short buffer length %zu", len);
2086 return -EMSGSIZE;
2087 }
2088
2089 /* Helper getting length of data determined by DTD for sequences */
sdp_get_seq_len(const uint8_t * data,size_t len)2090 static inline ssize_t sdp_get_seq_len(const uint8_t *data, size_t len)
2091 {
2092 const uint8_t *pnext;
2093
2094 BT_ASSERT(data);
2095
2096 /* validate len for pnext safe use to read 8bit bit value */
2097 if (len < 2) {
2098 goto err;
2099 }
2100
2101 pnext = data + sizeof(uint8_t);
2102
2103 switch (data[0]) {
2104 case BT_SDP_SEQ8:
2105 case BT_SDP_ALT8:
2106 if (len < (2 + pnext[0])) {
2107 break;
2108 }
2109
2110 return 2 + pnext[0];
2111 case BT_SDP_SEQ16:
2112 case BT_SDP_ALT16:
2113 /* validate len for pnext safe use to read 16bit value */
2114 if (len < 3) {
2115 break;
2116 }
2117
2118 if (len < (3 + sys_get_be16(pnext))) {
2119 break;
2120 }
2121
2122 return 3 + sys_get_be16(pnext);
2123 case BT_SDP_SEQ32:
2124 case BT_SDP_ALT32:
2125 default:
2126 BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
2127 return -EINVAL;
2128 }
2129 err:
2130 BT_ERR("Too short buffer length %zu", len);
2131 return -EMSGSIZE;
2132 }
2133
2134 /* Helper getting length of attribute value data */
sdp_get_attr_value_len(const uint8_t * data,size_t len)2135 static ssize_t sdp_get_attr_value_len(const uint8_t *data, size_t len)
2136 {
2137 BT_ASSERT(data);
2138
2139 BT_DBG("Attr val DTD 0x%02x", data[0]);
2140
2141 if (len < 1) {
2142 goto err;
2143 }
2144
2145 switch (data[0]) {
2146 case BT_SDP_DATA_NIL:
2147 case BT_SDP_BOOL:
2148 case BT_SDP_UINT8:
2149 case BT_SDP_UINT16:
2150 case BT_SDP_UINT32:
2151 case BT_SDP_UINT64:
2152 case BT_SDP_UINT128:
2153 case BT_SDP_INT8:
2154 case BT_SDP_INT16:
2155 case BT_SDP_INT32:
2156 case BT_SDP_INT64:
2157 case BT_SDP_INT128:
2158 return sdp_get_int_len(data, len);
2159 case BT_SDP_UUID16:
2160 case BT_SDP_UUID32:
2161 case BT_SDP_UUID128:
2162 return sdp_get_uuid_len(data, len);
2163 case BT_SDP_TEXT_STR8:
2164 case BT_SDP_TEXT_STR16:
2165 case BT_SDP_TEXT_STR32:
2166 case BT_SDP_URL_STR8:
2167 case BT_SDP_URL_STR16:
2168 case BT_SDP_URL_STR32:
2169 return sdp_get_str_len(data, len);
2170 case BT_SDP_SEQ8:
2171 case BT_SDP_SEQ16:
2172 case BT_SDP_SEQ32:
2173 case BT_SDP_ALT8:
2174 case BT_SDP_ALT16:
2175 case BT_SDP_ALT32:
2176 return sdp_get_seq_len(data, len);
2177 default:
2178 BT_ERR("Unknown DTD 0x%02x", data[0]);
2179 return -EINVAL;
2180 }
2181 err:
2182 BT_ERR("Too short buffer length %zu", len);
2183 return -EMSGSIZE;
2184
2185 }
2186
2187 /* Type holding UUID item and related to it specific information. */
2188 struct bt_sdp_uuid_desc {
2189 union {
2190 struct bt_uuid uuid;
2191 struct bt_uuid_16 uuid16;
2192 struct bt_uuid_32 uuid32;
2193 };
2194 uint16_t attr_id;
2195 uint8_t *params;
2196 uint16_t params_len;
2197 };
2198
2199 /* Generic attribute item collector. */
2200 struct bt_sdp_attr_item {
2201 /* Attribute identifier. */
2202 uint16_t attr_id;
2203 /* Address of beginning attribute value taken from original buffer
2204 * holding response from server.
2205 */
2206 uint8_t *val;
2207 /* Says about the length of attribute value. */
2208 uint16_t len;
2209 };
2210
bt_sdp_get_attr(const struct net_buf * buf,struct bt_sdp_attr_item * attr,uint16_t attr_id)2211 static int bt_sdp_get_attr(const struct net_buf *buf,
2212 struct bt_sdp_attr_item *attr, uint16_t attr_id)
2213 {
2214 uint8_t *data;
2215 uint16_t id;
2216
2217 data = buf->data;
2218 while (data - buf->data < buf->len) {
2219 ssize_t dlen;
2220
2221 /* data need to point to attribute id descriptor field (DTD)*/
2222 if (data[0] != BT_SDP_UINT16) {
2223 BT_ERR("Invalid descriptor 0x%02x", data[0]);
2224 return -EINVAL;
2225 }
2226
2227 data += sizeof(uint8_t);
2228 if ((data + sizeof(id) - buf->data) > buf->len) {
2229 return -EINVAL;
2230 }
2231 id = sys_get_be16(data);
2232 BT_DBG("Attribute ID 0x%04x", id);
2233 data += sizeof(uint16_t);
2234
2235 dlen = sdp_get_attr_value_len(data,
2236 buf->len - (data - buf->data));
2237 if (dlen < 0) {
2238 BT_ERR("Invalid attribute value data");
2239 return -EINVAL;
2240 }
2241
2242 if (id == attr_id) {
2243 BT_DBG("Attribute ID 0x%04x Value found", id);
2244 /*
2245 * Initialize attribute value buffer data using selected
2246 * data slice from original buffer.
2247 */
2248 attr->val = data;
2249 attr->len = dlen;
2250 attr->attr_id = id;
2251 return 0;
2252 }
2253
2254 data += dlen;
2255 }
2256
2257 return -ENOENT;
2258 }
2259
2260 /* reads SEQ item length, moves input buffer data reader forward */
sdp_get_seq_len_item(uint8_t ** data,size_t len)2261 static ssize_t sdp_get_seq_len_item(uint8_t **data, size_t len)
2262 {
2263 const uint8_t *pnext;
2264
2265 BT_ASSERT(data);
2266 BT_ASSERT(*data);
2267
2268 /* validate len for pnext safe use to read 8bit bit value */
2269 if (len < 2) {
2270 goto err;
2271 }
2272
2273 pnext = *data + sizeof(uint8_t);
2274
2275 switch (*data[0]) {
2276 case BT_SDP_SEQ8:
2277 if (len < (2 + pnext[0])) {
2278 break;
2279 }
2280
2281 *data += 2;
2282 return pnext[0];
2283 case BT_SDP_SEQ16:
2284 /* validate len for pnext safe use to read 16bit value */
2285 if (len < 3) {
2286 break;
2287 }
2288
2289 if (len < (3 + sys_get_be16(pnext))) {
2290 break;
2291 }
2292
2293 *data += 3;
2294 return sys_get_be16(pnext);
2295 case BT_SDP_SEQ32:
2296 /* validate len for pnext safe use to read 32bit value */
2297 if (len < 5) {
2298 break;
2299 }
2300
2301 if (len < (5 + sys_get_be32(pnext))) {
2302 break;
2303 }
2304
2305 *data += 5;
2306 return sys_get_be32(pnext);
2307 default:
2308 BT_ERR("Invalid/unhandled DTD 0x%02x", *data[0]);
2309 return -EINVAL;
2310 }
2311 err:
2312 BT_ERR("Too short buffer length %zu", len);
2313 return -EMSGSIZE;
2314 }
2315
sdp_loop_seqs(uint8_t ** data,size_t len)2316 static int sdp_loop_seqs(uint8_t **data, size_t len)
2317 {
2318 ssize_t slen;
2319 ssize_t pre_slen;
2320 uint8_t *end;
2321
2322 if (len <= 0) {
2323 return -EMSGSIZE;
2324 }
2325
2326 pre_slen = -EINVAL;
2327 slen = -EINVAL;
2328 end = *data + len;
2329 /* loop all the SEQ */
2330 while (*data < end) {
2331 /* how long is current UUID's item data associated to */
2332 slen = sdp_get_seq_len_item(data, end - *data);
2333 if (slen < 0) {
2334 break;
2335 }
2336 pre_slen = slen;
2337 }
2338
2339 /* return the last seq len */
2340 if (pre_slen < 0) {
2341 return slen;
2342 }
2343
2344 return pre_slen;
2345 }
2346
sdp_get_uuid_data(const struct bt_sdp_attr_item * attr,struct bt_sdp_uuid_desc * pd,uint16_t proto_profile,uint8_t proto_profile_index)2347 static int sdp_get_uuid_data(const struct bt_sdp_attr_item *attr,
2348 struct bt_sdp_uuid_desc *pd,
2349 uint16_t proto_profile,
2350 uint8_t proto_profile_index)
2351 {
2352 /* get start address of attribute value */
2353 uint8_t *p = attr->val;
2354 ssize_t slen;
2355
2356 BT_ASSERT(p);
2357
2358 /* start reading stacked UUIDs in analyzed sequences tree */
2359 while (p - attr->val < attr->len) {
2360 size_t to_end, left = 0;
2361 uint8_t dtd;
2362
2363 /* to_end tells how far to the end of input buffer */
2364 to_end = attr->len - (p - attr->val);
2365 /* loop all the SEQ, get the last SEQ len */
2366 slen = sdp_loop_seqs(&p, to_end);
2367
2368 if (slen < 0) {
2369 return slen;
2370 }
2371
2372 /* left tells how far is to the end of current UUID */
2373 left = slen;
2374
2375 /* check if at least DTD + UUID16 can be read safely */
2376 if (left < (sizeof(dtd) + BT_UUID_SIZE_16)) {
2377 return -EMSGSIZE;
2378 }
2379
2380 /* check DTD and get stacked UUID value */
2381 dtd = p[0];
2382 p++;
2383 /* include last DTD in p[0] size itself updating left */
2384 left -= sizeof(dtd);
2385 switch (dtd) {
2386 case BT_SDP_UUID16:
2387 memcpy(&pd->uuid16,
2388 BT_UUID_DECLARE_16(sys_get_be16(p)),
2389 sizeof(struct bt_uuid_16));
2390 p += sizeof(uint16_t);
2391 left -= sizeof(uint16_t);
2392 break;
2393 case BT_SDP_UUID32:
2394 /* check if valid UUID32 can be read safely */
2395 if (left < BT_UUID_SIZE_32) {
2396 return -EMSGSIZE;
2397 }
2398
2399 memcpy(&pd->uuid32,
2400 BT_UUID_DECLARE_32(sys_get_be32(p)),
2401 sizeof(struct bt_uuid_32));
2402 p += sizeof(BT_UUID_SIZE_32);
2403 left -= sizeof(BT_UUID_SIZE_32);
2404 break;
2405 default:
2406 BT_ERR("Invalid/unhandled DTD 0x%02x\n", dtd);
2407 return -EINVAL;
2408 }
2409
2410 /*
2411 * Check if current UUID value matches input one given by user.
2412 * If found save it's location and length and return.
2413 */
2414 if ((proto_profile == BT_UUID_16(&pd->uuid)->val) ||
2415 (proto_profile == BT_UUID_32(&pd->uuid)->val)) {
2416 pd->params = p;
2417 pd->params_len = left;
2418
2419 BT_DBG("UUID 0x%s found", bt_uuid_str(&pd->uuid));
2420 if (proto_profile_index > 0U) {
2421 proto_profile_index--;
2422 p += left;
2423 continue;
2424 } else {
2425 return 0;
2426 }
2427 }
2428
2429 /* skip left octets to point beginning of next UUID in tree */
2430 p += left;
2431 }
2432
2433 BT_DBG("Value 0x%04x index %d not found", proto_profile, proto_profile_index);
2434 return -ENOENT;
2435 }
2436
2437 /*
2438 * Helper extracting specific parameters associated with UUID node given in
2439 * protocol descriptor list or profile descriptor list.
2440 */
sdp_get_param_item(struct bt_sdp_uuid_desc * pd_item,uint16_t * param)2441 static int sdp_get_param_item(struct bt_sdp_uuid_desc *pd_item, uint16_t *param)
2442 {
2443 const uint8_t *p = pd_item->params;
2444 bool len_err = false;
2445
2446 BT_ASSERT(p);
2447
2448 BT_DBG("Getting UUID's 0x%s params", bt_uuid_str(&pd_item->uuid));
2449
2450 switch (p[0]) {
2451 case BT_SDP_UINT8:
2452 /* check if 8bits value can be read safely */
2453 if (pd_item->params_len < 2) {
2454 len_err = true;
2455 break;
2456 }
2457 *param = (++p)[0];
2458 p += sizeof(uint8_t);
2459 break;
2460 case BT_SDP_UINT16:
2461 /* check if 16bits value can be read safely */
2462 if (pd_item->params_len < 3) {
2463 len_err = true;
2464 break;
2465 }
2466 *param = sys_get_be16(++p);
2467 p += sizeof(uint16_t);
2468 break;
2469 case BT_SDP_UINT32:
2470 /* check if 32bits value can be read safely */
2471 if (pd_item->params_len < 5) {
2472 len_err = true;
2473 break;
2474 }
2475 *param = sys_get_be32(++p);
2476 p += sizeof(uint32_t);
2477 break;
2478 default:
2479 BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]);
2480 return -EINVAL;
2481 }
2482 /*
2483 * Check if no more data than already read is associated with UUID. In
2484 * valid case after getting parameter we should reach data buf end.
2485 */
2486 if (p - pd_item->params != pd_item->params_len || len_err) {
2487 BT_DBG("Invalid param buffer length");
2488 return -EMSGSIZE;
2489 }
2490
2491 return 0;
2492 }
2493
bt_sdp_get_proto_param(const struct net_buf * buf,enum bt_sdp_proto proto,uint16_t * param)2494 int bt_sdp_get_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto,
2495 uint16_t *param)
2496 {
2497 struct bt_sdp_attr_item attr;
2498 struct bt_sdp_uuid_desc pd;
2499 int res;
2500
2501 if (proto != BT_SDP_PROTO_RFCOMM && proto != BT_SDP_PROTO_L2CAP) {
2502 BT_ERR("Invalid protocol specifier");
2503 return -EINVAL;
2504 }
2505
2506 res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROTO_DESC_LIST);
2507 if (res < 0) {
2508 BT_WARN("Attribute 0x%04x not found, err %d",
2509 BT_SDP_ATTR_PROTO_DESC_LIST, res);
2510 return res;
2511 }
2512
2513 res = sdp_get_uuid_data(&attr, &pd, proto, 0U);
2514 if (res < 0) {
2515 BT_WARN("Protocol specifier 0x%04x not found, err %d", proto,
2516 res);
2517 return res;
2518 }
2519
2520 return sdp_get_param_item(&pd, param);
2521 }
2522
bt_sdp_get_addl_proto_param(const struct net_buf * buf,enum bt_sdp_proto proto,uint8_t param_index,uint16_t * param)2523 int bt_sdp_get_addl_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto,
2524 uint8_t param_index, uint16_t *param)
2525 {
2526 struct bt_sdp_attr_item attr;
2527 struct bt_sdp_uuid_desc pd;
2528 int res;
2529
2530 if (proto != BT_SDP_PROTO_RFCOMM && proto != BT_SDP_PROTO_L2CAP) {
2531 BT_ERR("Invalid protocol specifier");
2532 return -EINVAL;
2533 }
2534
2535 res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_ADD_PROTO_DESC_LIST);
2536 if (res < 0) {
2537 BT_WARN("Attribute 0x%04x not found, err %d",
2538 BT_SDP_ATTR_PROTO_DESC_LIST, res);
2539 return res;
2540 }
2541
2542 res = sdp_get_uuid_data(&attr, &pd, proto, param_index);
2543 if (res < 0) {
2544 BT_WARN("Protocol specifier 0x%04x not found, err %d", proto,
2545 res);
2546 return res;
2547 }
2548
2549 return sdp_get_param_item(&pd, param);
2550 }
2551
bt_sdp_get_profile_version(const struct net_buf * buf,uint16_t profile,uint16_t * version)2552 int bt_sdp_get_profile_version(const struct net_buf *buf, uint16_t profile,
2553 uint16_t *version)
2554 {
2555 struct bt_sdp_attr_item attr;
2556 struct bt_sdp_uuid_desc pd;
2557 int res;
2558
2559 res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROFILE_DESC_LIST);
2560 if (res < 0) {
2561 BT_WARN("Attribute 0x%04x not found, err %d",
2562 BT_SDP_ATTR_PROFILE_DESC_LIST, res);
2563 return res;
2564 }
2565
2566 res = sdp_get_uuid_data(&attr, &pd, profile, 0U);
2567 if (res < 0) {
2568 BT_WARN("Profile 0x%04x not found, err %d", profile, res);
2569 return res;
2570 }
2571
2572 return sdp_get_param_item(&pd, version);
2573 }
2574
bt_sdp_get_features(const struct net_buf * buf,uint16_t * features)2575 int bt_sdp_get_features(const struct net_buf *buf, uint16_t *features)
2576 {
2577 struct bt_sdp_attr_item attr;
2578 const uint8_t *p;
2579 int res;
2580
2581 res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_SUPPORTED_FEATURES);
2582 if (res < 0) {
2583 BT_WARN("Attribute 0x%04x not found, err %d",
2584 BT_SDP_ATTR_SUPPORTED_FEATURES, res);
2585 return res;
2586 }
2587
2588 p = attr.val;
2589 BT_ASSERT(p);
2590
2591 if (p[0] != BT_SDP_UINT16) {
2592 BT_ERR("Invalid DTD 0x%02x", p[0]);
2593 return -EINVAL;
2594 }
2595
2596 /* assert 16bit can be read safely */
2597 if (attr.len < 3) {
2598 BT_ERR("Data length too short %u", attr.len);
2599 return -EMSGSIZE;
2600 }
2601
2602 *features = sys_get_be16(++p);
2603 p += sizeof(uint16_t);
2604
2605 if (p - attr.val != attr.len) {
2606 BT_ERR("Invalid data length %u", attr.len);
2607 return -EMSGSIZE;
2608 }
2609
2610 return 0;
2611 }
2612
