Bluetooth: hci_core: Enable buffer flow control for SCO/eSCO

This enables buffer flow control for SCO/eSCO
(see: Bluetooth Core 6.0 spec: 6.22. Synchronous Flow Control Enable),
recently this has caused the following problem and is actually a nice
addition for the likes of Socket TX complete:

< HCI Command: Read Buffer Size (0x04|0x0005) plen 0
> HCI Event: Command Complete (0x0e) plen 11
      Read Buffer Size (0x04|0x0005) ncmd 1
        Status: Success (0x00)
        ACL MTU: 1021 ACL max packet: 5
        SCO MTU: 240  SCO max packet: 8
...
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
< SCO Data TX: Handle 257 flags 0x00 dlen 120
> HCI Event: Hardware Error (0x10) plen 1
        Code: 0x0a

To fix the code will now attempt to enable buffer flow control when
HCI_QUIRK_SYNC_FLOWCTL_SUPPORTED is set by the driver:

< HCI Command: Write Sync Fl.. (0x03|0x002f) plen 1
        Flow control: Enabled (0x01)
> HCI Event: Command Complete (0x0e) plen 4
      Write Sync Flow Control Enable (0x03|0x002f) ncmd 1
        Status: Success (0x00)

On success then HCI_SCO_FLOWCTL would be set which indicates sco_cnt
shall be used for flow contro.

Fixes: 7fedd3bb6b77 ("Bluetooth: Prioritize SCO traffic")
Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
Tested-by: Pauli Virtanen <pav@iki.fi>

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index aa684d2b079fabb80e0da1501101b24651e89f84..6da61c185c94984ae1566d245146d3fc3d935fa4 100644 (file)
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -208,6 +208,13 @@ enum {
         */
        HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED,
 
+       /* When this quirk is set consider Sync Flow Control as supported by
+        * the driver.
+        *
+        * This quirk must be set before hci_register_dev is called.
+        */
+       HCI_QUIRK_SYNC_FLOWCTL_SUPPORTED,
+
        /* When this quirk is set, the LE states reported through the
         * HCI_LE_READ_SUPPORTED_STATES are invalid/broken.
         *
@@ -448,6 +455,7 @@ enum {
        HCI_WIDEBAND_SPEECH_ENABLED,
        HCI_EVENT_FILTER_CONFIGURED,
        HCI_PA_SYNC,
+       HCI_SCO_FLOWCTL,
 
        HCI_DUT_MODE,
        HCI_VENDOR_DIAG,
@@ -1544,6 +1552,11 @@ struct hci_rp_read_tx_power {
        __s8     tx_power;
 } __packed;
 
+#define HCI_OP_WRITE_SYNC_FLOWCTL      0x0c2f
+struct hci_cp_write_sync_flowctl {
+       __u8     enable;
+} __packed;
+
 #define HCI_OP_READ_PAGE_SCAN_TYPE     0x0c46
 struct hci_rp_read_page_scan_type {
        __u8     status;

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 7966db4038ccae363e5dc61be7df6ca988ca868b..f78e4298e39ac13fccc1c8a04a80803199d401fa 100644 (file)
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1858,6 +1858,7 @@ void hci_conn_del_sysfs(struct hci_conn *conn);
 #define lmp_hold_capable(dev)      ((dev)->features[0][0] & LMP_HOLD)
 #define lmp_sniff_capable(dev)     ((dev)->features[0][0] & LMP_SNIFF)
 #define lmp_park_capable(dev)      ((dev)->features[0][1] & LMP_PARK)
+#define lmp_sco_capable(dev)       ((dev)->features[0][1] & LMP_SCO)
 #define lmp_inq_rssi_capable(dev)  ((dev)->features[0][3] & LMP_RSSI_INQ)
 #define lmp_esco_capable(dev)      ((dev)->features[0][3] & LMP_ESCO)
 #define lmp_bredr_capable(dev)     (!((dev)->features[0][4] & LMP_NO_BREDR))

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 012fc107901a6e4502fbad269034ad43450561dc..94d9147612daf0d74954cfbc9bd74c9d6cb377d2 100644 (file)
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -3552,42 +3552,27 @@ static void __check_timeout(struct hci_dev *hdev, unsigned int cnt, u8 type)
 }
 
 /* Schedule SCO */
-static void hci_sched_sco(struct hci_dev *hdev)
+static void hci_sched_sco(struct hci_dev *hdev, __u8 type)
 {
        struct hci_conn *conn;
        struct sk_buff *skb;
-       int quote;
+       int quote, *cnt;
+       unsigned int pkts = hdev->sco_pkts;
 
-       BT_DBG("%s", hdev->name);
+       bt_dev_dbg(hdev, "type %u", type);
 
-       if (!hci_conn_num(hdev, SCO_LINK))
+       if (!hci_conn_num(hdev, type) || !pkts)
                return;
 
-       while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
-               while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
-                       BT_DBG("skb %p len %d", skb, skb->len);
-                       hci_send_frame(hdev, skb);
-
-                       conn->sent++;
-                       if (conn->sent == ~0)
-                               conn->sent = 0;
-               }
-       }
-}
-
-static void hci_sched_esco(struct hci_dev *hdev)
-{
-       struct hci_conn *conn;
-       struct sk_buff *skb;
-       int quote;
-
-       BT_DBG("%s", hdev->name);
-
-       if (!hci_conn_num(hdev, ESCO_LINK))
-               return;
+       /* Use sco_pkts if flow control has not been enabled which will limit
+        * the amount of buffer sent in a row.
+        */
+       if (!hci_dev_test_flag(hdev, HCI_SCO_FLOWCTL))
+               cnt = &pkts;
+       else
+               cnt = &hdev->sco_cnt;
 
-       while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK,
-                                                    &quote))) {
+       while (*cnt && (conn = hci_low_sent(hdev, type, &quote))) {
                while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
                        BT_DBG("skb %p len %d", skb, skb->len);
                        hci_send_frame(hdev, skb);
@@ -3595,8 +3580,17 @@ static void hci_sched_esco(struct hci_dev *hdev)
                        conn->sent++;
                        if (conn->sent == ~0)
                                conn->sent = 0;
+                       (*cnt)--;
                }
        }
+
+       /* Rescheduled if all packets were sent and flow control is not enabled
+        * as there could be more packets queued that could not be sent and
+        * since no HCI_EV_NUM_COMP_PKTS event will be generated the reschedule
+        * needs to be forced.
+        */
+       if (!pkts && !hci_dev_test_flag(hdev, HCI_SCO_FLOWCTL))
+               queue_work(hdev->workqueue, &hdev->tx_work);
 }
 
 static void hci_sched_acl_pkt(struct hci_dev *hdev)
@@ -3632,8 +3626,8 @@ static void hci_sched_acl_pkt(struct hci_dev *hdev)
                        chan->conn->sent++;
 
                        /* Send pending SCO packets right away */
-                       hci_sched_sco(hdev);
-                       hci_sched_esco(hdev);
+                       hci_sched_sco(hdev, SCO_LINK);
+                       hci_sched_sco(hdev, ESCO_LINK);
                }
        }
 
@@ -3688,8 +3682,8 @@ static void hci_sched_le(struct hci_dev *hdev)
                        chan->conn->sent++;
 
                        /* Send pending SCO packets right away */
-                       hci_sched_sco(hdev);
-                       hci_sched_esco(hdev);
+                       hci_sched_sco(hdev, SCO_LINK);
+                       hci_sched_sco(hdev, ESCO_LINK);
                }
        }
 
@@ -3734,8 +3728,8 @@ static void hci_tx_work(struct work_struct *work)
 
        if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) {
                /* Schedule queues and send stuff to HCI driver */
-               hci_sched_sco(hdev);
-               hci_sched_esco(hdev);
+               hci_sched_sco(hdev, SCO_LINK);
+               hci_sched_sco(hdev, ESCO_LINK);
                hci_sched_iso(hdev);
                hci_sched_acl(hdev);
                hci_sched_le(hdev);

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 19e19c9f5e68a70458745feedbe9a9782785da0e..6d0138b778aa257272f29a0d2933ab3ad8ad295e 100644 (file)
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -4445,9 +4445,11 @@ static void hci_num_comp_pkts_evt(struct hci_dev *hdev, void *data,
                        break;
 
                case SCO_LINK:
+               case ESCO_LINK:
                        hdev->sco_cnt += count;
                        if (hdev->sco_cnt > hdev->sco_pkts)
                                hdev->sco_cnt = hdev->sco_pkts;
+
                        break;
 
                case ISO_LINK:

diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c
index c4c2cf51b219ca5281b75d2b7b6a90d01f0e7c10..609b035e5c9041f032af6d33c055ecad70080030 100644 (file)
--- a/net/bluetooth/hci_sync.c
+++ b/net/bluetooth/hci_sync.c
@@ -3769,6 +3769,28 @@ static int hci_write_ca_timeout_sync(struct hci_dev *hdev)
                                     sizeof(param), &param, HCI_CMD_TIMEOUT);
 }
 
+/* Enable SCO flow control if supported */
+static int hci_write_sync_flowctl_sync(struct hci_dev *hdev)
+{
+       struct hci_cp_write_sync_flowctl cp;
+       int err;
+
+       /* Check if the controller supports SCO and HCI_OP_WRITE_SYNC_FLOWCTL */
+       if (!lmp_sco_capable(hdev) || !(hdev->commands[10] & BIT(4)) ||
+           !test_bit(HCI_QUIRK_SYNC_FLOWCTL_SUPPORTED, &hdev->quirks))
+               return 0;
+
+       memset(&cp, 0, sizeof(cp));
+       cp.enable = 0x01;
+
+       err = __hci_cmd_sync_status(hdev, HCI_OP_WRITE_SYNC_FLOWCTL,
+                                   sizeof(cp), &cp, HCI_CMD_TIMEOUT);
+       if (!err)
+               hci_dev_set_flag(hdev, HCI_SCO_FLOWCTL);
+
+       return err;
+}
+
 /* BR Controller init stage 2 command sequence */
 static const struct hci_init_stage br_init2[] = {
        /* HCI_OP_READ_BUFFER_SIZE */
@@ -3787,6 +3809,8 @@ static const struct hci_init_stage br_init2[] = {
        HCI_INIT(hci_clear_event_filter_sync),
        /* HCI_OP_WRITE_CA_TIMEOUT */
        HCI_INIT(hci_write_ca_timeout_sync),
+       /* HCI_OP_WRITE_SYNC_FLOWCTL */
+       HCI_INIT(hci_write_sync_flowctl_sync),
        {}
 };