src/shared/ad.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2015  Google Inc.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #include "src/shared/ad.h"
  25
  26 #include "src/eir.h"
  27 #include "src/shared/queue.h"
  28 #include "src/shared/util.h"
  29
  30 #define MAX_ADV_DATA_LEN 31
  31
  32 struct bt_ad {
  33         int ref_count;
  34         struct queue *service_uuids;
  35         struct queue *manufacturer_data;
  36         struct queue *solicit_uuids;
  37         struct queue *service_data;
  38 };
  39
  40 struct bt_ad *bt_ad_new(void)
  41 {
  42         struct bt_ad *ad;
  43
  44         ad = new0(struct bt_ad, 1);
  45         ad->service_uuids = queue_new();
  46         ad->manufacturer_data = queue_new();
  47         ad->solicit_uuids = queue_new();
  48         ad->service_data = queue_new();
  49
  50         return bt_ad_ref(ad);
  51 }
  52
  53 struct bt_ad *bt_ad_ref(struct bt_ad *ad)
  54 {
  55         if (!ad)
  56                 return NULL;
  57
  58         ad->ref_count++;
  59         return ad;
  60 }
  61
  62 static void uuid_destroy(void *data)
  63 {
  64         struct bt_ad_service_data *uuid_data = data;
  65
  66         free(uuid_data->data);
  67         free(uuid_data);
  68 }
  69
  70 static bool uuid_data_match(const void *data, const void *elem)
  71 {
  72         const struct bt_ad_service_data *uuid_data = elem;
  73         const bt_uuid_t *uuid = data;
  74
  75         return !bt_uuid_cmp(&uuid_data->uuid, uuid);
  76 }
  77
  78 static void manuf_destroy(void *data)
  79 {
  80         struct bt_ad_manufacturer_data *manuf = data;
  81
  82         free(manuf->data);
  83         free(manuf);
  84 }
  85
  86 static bool manuf_match(const void *data, const void *elem)
  87 {
  88         const struct bt_ad_manufacturer_data *manuf = elem;
  89         uint16_t manuf_id = PTR_TO_UINT(elem);
  90
  91         return manuf->manufacturer_id == manuf_id;
  92 }
  93
  94 void bt_ad_unref(struct bt_ad *ad)
  95 {
  96         if (!ad)
  97                 return;
  98
  99         if (__sync_sub_and_fetch(&ad->ref_count, 1))
 100                 return;
 101
 102         queue_destroy(ad->service_uuids, free);
 103
 104         queue_destroy(ad->manufacturer_data, manuf_destroy);
 105
 106         queue_destroy(ad->solicit_uuids, free);
 107
 108         queue_destroy(ad->service_data, uuid_destroy);
 109
 110         free(ad);
 111 }
 112
 113 static size_t uuid_list_length(struct queue *uuid_queue)
 114 {
 115         bool uuid16_included = false;
 116         bool uuid32_included = false;
 117         bool uuid128_included = false;
 118         size_t length = 0;
 119         const struct queue_entry *entry;
 120
 121         entry = queue_get_entries(uuid_queue);
 122
 123         while (entry) {
 124                 bt_uuid_t *uuid = entry->data;
 125
 126                 length += bt_uuid_len(uuid);
 127
 128                 if (uuid->type == BT_UUID16)
 129                         uuid16_included = true;
 130                 else if (uuid->type == BT_UUID32)
 131                         uuid32_included = true;
 132                 else
 133                         uuid128_included = true;
 134
 135                 entry = entry->next;
 136         }
 137
 138         if (uuid16_included)
 139                 length += 2;
 140
 141         if (uuid32_included)
 142                 length += 2;
 143
 144         if (uuid128_included)
 145                 length += 2;
 146
 147         return length;
 148 }
 149
 150 static size_t mfg_data_length(struct queue *manuf_data)
 151 {
 152         size_t length = 0;
 153         const struct queue_entry *entry;
 154
 155         entry = queue_get_entries(manuf_data);
 156
 157         while (entry) {
 158                 struct bt_ad_manufacturer_data *data = entry->data;
 159
 160                 length += 2 + sizeof(uint16_t) + data->len;
 161
 162                 entry = entry->next;
 163         }
 164
 165         return length;
 166 }
 167
 168 static size_t uuid_data_length(struct queue *uuid_data)
 169 {
 170         size_t length = 0;
 171         const struct queue_entry *entry;
 172
 173         entry = queue_get_entries(uuid_data);
 174
 175         while (entry) {
 176                 struct bt_ad_service_data *data = entry->data;
 177
 178                 length += 2 + bt_uuid_len(&data->uuid) + data->len;
 179
 180                 entry = entry->next;
 181         }
 182
 183         return length;
 184 }
 185
 186 static size_t calculate_length(struct bt_ad *ad)
 187 {
 188         size_t length = 0;
 189
 190         length += uuid_list_length(ad->service_uuids);
 191
 192         length += uuid_list_length(ad->solicit_uuids);
 193
 194         length += mfg_data_length(ad->manufacturer_data);
 195
 196         length += uuid_data_length(ad->service_data);
 197
 198         return length;
 199 }
 200
 201 static void serialize_uuids(struct queue *uuids, uint8_t uuid_type,
 202                                                 uint8_t ad_type, uint8_t *buf,
 203                                                 uint8_t *pos)
 204 {
 205         const struct queue_entry *entry = queue_get_entries(uuids);
 206         bool added = false;
 207         uint8_t length_pos = 0;
 208
 209         while (entry) {
 210                 bt_uuid_t *uuid = entry->data;
 211
 212                 if (uuid->type == uuid_type) {
 213                         if (!added) {
 214                                 length_pos = (*pos)++;
 215                                 buf[(*pos)++] = ad_type;
 216                                 added = true;
 217                         }
 218
 219                         if (uuid_type != BT_UUID32)
 220                                 bt_uuid_to_le(uuid, buf + *pos);
 221                         else
 222                                 bt_put_le32(uuid->value.u32, buf + *pos);
 223
 224                         *pos += bt_uuid_len(uuid);
 225                 }
 226
 227                 entry = entry->next;
 228         }
 229
 230         if (added)
 231                 buf[length_pos] = *pos - length_pos - 1;
 232 }
 233
 234 static void serialize_service_uuids(struct queue *uuids, uint8_t *buf,
 235                                                                 uint8_t *pos)
 236 {
 237         serialize_uuids(uuids, BT_UUID16, EIR_UUID16_ALL, buf, pos);
 238
 239         serialize_uuids(uuids, BT_UUID32, EIR_UUID32_ALL, buf, pos);
 240
 241         serialize_uuids(uuids, BT_UUID128, EIR_UUID128_ALL, buf, pos);
 242 }
 243
 244 static void serialize_solicit_uuids(struct queue *uuids, uint8_t *buf,
 245                                                                 uint8_t *pos)
 246 {
 247         serialize_uuids(uuids, BT_UUID16, EIR_SOLICIT16, buf, pos);
 248
 249         serialize_uuids(uuids, BT_UUID32, EIR_SOLICIT32, buf, pos);
 250
 251         serialize_uuids(uuids, BT_UUID128, EIR_SOLICIT128, buf, pos);
 252 }
 253
 254 static void serialize_manuf_data(struct queue *manuf_data, uint8_t *buf,
 255                                                                 uint8_t *pos)
 256 {
 257         const struct queue_entry *entry = queue_get_entries(manuf_data);
 258
 259         while (entry) {
 260                 struct bt_ad_manufacturer_data *data = entry->data;
 261
 262                 buf[(*pos)++] = data->len + 2 + 1;
 263
 264                 buf[(*pos)++] = EIR_MANUFACTURER_DATA;
 265
 266                 bt_put_le16(data->manufacturer_id, buf + (*pos));
 267
 268                 *pos += 2;
 269
 270                 memcpy(buf + *pos, data->data, data->len);
 271
 272                 *pos += data->len;
 273
 274                 entry = entry->next;
 275         }
 276 }
 277
 278 static void serialize_service_data(struct queue *service_data, uint8_t *buf,
 279                                                                 uint8_t *pos)
 280 {
 281         const struct queue_entry *entry = queue_get_entries(service_data);
 282
 283         while (entry) {
 284                 struct bt_ad_service_data *data = entry->data;
 285                 int uuid_len = bt_uuid_len(&data->uuid);
 286
 287                 buf[(*pos)++] =  uuid_len + data->len + 1;
 288
 289                 switch (uuid_len) {
 290                 case 2:
 291                         buf[(*pos)++] = EIR_SVC_DATA16;
 292                         break;
 293                 case 4:
 294                         buf[(*pos)++] = EIR_SVC_DATA32;
 295                         break;
 296                 case 16:
 297                         buf[(*pos)++] = EIR_SVC_DATA128;
 298                         break;
 299                 }
 300
 301                 if (uuid_len != 4)
 302                         bt_uuid_to_le(&data->uuid, buf + *pos);
 303                 else
 304                         bt_put_le32(data->uuid.value.u32, buf + *pos);
 305
 306                 *pos += uuid_len;
 307
 308                 memcpy(buf + *pos, data->data, data->len);
 309
 310                 *pos += data->len;
 311
 312                 entry = entry->next;
 313         }
 314 }
 315
 316 uint8_t *bt_ad_generate(struct bt_ad *ad, size_t *length)
 317 {
 318         uint8_t *adv_data;
 319         uint8_t pos = 0;
 320
 321         if (!ad)
 322                 return NULL;
 323
 324         *length = calculate_length(ad);
 325
 326         if (*length > MAX_ADV_DATA_LEN)
 327                 return NULL;
 328
 329         adv_data = malloc0(*length);
 330         if (!adv_data)
 331                 return NULL;
 332
 333         serialize_service_uuids(ad->service_uuids, adv_data, &pos);
 334
 335         serialize_solicit_uuids(ad->solicit_uuids, adv_data, &pos);
 336
 337         serialize_manuf_data(ad->manufacturer_data, adv_data, &pos);
 338
 339         serialize_service_data(ad->service_data, adv_data, &pos);
 340
 341         return adv_data;
 342 }
 343
 344 static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
 345 {
 346         bt_uuid_t *new_uuid;
 347
 348         if (!queue)
 349                 return false;
 350
 351         new_uuid = new0(bt_uuid_t, 1);
 352
 353         *new_uuid = *uuid;
 354
 355         if (queue_push_tail(queue, new_uuid))
 356                 return true;
 357
 358         free(new_uuid);
 359
 360         return false;
 361 }
 362
 363 static bool uuid_match(const void *data, const void *elem)
 364 {
 365         const bt_uuid_t *match_uuid = data;
 366         const bt_uuid_t *uuid = elem;
 367
 368         return bt_uuid_cmp(match_uuid, uuid);
 369 }
 370
 371 static bool queue_remove_uuid(struct queue *queue, bt_uuid_t *uuid)
 372 {
 373         bt_uuid_t *removed;
 374
 375         if (!queue || !uuid)
 376                 return false;
 377
 378         removed = queue_remove_if(queue, uuid_match, uuid);
 379
 380         if (removed) {
 381                 free(removed);
 382                 return true;
 383         }
 384
 385         return false;
 386 }
 387
 388 bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
 389 {
 390         if (!ad)
 391                 return false;
 392
 393         return queue_add_uuid(ad->service_uuids, uuid);
 394 }
 395
 396 bool bt_ad_remove_service_uuid(struct bt_ad *ad, bt_uuid_t *uuid)
 397 {
 398         if (!ad)
 399                 return false;
 400
 401         return queue_remove_uuid(ad->service_uuids, uuid);
 402 }
 403
 404 void bt_ad_clear_service_uuid(struct bt_ad *ad)
 405 {
 406         if (!ad)
 407                 return;
 408
 409         queue_remove_all(ad->service_uuids, NULL, NULL, free);
 410 }
 411
 412 static bool manufacturer_id_data_match(const void *data, const void *user_data)
 413 {
 414         const struct bt_ad_manufacturer_data *m = data;
 415         uint16_t id = PTR_TO_UINT(user_data);
 416
 417         return m->manufacturer_id == id;
 418 }
 419
 420 bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_id,
 421                                                         void *data, size_t len)
 422 {
 423         struct bt_ad_manufacturer_data *new_data;
 424
 425         if (!ad)
 426                 return false;
 427
 428         if (len > (MAX_ADV_DATA_LEN - 2 - sizeof(uint16_t)))
 429                 return false;
 430
 431         new_data = queue_find(ad->manufacturer_data, manufacturer_id_data_match,
 432                                                 UINT_TO_PTR(manufacturer_id));
 433         if (new_data) {
 434                 if (new_data->len == len && !memcmp(new_data->data, data, len))
 435                         return false;
 436                 new_data->data = realloc(new_data->data, len);
 437                 memcpy(new_data->data, data, len);
 438                 new_data->len = len;
 439                 return true;
 440         }
 441
 442         new_data = new0(struct bt_ad_manufacturer_data, 1);
 443         new_data->manufacturer_id = manufacturer_id;
 444
 445         new_data->data = malloc(len);
 446         if (!new_data->data) {
 447                 free(new_data);
 448                 return false;
 449         }
 450
 451         memcpy(new_data->data, data, len);
 452
 453         new_data->len = len;
 454
 455         if (queue_push_tail(ad->manufacturer_data, new_data))
 456                 return true;
 457
 458         manuf_destroy(new_data);
 459
 460         return false;
 461 }
 462
 463 static bool manufacturer_data_match(const void *data, const void *user_data)
 464 {
 465         const struct bt_ad_manufacturer_data *m1 = data;
 466         const struct bt_ad_manufacturer_data *m2 = user_data;
 467
 468         if (m1->manufacturer_id != m2->manufacturer_id)
 469                 return false;
 470
 471         if (m1->len != m2->len)
 472                 return false;
 473
 474         return !memcmp(m1->data, m2->data, m1->len);
 475 }
 476
 477 bool bt_ad_has_manufacturer_data(struct bt_ad *ad,
 478                                 const struct bt_ad_manufacturer_data *data)
 479 {
 480         if (!ad)
 481                 return false;
 482
 483         if (!data)
 484                 return !queue_isempty(ad->manufacturer_data);
 485
 486         return queue_find(ad->manufacturer_data, manufacturer_data_match, data);
 487 }
 488
 489 void bt_ad_foreach_manufacturer_data(struct bt_ad *ad, bt_ad_func_t func,
 490                                                         void *user_data)
 491 {
 492         if (!ad)
 493                 return;
 494
 495         queue_foreach(ad->manufacturer_data, func, user_data);
 496 }
 497
 498 bool bt_ad_remove_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_id)
 499 {
 500         struct bt_ad_manufacturer_data *data;
 501
 502         if (!ad)
 503                 return false;
 504
 505         data = queue_remove_if(ad->manufacturer_data, manuf_match,
 506                                                 UINT_TO_PTR(manufacturer_id));
 507
 508         if (!data)
 509                 return false;
 510
 511         manuf_destroy(data);
 512
 513         return true;
 514 }
 515
 516 void bt_ad_clear_manufacturer_data(struct bt_ad *ad)
 517 {
 518         if (!ad)
 519                 return;
 520
 521         queue_remove_all(ad->manufacturer_data, NULL, NULL, manuf_destroy);
 522 }
 523
 524 bool bt_ad_add_solicit_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
 525 {
 526         if (!ad)
 527                 return false;
 528
 529         return queue_add_uuid(ad->solicit_uuids, uuid);
 530 }
 531
 532 bool bt_ad_remove_solicit_uuid(struct bt_ad *ad, bt_uuid_t *uuid)
 533 {
 534         if (!ad)
 535                 return false;
 536
 537         return queue_remove_uuid(ad->solicit_uuids, uuid);
 538 }
 539
 540 void bt_ad_clear_solicit_uuid(struct bt_ad *ad)
 541 {
 542         if (!ad)
 543                 return;
 544
 545         queue_remove_all(ad->solicit_uuids, NULL, NULL, free);
 546 }
 547
 548
 549 static bool service_uuid_match(const void *data, const void *user_data)
 550 {
 551         const struct bt_ad_service_data *s = data;
 552         const bt_uuid_t *uuid = user_data;
 553
 554         return !bt_uuid_cmp(&s->uuid, uuid);
 555 }
 556
 557 bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void *data,
 558                                                                 size_t len)
 559 {
 560         struct bt_ad_service_data *new_data;
 561
 562         if (!ad)
 563                 return false;
 564
 565         if (len > (MAX_ADV_DATA_LEN - 2 - (size_t)bt_uuid_len(uuid)))
 566                 return false;
 567
 568         new_data = queue_find(ad->service_data, service_uuid_match, uuid);
 569         if (new_data) {
 570                 if (new_data->len == len && !memcmp(new_data->data, data, len))
 571                         return false;
 572                 new_data->data = realloc(new_data->data, len);
 573                 memcpy(new_data->data, data, len);
 574                 new_data->len = len;
 575                 return true;
 576         }
 577
 578         new_data = new0(struct bt_ad_service_data, 1);
 579
 580         new_data->uuid = *uuid;
 581
 582         new_data->data = malloc(len);
 583         if (!new_data->data) {
 584                 free(new_data);
 585                 return false;
 586         }
 587
 588         memcpy(new_data->data, data, len);
 589
 590         new_data->len = len;
 591
 592         if (queue_push_tail(ad->service_data, new_data))
 593                 return true;
 594
 595         uuid_destroy(new_data);
 596
 597         return false;
 598 }
 599
 600 static bool service_data_match(const void *data, const void *user_data)
 601 {
 602         const struct bt_ad_service_data *s1 = data;
 603         const struct bt_ad_service_data *s2 = user_data;
 604
 605         if (bt_uuid_cmp(&s1->uuid, &s2->uuid))
 606                 return false;
 607
 608         if (s1->len != s2->len)
 609                 return false;
 610
 611         return !memcmp(s1->data, s2->data, s1->len);
 612 }
 613
 614 bool bt_ad_has_service_data(struct bt_ad *ad,
 615                                         const struct bt_ad_service_data *data)
 616 {
 617         if (!ad)
 618                 return false;
 619
 620         if (!data)
 621                 return !queue_isempty(ad->service_data);
 622
 623         return queue_find(ad->service_data, service_data_match, data);
 624 }
 625
 626 void bt_ad_foreach_service_data(struct bt_ad *ad, bt_ad_func_t func,
 627                                                         void *user_data)
 628 {
 629         if (!ad)
 630                 return;
 631
 632         queue_foreach(ad->service_data, func, user_data);
 633 }
 634
 635 bool bt_ad_remove_service_data(struct bt_ad *ad, bt_uuid_t *uuid)
 636 {
 637         struct bt_ad_service_data *data;
 638
 639         if (!ad)
 640                 return false;
 641
 642         data = queue_remove_if(ad->service_data, uuid_data_match, uuid);
 643
 644         if (!data)
 645                 return false;
 646
 647         uuid_destroy(data);
 648
 649         return true;
 650 }
 651
 652 void bt_ad_clear_service_data(struct bt_ad *ad)
 653 {
 654         if (!ad)
 655                 return;
 656
 657         queue_remove_all(ad->service_data, NULL, NULL, uuid_destroy);
 658 }