Bluetooth: Add support for logging host packets into monitor

This adds support for logging host (enqueued/dequeued) packets so their
timing is visible in the logs:

l2cap-tester[41]: < HOST Data TX: len 12                 [hci0] 15:50:02.974141
l2cap-tester[41]: < ACL Data T.. flags 0x00 dlen 12  torvalds#83 [hci0] 15:50:02.974199
      Channel: 64 len 8 [PSM 4097 mode Basic (0x00)] {chan 0}
        01 02 03 04 05 06 07 08                          ........

> ACL Data RX: Handle 42 flags 0x02 dlen 12         torvalds#170 [hci0] 15:51:43.269961
      Channel: 64 len 8 [PSM 4097 mode Basic (0x00)] {chan 0}
        01 02 03 04 05 06 07 08                          ........
[45]: > HOST Data RX: len 8                              [hci0] 15:51:43.269976

Signed-off-by: Luiz Augusto von Dentz <[email protected]>

Author: Vudentz

include/net/bluetooth/bluetooth.h

@@ -459,6 +459,8 @@ void hci_req_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 status,
 int hci_ethtool_ts_info(unsigned int index, int sk_proto,
 			struct kernel_ethtool_ts_info *ts_info);
 
+void hci_sock_copy_creds(struct sock *sk, struct sk_buff *skb);
+
 #define HCI_REQ_START	BIT(0)
 #define HCI_REQ_SKB	BIT(1)
 

include/net/bluetooth/hci.h

@@ -497,6 +497,7 @@ enum {
 #define HCI_ISODATA_PKT		0x05
 #define HCI_DIAG_PKT		0xf0
 #define HCI_DRV_PKT		0xf1
+#define HCI_HOST_PKT		0xf2
 #define HCI_VENDOR_PKT		0xff
 
 /* HCI packet types */

include/net/bluetooth/hci_core.h

@@ -2248,6 +2248,7 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
 void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags);
 void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb);
 void hci_send_iso(struct hci_conn *conn, struct sk_buff *skb);
+void hci_send_host(struct hci_dev *hdev, struct sk_buff *skb);
 
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode);
 void *hci_recv_event_data(struct hci_dev *hdev, __u8 event);

include/net/bluetooth/hci_mon.h

@@ -53,6 +53,8 @@ struct hci_mon_hdr {
 #define HCI_MON_ISO_RX_PKT	19
 #define HCI_MON_DRV_TX_PKT	20
 #define HCI_MON_DRV_RX_PKT	21
+#define HCI_MON_SOCK_TX_PKT	22
+#define HCI_MON_SOCK_RX_PKT	23
 
 struct hci_mon_new_index {
 	__u8		type;

net/bluetooth/hci_core.c

@@ -3269,12 +3269,26 @@ static void hci_queue_acl(struct hci_chan *chan, struct sk_buff_head *queue,
 	}
 }
 
+void hci_send_host(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	/* Mark as socket packet */
+	hci_skb_pkt_type(skb) = HCI_HOST_PKT;
+
+	/* Time stamp */
+	__net_timestamp(skb);
+
+	/* Send copy to monitor */
+	hci_send_to_monitor(hdev, skb);
+}
+
 void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
 {
 	struct hci_dev *hdev = chan->conn->hdev;
 
 	BT_DBG("%s chan %p flags 0x%4.4x", hdev->name, chan, flags);
 
+	hci_send_host(hdev, skb);
+
 	hci_queue_acl(chan, &chan->data_q, skb, flags);
 
 	queue_work(hdev->workqueue, &hdev->tx_work);
@@ -3288,6 +3302,8 @@ void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb)
 
 	BT_DBG("%s len %d", hdev->name, skb->len);
 
+	hci_send_host(hdev, skb);
+
 	hdr.handle = cpu_to_le16(conn->handle);
 	hdr.dlen   = skb->len;
 
@@ -3365,6 +3381,8 @@ void hci_send_iso(struct hci_conn *conn, struct sk_buff *skb)
 
 	BT_DBG("%s len %d", hdev->name, skb->len);
 
+	hci_send_host(hdev, skb);
+
 	hci_queue_iso(conn, &conn->data_q, skb);
 
 	queue_work(hdev->workqueue, &hdev->tx_work);

net/bluetooth/hci_sock.c

@@ -265,7 +265,7 @@ void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb)
 	kfree_skb(skb_copy);
 }
 
-static void hci_sock_copy_creds(struct sock *sk, struct sk_buff *skb)
+void hci_sock_copy_creds(struct sock *sk, struct sk_buff *skb)
 {
 	struct scm_creds *creds;
 
@@ -398,6 +398,12 @@ void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb)
 		else
 			opcode = cpu_to_le16(HCI_MON_DRV_TX_PKT);
 		break;
+	case HCI_HOST_PKT:
+		if (bt_cb(skb)->incoming)
+			opcode = cpu_to_le16(HCI_MON_SOCK_RX_PKT);
+		else
+			opcode = cpu_to_le16(HCI_MON_SOCK_TX_PKT);
+		break;
 	case HCI_DIAG_PKT:
 		opcode = cpu_to_le16(HCI_MON_VENDOR_DIAG);
 		break;

net/bluetooth/iso.c

@@ -569,6 +569,13 @@ static void iso_recv_frame(struct iso_conn *conn, struct sk_buff *skb)
 	if (sk->sk_state != BT_CONNECTED)
 		goto drop;
 
+	/* Copy pid information since the skb was not allocated using the sk as
+	 * source, otherwise tools cannot decode the process that is receiving
+	 * the packet.
+	 */
+	hci_sock_copy_creds(sk, skb);
+	hci_send_host(conn->hcon->hdev, skb);
+
 	if (!sock_queue_rcv_skb(sk, skb))
 		return;
 

net/bluetooth/l2cap_core.c

@@ -7440,6 +7440,8 @@ static int l2cap_recv_frag(struct l2cap_conn *conn, struct sk_buff *skb,
 			return -ENOMEM;
 		/* Init rx_len */
 		conn->rx_len = len;
+		/* Set rx_skb as incoming so it can be properly decoded */
+		bt_cb(conn->rx_skb)->incoming = 1;
 
 		skb_set_delivery_time(conn->rx_skb, skb->tstamp,
 				      skb->tstamp_type);

net/bluetooth/l2cap_sock.c

@@ -1526,6 +1526,13 @@ static int l2cap_sock_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			goto done;
 	}
 
+	/* Copy pid information since the skb was not allocated using the sk as
+	 * source, otherwise tools cannot decode the process that is receiving
+	 * the packet.
+	 */
+	hci_sock_copy_creds(sk, skb);
+	hci_send_host(chan->conn->hcon->hdev, skb);
+
 	err = __sock_queue_rcv_skb(sk, skb);
 
 	l2cap_publish_rx_avail(chan);

net/bluetooth/sco.c

@@ -412,6 +412,13 @@ static void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb)
 	if (sk->sk_state != BT_CONNECTED)
 		goto drop;
 
+	/* Copy pid information since the skb was not allocated using the sk as
+	 * source, otherwise tools cannot decode the process that is receiving
+	 * the packet.
+	 */
+	hci_sock_copy_creds(sk, skb);
+	hci_send_host(conn->hcon->hdev, skb);
+
 	if (!sock_queue_rcv_skb(sk, skb))
 		return;