From 466f8004f364e9cb46d9124109972489eccfb404 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:01 +0300 Subject: Bluetooth: A2MP: Create A2MP channel Create and initialize fixed A2MP channel Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 net/bluetooth/a2mp.c (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c new file mode 100644 index 000000000000..de455a264451 --- /dev/null +++ b/net/bluetooth/a2mp.c @@ -0,0 +1,69 @@ +/* + Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved. + Copyright (c) 2011,2012 Intel Corp. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License version 2 and + only version 2 as published by the Free Software Foundation. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. +*/ + +#include +#include +#include + +static struct l2cap_ops a2mp_chan_ops = { + .name = "L2CAP A2MP channel", +}; + +static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan; + int err; + + chan = l2cap_chan_create(); + if (!chan) + return NULL; + + BT_DBG("chan %p", chan); + + hci_conn_hold(conn->hcon); + + chan->omtu = L2CAP_A2MP_DEFAULT_MTU; + chan->imtu = L2CAP_A2MP_DEFAULT_MTU; + chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; + + chan->ops = &a2mp_chan_ops; + + l2cap_chan_set_defaults(chan); + chan->remote_max_tx = chan->max_tx; + chan->remote_tx_win = chan->tx_win; + + chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; + chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; + + skb_queue_head_init(&chan->tx_q); + + chan->mode = L2CAP_MODE_ERTM; + + err = l2cap_ertm_init(chan); + if (err < 0) { + l2cap_chan_del(chan, 0); + return NULL; + } + + chan->conf_state = 0; + + l2cap_chan_add(conn, chan); + + chan->remote_mps = chan->omtu; + chan->mps = chan->omtu; + + chan->state = BT_CONNECTED; + + return chan; +} -- cgit From 9740e49d17e55f3832661fd99a8e0a17e921a82e Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:02 +0300 Subject: Bluetooth: A2MP: AMP Manager basic functions Define AMP Manager and some basic functions. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 54 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index de455a264451..3c241c2b3e1a 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -15,6 +15,7 @@ #include #include #include +#include static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", @@ -67,3 +68,56 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) return chan; } + +/* AMP Manager functions */ +void amp_mgr_get(struct amp_mgr *mgr) +{ + BT_DBG("mgr %p", mgr); + + kref_get(&mgr->kref); +} + +static void amp_mgr_destroy(struct kref *kref) +{ + struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); + + BT_DBG("mgr %p", mgr); + + kfree(mgr); +} + +int amp_mgr_put(struct amp_mgr *mgr) +{ + BT_DBG("mgr %p", mgr); + + return kref_put(&mgr->kref, &_mgr_destroy); +} + +static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn) +{ + struct amp_mgr *mgr; + struct l2cap_chan *chan; + + mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); + if (!mgr) + return NULL; + + BT_DBG("conn %p mgr %p", conn, mgr); + + mgr->l2cap_conn = conn; + + chan = a2mp_chan_open(conn); + if (!chan) { + kfree(mgr); + return NULL; + } + + mgr->a2mp_chan = chan; + chan->data = mgr; + + conn->hcon->amp_mgr = mgr; + + kref_init(&mgr->kref); + + return mgr; +} -- cgit From f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:03 +0300 Subject: Bluetooth: A2MP: Build and Send msg helpers Helper function to build and send A2MP messages. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 46 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 3c241c2b3e1a..53f49a0b7f9a 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -17,6 +17,52 @@ #include #include +/* A2MP build & send command helper functions */ +static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data) +{ + struct a2mp_cmd *cmd; + int plen; + + plen = sizeof(*cmd) + len; + cmd = kzalloc(plen, GFP_KERNEL); + if (!cmd) + return NULL; + + cmd->code = code; + cmd->ident = ident; + cmd->len = cpu_to_le16(len); + + memcpy(cmd->data, data, len); + + return cmd; +} + +static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, + void *data) +{ + struct l2cap_chan *chan = mgr->a2mp_chan; + struct a2mp_cmd *cmd; + u16 total_len = len + sizeof(*cmd); + struct kvec iv; + struct msghdr msg; + + cmd = __a2mp_build(code, ident, len, data); + if (!cmd) + return; + + iv.iov_base = cmd; + iv.iov_len = total_len; + + memset(&msg, 0, sizeof(msg)); + + msg.msg_iov = (struct iovec *) &iv; + msg.msg_iovlen = 1; + + l2cap_chan_send(chan, &msg, total_len, 0); + + kfree(cmd); +} + static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", }; -- cgit From 46d5c9088fbcc8a570bc271f77940973d9cae074 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:04 +0300 Subject: Bluetooth: A2MP: Add chan callbacks Add L2CAP chan ops callbacks. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 53f49a0b7f9a..f7c710cacf44 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -63,8 +63,63 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, kfree(cmd); } +static void a2mp_chan_close_cb(struct l2cap_chan *chan) +{ + l2cap_chan_destroy(chan); +} + +static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state) +{ + struct amp_mgr *mgr = chan->data; + + if (!mgr) + return; + + BT_DBG("chan %p state %s", chan, state_to_string(state)); + + chan->state = state; + + switch (state) { + case BT_CLOSED: + if (mgr) + amp_mgr_put(mgr); + break; + } +} + +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, + unsigned long len, int nb) +{ + return bt_skb_alloc(len, GFP_KERNEL); +} + +static struct l2cap_chan *a2mp_chan_no_new_conn_cb(struct l2cap_chan *chan) +{ + BT_ERR("new_connection for chan %p not implemented", chan); + + return NULL; +} + +static void a2mp_chan_no_teardown_cb(struct l2cap_chan *chan, int err) +{ + BT_ERR("teardown for chan %p not implemented", chan); +} + +static void a2mp_chan_no_ready(struct l2cap_chan *chan) +{ + BT_ERR("ready for chan %p not implemented", chan); +} + static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", + .close = a2mp_chan_close_cb, + .state_change = a2mp_chan_state_change_cb, + .alloc_skb = a2mp_chan_alloc_skb_cb, + + /* Not implemented for A2MP */ + .new_connection = a2mp_chan_no_new_conn_cb, + .teardown = a2mp_chan_no_teardown_cb, + .ready = a2mp_chan_no_ready, }; static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) -- cgit From 6b44d9b8d96b37f72ccd7335b32f386a67b7f1f4 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:07 +0300 Subject: Bluetooth: A2MP: Process A2MP messages Implement basic processing for AMP Manager Protocol (A2MP). Example below shows processing unrecognized command. ... > ACL data: handle 11 flags 0x02 dlen 12 A2MP: code 0x00 ident 3 len 0 < ACL data: handle 11 flags 0x00 dlen 14 A2MP: Command Reject: reason (0) - Command not recognized ... Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index f7c710cacf44..0726c9fe005c 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -63,6 +63,70 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, kfree(cmd); } +/* Handle A2MP signalling */ +static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct a2mp_cmd *hdr = (void *) skb->data; + struct amp_mgr *mgr = chan->data; + int err = 0; + + amp_mgr_get(mgr); + + while (skb->len >= sizeof(*hdr)) { + struct a2mp_cmd *hdr = (void *) skb->data; + u16 len = le16_to_cpu(hdr->len); + + BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len); + + skb_pull(skb, sizeof(*hdr)); + + if (len > skb->len || !hdr->ident) { + err = -EINVAL; + break; + } + + mgr->ident = hdr->ident; + + switch (hdr->code) { + case A2MP_COMMAND_REJ: + case A2MP_DISCOVER_REQ: + case A2MP_CHANGE_NOTIFY: + case A2MP_GETINFO_REQ: + case A2MP_GETAMPASSOC_REQ: + case A2MP_CREATEPHYSLINK_REQ: + case A2MP_DISCONNPHYSLINK_REQ: + case A2MP_CHANGE_RSP: + case A2MP_DISCOVER_RSP: + case A2MP_GETINFO_RSP: + case A2MP_GETAMPASSOC_RSP: + case A2MP_CREATEPHYSLINK_RSP: + case A2MP_DISCONNPHYSLINK_RSP: + default: + BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); + err = -EINVAL; + break; + } + } + + if (err) { + struct a2mp_cmd_rej rej; + rej.reason = __constant_cpu_to_le16(0); + + BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); + + a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), + &rej); + } + + /* Always free skb and return success error code to prevent + from sending L2CAP Disconnect over A2MP channel */ + kfree_skb(skb); + + amp_mgr_put(mgr); + + return 0; +} + static void a2mp_chan_close_cb(struct l2cap_chan *chan) { l2cap_chan_destroy(chan); @@ -112,6 +176,7 @@ static void a2mp_chan_no_ready(struct l2cap_chan *chan) static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", + .recv = a2mp_chan_recv_cb, .close = a2mp_chan_close_cb, .state_change = a2mp_chan_state_change_cb, .alloc_skb = a2mp_chan_alloc_skb_cb, -- cgit From 21dbd2ce35f6d2b4aa5363be6c839cdb50644e11 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:08 +0300 Subject: Bluetooth: A2MP: Process A2MP Command Reject Placeholder for future A2MP Command Reject handler. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 0726c9fe005c..188b42120074 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -63,6 +63,22 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, kfree(cmd); } +/* Processing A2MP messages */ +static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_cmd_rej *rej = (void *) skb->data; + + if (le16_to_cpu(hdr->len) < sizeof(*rej)) + return -EINVAL; + + BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason)); + + skb_pull(skb, sizeof(*rej)); + + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -89,6 +105,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) switch (hdr->code) { case A2MP_COMMAND_REJ: + a2mp_command_rej(mgr, skb, hdr); + break; + case A2MP_DISCOVER_REQ: case A2MP_CHANGE_NOTIFY: case A2MP_GETINFO_REQ: -- cgit From 8598d064cbf22b2d84c7cd8a9fcb97138baffe3f Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:09 +0300 Subject: Bluetooth: A2MP: Process A2MP Discover Request Adds helper functions to count HCI devs and process A2MP Discover Request, code makes sure that first controller in the list is BREDR one. Trace is shown below: ... > ACL data: handle 11 flags 0x02 dlen 16 A2MP: Discover req: mtu/mps 670 mask: 0x0000 < ACL data: handle 11 flags 0x00 dlen 22 A2MP: Discover rsp: mtu/mps 670 mask: 0x0000 Controller list: id 0 type 0 (BR-EDR) status 0x01 (Bluetooth only) id 1 type 1 (802.11 AMP) status 0x01 (Bluetooth only) ... Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 85 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 85 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 188b42120074..1cc920a62b0f 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -63,6 +63,36 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, kfree(cmd); } +static inline void __a2mp_cl_bredr(struct a2mp_cl *cl) +{ + cl->id = 0; + cl->type = 0; + cl->status = 1; +} + +/* hci_dev_list shall be locked */ +static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl) +{ + int i = 0; + struct hci_dev *hdev; + + __a2mp_cl_bredr(cl); + + list_for_each_entry(hdev, &hci_dev_list, list) { + /* Iterate through AMP controllers */ + if (hdev->id == HCI_BREDR_ID) + continue; + + /* Starting from second entry */ + if (++i >= num_ctrl) + return; + + cl[i].id = hdev->id; + cl[i].type = hdev->amp_type; + cl[i].status = hdev->amp_status; + } +} + /* Processing A2MP messages */ static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) @@ -79,6 +109,58 @@ static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, return 0; } +static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_discov_req *req = (void *) skb->data; + u16 len = le16_to_cpu(hdr->len); + struct a2mp_discov_rsp *rsp; + u16 ext_feat; + u8 num_ctrl; + + if (len < sizeof(*req)) + return -EINVAL; + + skb_pull(skb, sizeof(*req)); + + ext_feat = le16_to_cpu(req->ext_feat); + + BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat); + + /* check that packet is not broken for now */ + while (ext_feat & A2MP_FEAT_EXT) { + if (len < sizeof(ext_feat)) + return -EINVAL; + + ext_feat = get_unaligned_le16(skb->data); + BT_DBG("efm 0x%4.4x", ext_feat); + len -= sizeof(ext_feat); + skb_pull(skb, sizeof(ext_feat)); + } + + read_lock(&hci_dev_list_lock); + + num_ctrl = __hci_num_ctrl(); + len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp); + rsp = kmalloc(len, GFP_ATOMIC); + if (!rsp) { + read_unlock(&hci_dev_list_lock); + return -ENOMEM; + } + + rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); + rsp->ext_feat = 0; + + __a2mp_add_cl(mgr, rsp->cl, num_ctrl); + + read_unlock(&hci_dev_list_lock); + + a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp); + + kfree(rsp); + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -109,6 +191,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) break; case A2MP_DISCOVER_REQ: + err = a2mp_discover_req(mgr, skb, hdr); + break; + case A2MP_CHANGE_NOTIFY: case A2MP_GETINFO_REQ: case A2MP_GETAMPASSOC_REQ: -- cgit From 329d81af29344a2ad2f9595310be74644421797a Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:10 +0300 Subject: Bluetooth: A2MP: Process A2MP Change Notify Placeholder for A2MP Change Notify handler. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 1cc920a62b0f..6cdaa85ab5e9 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -161,6 +161,22 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, return 0; } +static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_cl *cl = (void *) skb->data; + + while (skb->len >= sizeof(*cl)) { + BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, + cl->status); + cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); + } + + /* TODO send A2MP_CHANGE_RSP */ + + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -195,6 +211,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) break; case A2MP_CHANGE_NOTIFY: + err = a2mp_change_notify(mgr, skb, hdr); + break; + case A2MP_GETINFO_REQ: case A2MP_GETAMPASSOC_REQ: case A2MP_CREATEPHYSLINK_REQ: -- cgit From 47f2d97d38816aaca94c9b6961c6eff1cfcd0bd6 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:11 +0300 Subject: Bluetooth: A2MP: Process A2MP Get Info Request Process A2MP Get Info Request. Example of trace log for invalid controller id is shown below: ... > ACL data: handle 11 flags 0x02 dlen 13 A2MP: Get Info req: id 238 < ACL data: handle 11 flags 0x00 dlen 30 A2MP: Get Info rsp: id 238 status (1) Invalid Controller ID ... Note that If the Status field is set to Invalid Controller ID all subsequent fields in the AMP Get Info Response shall be ignored by the receiver. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 6cdaa85ab5e9..350088e2015a 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -177,6 +177,40 @@ static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, return 0; } +static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_info_req *req = (void *) skb->data; + struct a2mp_info_rsp rsp; + struct hci_dev *hdev; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("id %d", req->id); + + rsp.id = req->id; + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + + hdev = hci_dev_get(req->id); + if (hdev && hdev->amp_type != HCI_BREDR) { + rsp.status = 0; + rsp.total_bw = cpu_to_le32(hdev->amp_total_bw); + rsp.max_bw = cpu_to_le32(hdev->amp_max_bw); + rsp.min_latency = cpu_to_le32(hdev->amp_min_latency); + rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap); + rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size); + } + + if (hdev) + hci_dev_put(hdev); + + a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp); + + skb_pull(skb, sizeof(*req)); + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -215,6 +249,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) break; case A2MP_GETINFO_REQ: + err = a2mp_getinfo_req(mgr, skb, hdr); + break; + case A2MP_GETAMPASSOC_REQ: case A2MP_CREATEPHYSLINK_REQ: case A2MP_DISCONNPHYSLINK_REQ: -- cgit From a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:12 +0300 Subject: Bluetooth: A2MP: Process A2MP Get AMP Assoc Request Example trace when receiving AMP Assoc Request with wrong AMP id. ... > ACL data: handle 11 flags 0x02 dlen 13 A2MP: Get AMP Assoc req: id 238 < ACL data: handle 11 flags 0x00 dlen 14 A2MP: Get AMP Assoc rsp: id 238 status (1) Invalid Controller ID assoc data: ... Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 350088e2015a..b8eeb33d60d7 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -211,6 +211,38 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, return 0; } +static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_amp_assoc_req *req = (void *) skb->data; + struct hci_dev *hdev; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("id %d", req->id); + + hdev = hci_dev_get(req->id); + if (!hdev || hdev->amp_type == HCI_BREDR) { + struct a2mp_amp_assoc_rsp rsp; + rsp.id = req->id; + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + + a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), + &rsp); + goto clean; + } + + /* Placeholder for HCI Read AMP Assoc */ + +clean: + if (hdev) + hci_dev_put(hdev); + + skb_pull(skb, sizeof(*req)); + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -253,6 +285,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) break; case A2MP_GETAMPASSOC_REQ: + err = a2mp_getampassoc_req(mgr, skb, hdr); + break; + case A2MP_CREATEPHYSLINK_REQ: case A2MP_DISCONNPHYSLINK_REQ: case A2MP_CHANGE_RSP: -- cgit From e072f5dab22e7bf0a10daf854acc0fc271396ee7 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:13 +0300 Subject: Bluetooth: A2MP: Process A2MP Create Physlink Request Placeholder for A2MP Create Physlink Request. Handles requests with invalid controler id as shown below: ... > ACL data: handle 11 flags 0x02 dlen 50 A2MP: Create Physical Link req: local id 1 remote id 85 Assoc data: < ACL data: handle 11 flags 0x00 dlen 15 A2MP: Create Physical Link rsp: local id 85 remote id 1 status 1 Invalid Controller ID ... Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 40 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index b8eeb33d60d7..ca52a91edd29 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -243,6 +243,43 @@ clean: return 0; } +static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_physlink_req *req = (void *) skb->data; + + struct a2mp_physlink_rsp rsp; + struct hci_dev *hdev; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); + + rsp.local_id = req->remote_id; + rsp.remote_id = req->local_id; + + hdev = hci_dev_get(req->remote_id); + if (!hdev || hdev->amp_type != HCI_AMP) { + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + goto send_rsp; + } + + /* TODO process physlink create */ + + rsp.status = A2MP_STATUS_SUCCESS; + +send_rsp: + if (hdev) + hci_dev_put(hdev); + + a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp), + &rsp); + + skb_pull(skb, le16_to_cpu(hdr->len)); + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -289,6 +326,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) break; case A2MP_CREATEPHYSLINK_REQ: + err = a2mp_createphyslink_req(mgr, skb, hdr); + break; + case A2MP_DISCONNPHYSLINK_REQ: case A2MP_CHANGE_RSP: case A2MP_DISCOVER_RSP: -- cgit From 6113f84fc1a8962aed25f54a115b196e9aea151f Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:14 +0300 Subject: Bluetooth: A2MP: Process A2MP Disc Physlink Request Placeholder for A2MP Disconnect Physlink Request. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index ca52a91edd29..400696b3b817 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -280,6 +280,39 @@ send_rsp: return 0; } +static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_physlink_req *req = (void *) skb->data; + struct a2mp_physlink_rsp rsp; + struct hci_dev *hdev; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); + + rsp.local_id = req->remote_id; + rsp.remote_id = req->local_id; + rsp.status = A2MP_STATUS_SUCCESS; + + hdev = hci_dev_get(req->local_id); + if (!hdev) { + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + goto send_rsp; + } + + /* TODO Disconnect Phys Link here */ + + hci_dev_put(hdev); + +send_rsp: + a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); + + skb_pull(skb, sizeof(*req)); + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -330,6 +363,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) break; case A2MP_DISCONNPHYSLINK_REQ: + err = a2mp_discphyslink_req(mgr, skb, hdr); + break; + case A2MP_CHANGE_RSP: case A2MP_DISCOVER_RSP: case A2MP_GETINFO_RSP: -- cgit From f6410a849b76f56c78d989786eb427b85a559b9f Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:15 +0300 Subject: Bluetooth: A2MP: Process A2MP Command Responses Process A2MP responses, print cmd code and ident for now. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 400696b3b817..6a933dab1b7f 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -313,6 +313,15 @@ send_rsp: return 0; } +static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + BT_DBG("ident %d code %d", hdr->ident, hdr->code); + + skb_pull(skb, le16_to_cpu(hdr->len)); + return 0; +} + /* Handle A2MP signalling */ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { @@ -372,6 +381,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) case A2MP_GETAMPASSOC_RSP: case A2MP_CREATEPHYSLINK_RSP: case A2MP_DISCONNPHYSLINK_RSP: + err = a2mp_cmd_rsp(mgr, skb, hdr); + break; + default: BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); err = -EINVAL; -- cgit From 416fa7527d6bf658e5517ea36d2de9270be2c11e Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:16 +0300 Subject: Bluetooth: A2MP: Handling fixed channels A2MP fixed channel do not have sk Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 6a933dab1b7f..f1ec1b1d308f 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -483,8 +483,7 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) hci_conn_hold(conn->hcon); - chan->omtu = L2CAP_A2MP_DEFAULT_MTU; - chan->imtu = L2CAP_A2MP_DEFAULT_MTU; + chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP; chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; chan->ops = &a2mp_chan_ops; -- cgit From 97e8e89d2d8185b7644c9941636d3682eedc517b Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Tue, 29 May 2012 13:59:17 +0300 Subject: Bluetooth: A2MP: Manage incoming connections Handle incoming A2MP connection by creating AMP manager and processing A2MP messages. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index f1ec1b1d308f..e08ca2ac31aa 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -569,3 +569,19 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn) return mgr; } + +struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct amp_mgr *mgr; + + mgr = amp_mgr_create(conn); + if (!mgr) { + BT_ERR("Could not create AMP manager"); + return NULL; + } + + BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan); + + return mgr->a2mp_chan; +} -- cgit From 3cabbfdad3150e0083a64c951701c970ef970c77 Mon Sep 17 00:00:00 2001 From: Andrei Emeltchenko Date: Thu, 31 May 2012 11:01:37 +0300 Subject: Bluetooth: A2MP: Do not reference hci_conn Make A2MP channel special channel which do not reference hci_conn. This prevents from keeping ACL connection open when all L2CAP channels got closed. hci_conn_hold and hci_conn_put are not reference counts on the hci_conn structure in the typical way. They are reference counts for the ACL. When you do the last hci_conn_put, the ACL is disconnected after a timeout. Reported-by: Mat Martineau Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo Padovan --- net/bluetooth/a2mp.c | 2 -- 1 file changed, 2 deletions(-) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index e08ca2ac31aa..0772c680abe6 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -481,8 +481,6 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) BT_DBG("chan %p", chan); - hci_conn_hold(conn->hcon); - chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP; chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; -- cgit From 7e1af8a3a51dbf5dc7392fb294a0830f7e853aa8 Mon Sep 17 00:00:00 2001 From: Gustavo Padovan Date: Tue, 29 May 2012 13:19:26 -0300 Subject: Bluetooth: Create empty l2cap ops function A2MP doesn't use part of the L2CAP chan ops API so we just create general empty function instead of the A2MP specific one. Signed-off-by: Gustavo Padovan Signed-off-by: Johan Hedberg --- net/bluetooth/a2mp.c | 23 +++-------------------- 1 file changed, 3 insertions(+), 20 deletions(-) (limited to 'net/bluetooth/a2mp.c') diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index 0772c680abe6..fb93250b3938 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -440,23 +440,6 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, return bt_skb_alloc(len, GFP_KERNEL); } -static struct l2cap_chan *a2mp_chan_no_new_conn_cb(struct l2cap_chan *chan) -{ - BT_ERR("new_connection for chan %p not implemented", chan); - - return NULL; -} - -static void a2mp_chan_no_teardown_cb(struct l2cap_chan *chan, int err) -{ - BT_ERR("teardown for chan %p not implemented", chan); -} - -static void a2mp_chan_no_ready(struct l2cap_chan *chan) -{ - BT_ERR("ready for chan %p not implemented", chan); -} - static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", .recv = a2mp_chan_recv_cb, @@ -465,9 +448,9 @@ static struct l2cap_ops a2mp_chan_ops = { .alloc_skb = a2mp_chan_alloc_skb_cb, /* Not implemented for A2MP */ - .new_connection = a2mp_chan_no_new_conn_cb, - .teardown = a2mp_chan_no_teardown_cb, - .ready = a2mp_chan_no_ready, + .new_connection = l2cap_chan_no_new_connection, + .teardown = l2cap_chan_no_teardown, + .ready = l2cap_chan_no_ready, }; static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) -- cgit