diff options
Diffstat (limited to '')
-rw-r--r-- | net/bluetooth/mgmt_util.c | 158 |
1 files changed, 139 insertions, 19 deletions
diff --git a/net/bluetooth/mgmt_util.c b/net/bluetooth/mgmt_util.c index 0d0a6d77b9e8..0115f783bde8 100644 --- a/net/bluetooth/mgmt_util.c +++ b/net/bluetooth/mgmt_util.c @@ -56,40 +56,73 @@ static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie, 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) +struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode, + unsigned int size) { struct sk_buff *skb; - struct mgmt_hdr *hdr; - skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL); + skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL); if (!skb) - return -ENOMEM; + return skb; - hdr = skb_put(skb, sizeof(*hdr)); - hdr->opcode = cpu_to_le16(event); - if (hdev) - hdr->index = cpu_to_le16(hdev->id); - else - hdr->index = cpu_to_le16(MGMT_INDEX_NONE); - hdr->len = cpu_to_le16(data_len); + skb_reserve(skb, sizeof(struct mgmt_hdr)); + bt_cb(skb)->mgmt.hdev = hdev; + bt_cb(skb)->mgmt.opcode = opcode; - if (data) - skb_put_data(skb, data, data_len); + return skb; +} + +int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag, + struct sock *skip_sk) +{ + struct hci_dev *hdev; + struct mgmt_hdr *hdr; + int len; + + if (!skb) + return -EINVAL; + + len = skb->len; + hdev = bt_cb(skb)->mgmt.hdev; /* Time stamp */ __net_timestamp(skb); - hci_send_to_channel(channel, skb, flag, skip_sk); - + /* Send just the data, without headers, to the monitor */ if (channel == HCI_CHANNEL_CONTROL) - hci_send_monitor_ctrl_event(hdev, event, data, data_len, + hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode, + skb->data, skb->len, skb_get_ktime(skb), flag, skip_sk); + hdr = skb_push(skb, sizeof(*hdr)); + hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode); + if (hdev) + hdr->index = cpu_to_le16(hdev->id); + else + hdr->index = cpu_to_le16(MGMT_INDEX_NONE); + hdr->len = cpu_to_le16(len); + + hci_send_to_channel(channel, skb, flag, skip_sk); + kfree_skb(skb); return 0; } +int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, + void *data, u16 data_len, int flag, struct sock *skip_sk) +{ + struct sk_buff *skb; + + skb = mgmt_alloc_skb(hdev, event, data_len); + if (!skb) + return -ENOMEM; + + if (data) + skb_put_data(skb, data, data_len); + + return mgmt_send_event_skb(channel, skb, flag, skip_sk); +} + int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) { struct sk_buff *skb, *mskb; @@ -227,7 +260,7 @@ void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, } } -struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, +struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode, struct hci_dev *hdev, void *data, u16 len) { @@ -251,7 +284,20 @@ struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, cmd->sk = sk; sock_hold(sk); - list_add(&cmd->list, &hdev->mgmt_pending); + return cmd; +} + +struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, + struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_pending_cmd *cmd; + + cmd = mgmt_pending_new(sk, opcode, hdev, data, len); + if (!cmd) + return NULL; + + list_add_tail(&cmd->list, &hdev->mgmt_pending); return cmd; } @@ -268,3 +314,77 @@ void mgmt_pending_remove(struct mgmt_pending_cmd *cmd) list_del(&cmd->list); mgmt_pending_free(cmd); } + +void mgmt_mesh_foreach(struct hci_dev *hdev, + void (*cb)(struct mgmt_mesh_tx *mesh_tx, void *data), + void *data, struct sock *sk) +{ + struct mgmt_mesh_tx *mesh_tx, *tmp; + + list_for_each_entry_safe(mesh_tx, tmp, &hdev->mgmt_pending, list) { + if (!sk || mesh_tx->sk == sk) + cb(mesh_tx, data); + } +} + +struct mgmt_mesh_tx *mgmt_mesh_next(struct hci_dev *hdev, struct sock *sk) +{ + struct mgmt_mesh_tx *mesh_tx; + + if (list_empty(&hdev->mesh_pending)) + return NULL; + + list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) { + if (!sk || mesh_tx->sk == sk) + return mesh_tx; + } + + return NULL; +} + +struct mgmt_mesh_tx *mgmt_mesh_find(struct hci_dev *hdev, u8 handle) +{ + struct mgmt_mesh_tx *mesh_tx; + + if (list_empty(&hdev->mesh_pending)) + return NULL; + + list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) { + if (mesh_tx->handle == handle) + return mesh_tx; + } + + return NULL; +} + +struct mgmt_mesh_tx *mgmt_mesh_add(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_mesh_tx *mesh_tx; + + mesh_tx = kzalloc(sizeof(*mesh_tx), GFP_KERNEL); + if (!mesh_tx) + return NULL; + + hdev->mesh_send_ref++; + if (!hdev->mesh_send_ref) + hdev->mesh_send_ref++; + + mesh_tx->handle = hdev->mesh_send_ref; + mesh_tx->index = hdev->id; + memcpy(mesh_tx->param, data, len); + mesh_tx->param_len = len; + mesh_tx->sk = sk; + sock_hold(sk); + + list_add_tail(&mesh_tx->list, &hdev->mesh_pending); + + return mesh_tx; +} + +void mgmt_mesh_remove(struct mgmt_mesh_tx *mesh_tx) +{ + list_del(&mesh_tx->list); + sock_put(mesh_tx->sk); + kfree(mesh_tx); +} |