Received-SPF: pass (google.com: domain of linux-bluetooth-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) client-ip=23.128.96.18;
From:   Ayush Garg <ayush.garg@samsung.com>
To:     linux-bluetooth@vger.kernel.org
Cc:     anupam.r@samsung.com, nitin.j@samsung.com
Subject: [PATCH 2/4] Bluetooth: Implement BT_PHY setsocket option
Date:   Thu, 22 Jul 2021 11:08:41 +0530
Message-Id: <20210722053843.6691-3-ayush.garg@samsung.com>
In-Reply-To: <20210722053843.6691-1-ayush.garg@samsung.com>
Content-Type: text/plain; charset="utf-8"
CMS-TYPE: 105P
References: <20210722053843.6691-1-ayush.garg@samsung.com>
        <CGME20210722053900epcas5p48430eb1335c3e566be02135e63749f33@epcas5p4.samsung.com>
Precedence: bulk

This adds BT_PHY setsocket option which can be used
to set the PHYs for the underline LE connection.

< HCI Command: LE Set PHY (0x08|0x0032) plen 7
		Handle: 0
		All PHYs preference: 0x00
		TX PHYs preference: 0x02
		  LE 2M
		RX PHYs preference: 0x02
		  LE 2M
		PHY options preference: Reserved (0x0000)
> HCI Event: Command Status (0x0f) plen 4
	  LE Set PHY (0x08|0x0032) ncmd 1
		Status: Success (0x00)
> HCI Event: LE Meta Event (0x3e) plen 6
	  LE PHY Update Complete (0x0c)
		Status: Success (0x00)
		Handle: 0
		TX PHY: LE 2M (0x02)
		RX PHY: LE 2M (0x02)

Reviewed-by: Anupam Roy <anupam.r@samsung.com>
Signed-off-by: Ayush Garg <ayush.garg@samsung.com>
---
 include/net/bluetooth/bluetooth.h |  7 ++++
 include/net/bluetooth/hci.h       | 12 +++++++
 include/net/bluetooth/hci_core.h  |  1 +
 net/bluetooth/hci_conn.c          | 56 +++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_sock.c        | 21 +++++++++++-
 5 files changed, 96 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 9125effbf448..af53a98e4236 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -140,6 +140,13 @@ struct bt_voice {
 #define BT_PHY_LE_2M_RX		0x00001000
 #define BT_PHY_LE_CODED_TX	0x00002000
 #define BT_PHY_LE_CODED_RX	0x00004000
+#define BT_PHY_LE_CODED_S2      0x00008000
+#define BT_PHY_LE_CODED_S8      0x00010000
+
+#define BT_PHY_LE_TX_MASK (BT_PHY_LE_1M_TX | BT_PHY_LE_2M_TX | \
+			     BT_PHY_LE_CODED_TX)
+#define BT_PHY_LE_RX_MASK (BT_PHY_LE_1M_RX | BT_PHY_LE_2M_RX | \
+			     BT_PHY_LE_CODED_RX)
 
 #define BT_MODE			15
 
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 56542a09ec43..06e10ccf2a1c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -1677,10 +1677,22 @@ struct hci_cp_le_set_default_phy {
 	__u8    rx_phys;
 } __packed;
 
+#define HCI_OP_LE_SET_PHY	0x2032
+struct hci_cp_le_set_phy {
+	__le16	handle;
+	__u8	all_phys;
+	__u8	tx_phys;
+	__u8	rx_phys;
+	__le16	phy_opt;
+} __packed;
+
 #define HCI_LE_SET_PHY_1M		0x01
 #define HCI_LE_SET_PHY_2M		0x02
 #define HCI_LE_SET_PHY_CODED		0x04
 
+#define HCI_LE_PHY_CODED_S2		0x01
+#define HCI_LE_PHY_CODED_S8		0x02
+
 #define HCI_LE_READ_PHY_1M		0x01
 #define HCI_LE_READ_PHY_2M		0x02
 #define HCI_LE_READ_PHY_CODED		0x03
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index a53e94459ecd..257467f9d28d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1702,6 +1702,7 @@ struct sk_buff *hci_cmd_sync(struct hci_dev *hdev, u16 opcode, u32 plen,
 			     const void *param, u32 timeout);
 
 u32 hci_conn_get_phy(struct hci_conn *conn);
+int hci_conn_set_phy(struct hci_conn *conn, u32 phys);
 
 /* ----- HCI Sockets ----- */
 void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb);
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 383efd969840..3b69c7cb9523 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -1935,3 +1935,59 @@ u32 hci_conn_get_phy(struct hci_conn *conn)
 
 	return phys;
 }
+
+int hci_conn_set_phy(struct hci_conn *conn, u32 phys)
+{
+	struct hci_cp_le_set_phy cp_phy;
+	u16 phy_opt = 0;
+
+	if (conn->type != LE_LINK)
+		return -ENOTCONN;
+
+	/* Check whether HCI_LE_Set_PHY command is supported or not */
+	if (!(conn->hdev->commands[35] & 0x40))
+		return -EOPNOTSUPP;
+
+	hci_dev_lock(conn->hdev);
+
+	memset(&cp_phy, 0, sizeof(cp_phy));
+	cp_phy.handle = cpu_to_le16(conn->handle);
+
+	if (!(phys & BT_PHY_LE_TX_MASK))
+		cp_phy.all_phys |= 0x01;
+
+	if (!(phys & BT_PHY_LE_RX_MASK))
+		cp_phy.all_phys |= 0x02;
+
+	if (phys & BT_PHY_LE_1M_TX)
+		cp_phy.tx_phys |= HCI_LE_SET_PHY_1M;
+
+	if (phys & BT_PHY_LE_2M_TX)
+		cp_phy.tx_phys |= HCI_LE_SET_PHY_2M;
+
+	if (phys & BT_PHY_LE_CODED_TX)
+		cp_phy.tx_phys |= HCI_LE_SET_PHY_CODED;
+
+	if (phys & BT_PHY_LE_1M_RX)
+		cp_phy.rx_phys |= HCI_LE_SET_PHY_1M;
+
+	if (phys & BT_PHY_LE_2M_RX)
+		cp_phy.rx_phys |= HCI_LE_SET_PHY_2M;
+
+	if (phys & BT_PHY_LE_CODED_RX)
+		cp_phy.rx_phys |= HCI_LE_SET_PHY_CODED;
+
+	if (phys & BT_PHY_LE_CODED_S2)
+		phy_opt |= HCI_LE_PHY_CODED_S2;
+
+	if (phys & BT_PHY_LE_CODED_S8)
+		phy_opt |= HCI_LE_PHY_CODED_S8;
+
+	cp_phy.phy_opt = cpu_to_le16(phy_opt);
+
+	hci_send_cmd(conn->hdev, HCI_OP_LE_SET_PHY, sizeof(cp_phy), &cp_phy);
+
+	hci_dev_unlock(conn->hdev);
+
+	return 0;
+}
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index c99d65ef13b1..eda5d61e37be 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -875,7 +875,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
 	struct bt_power pwr;
 	struct l2cap_conn *conn;
 	int len, err = 0;
-	u32 opt;
+	u32 opt, phys;
 
 	BT_DBG("sk %p", sk);
 
@@ -1071,6 +1071,25 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
 
 		break;
 
+	case BT_PHY:
+		if (sk->sk_state != BT_CONNECTED) {
+			err = -ENOTCONN;
+			break;
+		}
+
+		if (copy_from_sockptr(&phys, optval, sizeof(u32))) {
+			err = -EFAULT;
+			break;
+		}
+
+		err = hci_conn_set_phy(chan->conn->hcon, phys);
+
+		if (err)
+			break;
+
+		BT_DBG("phys %u", phys);
+		break;
+
 	case BT_MODE:
 		if (!enable_ecred) {
 			err = -ENOPROTOOPT;
-- 
2.17.1