1 /* $NetBSD: ssr.c,v 1.2 2007/11/09 20:08:41 plunky Exp $ */
2 /* $DragonFly: src/usr.sbin/sdpd/ssr.c,v 1.1 2008/01/06 21:51:30 hasso Exp $ */
3
4 /*
5 * ssr.c
6 *
7 * Copyright (c) 2004 Maksim Yevmenkin <m_evmenkin@yahoo.com>
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 * 1. Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * 2. Redistributions in binary form must reproduce the above copyright
16 * notice, this list of conditions and the following disclaimer in the
17 * documentation and/or other materials provided with the distribution.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
20 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
23 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
25 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
26 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
28 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
29 * SUCH DAMAGE.
30 *
31 * $Id: ssr.c,v 1.2 2007/11/30 07:39:37 griffin Exp $
32 * $FreeBSD: src/usr.sbin/bluetooth/sdpd/ssr.c,v 1.3 2005/01/05 18:37:37 emax Exp $
33 */
34
35 #include <sys/queue.h>
36 #include <sys/uio.h>
37 #include <netinet/in.h>
38 #include <arpa/inet.h>
39 #include <assert.h>
40 #include <bluetooth.h>
41 #include <errno.h>
42 #include <sdp.h>
43 #include <string.h>
44 #include "profile.h"
45 #include "provider.h"
46 #include "server.h"
47 #include "uuid-private.h"
48
49 /*
50 * Extract ServiceSearchPattern from request to uuid array
51 * return count or 0 if error
52 */
53 int
server_get_service_search_pattern(uint8_t const ** buf,uint8_t const * end,uint128_t * uuid)54 server_get_service_search_pattern(uint8_t const **buf, uint8_t const *end, uint128_t *uuid)
55 {
56 uint8_t const *req = *buf;
57 uint32_t type, ssplen;
58 int count;
59
60 if (req + 1 > end)
61 return 0;
62
63 SDP_GET8(type, req);
64
65 ssplen = 0;
66 switch (type) {
67 case SDP_DATA_SEQ8:
68 if (req + 1 > end)
69 return 0;
70
71 SDP_GET8(ssplen, req);
72 break;
73
74 case SDP_DATA_SEQ16:
75 if (req + 2 > end)
76 return 0;
77
78 SDP_GET16(ssplen, req);
79 break;
80
81 case SDP_DATA_SEQ32:
82 if (req + 4 > end)
83 return 0;
84
85 SDP_GET32(ssplen, req);
86 break;
87
88 default:
89 return 0;
90 }
91
92 if (req + ssplen > end)
93 return 0;
94
95 count = 0;
96 while (ssplen > 0) {
97 if (count == 12)
98 return 0;
99
100 SDP_GET8(type, req);
101 ssplen--;
102
103 switch (type) {
104 case SDP_DATA_UUID16:
105 if (ssplen < 2)
106 return 0;
107
108 memcpy(uuid, &uuid_base, sizeof(*uuid));
109 uuid->b[2] = *req++;
110 uuid->b[3] = *req++;
111 ssplen -= 2;
112 break;
113
114 case SDP_DATA_UUID32:
115 if (ssplen < 4)
116 return 0;
117
118 memcpy(uuid, &uuid_base, sizeof(*uuid));
119 uuid->b[0] = *req++;
120 uuid->b[1] = *req++;
121 uuid->b[2] = *req++;
122 uuid->b[3] = *req++;
123 ssplen -= 4;
124 break;
125
126 case SDP_DATA_UUID128:
127 if (ssplen < 16)
128 return 0;
129
130 memcpy(uuid, req, 16);
131 req += 16;
132 ssplen -= 16;
133 break;
134
135 default:
136 return 0;
137 }
138
139 count++;
140 uuid++;
141 }
142
143 *buf = req;
144 return count;
145 }
146
147 /*
148 * Prepare SDP Service Search Response
149 */
150
151 int32_t
server_prepare_service_search_response(server_p srv,int32_t fd)152 server_prepare_service_search_response(server_p srv, int32_t fd)
153 {
154 uint8_t const *req = srv->req + sizeof(sdp_pdu_t);
155 uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len;
156 uint8_t *rsp = srv->fdidx[fd].rsp;
157 uint8_t const *rsp_end = rsp + L2CAP_MTU_MAXIMUM;
158
159 provider_t *provider = NULL;
160 int32_t ucount, rsp_limit, cslen, cs;
161 uint128_t ulist[12];
162
163 /*
164 * Minimal SDP Service Search Request
165 *
166 * seq8 len8 - 2 bytes
167 * uuid16 value16 - 3 bytes ServiceSearchPattern
168 * value16 - 2 bytes MaximumServiceRecordCount
169 * value8 - 1 byte ContinuationState
170 */
171
172 /* Get ServiceSearchPattern into uuid array */
173 ucount = server_get_service_search_pattern(&req, req_end, ulist);
174 if (ucount < 1 || ucount > 12)
175 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
176
177 /* Get MaximumServiceRecordCount */
178 if (req + 2 > req_end)
179 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
180
181 SDP_GET16(rsp_limit, req);
182 if (rsp_limit <= 0)
183 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
184
185 /* Get ContinuationState */
186 if (req + 1 > req_end)
187 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX);
188
189 SDP_GET8(cslen, req);
190 if (cslen == 2 && req + 2 == req_end)
191 SDP_GET16(cs, req);
192 else if (cslen == 0 && req == req_end)
193 cs = 0;
194 else
195 return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
196
197 /* Process the request. First, check continuation state */
198 if (srv->fdidx[fd].rsp_cs != cs)
199 return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE);
200 if (srv->fdidx[fd].rsp_size > 0)
201 return (0);
202
203 /*
204 * Service Search Response format
205 *
206 * value16 - 2 bytes TotalServiceRecordCount (not incl.)
207 * value16 - 2 bytes CurrentServiceRecordCount (not incl.)
208 * value32 - 4 bytes handle
209 * [ value32 ]
210 */
211
212 /* Look for the record handles and add to the rsp buffer */
213 for (provider = provider_get_first();
214 provider != NULL;
215 provider = provider_get_next(provider)) {
216 if (!provider_match_bdaddr(provider, &srv->req_sa.bt_bdaddr))
217 continue;
218
219 if (!provider_match_uuid(provider, ulist, ucount))
220 continue;
221
222 if (rsp + 4 > rsp_end)
223 break;
224
225 SDP_PUT32(provider->handle, rsp);
226 }
227
228 /* Set reply size (not counting PDU header and continuation state) */
229 srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 4;
230 srv->fdidx[fd].rsp_size = rsp - srv->fdidx[fd].rsp;
231 srv->fdidx[fd].rsp_cs = 0;
232
233 return (0);
234 }
235
236 /*
237 * Send SDP Service Search Response
238 */
239
240 int32_t
server_send_service_search_response(server_p srv,int32_t fd)241 server_send_service_search_response(server_p srv, int32_t fd)
242 {
243 uint8_t *rsp = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_cs;
244 uint8_t *rsp_end = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_size;
245
246 struct iovec iov[4];
247 sdp_pdu_t pdu;
248 uint16_t rcounts[2];
249 uint8_t cs[3];
250 int32_t size;
251
252 /* First update continuation state (assume we will send all data) */
253 size = rsp_end - rsp;
254 srv->fdidx[fd].rsp_cs += size;
255
256 if (size + 1 > srv->fdidx[fd].rsp_limit) {
257 /*
258 * We need to split out response. Add 3 more bytes for the
259 * continuation state and move rsp_end and rsp_cs backwards.
260 */
261
262 while ((rsp_end - rsp) + 3 > srv->fdidx[fd].rsp_limit) {
263 rsp_end -= 4;
264 srv->fdidx[fd].rsp_cs -= 4;
265 }
266
267 cs[0] = 2;
268 cs[1] = srv->fdidx[fd].rsp_cs >> 8;
269 cs[2] = srv->fdidx[fd].rsp_cs & 0xff;
270 } else
271 cs[0] = 0;
272
273 assert(rsp_end >= rsp);
274
275 rcounts[0] = srv->fdidx[fd].rsp_size / 4; /* TotalServiceRecordCount */
276 rcounts[1] = (rsp_end - rsp) / 4; /* CurrentServiceRecordCount */
277
278 pdu.pid = SDP_PDU_SERVICE_SEARCH_RESPONSE;
279 pdu.tid = ((sdp_pdu_p)(srv->req))->tid;
280 pdu.len = htons(sizeof(rcounts) + rcounts[1] * 4 + 1 + cs[0]);
281
282 rcounts[0] = htons(rcounts[0]);
283 rcounts[1] = htons(rcounts[1]);
284
285 iov[0].iov_base = &pdu;
286 iov[0].iov_len = sizeof(pdu);
287
288 iov[1].iov_base = rcounts;
289 iov[1].iov_len = sizeof(rcounts);
290
291 iov[2].iov_base = rsp;
292 iov[2].iov_len = rsp_end - rsp;
293
294 iov[3].iov_base = cs;
295 iov[3].iov_len = 1 + cs[0];
296
297 do {
298 size = writev(fd, (struct iovec const *) &iov, sizeof(iov)/sizeof(iov[0]));
299 } while (size < 0 && errno == EINTR);
300
301 /* Check if we have sent (or failed to sent) last response chunk */
302 if (srv->fdidx[fd].rsp_cs == srv->fdidx[fd].rsp_size) {
303 srv->fdidx[fd].rsp_cs = 0;
304 srv->fdidx[fd].rsp_size = 0;
305 srv->fdidx[fd].rsp_limit = 0;
306 }
307
308 return ((size < 0)? errno : 0);
309 }
310