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