Bluetooth: hci_sync: Use QoS to determine which PHY to scan

This used the hci_conn QoS to determine which PHY to scan when creating
a PA Sync.

Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c
index d2928baaa4ee7c15c0469b3bd8a386d6b043f85c..10aa59da735b88ae8a73a2ebb6cd491dfae10686 100644 (file)
--- a/net/bluetooth/hci_sync.c
+++ b/net/bluetooth/hci_sync.c
@@ -2754,6 +2754,14 @@ done:
        return filter_policy;
 }
 
+static void hci_le_scan_phy_params(struct hci_cp_le_scan_phy_params *cp,
+                                  u8 type, u16 interval, u16 window)
+{
+       cp->type = type;
+       cp->interval = cpu_to_le16(interval);
+       cp->window = cpu_to_le16(window);
+}
+
 static int hci_le_set_ext_scan_param_sync(struct hci_dev *hdev, u8 type,
                                          u16 interval, u16 window,
                                          u8 own_addr_type, u8 filter_policy)
@@ -2761,7 +2769,7 @@ static int hci_le_set_ext_scan_param_sync(struct hci_dev *hdev, u8 type,
        struct hci_cp_le_set_ext_scan_params *cp;
        struct hci_cp_le_scan_phy_params *phy;
        u8 data[sizeof(*cp) + sizeof(*phy) * 2];
-       u8 num_phy = 0;
+       u8 num_phy = 0x00;
 
        cp = (void *)data;
        phy = (void *)cp->data;
@@ -2771,28 +2779,64 @@ static int hci_le_set_ext_scan_param_sync(struct hci_dev *hdev, u8 type,
        cp->own_addr_type = own_addr_type;
        cp->filter_policy = filter_policy;
 
-       if (scan_1m(hdev) || scan_2m(hdev)) {
-               cp->scanning_phys |= LE_SCAN_PHY_1M;
+       /* Check if PA Sync is in progress then select the PHY based on the
+        * hci_conn.iso_qos.
+        */
+       if (hci_dev_test_flag(hdev, HCI_PA_SYNC)) {
+               struct hci_cp_le_add_to_accept_list *sent;
 
-               phy->type = type;
-               phy->interval = cpu_to_le16(interval);
-               phy->window = cpu_to_le16(window);
+               sent = hci_sent_cmd_data(hdev, HCI_OP_LE_ADD_TO_ACCEPT_LIST);
+               if (sent) {
+                       struct hci_conn *conn;
+
+                       conn = hci_conn_hash_lookup_ba(hdev, ISO_LINK,
+                                                      &sent->bdaddr);
+                       if (conn) {
+                               struct bt_iso_qos *qos = &conn->iso_qos;
+
+                               if (qos->bcast.in.phy & BT_ISO_PHY_1M ||
+                                   qos->bcast.in.phy & BT_ISO_PHY_2M) {
+                                       cp->scanning_phys |= LE_SCAN_PHY_1M;
+                                       hci_le_scan_phy_params(phy, type,
+                                                              interval,
+                                                              window);
+                                       num_phy++;
+                                       phy++;
+                               }
+
+                               if (qos->bcast.in.phy & BT_ISO_PHY_CODED) {
+                                       cp->scanning_phys |= LE_SCAN_PHY_CODED;
+                                       hci_le_scan_phy_params(phy, type,
+                                                              interval,
+                                                              window);
+                                       num_phy++;
+                                       phy++;
+                               }
+
+                               if (num_phy)
+                                       goto done;
+                       }
+               }
+       }
 
+       if (scan_1m(hdev) || scan_2m(hdev)) {
+               cp->scanning_phys |= LE_SCAN_PHY_1M;
+               hci_le_scan_phy_params(phy, type, interval, window);
                num_phy++;
                phy++;
        }
 
        if (scan_coded(hdev)) {
                cp->scanning_phys |= LE_SCAN_PHY_CODED;
-
-               phy->type = type;
-               phy->interval = cpu_to_le16(interval);
-               phy->window = cpu_to_le16(window);
-
+               hci_le_scan_phy_params(phy, type, interval, window);
                num_phy++;
                phy++;
        }
 
+done:
+       if (!num_phy)
+               return -EINVAL;
+
        return __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_EXT_SCAN_PARAMS,
                                     sizeof(*cp) + sizeof(*phy) * num_phy,
                                     data, HCI_CMD_TIMEOUT);