OSDN Git Service

Merge tag 'sound-fix-4.2-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/tiwai...
[uclinux-h8/linux.git] / net / bluetooth / mgmt.c
index 55b9153..7998fb2 100644 (file)
@@ -38,7 +38,7 @@
 #include "mgmt_util.h"
 
 #define MGMT_VERSION   1
-#define MGMT_REVISION  9
+#define MGMT_REVISION  10
 
 static const u16 mgmt_commands[] = {
        MGMT_OP_READ_INDEX_LIST,
@@ -872,15 +872,22 @@ static u8 create_default_scan_rsp_data(struct hci_dev *hdev, u8 *ptr)
        return ad_len;
 }
 
-static u8 create_instance_scan_rsp_data(struct hci_dev *hdev, u8 *ptr)
+static u8 create_instance_scan_rsp_data(struct hci_dev *hdev, u8 instance,
+                                       u8 *ptr)
 {
+       struct adv_info *adv_instance;
+
+       adv_instance = hci_find_adv_instance(hdev, instance);
+       if (!adv_instance)
+               return 0;
+
        /* TODO: Set the appropriate entries based on advertising instance flags
         * here once flags other than 0 are supported.
         */
-       memcpy(ptr, hdev->adv_instance.scan_rsp_data,
-              hdev->adv_instance.scan_rsp_len);
+       memcpy(ptr, adv_instance->scan_rsp_data,
+              adv_instance->scan_rsp_len);
 
-       return hdev->adv_instance.scan_rsp_len;
+       return adv_instance->scan_rsp_len;
 }
 
 static void update_inst_scan_rsp_data(struct hci_request *req, u8 instance)
@@ -895,7 +902,7 @@ static void update_inst_scan_rsp_data(struct hci_request *req, u8 instance)
        memset(&cp, 0, sizeof(cp));
 
        if (instance)
-               len = create_instance_scan_rsp_data(hdev, cp.data);
+               len = create_instance_scan_rsp_data(hdev, instance, cp.data);
        else
                len = create_default_scan_rsp_data(hdev, cp.data);
 
@@ -960,41 +967,65 @@ static bool get_connectable(struct hci_dev *hdev)
 static u32 get_adv_instance_flags(struct hci_dev *hdev, u8 instance)
 {
        u32 flags;
+       struct adv_info *adv_instance;
 
-       if (instance > 0x01)
-               return 0;
+       if (instance == 0x00) {
+               /* Instance 0 always manages the "Tx Power" and "Flags"
+                * fields
+                */
+               flags = MGMT_ADV_FLAG_TX_POWER | MGMT_ADV_FLAG_MANAGED_FLAGS;
 
-       if (instance == 0x01)
-               return hdev->adv_instance.flags;
+               /* For instance 0, the HCI_ADVERTISING_CONNECTABLE setting
+                * corresponds to the "connectable" instance flag.
+                */
+               if (hci_dev_test_flag(hdev, HCI_ADVERTISING_CONNECTABLE))
+                       flags |= MGMT_ADV_FLAG_CONNECTABLE;
 
-       /* Instance 0 always manages the "Tx Power" and "Flags" fields */
-       flags = MGMT_ADV_FLAG_TX_POWER | MGMT_ADV_FLAG_MANAGED_FLAGS;
+               return flags;
+       }
 
-       /* For instance 0, the HCI_ADVERTISING_CONNECTABLE setting corresponds
-        * to the "connectable" instance flag.
-        */
-       if (hci_dev_test_flag(hdev, HCI_ADVERTISING_CONNECTABLE))
-               flags |= MGMT_ADV_FLAG_CONNECTABLE;
+       adv_instance = hci_find_adv_instance(hdev, instance);
 
-       return flags;
+       /* Return 0 when we got an invalid instance identifier. */
+       if (!adv_instance)
+               return 0;
+
+       return adv_instance->flags;
 }
 
-static u8 get_adv_instance_scan_rsp_len(struct hci_dev *hdev, u8 instance)
+static u8 get_cur_adv_instance_scan_rsp_len(struct hci_dev *hdev)
 {
-       /* Ignore instance 0 and other unsupported instances */
-       if (instance != 0x01)
+       u8 instance = get_current_adv_instance(hdev);
+       struct adv_info *adv_instance;
+
+       /* Ignore instance 0 */
+       if (instance == 0x00)
+               return 0;
+
+       adv_instance = hci_find_adv_instance(hdev, instance);
+       if (!adv_instance)
                return 0;
 
        /* TODO: Take into account the "appearance" and "local-name" flags here.
         * These are currently being ignored as they are not supported.
         */
-       return hdev->adv_instance.scan_rsp_len;
+       return adv_instance->scan_rsp_len;
 }
 
 static u8 create_instance_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr)
 {
+       struct adv_info *adv_instance = NULL;
        u8 ad_len = 0, flags = 0;
-       u32 instance_flags = get_adv_instance_flags(hdev, instance);
+       u32 instance_flags;
+
+       /* Return 0 when the current instance identifier is invalid. */
+       if (instance) {
+               adv_instance = hci_find_adv_instance(hdev, instance);
+               if (!adv_instance)
+                       return 0;
+       }
+
+       instance_flags = get_adv_instance_flags(hdev, instance);
 
        /* The Add Advertising command allows userspace to set both the general
         * and limited discoverable flags.
@@ -1028,12 +1059,11 @@ static u8 create_instance_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr)
                }
        }
 
-       if (instance) {
-               memcpy(ptr, hdev->adv_instance.adv_data,
-                      hdev->adv_instance.adv_data_len);
-
-               ad_len += hdev->adv_instance.adv_data_len;
-               ptr += hdev->adv_instance.adv_data_len;
+       if (adv_instance) {
+               memcpy(ptr, adv_instance->adv_data,
+                      adv_instance->adv_data_len);
+               ad_len += adv_instance->adv_data_len;
+               ptr += adv_instance->adv_data_len;
        }
 
        /* Provide Tx Power only if we can provide a valid value for it */
@@ -1259,7 +1289,7 @@ static void enable_advertising(struct hci_request *req)
 
        if (connectable)
                cp.type = LE_ADV_IND;
-       else if (get_adv_instance_scan_rsp_len(hdev, instance))
+       else if (get_cur_adv_instance_scan_rsp_len(hdev))
                cp.type = LE_ADV_SCAN_IND;
        else
                cp.type = LE_ADV_NONCONN_IND;
@@ -1441,27 +1471,141 @@ static void advertising_removed(struct sock *sk, struct hci_dev *hdev,
        mgmt_event(MGMT_EV_ADVERTISING_REMOVED, hdev, &ev, sizeof(ev), sk);
 }
 
-static void clear_adv_instance(struct hci_dev *hdev)
-{
-       struct hci_request req;
+static int schedule_adv_instance(struct hci_request *req, u8 instance,
+                                bool force) {
+       struct hci_dev *hdev = req->hdev;
+       struct adv_info *adv_instance = NULL;
+       u16 timeout;
 
-       if (!hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE))
-               return;
+       if (hci_dev_test_flag(hdev, HCI_ADVERTISING) ||
+           !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE))
+               return -EPERM;
 
        if (hdev->adv_instance_timeout)
+               return -EBUSY;
+
+       adv_instance = hci_find_adv_instance(hdev, instance);
+       if (!adv_instance)
+               return -ENOENT;
+
+       /* A zero timeout means unlimited advertising. As long as there is
+        * only one instance, duration should be ignored. We still set a timeout
+        * in case further instances are being added later on.
+        *
+        * If the remaining lifetime of the instance is more than the duration
+        * then the timeout corresponds to the duration, otherwise it will be
+        * reduced to the remaining instance lifetime.
+        */
+       if (adv_instance->timeout == 0 ||
+           adv_instance->duration <= adv_instance->remaining_time)
+               timeout = adv_instance->duration;
+       else
+               timeout = adv_instance->remaining_time;
+
+       /* The remaining time is being reduced unless the instance is being
+        * advertised without time limit.
+        */
+       if (adv_instance->timeout)
+               adv_instance->remaining_time =
+                               adv_instance->remaining_time - timeout;
+
+       hdev->adv_instance_timeout = timeout;
+       queue_delayed_work(hdev->workqueue,
+                          &hdev->adv_instance_expire,
+                          msecs_to_jiffies(timeout * 1000));
+
+       /* If we're just re-scheduling the same instance again then do not
+        * execute any HCI commands. This happens when a single instance is
+        * being advertised.
+        */
+       if (!force && hdev->cur_adv_instance == instance &&
+           hci_dev_test_flag(hdev, HCI_LE_ADV))
+               return 0;
+
+       hdev->cur_adv_instance = instance;
+       update_adv_data(req);
+       update_scan_rsp_data(req);
+       enable_advertising(req);
+
+       return 0;
+}
+
+static void cancel_adv_timeout(struct hci_dev *hdev)
+{
+       if (hdev->adv_instance_timeout) {
+               hdev->adv_instance_timeout = 0;
                cancel_delayed_work(&hdev->adv_instance_expire);
+       }
+}
 
-       memset(&hdev->adv_instance, 0, sizeof(hdev->adv_instance));
-       advertising_removed(NULL, hdev, 1);
-       hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE);
+/* For a single instance:
+ * - force == true: The instance will be removed even when its remaining
+ *   lifetime is not zero.
+ * - force == false: the instance will be deactivated but kept stored unless
+ *   the remaining lifetime is zero.
+ *
+ * For instance == 0x00:
+ * - force == true: All instances will be removed regardless of their timeout
+ *   setting.
+ * - force == false: Only instances that have a timeout will be removed.
+ */
+static void clear_adv_instance(struct hci_dev *hdev, struct hci_request *req,
+                              u8 instance, bool force)
+{
+       struct adv_info *adv_instance, *n, *next_instance = NULL;
+       int err;
+       u8 rem_inst;
 
-       if (!hdev_is_powered(hdev) ||
+       /* Cancel any timeout concerning the removed instance(s). */
+       if (!instance || hdev->cur_adv_instance == instance)
+               cancel_adv_timeout(hdev);
+
+       /* Get the next instance to advertise BEFORE we remove
+        * the current one. This can be the same instance again
+        * if there is only one instance.
+        */
+       if (instance && hdev->cur_adv_instance == instance)
+               next_instance = hci_get_next_instance(hdev, instance);
+
+       if (instance == 0x00) {
+               list_for_each_entry_safe(adv_instance, n, &hdev->adv_instances,
+                                        list) {
+                       if (!(force || adv_instance->timeout))
+                               continue;
+
+                       rem_inst = adv_instance->instance;
+                       err = hci_remove_adv_instance(hdev, rem_inst);
+                       if (!err)
+                               advertising_removed(NULL, hdev, rem_inst);
+               }
+               hdev->cur_adv_instance = 0x00;
+       } else {
+               adv_instance = hci_find_adv_instance(hdev, instance);
+
+               if (force || (adv_instance && adv_instance->timeout &&
+                             !adv_instance->remaining_time)) {
+                       /* Don't advertise a removed instance. */
+                       if (next_instance &&
+                           next_instance->instance == instance)
+                               next_instance = NULL;
+
+                       err = hci_remove_adv_instance(hdev, instance);
+                       if (!err)
+                               advertising_removed(NULL, hdev, instance);
+               }
+       }
+
+       if (list_empty(&hdev->adv_instances)) {
+               hdev->cur_adv_instance = 0x00;
+               hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE);
+       }
+
+       if (!req || !hdev_is_powered(hdev) ||
            hci_dev_test_flag(hdev, HCI_ADVERTISING))
                return;
 
-       hci_req_init(&req, hdev);
-       disable_advertising(&req);
-       hci_req_run(&req, NULL);
+       if (next_instance)
+               schedule_adv_instance(req, next_instance->instance, false);
 }
 
 static int clean_up_hci_state(struct hci_dev *hdev)
@@ -1479,8 +1623,7 @@ static int clean_up_hci_state(struct hci_dev *hdev)
                hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
        }
 
-       if (hdev->adv_instance_timeout)
-               clear_adv_instance(hdev);
+       clear_adv_instance(hdev, NULL, 0x00, false);
 
        if (hci_dev_test_flag(hdev, HCI_LE_ADV))
                disable_advertising(&req);
@@ -2435,6 +2578,9 @@ static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
        val = !!cp->val;
        enabled = lmp_host_le_capable(hdev);
 
+       if (!val)
+               clear_adv_instance(hdev, NULL, 0x00, true);
+
        if (!hdev_is_powered(hdev) || val == enabled) {
                bool changed = false;
 
@@ -4069,6 +4215,7 @@ static bool trigger_le_scan(struct hci_request *req, u16 interval, u8 *status)
                        return false;
                }
 
+               cancel_adv_timeout(hdev);
                disable_advertising(req);
        }
 
@@ -4651,6 +4798,9 @@ static void set_advertising_complete(struct hci_dev *hdev, u8 status,
 {
        struct cmd_lookup match = { NULL, hdev };
        struct hci_request req;
+       u8 instance;
+       struct adv_info *adv_instance;
+       int err;
 
        hci_dev_lock(hdev);
 
@@ -4676,18 +4826,31 @@ static void set_advertising_complete(struct hci_dev *hdev, u8 status,
                sock_put(match.sk);
 
        /* If "Set Advertising" was just disabled and instance advertising was
-        * set up earlier, then enable the advertising instance.
+        * set up earlier, then re-enable multi-instance advertising.
         */
        if (hci_dev_test_flag(hdev, HCI_ADVERTISING) ||
-           !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE))
+           !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE) ||
+           list_empty(&hdev->adv_instances))
                goto unlock;
 
+       instance = hdev->cur_adv_instance;
+       if (!instance) {
+               adv_instance = list_first_entry_or_null(&hdev->adv_instances,
+                                                       struct adv_info, list);
+               if (!adv_instance)
+                       goto unlock;
+
+               instance = adv_instance->instance;
+       }
+
        hci_req_init(&req, hdev);
 
-       update_adv_data(&req);
-       enable_advertising(&req);
+       err = schedule_adv_instance(&req, instance, true);
+
+       if (!err)
+               err = hci_req_run(&req, enable_advertising_instance);
 
-       if (hci_req_run(&req, enable_advertising_instance) < 0)
+       if (err)
                BT_ERR("Failed to re-configure advertising");
 
 unlock:
@@ -4772,8 +4935,13 @@ static int set_advertising(struct sock *sk, struct hci_dev *hdev, void *data,
        else
                hci_dev_clear_flag(hdev, HCI_ADVERTISING_CONNECTABLE);
 
+       cancel_adv_timeout(hdev);
+
        if (val) {
-               /* Switch to instance "0" for the Set Advertising setting. */
+               /* Switch to instance "0" for the Set Advertising setting.
+                * We cannot use update_[adv|scan_rsp]_data() here as the
+                * HCI_ADVERTISING flag is not yet set.
+                */
                update_inst_adv_data(&req, 0x00);
                update_inst_scan_rsp_data(&req, 0x00);
                enable_advertising(&req);
@@ -6866,7 +7034,10 @@ static void add_advertising_complete(struct hci_dev *hdev, u8 status,
                                     u16 opcode)
 {
        struct mgmt_pending_cmd *cmd;
+       struct mgmt_cp_add_advertising *cp;
        struct mgmt_rp_add_advertising rp;
+       struct adv_info *adv_instance, *n;
+       u8 instance;
 
        BT_DBG("status %d", status);
 
@@ -6874,16 +7045,32 @@ static void add_advertising_complete(struct hci_dev *hdev, u8 status,
 
        cmd = pending_find(MGMT_OP_ADD_ADVERTISING, hdev);
 
-       if (status) {
+       if (status)
                hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE);
-               memset(&hdev->adv_instance, 0, sizeof(hdev->adv_instance));
-               advertising_removed(cmd ? cmd->sk : NULL, hdev, 1);
+
+       list_for_each_entry_safe(adv_instance, n, &hdev->adv_instances, list) {
+               if (!adv_instance->pending)
+                       continue;
+
+               if (!status) {
+                       adv_instance->pending = false;
+                       continue;
+               }
+
+               instance = adv_instance->instance;
+
+               if (hdev->cur_adv_instance == instance)
+                       cancel_adv_timeout(hdev);
+
+               hci_remove_adv_instance(hdev, instance);
+               advertising_removed(cmd ? cmd->sk : NULL, hdev, instance);
        }
 
        if (!cmd)
                goto unlock;
 
-       rp.instance = 0x01;
+       cp = cmd->param;
+       rp.instance = cp->instance;
 
        if (status)
                mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode,
@@ -6900,10 +7087,26 @@ unlock:
 
 void mgmt_adv_timeout_expired(struct hci_dev *hdev)
 {
+       u8 instance;
+       struct hci_request req;
+
        hdev->adv_instance_timeout = 0;
 
+       instance = get_current_adv_instance(hdev);
+       if (instance == 0x00)
+               return;
+
        hci_dev_lock(hdev);
-       clear_adv_instance(hdev);
+       hci_req_init(&req, hdev);
+
+       clear_adv_instance(hdev, &req, instance, false);
+
+       if (list_empty(&hdev->adv_instances))
+               disable_advertising(&req);
+
+       if (!skb_queue_empty(&req.cmd_q))
+               hci_req_run(&req, NULL);
+
        hci_dev_unlock(hdev);
 }
 
@@ -6915,7 +7118,10 @@ static int add_advertising(struct sock *sk, struct hci_dev *hdev,
        u32 flags;
        u32 supported_flags;
        u8 status;
-       u16 timeout;
+       u16 timeout, duration;
+       unsigned int prev_instance_cnt = hdev->adv_instance_cnt;
+       u8 schedule_instance = 0;
+       struct adv_info *next_instance;
        int err;
        struct mgmt_pending_cmd *cmd;
        struct hci_request req;
@@ -6929,12 +7135,13 @@ static int add_advertising(struct sock *sk, struct hci_dev *hdev,
 
        flags = __le32_to_cpu(cp->flags);
        timeout = __le16_to_cpu(cp->timeout);
+       duration = __le16_to_cpu(cp->duration);
 
-       /* The current implementation only supports adding one instance and only
-        * a subset of the specified flags.
+       /* The current implementation only supports a subset of the specified
+        * flags.
         */
        supported_flags = get_supported_adv_flags(hdev);
-       if (cp->instance != 0x01 || (flags & ~supported_flags))
+       if (flags & ~supported_flags)
                return mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING,
                                       MGMT_STATUS_INVALID_PARAMS);
 
@@ -6962,36 +7169,51 @@ static int add_advertising(struct sock *sk, struct hci_dev *hdev,
                goto unlock;
        }
 
-       hdev->adv_instance.flags = flags;
-       hdev->adv_instance.adv_data_len = cp->adv_data_len;
-       hdev->adv_instance.scan_rsp_len = cp->scan_rsp_len;
-
-       if (cp->adv_data_len)
-               memcpy(hdev->adv_instance.adv_data, cp->data, cp->adv_data_len);
-
-       if (cp->scan_rsp_len)
-               memcpy(hdev->adv_instance.scan_rsp_data,
-                      cp->data + cp->adv_data_len, cp->scan_rsp_len);
+       err = hci_add_adv_instance(hdev, cp->instance, flags,
+                                  cp->adv_data_len, cp->data,
+                                  cp->scan_rsp_len,
+                                  cp->data + cp->adv_data_len,
+                                  timeout, duration);
+       if (err < 0) {
+               err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING,
+                                     MGMT_STATUS_FAILED);
+               goto unlock;
+       }
 
-       if (hdev->adv_instance_timeout)
-               cancel_delayed_work(&hdev->adv_instance_expire);
+       /* Only trigger an advertising added event if a new instance was
+        * actually added.
+        */
+       if (hdev->adv_instance_cnt > prev_instance_cnt)
+               advertising_added(sk, hdev, cp->instance);
 
-       hdev->adv_instance_timeout = timeout;
+       hci_dev_set_flag(hdev, HCI_ADVERTISING_INSTANCE);
 
-       if (timeout)
-               queue_delayed_work(hdev->workqueue,
-                                  &hdev->adv_instance_expire,
-                                  msecs_to_jiffies(timeout * 1000));
-
-       if (!hci_dev_test_and_set_flag(hdev, HCI_ADVERTISING_INSTANCE))
-               advertising_added(sk, hdev, 1);
+       if (hdev->cur_adv_instance == cp->instance) {
+               /* If the currently advertised instance is being changed then
+                * cancel the current advertising and schedule the next
+                * instance. If there is only one instance then the overridden
+                * advertising data will be visible right away.
+                */
+               cancel_adv_timeout(hdev);
+
+               next_instance = hci_get_next_instance(hdev, cp->instance);
+               if (next_instance)
+                       schedule_instance = next_instance->instance;
+       } else if (!hdev->adv_instance_timeout) {
+               /* Immediately advertise the new instance if no other
+                * instance is currently being advertised.
+                */
+               schedule_instance = cp->instance;
+       }
 
-       /* If the HCI_ADVERTISING flag is set or the device isn't powered then
-        * we have no HCI communication to make. Simply return.
+       /* If the HCI_ADVERTISING flag is set or the device isn't powered or
+        * there is no instance to be advertised then we have no HCI
+        * communication to make. Simply return.
         */
        if (!hdev_is_powered(hdev) ||
-           hci_dev_test_flag(hdev, HCI_ADVERTISING)) {
-               rp.instance = 0x01;
+           hci_dev_test_flag(hdev, HCI_ADVERTISING) ||
+           !schedule_instance) {
+               rp.instance = cp->instance;
                err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_ADVERTISING,
                                        MGMT_STATUS_SUCCESS, &rp, sizeof(rp));
                goto unlock;
@@ -7009,11 +7231,11 @@ static int add_advertising(struct sock *sk, struct hci_dev *hdev,
 
        hci_req_init(&req, hdev);
 
-       update_adv_data(&req);
-       update_scan_rsp_data(&req);
-       enable_advertising(&req);
+       err = schedule_adv_instance(&req, schedule_instance, true);
+
+       if (!err)
+               err = hci_req_run(&req, add_advertising_complete);
 
-       err = hci_req_run(&req, add_advertising_complete);
        if (err < 0)
                mgmt_pending_remove(cmd);
 
@@ -7027,6 +7249,7 @@ static void remove_advertising_complete(struct hci_dev *hdev, u8 status,
                                        u16 opcode)
 {
        struct mgmt_pending_cmd *cmd;
+       struct mgmt_cp_remove_advertising *cp;
        struct mgmt_rp_remove_advertising rp;
 
        BT_DBG("status %d", status);
@@ -7041,7 +7264,8 @@ static void remove_advertising_complete(struct hci_dev *hdev, u8 status,
        if (!cmd)
                goto unlock;
 
-       rp.instance = 1;
+       cp = cmd->param;
+       rp.instance = cp->instance;
 
        mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, MGMT_STATUS_SUCCESS,
                          &rp, sizeof(rp));
@@ -7056,21 +7280,21 @@ static int remove_advertising(struct sock *sk, struct hci_dev *hdev,
 {
        struct mgmt_cp_remove_advertising *cp = data;
        struct mgmt_rp_remove_advertising rp;
-       int err;
        struct mgmt_pending_cmd *cmd;
        struct hci_request req;
+       int err;
 
        BT_DBG("%s", hdev->name);
 
-       /* The current implementation only allows modifying instance no 1. A
-        * value of 0 indicates that all instances should be cleared.
-        */
-       if (cp->instance > 1)
-               return mgmt_cmd_status(sk, hdev->id, MGMT_OP_REMOVE_ADVERTISING,
-                                      MGMT_STATUS_INVALID_PARAMS);
-
        hci_dev_lock(hdev);
 
+       if (cp->instance && !hci_find_adv_instance(hdev, cp->instance)) {
+               err = mgmt_cmd_status(sk, hdev->id,
+                                     MGMT_OP_REMOVE_ADVERTISING,
+                                     MGMT_STATUS_INVALID_PARAMS);
+               goto unlock;
+       }
+
        if (pending_find(MGMT_OP_ADD_ADVERTISING, hdev) ||
            pending_find(MGMT_OP_REMOVE_ADVERTISING, hdev) ||
            pending_find(MGMT_OP_SET_LE, hdev)) {
@@ -7085,21 +7309,21 @@ static int remove_advertising(struct sock *sk, struct hci_dev *hdev,
                goto unlock;
        }
 
-       if (hdev->adv_instance_timeout)
-               cancel_delayed_work(&hdev->adv_instance_expire);
-
-       memset(&hdev->adv_instance, 0, sizeof(hdev->adv_instance));
+       hci_req_init(&req, hdev);
 
-       advertising_removed(sk, hdev, 1);
+       clear_adv_instance(hdev, &req, cp->instance, true);
 
-       hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE);
+       if (list_empty(&hdev->adv_instances))
+               disable_advertising(&req);
 
-       /* If the HCI_ADVERTISING flag is set or the device isn't powered then
-        * we have no HCI communication to make. Simply return.
+       /* If no HCI commands have been collected so far or the HCI_ADVERTISING
+        * flag is set or the device isn't powered then we have no HCI
+        * communication to make. Simply return.
         */
-       if (!hdev_is_powered(hdev) ||
+       if (skb_queue_empty(&req.cmd_q) ||
+           !hdev_is_powered(hdev) ||
            hci_dev_test_flag(hdev, HCI_ADVERTISING)) {
-               rp.instance = 1;
+               rp.instance = cp->instance;
                err = mgmt_cmd_complete(sk, hdev->id,
                                        MGMT_OP_REMOVE_ADVERTISING,
                                        MGMT_STATUS_SUCCESS, &rp, sizeof(rp));
@@ -7113,9 +7337,6 @@ static int remove_advertising(struct sock *sk, struct hci_dev *hdev,
                goto unlock;
        }
 
-       hci_req_init(&req, hdev);
-       disable_advertising(&req);
-
        err = hci_req_run(&req, remove_advertising_complete);
        if (err < 0)
                mgmt_pending_remove(cmd);
@@ -7340,6 +7561,7 @@ static void powered_complete(struct hci_dev *hdev, u8 status, u16 opcode)
 static int powered_update_hci(struct hci_dev *hdev)
 {
        struct hci_request req;
+       struct adv_info *adv_instance;
        u8 link_sec;
 
        hci_req_init(&req, hdev);
@@ -7379,14 +7601,27 @@ static int powered_update_hci(struct hci_dev *hdev)
                 * advertising data. This also applies to the case
                 * where BR/EDR was toggled during the AUTO_OFF phase.
                 */
-               if (hci_dev_test_flag(hdev, HCI_LE_ENABLED)) {
+               if (hci_dev_test_flag(hdev, HCI_LE_ENABLED) &&
+                   (hci_dev_test_flag(hdev, HCI_ADVERTISING) ||
+                    !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE))) {
                        update_adv_data(&req);
                        update_scan_rsp_data(&req);
                }
 
-               if (hci_dev_test_flag(hdev, HCI_ADVERTISING) ||
-                   hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE))
+               if (hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE) &&
+                   hdev->cur_adv_instance == 0x00 &&
+                   !list_empty(&hdev->adv_instances)) {
+                       adv_instance = list_first_entry(&hdev->adv_instances,
+                                                       struct adv_info, list);
+                       hdev->cur_adv_instance = adv_instance->instance;
+               }
+
+               if (hci_dev_test_flag(hdev, HCI_ADVERTISING))
                        enable_advertising(&req);
+               else if (hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE) &&
+                        hdev->cur_adv_instance)
+                       schedule_adv_instance(&req, hdev->cur_adv_instance,
+                                             true);
 
                restart_le_actions(&req);
        }
@@ -8371,13 +8606,24 @@ static void adv_enable_complete(struct hci_dev *hdev, u8 status, u16 opcode)
 void mgmt_reenable_advertising(struct hci_dev *hdev)
 {
        struct hci_request req;
+       u8 instance;
 
        if (!hci_dev_test_flag(hdev, HCI_ADVERTISING) &&
            !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE))
                return;
 
+       instance = get_current_adv_instance(hdev);
+
        hci_req_init(&req, hdev);
-       enable_advertising(&req);
+
+       if (instance) {
+               schedule_adv_instance(&req, instance, true);
+       } else {
+               update_adv_data(&req);
+               update_scan_rsp_data(&req);
+               enable_advertising(&req);
+       }
+
        hci_req_run(&req, adv_enable_complete);
 }