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