diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h index 1d822f2c0f1a..3d8d589fa559 100644 --- a/include/net/bluetooth/mgmt.h +++ b/include/net/bluetooth/mgmt.h @@ -130,6 +130,16 @@ struct mgmt_rp_disconnect { bdaddr_t bdaddr; } __packed; +#define MGMT_OP_GET_CONNECTIONS 0x0010 +struct mgmt_cp_get_connections { + __le16 index; +} __packed; +struct mgmt_rp_get_connections { + __le16 index; + __le16 conn_count; + bdaddr_t conn[0]; +} __packed; + #define MGMT_EV_CMD_COMPLETE 0x0001 struct mgmt_ev_cmd_complete { __le16 opcode; diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 9fb989f4216e..8f4f47e9d5c9 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -941,6 +941,75 @@ failed: return err; } +static int get_connections(struct sock *sk, unsigned char *data, u16 len) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + struct mgmt_cp_get_connections *cp; + struct mgmt_ev_cmd_complete *ev; + struct mgmt_rp_get_connections *rp; + struct hci_dev *hdev; + struct list_head *p; + size_t body_len; + u16 dev_id, count; + int i, err; + + BT_DBG(""); + + cp = (void *) data; + dev_id = get_unaligned_le16(&cp->index); + + hdev = hci_dev_get(dev_id); + if (!hdev) + return cmd_status(sk, MGMT_OP_GET_CONNECTIONS, ENODEV); + + hci_dev_lock_bh(hdev); + + count = 0; + list_for_each(p, &hdev->conn_hash.list) { + count++; + } + + body_len = sizeof(*ev) + sizeof(*rp) + (count * sizeof(bdaddr_t)); + skb = alloc_skb(sizeof(*hdr) + body_len, GFP_ATOMIC); + if (!skb) { + err = -ENOMEM; + goto unlock; + } + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); + hdr->len = cpu_to_le16(body_len); + + ev = (void *) skb_put(skb, sizeof(*ev)); + put_unaligned_le16(MGMT_OP_GET_CONNECTIONS, &ev->opcode); + + rp = (void *) skb_put(skb, sizeof(*rp) + (count * sizeof(bdaddr_t))); + put_unaligned_le16(dev_id, &rp->index); + put_unaligned_le16(count, &rp->conn_count); + + read_lock(&hci_dev_list_lock); + + i = 0; + list_for_each(p, &hdev->conn_hash.list) { + struct hci_conn *c = list_entry(p, struct hci_conn, list); + + bacpy(&rp->conn[i++], &c->dst); + } + + read_unlock(&hci_dev_list_lock); + + if (sock_queue_rcv_skb(sk, skb) < 0) + kfree_skb(skb); + + err = 0; + +unlock: + hci_dev_unlock_bh(hdev); + hci_dev_put(hdev); + return err; +} + int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen) { unsigned char *buf; @@ -1014,6 +1083,9 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen) case MGMT_OP_DISCONNECT: err = disconnect(sk, buf + sizeof(*hdr), len); break; + case MGMT_OP_GET_CONNECTIONS: + err = get_connections(sk, buf + sizeof(*hdr), len); + break; default: BT_DBG("Unknown op %u", opcode); err = cmd_status(sk, opcode, 0x01);