Bluetooth: Add handler of MGMT_OP_ADD_ADV_PATTERNS_MONITOR

This adds the request handler of MGMT_OP_ADD_ADV_PATTERNS_MONITOR command.
Note that the controller-based monitoring is not yet in place. This tracks
the content of the monitor without sending HCI traffic, so the request
returns immediately.

The following manual test was performed.
- Issue btmgmt advmon-add with valid and invalid inputs.
- Issue btmgmt advmon-add more the allowed number of monitors.

Signed-off-by: Miao-chen Chou <mcchou@chromium.org>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 4e9d510..13fad41 100644 (file)
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1281,6 +1281,8 @@ int hci_remove_adv_instance(struct hci_dev *hdev, u8 instance);
 void hci_adv_instances_set_rpa_expired(struct hci_dev *hdev, bool rpa_expired);
 
 void hci_adv_monitors_clear(struct hci_dev *hdev);
+void hci_free_adv_monitor(struct adv_monitor *monitor);
+int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor);
 
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb);
 

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 53aec32..ce481fa 100644 (file)
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2998,9 +2998,49 @@ int hci_add_adv_instance(struct hci_dev *hdev, u8 instance, u32 flags,
 /* This function requires the caller holds hdev->lock */
 void hci_adv_monitors_clear(struct hci_dev *hdev)
 {
+       struct adv_monitor *monitor;
+       int handle;
+
+       idr_for_each_entry(&hdev->adv_monitors_idr, monitor, handle)
+               hci_free_adv_monitor(monitor);
+
        idr_destroy(&hdev->adv_monitors_idr);
 }
 
+void hci_free_adv_monitor(struct adv_monitor *monitor)
+{
+       struct adv_pattern *pattern;
+       struct adv_pattern *tmp;
+
+       if (!monitor)
+               return;
+
+       list_for_each_entry_safe(pattern, tmp, &monitor->patterns, list)
+               kfree(pattern);
+
+       kfree(monitor);
+}
+
+/* This function requires the caller holds hdev->lock */
+int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
+{
+       int min, max, handle;
+
+       if (!monitor)
+               return -EINVAL;
+
+       min = HCI_MIN_ADV_MONITOR_HANDLE;
+       max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES;
+       handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max,
+                          GFP_KERNEL);
+       if (handle < 0)
+               return handle;
+
+       hdev->adv_monitors_cnt++;
+       monitor->handle = handle;
+       return 0;
+}
+
 struct bdaddr_list *hci_bdaddr_list_lookup(struct list_head *bdaddr_list,
                                         bdaddr_t *bdaddr, u8 type)
 {

diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 8aec7fb..1eca36e 100644 (file)
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -120,6 +120,7 @@ static const u16 mgmt_commands[] = {
        MGMT_OP_GET_DEVICE_FLAGS,
        MGMT_OP_SET_DEVICE_FLAGS,
        MGMT_OP_READ_ADV_MONITOR_FEATURES,
+       MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
 };
 
 static const u16 mgmt_events[] = {
@@ -4020,6 +4021,103 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
                                 MGMT_STATUS_SUCCESS, rp, rp_size);
 }
 
+static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
+                                   void *data, u16 len)
+{
+       struct mgmt_cp_add_adv_patterns_monitor *cp = data;
+       struct mgmt_rp_add_adv_patterns_monitor rp;
+       struct adv_monitor *m = NULL;
+       struct adv_pattern *p = NULL;
+       __u8 cp_ofst = 0, cp_len = 0;
+       unsigned int mp_cnt = 0;
+       int err, i;
+
+       BT_DBG("request for %s", hdev->name);
+
+       if (len <= sizeof(*cp) || cp->pattern_count == 0) {
+               err = mgmt_cmd_status(sk, hdev->id,
+                                     MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+                                     MGMT_STATUS_INVALID_PARAMS);
+               goto failed;
+       }
+
+       m = kmalloc(sizeof(*m), GFP_KERNEL);
+       if (!m) {
+               err = -ENOMEM;
+               goto failed;
+       }
+
+       INIT_LIST_HEAD(&m->patterns);
+       m->active = false;
+
+       for (i = 0; i < cp->pattern_count; i++) {
+               if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
+                       err = mgmt_cmd_status(sk, hdev->id,
+                                             MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+                                             MGMT_STATUS_INVALID_PARAMS);
+                       goto failed;
+               }
+
+               cp_ofst = cp->patterns[i].offset;
+               cp_len = cp->patterns[i].length;
+               if (cp_ofst >= HCI_MAX_AD_LENGTH ||
+                   cp_len > HCI_MAX_AD_LENGTH ||
+                   (cp_ofst + cp_len) > HCI_MAX_AD_LENGTH) {
+                       err = mgmt_cmd_status(sk, hdev->id,
+                                             MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+                                             MGMT_STATUS_INVALID_PARAMS);
+                       goto failed;
+               }
+
+               p = kmalloc(sizeof(*p), GFP_KERNEL);
+               if (!p) {
+                       err = -ENOMEM;
+                       goto failed;
+               }
+
+               p->ad_type = cp->patterns[i].ad_type;
+               p->offset = cp->patterns[i].offset;
+               p->length = cp->patterns[i].length;
+               memcpy(p->value, cp->patterns[i].value, p->length);
+
+               INIT_LIST_HEAD(&p->list);
+               list_add(&p->list, &m->patterns);
+       }
+
+       if (mp_cnt != cp->pattern_count) {
+               err = mgmt_cmd_status(sk, hdev->id,
+                                     MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+                                     MGMT_STATUS_INVALID_PARAMS);
+               goto failed;
+       }
+
+       hci_dev_lock(hdev);
+
+       err = hci_add_adv_monitor(hdev, m);
+       if (err) {
+               if (err == -ENOSPC) {
+                       mgmt_cmd_status(sk, hdev->id,
+                                       MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+                                       MGMT_STATUS_NO_RESOURCES);
+               }
+               goto unlock;
+       }
+
+       hci_dev_unlock(hdev);
+
+       rp.monitor_handle = cpu_to_le16(m->handle);
+
+       return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+                                MGMT_STATUS_SUCCESS, &rp, sizeof(rp));
+
+unlock:
+       hci_dev_unlock(hdev);
+
+failed:
+       hci_free_adv_monitor(m);
+       return err;
+}
+
 static void read_local_oob_data_complete(struct hci_dev *hdev, u8 status,
                                         u16 opcode, struct sk_buff *skb)
 {
@@ -7489,6 +7587,8 @@ static const struct hci_mgmt_handler mgmt_handlers[] = {
        { get_device_flags,        MGMT_GET_DEVICE_FLAGS_SIZE },
        { set_device_flags,        MGMT_SET_DEVICE_FLAGS_SIZE },
        { read_adv_mon_features,   MGMT_READ_ADV_MONITOR_FEATURES_SIZE },
+       { add_adv_patterns_monitor,MGMT_ADD_ADV_PATTERNS_MONITOR_SIZE,
+                                               HCI_MGMT_VAR_LEN },
 };
 
 void mgmt_index_added(struct hci_dev *hdev)