From 41bb0e8fd2568243020852e22a6d176bccfa60cd Mon Sep 17 00:00:00 2001 From: emax Date: Sun, 12 Oct 2003 22:04:24 +0000 Subject: Update Bluetooth code. Reviewed by: M. Warner Losh ; John Hay Approved by: M. Warner Losh (mentor) --- lib/libsdp/search.c | 383 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 383 insertions(+) create mode 100644 lib/libsdp/search.c (limited to 'lib/libsdp/search.c') diff --git a/lib/libsdp/search.c b/lib/libsdp/search.c new file mode 100644 index 0000000..ab495e3 --- /dev/null +++ b/lib/libsdp/search.c @@ -0,0 +1,383 @@ +/* + * search.c + * + * Copyright (c) 2001-2003 Maksim Yevmenkin + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $Id: search.c,v 1.2 2003/09/04 22:12:13 max Exp $ + * $FreeBSD$ + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +int32_t +sdp_search(void *xss, + u_int32_t plen, u_int16_t const *pp, + u_int32_t alen, u_int32_t const *ap, + u_int32_t vlen, sdp_attr_t *vp) +{ + struct sdp_xpdu { + sdp_pdu_t pdu; + u_int16_t len; + } __attribute__ ((packed)) xpdu; + + sdp_session_p ss = (sdp_session_p) xss; + u_int8_t *req = NULL, *rsp = NULL, *rsp_tmp = NULL; + int32_t type, len; + + if (ss == NULL) + return (-1); + + if (ss->req == NULL || ss->rsp == NULL || + plen == 0 || pp == NULL || alen == 0 || ap == NULL) { + ss->error = EINVAL; + return (-1); + } + + /* Calculate length of the request */ + req = ss->req; + plen = plen * (sizeof(pp[0]) + 1); + alen = alen * (sizeof(ap[0]) + 1); + + len = plen + sizeof(u_int8_t) + sizeof(u_int16_t) + + /* ServiceSearchPattern */ + sizeof(u_int16_t) + + /* MaximumAttributeByteCount */ + alen + sizeof(u_int8_t) + sizeof(u_int16_t); + /* AttributeIDList */ + + if (ss->req_e - req < len) { + ss->error = ENOBUFS; + return (-1); + } + + /* Put ServiceSearchPattern */ + SDP_PUT8(SDP_DATA_SEQ16, req); + SDP_PUT16(plen, req); + for (; plen > 0; pp ++, plen -= (sizeof(pp[0]) + 1)) { + SDP_PUT8(SDP_DATA_UUID16, req); + SDP_PUT16(*pp, req); + } + + /* Put MaximumAttributeByteCount */ + SDP_PUT16(0xffff, req); + + /* Put AttributeIDList */ + SDP_PUT8(SDP_DATA_SEQ16, req); + SDP_PUT16(alen, req); + for (; alen > 0; ap ++, alen -= (sizeof(ap[0]) + 1)) { + SDP_PUT8(SDP_DATA_UINT32, req); + SDP_PUT32(*ap, req); + } + + /* Submit ServiceSearchAttributeRequest and wait for response */ + ss->cslen = 0; + rsp = ss->rsp; + + do { + struct iovec iov[2]; + u_int8_t *req_cs = req; + + /* Add continuation state (if any) */ + if (ss->req_e - req_cs < ss->cslen + 1) { + ss->error = ENOBUFS; + return (-1); + } + + SDP_PUT8(ss->cslen, req_cs); + if (ss->cslen > 0) { + memcpy(req_cs, ss->cs, ss->cslen); + req_cs += ss->cslen; + } + + /* Prepare SDP PDU header */ + xpdu.pdu.pid = SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST; + xpdu.pdu.tid = htons(ss->tid); + xpdu.pdu.len = htons(req_cs - ss->req); + + /* Submit request */ + iov[0].iov_base = (void *) &xpdu; + iov[0].iov_len = sizeof(xpdu.pdu); + iov[1].iov_base = (void *) ss->req; + iov[1].iov_len = req_cs - ss->req; + + len = writev(ss->s, iov, sizeof(iov)/sizeof(iov[0])); + if (len < 0) { + ss->error = errno; + return (-1); + } + + /* Read response */ + iov[0].iov_base = (void *) &xpdu; + iov[0].iov_len = sizeof(xpdu); + iov[1].iov_base = (void *) rsp; + iov[1].iov_len = ss->imtu; + + len = readv(ss->s, iov, sizeof(iov)/sizeof(iov[0])); + if (len < 0) { + ss->error = errno; + return (-1); + } + if (len < sizeof(xpdu)) { + ss->error = ENOMSG; + return (-1); + } + + xpdu.pdu.tid = ntohs(xpdu.pdu.tid); + xpdu.pdu.len = ntohs(xpdu.pdu.len); + xpdu.len = ntohs(xpdu.len); + + if (xpdu.pdu.pid == SDP_PDU_ERROR_RESPONSE || + xpdu.pdu.tid != ss->tid || + xpdu.len > xpdu.pdu.len) { + ss->error = EIO; + return (-1); + } + + /* Save continuation state (if any) */ + ss->cslen = rsp[xpdu.len]; + if (ss->cslen > 0) { + if (ss->cslen > sizeof(ss->cs)) { + ss->error = ENOBUFS; + return (-1); + } + + memcpy(ss->cs, rsp + xpdu.len + 1, ss->cslen); + + /* + * Ensure that we always have ss->imtu bytes + * available in the ss->rsp buffer + */ + + if (ss->rsp_e - rsp <= ss->imtu) { + u_int32_t size, offset; + + size = ss->rsp_e - ss->rsp + ss->imtu; + offset = rsp - ss->rsp; + + rsp_tmp = realloc(ss->rsp, size); + if (rsp_tmp == NULL) { + ss->error = ENOMEM; + return (-1); + } + + ss->rsp = rsp_tmp; + ss->rsp_e = ss->rsp + size; + rsp = ss->rsp + offset; + } + } + + rsp += xpdu.len; + ss->tid ++; + } while (ss->cslen > 0); + + /* + * If we got here then we have completed SDP transaction and now + * we must populate attribute values into vp array. At this point + * ss->rsp points to the beginning of the response and rsp points + * to the end of the response. + * + * From Bluetooth v1.1 spec page 364 + * + * The AttributeLists is a data element sequence where each element + * in turn is a data element sequence representing an attribute list. + * Each attribute list contains attribute IDs and attribute values + * from one service record. The first element in each attribute list + * contains the attribute ID of the first attribute to be returned for + * that service record. The second element in each attribute list + * contains the corresponding attribute value. Successive pairs of + * elements in each attribute list contain additional attribute ID + * and value pairs. Only attributes that have non-null values within + * the service record and whose attribute IDs were specified in the + * SDP_ServiceSearchAttributeRequest are contained in the AttributeLists + * Neither an attribute ID nor attribute value is placed in + * AttributeLists for attributes in the service record that have no + * value. Within each attribute list, the attributes are listed in + * ascending order of attribute ID value. + */ + + if (vp == NULL) + goto done; + + rsp_tmp = ss->rsp; + + /* Skip the first SEQ */ + SDP_GET8(type, rsp_tmp); + switch (type) { + case SDP_DATA_SEQ8: + SDP_GET8(len, rsp_tmp); + break; + + case SDP_DATA_SEQ16: + SDP_GET16(len, rsp_tmp); + break; + + case SDP_DATA_SEQ32: + SDP_GET32(len, rsp_tmp); + break; + + default: + ss->error = ENOATTR; + return (-1); + /* NOT REACHED */ + } + + for (; rsp_tmp < rsp && vlen > 0; ) { + /* Get set of attributes for the next record */ + SDP_GET8(type, rsp_tmp); + switch (type) { + case SDP_DATA_SEQ8: + SDP_GET8(len, rsp_tmp); + break; + + case SDP_DATA_SEQ16: + SDP_GET16(len, rsp_tmp); + break; + + case SDP_DATA_SEQ32: + SDP_GET32(len, rsp_tmp); + break; + + default: + ss->error = ENOATTR; + return (-1); + /* NOT REACHED */ + } + + /* Now rsp_tmp points to list of (attr,value) pairs */ + for (; len > 0 && vlen > 0; vp ++, vlen --) { + /* Attribute */ + SDP_GET8(type, rsp_tmp); + if (type != SDP_DATA_UINT16) { + ss->error = ENOATTR; + return (-1); + } + SDP_GET16(vp->attr, rsp_tmp); + + /* Attribute value */ + switch (rsp_tmp[0]) { + case SDP_DATA_NIL: + alen = 0; + break; + + case SDP_DATA_UINT8: + case SDP_DATA_INT8: + case SDP_DATA_BOOL: + alen = sizeof(u_int8_t); + break; + + case SDP_DATA_UINT16: + case SDP_DATA_INT16: + case SDP_DATA_UUID16: + alen = sizeof(u_int16_t); + break; + + case SDP_DATA_UINT32: + case SDP_DATA_INT32: + case SDP_DATA_UUID32: + alen = sizeof(u_int32_t); + break; + + case SDP_DATA_UINT64: + case SDP_DATA_INT64: + alen = sizeof(u_int64_t); + break; + + case SDP_DATA_UINT128: + case SDP_DATA_INT128: + case SDP_DATA_UUID128: + alen = sizeof(u_int128_t); + break; + + case SDP_DATA_STR8: + case SDP_DATA_URL8: + case SDP_DATA_SEQ8: + case SDP_DATA_ALT8: + alen = rsp_tmp[1] + sizeof(u_int8_t); + break; + + case SDP_DATA_STR16: + case SDP_DATA_URL16: + case SDP_DATA_SEQ16: + case SDP_DATA_ALT16: + alen = ((u_int16_t)rsp_tmp[1] << 8) + | ((u_int16_t)rsp_tmp[2]); + alen += sizeof(u_int16_t); + break; + + case SDP_DATA_STR32: + case SDP_DATA_URL32: + case SDP_DATA_SEQ32: + case SDP_DATA_ALT32: + alen = ((u_int32_t)rsp_tmp[1] << 24) + | ((u_int32_t)rsp_tmp[2] << 16) + | ((u_int32_t)rsp_tmp[3] << 8) + | ((u_int32_t)rsp_tmp[4]); + alen += sizeof(u_int32_t); + break; + + default: + ss->error = ENOATTR; + return (-1); + /* NOT REACHED */ + } + + alen += sizeof(u_int8_t); + + if (vp->value != NULL) { + if (alen <= vp->vlen) { + vp->flags = SDP_ATTR_OK; + vp->vlen = alen; + } else + vp->flags = SDP_ATTR_TRUNCATED; + + memcpy(vp->value, rsp_tmp, vp->vlen); + } else + vp->flags = SDP_ATTR_INVALID; + + len -= ( + sizeof(u_int8_t) + sizeof(u_int16_t) + + alen + ); + + rsp_tmp += alen; + } + } +done: + ss->error = 0; + + return (0); +} + -- cgit v1.1