hw/bt/sdp.c

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