hw/bt-sdp.c

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