1 /* $NetBSD: sar.c,v 1.1 2006/06/19 15:44:56 gdamore Exp $ */ 2 /* $DragonFly: src/usr.sbin/sdpd/sar.c,v 1.1 2008/01/06 21:51:30 hasso Exp $ */ 3 4 /* 5 * sar.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: sar.c,v 1.2 2007/11/30 07:39:37 griffin Exp $ 32 * $FreeBSD: src/usr.sbin/bluetooth/sdpd/sar.c,v 1.2 2004/02/26 20:44:55 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 <stdio.h> /* for NULL */ 44 #include "profile.h" 45 #include "provider.h" 46 #include "server.h" 47 48 /* 49 * Prepare SDP attr/value pair. Check if profile implements the attribute 50 * and if so call the attribute value function. 51 * 52 * uint16 value16 - 3 bytes (attribute) 53 * value - N bytes (value) 54 */ 55 56 static int32_t 57 server_prepare_attr_value_pair( 58 provider_p const provider, uint16_t attr, 59 uint8_t *buf, uint8_t const * const eob) 60 { 61 profile_attr_create_p cf = profile_get_attr(provider->profile, attr); 62 int32_t len; 63 64 if (cf == NULL) 65 return (0); /* no attribute */ 66 67 if (buf + 3 > eob) 68 return (-1); 69 70 SDP_PUT8(SDP_DATA_UINT16, buf); 71 SDP_PUT16(attr, buf); 72 73 len = cf(buf, eob, (uint8_t const *) provider, sizeof(*provider)); 74 if (len < 0) 75 return (-1); 76 77 return (3 + len); 78 } 79 80 /* 81 * seq16 value16 - 3 bytes 82 * attr value - 3+ bytes 83 * [ attr value ] 84 */ 85 86 int32_t 87 server_prepare_attr_list(provider_p const provider, 88 uint8_t const *req, uint8_t const * const req_end, 89 uint8_t *rsp, uint8_t const * const rsp_end) 90 { 91 uint8_t *ptr = rsp + 3; 92 int32_t type, hi, lo, len; 93 94 if (ptr > rsp_end) 95 return (-1); 96 97 while (req < req_end) { 98 SDP_GET8(type, req); 99 100 switch (type) { 101 case SDP_DATA_UINT16: 102 if (req + 2 > req_end) 103 return (-1); 104 105 SDP_GET16(lo, req); 106 hi = lo; 107 break; 108 109 case SDP_DATA_UINT32: 110 if (req + 4 > req_end) 111 return (-1); 112 113 SDP_GET16(lo, req); 114 SDP_GET16(hi, req); 115 break; 116 117 default: 118 return (-1); 119 /* NOT REACHED */ 120 } 121 122 for (; lo <= hi; lo ++) { 123 len = server_prepare_attr_value_pair(provider, lo, ptr, rsp_end); 124 if (len < 0) 125 return (-1); 126 127 ptr += len; 128 } 129 } 130 131 len = ptr - rsp; /* we put this much bytes in rsp */ 132 133 /* Fix SEQ16 header for the rsp */ 134 SDP_PUT8(SDP_DATA_SEQ16, rsp); 135 SDP_PUT16(len - 3, rsp); 136 137 return (len); 138 } 139 140 /* 141 * Prepare SDP Service Attribute Response 142 */ 143 144 int32_t 145 server_prepare_service_attribute_response(server_p srv, int32_t fd) 146 { 147 uint8_t const *req = srv->req + sizeof(sdp_pdu_t); 148 uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len; 149 uint8_t *rsp = srv->fdidx[fd].rsp; 150 uint8_t const *rsp_end = rsp + L2CAP_MTU_MAXIMUM; 151 152 uint8_t const *ptr = NULL; 153 provider_t *provider = NULL; 154 uint32_t handle; 155 int32_t type, rsp_limit, aidlen, cslen, cs; 156 157 /* 158 * Minimal Service Attribute Request request 159 * 160 * value32 - 4 bytes ServiceRecordHandle 161 * value16 - 2 bytes MaximumAttributeByteCount 162 * seq8 len8 - 2 bytes 163 * uint16 value16 - 3 bytes AttributeIDList 164 * value8 - 1 byte ContinuationState 165 */ 166 167 if (req_end - req < 12) 168 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); 169 170 /* Get ServiceRecordHandle and MaximumAttributeByteCount */ 171 SDP_GET32(handle, req); 172 SDP_GET16(rsp_limit, req); 173 if (rsp_limit <= 0) 174 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); 175 176 /* Get size of AttributeIDList */ 177 aidlen = 0; 178 SDP_GET8(type, req); 179 switch (type) { 180 case SDP_DATA_SEQ8: 181 SDP_GET8(aidlen, req); 182 break; 183 184 case SDP_DATA_SEQ16: 185 SDP_GET16(aidlen, req); 186 break; 187 188 case SDP_DATA_SEQ32: 189 SDP_GET32(aidlen, req); 190 break; 191 } 192 if (aidlen <= 0) 193 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); 194 195 ptr = (uint8_t const *) req + aidlen; 196 197 /* Get ContinuationState */ 198 if (ptr + 1 > req_end) 199 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); 200 201 SDP_GET8(cslen, ptr); 202 if (cslen != 0) { 203 if (cslen != 2 || req_end - ptr != 2) 204 return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); 205 206 SDP_GET16(cs, ptr); 207 } else 208 cs = 0; 209 210 /* Process the request. First, check continuation state */ 211 if (srv->fdidx[fd].rsp_cs != cs) 212 return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE); 213 if (srv->fdidx[fd].rsp_size > 0) 214 return (0); 215 216 /* Lookup record handle */ 217 if ((provider = provider_by_handle(handle)) == NULL) 218 return (SDP_ERROR_CODE_INVALID_SERVICE_RECORD_HANDLE); 219 220 /* 221 * Service Attribute Response format 222 * 223 * value16 - 2 bytes AttributeListByteCount (not incl.) 224 * seq8 len16 - 3 bytes 225 * attr value - 3+ bytes AttributeList 226 * [ attr value ] 227 */ 228 229 cs = server_prepare_attr_list(provider, req, req+aidlen, rsp, rsp_end); 230 if (cs < 0) 231 return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES); 232 233 /* Set reply size (not counting PDU header and continuation state) */ 234 srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 2; 235 if (srv->fdidx[fd].rsp_limit > rsp_limit) 236 srv->fdidx[fd].rsp_limit = rsp_limit; 237 238 srv->fdidx[fd].rsp_size = cs; 239 srv->fdidx[fd].rsp_cs = 0; 240 241 return (0); 242 } 243 244 /* 245 * Send SDP Service [Search] Attribute Response 246 */ 247 248 int32_t 249 server_send_service_attribute_response(server_p srv, int32_t fd) 250 { 251 uint8_t *rsp = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_cs; 252 uint8_t *rsp_end = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_size; 253 254 struct iovec iov[4]; 255 sdp_pdu_t pdu; 256 uint16_t bcount; 257 uint8_t cs[3]; 258 int32_t size; 259 260 /* First update continuation state (assume we will send all data) */ 261 size = rsp_end - rsp; 262 srv->fdidx[fd].rsp_cs += size; 263 264 if (size + 1 > srv->fdidx[fd].rsp_limit) { 265 /* 266 * We need to split out response. Add 3 more bytes for the 267 * continuation state and move rsp_end and rsp_cs backwards. 268 */ 269 270 while ((rsp_end - rsp) + 3 > srv->fdidx[fd].rsp_limit) { 271 rsp_end --; 272 srv->fdidx[fd].rsp_cs --; 273 } 274 275 cs[0] = 2; 276 cs[1] = srv->fdidx[fd].rsp_cs >> 8; 277 cs[2] = srv->fdidx[fd].rsp_cs & 0xff; 278 } else 279 cs[0] = 0; 280 281 assert(rsp_end >= rsp); 282 283 bcount = rsp_end - rsp; 284 285 if (((sdp_pdu_p)(srv->req))->pid == SDP_PDU_SERVICE_ATTRIBUTE_REQUEST) 286 pdu.pid = SDP_PDU_SERVICE_ATTRIBUTE_RESPONSE; 287 else 288 pdu.pid = SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_RESPONSE; 289 290 pdu.tid = ((sdp_pdu_p)(srv->req))->tid; 291 pdu.len = htons(sizeof(bcount) + bcount + 1 + cs[0]); 292 293 bcount = htons(bcount); 294 295 iov[0].iov_base = &pdu; 296 iov[0].iov_len = sizeof(pdu); 297 298 iov[1].iov_base = &bcount; 299 iov[1].iov_len = sizeof(bcount); 300 301 iov[2].iov_base = rsp; 302 iov[2].iov_len = rsp_end - rsp; 303 304 iov[3].iov_base = cs; 305 iov[3].iov_len = 1 + cs[0]; 306 307 do { 308 size = writev(fd, (struct iovec const *) &iov, sizeof(iov)/sizeof(iov[0])); 309 } while (size < 0 && errno == EINTR); 310 311 /* Check if we have sent (or failed to sent) last response chunk */ 312 if (srv->fdidx[fd].rsp_cs == srv->fdidx[fd].rsp_size) { 313 srv->fdidx[fd].rsp_cs = 0; 314 srv->fdidx[fd].rsp_size = 0; 315 srv->fdidx[fd].rsp_limit = 0; 316 } 317 318 return ((size < 0)? errno : 0); 319 } 320