[RFC PATCH 3/3] Bluetooth: L2CAP: add new ioctl() for reading tx latency

[Pauli Virtanen <pav@iki.fi>]

Add ioctl BTGETTXINFO to read information of a previous packet
completion event concerning a given L2CAP socket.

Mark skbs submitted so that hci_core updates latency info in the
hci_chan.  Read the information in the ioctl.

The ioctl is the same as for ISO sockets.

Signed-off-by: Pauli Virtanen <pav@iki.fi>
---
 include/net/bluetooth/bluetooth.h |  2 +-
 net/bluetooth/l2cap_core.c        | 12 ++++++++
 net/bluetooth/l2cap_sock.c        | 50 ++++++++++++++++++++++++++++++-
 3 files changed, 62 insertions(+), 2 deletions(-)

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index ff4230d15461..480f5cbc24e4 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -244,7 +244,7 @@ struct bt_codecs {
  * Produces information of a previous event when a packet was sent, and the
  * length of packet queue at that time.
  *
- * Applicable to: Bluetooth ISO sockets.
+ * Applicable to: Bluetooth ISO and L2CAP sockets.
  *
  * Input: Zero-initialize reserved flag bits and other fields.
  *
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 467b242d8be0..c70f935d9242 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -2488,6 +2488,13 @@ static void l2cap_le_flowctl_send(struct l2cap_chan *chan)
 	       skb_queue_len(&chan->tx_q));
 }
 
+static void mark_tx_latency(struct l2cap_chan *chan, struct sk_buff *skb)
+{
+	struct hci_chan *hchan = chan->conn->hchan;
+
+	hci_mark_tx_latency(&hchan->tx_latency, skb);
+}
+
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
 {
 	struct sk_buff *skb;
@@ -2526,6 +2533,8 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
 		if (err)
 			return err;
 
+		mark_tx_latency(chan, skb_peek(&seg_queue));
+
 		skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
 
 		l2cap_le_flowctl_send(chan);
@@ -2547,6 +2556,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
 		if (IS_ERR(skb))
 			return PTR_ERR(skb);
 
+		mark_tx_latency(chan, skb);
 		l2cap_do_send(chan, skb);
 		err = len;
 		break;
@@ -2570,6 +2580,8 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
 		if (err)
 			break;
 
+		mark_tx_latency(chan, skb_peek(&seg_queue));
+
 		if (chan->mode == L2CAP_MODE_ERTM)
 			l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
 		else
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 4287aa6cc988..08c130bd8b98 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -1207,6 +1207,54 @@ static int l2cap_sock_recvmsg(struct socket *sock, struct msghdr *msg,
 	return err;
 }
 
+static int l2cap_sock_ioctl(struct socket *sock, unsigned int cmd,
+			    unsigned long arg)
+{
+	struct sock *sk = sock->sk;
+	struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+	struct tx_latency latency;
+	struct bt_tx_info info;
+
+	BT_DBG("sk %p cmd %x arg %lx", sk, cmd, arg);
+
+	switch (cmd) {
+	case BTGETTXINFO:
+		/* Require zero-initialized, to allow later extensions */
+		if (copy_from_user(&info, (void __user *)arg, sizeof(info)))
+			return -EFAULT;
+		if (info.flags || info.queue || info.time || info.offset)
+			return -EINVAL;
+
+		memset(&info, 0, sizeof(info));
+
+		lock_sock(sk);
+
+		if (sk->sk_state != BT_CONNECTED || !chan->conn ||
+		    !chan->conn->hchan) {
+			release_sock(sk);
+			return -EINVAL;
+		}
+
+		hci_copy_tx_latency(&latency, &chan->conn->hchan->tx_latency);
+
+		release_sock(sk);
+
+		if (!latency.now.time)
+			return -ENOENT;
+
+		info.queue = latency.now.queue;
+		info.time = ktime_to_ns(latency.now.time);
+		info.offset = latency.now.offset;
+
+		if (copy_to_user((void __user *)arg, &info, sizeof(info)))
+			return -EFAULT;
+
+		return 0;
+	}
+
+	return bt_sock_ioctl(sock, cmd, arg);
+}
+
 /* Kill socket (only if zapped and orphan)
  * Must be called on unlocked socket, with l2cap channel lock.
  */
@@ -1883,7 +1931,7 @@ static const struct proto_ops l2cap_sock_ops = {
 	.sendmsg	= l2cap_sock_sendmsg,
 	.recvmsg	= l2cap_sock_recvmsg,
 	.poll		= bt_sock_poll,
-	.ioctl		= bt_sock_ioctl,
+	.ioctl		= l2cap_sock_ioctl,
 	.gettstamp	= sock_gettstamp,
 	.mmap		= sock_no_mmap,
 	.socketpair	= sock_no_socketpair,
-- 
2.44.0