Commit 38ceaa00 authored by Marcel Holtmann's avatar Marcel Holtmann
Browse files

Bluetooth: Add support for sending MGMT commands and events to monitor 



This adds support for tracing all management commands and events via the
monitor interface.

Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
Signed-off-by: default avatarJohan Hedberg <johan.hedberg@intel.com>
parent 249fa169

include/net/bluetooth/hci_core.h

+3 −0
Original line number Diff line number Diff line
@@ -1406,6 +1406,9 @@ void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb); 
void hci_send_to_channel(unsigned short channel, struct sk_buff *skb,

			 int flag, struct sock *skip_sk);

void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb);

void hci_send_monitor_ctrl_event(struct hci_dev *hdev, u16 event,

				 void *data, u16 data_len, ktime_t tstamp,

				 int flag, struct sock *skip_sk);



void hci_sock_dev_event(struct hci_dev *hdev, int event);

include/net/bluetooth/hci_mon.h

+2 −0
Original line number Diff line number Diff line
@@ -47,6 +47,8 @@ struct hci_mon_hdr { 
#define HCI_MON_USER_LOGGING	13

#define HCI_MON_CTRL_OPEN	14

#define HCI_MON_CTRL_CLOSE	15

#define HCI_MON_CTRL_COMMAND	16

#define HCI_MON_CTRL_EVENT	17



struct hci_mon_new_index {

	__u8		type;

net/bluetooth/hci_sock.c

+94 −0
Original line number Diff line number Diff line
@@ -315,6 +315,60 @@ void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb) 
	kfree_skb(skb_copy);

}



void hci_send_monitor_ctrl_event(struct hci_dev *hdev, u16 event,

				 void *data, u16 data_len, ktime_t tstamp,

				 int flag, struct sock *skip_sk)

{

	struct sock *sk;

	__le16 index;



	if (hdev)

		index = cpu_to_le16(hdev->id);

	else

		index = cpu_to_le16(MGMT_INDEX_NONE);



	read_lock(&hci_sk_list.lock);



	sk_for_each(sk, &hci_sk_list.head) {

		struct hci_mon_hdr *hdr;

		struct sk_buff *skb;



		if (hci_pi(sk)->channel != HCI_CHANNEL_CONTROL)

			continue;



		/* Ignore socket without the flag set */

		if (!hci_sock_test_flag(sk, flag))

			continue;



		/* Skip the original socket */

		if (sk == skip_sk)

			continue;



		skb = bt_skb_alloc(6 + data_len, GFP_ATOMIC);

		if (!skb)

			continue;



		put_unaligned_le32(hci_pi(sk)->cookie, skb_put(skb, 4));

		put_unaligned_le16(event, skb_put(skb, 2));



		if (data)

			memcpy(skb_put(skb, data_len), data, data_len);



		skb->tstamp = tstamp;



		hdr = (void *)skb_push(skb, HCI_MON_HDR_SIZE);

		hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);

		hdr->index = index;

		hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);



		hci_send_to_channel(HCI_CHANNEL_MONITOR, skb,

				    HCI_SOCK_TRUSTED, NULL);

		kfree_skb(skb);

	}



	read_unlock(&hci_sk_list.lock);

}



static struct sk_buff *create_monitor_event(struct hci_dev *hdev, int event)

{

	struct hci_mon_hdr *hdr;
@@ -447,6 +501,33 @@ static struct sk_buff *create_monitor_ctrl_close(struct sock *sk) 
	return skb;

}



static struct sk_buff *create_monitor_ctrl_command(struct sock *sk, u16 index,

						   u16 opcode, u16 len,

						   const void *buf)

{

	struct hci_mon_hdr *hdr;

	struct sk_buff *skb;



	skb = bt_skb_alloc(6 + len, GFP_ATOMIC);

	if (!skb)

		return NULL;



	put_unaligned_le32(hci_pi(sk)->cookie, skb_put(skb, 4));

	put_unaligned_le16(opcode, skb_put(skb, 2));



	if (buf)

		memcpy(skb_put(skb, len), buf, len);



	__net_timestamp(skb);



	hdr = (void *)skb_push(skb, HCI_MON_HDR_SIZE);

	hdr->opcode = cpu_to_le16(HCI_MON_CTRL_COMMAND);

	hdr->index = cpu_to_le16(index);

	hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);



	return skb;

}



static void __printf(2, 3)

send_monitor_note(struct sock *sk, const char *fmt, ...)

{
@@ -1257,6 +1338,19 @@ static int hci_mgmt_cmd(struct hci_mgmt_chan *chan, struct sock *sk, 
		goto done;

	}



	if (chan->channel == HCI_CHANNEL_CONTROL) {

		struct sk_buff *skb;



		/* Send event to monitor */

		skb = create_monitor_ctrl_command(sk, index, opcode, len,

						  buf + sizeof(*hdr));

		if (skb) {

			hci_send_to_channel(HCI_CHANNEL_MONITOR, skb,

					    HCI_SOCK_TRUSTED, NULL);

			kfree_skb(skb);

		}

	}



	if (opcode >= chan->handler_count ||

	    chan->handlers[opcode].func == NULL) {

		BT_DBG("Unknown op %u", opcode);

net/bluetooth/mgmt_util.c

+63 −3
Original line number Diff line number Diff line
@@ -21,12 +21,41 @@ 
   SOFTWARE IS DISCLAIMED.

*/



#include <asm/unaligned.h>



#include <net/bluetooth/bluetooth.h>

#include <net/bluetooth/hci_core.h>

#include <net/bluetooth/hci_mon.h>

#include <net/bluetooth/mgmt.h>



#include "mgmt_util.h"



static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie,

						 u16 opcode, u16 len, void *buf)

{

	struct hci_mon_hdr *hdr;

	struct sk_buff *skb;



	skb = bt_skb_alloc(6 + len, GFP_ATOMIC);

	if (!skb)

		return NULL;



	put_unaligned_le32(cookie, skb_put(skb, 4));

	put_unaligned_le16(opcode, skb_put(skb, 2));



	if (buf)

		memcpy(skb_put(skb, len), buf, len);



	__net_timestamp(skb);



	hdr = (void *)skb_push(skb, HCI_MON_HDR_SIZE);

	hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);

	hdr->index = index;

	hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);



	return skb;

}



int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,

		    void *data, u16 data_len, int flag, struct sock *skip_sk)

{
@@ -52,14 +81,18 @@ int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, 
	__net_timestamp(skb);



	hci_send_to_channel(channel, skb, flag, skip_sk);

	kfree_skb(skb);



	if (channel == HCI_CHANNEL_CONTROL)

		hci_send_monitor_ctrl_event(hdev, event, data, data_len,

					    skb_get_ktime(skb), flag, skip_sk);



	kfree_skb(skb);

	return 0;

}



int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)

{

	struct sk_buff *skb;

	struct sk_buff *skb, *mskb;

	struct mgmt_hdr *hdr;

	struct mgmt_ev_cmd_status *ev;

	int err;
@@ -80,17 +113,30 @@ int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) 
	ev->status = status;

	ev->opcode = cpu_to_le16(cmd);



	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),

					 MGMT_EV_CMD_STATUS, sizeof(*ev), ev);

	if (mskb)

		skb->tstamp = mskb->tstamp;

	else

		__net_timestamp(skb);



	err = sock_queue_rcv_skb(sk, skb);

	if (err < 0)

		kfree_skb(skb);



	if (mskb) {

		hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,

				    HCI_SOCK_TRUSTED, NULL);

		kfree_skb(mskb);

	}



	return err;

}



int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,

		      void *rp, size_t rp_len)

{

	struct sk_buff *skb;

	struct sk_buff *skb, *mskb;

	struct mgmt_hdr *hdr;

	struct mgmt_ev_cmd_complete *ev;

	int err;
@@ -114,10 +160,24 @@ int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, 
	if (rp)

		memcpy(ev->data, rp, rp_len);



	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),

					 MGMT_EV_CMD_COMPLETE,

					 sizeof(*ev) + rp_len, ev);

	if (mskb)

		skb->tstamp = mskb->tstamp;

	else

		__net_timestamp(skb);



	err = sock_queue_rcv_skb(sk, skb);

	if (err < 0)

		kfree_skb(skb);



	if (mskb) {

		hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,

				    HCI_SOCK_TRUSTED, NULL);

		kfree_skb(mskb);

	}



	return err;

}