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