From: Andrei Emeltchenko <[email protected]>
Changes:
* RFCv2 rebased against "workqueue" patches.
* RFCv1 added refcnt to amp_mgr, fixed sleeping in atomic
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: Add data_ready and state_change
Bluetooth: A2MP: AMP Manager basic functions
Bluetooth: A2MP: Build and Send msg helpers
Bluetooth: A2MP: Remove AMP manager in state_change
Bluetooth: A2MP: AMP manager initialization
Bluetooth: A2MP: Definitions for A2MP commands
Bluetooth: A2MP: Define A2MP status codes
Bluetooth: A2MP: Process A2MP messages
Bluetooth: A2MP: Process 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 connections
include/net/bluetooth/a2mp.h | 128 ++++++++
include/net/bluetooth/hci.h | 2 +
include/net/bluetooth/hci_core.h | 14 +
include/net/bluetooth/l2cap.h | 6 +
net/bluetooth/a2mp.c | 600 ++++++++++++++++++++++++++++++++++++++
net/bluetooth/l2cap_core.c | 90 ++++--
6 files changed, 812 insertions(+), 28 deletions(-)
create mode 100644 include/net/bluetooth/a2mp.h
create mode 100644 net/bluetooth/a2mp.c
--
1.7.4.1
Hi Marcel, Gustavo,
On Thu, Dec 22, 2011 at 04:12:40PM -0200, Gustavo Padovan wrote:
...
> > > +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;
> > > + }
> > > +}
> >
> > And now I am confused. So either we use the socket inside the kernel
> > directly or we make internal calls to L2CAP. However this looks like a
> > really weird mix between we do whatever we please.
> >
> > If this is has a long term plan that I am not away, I like to see these
> > parts properly commented in the code. Otherwise we end up in a disaster
> > area later on.
>
> We can also understand this as another call to create a proper way to access
> L2CAP inside the kernel. A big part of the work to create such interface is
> done, though the hardest part was left to the final.
The main idea is to use existing code to handle L2CAP fixed channel
without L2CAP channel initialization (Connect Req/Rsp, ...). The amount of
code change is really small.
The other patch "[RFCv2 19/20] Bluetooth: A2MP: Handling fixed channel"
continues this idea.
Do you think that approach is a bit hackish?
Best regards
Andrei Emeltchenko
> We could go L2CAP and fix this first and then create clean A2DP right from the
> beginning.
>
> Gustavo
Hi Marcel,
* Marcel Holtmann <[email protected]> [2011-12-22 09:18:16 -0800]:
> Hi Andrei,
>
> > diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> > index f141fbe..b6ed4e5 100644
> > --- a/include/net/bluetooth/l2cap.h
> > +++ b/include/net/bluetooth/l2cap.h
> > @@ -51,6 +51,8 @@
> > #define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
> > #define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */
> >
> > +#define L2CAP_A2MP_DEFAULT_MTU 670
> > +
> > /* L2CAP socket address */
> > struct sockaddr_l2 {
> > sa_family_t l2_family;
> > @@ -224,6 +226,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
> > @@ -827,5 +830,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
> > u32 priority);
> > void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
> > int l2cap_chan_check_security(struct l2cap_chan *chan);
> > +void l2cap_ertm_init(struct l2cap_chan *chan);
>
> these kind of changes that change the scope of a function deserve a
> separate patch and proper commit message why we are doing it.
>
> > #endif /* __L2CAP_H */
> > diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> > new file mode 100644
> > index 0000000..a5e9d17
> > --- /dev/null
> > +++ b/net/bluetooth/a2mp.c
> > @@ -0,0 +1,91 @@
> > +/*
> > + 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 <net/bluetooth/bluetooth.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;
> > +
> > + /* Send 10 packets without ack */
> > + chan->tx_win = 10;
> > + 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;
> > + }
> > +}
>
> And now I am confused. So either we use the socket inside the kernel
> directly or we make internal calls to L2CAP. However this looks like a
> really weird mix between we do whatever we please.
>
> If this is has a long term plan that I am not away, I like to see these
> parts properly commented in the code. Otherwise we end up in a disaster
> area later on.
We can also understand this as another call to create a proper way to access
L2CAP inside the kernel. A big part of the work to create such interface is
done, though the hardest part was left to the final.
We could go L2CAP and fix this first and then create clean A2DP right from the
beginning.
Gustavo
Hi Andrei,
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index f141fbe..b6ed4e5 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -51,6 +51,8 @@
> #define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
> #define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */
>
> +#define L2CAP_A2MP_DEFAULT_MTU 670
> +
> /* L2CAP socket address */
> struct sockaddr_l2 {
> sa_family_t l2_family;
> @@ -224,6 +226,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
> @@ -827,5 +830,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
> u32 priority);
> void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
> int l2cap_chan_check_security(struct l2cap_chan *chan);
> +void l2cap_ertm_init(struct l2cap_chan *chan);
these kind of changes that change the scope of a function deserve a
separate patch and proper commit message why we are doing it.
> #endif /* __L2CAP_H */
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> new file mode 100644
> index 0000000..a5e9d17
> --- /dev/null
> +++ b/net/bluetooth/a2mp.c
> @@ -0,0 +1,91 @@
> +/*
> + 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 <net/bluetooth/bluetooth.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;
> +
> + /* Send 10 packets without ack */
> + chan->tx_win = 10;
> + 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;
> + }
> +}
And now I am confused. So either we use the socket inside the kernel
directly or we make internal calls to L2CAP. However this looks like a
really weird mix between we do whatever we please.
If this is has a long term plan that I am not away, I like to see these
parts properly commented in the code. Otherwise we end up in a disaster
area later on.
Regards
Marcel
On Thu, Dec 22, 2011 at 01:06:07PM +0200, Emeltchenko Andrei wrote:
> From: Andrei Emeltchenko <[email protected]>
>
> Define physical link structure
This is actually RFC not patch.
Best regards
Andrei Emeltchenko
> ---
> include/net/bluetooth/a2mp.h | 10 +++++
> include/net/bluetooth/hci_core.h | 3 ++
> net/bluetooth/a2mp.c | 72 ++++++++++++++++++++++++++++++++++++++
> net/bluetooth/hci_core.c | 1 +
> 4 files changed, 86 insertions(+), 0 deletions(-)
>
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index b1612aa..f4d2f0b 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -122,6 +122,16 @@ struct a2mp_physlink_rsp {
> #define A2MP_STATUS_PHYS_LINK_EXISTS 0x05
> #define A2MP_STATUS_SECURITY_VIOLATION 0x06
>
> +struct phy_link {
> + struct list_head list;
> + __u8 handle;
> + __u16 state;
> + __u8 amp_role;
> + struct hci_dev *hdev;
> + struct amp_assoc rem_assoc;
> + atomic_t refcnt;
> +};
> +
> void amp_mgr_get(struct amp_mgr *mgr);
> int amp_mgr_put(struct amp_mgr *mgr);
>
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index 74e2953..6cd728a 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -135,7 +135,10 @@ struct amp_assoc {
> struct hci_dev {
> struct list_head list;
> struct mutex lock;
> + struct list_head phy_links;
> +
> atomic_t refcnt;
> + rwlock_t phy_link_lock;
>
> char name[8];
> unsigned long flags;
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index ebaec0b..35c3d87 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -605,3 +605,75 @@ void a2mp_exit(void)
> destroy_workqueue(a2mp_workqueue);
> #endif
> }
> +
> +/* Physical Link interface */
> +static inline void phylink_hold(struct phy_link *plink)
> +{
> + atomic_inc(&plink->refcnt);
> +}
> +
> +static inline void phylink_put(struct phy_link *plink)
> +{
> + if (atomic_dec_and_test(&plink->refcnt))
> + kfree(plink);
> +}
> +
> +static struct phy_link *hci_phylink_add(struct hci_dev *hdev, __u8 handle,
> + __u8 amp_role, __u8 *rem_assoc, __u16 assoc_size)
> +
> +{
> + struct phy_link *plink;
> +
> + BT_DBG("handle %d", handle);
> +
> + plink = kzalloc(sizeof(*plink), GFP_KERNEL);
> + if (!plink)
> + return NULL;
> +
> + plink->handle = handle;
> + plink->hdev = hdev;
> + plink->amp_role = amp_role;
> +
> + plink->rem_assoc.len = min_t(__u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
> + memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
> +
> + plink->state = BT_OPEN;
> +
> + write_lock(&hdev->phy_link_lock);
> + list_add(&plink->list, &hdev->phy_links);
> + write_unlock(&hdev->phy_link_lock);
> +
> + atomic_set(&plink->refcnt, 1);
> +
> + return plink;
> +}
> +
> +static void hci_phylink_del(struct hci_dev *hdev, struct phy_link *plink)
> +{
> + BT_DBG("phylink %p", plink);
> +
> + write_lock(&hdev->phy_link_lock);
> + list_del(&plink->list);
> + write_unlock(&hdev->phy_link_lock);
> +
> + phylink_put(plink);
> +}
> +
> +static inline struct phy_link *hci_phylink_lookup_handle(struct hci_dev *hdev,
> + __u8 handle)
> +{
> + struct phy_link *plink;
> +
> + BT_DBG("");
> +
> + read_lock(&hdev->phy_link_lock);
> + list_for_each_entry(plink, &hdev->phy_links, list) {
> + if (plink->handle == handle) {
> + read_unlock(&hdev->phy_link_lock);
> + return plink;
> + }
> + }
> + read_unlock(&hdev->phy_link_lock);
> +
> + return NULL;
> +}
> diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
> index b031c7a..74f8819 100644
> --- a/net/bluetooth/hci_core.c
> +++ b/net/bluetooth/hci_core.c
> @@ -1522,6 +1522,7 @@ int hci_register_dev(struct hci_dev *hdev)
>
> atomic_set(&hdev->refcnt, 1);
> mutex_init(&hdev->lock);
> + rwlock_init(&hdev->phy_link_lock);
>
> hdev->flags = 0;
> hdev->dev_flags = 0;
> --
> 1.7.4.1
>
> --
> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
> the body of a message to [email protected]
> More majordomo info at http://vger.kernel.org/majordomo-info.html
From: Andrei Emeltchenko <[email protected]>
Define physical link structure
---
include/net/bluetooth/a2mp.h | 10 +++++
include/net/bluetooth/hci_core.h | 3 ++
net/bluetooth/a2mp.c | 72 ++++++++++++++++++++++++++++++++++++++
net/bluetooth/hci_core.c | 1 +
4 files changed, 86 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index b1612aa..f4d2f0b 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,6 +122,16 @@ struct a2mp_physlink_rsp {
#define A2MP_STATUS_PHYS_LINK_EXISTS 0x05
#define A2MP_STATUS_SECURITY_VIOLATION 0x06
+struct phy_link {
+ struct list_head list;
+ __u8 handle;
+ __u16 state;
+ __u8 amp_role;
+ struct hci_dev *hdev;
+ struct amp_assoc rem_assoc;
+ atomic_t refcnt;
+};
+
void amp_mgr_get(struct amp_mgr *mgr);
int amp_mgr_put(struct amp_mgr *mgr);
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 74e2953..6cd728a 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -135,7 +135,10 @@ struct amp_assoc {
struct hci_dev {
struct list_head list;
struct mutex lock;
+ struct list_head phy_links;
+
atomic_t refcnt;
+ rwlock_t phy_link_lock;
char name[8];
unsigned long flags;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ebaec0b..35c3d87 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -605,3 +605,75 @@ void a2mp_exit(void)
destroy_workqueue(a2mp_workqueue);
#endif
}
+
+/* Physical Link interface */
+static inline void phylink_hold(struct phy_link *plink)
+{
+ atomic_inc(&plink->refcnt);
+}
+
+static inline void phylink_put(struct phy_link *plink)
+{
+ if (atomic_dec_and_test(&plink->refcnt))
+ kfree(plink);
+}
+
+static struct phy_link *hci_phylink_add(struct hci_dev *hdev, __u8 handle,
+ __u8 amp_role, __u8 *rem_assoc, __u16 assoc_size)
+
+{
+ struct phy_link *plink;
+
+ BT_DBG("handle %d", handle);
+
+ plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+ if (!plink)
+ return NULL;
+
+ plink->handle = handle;
+ plink->hdev = hdev;
+ plink->amp_role = amp_role;
+
+ plink->rem_assoc.len = min_t(__u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+ memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+ plink->state = BT_OPEN;
+
+ write_lock(&hdev->phy_link_lock);
+ list_add(&plink->list, &hdev->phy_links);
+ write_unlock(&hdev->phy_link_lock);
+
+ atomic_set(&plink->refcnt, 1);
+
+ return plink;
+}
+
+static void hci_phylink_del(struct hci_dev *hdev, struct phy_link *plink)
+{
+ BT_DBG("phylink %p", plink);
+
+ write_lock(&hdev->phy_link_lock);
+ list_del(&plink->list);
+ write_unlock(&hdev->phy_link_lock);
+
+ phylink_put(plink);
+}
+
+static inline struct phy_link *hci_phylink_lookup_handle(struct hci_dev *hdev,
+ __u8 handle)
+{
+ struct phy_link *plink;
+
+ BT_DBG("");
+
+ read_lock(&hdev->phy_link_lock);
+ list_for_each_entry(plink, &hdev->phy_links, list) {
+ if (plink->handle == handle) {
+ read_unlock(&hdev->phy_link_lock);
+ return plink;
+ }
+ }
+ read_unlock(&hdev->phy_link_lock);
+
+ return NULL;
+}
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index b031c7a..74f8819 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1522,6 +1522,7 @@ int hci_register_dev(struct hci_dev *hdev)
atomic_set(&hdev->refcnt, 1);
mutex_init(&hdev->lock);
+ rwlock_init(&hdev->phy_link_lock);
hdev->flags = 0;
hdev->dev_flags = 0;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
A2MP status codes copied from Bluez patch sent by Peter Krystad
<[email protected]>.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 10 ++++++++++
1 files changed, 10 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 461ff88..2b6fa0f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -105,6 +105,16 @@ struct a2mp_physlink_rsp {
__u8 status;
} __packed;
+/* A2MP response status */
+#define A2MP_STATUS_SUCCESS 0x00
+#define A2MP_STATUS_INVALID_CTRL_ID 0x01
+#define A2MP_STATUS_UNABLE_START_LINK_CREATION 0x02
+#define A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS 0x02
+#define A2MP_STATUS_COLLISION_OCCURED 0x03
+#define A2MP_STATUS_DISCONN_REQ_RECVD 0x04
+#define A2MP_STATUS_PHYS_LINK_EXISTS 0x05
+#define A2MP_STATUS_SECURITY_VIOLATION 0x06
+
void amp_mgr_get(struct amp_mgr *mgr);
int amp_mgr_put(struct amp_mgr *mgr);
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
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 19
A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
Controller list:
id 0, type 0, status 0x01 (Bluetooth only)
...
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 2 +
net/bluetooth/a2mp.c | 49 ++++++++++++++++++++++++++++++++++++++++++
2 files changed, 51 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 2b6fa0f..d204802 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
+
void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
struct amp_mgr {
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c18eb59..ae1a5671 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -117,6 +117,52 @@ 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 (le16_to_cpu(hdr->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(mgr, rsp->cl, num_ctrl);
+
+ a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+ kfree(rsp);
+ return 0;
+}
+
/* Handle A2MP signalling */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -147,6 +193,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]>
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 <[email protected]>
---
net/bluetooth/a2mp.c | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 57 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 44b0887..3c75de9 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,63 @@ static void a2mp_send(struct amp_mgr *mgr, 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) {
+ struct a2mp_cmd_rej rej;
+ rej.reason = cpu_to_le16(0);
+
+ a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
+ &rej);
+ }
+
+ amp_mgr_put(mgr);
+}
+
static void a2mp_data_ready(struct sock *sk, int bytes)
{
struct sk_buff *skb;
--
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 f9526fc..461ff88 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;
+
void amp_mgr_get(struct amp_mgr *mgr);
int amp_mgr_put(struct amp_mgr *mgr);
--
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.h | 2 ++
include/net/bluetooth/hci_core.h | 14 ++++++++++++++
net/bluetooth/a2mp.c | 34 ++++++++++++++++++++++++++++++++++
3 files changed, 50 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index cd52d21..3d3cc5d 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -56,6 +56,8 @@
#define HCI_BREDR 0x00
#define HCI_AMP 0x01
+#define HCI_BREDR_ID 0
+
/* HCI device quirks */
enum {
HCI_QUIRK_NO_RESET,
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 25c161a..bd72ef7 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -594,6 +594,20 @@ static inline struct hci_dev *__hci_dev_hold(struct hci_dev *d)
#define hci_dev_lock(d) mutex_lock(&d->lock)
#define hci_dev_unlock(d) mutex_unlock(&d->lock)
+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;
+}
+
struct hci_dev *hci_dev_get(int index);
struct hci_dev *hci_get_route(bdaddr_t *src, bdaddr_t *dst);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b1400ad..c18eb59 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -13,6 +13,7 @@
*/
#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/a2mp.h>
@@ -67,6 +68,39 @@ 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;
+}
+
+static void __a2mp_add_cl(struct amp_mgr *mgr, 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) {
+ /* Iterate through AMP controllers */
+ if (hdev->id == HCI_BREDR_ID)
+ continue;
+
+ /* Starting from second entry */
+ 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 | 91 +++++++++++++++++++++++++++++++++++++++++
net/bluetooth/l2cap_core.c | 2 +-
3 files changed, 96 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 f141fbe..b6ed4e5 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -51,6 +51,8 @@
#define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
#define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */
+#define L2CAP_A2MP_DEFAULT_MTU 670
+
/* L2CAP socket address */
struct sockaddr_l2 {
sa_family_t l2_family;
@@ -224,6 +226,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
@@ -827,5 +830,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
u32 priority);
void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
int l2cap_chan_check_security(struct l2cap_chan *chan);
+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..a5e9d17
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,91 @@
+/*
+ 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 <net/bluetooth/bluetooth.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;
+
+ /* Send 10 packets without ack */
+ chan->tx_win = 10;
+ 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 a898285..2d42382 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -2001,7 +2001,7 @@ static void l2cap_ack_timeout(struct work_struct *work)
release_sock(chan->sk);
}
-static inline void l2cap_ertm_init(struct l2cap_chan *chan)
+void l2cap_ertm_init(struct l2cap_chan *chan)
{
chan->expected_ack_seq = 0;
chan->unacked_frames = 0;
--
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 e336f20..195af57 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -258,6 +258,40 @@ 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;
+
+ if (le16_to_cpu(hdr->len) < sizeof(*req))
+ return -EINVAL;
+
+ rsp.local_id = req->remote_id;
+ rsp.remote_id = req->local_id;
+ rsp.status = A2MP_STATUS_SUCCESS;
+
+ BT_DBG("local_id %d remote_id %d", rsp.local_id, rsp.remote_id);
+
+ hdev = hci_dev_get(req->local_id);
+ if (!hdev) {
+ rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+ goto send_rsp;
+ }
+
+ /* TODO Disconnect Phys Link here */
+
+send_rsp:
+ a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &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)
{
@@ -308,6 +342,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]>
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 3c75de9..b1400ad 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,22 @@ static void a2mp_send(struct amp_mgr *mgr, 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 (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 */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -93,6 +109,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]>
---
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 a6891b0..e336f20 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -242,6 +242,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)
{
@@ -288,6 +304,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]>
Handle BT_CLOSED state change by removing AMP manager.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
net/bluetooth/a2mp.c | 16 +++++++++++++++-
1 files changed, 15 insertions(+), 1 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3f69823..b197cf3 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -82,7 +82,21 @@ static void a2mp_data_ready(struct sock *sk, int bytes)
static void a2mp_state_change(struct sock *sk)
{
- BT_DBG("sk %p", sk);
+ struct amp_mgr *mgr = NULL;
+
+ mgr = get_amp_mgr_sk(sk);
+ if (!mgr)
+ return;
+
+ BT_DBG("sk %p mgr %p state %d", sk, mgr, sk->sk_state);
+
+ switch (sk->sk_state) {
+ case BT_CLOSED:
+ amp_mgr_put(mgr);
+ break;
+ }
+
+ amp_mgr_put(mgr);
}
static void l2cap_fixed_channel_config(struct sock *sk)
--
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 ae1a5671..f7aae8b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -163,6 +163,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)
{
@@ -197,6 +213,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]>
Handle incoming A2MP connection by creating AMP manager and
processing A2MP messages.
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 2 ++
include/net/bluetooth/l2cap.h | 1 +
net/bluetooth/a2mp.c | 13 +++++++++++++
net/bluetooth/l2cap_core.c | 13 ++++++++-----
4 files changed, 24 insertions(+), 5 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index d204802..1ceda7c 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,8 @@
#ifndef __A2MP_H
#define __A2MP_H
+#include <net/bluetooth/l2cap.h>
+
#define A2MP_FEAT_EXT 0x8000
void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index c5dabf0..8bc6fb4 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -832,5 +832,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
int l2cap_chan_check_security(struct l2cap_chan *chan);
void l2cap_ertm_init(struct l2cap_chan *chan);
+int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
#endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index da3873b..63e6aaf 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -568,6 +568,19 @@ finished:
return mgr;
}
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+ struct amp_mgr *mgr;
+ struct sock *sk;
+
+ mgr = amp_mgr_create(conn);
+ sk = mgr->a2mp_sock->sk;
+
+ BT_DBG("mgr %p sk %p", mgr, sk);
+
+ l2cap_ertm_data_rcv(sk, skb);
+}
+
int a2mp_init(void)
{
#if 0
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 6fbe7ba..ba0f132 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -74,8 +74,6 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
static void l2cap_send_disconn_req(struct l2cap_conn *conn,
struct l2cap_chan *chan, int err);
-static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
-
/* ---- L2CAP channels ---- */
static inline void chan_hold(struct l2cap_chan *c)
@@ -4167,7 +4165,7 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_cont
return 0;
}
-static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
+int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
{
struct l2cap_chan *chan = l2cap_pi(sk)->chan;
u32 control;
@@ -4244,8 +4242,13 @@ 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) {
+ 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]>
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 <[email protected]>
---
net/bluetooth/a2mp.c | 32 ++++++++++++++++++++++++++++++++
1 files changed, 32 insertions(+), 0 deletions(-)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 666d8e5..a6891b0 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -213,6 +213,35 @@ 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;
+ 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);
+ }
+
+ 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)
{
@@ -255,6 +284,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]>
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
total bandwidth -381650496
max guaranteed bandwidth -274362188
min latency -158843144
pal capabilities 0x9750
assoc size 49478
...
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 <[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 f7aae8b..666d8e5 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -12,6 +12,8 @@
GNU General Public License for more details.
*/
+#include <linux/module.h>
+
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
@@ -179,6 +181,38 @@ 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 = 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 */
void a2mp_receive(struct sock *sk, struct sk_buff *skb)
{
@@ -217,6 +251,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 | 67 +++++++++++++++++++++++++++-------------
3 files changed, 48 insertions(+), 22 deletions(-)
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index b6ed4e5..c5dabf0 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -596,6 +596,7 @@ enum {
FLAG_FLUSHABLE,
FLAG_EXT_CTRL,
FLAG_EFS_ENABLE,
+ FLAG_FIXED_CHANNEL,
};
static inline void l2cap_set_timer(struct l2cap_chan *chan,
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 64a0034..da3873b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -420,6 +420,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 0b47063..6fbe7ba 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -334,6 +334,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 */
@@ -357,6 +361,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;
@@ -389,7 +395,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);
@@ -405,7 +412,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);
@@ -1140,10 +1148,9 @@ inline int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdad
struct hci_conn *hcon;
struct hci_dev *hdev;
__u8 auth_type;
- int err;
+ int err = 0;
- BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
- chan->psm);
+ BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), chan->psm);
hdev = hci_get_route(dst, src);
if (!hdev)
@@ -1208,23 +1215,34 @@ inline int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdad
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 */
@@ -1232,6 +1250,13 @@ inline int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdad
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);
+ goto done;
+ }
+
l2cap_state_change(chan, BT_CONNECT);
__set_chan_timer(chan, sk->sk_sndtimeo);
@@ -1244,8 +1269,6 @@ inline int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdad
l2cap_do_start(chan);
}
- err = 0;
-
done:
hci_dev_unlock(hdev);
hci_dev_put(hdev);
--
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 195af57..64a0034 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -292,6 +292,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)
{
@@ -351,6 +360,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]>
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 ee42c86..6c160d4 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -26,6 +26,13 @@ struct amp_mgr {
unsigned long flags;
};
+struct a2mp_cmd {
+ __u8 code;
+ __u8 ident;
+ __le16 len;
+ __u8 data[0];
+} __packed;
+
void amp_mgr_get(struct amp_mgr *mgr);
int amp_mgr_put(struct amp_mgr *mgr);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index aa1fc95..3f69823 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -21,6 +21,52 @@ static struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
LIST_HEAD(amp_mgr_list);
DEFINE_MUTEX(amp_mgr_list_lock);
+/* 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 amp_mgr *mgr, u8 code, u8 ident, u16 len,
+ void *data)
+{
+ struct a2mp_cmd *cmd;
+
+ if (!mgr->a2mp_sock)
+ return;
+
+ cmd = __a2mp_build(code, ident, len, data);
+ if (!cmd)
+ return;
+
+ __a2mp_send(mgr->a2mp_sock, (u8 *)cmd, len + sizeof(*cmd));
+ kfree(cmd);
+}
+
static void a2mp_data_ready(struct sock *sk, int bytes)
{
struct sk_buff *skb;
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Handle incoming A2MP connection by creating AMP manager and
processing A2MP messages.
---
include/net/bluetooth/a2mp.h | 5 +++++
include/net/bluetooth/l2cap.h | 1 +
net/bluetooth/a2mp.c | 13 +++++++++++++
net/bluetooth/l2cap_core.c | 13 ++++++++-----
4 files changed, 27 insertions(+), 5 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index d204802..903a3e7 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,8 +15,13 @@
#ifndef __A2MP_H
#define __A2MP_H
+#include <net/bluetooth/l2cap.h>
+
#define A2MP_FEAT_EXT 0x8000
+
+int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
+
void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
struct amp_mgr {
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index c5dabf0..8bc6fb4 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -832,5 +832,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
int l2cap_chan_check_security(struct l2cap_chan *chan);
void l2cap_ertm_init(struct l2cap_chan *chan);
+int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
#endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index da3873b..63e6aaf 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -568,6 +568,19 @@ finished:
return mgr;
}
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+ struct amp_mgr *mgr;
+ struct sock *sk;
+
+ mgr = amp_mgr_create(conn);
+ sk = mgr->a2mp_sock->sk;
+
+ BT_DBG("mgr %p sk %p", mgr, sk);
+
+ l2cap_ertm_data_rcv(sk, skb);
+}
+
int a2mp_init(void)
{
#if 0
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 6fbe7ba..ba0f132 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -74,8 +74,6 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
static void l2cap_send_disconn_req(struct l2cap_conn *conn,
struct l2cap_chan *chan, int err);
-static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
-
/* ---- L2CAP channels ---- */
static inline void chan_hold(struct l2cap_chan *c)
@@ -4167,7 +4165,7 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_cont
return 0;
}
-static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
+int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
{
struct l2cap_chan *chan = l2cap_pi(sk)->chan;
u32 control;
@@ -4244,8 +4242,13 @@ 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) {
+ 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]>
---
include/net/bluetooth/a2mp.h | 3 +++
net/bluetooth/a2mp.c | 18 ++++++++++++++++++
net/bluetooth/l2cap_core.c | 8 ++++++++
3 files changed, 29 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6c160d4..f9526fc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -36,4 +36,7 @@ struct a2mp_cmd {
void amp_mgr_get(struct amp_mgr *mgr);
int amp_mgr_put(struct amp_mgr *mgr);
+int a2mp_init(void);
+void a2mp_exit(void);
+
#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b197cf3..44b0887 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -250,3 +250,21 @@ struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
finished:
return mgr;
}
+
+int a2mp_init(void)
+{
+#if 0
+ a2mp_workqueue = create_singlethread_workqueue("a2mp");
+ if (!a2mp_workqueue)
+ return -EPERM;
+#endif
+ return 0;
+}
+
+void a2mp_exit(void)
+{
+#if 0
+ flush_workqueue(a2mp_workqueue);
+ destroy_workqueue(a2mp_workqueue);
+#endif
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 2d42382..0b47063 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -56,6 +56,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;
@@ -4787,6 +4788,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:
@@ -4796,6 +4803,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 | 12 ++++++
net/bluetooth/a2mp.c | 79 ++++++++++++++++++++++++++++++++++++++++++
2 files changed, 91 insertions(+), 0 deletions(-)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 97a0752..ee42c86 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -17,4 +17,16 @@
void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
+struct amp_mgr {
+ struct list_head list;
+ struct l2cap_conn *l2cap_conn;
+ struct socket *a2mp_sock;
+ struct kref kref;
+ __u8 ident;
+ unsigned long flags;
+};
+
+void amp_mgr_get(struct amp_mgr *mgr);
+int amp_mgr_put(struct amp_mgr *mgr);
+
#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 87ff996..aa1fc95 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,11 @@
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/a2mp.h>
+static struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
+
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
static void a2mp_data_ready(struct sock *sk, int bytes)
{
struct sk_buff *skb;
@@ -111,3 +116,77 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
return NULL;
}
}
+
+/* AMP Manager functions */
+void amp_mgr_get(struct amp_mgr *mgr)
+{
+ kref_get(&mgr->kref);
+}
+
+static void amp_mgr_destroy(struct kref *kref)
+{
+ struct amp_mgr *mgr;
+ mgr = container_of(kref, struct amp_mgr, kref);
+
+ BT_DBG("Destroy amp_mgr %p", mgr);
+
+ mutex_lock(&_mgr_list_lock);
+ list_del(&mgr->list);
+ mutex_unlock(&_mgr_list_lock);
+
+ sock_release(mgr->a2mp_sock);
+
+ kfree(mgr);
+}
+
+int amp_mgr_put(struct amp_mgr *mgr)
+{
+ return kref_put(&mgr->kref, &_mgr_destroy);
+}
+
+static struct amp_mgr *get_amp_mgr_sk(struct sock *sk)
+{
+ struct amp_mgr *mgr;
+ struct amp_mgr *found = NULL;
+
+ mutex_lock(&_mgr_list_lock);
+ list_for_each_entry(mgr, &_mgr_list, list) {
+ if ((mgr->a2mp_sock) && (mgr->a2mp_sock->sk == sk)) {
+ found = mgr;
+ amp_mgr_get(mgr);
+ break;
+ }
+ }
+ mutex_unlock(&_mgr_list_lock);
+
+ return found;
+}
+
+struct amp_mgr *amp_mgr_create(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;
+ }
+
+ mutex_lock(&_mgr_list_lock);
+ list_add(&(mgr->list), &_mgr_list);
+ mutex_unlock(&_mgr_list_lock);
+
+ kref_init(&mgr->kref);
+
+finished:
+ return mgr;
+}
--
1.7.4.1
From: Andrei Emeltchenko <[email protected]>
Adds sk_data_ready and sk_state_change for A2MP socket
Signed-off-by: Andrei Emeltchenko <[email protected]>
---
include/net/bluetooth/a2mp.h | 20 ++++++++++++++++++++
net/bluetooth/a2mp.c | 22 ++++++++++++++++++++++
2 files changed, 42 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..97a0752
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,20 @@
+/*
+ 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
+
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
+
+#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a5e9d17..87ff996 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -14,6 +14,25 @@
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+
+static void a2mp_data_ready(struct sock *sk, int bytes)
+{
+ 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);
+ }
+}
+
+static void a2mp_state_change(struct sock *sk)
+{
+ BT_DBG("sk %p", sk);
+}
static void l2cap_fixed_channel_config(struct sock *sk)
{
@@ -62,6 +81,9 @@ 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