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 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 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 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