root / hw / bt-sdp.c @ 562183de
History | View | Annotate | Download (28.9 kB)
1 | 4d2d181c | balrog | /*
|
---|---|---|---|
2 | 4d2d181c | balrog | * Service Discover Protocol server for QEMU L2CAP devices
|
3 | 4d2d181c | balrog | *
|
4 | 4d2d181c | balrog | * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
|
5 | 4d2d181c | balrog | *
|
6 | 4d2d181c | balrog | * This program is free software; you can redistribute it and/or
|
7 | 4d2d181c | balrog | * modify it under the terms of the GNU General Public License as
|
8 | 4d2d181c | balrog | * published by the Free Software Foundation; either version 2 of
|
9 | 4d2d181c | balrog | * the License, or (at your option) any later version.
|
10 | 4d2d181c | balrog | *
|
11 | 4d2d181c | balrog | * This program is distributed in the hope that it will be useful,
|
12 | 4d2d181c | balrog | * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13 | 4d2d181c | balrog | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14 | 4d2d181c | balrog | * GNU General Public License for more details.
|
15 | 4d2d181c | balrog | *
|
16 | fad6cb1a | aurel32 | * You should have received a copy of the GNU General Public License along
|
17 | fad6cb1a | aurel32 | * with this program; if not, write to the Free Software Foundation, Inc.,
|
18 | fad6cb1a | aurel32 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
19 | 4d2d181c | balrog | */
|
20 | 4d2d181c | balrog | |
21 | 4d2d181c | balrog | #include "qemu-common.h" |
22 | 4d2d181c | balrog | #include "bt.h" |
23 | 4d2d181c | balrog | |
24 | 4d2d181c | balrog | struct bt_l2cap_sdp_state_s {
|
25 | 4d2d181c | balrog | struct bt_l2cap_conn_params_s *channel;
|
26 | 4d2d181c | balrog | |
27 | 4d2d181c | balrog | struct sdp_service_record_s {
|
28 | 4d2d181c | balrog | int match;
|
29 | 4d2d181c | balrog | |
30 | 4d2d181c | balrog | int *uuid;
|
31 | 4d2d181c | balrog | int uuids;
|
32 | 4d2d181c | balrog | struct sdp_service_attribute_s {
|
33 | 4d2d181c | balrog | int match;
|
34 | 4d2d181c | balrog | |
35 | 4d2d181c | balrog | int attribute_id;
|
36 | 4d2d181c | balrog | int len;
|
37 | 4d2d181c | balrog | void *pair;
|
38 | 4d2d181c | balrog | } *attribute_list; |
39 | 4d2d181c | balrog | int attributes;
|
40 | 4d2d181c | balrog | } *service_list; |
41 | 4d2d181c | balrog | int services;
|
42 | 4d2d181c | balrog | }; |
43 | 4d2d181c | balrog | |
44 | 4d2d181c | balrog | static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left) |
45 | 4d2d181c | balrog | { |
46 | 4d2d181c | balrog | size_t len = *(*element) ++ & SDP_DSIZE_MASK; |
47 | 4d2d181c | balrog | |
48 | 4d2d181c | balrog | if (!*left)
|
49 | 4d2d181c | balrog | return -1; |
50 | 4d2d181c | balrog | (*left) --; |
51 | 4d2d181c | balrog | |
52 | 4d2d181c | balrog | if (len < SDP_DSIZE_NEXT1)
|
53 | 4d2d181c | balrog | return 1 << len; |
54 | 4d2d181c | balrog | else if (len == SDP_DSIZE_NEXT1) { |
55 | 4d2d181c | balrog | if (*left < 1) |
56 | 4d2d181c | balrog | return -1; |
57 | 4d2d181c | balrog | (*left) --; |
58 | 4d2d181c | balrog | |
59 | 4d2d181c | balrog | return *(*element) ++;
|
60 | 4d2d181c | balrog | } else if (len == SDP_DSIZE_NEXT2) { |
61 | 4d2d181c | balrog | if (*left < 2) |
62 | 4d2d181c | balrog | return -1; |
63 | 4d2d181c | balrog | (*left) -= 2;
|
64 | 4d2d181c | balrog | |
65 | 4d2d181c | balrog | len = (*(*element) ++) << 8;
|
66 | 4d2d181c | balrog | return len | (*(*element) ++);
|
67 | 4d2d181c | balrog | } else {
|
68 | 4d2d181c | balrog | if (*left < 4) |
69 | 4d2d181c | balrog | return -1; |
70 | 4d2d181c | balrog | (*left) -= 4;
|
71 | 4d2d181c | balrog | |
72 | 4d2d181c | balrog | len = (*(*element) ++) << 24;
|
73 | 4d2d181c | balrog | len |= (*(*element) ++) << 16;
|
74 | 4d2d181c | balrog | len |= (*(*element) ++) << 8;
|
75 | 4d2d181c | balrog | return len | (*(*element) ++);
|
76 | 4d2d181c | balrog | } |
77 | 4d2d181c | balrog | } |
78 | 4d2d181c | balrog | |
79 | 4d2d181c | balrog | static const uint8_t bt_base_uuid[12] = { |
80 | 4d2d181c | balrog | 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb, |
81 | 4d2d181c | balrog | }; |
82 | 4d2d181c | balrog | |
83 | 4d2d181c | balrog | static int sdp_uuid_match(struct sdp_service_record_s *record, |
84 | 4d2d181c | balrog | const uint8_t *uuid, ssize_t datalen)
|
85 | 4d2d181c | balrog | { |
86 | 4d2d181c | balrog | int *lo, hi, val;
|
87 | 4d2d181c | balrog | |
88 | 4d2d181c | balrog | if (datalen == 16 || datalen == 4) { |
89 | 4d2d181c | balrog | if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12)) |
90 | 4d2d181c | balrog | return 0; |
91 | 4d2d181c | balrog | |
92 | 4d2d181c | balrog | if (uuid[0] | uuid[1]) |
93 | 4d2d181c | balrog | return 0; |
94 | 4d2d181c | balrog | uuid += 2;
|
95 | 4d2d181c | balrog | } |
96 | 4d2d181c | balrog | |
97 | 4d2d181c | balrog | val = (uuid[0] << 8) | uuid[1]; |
98 | 4d2d181c | balrog | lo = record->uuid; |
99 | 4d2d181c | balrog | hi = record->uuids; |
100 | 4d2d181c | balrog | while (hi >>= 1) |
101 | 4d2d181c | balrog | if (lo[hi] <= val)
|
102 | 4d2d181c | balrog | lo += hi; |
103 | 4d2d181c | balrog | |
104 | 4d2d181c | balrog | return *lo == val;
|
105 | 4d2d181c | balrog | } |
106 | 4d2d181c | balrog | |
107 | 4d2d181c | balrog | #define CONTINUATION_PARAM_SIZE (1 + sizeof(int)) |
108 | 4d2d181c | balrog | #define MAX_PDU_OUT_SIZE 96 /* Arbitrary */ |
109 | 4d2d181c | balrog | #define PDU_HEADER_SIZE 5 |
110 | 4d2d181c | balrog | #define MAX_RSP_PARAM_SIZE (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \
|
111 | 4d2d181c | balrog | CONTINUATION_PARAM_SIZE) |
112 | 4d2d181c | balrog | |
113 | 4d2d181c | balrog | static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp, |
114 | 4d2d181c | balrog | const uint8_t **req, ssize_t *len)
|
115 | 4d2d181c | balrog | { |
116 | 4d2d181c | balrog | size_t datalen; |
117 | 4d2d181c | balrog | int i;
|
118 | 4d2d181c | balrog | |
119 | 4d2d181c | balrog | if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)
|
120 | 4d2d181c | balrog | return 1; |
121 | 4d2d181c | balrog | |
122 | 4d2d181c | balrog | datalen = sdp_datalen(req, len); |
123 | 4d2d181c | balrog | if (datalen != 2 && datalen != 4 && datalen != 16) |
124 | 4d2d181c | balrog | return 1; |
125 | 4d2d181c | balrog | |
126 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) |
127 | 4d2d181c | balrog | if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))
|
128 | 4d2d181c | balrog | sdp->service_list[i].match = 1;
|
129 | 4d2d181c | balrog | |
130 | 4d2d181c | balrog | (*req) += datalen; |
131 | 4d2d181c | balrog | (*len) -= datalen; |
132 | 4d2d181c | balrog | |
133 | 4d2d181c | balrog | return 0; |
134 | 4d2d181c | balrog | } |
135 | 4d2d181c | balrog | |
136 | 4d2d181c | balrog | static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp, |
137 | 4d2d181c | balrog | uint8_t *rsp, const uint8_t *req, ssize_t len)
|
138 | 4d2d181c | balrog | { |
139 | 4d2d181c | balrog | ssize_t seqlen; |
140 | 4d2d181c | balrog | int i, count, start, end, max;
|
141 | 4d2d181c | balrog | int32_t handle; |
142 | 4d2d181c | balrog | |
143 | 4d2d181c | balrog | /* Perform the search */
|
144 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) |
145 | 4d2d181c | balrog | sdp->service_list[i].match = 0;
|
146 | 4d2d181c | balrog | |
147 | 4d2d181c | balrog | if (len < 1) |
148 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
149 | 4d2d181c | balrog | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
150 | 4d2d181c | balrog | seqlen = sdp_datalen(&req, &len); |
151 | 4d2d181c | balrog | if (seqlen < 3 || len < seqlen) |
152 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
153 | 4d2d181c | balrog | len -= seqlen; |
154 | 4d2d181c | balrog | |
155 | 4d2d181c | balrog | while (seqlen)
|
156 | 4d2d181c | balrog | if (sdp_svc_match(sdp, &req, &seqlen))
|
157 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
158 | 4d2d181c | balrog | } else if (sdp_svc_match(sdp, &req, &seqlen)) |
159 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
160 | 4d2d181c | balrog | |
161 | 4d2d181c | balrog | if (len < 3) |
162 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
163 | 4d2d181c | balrog | end = (req[0] << 8) | req[1]; |
164 | 4d2d181c | balrog | req += 2;
|
165 | 4d2d181c | balrog | len -= 2;
|
166 | 4d2d181c | balrog | |
167 | 4d2d181c | balrog | if (*req) {
|
168 | 4d2d181c | balrog | if (len <= sizeof(int)) |
169 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
170 | 4d2d181c | balrog | len -= sizeof(int); |
171 | 4d2d181c | balrog | memcpy(&start, req + 1, sizeof(int)); |
172 | 4d2d181c | balrog | } else
|
173 | 4d2d181c | balrog | start = 0;
|
174 | 4d2d181c | balrog | |
175 | 4d2d181c | balrog | if (len > 1); |
176 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
177 | 4d2d181c | balrog | |
178 | 4d2d181c | balrog | /* Output the results */
|
179 | 4d2d181c | balrog | len = 4;
|
180 | 4d2d181c | balrog | count = 0;
|
181 | 4d2d181c | balrog | end = start; |
182 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) |
183 | 4d2d181c | balrog | if (sdp->service_list[i].match) {
|
184 | 4d2d181c | balrog | if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) { |
185 | 4d2d181c | balrog | handle = i; |
186 | 4d2d181c | balrog | memcpy(rsp + len, &handle, 4);
|
187 | 4d2d181c | balrog | len += 4;
|
188 | 4d2d181c | balrog | end = count + 1;
|
189 | 4d2d181c | balrog | } |
190 | 4d2d181c | balrog | |
191 | 4d2d181c | balrog | count ++; |
192 | 4d2d181c | balrog | } |
193 | 4d2d181c | balrog | |
194 | 4d2d181c | balrog | rsp[0] = count >> 8; |
195 | 4d2d181c | balrog | rsp[1] = count & 0xff; |
196 | 4d2d181c | balrog | rsp[2] = (end - start) >> 8; |
197 | 4d2d181c | balrog | rsp[3] = (end - start) & 0xff; |
198 | 4d2d181c | balrog | |
199 | 4d2d181c | balrog | if (end < count) {
|
200 | 4d2d181c | balrog | rsp[len ++] = sizeof(int); |
201 | 4d2d181c | balrog | memcpy(rsp + len, &end, sizeof(int)); |
202 | 4d2d181c | balrog | len += 4;
|
203 | 4d2d181c | balrog | } else
|
204 | 4d2d181c | balrog | rsp[len ++] = 0;
|
205 | 4d2d181c | balrog | |
206 | 4d2d181c | balrog | return len;
|
207 | 4d2d181c | balrog | } |
208 | 4d2d181c | balrog | |
209 | 4d2d181c | balrog | static int sdp_attr_match(struct sdp_service_record_s *record, |
210 | 4d2d181c | balrog | const uint8_t **req, ssize_t *len)
|
211 | 4d2d181c | balrog | { |
212 | 4d2d181c | balrog | int i, start, end;
|
213 | 4d2d181c | balrog | |
214 | 4d2d181c | balrog | if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
|
215 | 4d2d181c | balrog | (*req) ++; |
216 | 4d2d181c | balrog | if (*len < 3) |
217 | 4d2d181c | balrog | return 1; |
218 | 4d2d181c | balrog | |
219 | 4d2d181c | balrog | start = (*(*req) ++) << 8;
|
220 | 4d2d181c | balrog | start |= *(*req) ++; |
221 | 4d2d181c | balrog | end = start; |
222 | 4d2d181c | balrog | *len -= 3;
|
223 | 4d2d181c | balrog | } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) { |
224 | 4d2d181c | balrog | (*req) ++; |
225 | 4d2d181c | balrog | if (*len < 5) |
226 | 4d2d181c | balrog | return 1; |
227 | 4d2d181c | balrog | |
228 | 4d2d181c | balrog | start = (*(*req) ++) << 8;
|
229 | 4d2d181c | balrog | start |= *(*req) ++; |
230 | 4d2d181c | balrog | end = (*(*req) ++) << 8;
|
231 | 4d2d181c | balrog | end |= *(*req) ++; |
232 | 4d2d181c | balrog | *len -= 5;
|
233 | 4d2d181c | balrog | } else
|
234 | 4d2d181c | balrog | return 1; |
235 | 4d2d181c | balrog | |
236 | 4d2d181c | balrog | for (i = 0; i < record->attributes; i ++) |
237 | 4d2d181c | balrog | if (record->attribute_list[i].attribute_id >= start &&
|
238 | 4d2d181c | balrog | record->attribute_list[i].attribute_id <= end) |
239 | 4d2d181c | balrog | record->attribute_list[i].match = 1;
|
240 | 4d2d181c | balrog | |
241 | 4d2d181c | balrog | return 0; |
242 | 4d2d181c | balrog | } |
243 | 4d2d181c | balrog | |
244 | 4d2d181c | balrog | static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp, |
245 | 4d2d181c | balrog | uint8_t *rsp, const uint8_t *req, ssize_t len)
|
246 | 4d2d181c | balrog | { |
247 | 4d2d181c | balrog | ssize_t seqlen; |
248 | 4d2d181c | balrog | int i, start, end, max;
|
249 | 4d2d181c | balrog | int32_t handle; |
250 | 4d2d181c | balrog | struct sdp_service_record_s *record;
|
251 | 4d2d181c | balrog | uint8_t *lst; |
252 | 4d2d181c | balrog | |
253 | 4d2d181c | balrog | /* Perform the search */
|
254 | 4d2d181c | balrog | if (len < 7) |
255 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
256 | fbc190d8 | balrog | memcpy(&handle, req, 4);
|
257 | 4d2d181c | balrog | req += 4;
|
258 | 4d2d181c | balrog | len -= 4;
|
259 | 4d2d181c | balrog | |
260 | 4d2d181c | balrog | if (handle < 0 || handle > sdp->services) |
261 | 4d2d181c | balrog | return -SDP_INVALID_RECORD_HANDLE;
|
262 | 4d2d181c | balrog | record = &sdp->service_list[handle]; |
263 | 4d2d181c | balrog | |
264 | 4d2d181c | balrog | for (i = 0; i < record->attributes; i ++) |
265 | 4d2d181c | balrog | record->attribute_list[i].match = 0;
|
266 | 4d2d181c | balrog | |
267 | 4d2d181c | balrog | max = (req[0] << 8) | req[1]; |
268 | 4d2d181c | balrog | req += 2;
|
269 | 4d2d181c | balrog | len -= 2;
|
270 | 4d2d181c | balrog | if (max < 0x0007) |
271 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
272 | 4d2d181c | balrog | |
273 | 4d2d181c | balrog | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
274 | 4d2d181c | balrog | seqlen = sdp_datalen(&req, &len); |
275 | 4d2d181c | balrog | if (seqlen < 3 || len < seqlen) |
276 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
277 | 4d2d181c | balrog | len -= seqlen; |
278 | 4d2d181c | balrog | |
279 | 4d2d181c | balrog | while (seqlen)
|
280 | 4d2d181c | balrog | if (sdp_attr_match(record, &req, &seqlen))
|
281 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
282 | 4d2d181c | balrog | } else if (sdp_attr_match(record, &req, &seqlen)) |
283 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
284 | 4d2d181c | balrog | |
285 | 4d2d181c | balrog | if (len < 1) |
286 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
287 | 4d2d181c | balrog | |
288 | 4d2d181c | balrog | if (*req) {
|
289 | 4d2d181c | balrog | if (len <= sizeof(int)) |
290 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
291 | 4d2d181c | balrog | len -= sizeof(int); |
292 | 4d2d181c | balrog | memcpy(&start, req + 1, sizeof(int)); |
293 | 4d2d181c | balrog | } else
|
294 | 4d2d181c | balrog | start = 0;
|
295 | 4d2d181c | balrog | |
296 | 4d2d181c | balrog | if (len > 1) |
297 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
298 | 4d2d181c | balrog | |
299 | 4d2d181c | balrog | /* Output the results */
|
300 | 4d2d181c | balrog | lst = rsp + 2;
|
301 | 4d2d181c | balrog | max = MIN(max, MAX_RSP_PARAM_SIZE); |
302 | 4d2d181c | balrog | len = 3 - start;
|
303 | 4d2d181c | balrog | end = 0;
|
304 | 4d2d181c | balrog | for (i = 0; i < record->attributes; i ++) |
305 | 4d2d181c | balrog | if (record->attribute_list[i].match) {
|
306 | 4d2d181c | balrog | if (len >= 0 && len + record->attribute_list[i].len < max) { |
307 | 4d2d181c | balrog | memcpy(lst + len, record->attribute_list[i].pair, |
308 | 4d2d181c | balrog | record->attribute_list[i].len); |
309 | 4d2d181c | balrog | end = len + record->attribute_list[i].len; |
310 | 4d2d181c | balrog | } |
311 | 4d2d181c | balrog | len += record->attribute_list[i].len; |
312 | 4d2d181c | balrog | } |
313 | 4d2d181c | balrog | if (0 >= start) { |
314 | 4d2d181c | balrog | lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
|
315 | 4d2d181c | balrog | lst[1] = (len + start - 3) >> 8; |
316 | 4d2d181c | balrog | lst[2] = (len + start - 3) & 0xff; |
317 | 4d2d181c | balrog | } |
318 | 4d2d181c | balrog | |
319 | 4d2d181c | balrog | rsp[0] = end >> 8; |
320 | 4d2d181c | balrog | rsp[1] = end & 0xff; |
321 | 4d2d181c | balrog | |
322 | 4d2d181c | balrog | if (end < len) {
|
323 | 4d2d181c | balrog | len = end + start; |
324 | 4d2d181c | balrog | lst[end ++] = sizeof(int); |
325 | 4d2d181c | balrog | memcpy(lst + end, &len, sizeof(int)); |
326 | 4d2d181c | balrog | end += sizeof(int); |
327 | 4d2d181c | balrog | } else
|
328 | 4d2d181c | balrog | lst[end ++] = 0;
|
329 | 4d2d181c | balrog | |
330 | 4d2d181c | balrog | return end + 2; |
331 | 4d2d181c | balrog | } |
332 | 4d2d181c | balrog | |
333 | 4d2d181c | balrog | static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp, |
334 | 4d2d181c | balrog | const uint8_t **req, ssize_t *len)
|
335 | 4d2d181c | balrog | { |
336 | 4d2d181c | balrog | int i, j, start, end;
|
337 | 4d2d181c | balrog | struct sdp_service_record_s *record;
|
338 | 4d2d181c | balrog | |
339 | 4d2d181c | balrog | if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
|
340 | 4d2d181c | balrog | (*req) ++; |
341 | 4d2d181c | balrog | if (*len < 3) |
342 | 4d2d181c | balrog | return 1; |
343 | 4d2d181c | balrog | |
344 | 4d2d181c | balrog | start = (*(*req) ++) << 8;
|
345 | 4d2d181c | balrog | start |= *(*req) ++; |
346 | 4d2d181c | balrog | end = start; |
347 | 4d2d181c | balrog | *len -= 3;
|
348 | 4d2d181c | balrog | } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) { |
349 | 4d2d181c | balrog | (*req) ++; |
350 | 4d2d181c | balrog | if (*len < 5) |
351 | 4d2d181c | balrog | return 1; |
352 | 4d2d181c | balrog | |
353 | 4d2d181c | balrog | start = (*(*req) ++) << 8;
|
354 | 4d2d181c | balrog | start |= *(*req) ++; |
355 | 4d2d181c | balrog | end = (*(*req) ++) << 8;
|
356 | 4d2d181c | balrog | end |= *(*req) ++; |
357 | 4d2d181c | balrog | *len -= 5;
|
358 | 4d2d181c | balrog | } else
|
359 | 4d2d181c | balrog | return 1; |
360 | 4d2d181c | balrog | |
361 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) |
362 | 4d2d181c | balrog | if ((record = &sdp->service_list[i])->match)
|
363 | 4d2d181c | balrog | for (j = 0; j < record->attributes; j ++) |
364 | 4d2d181c | balrog | if (record->attribute_list[j].attribute_id >= start &&
|
365 | 4d2d181c | balrog | record->attribute_list[j].attribute_id <= end) |
366 | 4d2d181c | balrog | record->attribute_list[j].match = 1;
|
367 | 4d2d181c | balrog | |
368 | 4d2d181c | balrog | return 0; |
369 | 4d2d181c | balrog | } |
370 | 4d2d181c | balrog | |
371 | 4d2d181c | balrog | static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp, |
372 | 4d2d181c | balrog | uint8_t *rsp, const uint8_t *req, ssize_t len)
|
373 | 4d2d181c | balrog | { |
374 | 4d2d181c | balrog | ssize_t seqlen; |
375 | 4d2d181c | balrog | int i, j, start, end, max;
|
376 | 4d2d181c | balrog | struct sdp_service_record_s *record;
|
377 | 4d2d181c | balrog | uint8_t *lst; |
378 | 4d2d181c | balrog | |
379 | 4d2d181c | balrog | /* Perform the search */
|
380 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) { |
381 | 4d2d181c | balrog | sdp->service_list[i].match = 0;
|
382 | 4d2d181c | balrog | for (j = 0; j < sdp->service_list[i].attributes; j ++) |
383 | 4d2d181c | balrog | sdp->service_list[i].attribute_list[j].match = 0;
|
384 | 4d2d181c | balrog | } |
385 | 4d2d181c | balrog | |
386 | 4d2d181c | balrog | if (len < 1) |
387 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
388 | 4d2d181c | balrog | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
389 | 4d2d181c | balrog | seqlen = sdp_datalen(&req, &len); |
390 | 4d2d181c | balrog | if (seqlen < 3 || len < seqlen) |
391 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
392 | 4d2d181c | balrog | len -= seqlen; |
393 | 4d2d181c | balrog | |
394 | 4d2d181c | balrog | while (seqlen)
|
395 | 4d2d181c | balrog | if (sdp_svc_match(sdp, &req, &seqlen))
|
396 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
397 | 4d2d181c | balrog | } else if (sdp_svc_match(sdp, &req, &seqlen)) |
398 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
399 | 4d2d181c | balrog | |
400 | 4d2d181c | balrog | if (len < 3) |
401 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
402 | 4d2d181c | balrog | max = (req[0] << 8) | req[1]; |
403 | 4d2d181c | balrog | req += 2;
|
404 | 4d2d181c | balrog | len -= 2;
|
405 | 4d2d181c | balrog | if (max < 0x0007) |
406 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
407 | 4d2d181c | balrog | |
408 | 4d2d181c | balrog | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
|
409 | 4d2d181c | balrog | seqlen = sdp_datalen(&req, &len); |
410 | 4d2d181c | balrog | if (seqlen < 3 || len < seqlen) |
411 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
412 | 4d2d181c | balrog | len -= seqlen; |
413 | 4d2d181c | balrog | |
414 | 4d2d181c | balrog | while (seqlen)
|
415 | 4d2d181c | balrog | if (sdp_svc_attr_match(sdp, &req, &seqlen))
|
416 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
417 | 4d2d181c | balrog | } else if (sdp_svc_attr_match(sdp, &req, &seqlen)) |
418 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
419 | 4d2d181c | balrog | |
420 | 4d2d181c | balrog | if (len < 1) |
421 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
422 | 4d2d181c | balrog | |
423 | 4d2d181c | balrog | if (*req) {
|
424 | 4d2d181c | balrog | if (len <= sizeof(int)) |
425 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
426 | 4d2d181c | balrog | len -= sizeof(int); |
427 | 4d2d181c | balrog | memcpy(&start, req + 1, sizeof(int)); |
428 | 4d2d181c | balrog | } else
|
429 | 4d2d181c | balrog | start = 0;
|
430 | 4d2d181c | balrog | |
431 | 4d2d181c | balrog | if (len > 1) |
432 | 4d2d181c | balrog | return -SDP_INVALID_SYNTAX;
|
433 | 4d2d181c | balrog | |
434 | 4d2d181c | balrog | /* Output the results */
|
435 | 4d2d181c | balrog | /* This assumes empty attribute lists are never to be returned even
|
436 | 4d2d181c | balrog | * for matching Service Records. In practice this shouldn't happen
|
437 | 4d2d181c | balrog | * as the requestor will usually include the always present
|
438 | 4d2d181c | balrog | * ServiceRecordHandle AttributeID in AttributeIDList. */
|
439 | 4d2d181c | balrog | lst = rsp + 2;
|
440 | 4d2d181c | balrog | max = MIN(max, MAX_RSP_PARAM_SIZE); |
441 | 4d2d181c | balrog | len = 3 - start;
|
442 | 4d2d181c | balrog | end = 0;
|
443 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) |
444 | 4d2d181c | balrog | if ((record = &sdp->service_list[i])->match) {
|
445 | 4d2d181c | balrog | len += 3;
|
446 | 4d2d181c | balrog | seqlen = len; |
447 | 4d2d181c | balrog | for (j = 0; j < record->attributes; j ++) |
448 | 4d2d181c | balrog | if (record->attribute_list[j].match) {
|
449 | 4d2d181c | balrog | if (len >= 0) |
450 | 4d2d181c | balrog | if (len + record->attribute_list[j].len < max) {
|
451 | 4d2d181c | balrog | memcpy(lst + len, record->attribute_list[j].pair, |
452 | 4d2d181c | balrog | record->attribute_list[j].len); |
453 | 4d2d181c | balrog | end = len + record->attribute_list[j].len; |
454 | 4d2d181c | balrog | } |
455 | 4d2d181c | balrog | len += record->attribute_list[j].len; |
456 | 4d2d181c | balrog | } |
457 | 4d2d181c | balrog | if (seqlen == len)
|
458 | 4d2d181c | balrog | len -= 3;
|
459 | 4d2d181c | balrog | else if (seqlen >= 3 && seqlen < max) { |
460 | 4d2d181c | balrog | lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
|
461 | 4d2d181c | balrog | lst[seqlen - 2] = (len - seqlen) >> 8; |
462 | 4d2d181c | balrog | lst[seqlen - 1] = (len - seqlen) & 0xff; |
463 | 4d2d181c | balrog | } |
464 | 4d2d181c | balrog | } |
465 | 4d2d181c | balrog | if (len == 3 - start) |
466 | 4d2d181c | balrog | len -= 3;
|
467 | 4d2d181c | balrog | else if (0 >= start) { |
468 | 4d2d181c | balrog | lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
|
469 | 4d2d181c | balrog | lst[1] = (len + start - 3) >> 8; |
470 | 4d2d181c | balrog | lst[2] = (len + start - 3) & 0xff; |
471 | 4d2d181c | balrog | } |
472 | 4d2d181c | balrog | |
473 | 4d2d181c | balrog | rsp[0] = end >> 8; |
474 | 4d2d181c | balrog | rsp[1] = end & 0xff; |
475 | 4d2d181c | balrog | |
476 | 4d2d181c | balrog | if (end < len) {
|
477 | 4d2d181c | balrog | len = end + start; |
478 | 4d2d181c | balrog | lst[end ++] = sizeof(int); |
479 | 4d2d181c | balrog | memcpy(lst + end, &len, sizeof(int)); |
480 | 4d2d181c | balrog | end += sizeof(int); |
481 | 4d2d181c | balrog | } else
|
482 | 4d2d181c | balrog | lst[end ++] = 0;
|
483 | 4d2d181c | balrog | |
484 | 4d2d181c | balrog | return end + 2; |
485 | 4d2d181c | balrog | } |
486 | 4d2d181c | balrog | |
487 | 4d2d181c | balrog | static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len) |
488 | 4d2d181c | balrog | { |
489 | 4d2d181c | balrog | struct bt_l2cap_sdp_state_s *sdp = opaque;
|
490 | 4d2d181c | balrog | enum bt_sdp_cmd pdu_id;
|
491 | 4d2d181c | balrog | uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out; |
492 | 4d2d181c | balrog | int transaction_id, plen;
|
493 | 4d2d181c | balrog | int err = 0; |
494 | 4d2d181c | balrog | int rsp_len = 0; |
495 | 4d2d181c | balrog | |
496 | 4d2d181c | balrog | if (len < 5) { |
497 | 4d2d181c | balrog | fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);
|
498 | 4d2d181c | balrog | return;
|
499 | 4d2d181c | balrog | } |
500 | 4d2d181c | balrog | |
501 | 4d2d181c | balrog | pdu_id = *data ++; |
502 | 4d2d181c | balrog | transaction_id = (data[0] << 8) | data[1]; |
503 | 4d2d181c | balrog | plen = (data[2] << 8) | data[3]; |
504 | 4d2d181c | balrog | data += 4;
|
505 | 4d2d181c | balrog | len -= 5;
|
506 | 4d2d181c | balrog | |
507 | 4d2d181c | balrog | if (len != plen) {
|
508 | 4d2d181c | balrog | fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",
|
509 | 4d2d181c | balrog | __FUNCTION__, plen, len); |
510 | 4d2d181c | balrog | err = SDP_INVALID_PDU_SIZE; |
511 | 4d2d181c | balrog | goto respond;
|
512 | 4d2d181c | balrog | } |
513 | 4d2d181c | balrog | |
514 | 4d2d181c | balrog | switch (pdu_id) {
|
515 | 4d2d181c | balrog | case SDP_SVC_SEARCH_REQ:
|
516 | 4d2d181c | balrog | rsp_len = sdp_svc_search(sdp, rsp, data, len); |
517 | 4d2d181c | balrog | pdu_id = SDP_SVC_SEARCH_RSP; |
518 | 4d2d181c | balrog | break;
|
519 | 4d2d181c | balrog | |
520 | 4d2d181c | balrog | case SDP_SVC_ATTR_REQ:
|
521 | 4d2d181c | balrog | rsp_len = sdp_attr_get(sdp, rsp, data, len); |
522 | 4d2d181c | balrog | pdu_id = SDP_SVC_ATTR_RSP; |
523 | 4d2d181c | balrog | break;
|
524 | 4d2d181c | balrog | |
525 | 4d2d181c | balrog | case SDP_SVC_SEARCH_ATTR_REQ:
|
526 | 4d2d181c | balrog | rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len); |
527 | 4d2d181c | balrog | pdu_id = SDP_SVC_SEARCH_ATTR_RSP; |
528 | 4d2d181c | balrog | break;
|
529 | 4d2d181c | balrog | |
530 | 4d2d181c | balrog | case SDP_ERROR_RSP:
|
531 | 4d2d181c | balrog | case SDP_SVC_ATTR_RSP:
|
532 | 4d2d181c | balrog | case SDP_SVC_SEARCH_RSP:
|
533 | 4d2d181c | balrog | case SDP_SVC_SEARCH_ATTR_RSP:
|
534 | 4d2d181c | balrog | default:
|
535 | 4d2d181c | balrog | fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n",
|
536 | 4d2d181c | balrog | __FUNCTION__, pdu_id); |
537 | 4d2d181c | balrog | err = SDP_INVALID_SYNTAX; |
538 | 4d2d181c | balrog | break;
|
539 | 4d2d181c | balrog | } |
540 | 4d2d181c | balrog | |
541 | 4d2d181c | balrog | if (rsp_len < 0) { |
542 | 4d2d181c | balrog | err = -rsp_len; |
543 | 4d2d181c | balrog | rsp_len = 0;
|
544 | 4d2d181c | balrog | } |
545 | 4d2d181c | balrog | |
546 | 4d2d181c | balrog | respond:
|
547 | 4d2d181c | balrog | if (err) {
|
548 | 4d2d181c | balrog | pdu_id = SDP_ERROR_RSP; |
549 | 4d2d181c | balrog | rsp[rsp_len ++] = err >> 8;
|
550 | 4d2d181c | balrog | rsp[rsp_len ++] = err & 0xff;
|
551 | 4d2d181c | balrog | } |
552 | 4d2d181c | balrog | |
553 | 4d2d181c | balrog | sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE); |
554 | 4d2d181c | balrog | |
555 | 4d2d181c | balrog | sdu_out[0] = pdu_id;
|
556 | 4d2d181c | balrog | sdu_out[1] = transaction_id >> 8; |
557 | 4d2d181c | balrog | sdu_out[2] = transaction_id & 0xff; |
558 | 4d2d181c | balrog | sdu_out[3] = rsp_len >> 8; |
559 | 4d2d181c | balrog | sdu_out[4] = rsp_len & 0xff; |
560 | 4d2d181c | balrog | memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len); |
561 | 4d2d181c | balrog | |
562 | 4d2d181c | balrog | sdp->channel->sdu_submit(sdp->channel); |
563 | 4d2d181c | balrog | } |
564 | 4d2d181c | balrog | |
565 | 4d2d181c | balrog | static void bt_l2cap_sdp_close_ch(void *opaque) |
566 | 4d2d181c | balrog | { |
567 | 4d2d181c | balrog | struct bt_l2cap_sdp_state_s *sdp = opaque;
|
568 | 4d2d181c | balrog | int i;
|
569 | 4d2d181c | balrog | |
570 | 4d2d181c | balrog | for (i = 0; i < sdp->services; i ++) { |
571 | 4d2d181c | balrog | qemu_free(sdp->service_list[i].attribute_list->pair); |
572 | 4d2d181c | balrog | qemu_free(sdp->service_list[i].attribute_list); |
573 | 4d2d181c | balrog | qemu_free(sdp->service_list[i].uuid); |
574 | 4d2d181c | balrog | } |
575 | 4d2d181c | balrog | qemu_free(sdp->service_list); |
576 | 4d2d181c | balrog | qemu_free(sdp); |
577 | 4d2d181c | balrog | } |
578 | 4d2d181c | balrog | |
579 | 4d2d181c | balrog | struct sdp_def_service_s {
|
580 | 4d2d181c | balrog | uint16_t class_uuid; |
581 | 4d2d181c | balrog | struct sdp_def_attribute_s {
|
582 | 4d2d181c | balrog | uint16_t id; |
583 | 4d2d181c | balrog | struct sdp_def_data_element_s {
|
584 | 4d2d181c | balrog | uint8_t type; |
585 | 4d2d181c | balrog | union {
|
586 | 4d2d181c | balrog | uint32_t uint; |
587 | 4d2d181c | balrog | const char *str; |
588 | 4d2d181c | balrog | struct sdp_def_data_element_s *list;
|
589 | 4d2d181c | balrog | } value; |
590 | 4d2d181c | balrog | } data; |
591 | 4d2d181c | balrog | } attributes[]; |
592 | 4d2d181c | balrog | }; |
593 | 4d2d181c | balrog | |
594 | 4d2d181c | balrog | /* Calculate a safe byte count to allocate that will store the given
|
595 | 4d2d181c | balrog | * element, at the same time count elements of a UUID type. */
|
596 | 4d2d181c | balrog | static int sdp_attr_max_size(struct sdp_def_data_element_s *element, |
597 | 4d2d181c | balrog | int *uuids)
|
598 | 4d2d181c | balrog | { |
599 | 4d2d181c | balrog | int type = element->type & ~SDP_DSIZE_MASK;
|
600 | 4d2d181c | balrog | int len;
|
601 | 4d2d181c | balrog | |
602 | 4d2d181c | balrog | if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID ||
|
603 | 4d2d181c | balrog | type == SDP_DTYPE_BOOL) { |
604 | 4d2d181c | balrog | if (type == SDP_DTYPE_UUID)
|
605 | 4d2d181c | balrog | (*uuids) ++; |
606 | 4d2d181c | balrog | return 1 + (1 << (element->type & SDP_DSIZE_MASK)); |
607 | 4d2d181c | balrog | } |
608 | 4d2d181c | balrog | |
609 | 4d2d181c | balrog | if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
|
610 | 4d2d181c | balrog | if (element->type & SDP_DSIZE_MASK) {
|
611 | 4d2d181c | balrog | for (len = 0; element->value.str[len] | |
612 | 4d2d181c | balrog | element->value.str[len + 1]; len ++);
|
613 | 4d2d181c | balrog | return len;
|
614 | 4d2d181c | balrog | } else
|
615 | 4d2d181c | balrog | return 2 + strlen(element->value.str); |
616 | 4d2d181c | balrog | } |
617 | 4d2d181c | balrog | |
618 | 4d2d181c | balrog | if (type != SDP_DTYPE_SEQ)
|
619 | 4d2d181c | balrog | exit(-1);
|
620 | 4d2d181c | balrog | len = 2;
|
621 | 4d2d181c | balrog | element = element->value.list; |
622 | 4d2d181c | balrog | while (element->type)
|
623 | 4d2d181c | balrog | len += sdp_attr_max_size(element ++, uuids); |
624 | 4d2d181c | balrog | if (len > 255) |
625 | 4d2d181c | balrog | exit (-1);
|
626 | 4d2d181c | balrog | |
627 | 4d2d181c | balrog | return len;
|
628 | 4d2d181c | balrog | } |
629 | 4d2d181c | balrog | |
630 | 4d2d181c | balrog | static int sdp_attr_write(uint8_t *data, |
631 | 4d2d181c | balrog | struct sdp_def_data_element_s *element, int **uuid) |
632 | 4d2d181c | balrog | { |
633 | 4d2d181c | balrog | int type = element->type & ~SDP_DSIZE_MASK;
|
634 | 4d2d181c | balrog | int len = 0; |
635 | 4d2d181c | balrog | |
636 | 4d2d181c | balrog | if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) {
|
637 | 4d2d181c | balrog | data[len ++] = element->type; |
638 | 4d2d181c | balrog | if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1)
|
639 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 0) & 0xff; |
640 | 4d2d181c | balrog | else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) { |
641 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 8) & 0xff; |
642 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 0) & 0xff; |
643 | 4d2d181c | balrog | } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) { |
644 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 24) & 0xff; |
645 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 16) & 0xff; |
646 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 8) & 0xff; |
647 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 0) & 0xff; |
648 | 4d2d181c | balrog | } |
649 | 4d2d181c | balrog | |
650 | 4d2d181c | balrog | return len;
|
651 | 4d2d181c | balrog | } |
652 | 4d2d181c | balrog | |
653 | 4d2d181c | balrog | if (type == SDP_DTYPE_UUID) {
|
654 | 4d2d181c | balrog | *(*uuid) ++ = element->value.uint; |
655 | 4d2d181c | balrog | |
656 | 4d2d181c | balrog | data[len ++] = element->type; |
657 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 24) & 0xff; |
658 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 16) & 0xff; |
659 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 8) & 0xff; |
660 | 4d2d181c | balrog | data[len ++] = (element->value.uint >> 0) & 0xff; |
661 | 4d2d181c | balrog | memcpy(data + len, bt_base_uuid, 12);
|
662 | 4d2d181c | balrog | |
663 | 4d2d181c | balrog | return len + 12; |
664 | 4d2d181c | balrog | } |
665 | 4d2d181c | balrog | |
666 | 4d2d181c | balrog | data[0] = type | SDP_DSIZE_NEXT1;
|
667 | 4d2d181c | balrog | if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
|
668 | 4d2d181c | balrog | if (element->type & SDP_DSIZE_MASK)
|
669 | 4d2d181c | balrog | for (len = 0; element->value.str[len] | |
670 | 4d2d181c | balrog | element->value.str[len + 1]; len ++);
|
671 | 4d2d181c | balrog | else
|
672 | 4d2d181c | balrog | len = strlen(element->value.str); |
673 | 4d2d181c | balrog | memcpy(data + 2, element->value.str, data[1] = len); |
674 | 4d2d181c | balrog | |
675 | 4d2d181c | balrog | return len + 2; |
676 | 4d2d181c | balrog | } |
677 | 4d2d181c | balrog | |
678 | 4d2d181c | balrog | len = 2;
|
679 | 4d2d181c | balrog | element = element->value.list; |
680 | 4d2d181c | balrog | while (element->type)
|
681 | 4d2d181c | balrog | len += sdp_attr_write(data + len, element ++, uuid); |
682 | 4d2d181c | balrog | data[1] = len - 2; |
683 | 4d2d181c | balrog | |
684 | 4d2d181c | balrog | return len;
|
685 | 4d2d181c | balrog | } |
686 | 4d2d181c | balrog | |
687 | 4d2d181c | balrog | static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a, |
688 | 4d2d181c | balrog | const struct sdp_service_attribute_s *b) |
689 | 4d2d181c | balrog | { |
690 | 4d2d181c | balrog | return (int) b->attribute_id - a->attribute_id; |
691 | 4d2d181c | balrog | } |
692 | 4d2d181c | balrog | |
693 | 4d2d181c | balrog | static int sdp_uuid_compare(const int *a, const int *b) |
694 | 4d2d181c | balrog | { |
695 | 4d2d181c | balrog | return *a - *b;
|
696 | 4d2d181c | balrog | } |
697 | 4d2d181c | balrog | |
698 | 4d2d181c | balrog | static void sdp_service_record_build(struct sdp_service_record_s *record, |
699 | 4d2d181c | balrog | struct sdp_def_service_s *def, int handle) |
700 | 4d2d181c | balrog | { |
701 | 4d2d181c | balrog | int len = 0; |
702 | 4d2d181c | balrog | uint8_t *data; |
703 | 4d2d181c | balrog | int *uuid;
|
704 | 4d2d181c | balrog | |
705 | 4d2d181c | balrog | record->uuids = 0;
|
706 | 4d2d181c | balrog | while (def->attributes[record->attributes].data.type) {
|
707 | 4d2d181c | balrog | len += 3;
|
708 | 4d2d181c | balrog | len += sdp_attr_max_size(&def->attributes[record->attributes ++].data, |
709 | 4d2d181c | balrog | &record->uuids); |
710 | 4d2d181c | balrog | } |
711 | 4d2d181c | balrog | record->uuids = 1 << ffs(record->uuids - 1); |
712 | 4d2d181c | balrog | record->attribute_list = |
713 | 4d2d181c | balrog | qemu_mallocz(record->attributes * sizeof(*record->attribute_list));
|
714 | 4d2d181c | balrog | record->uuid = |
715 | 4d2d181c | balrog | qemu_mallocz(record->uuids * sizeof(*record->uuid));
|
716 | 4d2d181c | balrog | data = qemu_malloc(len); |
717 | 4d2d181c | balrog | |
718 | 4d2d181c | balrog | record->attributes = 0;
|
719 | 4d2d181c | balrog | uuid = record->uuid; |
720 | 4d2d181c | balrog | while (def->attributes[record->attributes].data.type) {
|
721 | 4d2d181c | balrog | record->attribute_list[record->attributes].pair = data; |
722 | 4d2d181c | balrog | |
723 | 4d2d181c | balrog | len = 0;
|
724 | 4d2d181c | balrog | data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2; |
725 | 4d2d181c | balrog | data[len ++] = def->attributes[record->attributes].id >> 8;
|
726 | 4d2d181c | balrog | data[len ++] = def->attributes[record->attributes].id & 0xff;
|
727 | 4d2d181c | balrog | len += sdp_attr_write(data + len, |
728 | 4d2d181c | balrog | &def->attributes[record->attributes].data, &uuid); |
729 | 4d2d181c | balrog | |
730 | 4d2d181c | balrog | /* Special case: assign a ServiceRecordHandle in sequence */
|
731 | 4d2d181c | balrog | if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE)
|
732 | 4d2d181c | balrog | def->attributes[record->attributes].data.value.uint = handle; |
733 | 4d2d181c | balrog | /* Note: we could also assign a ServiceDescription based on
|
734 | 4d2d181c | balrog | * sdp->device.device->lmp_name. */
|
735 | 4d2d181c | balrog | |
736 | 4d2d181c | balrog | record->attribute_list[record->attributes ++].len = len; |
737 | 4d2d181c | balrog | data += len; |
738 | 4d2d181c | balrog | } |
739 | 4d2d181c | balrog | |
740 | 4d2d181c | balrog | /* Sort the attribute list by the AttributeID */
|
741 | 4d2d181c | balrog | qsort(record->attribute_list, record->attributes, |
742 | 4d2d181c | balrog | sizeof(*record->attribute_list),
|
743 | 4d2d181c | balrog | (void *) sdp_attributeid_compare);
|
744 | 4d2d181c | balrog | /* Sort the searchable UUIDs list for bisection */
|
745 | 4d2d181c | balrog | qsort(record->uuid, record->uuids, |
746 | 4d2d181c | balrog | sizeof(*record->uuid),
|
747 | 4d2d181c | balrog | (void *) sdp_uuid_compare);
|
748 | 4d2d181c | balrog | } |
749 | 4d2d181c | balrog | |
750 | 4d2d181c | balrog | static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp, |
751 | 4d2d181c | balrog | struct sdp_def_service_s **service)
|
752 | 4d2d181c | balrog | { |
753 | 4d2d181c | balrog | sdp->services = 0;
|
754 | 4d2d181c | balrog | while (service[sdp->services])
|
755 | 4d2d181c | balrog | sdp->services ++; |
756 | 4d2d181c | balrog | sdp->service_list = |
757 | 4d2d181c | balrog | qemu_mallocz(sdp->services * sizeof(*sdp->service_list));
|
758 | 4d2d181c | balrog | |
759 | 4d2d181c | balrog | sdp->services = 0;
|
760 | 4d2d181c | balrog | while (*service) {
|
761 | 4d2d181c | balrog | sdp_service_record_build(&sdp->service_list[sdp->services], |
762 | 4d2d181c | balrog | *service, sdp->services); |
763 | 4d2d181c | balrog | service ++; |
764 | 4d2d181c | balrog | sdp->services ++; |
765 | 4d2d181c | balrog | } |
766 | 4d2d181c | balrog | } |
767 | 4d2d181c | balrog | |
768 | 4d2d181c | balrog | #define LAST { .type = 0 } |
769 | 4d2d181c | balrog | #define SERVICE(name, attrs) \
|
770 | 4d2d181c | balrog | static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \ |
771 | 4d2d181c | balrog | .attributes = { attrs { .data = LAST } }, \ |
772 | 4d2d181c | balrog | }; |
773 | 4d2d181c | balrog | #define ATTRIBUTE(attrid, val) { .id = glue(SDP_ATTR_, attrid), .data = val },
|
774 | 4d2d181c | balrog | #define UINT8(val) { \
|
775 | 4d2d181c | balrog | .type = SDP_DTYPE_UINT | SDP_DSIZE_1, \ |
776 | 4d2d181c | balrog | .value.uint = val, \ |
777 | 4d2d181c | balrog | }, |
778 | 4d2d181c | balrog | #define UINT16(val) { \
|
779 | 4d2d181c | balrog | .type = SDP_DTYPE_UINT | SDP_DSIZE_2, \ |
780 | 4d2d181c | balrog | .value.uint = val, \ |
781 | 4d2d181c | balrog | }, |
782 | 4d2d181c | balrog | #define UINT32(val) { \
|
783 | 4d2d181c | balrog | .type = SDP_DTYPE_UINT | SDP_DSIZE_4, \ |
784 | 4d2d181c | balrog | .value.uint = val, \ |
785 | 4d2d181c | balrog | }, |
786 | 4d2d181c | balrog | #define UUID128(val) { \
|
787 | 4d2d181c | balrog | .type = SDP_DTYPE_UUID | SDP_DSIZE_16, \ |
788 | 4d2d181c | balrog | .value.uint = val, \ |
789 | 4d2d181c | balrog | }, |
790 | 4d2d181c | balrog | #define TRUE { \
|
791 | 4d2d181c | balrog | .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ |
792 | 4d2d181c | balrog | .value.uint = 1, \
|
793 | 4d2d181c | balrog | }, |
794 | 4d2d181c | balrog | #define FALSE { \
|
795 | 4d2d181c | balrog | .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ |
796 | 4d2d181c | balrog | .value.uint = 0, \
|
797 | 4d2d181c | balrog | }, |
798 | 4d2d181c | balrog | #define STRING(val) { \
|
799 | 4d2d181c | balrog | .type = SDP_DTYPE_STRING, \ |
800 | 4d2d181c | balrog | .value.str = val, \ |
801 | 4d2d181c | balrog | }, |
802 | 4d2d181c | balrog | #define ARRAY(...) { \
|
803 | 4d2d181c | balrog | .type = SDP_DTYPE_STRING | SDP_DSIZE_2, \ |
804 | 4d2d181c | balrog | .value.str = (char []) { __VA_ARGS__, 0, 0 }, \ |
805 | 4d2d181c | balrog | }, |
806 | 4d2d181c | balrog | #define URL(val) { \
|
807 | 4d2d181c | balrog | .type = SDP_DTYPE_URL, \ |
808 | 4d2d181c | balrog | .value.str = val, \ |
809 | 4d2d181c | balrog | }, |
810 | 4d2d181c | balrog | #if 1 |
811 | 4d2d181c | balrog | #define LIST(val) { \
|
812 | 4d2d181c | balrog | .type = SDP_DTYPE_SEQ, \ |
813 | 4d2d181c | balrog | .value.list = (struct sdp_def_data_element_s []) { val LAST }, \
|
814 | 4d2d181c | balrog | }, |
815 | 4d2d181c | balrog | #endif
|
816 | 4d2d181c | balrog | |
817 | 4d2d181c | balrog | /* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes
|
818 | 4d2d181c | balrog | * in resulting SDP data representation size. */
|
819 | 4d2d181c | balrog | |
820 | 4d2d181c | balrog | SERVICE(hid, |
821 | 4d2d181c | balrog | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
822 | 4d2d181c | balrog | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID))) |
823 | 4d2d181c | balrog | ATTRIBUTE(RECORD_STATE, UINT32(1))
|
824 | 4d2d181c | balrog | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
825 | 4d2d181c | balrog | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL)) |
826 | 4d2d181c | balrog | LIST(UUID128(HIDP_UUID)) |
827 | 4d2d181c | balrog | )) |
828 | 4d2d181c | balrog | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
|
829 | 4d2d181c | balrog | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
830 | 4d2d181c | balrog | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
831 | 4d2d181c | balrog | )) |
832 | 4d2d181c | balrog | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
833 | 4d2d181c | balrog | LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100))
|
834 | 4d2d181c | balrog | )) |
835 | 4d2d181c | balrog | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
|
836 | 4d2d181c | balrog | ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID"))
|
837 | 4d2d181c | balrog | ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse"))
|
838 | 4d2d181c | balrog | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
|
839 | 4d2d181c | balrog | |
840 | 4d2d181c | balrog | /* Profile specific */
|
841 | 4d2d181c | balrog | ATTRIBUTE(DEVICE_RELEASE_NUMBER, UINT16(0x0091)) /* Deprecated, remove */ |
842 | 4d2d181c | balrog | ATTRIBUTE(PARSER_VERSION, UINT16(0x0111))
|
843 | 4d2d181c | balrog | /* TODO: extract from l2cap_device->device.class[0] */
|
844 | 4d2d181c | balrog | ATTRIBUTE(DEVICE_SUBCLASS, UINT8(0x40))
|
845 | 4d2d181c | balrog | ATTRIBUTE(COUNTRY_CODE, UINT8(0x15))
|
846 | 4d2d181c | balrog | ATTRIBUTE(VIRTUAL_CABLE, TRUE) |
847 | 4d2d181c | balrog | ATTRIBUTE(RECONNECT_INITIATE, FALSE) |
848 | 4d2d181c | balrog | /* TODO: extract from hid->usbdev->report_desc */
|
849 | 4d2d181c | balrog | ATTRIBUTE(DESCRIPTOR_LIST, LIST( |
850 | 4d2d181c | balrog | LIST(UINT8(0x22) ARRAY(
|
851 | 4d2d181c | balrog | 0x05, 0x01, /* Usage Page (Generic Desktop) */ |
852 | 4d2d181c | balrog | 0x09, 0x06, /* Usage (Keyboard) */ |
853 | 4d2d181c | balrog | 0xa1, 0x01, /* Collection (Application) */ |
854 | 4d2d181c | balrog | 0x75, 0x01, /* Report Size (1) */ |
855 | 4d2d181c | balrog | 0x95, 0x08, /* Report Count (8) */ |
856 | 4d2d181c | balrog | 0x05, 0x07, /* Usage Page (Key Codes) */ |
857 | 4d2d181c | balrog | 0x19, 0xe0, /* Usage Minimum (224) */ |
858 | 4d2d181c | balrog | 0x29, 0xe7, /* Usage Maximum (231) */ |
859 | 4d2d181c | balrog | 0x15, 0x00, /* Logical Minimum (0) */ |
860 | 4d2d181c | balrog | 0x25, 0x01, /* Logical Maximum (1) */ |
861 | 4d2d181c | balrog | 0x81, 0x02, /* Input (Data, Variable, Absolute) */ |
862 | 4d2d181c | balrog | 0x95, 0x01, /* Report Count (1) */ |
863 | 4d2d181c | balrog | 0x75, 0x08, /* Report Size (8) */ |
864 | 4d2d181c | balrog | 0x81, 0x01, /* Input (Constant) */ |
865 | 4d2d181c | balrog | 0x95, 0x05, /* Report Count (5) */ |
866 | 4d2d181c | balrog | 0x75, 0x01, /* Report Size (1) */ |
867 | 4d2d181c | balrog | 0x05, 0x08, /* Usage Page (LEDs) */ |
868 | 4d2d181c | balrog | 0x19, 0x01, /* Usage Minimum (1) */ |
869 | 4d2d181c | balrog | 0x29, 0x05, /* Usage Maximum (5) */ |
870 | 4d2d181c | balrog | 0x91, 0x02, /* Output (Data, Variable, Absolute) */ |
871 | 4d2d181c | balrog | 0x95, 0x01, /* Report Count (1) */ |
872 | 4d2d181c | balrog | 0x75, 0x03, /* Report Size (3) */ |
873 | 4d2d181c | balrog | 0x91, 0x01, /* Output (Constant) */ |
874 | 4d2d181c | balrog | 0x95, 0x06, /* Report Count (6) */ |
875 | 4d2d181c | balrog | 0x75, 0x08, /* Report Size (8) */ |
876 | 4d2d181c | balrog | 0x15, 0x00, /* Logical Minimum (0) */ |
877 | 4d2d181c | balrog | 0x25, 0xff, /* Logical Maximum (255) */ |
878 | 4d2d181c | balrog | 0x05, 0x07, /* Usage Page (Key Codes) */ |
879 | 4d2d181c | balrog | 0x19, 0x00, /* Usage Minimum (0) */ |
880 | 4d2d181c | balrog | 0x29, 0xff, /* Usage Maximum (255) */ |
881 | 4d2d181c | balrog | 0x81, 0x00, /* Input (Data, Array) */ |
882 | 4d2d181c | balrog | 0xc0 /* End Collection */ |
883 | 4d2d181c | balrog | )))) |
884 | 4d2d181c | balrog | ATTRIBUTE(LANG_ID_BASE_LIST, LIST( |
885 | 4d2d181c | balrog | LIST(UINT16(0x0409) UINT16(0x0100)) |
886 | 4d2d181c | balrog | )) |
887 | 4d2d181c | balrog | ATTRIBUTE(SDP_DISABLE, FALSE) |
888 | 4d2d181c | balrog | ATTRIBUTE(BATTERY_POWER, TRUE) |
889 | 4d2d181c | balrog | ATTRIBUTE(REMOTE_WAKEUP, TRUE) |
890 | 4d2d181c | balrog | ATTRIBUTE(BOOT_DEVICE, TRUE) /* XXX: untested */
|
891 | 4d2d181c | balrog | ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80))
|
892 | 4d2d181c | balrog | ATTRIBUTE(NORMALLY_CONNECTABLE, TRUE) |
893 | 4d2d181c | balrog | ATTRIBUTE(PROFILE_VERSION, UINT16(0x0100))
|
894 | 4d2d181c | balrog | ) |
895 | 4d2d181c | balrog | |
896 | 4d2d181c | balrog | SERVICE(sdp, |
897 | 4d2d181c | balrog | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
898 | 4d2d181c | balrog | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID))) |
899 | 4d2d181c | balrog | ATTRIBUTE(RECORD_STATE, UINT32(1))
|
900 | 4d2d181c | balrog | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
901 | 4d2d181c | balrog | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) |
902 | 4d2d181c | balrog | LIST(UUID128(SDP_UUID)) |
903 | 4d2d181c | balrog | )) |
904 | 4d2d181c | balrog | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
|
905 | 4d2d181c | balrog | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
906 | 4d2d181c | balrog | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
907 | 4d2d181c | balrog | )) |
908 | 4d2d181c | balrog | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
909 | 4d2d181c | balrog | LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100))
|
910 | 4d2d181c | balrog | )) |
911 | 4d2d181c | balrog | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
|
912 | 4d2d181c | balrog | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
|
913 | 4d2d181c | balrog | |
914 | 4d2d181c | balrog | /* Profile specific */
|
915 | 4d2d181c | balrog | ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100)))
|
916 | 4d2d181c | balrog | ATTRIBUTE(SVCDB_STATE , UINT32(1))
|
917 | 4d2d181c | balrog | ) |
918 | 4d2d181c | balrog | |
919 | 4d2d181c | balrog | SERVICE(pnp, |
920 | 4d2d181c | balrog | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
921 | 4d2d181c | balrog | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID))) |
922 | 4d2d181c | balrog | ATTRIBUTE(RECORD_STATE, UINT32(1))
|
923 | 4d2d181c | balrog | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
924 | 4d2d181c | balrog | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) |
925 | 4d2d181c | balrog | LIST(UUID128(SDP_UUID)) |
926 | 4d2d181c | balrog | )) |
927 | 4d2d181c | balrog | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
|
928 | 4d2d181c | balrog | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
929 | 4d2d181c | balrog | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
930 | 4d2d181c | balrog | )) |
931 | 4d2d181c | balrog | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
932 | 4d2d181c | balrog | LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100))
|
933 | 4d2d181c | balrog | )) |
934 | 4d2d181c | balrog | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html"))
|
935 | 4d2d181c | balrog | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
|
936 | 4d2d181c | balrog | |
937 | 4d2d181c | balrog | /* Profile specific */
|
938 | 4d2d181c | balrog | ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100))
|
939 | 4d2d181c | balrog | ATTRIBUTE(VERSION, UINT16(0x0100))
|
940 | 4d2d181c | balrog | ATTRIBUTE(PRIMARY_RECORD, TRUE) |
941 | 4d2d181c | balrog | ) |
942 | 4d2d181c | balrog | |
943 | 4d2d181c | balrog | static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev, |
944 | 4d2d181c | balrog | struct bt_l2cap_conn_params_s *params)
|
945 | 4d2d181c | balrog | { |
946 | 4d2d181c | balrog | struct bt_l2cap_sdp_state_s *sdp = qemu_mallocz(sizeof(*sdp)); |
947 | 4d2d181c | balrog | struct sdp_def_service_s *services[] = {
|
948 | 4d2d181c | balrog | &sdp_service_sdp_s, |
949 | 4d2d181c | balrog | &sdp_service_hid_s, |
950 | 4d2d181c | balrog | &sdp_service_pnp_s, |
951 | 511d2b14 | blueswir1 | NULL,
|
952 | 4d2d181c | balrog | }; |
953 | 4d2d181c | balrog | |
954 | 4d2d181c | balrog | sdp->channel = params; |
955 | 4d2d181c | balrog | sdp->channel->opaque = sdp; |
956 | 4d2d181c | balrog | sdp->channel->close = bt_l2cap_sdp_close_ch; |
957 | 4d2d181c | balrog | sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in; |
958 | 4d2d181c | balrog | |
959 | 4d2d181c | balrog | sdp_service_db_build(sdp, services); |
960 | 4d2d181c | balrog | |
961 | 4d2d181c | balrog | return 0; |
962 | 4d2d181c | balrog | } |
963 | 4d2d181c | balrog | |
964 | 4d2d181c | balrog | void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev) |
965 | 4d2d181c | balrog | { |
966 | 4d2d181c | balrog | bt_l2cap_psm_register(dev, BT_PSM_SDP, |
967 | 4d2d181c | balrog | MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch); |
968 | 4d2d181c | balrog | } |