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