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