__u8 evt_prefix_len;
__u8 *evt_prefix;
struct list_head handle_map;
- __u16 pending_add_handle;
- __u16 pending_remove_handle;
__u8 resuming;
__u8 suspending;
__u8 filter_enabled;
};
-static int __msft_add_monitor_pattern(struct hci_dev *hdev,
- struct adv_monitor *monitor);
-static int __msft_remove_monitor(struct hci_dev *hdev,
- struct adv_monitor *monitor, u16 handle);
-
bool msft_monitor_supported(struct hci_dev *hdev)
{
return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
return false;
}
-static void reregister_monitor(struct hci_dev *hdev, int handle)
-{
- struct adv_monitor *monitor;
- struct msft_data *msft = hdev->msft_data;
- int err;
-
- while (1) {
- monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
- if (!monitor) {
- /* All monitors have been resumed */
- msft->resuming = false;
- hci_update_passive_scan(hdev);
- return;
- }
-
- msft->pending_add_handle = (u16)handle;
- err = __msft_add_monitor_pattern(hdev, monitor);
-
- /* If success, we return and wait for monitor added callback */
- if (!err)
- return;
-
- /* Otherwise remove the monitor and keep registering */
- hci_free_adv_monitor(hdev, monitor);
- handle++;
- }
-}
-
/* is_mgmt = true matches the handle exposed to userspace via mgmt.
* is_mgmt = false matches the handle used by the msft controller.
* This function requires the caller holds hdev->lock
return count;
}
-static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
- u8 status, u16 opcode,
- struct sk_buff *skb)
+static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
+ struct adv_monitor *monitor,
+ struct sk_buff *skb)
{
struct msft_rp_le_monitor_advertisement *rp;
- struct adv_monitor *monitor;
struct msft_monitor_advertisement_handle_data *handle_data;
struct msft_data *msft = hdev->msft_data;
+ int status = 0;
hci_dev_lock(hdev);
- monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
- if (!monitor) {
- bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
- msft->pending_add_handle);
+ rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
+ if (skb->len < sizeof(*rp)) {
status = HCI_ERROR_UNSPECIFIED;
goto unlock;
}
+ status = rp->status;
if (status)
goto unlock;
- rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
- if (skb->len < sizeof(*rp)) {
- status = HCI_ERROR_UNSPECIFIED;
- goto unlock;
- }
-
handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
if (!handle_data) {
status = HCI_ERROR_UNSPECIFIED;
monitor->state = ADV_MONITOR_STATE_OFFLOADED;
unlock:
- if (status && monitor)
+ if (status)
hci_free_adv_monitor(hdev, monitor);
hci_dev_unlock(hdev);
- if (!msft->resuming)
- hci_add_adv_patterns_monitor_complete(hdev, status);
+ return status;
}
-static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
- u8 status, u16 opcode,
- struct sk_buff *skb)
+static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
+ u16 opcode,
+ struct adv_monitor *monitor,
+ struct sk_buff *skb)
{
- struct msft_cp_le_cancel_monitor_advertisement *cp;
struct msft_rp_le_cancel_monitor_advertisement *rp;
- struct adv_monitor *monitor;
struct msft_monitor_advertisement_handle_data *handle_data;
struct msft_data *msft = hdev->msft_data;
- int err;
- bool pending;
-
- if (status)
- goto done;
+ int status = 0;
rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
if (skb->len < sizeof(*rp)) {
goto done;
}
+ status = rp->status;
+ if (status)
+ goto done;
+
hci_dev_lock(hdev);
- cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
- handle_data = msft_find_handle_data(hdev, cp->handle, false);
+ handle_data = msft_find_handle_data(hdev, monitor->handle, true);
if (handle_data) {
- monitor = idr_find(&hdev->adv_monitors_idr,
- handle_data->mgmt_handle);
-
- if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
+ if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)
monitor->state = ADV_MONITOR_STATE_REGISTERED;
/* Do not free the monitor if it is being removed due to
* suspend. It will be re-monitored on resume.
*/
- if (monitor && !msft->suspending) {
+ if (!msft->suspending) {
hci_free_adv_monitor(hdev, monitor);
/* Clear any monitored devices by this Adv Monitor */
kfree(handle_data);
}
- /* If remove all monitors is required, we need to continue the process
- * here because the earlier it was paused when waiting for the
- * response from controller.
- */
- if (msft->pending_remove_handle == 0) {
- pending = hci_remove_all_adv_monitor(hdev, &err);
- if (pending) {
- hci_dev_unlock(hdev);
- return;
- }
-
- if (err)
- status = HCI_ERROR_UNSPECIFIED;
- }
-
hci_dev_unlock(hdev);
done:
- if (!msft->suspending)
- hci_remove_adv_monitor_complete(hdev, status);
+ return status;
}
+/* This function requires the caller holds hci_req_sync_lock */
static int msft_remove_monitor_sync(struct hci_dev *hdev,
struct adv_monitor *monitor)
{
struct msft_cp_le_cancel_monitor_advertisement cp;
struct msft_monitor_advertisement_handle_data *handle_data;
struct sk_buff *skb;
- u8 status;
handle_data = msft_find_handle_data(hdev, monitor->handle, true);
if (IS_ERR(skb))
return PTR_ERR(skb);
- status = skb->data[0];
- skb_pull(skb, 1);
-
- msft_le_cancel_monitor_advertisement_cb(hdev, status, hdev->msft_opcode,
- skb);
-
- return status;
+ return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
+ monitor, skb);
}
/* This function requires the caller holds hci_req_sync_lock */
ptrdiff_t offset = 0;
u8 pattern_count = 0;
struct sk_buff *skb;
- u8 status;
if (!msft_monitor_pattern_valid(monitor))
return -EINVAL;
if (IS_ERR(skb))
return PTR_ERR(skb);
- status = skb->data[0];
- skb_pull(skb, 1);
+ return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
+ monitor, skb);
+}
- msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb);
+/* This function requires the caller holds hci_req_sync_lock */
+static void reregister_monitor(struct hci_dev *hdev)
+{
+ struct adv_monitor *monitor;
+ struct msft_data *msft = hdev->msft_data;
+ int handle = 0;
- return status;
+ if (!msft)
+ return;
+
+ msft->resuming = true;
+
+ while (1) {
+ monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
+ if (!monitor)
+ break;
+
+ msft_add_monitor_sync(hdev, monitor);
+
+ handle++;
+ }
+
+ /* All monitors have been reregistered */
+ msft->resuming = false;
}
/* This function requires the caller holds hci_req_sync_lock */
int msft_resume_sync(struct hci_dev *hdev)
{
struct msft_data *msft = hdev->msft_data;
- struct adv_monitor *monitor;
- int handle = 0;
if (!msft || !msft_monitor_supported(hdev))
return 0;
hci_dev_unlock(hdev);
- msft->resuming = true;
-
- while (1) {
- monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
- if (!monitor)
- break;
-
- msft_add_monitor_sync(hdev, monitor);
-
- handle++;
- }
-
- /* All monitors have been resumed */
- msft->resuming = false;
+ reregister_monitor(hdev);
return 0;
}
+/* This function requires the caller holds hci_req_sync_lock */
void msft_do_open(struct hci_dev *hdev)
{
struct msft_data *msft = hdev->msft_data;
/* Monitors get removed on power off, so we need to explicitly
* tell the controller to re-monitor.
*/
- reregister_monitor(hdev, 0);
+ reregister_monitor(hdev);
}
}
hci_dev_unlock(hdev);
}
-/* This function requires the caller holds hdev->lock */
-static int __msft_add_monitor_pattern(struct hci_dev *hdev,
- struct adv_monitor *monitor)
-{
- struct msft_cp_le_monitor_advertisement *cp;
- struct msft_le_monitor_advertisement_pattern_data *pattern_data;
- struct msft_le_monitor_advertisement_pattern *pattern;
- struct adv_pattern *entry;
- struct hci_request req;
- struct msft_data *msft = hdev->msft_data;
- size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
- ptrdiff_t offset = 0;
- u8 pattern_count = 0;
- int err = 0;
-
- if (!msft_monitor_pattern_valid(monitor))
- return -EINVAL;
-
- list_for_each_entry(entry, &monitor->patterns, list) {
- pattern_count++;
- total_size += sizeof(*pattern) + entry->length;
- }
-
- cp = kmalloc(total_size, GFP_KERNEL);
- if (!cp)
- return -ENOMEM;
-
- cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
- cp->rssi_high = monitor->rssi.high_threshold;
- cp->rssi_low = monitor->rssi.low_threshold;
- cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
- cp->rssi_sampling_period = monitor->rssi.sampling_period;
-
- cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
-
- pattern_data = (void *)cp->data;
- pattern_data->count = pattern_count;
-
- list_for_each_entry(entry, &monitor->patterns, list) {
- pattern = (void *)(pattern_data->data + offset);
- /* the length also includes data_type and offset */
- pattern->length = entry->length + 2;
- pattern->data_type = entry->ad_type;
- pattern->start_byte = entry->offset;
- memcpy(pattern->pattern, entry->value, entry->length);
- offset += sizeof(*pattern) + entry->length;
- }
-
- hci_req_init(&req, hdev);
- hci_req_add(&req, hdev->msft_opcode, total_size, cp);
- err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
- kfree(cp);
-
- if (!err)
- msft->pending_add_handle = monitor->handle;
-
- return err;
-}
-
-/* This function requires the caller holds hdev->lock */
+/* This function requires the caller holds hci_req_sync_lock */
int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
{
struct msft_data *msft = hdev->msft_data;
if (msft->resuming || msft->suspending)
return -EBUSY;
- return __msft_add_monitor_pattern(hdev, monitor);
+ return msft_add_monitor_sync(hdev, monitor);
}
-/* This function requires the caller holds hdev->lock */
-static int __msft_remove_monitor(struct hci_dev *hdev,
- struct adv_monitor *monitor, u16 handle)
-{
- struct msft_cp_le_cancel_monitor_advertisement cp;
- struct msft_monitor_advertisement_handle_data *handle_data;
- struct hci_request req;
- struct msft_data *msft = hdev->msft_data;
- int err = 0;
-
- handle_data = msft_find_handle_data(hdev, monitor->handle, true);
-
- /* If no matched handle, just remove without telling controller */
- if (!handle_data)
- return -ENOENT;
-
- cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
- cp.handle = handle_data->msft_handle;
-
- hci_req_init(&req, hdev);
- hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
- err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
-
- if (!err)
- msft->pending_remove_handle = handle;
-
- return err;
-}
-
-/* This function requires the caller holds hdev->lock */
-int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
- u16 handle)
+/* This function requires the caller holds hci_req_sync_lock */
+int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
{
struct msft_data *msft = hdev->msft_data;
if (msft->resuming || msft->suspending)
return -EBUSY;
- return __msft_remove_monitor(hdev, monitor, handle);
+ return msft_remove_monitor_sync(hdev, monitor);
}
void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
projects / linux-2.6-microblaze.git / blobdiff