Skip to content

Commit

Permalink
Bluetooth: Add support for sending MGMT commands and events to monitor
Browse files Browse the repository at this point in the history 
This adds support for tracing all management commands and events via the
monitor interface.

Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
  • Loading branch information
Marcel Holtmann committed Sep 19, 2016
1 parent 249fa16 commit 38ceaa0
Show file tree
Hide file tree
Showing 4 changed files with 162 additions and 3 deletions.
3 changes: 3 additions & 0 deletions include/net/bluetooth/hci_core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
2 changes: 2 additions & 0 deletions include/net/bluetooth/hci_mon.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
94 changes: 94 additions & 0 deletions net/bluetooth/hci_sock.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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, ...)
{
Expand Down Expand Up @@ -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);
Expand Down
66 changes: 63 additions & 3 deletions net/bluetooth/mgmt_util.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
}

Expand Down

0 comments on commit 38ceaa0

Please sign in to comment.