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