src/sdpd-request.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2001-2002  Nokia Corporation
   6  *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
   7  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *  Copyright (C) 2002-2003  Stephen Crane <steve.crane@rococosoft.com>
   9  *
  10  *
  11  *  This program is free software; you can redistribute it and/or modify
  12  *  it under the terms of the GNU General Public License as published by
  13  *  the Free Software Foundation; either version 2 of the License, or
  14  *  (at your option) any later version.
  15  *
  16  *  This program is distributed in the hope that it will be useful,
  17  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  *  GNU General Public License for more details.
  20  *
  21  *  You should have received a copy of the GNU General Public License
  22  *  along with this program; if not, write to the Free Software
  23  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  24  *
  25  */
  26
  27 #ifdef HAVE_CONFIG_H
  28 #include <config.h>
  29 #endif
  30
  31 #include <errno.h>
  32 #include <stdlib.h>
  33 #include <limits.h>
  34 #include <stdbool.h>
  35
  36 #include "lib/bluetooth.h"
  37 #include "lib/l2cap.h"
  38 #include "lib/sdp.h"
  39 #include "lib/sdp_lib.h"
  40
  41 #include "src/shared/util.h"
  42
  43 #include "sdpd.h"
  44 #include "log.h"
  45
  46 typedef struct {
  47         uint32_t timestamp;
  48         union {
  49                 uint16_t maxBytesSent;
  50                 uint16_t lastIndexSent;
  51         } cStateValue;
  52 } sdp_cont_state_t;
  53
  54 #define SDP_CONT_STATE_SIZE (sizeof(uint8_t) + sizeof(sdp_cont_state_t))
  55
  56 #define MIN(x, y) ((x) < (y)) ? (x): (y)
  57
  58 typedef struct _sdp_cstate_list sdp_cstate_list_t;
  59
  60 struct _sdp_cstate_list {
  61         sdp_cstate_list_t *next;
  62         uint32_t timestamp;
  63         sdp_buf_t buf;
  64 };
  65
  66 static sdp_cstate_list_t *cstates;
  67
  68 /* FIXME: should probably remove it when it's found */
  69 static sdp_buf_t *sdp_get_cached_rsp(sdp_cont_state_t *cstate)
  70 {
  71         sdp_cstate_list_t *p;
  72
  73         for (p = cstates; p; p = p->next)
  74                 if (p->timestamp == cstate->timestamp)
  75                         return &p->buf;
  76         return 0;
  77 }
  78
  79 static uint32_t sdp_cstate_alloc_buf(sdp_buf_t *buf)
  80 {
  81         sdp_cstate_list_t *cstate = malloc(sizeof(sdp_cstate_list_t));
  82         uint8_t *data = malloc(buf->data_size);
  83
  84         memcpy(data, buf->data, buf->data_size);
  85         memset((char *)cstate, 0, sizeof(sdp_cstate_list_t));
  86         cstate->buf.data = data;
  87         cstate->buf.data_size = buf->data_size;
  88         cstate->buf.buf_size = buf->data_size;
  89         cstate->timestamp = sdp_get_time();
  90         cstate->next = cstates;
  91         cstates = cstate;
  92         return cstate->timestamp;
  93 }
  94
  95 /* Additional values for checking datatype (not in spec) */
  96 #define SDP_TYPE_UUID   0xfe
  97 #define SDP_TYPE_ATTRID 0xff
  98
  99 struct attrid {
 100         uint8_t dtd;
 101         union {
 102                 uint16_t uint16;
 103                 uint32_t uint32;
 104         };
 105 };
 106
 107 /*
 108  * Generic data element sequence extractor. Builds
 109  * a list whose elements are those found in the
 110  * sequence. The data type of elements found in the
 111  * sequence is returned in the reference pDataType
 112  */
 113 static int extract_des(uint8_t *buf, int len, sdp_list_t **svcReqSeq, uint8_t *pDataType, uint8_t expectedType)
 114 {
 115         uint8_t seqType;
 116         int scanned, data_size = 0;
 117         short numberOfElements = 0;
 118         int seqlen = 0;
 119         sdp_list_t *pSeq = NULL;
 120         uint8_t dataType;
 121         int status = 0;
 122         const uint8_t *p;
 123         size_t bufsize;
 124
 125         scanned = sdp_extract_seqtype(buf, len, &seqType, &data_size);
 126
 127         SDPDBG("Seq type : %d", seqType);
 128         if (!scanned || (seqType != SDP_SEQ8 && seqType != SDP_SEQ16)) {
 129                 error("Unknown seq type");
 130                 return -1;
 131         }
 132         p = buf + scanned;
 133         bufsize = len - scanned;
 134
 135         SDPDBG("Data size : %d", data_size);
 136
 137         for (;;) {
 138                 char *pElem = NULL;
 139                 int localSeqLength = 0;
 140                 uuid_t *puuid;
 141
 142                 if (bufsize < sizeof(uint8_t)) {
 143                         SDPDBG("->Unexpected end of buffer");
 144                         goto failed;
 145                 }
 146
 147                 dataType = *p;
 148
 149                 SDPDBG("Data type: 0x%02x", dataType);
 150
 151                 if (expectedType == SDP_TYPE_UUID) {
 152                         if (dataType != SDP_UUID16 && dataType != SDP_UUID32 && dataType != SDP_UUID128) {
 153                                 SDPDBG("->Unexpected Data type (expected UUID_ANY)");
 154                                 goto failed;
 155                         }
 156                 } else if (expectedType == SDP_TYPE_ATTRID &&
 157                                 (dataType != SDP_UINT16 && dataType != SDP_UINT32)) {
 158                         SDPDBG("->Unexpected Data type (expected 0x%02x or 0x%02x)",
 159                                                                 SDP_UINT16, SDP_UINT32);
 160                         goto failed;
 161                 } else if (expectedType != SDP_TYPE_ATTRID && dataType != expectedType) {
 162                         SDPDBG("->Unexpected Data type (expected 0x%02x)", expectedType);
 163                         goto failed;
 164                 }
 165
 166                 switch (dataType) {
 167                 case SDP_UINT16:
 168                         p += sizeof(uint8_t);
 169                         seqlen += sizeof(uint8_t);
 170                         bufsize -= sizeof(uint8_t);
 171                         if (bufsize < sizeof(uint16_t)) {
 172                                 SDPDBG("->Unexpected end of buffer");
 173                                 goto failed;
 174                         }
 175
 176                         if (expectedType == SDP_TYPE_ATTRID) {
 177                                 struct attrid *aid;
 178                                 aid = malloc(sizeof(struct attrid));
 179                                 aid->dtd = dataType;
 180                                 aid->uint16 = get_be16(p);
 181                                 pElem = (char *) aid;
 182                         } else {
 183                                 uint16_t tmp;
 184
 185                                 memcpy(&tmp, p, sizeof(tmp));
 186
 187                                 pElem = malloc(sizeof(uint16_t));
 188                                 put_be16(tmp, pElem);
 189                         }
 190                         p += sizeof(uint16_t);
 191                         seqlen += sizeof(uint16_t);
 192                         bufsize -= sizeof(uint16_t);
 193                         break;
 194                 case SDP_UINT32:
 195                         p += sizeof(uint8_t);
 196                         seqlen += sizeof(uint8_t);
 197                         bufsize -= sizeof(uint8_t);
 198                         if (bufsize < (int)sizeof(uint32_t)) {
 199                                 SDPDBG("->Unexpected end of buffer");
 200                                 goto failed;
 201                         }
 202
 203                         if (expectedType == SDP_TYPE_ATTRID) {
 204                                 struct attrid *aid;
 205                                 aid = malloc(sizeof(struct attrid));
 206                                 aid->dtd = dataType;
 207                                 aid->uint32 = get_be32(p);
 208
 209                                 pElem = (char *) aid;
 210                         } else {
 211                                 uint32_t tmp;
 212
 213                                 memcpy(&tmp, p, sizeof(tmp));
 214
 215                                 pElem = malloc(sizeof(uint32_t));
 216                                 put_be32(tmp, pElem);
 217                         }
 218                         p += sizeof(uint32_t);
 219                         seqlen += sizeof(uint32_t);
 220                         bufsize -= sizeof(uint32_t);
 221                         break;
 222                 case SDP_UUID16:
 223                 case SDP_UUID32:
 224                 case SDP_UUID128:
 225                         puuid = malloc(sizeof(uuid_t));
 226                         status = sdp_uuid_extract(p, bufsize, puuid, &localSeqLength);
 227                         if (status < 0) {
 228                                 free(puuid);
 229                                 goto failed;
 230                         }
 231
 232                         pElem = (char *) puuid;
 233                         seqlen += localSeqLength;
 234                         p += localSeqLength;
 235                         bufsize -= localSeqLength;
 236                         break;
 237                 default:
 238                         return -1;
 239                 }
 240                 if (status == 0) {
 241                         pSeq = sdp_list_append(pSeq, pElem);
 242                         numberOfElements++;
 243                         SDPDBG("No of elements : %d", numberOfElements);
 244
 245                         if (seqlen == data_size)
 246                                 break;
 247                         else if (seqlen > data_size || seqlen > len)
 248                                 goto failed;
 249                 } else
 250                         free(pElem);
 251         }
 252         *svcReqSeq = pSeq;
 253         scanned += seqlen;
 254         *pDataType = dataType;
 255         return scanned;
 256
 257 failed:
 258         sdp_list_free(pSeq, free);
 259         return -1;
 260 }
 261
 262 static int sdp_set_cstate_pdu(sdp_buf_t *buf, sdp_cont_state_t *cstate)
 263 {
 264         uint8_t *pdata = buf->data + buf->data_size;
 265         int length = 0;
 266
 267         if (cstate) {
 268                 SDPDBG("Non null sdp_cstate_t id : 0x%x", cstate->timestamp);
 269                 *pdata = sizeof(sdp_cont_state_t);
 270                 pdata += sizeof(uint8_t);
 271                 length += sizeof(uint8_t);
 272                 memcpy(pdata, cstate, sizeof(sdp_cont_state_t));
 273                 length += sizeof(sdp_cont_state_t);
 274         } else {
 275                 /* set "null" continuation state */
 276                 *pdata = 0;
 277                 length += sizeof(uint8_t);
 278         }
 279         buf->data_size += length;
 280         return length;
 281 }
 282
 283 static int sdp_cstate_get(uint8_t *buffer, size_t len,
 284                                                 sdp_cont_state_t **cstate)
 285 {
 286         uint8_t cStateSize = *buffer;
 287
 288         SDPDBG("Continuation State size : %d", cStateSize);
 289
 290         if (cStateSize == 0) {
 291                 *cstate = NULL;
 292                 return 0;
 293         }
 294
 295         buffer++;
 296         len--;
 297
 298         if (len < sizeof(sdp_cont_state_t))
 299                 return -EINVAL;
 300
 301         /*
 302          * Check if continuation state exists, if yes attempt
 303          * to get response remainder from cache, else send error
 304          */
 305
 306         *cstate = malloc(sizeof(sdp_cont_state_t));
 307         if (!(*cstate))
 308                 return -ENOMEM;
 309
 310         memcpy(*cstate, buffer, sizeof(sdp_cont_state_t));
 311
 312         SDPDBG("Cstate TS : 0x%x", (*cstate)->timestamp);
 313         SDPDBG("Bytes sent : %d", (*cstate)->cStateValue.maxBytesSent);
 314
 315         return 0;
 316 }
 317
 318 /*
 319  * The matching process is defined as "each and every UUID
 320  * specified in the "search pattern" must be present in the
 321  * "target pattern". Here "search pattern" is the set of UUIDs
 322  * specified by the service discovery client and "target pattern"
 323  * is the set of UUIDs present in a service record.
 324  *
 325  * Return 1 if each and every UUID in the search
 326  * pattern exists in the target pattern, 0 if the
 327  * match succeeds and -1 on error.
 328  */
 329 static int sdp_match_uuid(sdp_list_t *search, sdp_list_t *pattern)
 330 {
 331         /*
 332          * The target is a sorted list, so we need not look
 333          * at all elements to confirm existence of an element
 334          * from the search pattern
 335          */
 336         int patlen = sdp_list_len(pattern);
 337
 338         if (patlen < sdp_list_len(search))
 339                 return -1;
 340         for (; search; search = search->next) {
 341                 uuid_t *uuid128;
 342                 void *data = search->data;
 343                 sdp_list_t *list;
 344                 if (data == NULL)
 345                         return -1;
 346
 347                 /* create 128-bit form of the search UUID */
 348                 uuid128 = sdp_uuid_to_uuid128((uuid_t *)data);
 349                 list = sdp_list_find(pattern, uuid128, sdp_uuid128_cmp);
 350                 bt_free(uuid128);
 351                 if (!list)
 352                         return 0;
 353         }
 354         return 1;
 355 }
 356
 357 /*
 358  * Service search request PDU. This method extracts the search pattern
 359  * (a sequence of UUIDs) and calls the matching function
 360  * to find matching services
 361  */
 362 static int service_search_req(sdp_req_t *req, sdp_buf_t *buf)
 363 {
 364         int status = 0, i, plen, mlen, mtu, scanned;
 365         sdp_list_t *pattern = NULL;
 366         uint16_t expected, actual, rsp_count = 0;
 367         uint8_t dtd;
 368         sdp_cont_state_t *cstate = NULL;
 369         uint8_t *pCacheBuffer = NULL;
 370         int handleSize = 0;
 371         uint32_t cStateId = 0;
 372         uint8_t *pTotalRecordCount, *pCurrentRecordCount;
 373         uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t);
 374         size_t data_left = req->len - sizeof(sdp_pdu_hdr_t);
 375
 376         scanned = extract_des(pdata, data_left, &pattern, &dtd, SDP_TYPE_UUID);
 377
 378         if (scanned == -1) {
 379                 status = SDP_INVALID_SYNTAX;
 380                 goto done;
 381         }
 382         pdata += scanned;
 383         data_left -= scanned;
 384
 385         plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
 386         mlen = scanned + sizeof(uint16_t) + 1;
 387         /* ensure we don't read past buffer */
 388         if (plen < mlen || plen != mlen + *(uint8_t *)(pdata+sizeof(uint16_t))) {
 389                 status = SDP_INVALID_SYNTAX;
 390                 goto done;
 391         }
 392
 393         if (data_left < sizeof(uint16_t)) {
 394                 status = SDP_INVALID_SYNTAX;
 395                 goto done;
 396         }
 397
 398         expected = get_be16(pdata);
 399
 400         SDPDBG("Expected count: %d", expected);
 401         SDPDBG("Bytes scanned : %d", scanned);
 402
 403         pdata += sizeof(uint16_t);
 404         data_left -= sizeof(uint16_t);
 405
 406         /*
 407          * Check if continuation state exists, if yes attempt
 408          * to get rsp remainder from cache, else send error
 409          */
 410         if (sdp_cstate_get(pdata, data_left, &cstate) < 0) {
 411                 status = SDP_INVALID_SYNTAX;
 412                 goto done;
 413         }
 414
 415         mtu = req->mtu - sizeof(sdp_pdu_hdr_t) - sizeof(uint16_t) - sizeof(uint16_t) - SDP_CONT_STATE_SIZE;
 416         actual = MIN(expected, mtu >> 2);
 417
 418         /* make space in the rsp buffer for total and current record counts */
 419         pdata = buf->data;
 420
 421         /* total service record count = 0 */
 422         pTotalRecordCount = pdata;
 423         put_be16(0, pdata);
 424         pdata += sizeof(uint16_t);
 425         buf->data_size += sizeof(uint16_t);
 426
 427         /* current service record count = 0 */
 428         pCurrentRecordCount = pdata;
 429         put_be16(0, pdata);
 430         pdata += sizeof(uint16_t);
 431         buf->data_size += sizeof(uint16_t);
 432
 433         if (cstate == NULL) {
 434                 /* for every record in the DB, do a pattern search */
 435                 sdp_list_t *list = sdp_get_record_list();
 436
 437                 handleSize = 0;
 438                 for (; list && rsp_count < expected; list = list->next) {
 439                         sdp_record_t *rec = list->data;
 440
 441                         SDPDBG("Checking svcRec : 0x%x", rec->handle);
 442
 443                         if (sdp_match_uuid(pattern, rec->pattern) > 0 &&
 444                                         sdp_check_access(rec->handle, &req->device)) {
 445                                 rsp_count++;
 446                                 put_be32(rec->handle, pdata);
 447                                 pdata += sizeof(uint32_t);
 448                                 handleSize += sizeof(uint32_t);
 449                         }
 450                 }
 451
 452                 SDPDBG("Match count: %d", rsp_count);
 453
 454                 buf->data_size += handleSize;
 455                 put_be16(rsp_count, pTotalRecordCount);
 456                 put_be16(rsp_count, pCurrentRecordCount);
 457
 458                 if (rsp_count > actual) {
 459                         /* cache the rsp and generate a continuation state */
 460                         cStateId = sdp_cstate_alloc_buf(buf);
 461                         /*
 462                          * subtract handleSize since we now send only
 463                          * a subset of handles
 464                          */
 465                         buf->data_size -= handleSize;
 466                 } else {
 467                         /* NULL continuation state */
 468                         sdp_set_cstate_pdu(buf, NULL);
 469                 }
 470         }
 471
 472         /* under both the conditions below, the rsp buffer is not built yet */
 473         if (cstate || cStateId > 0) {
 474                 short lastIndex = 0;
 475
 476                 if (cstate) {
 477                         /*
 478                          * Get the previous sdp_cont_state_t and obtain
 479                          * the cached rsp
 480                          */
 481                         sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
 482                         if (pCache) {
 483                                 pCacheBuffer = pCache->data;
 484                                 /* get the rsp_count from the cached buffer */
 485                                 rsp_count = get_be16(pCacheBuffer);
 486
 487                                 /* get index of the last sdp_record_t sent */
 488                                 lastIndex = cstate->cStateValue.lastIndexSent;
 489                         } else {
 490                                 status = SDP_INVALID_CSTATE;
 491                                 goto done;
 492                         }
 493                 } else {
 494                         pCacheBuffer = buf->data;
 495                         lastIndex = 0;
 496                 }
 497
 498                 /*
 499                  * Set the local buffer pointer to after the
 500                  * current record count and increment the cached
 501                  * buffer pointer to beyond the counters
 502                  */
 503                 pdata = pCurrentRecordCount + sizeof(uint16_t);
 504
 505                 /* increment beyond the totalCount and the currentCount */
 506                 pCacheBuffer += 2 * sizeof(uint16_t);
 507
 508                 if (cstate) {
 509                         handleSize = 0;
 510                         for (i = lastIndex; (i - lastIndex) < actual && i < rsp_count; i++) {
 511                                 memcpy(pdata, pCacheBuffer + i * sizeof(uint32_t), sizeof(uint32_t));
 512                                 pdata += sizeof(uint32_t);
 513                                 handleSize += sizeof(uint32_t);
 514                         }
 515                 } else {
 516                         handleSize = actual << 2;
 517                         i = actual;
 518                 }
 519
 520                 buf->data_size += handleSize;
 521                 put_be16(rsp_count, pTotalRecordCount);
 522                 put_be16(i - lastIndex, pCurrentRecordCount);
 523
 524                 if (i == rsp_count) {
 525                         /* set "null" continuationState */
 526                         sdp_set_cstate_pdu(buf, NULL);
 527                 } else {
 528                         /*
 529                          * there's more: set lastIndexSent to
 530                          * the new value and move on
 531                          */
 532                         sdp_cont_state_t newState;
 533
 534                         SDPDBG("Setting non-NULL sdp_cstate_t");
 535
 536                         if (cstate)
 537                                 memcpy(&newState, cstate, sizeof(sdp_cont_state_t));
 538                         else {
 539                                 memset(&newState, 0, sizeof(sdp_cont_state_t));
 540                                 newState.timestamp = cStateId;
 541                         }
 542                         newState.cStateValue.lastIndexSent = i;
 543                         sdp_set_cstate_pdu(buf, &newState);
 544                 }
 545         }
 546
 547 done:
 548         free(cstate);
 549         if (pattern)
 550                 sdp_list_free(pattern, free);
 551
 552         return status;
 553 }
 554
 555 /*
 556  * Extract attribute identifiers from the request PDU.
 557  * Clients could request a subset of attributes (by id)
 558  * from a service record, instead of the whole set. The
 559  * requested identifiers are present in the PDU form of
 560  * the request
 561  */
 562 static int extract_attrs(sdp_record_t *rec, sdp_list_t *seq, sdp_buf_t *buf)
 563 {
 564         sdp_buf_t pdu;
 565
 566         if (!rec)
 567                 return SDP_INVALID_RECORD_HANDLE;
 568
 569         if (seq == NULL) {
 570                 SDPDBG("Attribute sequence is NULL");
 571                 return 0;
 572         }
 573
 574         SDPDBG("Entries in attr seq : %d", sdp_list_len(seq));
 575
 576         sdp_gen_record_pdu(rec, &pdu);
 577
 578         for (; seq; seq = seq->next) {
 579                 struct attrid *aid = seq->data;
 580
 581                 SDPDBG("AttrDataType : %d", aid->dtd);
 582
 583                 if (aid->dtd == SDP_UINT16) {
 584                         uint16_t attr = aid->uint16;
 585                         sdp_data_t *a = sdp_data_get(rec, attr);
 586                         if (a)
 587                                 sdp_append_to_pdu(buf, a);
 588                 } else if (aid->dtd == SDP_UINT32) {
 589                         uint32_t range = aid->uint32;
 590                         uint16_t attr;
 591                         uint16_t low = (0xffff0000 & range) >> 16;
 592                         uint16_t high = 0x0000ffff & range;
 593                         sdp_data_t *data;
 594
 595                         SDPDBG("attr range : 0x%x", range);
 596                         SDPDBG("Low id : 0x%x", low);
 597                         SDPDBG("High id : 0x%x", high);
 598
 599                         if (low == 0x0000 && high == 0xffff && pdu.data_size <= buf->buf_size) {
 600                                 /* copy it */
 601                                 memcpy(buf->data, pdu.data, pdu.data_size);
 602                                 buf->data_size = pdu.data_size;
 603                                 break;
 604                         }
 605                         /* (else) sub-range of attributes */
 606                         for (attr = low; attr < high; attr++) {
 607                                 data = sdp_data_get(rec, attr);
 608                                 if (data)
 609                                         sdp_append_to_pdu(buf, data);
 610                         }
 611                         data = sdp_data_get(rec, high);
 612                         if (data)
 613                                 sdp_append_to_pdu(buf, data);
 614                 } else {
 615                         error("Unexpected data type : 0x%x", aid->dtd);
 616                         error("Expect uint16_t or uint32_t");
 617                         free(pdu.data);
 618                         return SDP_INVALID_SYNTAX;
 619                 }
 620         }
 621
 622         free(pdu.data);
 623
 624         return 0;
 625 }
 626
 627 /*
 628  * A request for the attributes of a service record.
 629  * First check if the service record (specified by
 630  * service record handle) exists, then call the attribute
 631  * streaming function
 632  */
 633 static int service_attr_req(sdp_req_t *req, sdp_buf_t *buf)
 634 {
 635         sdp_cont_state_t *cstate = NULL;
 636         uint8_t *pResponse = NULL;
 637         short cstate_size = 0;
 638         sdp_list_t *seq = NULL;
 639         uint8_t dtd = 0;
 640         int scanned = 0;
 641         unsigned int max_rsp_size;
 642         int status = 0, plen, mlen;
 643         uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t);
 644         size_t data_left = req->len - sizeof(sdp_pdu_hdr_t);
 645         uint32_t handle;
 646
 647         if (data_left < sizeof(uint32_t)) {
 648                 status = SDP_INVALID_SYNTAX;
 649                 goto done;
 650         }
 651
 652         handle = get_be32(pdata);
 653
 654         pdata += sizeof(uint32_t);
 655         data_left -= sizeof(uint32_t);
 656
 657         if (data_left < sizeof(uint16_t)) {
 658                 status = SDP_INVALID_SYNTAX;
 659                 goto done;
 660         }
 661
 662         max_rsp_size = get_be16(pdata);
 663
 664         pdata += sizeof(uint16_t);
 665         data_left -= sizeof(uint16_t);
 666
 667         if (data_left < sizeof(sdp_pdu_hdr_t)) {
 668                 status = SDP_INVALID_SYNTAX;
 669                 goto done;
 670         }
 671
 672         /* extract the attribute list */
 673         scanned = extract_des(pdata, data_left, &seq, &dtd, SDP_TYPE_ATTRID);
 674         if (scanned == -1) {
 675                 status = SDP_INVALID_SYNTAX;
 676                 goto done;
 677         }
 678         pdata += scanned;
 679         data_left -= scanned;
 680
 681         plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
 682         mlen = scanned + sizeof(uint32_t) + sizeof(uint16_t) + 1;
 683         /* ensure we don't read past buffer */
 684         if (plen < mlen || plen != mlen + *(uint8_t *)pdata) {
 685                 status = SDP_INVALID_PDU_SIZE;
 686                 goto done;
 687         }
 688
 689         /*
 690          * if continuation state exists, attempt
 691          * to get rsp remainder from cache, else send error
 692          */
 693         if (sdp_cstate_get(pdata, data_left, &cstate) < 0) {
 694                 status = SDP_INVALID_SYNTAX;
 695                 goto done;
 696         }
 697
 698         SDPDBG("SvcRecHandle : 0x%x", handle);
 699         SDPDBG("max_rsp_size : %d", max_rsp_size);
 700
 701         /*
 702          * Check that max_rsp_size is within valid range
 703          * a minimum size of 0x0007 has to be used for data field
 704          */
 705         if (max_rsp_size < 0x0007) {
 706                 status = SDP_INVALID_SYNTAX;
 707                 goto done;
 708         }
 709
 710         /*
 711          * Calculate Attribute size according to MTU
 712          * We can send only (MTU - sizeof(sdp_pdu_hdr_t) - sizeof(sdp_cont_state_t))
 713          */
 714         max_rsp_size = MIN(max_rsp_size, req->mtu - sizeof(sdp_pdu_hdr_t) -
 715                         sizeof(uint32_t) - SDP_CONT_STATE_SIZE - sizeof(uint16_t));
 716
 717         /* pull header for AttributeList byte count */
 718         buf->data += sizeof(uint16_t);
 719         buf->buf_size -= sizeof(uint16_t);
 720
 721         if (cstate) {
 722                 sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
 723
 724                 SDPDBG("Obtained cached rsp : %p", pCache);
 725
 726                 if (pCache) {
 727                         short sent = MIN(max_rsp_size, pCache->data_size - cstate->cStateValue.maxBytesSent);
 728                         pResponse = pCache->data;
 729                         memcpy(buf->data, pResponse + cstate->cStateValue.maxBytesSent, sent);
 730                         buf->data_size += sent;
 731                         cstate->cStateValue.maxBytesSent += sent;
 732
 733                         SDPDBG("Response size : %d sending now : %d bytes sent so far : %d",
 734                                 pCache->data_size, sent, cstate->cStateValue.maxBytesSent);
 735                         if (cstate->cStateValue.maxBytesSent == pCache->data_size)
 736                                 cstate_size = sdp_set_cstate_pdu(buf, NULL);
 737                         else
 738                                 cstate_size = sdp_set_cstate_pdu(buf, cstate);
 739                 } else {
 740                         status = SDP_INVALID_CSTATE;
 741                         error("NULL cache buffer and non-NULL continuation state");
 742                 }
 743         } else {
 744                 sdp_record_t *rec = sdp_record_find(handle);
 745                 status = extract_attrs(rec, seq, buf);
 746                 if (buf->data_size > max_rsp_size) {
 747                         sdp_cont_state_t newState;
 748
 749                         memset((char *)&newState, 0, sizeof(sdp_cont_state_t));
 750                         newState.timestamp = sdp_cstate_alloc_buf(buf);
 751                         /*
 752                          * Reset the buffer size to the maximum expected and
 753                          * set the sdp_cont_state_t
 754                          */
 755                         SDPDBG("Creating continuation state of size : %d", buf->data_size);
 756                         buf->data_size = max_rsp_size;
 757                         newState.cStateValue.maxBytesSent = max_rsp_size;
 758                         cstate_size = sdp_set_cstate_pdu(buf, &newState);
 759                 } else {
 760                         if (buf->data_size == 0)
 761                                 sdp_append_to_buf(buf, NULL, 0);
 762                         cstate_size = sdp_set_cstate_pdu(buf, NULL);
 763                 }
 764         }
 765
 766         /* push header */
 767         buf->data -= sizeof(uint16_t);
 768         buf->buf_size += sizeof(uint16_t);
 769
 770 done:
 771         free(cstate);
 772         if (seq)
 773                 sdp_list_free(seq, free);
 774         if (status)
 775                 return status;
 776
 777         /* set attribute list byte count */
 778         put_be16(buf->data_size - cstate_size, buf->data);
 779         buf->data_size += sizeof(uint16_t);
 780         return 0;
 781 }
 782
 783 /*
 784  * combined service search and attribute extraction
 785  */
 786 static int service_search_attr_req(sdp_req_t *req, sdp_buf_t *buf)
 787 {
 788         int status = 0, plen, totscanned;
 789         uint8_t *pdata, *pResponse = NULL;
 790         unsigned int max;
 791         int scanned, rsp_count = 0;
 792         sdp_list_t *pattern = NULL, *seq = NULL, *svcList;
 793         sdp_cont_state_t *cstate = NULL;
 794         short cstate_size = 0;
 795         uint8_t dtd = 0;
 796         sdp_buf_t tmpbuf;
 797         size_t data_left;
 798
 799         tmpbuf.data = NULL;
 800         pdata = req->buf + sizeof(sdp_pdu_hdr_t);
 801         data_left = req->len - sizeof(sdp_pdu_hdr_t);
 802         scanned = extract_des(pdata, data_left, &pattern, &dtd, SDP_TYPE_UUID);
 803         if (scanned == -1) {
 804                 status = SDP_INVALID_SYNTAX;
 805                 goto done;
 806         }
 807         totscanned = scanned;
 808
 809         SDPDBG("Bytes scanned: %d", scanned);
 810
 811         pdata += scanned;
 812         data_left -= scanned;
 813
 814         if (data_left < sizeof(uint16_t)) {
 815                 status = SDP_INVALID_SYNTAX;
 816                 goto done;
 817         }
 818
 819         max = get_be16(pdata);
 820
 821         pdata += sizeof(uint16_t);
 822         data_left -= sizeof(uint16_t);
 823
 824         SDPDBG("Max Attr expected: %d", max);
 825
 826         if (data_left < sizeof(sdp_pdu_hdr_t)) {
 827                 status = SDP_INVALID_SYNTAX;
 828                 goto done;
 829         }
 830
 831         /* extract the attribute list */
 832         scanned = extract_des(pdata, data_left, &seq, &dtd, SDP_TYPE_ATTRID);
 833         if (scanned == -1) {
 834                 status = SDP_INVALID_SYNTAX;
 835                 goto done;
 836         }
 837
 838         pdata += scanned;
 839         data_left -= scanned;
 840
 841         totscanned += scanned + sizeof(uint16_t) + 1;
 842
 843         plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
 844         if (plen < totscanned || plen != totscanned + *(uint8_t *)pdata) {
 845                 status = SDP_INVALID_PDU_SIZE;
 846                 goto done;
 847         }
 848
 849         /*
 850          * if continuation state exists attempt
 851          * to get rsp remainder from cache, else send error
 852          */
 853         if (sdp_cstate_get(pdata, data_left, &cstate) < 0) {
 854                 status = SDP_INVALID_SYNTAX;
 855                 goto done;
 856         }
 857
 858         svcList = sdp_get_record_list();
 859
 860         tmpbuf.data = malloc(USHRT_MAX);
 861         tmpbuf.data_size = 0;
 862         tmpbuf.buf_size = USHRT_MAX;
 863         memset(tmpbuf.data, 0, USHRT_MAX);
 864
 865         /*
 866          * Calculate Attribute size according to MTU
 867          * We can send only (MTU - sizeof(sdp_pdu_hdr_t) - sizeof(sdp_cont_state_t))
 868          */
 869         max = MIN(max, req->mtu - sizeof(sdp_pdu_hdr_t) - SDP_CONT_STATE_SIZE - sizeof(uint16_t));
 870
 871         /* pull header for AttributeList byte count */
 872         buf->data += sizeof(uint16_t);
 873         buf->buf_size -= sizeof(uint16_t);
 874
 875         if (cstate == NULL) {
 876                 /* no continuation state -> create new response */
 877                 sdp_list_t *p;
 878                 for (p = svcList; p; p = p->next) {
 879                         sdp_record_t *rec = p->data;
 880                         if (sdp_match_uuid(pattern, rec->pattern) > 0 &&
 881                                         sdp_check_access(rec->handle, &req->device)) {
 882                                 rsp_count++;
 883                                 status = extract_attrs(rec, seq, &tmpbuf);
 884
 885                                 SDPDBG("Response count : %d", rsp_count);
 886                                 SDPDBG("Local PDU size : %d", tmpbuf.data_size);
 887                                 if (status) {
 888                                         SDPDBG("Extract attr from record returns err");
 889                                         break;
 890                                 }
 891                                 if (buf->data_size + tmpbuf.data_size < buf->buf_size) {
 892                                         /* to be sure no relocations */
 893                                         sdp_append_to_buf(buf, tmpbuf.data, tmpbuf.data_size);
 894                                         tmpbuf.data_size = 0;
 895                                         memset(tmpbuf.data, 0, USHRT_MAX);
 896                                 } else {
 897                                         error("Relocation needed");
 898                                         break;
 899                                 }
 900                                 SDPDBG("Net PDU size : %d", buf->data_size);
 901                         }
 902                 }
 903                 if (buf->data_size > max) {
 904                         sdp_cont_state_t newState;
 905
 906                         memset((char *)&newState, 0, sizeof(sdp_cont_state_t));
 907                         newState.timestamp = sdp_cstate_alloc_buf(buf);
 908                         /*
 909                          * Reset the buffer size to the maximum expected and
 910                          * set the sdp_cont_state_t
 911                          */
 912                         buf->data_size = max;
 913                         newState.cStateValue.maxBytesSent = max;
 914                         cstate_size = sdp_set_cstate_pdu(buf, &newState);
 915                 } else
 916                         cstate_size = sdp_set_cstate_pdu(buf, NULL);
 917         } else {
 918                 /* continuation State exists -> get from cache */
 919                 sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
 920                 if (pCache) {
 921                         uint16_t sent = MIN(max, pCache->data_size - cstate->cStateValue.maxBytesSent);
 922                         pResponse = pCache->data;
 923                         memcpy(buf->data, pResponse + cstate->cStateValue.maxBytesSent, sent);
 924                         buf->data_size += sent;
 925                         cstate->cStateValue.maxBytesSent += sent;
 926                         if (cstate->cStateValue.maxBytesSent == pCache->data_size)
 927                                 cstate_size = sdp_set_cstate_pdu(buf, NULL);
 928                         else
 929                                 cstate_size = sdp_set_cstate_pdu(buf, cstate);
 930                 } else {
 931                         status = SDP_INVALID_CSTATE;
 932                         SDPDBG("Non-null continuation state, but null cache buffer");
 933                 }
 934         }
 935
 936         if (!rsp_count && !cstate) {
 937                 /* found nothing */
 938                 buf->data_size = 0;
 939                 sdp_append_to_buf(buf, tmpbuf.data, tmpbuf.data_size);
 940                 sdp_set_cstate_pdu(buf, NULL);
 941         }
 942
 943         /* push header */
 944         buf->data -= sizeof(uint16_t);
 945         buf->buf_size += sizeof(uint16_t);
 946
 947         if (!status) {
 948                 /* set attribute list byte count */
 949                 put_be16(buf->data_size - cstate_size, buf->data);
 950                 buf->data_size += sizeof(uint16_t);
 951         }
 952
 953 done:
 954         free(cstate);
 955         free(tmpbuf.data);
 956         if (pattern)
 957                 sdp_list_free(pattern, free);
 958         if (seq)
 959                 sdp_list_free(seq, free);
 960         return status;
 961 }
 962
 963 /*
 964  * Top level request processor. Calls the appropriate processing
 965  * function based on request type. Handles service registration
 966  * client requests also.
 967  */
 968 static void process_request(sdp_req_t *req)
 969 {
 970         sdp_pdu_hdr_t *reqhdr = (sdp_pdu_hdr_t *)req->buf;
 971         sdp_pdu_hdr_t *rsphdr;
 972         sdp_buf_t rsp;
 973         uint8_t *buf = malloc(USHRT_MAX);
 974         int status = SDP_INVALID_SYNTAX;
 975
 976         memset(buf, 0, USHRT_MAX);
 977         rsp.data = buf + sizeof(sdp_pdu_hdr_t);
 978         rsp.data_size = 0;
 979         rsp.buf_size = USHRT_MAX - sizeof(sdp_pdu_hdr_t);
 980         rsphdr = (sdp_pdu_hdr_t *)buf;
 981
 982         if (ntohs(reqhdr->plen) != req->len - sizeof(sdp_pdu_hdr_t)) {
 983                 status = SDP_INVALID_PDU_SIZE;
 984                 goto send_rsp;
 985         }
 986         switch (reqhdr->pdu_id) {
 987         case SDP_SVC_SEARCH_REQ:
 988                 SDPDBG("Got a svc srch req");
 989                 status = service_search_req(req, &rsp);
 990                 rsphdr->pdu_id = SDP_SVC_SEARCH_RSP;
 991                 break;
 992         case SDP_SVC_ATTR_REQ:
 993                 SDPDBG("Got a svc attr req");
 994                 status = service_attr_req(req, &rsp);
 995                 rsphdr->pdu_id = SDP_SVC_ATTR_RSP;
 996                 break;
 997         case SDP_SVC_SEARCH_ATTR_REQ:
 998                 SDPDBG("Got a svc srch attr req");
 999                 status = service_search_attr_req(req, &rsp);
1000                 rsphdr->pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
1001                 break;
1002         /* Following requests are allowed only for local connections */
1003         case SDP_SVC_REGISTER_REQ:
1004                 SDPDBG("Service register request");
1005                 if (req->local) {
1006                         status = service_register_req(req, &rsp);
1007                         rsphdr->pdu_id = SDP_SVC_REGISTER_RSP;
1008                 }
1009                 break;
1010         case SDP_SVC_UPDATE_REQ:
1011                 SDPDBG("Service update request");
1012                 if (req->local) {
1013                         status = service_update_req(req, &rsp);
1014                         rsphdr->pdu_id = SDP_SVC_UPDATE_RSP;
1015                 }
1016                 break;
1017         case SDP_SVC_REMOVE_REQ:
1018                 SDPDBG("Service removal request");
1019                 if (req->local) {
1020                         status = service_remove_req(req, &rsp);
1021                         rsphdr->pdu_id = SDP_SVC_REMOVE_RSP;
1022                 }
1023                 break;
1024         default:
1025                 error("Unknown PDU ID : 0x%x received", reqhdr->pdu_id);
1026                 status = SDP_INVALID_SYNTAX;
1027                 break;
1028         }
1029
1030 send_rsp:
1031         if (status) {
1032                 rsphdr->pdu_id = SDP_ERROR_RSP;
1033                 put_be16(status, rsp.data);
1034                 rsp.data_size = sizeof(uint16_t);
1035         }
1036
1037         SDPDBG("Sending rsp. status %d", status);
1038
1039         rsphdr->tid  = reqhdr->tid;
1040         rsphdr->plen = htons(rsp.data_size);
1041
1042         /* point back to the real buffer start and set the real rsp length */
1043         rsp.data_size += sizeof(sdp_pdu_hdr_t);
1044         rsp.data = buf;
1045
1046         /* stream the rsp PDU */
1047         if (send(req->sock, rsp.data, rsp.data_size, 0) < 0)
1048                 error("send: %s (%d)", strerror(errno), errno);
1049
1050         SDPDBG("Bytes Sent : %d", rsp.data_size);
1051
1052         free(rsp.data);
1053         free(req->buf);
1054 }
1055
1056 void handle_internal_request(int sk, int mtu, void *data, int len)
1057 {
1058         sdp_req_t req;
1059
1060         bacpy(&req.device, BDADDR_ANY);
1061         bacpy(&req.bdaddr, BDADDR_LOCAL);
1062         req.local = 0;
1063         req.sock = sk;
1064         req.mtu = mtu;
1065         req.flags = 0;
1066         req.buf = data;
1067         req.len = len;
1068
1069         process_request(&req);
1070 }
1071
1072 void handle_request(int sk, uint8_t *data, int len)
1073 {
1074         struct sockaddr_l2 sa;
1075         socklen_t size;
1076         sdp_req_t req;
1077
1078         size = sizeof(sa);
1079         if (getpeername(sk, (struct sockaddr *) &sa, &size) < 0) {
1080                 error("getpeername: %s", strerror(errno));
1081                 return;
1082         }
1083
1084         if (sa.l2_family == AF_BLUETOOTH) {
1085                 struct l2cap_options lo;
1086
1087                 memset(&lo, 0, sizeof(lo));
1088                 size = sizeof(lo);
1089
1090                 if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &lo, &size) < 0) {
1091                         error("getsockopt: %s", strerror(errno));
1092                         return;
1093                 }
1094
1095                 bacpy(&req.bdaddr, &sa.l2_bdaddr);
1096                 req.mtu = lo.omtu;
1097                 req.local = 0;
1098                 memset(&sa, 0, sizeof(sa));
1099                 size = sizeof(sa);
1100
1101                 if (getsockname(sk, (struct sockaddr *) &sa, &size) < 0) {
1102                         error("getsockname: %s", strerror(errno));
1103                         return;
1104                 }
1105
1106                 bacpy(&req.device, &sa.l2_bdaddr);
1107         } else {
1108                 bacpy(&req.device, BDADDR_ANY);
1109                 bacpy(&req.bdaddr, BDADDR_LOCAL);
1110                 req.mtu = 2048;
1111                 req.local = 1;
1112         }
1113
1114         req.sock = sk;
1115         req.buf  = data;
1116         req.len  = len;
1117
1118         process_request(&req);
1119 }