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