From: Andrei Emeltchenko <[email protected]>
RFC meant to indicate our current development status.
Code based on Code Aurora (git://codeaurora.org/kernel/msm.git) and Atheros
(search for: [PATCH 2/3] Add a2mp protocol/AMP manager, by Atheros Linux BT3)
implementations (mostly Code Aurora). Main difference to the original code is
- code rebase against recent bluetooth-next
- rewritten way to send A2MP msgs
- handling fixed channel modifications
- I was trying to separate AMP vs A2MP name usage. AMP is used for
"Alternate MAC PHY" controller, A2MP for "AMP Manager Protocol".
- fixes related to PTS beta A2MP tests
Andrei Emeltchenko (20):
Bluetooth: A2MP: create A2MP socket
Bluetooth: A2MP: sk_data_ready for a2mp socket
Bluetooth: A2MP: sk_state_change for a2mp socket
Bluetooth: A2MP: build and send msg helpers
Bluetooth: A2MP: AMP Manager basic functions
Bluetooth: A2MP: Remove AMP manager in state_change
Bluetooth: A2MP: AMP manager initialization
Bluetooth: A2MP: Definitions for A2MP commands
Bluetooth: A2MP: process A2MP messages
Bluetooth: A2MP: processing A2MP command reject
Bluetooth: A2MP: helper functions to count HCI devs
Bluetooth: A2MP: process A2MP Discover Request
Bluetooth: A2MP: process A2MP Change Notify
Bluetooth: A2MP: process A2MP Get Info Request
Bluetooth: A2MP: process A2MP Get AMP Assoc Request
Bluetooth: A2MP: process A2MP Create Physlink Request
Bluetooth: A2MP: process A2MP Disc Physlink Request
Bluetooth: A2MP: process A2MP Command Responses
Bluetooth: A2MP: handling fixed channel
Bluetooth: A2MP: manage incoming connection
include/net/bluetooth/a2mp.h | 135 ++++++++
include/net/bluetooth/hci_core.h | 14 +
include/net/bluetooth/l2cap.h | 5 +
net/bluetooth/a2mp.c | 630 ++++++++++++++++++++++++++++++++++++++
net/bluetooth/l2cap_core.c | 81 ++++--
5 files changed, 845 insertions(+), 20 deletions(-)
create mode 100644 include/net/bluetooth/a2mp.h
create mode 100644 net/bluetooth/a2mp.c
--
1.7.4.1
Hi Peter,
On Thu, Nov 03, 2011 at 05:20:38PM -0700, Peter Krystad wrote:
>
> Hi Andrei,
>
> > From: Andrei Emeltchenko <[email protected]>
> >
> > RFC meant to indicate our current development status.
> >
> > Code based on Code Aurora (git://codeaurora.org/kernel/msm.git) and
> Atheros
> > (search for: [PATCH 2/3] Add a2mp protocol/AMP manager, by Atheros Linux
> BT3)
> > implementations (mostly Code Aurora). Main difference to the original code
> is
> > - code rebase against recent bluetooth-next
> > - rewritten way to send A2MP msgs
> > - handling fixed channel modifications
> > - I was trying to separate AMP vs A2MP name usage. AMP is used for
> > "Alternate MAC PHY" controller, A2MP for "AMP Manager Protocol".
> > - fixes related to PTS beta A2MP tests
>
> I think your approach looks fine so far. Do you plan to continue on with
> adding detail to the patches in this series or will just do cleanup on them
> and then prepare another set?
I was thinking about rebasing first against recent bluetooth-next (since
my code was based upon a bit different approach (like fixed channel
features list patch).
Then I am planning to add some details to A2MP message
processing. Those I could test and verify with PTS "beta A2MP" tests.
After that I was thinking about adding physlink structure (at least
heplers) and then make interface to create fake/virtual AMP so that I
could use it for A2MP Discover, etc. (maybe we could use real wifi cheap
for e.g. AMPAssoc data).
What are your plans?
Best regards
Andrei Emeltchenko
Hi Andrei,
> From: Andrei Emeltchenko <[email protected]>
>
> RFC meant to indicate our current development status.
>
> Code based on Code Aurora (git://codeaurora.org/kernel/msm.git) and
Atheros
> (search for: [PATCH 2/3] Add a2mp protocol/AMP manager, by Atheros Linux
BT3)
> implementations (mostly Code Aurora). Main difference to the original code
is
> - code rebase against recent bluetooth-next
> - rewritten way to send A2MP msgs
> - handling fixed channel modifications
> - I was trying to separate AMP vs A2MP name usage. AMP is used for
> "Alternate MAC PHY" controller, A2MP for "AMP Manager Protocol".
> - fixes related to PTS beta A2MP tests
I think your approach looks fine so far. Do you plan to continue on with
adding detail to the patches in this series or will just do cleanup on them
and then prepare another set?
Peter.
--Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum
From: Andrei Emeltchenko <[email protected]>
Implement basic processing for AMP Manager Protocol (A2MP).
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 57 +++++++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 56 insertions(+), 1 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index afb01e8..5071acc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -71,6 +71,61 @@ static void a2mp_send(struct socket *a2mp_sock, u8 code, u8 ident, u16 len,
kfree(cmd);
}
+/* Handle A2MP signalling */
+void a2mp_receive(struct sock *sk, struct sk_buff *skb)
+{
+ struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+ struct amp_mgr *mgr;
+ int err = 0;
+
+ mgr = get_amp_mgr_sk(sk);
+ if (!mgr)
+ return;
+
+ while (skb->len >= sizeof(*hdr)) {
+ struct a2mp_cmd *hdr = (struct a2mp_cmd *) 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;
+ }
+
+ 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 signaling command 0x%2.2x",
+ hdr->code);
+ err = -EINVAL;
+ break;
+ }
+ }
+
+ if (err && mgr) {
+ struct a2mp_cmd_rej rej;
+ rej.reason = cpu_to_le16(0);
+
+ a2mp_send(mgr->a2mp_sock, A2MP_COMMAND_REJ, hdr->ident,
+ sizeof(rej), &rej);
+ }
+}
+
static void data_ready_worker(struct work_struct *w)
{
struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
@@ -81,7 +136,7 @@ static void data_ready_worker(struct work_struct *w)
/* skb_dequeue() is thread-safe */
while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
- /* process a2mp here */
+ a2mp_receive(sk, skb);
kfree_skb(skb);
}
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 20 ++++++++++++++++++++
1 files changed, 20 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index af3f2db..3e06c93 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -212,6 +212,23 @@ static inline 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 =
+ (struct a2mp_amp_assoc_req *)skb->data;
+
+ if (le16_to_cpu(hdr->len) < sizeof(*req))
+ return -EINVAL;
+
+ BT_DBG("id %d", req->id);
+
+ /* TODO send A2MP_GETAMPASSOC_RSP */
+
+ skb_pull(skb, sizeof(*req));
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -254,6 +271,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 19 +++++++++++++++++++
1 files changed, 19 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b67696f..3dd3cdb 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -162,6 +162,22 @@ static inline int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
return 0;
}
+static inline int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
+ struct a2mp_cmd *hdr)
+{
+ struct a2mp_cl *cl = (struct a2mp_cl *)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 */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -196,6 +212,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 37 +++++++++++++++++++++++++++++++++++++
1 files changed, 37 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3dd3cdb..af3f2db 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -178,6 +178,40 @@ static inline int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
return 0;
}
+static inline int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
+ struct a2mp_cmd *hdr)
+{
+ struct a2mp_info_req *req = (struct a2mp_info_req *)skb->data;
+ struct hci_dev *hdev;
+ struct a2mp_info_rsp rsp;
+
+ if (le16_to_cpu(hdr->len) < sizeof(*req))
+ return -EINVAL;
+
+ rsp.id = req->id;
+ rsp.status = 1;
+
+ BT_DBG("id %d", req->id);
+ hdev = hci_dev_get(AMP_HCI_ID(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);
+ }
+
+ /* TODO send A2MP_GETINFO_RSP */
+
+ if (hdev)
+ hci_dev_put(hdev);
+
+ skb_pull(skb, sizeof(*req));
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -216,6 +250,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
A2MP fixed channel is handled differently from normal L2CAP channel.
There is no connect and config req/rsp sequence.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/l2cap.h | 1 +
net/bluetooth/a2mp.c | 2 +
net/bluetooth/l2cap_core.c | 61 +++++++++++++++++++++++++++++-----------
3 files changed, 47 insertions(+), 17 deletions(-)
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index cc3b81f..473976e 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -559,6 +559,7 @@ enum {
FLAG_FLUSHABLE,
FLAG_EXT_CTRL,
FLAG_EFS_ENABLE,
+ FLAG_FIXED_CHANNEL,
};
#define __set_chan_timer(c, t) l2cap_set_timer(c, &c->chan_timer, (t))
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4414d74..b4d3a13 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -451,6 +451,8 @@ static void l2cap_fixed_channel_config(struct sock *sk)
lock_sock(sk);
+ set_bit(FLAG_FIXED_CHANNEL, &chan->flags);
+
chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 8b930ca..3dd80a5 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -316,6 +316,10 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
chan->conn = conn;
+ /* Fixed channel setup is already done */
+ if (test_bit(FLAG_FIXED_CHANNEL, &chan->flags))
+ goto fixed_chan;
+
if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
if (conn->hcon->type == LE_LINK) {
/* LE connection */
@@ -339,6 +343,8 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
chan->omtu = L2CAP_DEFAULT_MTU;
}
+fixed_chan:
+
chan->local_id = L2CAP_BESTEFFORT_ID;
chan->local_stype = L2CAP_SERV_BESTEFFORT;
chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE;
@@ -371,7 +377,8 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
chan_put(chan);
chan->conn = NULL;
- hci_conn_put(conn->hcon);
+ if (!test_bit(FLAG_FIXED_CHANNEL, &chan->flags))
+ hci_conn_put(conn->hcon);
}
l2cap_state_change(chan, BT_CLOSED);
@@ -387,7 +394,8 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
sk->sk_state_change(sk);
if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
- test_bit(CONF_INPUT_DONE, &chan->conf_state)))
+ test_bit(CONF_INPUT_DONE, &chan->conf_state)) &&
+ !test_bit(FLAG_FIXED_CHANNEL, &chan->flags))
return;
skb_queue_purge(&chan->tx_q);
@@ -1128,23 +1136,34 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
auth_type = l2cap_get_auth_type(chan);
- if (chan->dcid == L2CAP_CID_LE_DATA)
- hcon = hci_connect(hdev, LE_LINK, dst,
- chan->sec_level, auth_type);
- else
- hcon = hci_connect(hdev, ACL_LINK, dst,
- chan->sec_level, auth_type);
+ if (test_bit(FLAG_FIXED_CHANNEL, &chan->flags)) {
+ /* Fixed channels piggyback on existing ACL connections */
+ hcon = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
+ if (!hcon || !hcon->l2cap_data) {
+ err = -ENOTCONN;
+ goto done;
+ }
- if (IS_ERR(hcon)) {
- err = PTR_ERR(hcon);
- goto done;
- }
+ conn = hcon->l2cap_data;
+ } else {
+ if (chan->dcid == L2CAP_CID_LE_DATA)
+ hcon = hci_connect(hdev, LE_LINK, dst,
+ chan->sec_level, auth_type);
+ else
+ hcon = hci_connect(hdev, ACL_LINK, dst,
+ chan->sec_level, auth_type);
- conn = l2cap_conn_add(hcon, 0);
- if (!conn) {
- hci_conn_put(hcon);
- err = -ENOMEM;
- goto done;
+ if (IS_ERR(hcon)) {
+ err = PTR_ERR(hcon);
+ goto done;
+ }
+
+ conn = l2cap_conn_add(hcon, 0);
+ if (!conn) {
+ hci_conn_put(hcon);
+ err = -ENOMEM;
+ goto done;
+ }
}
/* Update source addr of the socket */
@@ -1152,6 +1171,14 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
l2cap_chan_add(conn, chan);
+ if (test_bit(FLAG_FIXED_CHANNEL, &chan->flags)) {
+ l2cap_state_change(chan, BT_CONNECTED);
+ sk->sk_state = BT_CONNECTED;
+ sk->sk_state_change(sk);
+ err = 0;
+ goto done;
+ }
+
l2cap_state_change(chan, BT_CONNECT);
__set_chan_timer(chan, sk->sk_sndtimeo);
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 42 ++++++++++++++++++++++++++++++++++++++++++
1 files changed, 42 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b584465..662ada7 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -245,6 +245,45 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
return 0;
}
+static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
+ struct a2mp_cmd *hdr)
+{
+ struct a2mp_physlink_req *req = (struct a2mp_physlink_req *)skb->data;
+ struct a2mp_physlink_rsp rsp;
+ struct hci_dev *hdev;
+ struct hci_conn *conn;
+
+ if (le16_to_cpu(hdr->len) < sizeof(*req))
+ return -EINVAL;
+
+ rsp.local_id = req->remote_id;
+ rsp.remote_id = req->local_id;
+ rsp.status = 0;
+
+ BT_DBG("local_id %d remote_id %d", rsp.local_id, rsp.remote_id);
+ hdev = hci_dev_get(AMP_HCI_ID(req->local_id));
+ if (!hdev) {
+ rsp.status = 1; /* Invalid Controller ID */
+ goto send_rsp;
+ }
+
+ conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
+ &mgr->l2cap_conn->hcon->dst);
+ if (conn) {
+ BT_DBG("conn %p", conn);
+ hci_acl_disconn(conn, 0x13);
+ }
+
+send_rsp:
+ /* TODO send A2MP_DISCONNPHYSLINK_RSP */
+
+ if (hdev)
+ hci_dev_put(hdev);
+
+ skb_pull(skb, sizeof(*req));
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -295,6 +334,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Handle incoming A2MP connection by creating AMP manager and
processing A2MP messages in a workqueue.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 7 +++++++
net/bluetooth/a2mp.c | 34 ++++++++++++++++++++++++++++++++++
net/bluetooth/l2cap_core.c | 10 ++++++++--
3 files changed, 49 insertions(+), 2 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 1782f47..fa1477f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,6 +20,7 @@
struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn);
void remove_amp_mgr(struct amp_mgr *mgr);
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
struct amp_mgr {
struct list_head list;
@@ -122,6 +123,12 @@ struct a2mp_work_state_change {
struct sock *sk;
};
+struct a2mp_work_incoming {
+ struct work_struct work;
+ struct l2cap_conn *conn;
+ struct sk_buff *skb;
+};
+
int a2mp_init(void);
void a2mp_exit(void);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b4d3a13..2ffe0dd 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -581,6 +581,40 @@ finished:
return mgr;
}
+static void incoming_worker(struct work_struct *w)
+{
+ struct a2mp_work_incoming *work = (struct a2mp_work_incoming *) w;
+ struct l2cap_conn *conn = work->conn;
+ struct amp_mgr *mgr;
+ struct sock *sk;
+
+ mgr = create_amp_mgr(conn);
+ sk = mgr->a2mp_sock->sk;
+
+ BT_DBG("mgr %p sk %p", mgr, sk);
+
+ lock_sock(sk);
+ sk->sk_backlog_rcv(sk, work->skb);
+ release_sock(sk);
+
+ kfree(work);
+}
+
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+ struct a2mp_work_incoming *work;
+
+ BT_DBG("conn %p", conn);
+ work = kmalloc(sizeof(*work), GFP_KERNEL);
+ if (work) {
+ INIT_WORK((struct work_struct *) work, incoming_worker);
+ work->conn = conn;
+ work->skb = skb;
+ if (queue_work(a2mp_workqueue, (struct work_struct *) work) == 0)
+ kfree(work);
+ }
+}
+
int a2mp_init(void)
{
a2mp_workqueue = create_singlethread_workqueue("a2mp");
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 3dd80a5..83cad81 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -4077,8 +4077,14 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
chan = l2cap_get_chan_by_scid(conn, cid);
if (!chan) {
- BT_DBG("unknown cid 0x%4.4x", cid);
- goto drop;
+ if (cid == L2CAP_CID_A2MP) {
+ BT_DBG("A2MP");
+ a2mp_incoming(conn, skb);
+ goto done;
+ } else {
+ BT_DBG("unknown cid 0x%4.4x", cid);
+ goto drop;
+ }
}
sk = chan->sk;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 12 ++++++++++++
1 files changed, 12 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 662ada7..4414d74 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -284,6 +284,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, hdr->len);
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -343,6 +352,9 @@ void a2mp_receive(struct sock *sk, 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 signaling command 0x%2.2x",
hdr->code);
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 19 +++++++++++++++++++
1 files changed, 19 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3e06c93..b584465 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -229,6 +229,22 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
return 0;
}
+static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
+ struct a2mp_cmd *hdr)
+{
+ struct a2mp_physlink_req *req = (struct a2mp_physlink_req *)skb->data;
+
+ if (le16_to_cpu(hdr->len) < sizeof(*req))
+ return -EINVAL;
+
+ BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
+
+ /* TODO process physlink create */
+
+ skb_pull(skb, hdr->len);
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -275,6 +291,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 19 +++++++++++++++++++
1 files changed, 19 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5071acc..996a9dc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -71,6 +71,22 @@ static void a2mp_send(struct socket *a2mp_sock, u8 code, u8 ident, u16 len,
kfree(cmd);
}
+/* Processing A2MP messages */
+static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
+ struct a2mp_cmd *hdr)
+{
+ struct a2mp_cmd_rej *rej = (struct a2mp_cmd_rej *)skb->data;
+
+ if (skb->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 */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -97,6 +113,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 3 +++
net/bluetooth/a2mp.c | 14 ++++++++++++++
net/bluetooth/l2cap_core.c | 8 ++++++++
3 files changed, 25 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 9827cec..aafaf09 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -44,4 +44,7 @@ struct a2mp_work_state_change {
struct sock *sk;
};
+int a2mp_init(void);
+void a2mp_exit(void);
+
#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 190827f..afb01e8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -277,3 +277,17 @@ struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn)
finished:
return mgr;
}
+
+int a2mp_init(void)
+{
+ a2mp_workqueue = create_singlethread_workqueue("a2mp");
+ if (!a2mp_workqueue)
+ return -EPERM;
+ return 0;
+}
+
+void a2mp_exit(void)
+{
+ flush_workqueue(a2mp_workqueue);
+ destroy_workqueue(a2mp_workqueue);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 1a1fe2d..8b930ca 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -55,6 +55,7 @@
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
int disable_ertm;
int enable_hs;
@@ -4621,6 +4622,12 @@ int __init l2cap_init(void)
BT_ERR("Failed to create L2CAP debug file");
}
+ err = a2mp_init();
+ if (err < 0) {
+ BT_ERR("AMP Manager initialization failed");
+ goto error;
+ }
+
return 0;
error:
@@ -4630,6 +4637,7 @@ error:
void l2cap_exit(void)
{
+ a2mp_exit();
debugfs_remove(l2cap_debugfs);
if (hci_unregister_proto(&l2cap_hci_proto) < 0)
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Define AMP Manager and some basic functions.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 11 +++++++
net/bluetooth/a2mp.c | 60 ++++++++++++++++++++++++++++++++++++++++++
2 files changed, 71 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 7ead6a3..9827cec 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,17 @@
#ifndef __A2MP_H
#define __A2MP_H
+struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
+struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn);
+void remove_amp_mgr(struct amp_mgr *mgr);
+
+struct amp_mgr {
+ struct list_head list;
+ struct l2cap_conn *l2cap_conn;
+ struct socket *a2mp_sock;
+ unsigned long flags;
+};
+
struct a2mp_cmd {
__u8 code;
__u8 ident;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2fe0004..d505c77 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -13,6 +13,7 @@
*/
#include <linux/workqueue.h>
+#include <linux/list.h>
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
@@ -21,6 +22,9 @@
static struct workqueue_struct *a2mp_workqueue;
+LIST_HEAD(amp_mgr_list);
+DEFINE_RWLOCK(amp_mgr_list_lock);
+
/* A2MP build & send command helper functions */
static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
{
@@ -205,3 +209,59 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
}
}
+/* AMP Manager functions */
+struct amp_mgr *get_amp_mgr_sk(struct sock *sk)
+{
+ struct amp_mgr *mgr;
+ struct amp_mgr *found = NULL;
+
+ read_lock(&_mgr_list_lock);
+ list_for_each_entry(mgr, &_mgr_list, list) {
+ if ((mgr->a2mp_sock) && (mgr->a2mp_sock->sk == sk)) {
+ found = mgr;
+ break;
+ }
+ }
+ read_unlock(&_mgr_list_lock);
+
+ return found;
+}
+
+void remove_amp_mgr(struct amp_mgr *mgr)
+{
+ BT_DBG("mgr %p", mgr);
+
+ write_lock(&_mgr_list_lock);
+ sock_release(mgr->a2mp_sock);
+ list_del(&mgr->list);
+ write_unlock(&_mgr_list_lock);
+
+ kfree(mgr);
+}
+
+struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn)
+{
+ struct amp_mgr *mgr;
+
+ mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
+ if (!mgr)
+ goto finished;
+
+ BT_DBG("conn %p mgr %p", conn, mgr);
+
+ mgr->l2cap_conn = conn;
+
+ mgr->a2mp_sock = open_a2mp_sock(conn);
+ if (!mgr->a2mp_sock) {
+ kfree(mgr);
+ mgr = NULL;
+ goto finished;
+ }
+
+ write_lock(&_mgr_list_lock);
+ list_add(&(mgr->list), &_mgr_list);
+ write_unlock(&_mgr_list_lock);
+
+finished:
+ return mgr;
+}
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 6 +++++
net/bluetooth/a2mp.c | 51 ++++++++++++++++++++++++++++++++++++++++++
2 files changed, 57 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index e2eec2b..1782f47 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,8 @@
#ifndef __A2MP_H
#define __A2MP_H
+#define A2MP_FEAT_EXT 0x8000
+
struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn);
void remove_amp_mgr(struct amp_mgr *mgr);
@@ -105,6 +107,10 @@ struct a2mp_physlink_rsp {
__u8 status;
} __packed;
+enum {
+ FLAG_DISCOVERED,
+};
+
struct a2mp_work_data_ready {
struct work_struct work;
struct sock *sk;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e10924e..b67696f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -114,6 +114,54 @@ static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
return 0;
}
+static inline int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
+ struct a2mp_cmd *hdr)
+{
+ struct a2mp_discov_req *req = (struct a2mp_discov_req *)skb->data;
+ struct a2mp_discov_rsp *rsp;
+ u16 ext_feat;
+ size_t len;
+ u8 num_ctrl;
+
+ if (skb->len < sizeof(*req))
+ return -EINVAL;
+
+ skb_pull(skb, sizeof(*req));
+
+ BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu),
+ le16_to_cpu(req->ext_feat));
+
+ ext_feat = le16_to_cpu(req->ext_feat);
+
+ /* check that packet is not broken for now */
+ while (ext_feat & A2MP_FEAT_EXT) {
+ if (skb->len < sizeof(ext_feat))
+ return -EINVAL;
+
+ ext_feat = get_unaligned_le16(skb->data);
+ BT_DBG("ext_feat 0x%4.4x", le16_to_cpu(req->ext_feat));
+ skb_pull(skb, sizeof(ext_feat));
+ }
+
+ num_ctrl = hci_num_ctrl();
+ len = num_ctrl * sizeof(struct a2mp_cl) + sizeof (*rsp);
+ rsp = kmalloc(len, GFP_KERNEL);
+ if (!rsp)
+ return -ENOMEM;
+
+ rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+ rsp->ext_feat = 0;
+
+ __a2mp_add_cl(rsp->cl, num_ctrl);
+
+ BT_DBG("num_ctrl = %d", num_ctrl);
+
+ a2mp_send(mgr->a2mp_sock, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+ kfree(rsp);
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -144,6 +192,9 @@ void a2mp_receive(struct sock *sk, 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:
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Handle BT_CLOSED state change by removing AMP manager.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 14 +++++++++++++-
1 files changed, 13 insertions(+), 1 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d505c77..190827f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -110,8 +110,20 @@ static void state_change_worker(struct work_struct *w)
{
struct a2mp_work_state_change *work = (struct a2mp_work_state_change *) w;
struct sock *sk = work->sk;
+ struct amp_mgr *mgr;
+
+ BT_DBG("sk %p state %d", sk, sk->sk_state);
+
+ switch(sk->sk_state) {
+ case BT_CLOSED:
+ mgr = get_amp_mgr_sk(sk);
+ if (mgr) {
+ BT_DBG("sk %p closing", sk);
+ remove_amp_mgr(mgr);
+ }
+ break;
+ }
- BT_DBG("sk %p", sk);
sock_put(work->sk);
kfree(work);
}
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Define A2MP command id and packet structures.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 72 ++++++++++++++++++++++++++++++++++++++++++
1 files changed, 72 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index aafaf09..e2eec2b 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -33,6 +33,78 @@ struct a2mp_cmd {
__u8 data[0];
} __packed;
+/* A2MP command codes */
+#define A2MP_COMMAND_REJ 0x01
+struct a2mp_cmd_rej {
+ __le16 reason;
+} __packed;
+
+#define A2MP_DISCOVER_REQ 0x02
+struct a2mp_discov_req {
+ __le16 mtu;
+ __le16 ext_feat;
+} __packed;
+
+struct a2mp_cl {
+ __u8 id;
+ __u8 type;
+ __u8 status;
+} __packed;
+
+#define A2MP_DISCOVER_RSP 0x03
+struct a2mp_discov_rsp {
+ __le16 mtu;
+ __le16 ext_feat;
+ struct a2mp_cl cl[0];
+} __packed;
+
+#define A2MP_CHANGE_NOTIFY 0x04
+#define A2MP_CHANGE_RSP 0x05
+
+#define A2MP_GETINFO_REQ 0x06
+struct a2mp_info_req {
+ __u8 id;
+} __packed;
+
+#define A2MP_GETINFO_RSP 0x07
+struct a2mp_info_rsp {
+ __u8 id;
+ __u8 status;
+ __le32 total_bw;
+ __le32 max_bw;
+ __le32 min_latency;
+ __le16 pal_cap;
+ __le16 assoc_size;
+} __packed;
+
+#define A2MP_GETAMPASSOC_REQ 0x08
+struct a2mp_amp_assoc_req {
+ __u8 id;
+} __packed;
+
+#define A2MP_GETAMPASSOC_RSP 0x09
+struct a2mp_amp_assoc_rsp {
+ __u8 id;
+ __u8 status;
+ __u8 amp_assoc[0];
+} __packed;
+
+#define A2MP_CREATEPHYSLINK_REQ 0x0A
+#define A2MP_DISCONNPHYSLINK_REQ 0x0C
+struct a2mp_physlink_req {
+ __u8 local_id;
+ __u8 remote_id;
+ __u8 amp_assoc[0];
+} __packed;
+
+#define A2MP_CREATEPHYSLINK_RSP 0x0B
+#define A2MP_DISCONNPHYSLINK_RSP 0x0D
+struct a2mp_physlink_rsp {
+ __u8 local_id;
+ __u8 remote_id;
+ __u8 status;
+} __packed;
+
struct a2mp_work_data_ready {
struct work_struct work;
struct sock *sk;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Helper functions used to cound HCI devices (AMP controllers) and
build controller list packet.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/hci_core.h | 14 ++++++++++++++
net/bluetooth/a2mp.c | 27 +++++++++++++++++++++++++++
2 files changed, 41 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index d037516..a9e5168 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -543,6 +543,20 @@ static inline struct hci_dev *hci_dev_hold(struct hci_dev *d)
return NULL;
}
+static inline uint8_t hci_num_ctrl(void)
+{
+ uint8_t count = 0;
+ struct list_head *p;
+
+ read_lock_bh(&hci_dev_list_lock);
+ list_for_each(p, &hci_dev_list) {
+ count++;
+ }
+ read_unlock_bh(&hci_dev_list_lock);
+
+ return count;
+}
+
#define hci_dev_lock(d) spin_lock(&d->lock)
#define hci_dev_unlock(d) spin_unlock(&d->lock)
#define hci_dev_lock_bh(d) spin_lock_bh(&d->lock)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 996a9dc..e10924e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -71,6 +71,33 @@ static void a2mp_send(struct socket *a2mp_sock, 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;
+}
+
+static void __a2mp_add_cl(struct a2mp_cl *cl, u8 num_ctrl)
+{
+ int i = 0;
+ struct hci_dev *hdev;
+
+ read_lock_bh(&hci_dev_list_lock);
+
+ __a2mp_cl_bredr(cl);
+
+ list_for_each_entry(hdev, &hci_dev_list, list) {
+ if (++i >= num_ctrl)
+ goto out;
+ cl[i].id = hdev->id;
+ cl[i].type = hdev->amp_type;
+ cl[i].status = hdev->amp_status;
+ }
+out:
+ read_unlock_bh(&hci_dev_list_lock);
+}
+
/* Processing A2MP messages */
static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
struct a2mp_cmd *hdr)
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Create fixed channel A2MP socket
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/l2cap.h | 4 ++
net/bluetooth/a2mp.c | 94 +++++++++++++++++++++++++++++++++++++++++
net/bluetooth/l2cap_core.c | 2 +-
3 files changed, 99 insertions(+), 1 deletions(-)
create mode 100644 net/bluetooth/a2mp.c
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index f478301..cc3b81f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -45,6 +45,8 @@
#define L2CAP_DEFAULT_SDU_ITIME 0xFFFFFFFF
#define L2CAP_DEFAULT_ACC_LAT 0xFFFFFFFF
+#define L2CAP_A2MP_DEFAULT_MTU 670
+
#define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
#define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */
@@ -217,6 +219,7 @@ struct l2cap_conn_rsp {
/* channel indentifier */
#define L2CAP_CID_SIGNALING 0x0001
#define L2CAP_CID_CONN_LESS 0x0002
+#define L2CAP_CID_A2MP 0x0003
#define L2CAP_CID_LE_DATA 0x0004
#define L2CAP_CID_LE_SIGNALING 0x0005
#define L2CAP_CID_SMP 0x0006
@@ -772,5 +775,6 @@ void l2cap_chan_destroy(struct l2cap_chan *chan);
int l2cap_chan_connect(struct l2cap_chan *chan);
int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len);
void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
+void l2cap_ertm_init(struct l2cap_chan *chan);
#endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 0000000..b69dd07
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,94 @@
+/*
+ Copyright (c) 2010-2011 Code Aurora Forum. All rights reserved.
+ Copyright (c) 2011 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 <linux/workqueue.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+
+static void l2cap_fixed_channel_config(struct sock *sk)
+{
+ struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+
+ lock_sock(sk);
+
+ chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+ chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+ chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
+ chan->fcs = L2CAP_FCS_CRC16;
+
+ chan->max_tx = 0xFF;
+ chan->remote_max_tx = chan->max_tx;
+
+ chan->tx_win = 1;
+ chan->remote_tx_win = chan->tx_win;
+
+ chan->remote_mps = chan->omtu;
+ chan->mps = chan->omtu;
+
+ 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;
+ l2cap_ertm_init(chan);
+
+ release_sock(sk);
+}
+
+static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
+{
+ int err;
+ struct socket *sock;
+ struct sockaddr_l2 addr;
+ struct sock *sk;
+
+ err = sock_create_kern(PF_BLUETOOTH, SOCK_SEQPACKET,
+ BTPROTO_L2CAP, &sock);
+ if (err) {
+ BT_ERR("sock_create_kern failed %d", err);
+ return NULL;
+ }
+
+ sk = sock->sk;
+ memset(&addr, 0, sizeof(addr));
+ bacpy(&addr.l2_bdaddr, conn->src);
+ addr.l2_family = AF_BLUETOOTH;
+ addr.l2_cid = L2CAP_CID_A2MP;
+ err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr));
+ if (err) {
+ BT_ERR("kernel_bind failed %d", err);
+ sock_release(sock);
+ return NULL;
+ }
+
+ l2cap_fixed_channel_config(sk);
+
+ memset(&addr, 0, sizeof(addr));
+ bacpy(&addr.l2_bdaddr, conn->dst);
+ addr.l2_family = AF_BLUETOOTH;
+ addr.l2_cid = L2CAP_CID_A2MP;
+ err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr),
+ O_NONBLOCK);
+ if ((err == 0) || (err == -EINPROGRESS))
+ return sock;
+ else {
+ BT_ERR("kernel_connect failed %d", err);
+ sock_release(sock);
+ return NULL;
+ }
+}
+
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index fdb72ea..1a1fe2d 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1924,7 +1924,7 @@ static void l2cap_ack_timeout(unsigned long arg)
bh_unlock_sock(chan->sk);
}
-static inline void l2cap_ertm_init(struct l2cap_chan *chan)
+void l2cap_ertm_init(struct l2cap_chan *chan)
{
struct sock *sk = chan->sk;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 24 ++++++++++++++++++++++++
net/bluetooth/a2mp.c | 39 +++++++++++++++++++++++++++++++++++++++
2 files changed, 63 insertions(+), 0 deletions(-)
create mode 100644 include/net/bluetooth/a2mp.h
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
new file mode 100644
index 0000000..95dfaeb
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,24 @@
+/*
+ Copyright (c) 2010-2011 Code Aurora Forum. All rights reserved.
+ Copyright (c) 2011 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.
+*/
+
+#ifndef __A2MP_H
+#define __A2MP_H
+
+struct a2mp_work_data_ready {
+ struct work_struct work;
+ struct sock *sk;
+ int bytes;
+};
+
+#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b69dd07..2b41882 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,44 @@
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+
+static struct workqueue_struct *a2mp_workqueue;
+
+static void data_ready_worker(struct work_struct *w)
+{
+ struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
+ struct sock *sk = work->sk;
+ struct sk_buff *skb;
+
+ BT_DBG("sk %p", sk);
+
+ /* skb_dequeue() is thread-safe */
+ while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
+ /* process a2mp here */
+ kfree_skb(skb);
+ }
+
+ sock_put(work->sk);
+ kfree(work);
+}
+
+static void a2mp_data_ready(struct sock *sk, int bytes)
+{
+ struct a2mp_work_data_ready *work;
+
+ work = kmalloc(sizeof(*work), GFP_KERNEL);
+ if (work) {
+ INIT_WORK(&work->work, data_ready_worker);
+ sock_hold(sk);
+ work->sk = sk;
+ work->bytes = bytes;
+ if (!queue_work(a2mp_workqueue, &work->work)) {
+ kfree(work);
+ sock_put(sk);
+ }
+ }
+}
static void l2cap_fixed_channel_config(struct sock *sk)
{
@@ -64,6 +102,7 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
}
sk = sock->sk;
+ sk->sk_data_ready = a2mp_data_ready;
memset(&addr, 0, sizeof(addr));
bacpy(&addr.l2_bdaddr, conn->src);
addr.l2_family = AF_BLUETOOTH;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Helper function to build and send A2MP messages.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 7 ++++++
net/bluetooth/a2mp.c | 46 ++++++++++++++++++++++++++++++++++++++++++
2 files changed, 53 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index a3938f9..7ead6a3 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,13 @@
#ifndef __A2MP_H
#define __A2MP_H
+struct a2mp_cmd {
+ __u8 code;
+ __u8 ident;
+ __le16 len;
+ __u8 data[0];
+} __packed;
+
struct a2mp_work_data_ready {
struct work_struct work;
struct sock *sk;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d0b15d8..2fe0004 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -21,6 +21,52 @@
static struct workqueue_struct *a2mp_workqueue;
+/* 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 inline int __a2mp_send(struct socket *sock, u8 *data, int len)
+{
+ struct kvec iv = { data, len };
+ struct msghdr msg;
+
+ memset(&msg, 0, sizeof(msg));
+
+ return kernel_sendmsg(sock, &msg, &iv, 1, len);
+}
+
+static void a2mp_send(struct socket *a2mp_sock, u8 code, u8 ident, u16 len,
+ void *data)
+{
+ struct a2mp_cmd *cmd;
+
+ if (!a2mp_sock)
+ return;
+
+ cmd = __a2mp_build(code, ident, len, data);
+ if (!cmd)
+ return;
+
+ __a2mp_send(a2mp_sock, (u8 *)cmd, len + sizeof(*cmd));
+ kfree(cmd);
+}
+
static void data_ready_worker(struct work_struct *w)
{
struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 5 +++++
net/bluetooth/a2mp.c | 28 ++++++++++++++++++++++++++++
2 files changed, 33 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 95dfaeb..a3938f9 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -21,4 +21,9 @@ struct a2mp_work_data_ready {
int bytes;
};
+struct a2mp_work_state_change {
+ struct work_struct work;
+ struct sock *sk;
+};
+
#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2b41882..d0b15d8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -56,6 +56,32 @@ static void a2mp_data_ready(struct sock *sk, int bytes)
}
}
+static void state_change_worker(struct work_struct *w)
+{
+ struct a2mp_work_state_change *work = (struct a2mp_work_state_change *) w;
+ struct sock *sk = work->sk;
+
+ BT_DBG("sk %p", sk);
+ sock_put(work->sk);
+ kfree(work);
+}
+
+static void a2mp_state_change(struct sock *sk)
+{
+ struct a2mp_work_state_change *work;
+
+ work = kmalloc(sizeof(*work), GFP_KERNEL);
+ if (work) {
+ INIT_WORK(&work->work, state_change_worker);
+ sock_hold(sk);
+ work->sk = sk;
+ if (!queue_work(a2mp_workqueue, &work->work)) {
+ kfree(work);
+ sock_put(sk);
+ }
+ }
+}
+
static void l2cap_fixed_channel_config(struct sock *sk)
{
struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -103,6 +129,8 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
sk = sock->sk;
sk->sk_data_ready = a2mp_data_ready;
+ sk->sk_state_change = a2mp_state_change;
+
memset(&addr, 0, sizeof(addr));
bacpy(&addr.l2_bdaddr, conn->src);
addr.l2_family = AF_BLUETOOTH;
--
1.7.4.1