qemu/hw/bt/sdp.c
<<
>>
Prefs
   1/*
   2 * Service Discover Protocol server for QEMU L2CAP devices
   3 *
   4 * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
   5 *
   6 * This program is free software; you can redistribute it and/or
   7 * modify it under the terms of the GNU General Public License as
   8 * published by the Free Software Foundation; either version 2 of
   9 * the License, or (at your option) any later version.
  10 *
  11 * This program is distributed in the hope that it will be useful,
  12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 * GNU General Public License for more details.
  15 *
  16 * You should have received a copy of the GNU General Public License along
  17 * with this program; if not, see <http://www.gnu.org/licenses/>.
  18 */
  19
  20#include "qemu/osdep.h"
  21#include "qemu-common.h"
  22#include "qemu/host-utils.h"
  23#include "hw/bt.h"
  24
  25struct bt_l2cap_sdp_state_s {
  26    struct bt_l2cap_conn_params_s *channel;
  27
  28    struct sdp_service_record_s {
  29        int match;
  30
  31        int *uuid;
  32        int uuids;
  33        struct sdp_service_attribute_s {
  34            int match;
  35
  36            int attribute_id;
  37            int len;
  38            void *pair;
  39        } *attribute_list;
  40        int attributes;
  41    } *service_list;
  42    int services;
  43};
  44
  45static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left)
  46{
  47    uint32_t len = *(*element) ++ & SDP_DSIZE_MASK;
  48
  49    if (!*left)
  50        return -1;
  51    (*left) --;
  52
  53    if (len < SDP_DSIZE_NEXT1)
  54        return 1 << len;
  55    else if (len == SDP_DSIZE_NEXT1) {
  56        if (*left < 1)
  57            return -1;
  58        (*left) --;
  59
  60        return *(*element) ++;
  61    } else if (len == SDP_DSIZE_NEXT2) {
  62        if (*left < 2)
  63            return -1;
  64        (*left) -= 2;
  65
  66        len = (*(*element) ++) << 8;
  67        return len | (*(*element) ++);
  68    } else {
  69        if (*left < 4)
  70            return -1;
  71        (*left) -= 4;
  72
  73        len = (*(*element) ++) << 24;
  74        len |= (*(*element) ++) << 16;
  75        len |= (*(*element) ++) << 8;
  76        return len | (*(*element) ++);
  77    }
  78}
  79
  80static const uint8_t bt_base_uuid[12] = {
  81    0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
  82};
  83
  84static int sdp_uuid_match(struct sdp_service_record_s *record,
  85                const uint8_t *uuid, ssize_t datalen)
  86{
  87    int *lo, hi, val;
  88
  89    if (datalen == 16 || datalen == 4) {
  90        if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12))
  91            return 0;
  92
  93        if (uuid[0] | uuid[1])
  94            return 0;
  95        uuid += 2;
  96    }
  97
  98    val = (uuid[0] << 8) | uuid[1];
  99    lo = record->uuid;
 100    hi = record->uuids;
 101    while (hi >>= 1)
 102        if (lo[hi] <= val)
 103            lo += hi;
 104
 105    return *lo == val;
 106}
 107
 108#define CONTINUATION_PARAM_SIZE (1 + sizeof(int))
 109#define MAX_PDU_OUT_SIZE        96      /* Arbitrary */
 110#define PDU_HEADER_SIZE         5
 111#define MAX_RSP_PARAM_SIZE      (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \
 112                CONTINUATION_PARAM_SIZE)
 113
 114static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp,
 115                const uint8_t **req, ssize_t *len)
 116{
 117    size_t datalen;
 118    int i;
 119
 120    if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)
 121        return 1;
 122
 123    datalen = sdp_datalen(req, len);
 124    if (datalen != 2 && datalen != 4 && datalen != 16)
 125        return 1;
 126
 127    for (i = 0; i < sdp->services; i ++)
 128        if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))
 129            sdp->service_list[i].match = 1;
 130
 131    (*req) += datalen;
 132    (*len) -= datalen;
 133
 134    return 0;
 135}
 136
 137static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp,
 138                uint8_t *rsp, const uint8_t *req, ssize_t len)
 139{
 140    ssize_t seqlen;
 141    int i, count, start, end, max;
 142    int32_t handle;
 143
 144    /* Perform the search */
 145    for (i = 0; i < sdp->services; i ++)
 146        sdp->service_list[i].match = 0;
 147
 148    if (len < 1)
 149        return -SDP_INVALID_SYNTAX;
 150    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 151        seqlen = sdp_datalen(&req, &len);
 152        if (seqlen < 3 || len < seqlen)
 153            return -SDP_INVALID_SYNTAX;
 154        len -= seqlen;
 155        while (seqlen)
 156            if (sdp_svc_match(sdp, &req, &seqlen))
 157                return -SDP_INVALID_SYNTAX;
 158    } else {
 159        if (sdp_svc_match(sdp, &req, &len)) {
 160            return -SDP_INVALID_SYNTAX;
 161        }
 162    }
 163
 164    if (len < 3)
 165        return -SDP_INVALID_SYNTAX;
 166    max = (req[0] << 8) | req[1];
 167    req += 2;
 168    len -= 2;
 169
 170    if (*req) {
 171        if (len <= sizeof(int))
 172            return -SDP_INVALID_SYNTAX;
 173        len -= sizeof(int);
 174        memcpy(&start, req + 1, sizeof(int));
 175    } else
 176        start = 0;
 177
 178    if (len > 1)
 179        return -SDP_INVALID_SYNTAX;
 180
 181    /* Output the results */
 182    len = 4;
 183    count = 0;
 184    end = start;
 185    for (i = 0; i < sdp->services; i ++)
 186        if (sdp->service_list[i].match) {
 187            if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) {
 188                handle = i;
 189                memcpy(rsp + len, &handle, 4);
 190                len += 4;
 191                end = count + 1;
 192            }
 193
 194            count ++;
 195        }
 196
 197    rsp[0] = count >> 8;
 198    rsp[1] = count & 0xff;
 199    rsp[2] = (end - start) >> 8;
 200    rsp[3] = (end - start) & 0xff;
 201
 202    if (end < count) {
 203        rsp[len ++] = sizeof(int);
 204        memcpy(rsp + len, &end, sizeof(int));
 205        len += 4;
 206    } else
 207        rsp[len ++] = 0;
 208
 209    return len;
 210}
 211
 212static int sdp_attr_match(struct sdp_service_record_s *record,
 213                const uint8_t **req, ssize_t *len)
 214{
 215    int i, start, end;
 216
 217    if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
 218        (*req) ++;
 219        if (*len < 3)
 220            return 1;
 221
 222        start = (*(*req) ++) << 8;
 223        start |= *(*req) ++;
 224        end = start;
 225        *len -= 3;
 226    } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
 227        (*req) ++;
 228        if (*len < 5)
 229            return 1;
 230
 231        start = (*(*req) ++) << 8;
 232        start |= *(*req) ++;
 233        end = (*(*req) ++) << 8;
 234        end |= *(*req) ++;
 235        *len -= 5;
 236    } else
 237        return 1;
 238
 239    for (i = 0; i < record->attributes; i ++)
 240        if (record->attribute_list[i].attribute_id >= start &&
 241                        record->attribute_list[i].attribute_id <= end)
 242            record->attribute_list[i].match = 1;
 243
 244    return 0;
 245}
 246
 247static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp,
 248                uint8_t *rsp, const uint8_t *req, ssize_t len)
 249{
 250    ssize_t seqlen;
 251    int i, start, end, max;
 252    int32_t handle;
 253    struct sdp_service_record_s *record;
 254    uint8_t *lst;
 255
 256    /* Perform the search */
 257    if (len < 7)
 258        return -SDP_INVALID_SYNTAX;
 259    memcpy(&handle, req, 4);
 260    req += 4;
 261    len -= 4;
 262
 263    if (handle < 0 || handle > sdp->services)
 264        return -SDP_INVALID_RECORD_HANDLE;
 265    record = &sdp->service_list[handle];
 266
 267    for (i = 0; i < record->attributes; i ++)
 268        record->attribute_list[i].match = 0;
 269
 270    max = (req[0] << 8) | req[1];
 271    req += 2;
 272    len -= 2;
 273    if (max < 0x0007)
 274        return -SDP_INVALID_SYNTAX;
 275
 276    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 277        seqlen = sdp_datalen(&req, &len);
 278        if (seqlen < 3 || len < seqlen)
 279            return -SDP_INVALID_SYNTAX;
 280        len -= seqlen;
 281
 282        while (seqlen)
 283            if (sdp_attr_match(record, &req, &seqlen))
 284                return -SDP_INVALID_SYNTAX;
 285    } else {
 286        if (sdp_attr_match(record, &req, &len)) {
 287            return -SDP_INVALID_SYNTAX;
 288        }
 289    }
 290
 291    if (len < 1)
 292        return -SDP_INVALID_SYNTAX;
 293
 294    if (*req) {
 295        if (len <= sizeof(int))
 296            return -SDP_INVALID_SYNTAX;
 297        len -= sizeof(int);
 298        memcpy(&start, req + 1, sizeof(int));
 299    } else
 300        start = 0;
 301
 302    if (len > 1)
 303        return -SDP_INVALID_SYNTAX;
 304
 305    /* Output the results */
 306    lst = rsp + 2;
 307    max = MIN(max, MAX_RSP_PARAM_SIZE);
 308    len = 3 - start;
 309    end = 0;
 310    for (i = 0; i < record->attributes; i ++)
 311        if (record->attribute_list[i].match) {
 312            if (len >= 0 && len + record->attribute_list[i].len < max) {
 313                memcpy(lst + len, record->attribute_list[i].pair,
 314                                record->attribute_list[i].len);
 315                end = len + record->attribute_list[i].len;
 316            }
 317            len += record->attribute_list[i].len;
 318        }
 319    if (0 >= start) {
 320       lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
 321       lst[1] = (len + start - 3) >> 8;
 322       lst[2] = (len + start - 3) & 0xff;
 323    }
 324
 325    rsp[0] = end >> 8;
 326    rsp[1] = end & 0xff;
 327
 328    if (end < len) {
 329        len = end + start;
 330        lst[end ++] = sizeof(int);
 331        memcpy(lst + end, &len, sizeof(int));
 332        end += sizeof(int);
 333    } else
 334        lst[end ++] = 0;
 335
 336    return end + 2;
 337}
 338
 339static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp,
 340                const uint8_t **req, ssize_t *len)
 341{
 342    int i, j, start, end;
 343    struct sdp_service_record_s *record;
 344
 345    if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
 346        (*req) ++;
 347        if (*len < 3)
 348            return 1;
 349
 350        start = (*(*req) ++) << 8;
 351        start |= *(*req) ++;
 352        end = start;
 353        *len -= 3;
 354    } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
 355        (*req) ++;
 356        if (*len < 5)
 357            return 1;
 358
 359        start = (*(*req) ++) << 8;
 360        start |= *(*req) ++;
 361        end = (*(*req) ++) << 8;
 362        end |= *(*req) ++;
 363        *len -= 5;
 364    } else
 365        return 1;
 366
 367    for (i = 0; i < sdp->services; i ++)
 368        if ((record = &sdp->service_list[i])->match)
 369            for (j = 0; j < record->attributes; j ++)
 370                if (record->attribute_list[j].attribute_id >= start &&
 371                                record->attribute_list[j].attribute_id <= end)
 372                    record->attribute_list[j].match = 1;
 373
 374    return 0;
 375}
 376
 377static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp,
 378                uint8_t *rsp, const uint8_t *req, ssize_t len)
 379{
 380    ssize_t seqlen;
 381    int i, j, start, end, max;
 382    struct sdp_service_record_s *record;
 383    uint8_t *lst;
 384
 385    /* Perform the search */
 386    for (i = 0; i < sdp->services; i ++) {
 387        sdp->service_list[i].match = 0;
 388            for (j = 0; j < sdp->service_list[i].attributes; j ++)
 389                sdp->service_list[i].attribute_list[j].match = 0;
 390    }
 391
 392    if (len < 1)
 393        return -SDP_INVALID_SYNTAX;
 394    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 395        seqlen = sdp_datalen(&req, &len);
 396        if (seqlen < 3 || len < seqlen)
 397            return -SDP_INVALID_SYNTAX;
 398        len -= seqlen;
 399
 400        while (seqlen)
 401            if (sdp_svc_match(sdp, &req, &seqlen))
 402                return -SDP_INVALID_SYNTAX;
 403    } else {
 404        if (sdp_svc_match(sdp, &req, &len)) {
 405            return -SDP_INVALID_SYNTAX;
 406        }
 407    }
 408
 409    if (len < 3)
 410        return -SDP_INVALID_SYNTAX;
 411    max = (req[0] << 8) | req[1];
 412    req += 2;
 413    len -= 2;
 414    if (max < 0x0007)
 415        return -SDP_INVALID_SYNTAX;
 416
 417    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 418        seqlen = sdp_datalen(&req, &len);
 419        if (seqlen < 3 || len < seqlen)
 420            return -SDP_INVALID_SYNTAX;
 421        len -= seqlen;
 422
 423        while (seqlen)
 424            if (sdp_svc_attr_match(sdp, &req, &seqlen))
 425                return -SDP_INVALID_SYNTAX;
 426    } else {
 427        if (sdp_svc_attr_match(sdp, &req, &len)) {
 428            return -SDP_INVALID_SYNTAX;
 429        }
 430    }
 431
 432    if (len < 1)
 433        return -SDP_INVALID_SYNTAX;
 434
 435    if (*req) {
 436        if (len <= sizeof(int))
 437            return -SDP_INVALID_SYNTAX;
 438        len -= sizeof(int);
 439        memcpy(&start, req + 1, sizeof(int));
 440    } else
 441        start = 0;
 442
 443    if (len > 1)
 444        return -SDP_INVALID_SYNTAX;
 445
 446    /* Output the results */
 447    /* This assumes empty attribute lists are never to be returned even
 448     * for matching Service Records.  In practice this shouldn't happen
 449     * as the requestor will usually include the always present
 450     * ServiceRecordHandle AttributeID in AttributeIDList.  */
 451    lst = rsp + 2;
 452    max = MIN(max, MAX_RSP_PARAM_SIZE);
 453    len = 3 - start;
 454    end = 0;
 455    for (i = 0; i < sdp->services; i ++)
 456        if ((record = &sdp->service_list[i])->match) {
 457            len += 3;
 458            seqlen = len;
 459            for (j = 0; j < record->attributes; j ++)
 460                if (record->attribute_list[j].match) {
 461                    if (len >= 0)
 462                        if (len + record->attribute_list[j].len < max) {
 463                            memcpy(lst + len, record->attribute_list[j].pair,
 464                                            record->attribute_list[j].len);
 465                            end = len + record->attribute_list[j].len;
 466                        }
 467                    len += record->attribute_list[j].len;
 468                }
 469            if (seqlen == len)
 470                len -= 3;
 471            else if (seqlen >= 3 && seqlen < max) {
 472                lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
 473                lst[seqlen - 2] = (len - seqlen) >> 8;
 474                lst[seqlen - 1] = (len - seqlen) & 0xff;
 475            }
 476        }
 477    if (len == 3 - start)
 478        len -= 3;
 479    else if (0 >= start) {
 480       lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
 481       lst[1] = (len + start - 3) >> 8;
 482       lst[2] = (len + start - 3) & 0xff;
 483    }
 484
 485    rsp[0] = end >> 8;
 486    rsp[1] = end & 0xff;
 487
 488    if (end < len) {
 489        len = end + start;
 490        lst[end ++] = sizeof(int);
 491        memcpy(lst + end, &len, sizeof(int));
 492        end += sizeof(int);
 493    } else
 494        lst[end ++] = 0;
 495
 496    return end + 2;
 497}
 498
 499static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len)
 500{
 501    struct bt_l2cap_sdp_state_s *sdp = opaque;
 502    enum bt_sdp_cmd pdu_id;
 503    uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out;
 504    int transaction_id, plen;
 505    int err = 0;
 506    int rsp_len = 0;
 507
 508    if (len < 5) {
 509        fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);
 510        return;
 511    }
 512
 513    pdu_id = *data ++;
 514    transaction_id = (data[0] << 8) | data[1];
 515    plen = (data[2] << 8) | data[3];
 516    data += 4;
 517    len -= 5;
 518
 519    if (len != plen) {
 520        fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",
 521                        __FUNCTION__, plen, len);
 522        err = SDP_INVALID_PDU_SIZE;
 523        goto respond;
 524    }
 525
 526    switch (pdu_id) {
 527    case SDP_SVC_SEARCH_REQ:
 528        rsp_len = sdp_svc_search(sdp, rsp, data, len);
 529        pdu_id = SDP_SVC_SEARCH_RSP;
 530        break;
 531
 532    case SDP_SVC_ATTR_REQ:
 533        rsp_len = sdp_attr_get(sdp, rsp, data, len);
 534        pdu_id = SDP_SVC_ATTR_RSP;
 535        break;
 536
 537    case SDP_SVC_SEARCH_ATTR_REQ:
 538        rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len);
 539        pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
 540        break;
 541
 542    case SDP_ERROR_RSP:
 543    case SDP_SVC_ATTR_RSP:
 544    case SDP_SVC_SEARCH_RSP:
 545    case SDP_SVC_SEARCH_ATTR_RSP:
 546    default:
 547        fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n",
 548                        __FUNCTION__, pdu_id);
 549        err = SDP_INVALID_SYNTAX;
 550        break;
 551    }
 552
 553    if (rsp_len < 0) {
 554        err = -rsp_len;
 555        rsp_len = 0;
 556    }
 557
 558respond:
 559    if (err) {
 560        pdu_id = SDP_ERROR_RSP;
 561        rsp[rsp_len ++] = err >> 8;
 562        rsp[rsp_len ++] = err & 0xff;
 563    }
 564
 565    sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE);
 566
 567    sdu_out[0] = pdu_id;
 568    sdu_out[1] = transaction_id >> 8;
 569    sdu_out[2] = transaction_id & 0xff;
 570    sdu_out[3] = rsp_len >> 8;
 571    sdu_out[4] = rsp_len & 0xff;
 572    memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len);
 573
 574    sdp->channel->sdu_submit(sdp->channel);
 575}
 576
 577static void bt_l2cap_sdp_close_ch(void *opaque)
 578{
 579    struct bt_l2cap_sdp_state_s *sdp = opaque;
 580    int i;
 581
 582    for (i = 0; i < sdp->services; i ++) {
 583        g_free(sdp->service_list[i].attribute_list[0].pair);
 584        g_free(sdp->service_list[i].attribute_list);
 585        g_free(sdp->service_list[i].uuid);
 586    }
 587    g_free(sdp->service_list);
 588    g_free(sdp);
 589}
 590
 591struct sdp_def_service_s {
 592    uint16_t class_uuid;
 593    struct sdp_def_attribute_s {
 594        uint16_t id;
 595        struct sdp_def_data_element_s {
 596            uint8_t type;
 597            union {
 598                uint32_t uint;
 599                const char *str;
 600                struct sdp_def_data_element_s *list;
 601            } value;
 602        } data;
 603    } attributes[];
 604};
 605
 606/* Calculate a safe byte count to allocate that will store the given
 607 * element, at the same time count elements of a UUID type.  */
 608static int sdp_attr_max_size(struct sdp_def_data_element_s *element,
 609                int *uuids)
 610{
 611    int type = element->type & ~SDP_DSIZE_MASK;
 612    int len;
 613
 614    if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID ||
 615                    type == SDP_DTYPE_BOOL) {
 616        if (type == SDP_DTYPE_UUID)
 617            (*uuids) ++;
 618        return 1 + (1 << (element->type & SDP_DSIZE_MASK));
 619    }
 620
 621    if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
 622        if (element->type & SDP_DSIZE_MASK) {
 623            for (len = 0; element->value.str[len] |
 624                            element->value.str[len + 1]; len ++);
 625            return len;
 626        } else
 627            return 2 + strlen(element->value.str);
 628    }
 629
 630    if (type != SDP_DTYPE_SEQ)
 631        exit(-1);
 632    len = 2;
 633    element = element->value.list;
 634    while (element->type)
 635        len += sdp_attr_max_size(element ++, uuids);
 636    if (len > 255)
 637        exit (-1);
 638
 639    return len;
 640}
 641
 642static int sdp_attr_write(uint8_t *data,
 643                struct sdp_def_data_element_s *element, int **uuid)
 644{
 645    int type = element->type & ~SDP_DSIZE_MASK;
 646    int len = 0;
 647
 648    if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) {
 649        data[len ++] = element->type;
 650        if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1)
 651            data[len ++] = (element->value.uint >>  0) & 0xff;
 652        else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) {
 653            data[len ++] = (element->value.uint >>  8) & 0xff;
 654            data[len ++] = (element->value.uint >>  0) & 0xff;
 655        } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) {
 656            data[len ++] = (element->value.uint >>  24) & 0xff;
 657            data[len ++] = (element->value.uint >>  16) & 0xff;
 658            data[len ++] = (element->value.uint >>  8) & 0xff;
 659            data[len ++] = (element->value.uint >>  0) & 0xff;
 660        }
 661
 662        return len;
 663    }
 664
 665    if (type == SDP_DTYPE_UUID) {
 666        *(*uuid) ++ = element->value.uint;
 667
 668        data[len ++] = element->type;
 669        data[len ++] = (element->value.uint >>  24) & 0xff;
 670        data[len ++] = (element->value.uint >>  16) & 0xff;
 671        data[len ++] = (element->value.uint >>  8) & 0xff;
 672        data[len ++] = (element->value.uint >>  0) & 0xff;
 673        memcpy(data + len, bt_base_uuid, 12);
 674
 675        return len + 12;
 676    }
 677
 678    data[0] = type | SDP_DSIZE_NEXT1;
 679    if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
 680        if (element->type & SDP_DSIZE_MASK)
 681            for (len = 0; element->value.str[len] |
 682                            element->value.str[len + 1]; len ++);
 683        else
 684            len = strlen(element->value.str);
 685        memcpy(data + 2, element->value.str, data[1] = len);
 686
 687        return len + 2;
 688    }
 689
 690    len = 2;
 691    element = element->value.list;
 692    while (element->type)
 693        len += sdp_attr_write(data + len, element ++, uuid);
 694    data[1] = len - 2;
 695
 696    return len;
 697}
 698
 699static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a,
 700                const struct sdp_service_attribute_s *b)
 701{
 702    return (int) b->attribute_id - a->attribute_id;
 703}
 704
 705static int sdp_uuid_compare(const int *a, const int *b)
 706{
 707    return *a - *b;
 708}
 709
 710static void sdp_service_record_build(struct sdp_service_record_s *record,
 711                struct sdp_def_service_s *def, int handle)
 712{
 713    int len = 0;
 714    uint8_t *data;
 715    int *uuid;
 716
 717    record->uuids = 0;
 718    while (def->attributes[record->attributes].data.type) {
 719        len += 3;
 720        len += sdp_attr_max_size(&def->attributes[record->attributes ++].data,
 721                        &record->uuids);
 722    }
 723
 724    assert(len > 0);
 725    record->uuids = pow2ceil(record->uuids);
 726    record->attribute_list =
 727            g_malloc0(record->attributes * sizeof(*record->attribute_list));
 728    record->uuid =
 729            g_malloc0(record->uuids * sizeof(*record->uuid));
 730    data = g_malloc(len);
 731
 732    record->attributes = 0;
 733    uuid = record->uuid;
 734    while (def->attributes[record->attributes].data.type) {
 735        int attribute_id = def->attributes[record->attributes].id;
 736        record->attribute_list[record->attributes].pair = data;
 737        record->attribute_list[record->attributes].attribute_id = attribute_id;
 738
 739        len = 0;
 740        data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2;
 741        data[len ++] = attribute_id >> 8;
 742        data[len ++] = attribute_id & 0xff;
 743        len += sdp_attr_write(data + len,
 744                        &def->attributes[record->attributes].data, &uuid);
 745
 746        /* Special case: assign a ServiceRecordHandle in sequence */
 747        if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE)
 748            def->attributes[record->attributes].data.value.uint = handle;
 749        /* Note: we could also assign a ServiceDescription based on
 750         * sdp->device.device->lmp_name.  */
 751
 752        record->attribute_list[record->attributes ++].len = len;
 753        data += len;
 754    }
 755
 756    /* Sort the attribute list by the AttributeID.  The first must be
 757     * SDP_ATTR_RECORD_HANDLE so that bt_l2cap_sdp_close_ch can free
 758     * the buffer.
 759     */
 760    qsort(record->attribute_list, record->attributes,
 761                    sizeof(*record->attribute_list),
 762                    (void *) sdp_attributeid_compare);
 763    assert(record->attribute_list[0].pair == data);
 764
 765    /* Sort the searchable UUIDs list for bisection */
 766    qsort(record->uuid, record->uuids,
 767                    sizeof(*record->uuid),
 768                    (void *) sdp_uuid_compare);
 769}
 770
 771static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp,
 772                struct sdp_def_service_s **service)
 773{
 774    sdp->services = 0;
 775    while (service[sdp->services])
 776        sdp->services ++;
 777    sdp->service_list =
 778            g_malloc0(sdp->services * sizeof(*sdp->service_list));
 779
 780    sdp->services = 0;
 781    while (*service) {
 782        sdp_service_record_build(&sdp->service_list[sdp->services],
 783                        *service, sdp->services);
 784        service ++;
 785        sdp->services ++;
 786    }
 787}
 788
 789#define LAST { .type = 0 }
 790#define SERVICE(name, attrs)                            \
 791    static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \
 792        .attributes = { attrs { .data = LAST } },       \
 793    };
 794#define ATTRIBUTE(attrid, val)  { .id = glue(SDP_ATTR_, attrid), .data = val },
 795#define UINT8(val)      {                               \
 796        .type       = SDP_DTYPE_UINT | SDP_DSIZE_1,     \
 797        .value.uint = val,                              \
 798    },
 799#define UINT16(val)     {                               \
 800        .type       = SDP_DTYPE_UINT | SDP_DSIZE_2,     \
 801        .value.uint = val,                              \
 802    },
 803#define UINT32(val)     {                               \
 804        .type       = SDP_DTYPE_UINT | SDP_DSIZE_4,     \
 805        .value.uint = val,                              \
 806    },
 807#define UUID128(val)    {                               \
 808        .type       = SDP_DTYPE_UUID | SDP_DSIZE_16,    \
 809        .value.uint = val,                              \
 810    },
 811#define SDP_TRUE        {                               \
 812        .type       = SDP_DTYPE_BOOL | SDP_DSIZE_1,     \
 813        .value.uint = 1,                                \
 814    },
 815#define SDP_FALSE       {                               \
 816        .type       = SDP_DTYPE_BOOL | SDP_DSIZE_1,     \
 817        .value.uint = 0,                                \
 818    },
 819#define STRING(val)     {                               \
 820        .type       = SDP_DTYPE_STRING,                 \
 821        .value.str  = val,                              \
 822    },
 823#define ARRAY(...)      {                               \
 824        .type       = SDP_DTYPE_STRING | SDP_DSIZE_2,   \
 825        .value.str  = (char []) { __VA_ARGS__, 0, 0 },  \
 826    },
 827#define URL(val)        {                               \
 828        .type       = SDP_DTYPE_URL,                    \
 829        .value.str  = val,                              \
 830    },
 831#if 1
 832#define LIST(val)       {                               \
 833        .type       = SDP_DTYPE_SEQ,                    \
 834        .value.list = (struct sdp_def_data_element_s []) { val LAST }, \
 835    },
 836#endif
 837
 838/* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes
 839 * in resulting SDP data representation size.  */
 840
 841SERVICE(hid,
 842    ATTRIBUTE(RECORD_HANDLE,   UINT32(0))       /* Filled in later */
 843    ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID)))
 844    ATTRIBUTE(RECORD_STATE,    UINT32(1))
 845    ATTRIBUTE(PROTO_DESC_LIST, LIST(
 846        LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL))
 847        LIST(UUID128(HIDP_UUID))
 848    ))
 849    ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
 850    ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
 851        UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
 852    ))
 853    ATTRIBUTE(PFILE_DESC_LIST, LIST(
 854        LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100))
 855    ))
 856    ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
 857    ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID"))
 858    ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse"))
 859    ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU"))
 860
 861    /* Profile specific */
 862    ATTRIBUTE(DEVICE_RELEASE_NUMBER,    UINT16(0x0091)) /* Deprecated, remove */
 863    ATTRIBUTE(PARSER_VERSION,           UINT16(0x0111))
 864    /* TODO: extract from l2cap_device->device.class[0] */
 865    ATTRIBUTE(DEVICE_SUBCLASS,          UINT8(0x40))
 866    ATTRIBUTE(COUNTRY_CODE,             UINT8(0x15))
 867    ATTRIBUTE(VIRTUAL_CABLE,            SDP_TRUE)
 868    ATTRIBUTE(RECONNECT_INITIATE,       SDP_FALSE)
 869    /* TODO: extract from hid->usbdev->report_desc */
 870    ATTRIBUTE(DESCRIPTOR_LIST,          LIST(
 871        LIST(UINT8(0x22) ARRAY(
 872            0x05, 0x01, /* Usage Page (Generic Desktop) */
 873            0x09, 0x06, /* Usage (Keyboard) */
 874            0xa1, 0x01, /* Collection (Application) */
 875            0x75, 0x01, /*   Report Size (1) */
 876            0x95, 0x08, /*   Report Count (8) */
 877            0x05, 0x07, /*   Usage Page (Key Codes) */
 878            0x19, 0xe0, /*   Usage Minimum (224) */
 879            0x29, 0xe7, /*   Usage Maximum (231) */
 880            0x15, 0x00, /*   Logical Minimum (0) */
 881            0x25, 0x01, /*   Logical Maximum (1) */
 882            0x81, 0x02, /*   Input (Data, Variable, Absolute) */
 883            0x95, 0x01, /*   Report Count (1) */
 884            0x75, 0x08, /*   Report Size (8) */
 885            0x81, 0x01, /*   Input (Constant) */
 886            0x95, 0x05, /*   Report Count (5) */
 887            0x75, 0x01, /*   Report Size (1) */
 888            0x05, 0x08, /*   Usage Page (LEDs) */
 889            0x19, 0x01, /*   Usage Minimum (1) */
 890            0x29, 0x05, /*   Usage Maximum (5) */
 891            0x91, 0x02, /*   Output (Data, Variable, Absolute) */
 892            0x95, 0x01, /*   Report Count (1) */
 893            0x75, 0x03, /*   Report Size (3) */
 894            0x91, 0x01, /*   Output (Constant) */
 895            0x95, 0x06, /*   Report Count (6) */
 896            0x75, 0x08, /*   Report Size (8) */
 897            0x15, 0x00, /*   Logical Minimum (0) */
 898            0x25, 0xff, /*   Logical Maximum (255) */
 899            0x05, 0x07, /*   Usage Page (Key Codes) */
 900            0x19, 0x00, /*   Usage Minimum (0) */
 901            0x29, 0xff, /*   Usage Maximum (255) */
 902            0x81, 0x00, /*   Input (Data, Array) */
 903            0xc0        /* End Collection */
 904    ))))
 905    ATTRIBUTE(LANG_ID_BASE_LIST,        LIST(
 906        LIST(UINT16(0x0409) UINT16(0x0100))
 907    ))
 908    ATTRIBUTE(SDP_DISABLE,              SDP_FALSE)
 909    ATTRIBUTE(BATTERY_POWER,            SDP_TRUE)
 910    ATTRIBUTE(REMOTE_WAKEUP,            SDP_TRUE)
 911    ATTRIBUTE(BOOT_DEVICE,              SDP_TRUE)       /* XXX: untested */
 912    ATTRIBUTE(SUPERVISION_TIMEOUT,      UINT16(0x0c80))
 913    ATTRIBUTE(NORMALLY_CONNECTABLE,     SDP_TRUE)
 914    ATTRIBUTE(PROFILE_VERSION,          UINT16(0x0100))
 915)
 916
 917SERVICE(sdp,
 918    ATTRIBUTE(RECORD_HANDLE,   UINT32(0))       /* Filled in later */
 919    ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID)))
 920    ATTRIBUTE(RECORD_STATE,    UINT32(1))
 921    ATTRIBUTE(PROTO_DESC_LIST, LIST(
 922        LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
 923        LIST(UUID128(SDP_UUID))
 924    ))
 925    ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
 926    ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
 927        UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
 928    ))
 929    ATTRIBUTE(PFILE_DESC_LIST, LIST(
 930        LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100))
 931    ))
 932    ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
 933    ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU"))
 934
 935    /* Profile specific */
 936    ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100)))
 937    ATTRIBUTE(SVCDB_STATE    , UINT32(1))
 938)
 939
 940SERVICE(pnp,
 941    ATTRIBUTE(RECORD_HANDLE,   UINT32(0))       /* Filled in later */
 942    ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID)))
 943    ATTRIBUTE(RECORD_STATE,    UINT32(1))
 944    ATTRIBUTE(PROTO_DESC_LIST, LIST(
 945        LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
 946        LIST(UUID128(SDP_UUID))
 947    ))
 948    ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
 949    ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
 950        UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
 951    ))
 952    ATTRIBUTE(PFILE_DESC_LIST, LIST(
 953        LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100))
 954    ))
 955    ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
 956    ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU"))
 957
 958    /* Profile specific */
 959    ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100))
 960    ATTRIBUTE(VERSION,         UINT16(0x0100))
 961    ATTRIBUTE(PRIMARY_RECORD,  SDP_TRUE)
 962)
 963
 964static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev,
 965                struct bt_l2cap_conn_params_s *params)
 966{
 967    struct bt_l2cap_sdp_state_s *sdp = g_malloc0(sizeof(*sdp));
 968    struct sdp_def_service_s *services[] = {
 969        &sdp_service_sdp_s,
 970        &sdp_service_hid_s,
 971        &sdp_service_pnp_s,
 972        NULL,
 973    };
 974
 975    sdp->channel = params;
 976    sdp->channel->opaque = sdp;
 977    sdp->channel->close = bt_l2cap_sdp_close_ch;
 978    sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in;
 979
 980    sdp_service_db_build(sdp, services);
 981
 982    return 0;
 983}
 984
 985void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev)
 986{
 987    bt_l2cap_psm_register(dev, BT_PSM_SDP,
 988                    MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch);
 989}
 990