mesh/mesh-io-generic.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2018-2019  Intel Corporation. All rights reserved.
   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  */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include <config.h>
  22 #endif
  23
  24 #include <errno.h>
  25 #include <string.h>
  26 #include <sys/time.h>
  27 #include <ell/ell.h>
  28
  29 #include "monitor/bt.h"
  30 #include "src/shared/hci.h"
  31 #include "lib/bluetooth.h"
  32 #include "lib/mgmt.h"
  33
  34 #include "mesh/mesh-defs.h"
  35 #include "mesh/mesh-mgmt.h"
  36 #include "mesh/mesh-io.h"
  37 #include "mesh/mesh-io-api.h"
  38 #include "mesh/mesh-io-generic.h"
  39
  40 struct mesh_io_private {
  41         struct bt_hci *hci;
  42         void *user_data;
  43         mesh_io_ready_func_t ready_callback;
  44         struct l_timeout *tx_timeout;
  45         struct l_queue *rx_regs;
  46         struct l_queue *tx_pkts;
  47         struct tx_pkt *tx;
  48         uint16_t index;
  49         uint16_t interval;
  50         bool sending;
  51         bool active;
  52 #ifdef TIZEN_FEATURE_BLUEZ_MODIFY
  53         bool use_raw;
  54 #endif
  55 };
  56
  57 struct pvt_rx_reg {
  58         mesh_io_recv_func_t cb;
  59         void *user_data;
  60         uint8_t len;
  61         uint8_t filter[0];
  62 };
  63
  64 struct process_data {
  65         struct mesh_io_private          *pvt;
  66         const uint8_t                   *data;
  67         uint8_t                         len;
  68         struct mesh_io_recv_info        info;
  69 };
  70
  71 struct tx_pkt {
  72         struct mesh_io_send_info        info;
  73         bool                            delete;
  74         uint8_t                         len;
  75         uint8_t                         pkt[30];
  76 };
  77
  78 struct tx_pattern {
  79         const uint8_t                   *data;
  80         uint8_t                         len;
  81 };
  82
  83 static uint32_t get_instant(void)
  84 {
  85         struct timeval tm;
  86         uint32_t instant;
  87
  88         gettimeofday(&tm, NULL);
  89         instant = tm.tv_sec * 1000;
  90         instant += tm.tv_usec / 1000;
  91
  92         return instant;
  93 }
  94
  95 static uint32_t instant_remaining_ms(uint32_t instant)
  96 {
  97         instant -= get_instant();
  98         return instant;
  99 }
 100
 101 static void process_rx_callbacks(void *v_reg, void *v_rx)
 102 {
 103         struct pvt_rx_reg *rx_reg = v_reg;
 104         struct process_data *rx = v_rx;
 105
 106         if (!memcmp(rx->data, rx_reg->filter, rx_reg->len))
 107                 rx_reg->cb(rx_reg->user_data, &rx->info, rx->data, rx->len);
 108 }
 109
 110 static void process_rx(struct mesh_io_private *pvt, int8_t rssi,
 111                                         uint32_t instant, const uint8_t *addr,
 112                                         const uint8_t *data, uint8_t len)
 113 {
 114         struct process_data rx = {
 115                 .pvt = pvt,
 116                 .data = data,
 117                 .len = len,
 118                 .info.instant = instant,
 119                 .info.addr = addr,
 120                 .info.chan = 7,
 121                 .info.rssi = rssi,
 122         };
 123
 124         l_queue_foreach(pvt->rx_regs, process_rx_callbacks, &rx);
 125 }
 126
 127 static void event_adv_report(struct mesh_io *io, const void *buf, uint8_t size)
 128 {
 129         const struct bt_hci_evt_le_adv_report *evt = buf;
 130         const uint8_t *adv;
 131         const uint8_t *addr;
 132         uint32_t instant;
 133         uint8_t adv_len;
 134         uint16_t len = 0;
 135         int8_t rssi;
 136
 137         if (evt->event_type != 0x03)
 138                 return;
 139
 140         instant = get_instant();
 141         adv = evt->data;
 142         adv_len = evt->data_len;
 143         addr = evt->addr;
 144
 145         /* rssi is just beyond last byte of data */
 146         rssi = (int8_t) adv[adv_len];
 147
 148         while (len < adv_len - 1) {
 149                 uint8_t field_len = adv[0];
 150
 151                 /* Check for the end of advertising data */
 152                 if (field_len == 0)
 153                         break;
 154
 155                 len += field_len + 1;
 156
 157                 /* Do not continue data parsing if got incorrect length */
 158                 if (len > adv_len)
 159                         break;
 160
 161                 /* TODO: Create an Instant to use */
 162                 process_rx(io->pvt, rssi, instant, addr, adv + 1, adv[0]);
 163
 164                 adv += field_len + 1;
 165         }
 166 }
 167
 168 static void event_callback(const void *buf, uint8_t size, void *user_data)
 169 {
 170         uint8_t event = l_get_u8(buf);
 171         struct mesh_io *io = user_data;
 172
 173         switch (event) {
 174         case BT_HCI_EVT_LE_ADV_REPORT:
 175                 event_adv_report(io, buf + 1, size - 1);
 176                 break;
 177
 178         default:
 179                 l_info("Other Meta Evt - %d", event);
 180         }
 181 }
 182
 183 static void local_commands_callback(const void *data, uint8_t size,
 184                                                         void *user_data)
 185 {
 186         const struct bt_hci_rsp_read_local_commands *rsp = data;
 187
 188         if (rsp->status)
 189                 l_error("Failed to read local commands");
 190 }
 191
 192 static void local_features_callback(const void *data, uint8_t size,
 193                                                         void *user_data)
 194 {
 195         const struct bt_hci_rsp_read_local_features *rsp = data;
 196
 197         if (rsp->status)
 198                 l_error("Failed to read local features");
 199 }
 200
 201 static void hci_generic_callback(const void *data, uint8_t size,
 202                                                                 void *user_data)
 203 {
 204         uint8_t status = l_get_u8(data);
 205
 206         if (status)
 207                 l_error("Failed to initialize HCI");
 208 }
 209
 210 static void configure_hci(struct mesh_io_private *io)
 211 {
 212         struct bt_hci_cmd_le_set_scan_parameters cmd;
 213         struct bt_hci_cmd_set_event_mask cmd_sem;
 214         struct bt_hci_cmd_le_set_event_mask cmd_slem;
 215
 216         /* Set scan parameters */
 217         cmd.type = 0x00; /* Passive Scanning. No scanning PDUs shall be sent */
 218         cmd.interval = 0x0030; /* Scan Interval = N * 0.625ms */
 219         cmd.window = 0x0030; /* Scan Window = N * 0.625ms */
 220         cmd.own_addr_type = 0x00; /* Public Device Address */
 221         /* Accept all advertising packets except directed advertising packets
 222          * not addressed to this device (default).
 223          */
 224         cmd.filter_policy = 0x00;
 225
 226         /* Set event mask
 227          *
 228          * Mask: 0x2000800002008890
 229          *   Disconnection Complete
 230          *   Encryption Change
 231          *   Read Remote Version Information Complete
 232          *   Hardware Error
 233          *   Data Buffer Overflow
 234          *   Encryption Key Refresh Complete
 235          *   LE Meta
 236          */
 237         cmd_sem.mask[0] = 0x90;
 238         cmd_sem.mask[1] = 0x88;
 239         cmd_sem.mask[2] = 0x00;
 240         cmd_sem.mask[3] = 0x02;
 241         cmd_sem.mask[4] = 0x00;
 242         cmd_sem.mask[5] = 0x80;
 243         cmd_sem.mask[6] = 0x00;
 244         cmd_sem.mask[7] = 0x20;
 245
 246         /* Set LE event mask
 247          *
 248          * Mask: 0x000000000000087f
 249          *   LE Connection Complete
 250          *   LE Advertising Report
 251          *   LE Connection Update Complete
 252          *   LE Read Remote Used Features Complete
 253          *   LE Long Term Key Request
 254          *   LE Remote Connection Parameter Request
 255          *   LE Data Length Change
 256          *   LE PHY Update Complete
 257          */
 258         cmd_slem.mask[0] = 0x7f;
 259         cmd_slem.mask[1] = 0x08;
 260         cmd_slem.mask[2] = 0x00;
 261         cmd_slem.mask[3] = 0x00;
 262         cmd_slem.mask[4] = 0x00;
 263         cmd_slem.mask[5] = 0x00;
 264         cmd_slem.mask[6] = 0x00;
 265         cmd_slem.mask[7] = 0x00;
 266
 267 #ifdef TIZEN_FEATURE_BLUEZ_MODIFY
 268         if (!(io->use_raw)) {
 269                 /* Reset Command in case of user channel */
 270                 bt_hci_send(io->hci, BT_HCI_CMD_RESET, NULL, 0, hci_generic_callback,
 271                                                                         NULL, NULL);
 272         }
 273 #else
 274         /* TODO: Move to suitable place. Set suitable masks */
 275         /* Reset Command */
 276         bt_hci_send(io->hci, BT_HCI_CMD_RESET, NULL, 0, hci_generic_callback,
 277                                                                 NULL, NULL);
 278 #endif
 279
 280         /* Read local supported commands */
 281         bt_hci_send(io->hci, BT_HCI_CMD_READ_LOCAL_COMMANDS, NULL, 0,
 282                                         local_commands_callback, NULL, NULL);
 283
 284         /* Read local supported features */
 285         bt_hci_send(io->hci, BT_HCI_CMD_READ_LOCAL_FEATURES, NULL, 0,
 286                                         local_features_callback, NULL, NULL);
 287
 288         /* Set event mask */
 289         bt_hci_send(io->hci, BT_HCI_CMD_SET_EVENT_MASK, &cmd_sem,
 290                         sizeof(cmd_sem), hci_generic_callback, NULL, NULL);
 291
 292         /* Set LE event mask */
 293         bt_hci_send(io->hci, BT_HCI_CMD_LE_SET_EVENT_MASK, &cmd_slem,
 294                         sizeof(cmd_slem), hci_generic_callback, NULL, NULL);
 295
 296         /* Scan Params */
 297         bt_hci_send(io->hci, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS, &cmd,
 298                                 sizeof(cmd), hci_generic_callback, NULL, NULL);
 299 }
 300
 301 static void scan_enable_rsp(const void *buf, uint8_t size,
 302                                                         void *user_data)
 303 {
 304         uint8_t status = *((uint8_t *) buf);
 305
 306         if (status)
 307                 l_error("LE Scan enable failed (0x%02x)", status);
 308 }
 309
 310 static void set_recv_scan_enable(const void *buf, uint8_t size,
 311                                                         void *user_data)
 312 {
 313         struct mesh_io_private *pvt = user_data;
 314         struct bt_hci_cmd_le_set_scan_enable cmd;
 315
 316         cmd.enable = 0x01;      /* Enable scanning */
 317         cmd.filter_dup = 0x00;  /* Report duplicates */
 318         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
 319                         &cmd, sizeof(cmd), scan_enable_rsp, pvt, NULL);
 320 }
 321
 322 static void scan_disable_rsp(const void *buf, uint8_t size,
 323                                                         void *user_data)
 324 {
 325         struct bt_hci_cmd_le_set_scan_parameters cmd;
 326         struct mesh_io_private *pvt = user_data;
 327         uint8_t status = *((uint8_t *) buf);
 328
 329         if (status)
 330                 l_error("LE Scan disable failed (0x%02x)", status);
 331
 332         cmd.type = pvt->active ? 0x01 : 0x00;   /* Passive/Active scanning */
 333         cmd.interval = L_CPU_TO_LE16(0x0010);   /* 10 ms */
 334         cmd.window = L_CPU_TO_LE16(0x0010);     /* 10 ms */
 335         cmd.own_addr_type = 0x01;               /* ADDR_TYPE_RANDOM */
 336         cmd.filter_policy = 0x00;               /* Accept all */
 337
 338         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
 339                         &cmd, sizeof(cmd),
 340                         set_recv_scan_enable, pvt, NULL);
 341 }
 342
 343 static bool find_active(const void *a, const void *b)
 344 {
 345         const struct pvt_rx_reg *rx_reg = a;
 346
 347         /* Mesh specific AD types do *not* require active scanning,
 348          * so do not turn on Active Scanning on their account.
 349          */
 350         if (rx_reg->filter[0] < MESH_AD_TYPE_PROVISION ||
 351                         rx_reg->filter[0] > MESH_AD_TYPE_BEACON)
 352                 return true;
 353
 354         return false;
 355 }
 356
 357 static void restart_scan(struct mesh_io_private *pvt)
 358 {
 359         struct bt_hci_cmd_le_set_scan_enable cmd;
 360
 361         if (l_queue_isempty(pvt->rx_regs))
 362                 return;
 363
 364         pvt->active = l_queue_find(pvt->rx_regs, find_active, NULL);
 365         cmd.enable = 0x00;      /* Disable scanning */
 366         cmd.filter_dup = 0x00;  /* Report duplicates */
 367         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
 368                                 &cmd, sizeof(cmd), scan_disable_rsp, pvt, NULL);
 369 }
 370
 371 static void hci_init(void *user_data)
 372 {
 373         struct mesh_io *io = user_data;
 374         bool result = true;
 375         bool restarted = false;
 376
 377         if (io->pvt->hci) {
 378                 restarted = true;
 379                 bt_hci_unref(io->pvt->hci);
 380         }
 381
 382 #ifdef TIZEN_FEATURE_BLUEZ_MODIFY
 383         if (io->pvt->use_raw) {
 384                 l_debug("Use HCI RAW channel");
 385
 386                 /* Power up HCI device */
 387                 uint16_t mode = 0x01;
 388                 if (!set_powered(mode, io->pvt->index))
 389                         return;
 390
 391                 io->pvt->hci = bt_hci_new_raw_device(io->pvt->index);
 392         } else {
 393                 l_debug("Use HCI USER channel");
 394                 io->pvt->hci = bt_hci_new_user_channel(io->pvt->index);
 395         }
 396 #else
 397         io->pvt->hci = bt_hci_new_user_channel(io->pvt->index);
 398 #endif
 399
 400         if (!io->pvt->hci) {
 401                 l_error("Failed to start mesh io (hci %u): %s", io->pvt->index,
 402                                                         strerror(errno));
 403                 result = false;
 404         }
 405
 406         if (result) {
 407                 configure_hci(io->pvt);
 408
 409                 bt_hci_register(io->pvt->hci, BT_HCI_EVT_LE_META_EVENT,
 410                                                 event_callback, io, NULL);
 411
 412                 l_debug("Started mesh on hci %u", io->pvt->index);
 413
 414                 if (restarted)
 415                         restart_scan(io->pvt);
 416         }
 417
 418         if (io->pvt->ready_callback)
 419                 io->pvt->ready_callback(io->pvt->user_data, result);
 420 }
 421
 422 static void read_info(int index, void *user_data)
 423 {
 424         struct mesh_io *io = user_data;
 425
 426         if (io->pvt->index != MGMT_INDEX_NONE &&
 427                                         index != io->pvt->index) {
 428                 l_debug("Ignore index %d", index);
 429                 return;
 430         }
 431
 432         io->pvt->index = index;
 433         hci_init(io);
 434 }
 435
 436 static bool dev_init(struct mesh_io *io, void *opts,
 437                                 mesh_io_ready_func_t cb, void *user_data)
 438 {
 439         if (!io || io->pvt)
 440                 return false;
 441
 442         io->pvt = l_new(struct mesh_io_private, 1);
 443 #ifdef TIZEN_FEATURE_BLUEZ_MODIFY
 444         struct mesh_io_opts *io_opts;
 445         io_opts = (struct mesh_io_opts *)opts;
 446         io->pvt->index = io_opts->index;
 447         io->pvt->use_raw = io_opts->use_raw;
 448 #else
 449         io->pvt->index = *(int *)opts;
 450 #endif
 451
 452         io->pvt->rx_regs = l_queue_new();
 453         io->pvt->tx_pkts = l_queue_new();
 454
 455         io->pvt->ready_callback = cb;
 456         io->pvt->user_data = user_data;
 457
 458         if (io->pvt->index == MGMT_INDEX_NONE)
 459                 return mesh_mgmt_list(read_info, io);
 460
 461         l_idle_oneshot(hci_init, io, NULL);
 462
 463         return true;
 464 }
 465
 466 static bool dev_destroy(struct mesh_io *io)
 467 {
 468         struct mesh_io_private *pvt = io->pvt;
 469
 470         if (!pvt)
 471                 return true;
 472
 473         bt_hci_unref(pvt->hci);
 474         l_timeout_remove(pvt->tx_timeout);
 475         l_queue_destroy(pvt->rx_regs, l_free);
 476         l_queue_destroy(pvt->tx_pkts, l_free);
 477         l_free(pvt);
 478         io->pvt = NULL;
 479
 480         return true;
 481 }
 482
 483 static bool dev_caps(struct mesh_io *io, struct mesh_io_caps *caps)
 484 {
 485         struct mesh_io_private *pvt = io->pvt;
 486
 487         if (!pvt || !caps)
 488                 return false;
 489
 490         caps->max_num_filters = 255;
 491         caps->window_accuracy = 50;
 492
 493         return true;
 494 }
 495
 496 static void send_cancel_done(const void *buf, uint8_t size,
 497                                                         void *user_data)
 498 {
 499         struct mesh_io_private *pvt = user_data;
 500         struct bt_hci_cmd_le_set_random_address cmd;
 501
 502         if (!pvt)
 503                 return;
 504
 505         pvt->sending = false;
 506
 507         /* At end of any burst of ADVs, change random address */
 508         l_getrandom(cmd.addr, 6);
 509         cmd.addr[5] |= 0xc0;
 510         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
 511                                 &cmd, sizeof(cmd), NULL, NULL, NULL);
 512 }
 513
 514 static void send_cancel(struct mesh_io_private *pvt)
 515 {
 516         struct bt_hci_cmd_le_set_adv_enable cmd;
 517
 518         if (!pvt)
 519                 return;
 520
 521         if (!pvt->sending) {
 522                 send_cancel_done(NULL, 0, pvt);
 523                 return;
 524         }
 525
 526         cmd.enable = 0x00;      /* Disable advertising */
 527         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
 528                                 &cmd, sizeof(cmd),
 529                                 send_cancel_done, pvt, NULL);
 530 }
 531
 532 static void set_send_adv_enable(const void *buf, uint8_t size,
 533                                                         void *user_data)
 534 {
 535         struct mesh_io_private *pvt = user_data;
 536         struct bt_hci_cmd_le_set_adv_enable cmd;
 537
 538         if (!pvt)
 539                 return;
 540
 541         pvt->sending = true;
 542         cmd.enable = 0x01;      /* Enable advertising */
 543         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
 544                                 &cmd, sizeof(cmd), NULL, NULL, NULL);
 545 }
 546
 547 static void set_send_adv_data(const void *buf, uint8_t size,
 548                                                         void *user_data)
 549 {
 550         struct mesh_io_private *pvt = user_data;
 551         struct tx_pkt *tx;
 552         struct bt_hci_cmd_le_set_adv_data cmd;
 553
 554         if (!pvt || !pvt->tx)
 555                 return;
 556
 557         tx = pvt->tx;
 558         if (tx->len >= sizeof(cmd.data))
 559                 goto done;
 560
 561         memset(&cmd, 0, sizeof(cmd));
 562
 563         cmd.len = tx->len + 1;
 564         cmd.data[0] = tx->len;
 565         memcpy(cmd.data + 1, tx->pkt, tx->len);
 566
 567         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_DATA,
 568                                         &cmd, sizeof(cmd),
 569                                         set_send_adv_enable, pvt, NULL);
 570 done:
 571         if (tx->delete)
 572                 l_free(tx);
 573
 574         pvt->tx = NULL;
 575 }
 576
 577 static void set_send_adv_params(const void *buf, uint8_t size,
 578                                                         void *user_data)
 579 {
 580         struct mesh_io_private *pvt = user_data;
 581         struct bt_hci_cmd_le_set_adv_parameters cmd;
 582         uint16_t hci_interval;
 583
 584         if (!pvt)
 585                 return;
 586
 587         hci_interval = (pvt->interval * 16) / 10;
 588         cmd.min_interval = L_CPU_TO_LE16(hci_interval);
 589         cmd.max_interval = L_CPU_TO_LE16(hci_interval);
 590         cmd.type = 0x03; /* ADV_NONCONN_IND */
 591         cmd.own_addr_type = 0x01; /* ADDR_TYPE_RANDOM */
 592         cmd.direct_addr_type = 0x00;
 593         memset(cmd.direct_addr, 0, 6);
 594         cmd.channel_map = 0x07;
 595         cmd.filter_policy = 0x03;
 596
 597         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
 598                                 &cmd, sizeof(cmd),
 599                                 set_send_adv_data, pvt, NULL);
 600 }
 601
 602 static void send_pkt(struct mesh_io_private *pvt, struct tx_pkt *tx,
 603                                                         uint16_t interval)
 604 {
 605         struct bt_hci_cmd_le_set_adv_enable cmd;
 606
 607         pvt->tx = tx;
 608         pvt->interval = interval;
 609
 610         if (!pvt->sending) {
 611                 set_send_adv_params(NULL, 0, pvt);
 612                 return;
 613         }
 614
 615         cmd.enable = 0x00;      /* Disable advertising */
 616         bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
 617                                 &cmd, sizeof(cmd),
 618                                 set_send_adv_params, pvt, NULL);
 619 }
 620
 621 static void tx_timeout(struct l_timeout *timeout, void *user_data)
 622 {
 623         struct mesh_io_private *pvt = user_data;
 624         struct tx_pkt *tx;
 625         uint16_t ms;
 626         uint8_t count;
 627
 628         if (!pvt)
 629                 return;
 630
 631         tx = l_queue_pop_head(pvt->tx_pkts);
 632         if (!tx) {
 633                 l_timeout_remove(timeout);
 634                 pvt->tx_timeout = NULL;
 635                 send_cancel(pvt);
 636                 return;
 637         }
 638
 639         if (tx->info.type == MESH_IO_TIMING_TYPE_GENERAL) {
 640                 ms = tx->info.u.gen.interval;
 641                 count = tx->info.u.gen.cnt;
 642                 if (count != MESH_IO_TX_COUNT_UNLIMITED)
 643                         tx->info.u.gen.cnt--;
 644         } else {
 645                 ms = 25;
 646                 count = 1;
 647         }
 648
 649         tx->delete = !!(count == 1);
 650
 651         send_pkt(pvt, tx, ms);
 652
 653         if (count == 1) {
 654                 /* send_pkt will delete when done */
 655                 tx = l_queue_peek_head(pvt->tx_pkts);
 656                 if (tx && tx->info.type == MESH_IO_TIMING_TYPE_POLL_RSP) {
 657                         ms = instant_remaining_ms(tx->info.u.poll_rsp.instant +
 658                                                 tx->info.u.poll_rsp.delay);
 659                 }
 660         } else
 661                 l_queue_push_tail(pvt->tx_pkts, tx);
 662
 663         if (timeout) {
 664                 pvt->tx_timeout = timeout;
 665                 l_timeout_modify_ms(timeout, ms);
 666         } else
 667                 pvt->tx_timeout = l_timeout_create_ms(ms, tx_timeout,
 668                                                                 pvt, NULL);
 669 }
 670
 671 static void tx_worker(void *user_data)
 672 {
 673         struct mesh_io_private *pvt = user_data;
 674         struct tx_pkt *tx;
 675         uint32_t delay;
 676
 677         tx = l_queue_peek_head(pvt->tx_pkts);
 678         if (!tx)
 679                 return;
 680
 681         switch (tx->info.type) {
 682         case MESH_IO_TIMING_TYPE_GENERAL:
 683                 if (tx->info.u.gen.min_delay == tx->info.u.gen.max_delay)
 684                         delay = tx->info.u.gen.min_delay;
 685                 else {
 686                         l_getrandom(&delay, sizeof(delay));
 687                         delay %= tx->info.u.gen.max_delay -
 688                                                 tx->info.u.gen.min_delay;
 689                         delay += tx->info.u.gen.min_delay;
 690                 }
 691                 break;
 692
 693         case MESH_IO_TIMING_TYPE_POLL:
 694                 if (tx->info.u.poll.min_delay == tx->info.u.poll.max_delay)
 695                         delay = tx->info.u.poll.min_delay;
 696                 else {
 697                         l_getrandom(&delay, sizeof(delay));
 698                         delay %= tx->info.u.poll.max_delay -
 699                                                 tx->info.u.poll.min_delay;
 700                         delay += tx->info.u.poll.min_delay;
 701                 }
 702                 break;
 703
 704         case MESH_IO_TIMING_TYPE_POLL_RSP:
 705                 /* Delay until Instant + Delay */
 706                 delay = instant_remaining_ms(tx->info.u.poll_rsp.instant +
 707                                                 tx->info.u.poll_rsp.delay);
 708                 if (delay > 255)
 709                         delay = 0;
 710                 break;
 711
 712         default:
 713                 return;
 714         }
 715
 716         if (!delay)
 717                 tx_timeout(pvt->tx_timeout, pvt);
 718         else if (pvt->tx_timeout)
 719                 l_timeout_modify_ms(pvt->tx_timeout, delay);
 720         else
 721                 pvt->tx_timeout = l_timeout_create_ms(delay, tx_timeout,
 722                                                                 pvt, NULL);
 723 }
 724
 725 static bool send_tx(struct mesh_io *io, struct mesh_io_send_info *info,
 726                                         const uint8_t *data, uint16_t len)
 727 {
 728         struct mesh_io_private *pvt = io->pvt;
 729         struct tx_pkt *tx;
 730         bool sending = false;
 731
 732         if (!info || !data || !len || len > sizeof(tx->pkt))
 733                 return false;
 734
 735         tx = l_new(struct tx_pkt, 1);
 736         if (!tx)
 737                 return false;
 738
 739         memcpy(&tx->info, info, sizeof(tx->info));
 740         memcpy(&tx->pkt, data, len);
 741         tx->len = len;
 742
 743         if (info->type == MESH_IO_TIMING_TYPE_POLL_RSP)
 744                 l_queue_push_head(pvt->tx_pkts, tx);
 745         else {
 746                 sending = !l_queue_isempty(pvt->tx_pkts);
 747                 l_queue_push_tail(pvt->tx_pkts, tx);
 748         }
 749
 750         if (!sending) {
 751                 l_timeout_remove(pvt->tx_timeout);
 752                 pvt->tx_timeout = NULL;
 753                 l_idle_oneshot(tx_worker, pvt, NULL);
 754         }
 755
 756         return true;
 757 }
 758
 759 static bool find_by_ad_type(const void *a, const void *b)
 760 {
 761         const struct tx_pkt *tx = a;
 762         uint8_t ad_type = L_PTR_TO_UINT(b);
 763
 764         return !ad_type || ad_type == tx->pkt[0];
 765 }
 766
 767 static bool find_by_pattern(const void *a, const void *b)
 768 {
 769         const struct tx_pkt *tx = a;
 770         const struct tx_pattern *pattern = b;
 771
 772         if (tx->len < pattern->len)
 773                 return false;
 774
 775         return (!memcmp(tx->pkt, pattern->data, pattern->len));
 776 }
 777
 778 static bool tx_cancel(struct mesh_io *io, const uint8_t *data, uint8_t len)
 779 {
 780         struct mesh_io_private *pvt = io->pvt;
 781         struct tx_pkt *tx;
 782
 783         if (!data)
 784                 return false;
 785
 786         if (len == 1) {
 787                 do {
 788                         tx = l_queue_remove_if(pvt->tx_pkts, find_by_ad_type,
 789                                                         L_UINT_TO_PTR(data[0]));
 790                         l_free(tx);
 791
 792                         if (tx == pvt->tx)
 793                                 pvt->tx = NULL;
 794
 795                 } while (tx);
 796         } else {
 797                 struct tx_pattern pattern = {
 798                         .data = data,
 799                         .len = len
 800                 };
 801
 802                 do {
 803                         tx = l_queue_remove_if(pvt->tx_pkts, find_by_pattern,
 804                                                                 &pattern);
 805                         l_free(tx);
 806
 807                         if (tx == pvt->tx)
 808                                 pvt->tx = NULL;
 809
 810                 } while (tx);
 811         }
 812
 813         if (l_queue_isempty(pvt->tx_pkts)) {
 814                 send_cancel(pvt);
 815                 l_timeout_remove(pvt->tx_timeout);
 816                 pvt->tx_timeout = NULL;
 817         }
 818
 819         return true;
 820 }
 821
 822 static bool find_by_filter(const void *a, const void *b)
 823 {
 824         const struct pvt_rx_reg *rx_reg = a;
 825         const uint8_t *filter = b;
 826
 827         return !memcmp(rx_reg->filter, filter, rx_reg->len);
 828 }
 829
 830 static bool recv_register(struct mesh_io *io, const uint8_t *filter,
 831                         uint8_t len, mesh_io_recv_func_t cb, void *user_data)
 832 {
 833         struct bt_hci_cmd_le_set_scan_enable cmd;
 834         struct mesh_io_private *pvt = io->pvt;
 835         struct pvt_rx_reg *rx_reg;
 836         bool already_scanning;
 837         bool active = false;
 838
 839         if (!cb || !filter || !len)
 840                 return false;
 841
 842         l_info("%s %2.2x", __func__, filter[0]);
 843         rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter);
 844
 845         l_free(rx_reg);
 846         rx_reg = l_malloc(sizeof(*rx_reg) + len);
 847
 848         memcpy(rx_reg->filter, filter, len);
 849         rx_reg->len = len;
 850         rx_reg->cb = cb;
 851         rx_reg->user_data = user_data;
 852
 853         already_scanning = !l_queue_isempty(pvt->rx_regs);
 854
 855         l_queue_push_head(pvt->rx_regs, rx_reg);
 856
 857         /* Look for any AD types requiring Active Scanning */
 858         if (l_queue_find(pvt->rx_regs, find_active, NULL))
 859                 active = true;
 860
 861         if (!already_scanning || pvt->active != active) {
 862                 pvt->active = active;
 863                 cmd.enable = 0x00;      /* Disable scanning */
 864                 cmd.filter_dup = 0x00;  /* Report duplicates */
 865                 bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
 866                                 &cmd, sizeof(cmd), scan_disable_rsp, pvt, NULL);
 867
 868         }
 869
 870         return true;
 871 }
 872
 873 static bool recv_deregister(struct mesh_io *io, const uint8_t *filter,
 874                                                                 uint8_t len)
 875 {
 876         struct bt_hci_cmd_le_set_scan_enable cmd = {0, 0};
 877         struct mesh_io_private *pvt = io->pvt;
 878         struct pvt_rx_reg *rx_reg;
 879         bool active = false;
 880
 881         rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter);
 882
 883         if (rx_reg)
 884                 l_free(rx_reg);
 885
 886         /* Look for any AD types requiring Active Scanning */
 887         if (l_queue_find(pvt->rx_regs, find_active, NULL))
 888                 active = true;
 889
 890         if (l_queue_isempty(pvt->rx_regs)) {
 891                 bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
 892                                         &cmd, sizeof(cmd), NULL, NULL, NULL);
 893
 894         } else if (active != pvt->active) {
 895                 pvt->active = active;
 896                 bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
 897                                 &cmd, sizeof(cmd), scan_disable_rsp, pvt, NULL);
 898         }
 899
 900         return true;
 901 }
 902
 903 const struct mesh_io_api mesh_io_generic = {
 904         .init = dev_init,
 905         .destroy = dev_destroy,
 906         .caps = dev_caps,
 907         .send = send_tx,
 908         .reg = recv_register,
 909         .dereg = recv_deregister,
 910         .cancel = tx_cancel,
 911 };