[RFCv3 00/34] RFC Bluetooth A2MP implementation

[RFCv3 00/34] RFC Bluetooth A2MP implementation

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Changes:
	* RFCv3 redesign code to use l2cap functions instead of kernel sockets
	L2CAP functions modified to work with channels without sk.
	* RFCv2 rebased against "workqueue" patches.
	* RFCv1 added refcnt to amp_mgr, fixed sleeping in atomic

Some code based of reference implementations below.

References: 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).

Andrei Emeltchenko (34):
  Bluetooth: trivial: space correction
  Bluetooth: trivial: clean up of l2cap_chan_connect
  Bluetooth: Make ertm_init available
  Bluetooth: Make l2cap_ertm_data_rcv available
  Bluetooth: Allocate skb depending on sk
  Bluetooth: Check sk before assigning sk_err
  Bluetooth: Add send function to chan ops
  Bluetooth: Make l2cap_chan_add available
  Bluetooth: A2MP: Create A2MP channel
  Bluetooth: A2MP: AMP Manager basic functions
  Bluetooth: A2MP: Add channel close callback
  Bluetooth: Add state_change for A2MP channel
  Bluetooth: A2MP: Build and Send msg helpers
  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: Clean up l2cap_chan_add
  Bluetooth: A2MP: Handling fixed channels
  Bluetooth: A2MP: Manage incoming connections
  Bluetooth: physical link HCI interface to AMP
  Bluetooth: Define AMP controller statuses
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Read Local Assoc support

 include/net/bluetooth/a2mp.h     |  127 +++++++++
 include/net/bluetooth/hci.h      |   50 ++++
 include/net/bluetooth/hci_core.h |   41 +++
 include/net/bluetooth/l2cap.h    |   10 +
 net/bluetooth/Makefile           |    4 +-
 net/bluetooth/a2mp.c             |  527 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/amp.c              |   91 +++++++
 net/bluetooth/hci_core.c         |  144 +++++++++++
 net/bluetooth/hci_event.c        |   58 ++++-
 net/bluetooth/l2cap_core.c       |   99 ++++++--
 10 files changed, 1121 insertions(+), 30 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h
 create mode 100644 net/bluetooth/a2mp.c
 create mode 100644 net/bluetooth/amp.c

-- 
1.7.4.1

[RFCv3 01/34] Bluetooth: trivial: space correction

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |    2 +-
 1 files changed, 1 insertions(+), 1 deletions(-)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index e42999c..48b5c0d 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -4720,7 +4720,7 @@ static int l2cap_debugfs_show(struct seq_file *f, void *p)
 					c->state, __le16_to_cpu(c->psm),
 					c->scid, c->dcid, c->imtu, c->omtu,
 					c->sec_level, c->mode);
-}
+	}
 
 	read_unlock(&chan_list_lock);
 
-- 
1.7.4.1

Re: [RFCv3 01/34] Bluetooth: trivial: space correction

[Marcel Holtmann]

Hi Andrei,

> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/l2cap_core.c |    2 +-
>  1 files changed, 1 insertions(+), 1 deletions(-)

Acked-by: Marcel Holtmann <marcel@holtmann.org>

Regards

Marcel

Re: [RFCv3 01/34] Bluetooth: trivial: space correction

[Johan Hedberg]

Hi,

On Thu, Jan 19, 2012, Marcel Holtmann wrote:
> Hi Andrei,
> 
> > Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> > ---
> >  net/bluetooth/l2cap_core.c |    2 +-
> >  1 files changed, 1 insertions(+), 1 deletions(-)
> 
> Acked-by: Marcel Holtmann <marcel@holtmann.org>

Applied to my bluetooth-next tree.

Johan

[RFCv3 02/34] Bluetooth: trivial: clean up of l2cap_chan_connect

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |    7 ++-----
 1 files changed, 2 insertions(+), 5 deletions(-)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 48b5c0d..5ace149 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1128,10 +1128,9 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
 	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)
@@ -1232,8 +1231,6 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
 			l2cap_do_start(chan);
 	}
 
-	err = 0;
-
 done:
 	hci_dev_unlock(hdev);
 	hci_dev_put(hdev);
-- 
1.7.4.1

Re: [RFCv3 02/34] Bluetooth: trivial: clean up of l2cap_chan_connect

[Marcel Holtmann]

Hi Andrei,

> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/l2cap_core.c |    7 ++-----
>  1 files changed, 2 insertions(+), 5 deletions(-)
> 
> diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> index 48b5c0d..5ace149 100644
> --- a/net/bluetooth/l2cap_core.c
> +++ b/net/bluetooth/l2cap_core.c
> @@ -1128,10 +1128,9 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
>  	struct hci_conn *hcon;
>  	struct hci_dev *hdev;
>  	__u8 auth_type;
> -	int err;
> +	int err = 0;

NAK here. I do not consider this a cleanup. This might just hide some
errors along the way in the future. I rather have the compile tell if it
thinks that variable is used unassigned.

Regards

Marcel

[RFCv3 03/34] Bluetooth: Make ertm_init available

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function l2cap_ertm_init will be used for initialization
fixed A2MP channel.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/l2cap_core.c    |    2 +-
 2 files changed, 2 insertions(+), 1 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 18242ea..60092cb 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -845,5 +845,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/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 5ace149..e36aaab 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1998,7 +1998,7 @@ static void l2cap_ack_timeout(struct work_struct *work)
 	l2cap_chan_put(chan);
 }
 
-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

[RFCv3 04/34] Bluetooth: Make l2cap_ertm_data_rcv available

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function l2cap_ertm_data_rcv will be used by A2MP

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/l2cap_core.c    |    2 --
 2 files changed, 1 insertions(+), 2 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 60092cb..34a882b 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -846,5 +846,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 l2cap_chan *chan, struct sk_buff *skb);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index e36aaab..598805b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -73,8 +73,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 l2cap_chan *chan, struct sk_buff *skb);
-
 /* ---- L2CAP channels ---- */
 
 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
-- 
1.7.4.1

[RFCv3 05/34] Bluetooth: Allocate skb depending on sk

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Some channels might not have sk so we allocate skbuff directly.
The idea is to use l2cap_chan_send for sending packets not through
sockets.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |   34 +++++++++++++++++++++++++++-------
 1 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 598805b..bc42ee8 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1531,7 +1531,12 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, struct msghdr
 	while (len) {
 		count = min_t(unsigned int, conn->mtu, len);
 
-		*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
+		if (sk)
+			*frag = bt_skb_send_alloc(sk, count,
+					msg->msg_flags & MSG_DONTWAIT, &err);
+		else
+			*frag = bt_skb_alloc(count, GFP_KERNEL);
+
 		if (!*frag)
 			return err;
 		if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
@@ -1561,8 +1566,13 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
 	BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
 
 	count = min_t(unsigned int, (conn->mtu - hlen), len);
-	skb = bt_skb_send_alloc(sk, count + hlen,
-			msg->msg_flags & MSG_DONTWAIT, &err);
+
+	if (sk)
+		skb = bt_skb_send_alloc(sk, count + hlen,
+					msg->msg_flags & MSG_DONTWAIT, &err);
+	else
+		skb = bt_skb_alloc(count, GFP_KERNEL);
+
 	if (!skb)
 		return ERR_PTR(err);
 
@@ -1595,8 +1605,13 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
 	BT_DBG("sk %p len %d", sk, (int)len);
 
 	count = min_t(unsigned int, (conn->mtu - hlen), len);
-	skb = bt_skb_send_alloc(sk, count + hlen,
-			msg->msg_flags & MSG_DONTWAIT, &err);
+
+	if (sk)
+		skb = bt_skb_send_alloc(sk, count + hlen,
+					msg->msg_flags & MSG_DONTWAIT, &err);
+	else
+		skb = bt_skb_alloc(count, GFP_KERNEL);
+
 	if (!skb)
 		return ERR_PTR(err);
 
@@ -1642,8 +1657,13 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
 		hlen += L2CAP_FCS_SIZE;
 
 	count = min_t(unsigned int, (conn->mtu - hlen), len);
-	skb = bt_skb_send_alloc(sk, count + hlen,
-			msg->msg_flags & MSG_DONTWAIT, &err);
+
+	if (sk)
+		skb = bt_skb_send_alloc(sk, count + hlen,
+					msg->msg_flags & MSG_DONTWAIT, &err);
+	else
+		skb = bt_skb_alloc(count, GFP_KERNEL);
+
 	if (!skb)
 		return ERR_PTR(err);
 
-- 
1.7.4.1

Re: [RFCv3 05/34] Bluetooth: Allocate skb depending on sk

[Marcel Holtmann]

Hi Andrei,

> Some channels might not have sk so we allocate skbuff directly.
> The idea is to use l2cap_chan_send for sending packets not through
> sockets.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/l2cap_core.c |   34 +++++++++++++++++++++++++++-------
>  1 files changed, 27 insertions(+), 7 deletions(-)
> 
> diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> index 598805b..bc42ee8 100644
> --- a/net/bluetooth/l2cap_core.c
> +++ b/net/bluetooth/l2cap_core.c
> @@ -1531,7 +1531,12 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, struct msghdr
>  	while (len) {
>  		count = min_t(unsigned int, conn->mtu, len);
>  
> -		*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
> +		if (sk)
> +			*frag = bt_skb_send_alloc(sk, count,
> +					msg->msg_flags & MSG_DONTWAIT, &err);
> +		else
> +			*frag = bt_skb_alloc(count, GFP_KERNEL);
> +
>  		if (!*frag)
>  			return err;
>  		if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
> @@ -1561,8 +1566,13 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
>  	BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
>  
>  	count = min_t(unsigned int, (conn->mtu - hlen), len);
> -	skb = bt_skb_send_alloc(sk, count + hlen,
> -			msg->msg_flags & MSG_DONTWAIT, &err);
> +
> +	if (sk)
> +		skb = bt_skb_send_alloc(sk, count + hlen,
> +					msg->msg_flags & MSG_DONTWAIT, &err);
> +	else
> +		skb = bt_skb_alloc(count, GFP_KERNEL);
> +
>  	if (!skb)
>  		return ERR_PTR(err);
>  
> @@ -1595,8 +1605,13 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
>  	BT_DBG("sk %p len %d", sk, (int)len);
>  
>  	count = min_t(unsigned int, (conn->mtu - hlen), len);
> -	skb = bt_skb_send_alloc(sk, count + hlen,
> -			msg->msg_flags & MSG_DONTWAIT, &err);
> +
> +	if (sk)
> +		skb = bt_skb_send_alloc(sk, count + hlen,
> +					msg->msg_flags & MSG_DONTWAIT, &err);
> +	else
> +		skb = bt_skb_alloc(count, GFP_KERNEL);
> +
>  	if (!skb)
>  		return ERR_PTR(err);
>  
> @@ -1642,8 +1657,13 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
>  		hlen += L2CAP_FCS_SIZE;
>  
>  	count = min_t(unsigned int, (conn->mtu - hlen), len);
> -	skb = bt_skb_send_alloc(sk, count + hlen,
> -			msg->msg_flags & MSG_DONTWAIT, &err);
> +
> +	if (sk)
> +		skb = bt_skb_send_alloc(sk, count + hlen,
> +					msg->msg_flags & MSG_DONTWAIT, &err);
> +	else
> +		skb = bt_skb_alloc(count, GFP_KERNEL);
> +
>  	if (!skb)
>  		return ERR_PTR(err);
>  

so I am not sure that I wanna change all these functions this way. It
might be a good way of handling this, but I am right now convinced.

Care to explain first on how you are using them.

Regards

Marcel

Re: [RFCv3 05/34] Bluetooth: Allocate skb depending on sk

[Emeltchenko Andrei]

Hi Marcel,

On Thu, Jan 19, 2012 at 10:56:06AM +0100, Marcel Holtmann wrote:
> > Some channels might not have sk so we allocate skbuff directly.
> > The idea is to use l2cap_chan_send for sending packets not through
> > sockets.
> > 
> > Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> > ---
> >  net/bluetooth/l2cap_core.c |   34 +++++++++++++++++++++++++++-------
> >  1 files changed, 27 insertions(+), 7 deletions(-)
> > 
> > diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> > index 598805b..bc42ee8 100644
> > --- a/net/bluetooth/l2cap_core.c
> > +++ b/net/bluetooth/l2cap_core.c
> > @@ -1531,7 +1531,12 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, struct msghdr
> >  	while (len) {
> >  		count = min_t(unsigned int, conn->mtu, len);
> >  
> > -		*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
> > +		if (sk)
> > +			*frag = bt_skb_send_alloc(sk, count,
> > +					msg->msg_flags & MSG_DONTWAIT, &err);
> > +		else
> > +			*frag = bt_skb_alloc(count, GFP_KERNEL);
> > +
> >  		if (!*frag)
> >  			return err;
> >  		if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
> > @@ -1561,8 +1566,13 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
> >  	BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
> >  
> >  	count = min_t(unsigned int, (conn->mtu - hlen), len);
> > -	skb = bt_skb_send_alloc(sk, count + hlen,
> > -			msg->msg_flags & MSG_DONTWAIT, &err);
> > +
> > +	if (sk)
> > +		skb = bt_skb_send_alloc(sk, count + hlen,
> > +					msg->msg_flags & MSG_DONTWAIT, &err);
> > +	else
> > +		skb = bt_skb_alloc(count, GFP_KERNEL);
> > +
> >  	if (!skb)
> >  		return ERR_PTR(err);
> >  
> > @@ -1595,8 +1605,13 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
> >  	BT_DBG("sk %p len %d", sk, (int)len);
> >  
> >  	count = min_t(unsigned int, (conn->mtu - hlen), len);
> > -	skb = bt_skb_send_alloc(sk, count + hlen,
> > -			msg->msg_flags & MSG_DONTWAIT, &err);
> > +
> > +	if (sk)
> > +		skb = bt_skb_send_alloc(sk, count + hlen,
> > +					msg->msg_flags & MSG_DONTWAIT, &err);
> > +	else
> > +		skb = bt_skb_alloc(count, GFP_KERNEL);
> > +
> >  	if (!skb)
> >  		return ERR_PTR(err);
> >  
> > @@ -1642,8 +1657,13 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
> >  		hlen += L2CAP_FCS_SIZE;
> >  
> >  	count = min_t(unsigned int, (conn->mtu - hlen), len);
> > -	skb = bt_skb_send_alloc(sk, count + hlen,
> > -			msg->msg_flags & MSG_DONTWAIT, &err);
> > +
> > +	if (sk)
> > +		skb = bt_skb_send_alloc(sk, count + hlen,
> > +					msg->msg_flags & MSG_DONTWAIT, &err);
> > +	else
> > +		skb = bt_skb_alloc(count, GFP_KERNEL);
> > +
> >  	if (!skb)
> >  		return ERR_PTR(err);
> >  
> 
> so I am not sure that I wanna change all these functions this way. It
> might be a good way of handling this, but I am right now convinced.

I have sent new patch concerning skb allocation.

> Care to explain first on how you are using them.

I use the same function to send packet which is used by socket operations
but I do not have sk. I can create my own function but I would need to
duplicate ERTM code.

Each channel shall have own allocation function now, old allocation
function did not change.

But sometimes we still need to check sk or duplicate similar
functionality.

Best regards 
Andrei Emeltchenko 

[RFCv3 06/34] Bluetooth: Check sk before assigning sk_err

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |    4 +++-
 1 files changed, 3 insertions(+), 1 deletions(-)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index bc42ee8..643bab8 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -732,7 +732,9 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
 			L2CAP_DISCONN_REQ, sizeof(req), &req);
 
 	l2cap_state_change(chan, BT_DISCONN);
-	sk->sk_err = err;
+
+	if (sk)
+		sk->sk_err = err;
 }
 
 /* ---- L2CAP connections ---- */
-- 
1.7.4.1

[RFCv3 07/34] Bluetooth: Add send function to chan ops

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    2 ++
 1 files changed, 2 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 34a882b..3c7406f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -504,6 +504,8 @@ struct l2cap_ops {
 
 	struct l2cap_chan	*(*new_connection) (void *data);
 	int			(*recv) (void *data, struct sk_buff *skb);
+	int			(*send) (struct l2cap_chan *chan,
+				struct msghdr *msg, size_t len, u32 priority);
 	void			(*close) (void *data);
 	void			(*state_change) (void *data, int state);
 };
-- 
1.7.4.1

[RFCv3 08/34] Bluetooth: Make l2cap_chan_add available

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function will be used when creating fixed channels

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    2 ++
 net/bluetooth/l2cap_core.c    |    2 +-
 2 files changed, 3 insertions(+), 1 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 3c7406f..33acd7d 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -850,4 +850,6 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_ertm_init(struct l2cap_chan *chan);
 int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb);
 
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 643bab8..0ac81cc 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -311,7 +311,7 @@ void l2cap_chan_destroy(struct l2cap_chan *chan)
 	l2cap_chan_put(chan);
 }
 
-static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 			chan->psm, chan->dcid);
-- 
1.7.4.1

[RFCv3 09/34] Bluetooth: A2MP: Create A2MP channel

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create fixed A2MP channel

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    3 ++
 net/bluetooth/a2mp.c          |   63 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+), 0 deletions(-)
 create mode 100644 net/bluetooth/a2mp.c

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 33acd7d..cc6f299 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
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 0000000..52bb7d8
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,63 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/l2cap.h>
+
+static struct l2cap_ops a2mp_chan_ops = {
+	.name		= "L2CAP A2MP channel",
+};
+
+static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
+{
+	struct l2cap_chan *chan;
+
+	chan = l2cap_chan_create(NULL);
+
+	hci_conn_hold(conn->hcon);
+
+	BT_DBG("chan %p", chan);
+
+	chan->sec_level = BT_SECURITY_LOW;
+	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->ops = &a2mp_chan_ops;
+
+	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+
+	chan->max_tx = 0xFF;
+	chan->remote_max_tx = chan->max_tx;
+
+	/* Send 10 packets without ack */
+	chan->tx_win = 10;
+	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
+	chan->remote_tx_win = chan->tx_win;
+
+	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
+	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
+
+	skb_queue_head_init(&chan->tx_q);
+
+	chan->mode = L2CAP_MODE_ERTM;
+	l2cap_ertm_init(chan);
+
+	l2cap_chan_add(conn, chan);
+	chan->remote_mps = chan->omtu;
+	chan->mps = chan->omtu;
+
+	return chan;
+}
-- 
1.7.4.1

[RFCv3 10/34] Bluetooth: A2MP: AMP Manager basic functions

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define AMP Manager and some basic functions.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |   30 +++++++++++++++++++++++++
 net/bluetooth/a2mp.c         |   49 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 79 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..0fe8ddd
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,30 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __A2MP_H
+#define __A2MP_H
+
+struct amp_mgr {
+	struct list_head	list;
+	struct l2cap_conn	*l2cap_conn;
+	struct l2cap_chan	*a2mp_chan;
+	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 52bb7d8..247a543 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -14,6 +14,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name		= "L2CAP A2MP channel",
@@ -61,3 +62,51 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
 
 	return chan;
 }
+
+/* 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("mgr %p", mgr);
+
+	kfree(mgr);
+}
+
+int amp_mgr_put(struct amp_mgr *mgr)
+{
+	return kref_put(&mgr->kref, &amp_mgr_destroy);
+}
+
+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_chan = open_a2mp_chan(conn);
+	if (!mgr->a2mp_chan) {
+		kfree(mgr);
+		mgr = NULL;
+		goto finished;
+	}
+
+	mgr->a2mp_chan->data = mgr;
+
+	kref_init(&mgr->kref);
+
+finished:
+	return mgr;
+}
-- 
1.7.4.1

[RFCv3 11/34] Bluetooth: A2MP: Add channel close callback

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |    8 ++++++++
 1 files changed, 8 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 247a543..3e306dc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,8 +16,16 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+static void a2mp_chan_close_cb(void *data)
+{
+	struct amp_mgr *mgr = data;
+
+	l2cap_chan_destroy(mgr->a2mp_chan);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name		= "L2CAP A2MP channel",
+	.close		= a2mp_chan_close_cb,
 };
 
 static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
-- 
1.7.4.1

[RFCv3 12/34] Bluetooth: Add state_change for A2MP channel

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Remove AMP Manager when A2MP channel closed.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   21 +++++++++++++++++++++
 1 files changed, 21 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3e306dc..6520b65 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,24 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+static void a2mp_chan_state_change_cb(void *data, int state)
+{
+	struct l2cap_chan *chan = data;
+	struct amp_mgr *mgr;
+
+	BT_DBG("chan %p state %d", chan, state);
+
+	chan->state = state;
+
+	switch (state) {
+	case BT_CLOSED:
+		mgr = chan->data;
+		if (mgr)
+			amp_mgr_put(mgr);
+		break;
+	}
+}
+
 static void a2mp_chan_close_cb(void *data)
 {
 	struct amp_mgr *mgr = data;
@@ -26,6 +44,7 @@ static void a2mp_chan_close_cb(void *data)
 static struct l2cap_ops a2mp_chan_ops = {
 	.name		= "L2CAP A2MP channel",
 	.close		= a2mp_chan_close_cb,
+	.state_change	= a2mp_chan_state_change_cb,
 };
 
 static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
@@ -68,6 +87,8 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
 
+	chan->ops->state_change(chan, BT_CONNECTED);
+
 	return chan;
 }
 
-- 
1.7.4.1

[RFCv3 13/34] Bluetooth: A2MP: Build and Send msg helpers

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Helper function to build and send A2MP messages.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    7 ++++++
 net/bluetooth/a2mp.c         |   49 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 56 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 0fe8ddd..995f1c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -24,6 +24,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 6520b65..2755e7e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,54 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* 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 amp_mgr *mgr, u8 *data, int len)
+{
+	struct l2cap_chan *chan = mgr->a2mp_chan;
+	struct kvec iv = { data, len };
+	struct msghdr msg;
+
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_iov = (struct iovec *) &iv;
+	msg.msg_iovlen = 1;
+
+	return chan->ops->send(chan, &msg, len, 0);
+}
+
+static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
+								void *data)
+{
+	struct a2mp_cmd *cmd;
+
+	cmd = __a2mp_build(code, ident, len, data);
+	if (!cmd)
+		return;
+
+	__a2mp_send(mgr, (u8 *)cmd, len + sizeof(*cmd));
+
+	kfree(cmd);
+}
+
 static void a2mp_chan_state_change_cb(void *data, int state)
 {
 	struct l2cap_chan *chan = data;
@@ -44,6 +92,7 @@ static void a2mp_chan_close_cb(void *data)
 static struct l2cap_ops a2mp_chan_ops = {
 	.name		= "L2CAP A2MP channel",
 	.close		= a2mp_chan_close_cb,
+	.send		= l2cap_chan_send,
 	.state_change	= a2mp_chan_state_change_cb,
 };
 
-- 
1.7.4.1

[RFCv3 14/34] Bluetooth: A2MP: Definitions for A2MP commands

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define A2MP command id and packet structures.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 995f1c0..6e6202a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,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

[RFCv3 15/34] Bluetooth: A2MP: Define A2MP status codes

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

A2MP status codes copied from Bluez patch sent by Peter Krystad
<pkrystad@codeaurora.org>.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 6e6202a..c99c375 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -103,6 +103,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

[RFCv3 16/34] Bluetooth: A2MP: Process A2MP messages

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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 <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   58 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 58 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2755e7e..2ca7820 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -64,6 +64,63 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Handle A2MP signalling */
+int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
+{
+	struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+	struct amp_mgr *mgr = data;
+	int err = 0;
+
+	amp_mgr_get(mgr);
+
+	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);
+
+	return err;
+}
+
 static void a2mp_chan_state_change_cb(void *data, int state)
 {
 	struct l2cap_chan *chan = data;
@@ -91,6 +148,7 @@ static void a2mp_chan_close_cb(void *data)
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name		= "L2CAP A2MP channel",
+	.recv		= a2mp_chan_recv_cb,
 	.close		= a2mp_chan_close_cb,
 	.send		= l2cap_chan_send,
 	.state_change	= a2mp_chan_state_change_cb,
-- 
1.7.4.1

[RFCv3 17/34] Bluetooth: A2MP: Process A2MP Command Reject

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 2ca7820..2693569 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -64,6 +64,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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -88,6 +104,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 18/34] Bluetooth: A2MP: Helper functions to count HCI devs

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Helper functions used to cound HCI devices (AMP controllers) and
build controller list packet.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |   13 +++++++++++++
 net/bluetooth/a2mp.c             |   31 +++++++++++++++++++++++++++++++
 3 files changed, 46 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index dd2cc6c..eed7a55 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 18af542..039d8b1 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -606,6 +606,19 @@ 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)
 
+/* hci_dev_list shall be locked */
+static inline uint8_t __hci_num_ctrl(void)
+{
+	uint8_t count = 0;
+	struct list_head *p;
+
+	list_for_each(p, &hci_dev_list) {
+		count++;
+	}
+
+	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 2693569..f31cb07 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>
 
@@ -64,6 +65,36 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
+{
+	cl->id = 0;
+	cl->type = 0;
+	cl->status = 1;
+}
+
+/* hci_dev_list shall be locked */
+static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
+{
+	int i = 0;
+	struct hci_dev *hdev;
+
+	__a2mp_cl_bredr(cl);
+
+	list_for_each_entry(hdev, &hci_dev_list, list) {
+		/* Iterate through AMP controllers */
+		if (hdev->id == HCI_BREDR_ID)
+			continue;
+
+		/* Starting from second entry */
+		if (++i >= num_ctrl)
+			return;
+
+		cl[i].id = hdev->id;
+		cl[i].type = hdev->amp_type;
+		cl[i].status = hdev->amp_status;
+	}
+}
+
 /* Processing A2MP messages */
 static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
 							struct a2mp_cmd *hdr)
-- 
1.7.4.1

[RFCv3 19/34] Bluetooth: A2MP: Process A2MP Discover Request

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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 <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 +
 net/bluetooth/a2mp.c         |   55 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 57 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c99c375..a748ab0 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 {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f31cb07..8371d55 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -111,6 +111,58 @@ 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));
+	}
+
+	read_lock(&hci_dev_list_lock);
+
+	num_ctrl = __hci_num_ctrl();
+	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
+	rsp = kmalloc(len, GFP_ATOMIC);
+	if (!rsp) {
+		read_unlock(&hci_dev_list_lock);
+		return -ENOMEM;
+	}
+
+	rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	rsp->ext_feat = 0;
+
+	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
+
+	read_unlock(&hci_dev_list_lock);
+
+	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+	kfree(rsp);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -139,6 +191,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 20/34] Bluetooth: A2MP: Process A2MP Change Notify

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 8371d55..7b54130 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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -195,6 +211,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 21/34] Bluetooth: A2MP: Process A2MP Get Info Request

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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 <andrei.emeltchenko@intel.com>
---
 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 7b54130..b26817d 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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -215,6 +249,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 22/34] Bluetooth: A2MP: Process A2MP Get AMP Assoc Request

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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 <andrei.emeltchenko@intel.com>
---
 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 b26817d..6996fd9 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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -253,6 +282,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 23/34] Bluetooth: A2MP: Process A2MP Create Physlink Request

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 6996fd9..ea881a9 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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -286,6 +302,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 24/34] Bluetooth: A2MP: Process A2MP Disc Physlink Request

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 ea881a9..2d821fc 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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -306,6 +340,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 25/34] Bluetooth: A2MP: Process A2MP Command Responses

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 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 2d821fc..f366f5f 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 */
 int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -349,6 +358,9 @@ int a2mp_chan_recv_cb(void *data, 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

[RFCv3 26/34] Bluetooth: Clean up l2cap_chan_add

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Change elseif to switch

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |   11 ++++++++---
 1 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 0ac81cc..a99009e 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -320,7 +320,8 @@ void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 
 	chan->conn = conn;
 
-	if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
+	switch (chan->chan_type) {
+	case L2CAP_CHAN_CONN_ORIENTED:
 		if (conn->hcon->type == LE_LINK) {
 			/* LE connection */
 			chan->omtu = L2CAP_LE_DEFAULT_MTU;
@@ -331,12 +332,16 @@ void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 			chan->scid = l2cap_alloc_cid(conn);
 			chan->omtu = L2CAP_DEFAULT_MTU;
 		}
-	} else if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
+		break;
+
+	case L2CAP_CHAN_CONN_LESS:
 		/* Connectionless socket */
 		chan->scid = L2CAP_CID_CONN_LESS;
 		chan->dcid = L2CAP_CID_CONN_LESS;
 		chan->omtu = L2CAP_DEFAULT_MTU;
-	} else {
+		break;
+
+	default:
 		/* Raw socket can send/recv signalling messages only */
 		chan->scid = L2CAP_CID_SIGNALING;
 		chan->dcid = L2CAP_CID_SIGNALING;
-- 
1.7.4.1

[RFCv3 27/34] Bluetooth: A2MP: Handling fixed channels

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

A2MP fixed channel do not have sk

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |    4 ++--
 net/bluetooth/l2cap_core.c    |   25 ++++++++++++++++++++++---
 3 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index cc6f299..f896db1 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -551,6 +551,7 @@ struct l2cap_conn {
 #define L2CAP_CHAN_RAW			1
 #define L2CAP_CHAN_CONN_LESS		2
 #define L2CAP_CHAN_CONN_ORIENTED	3
+#define L2CAP_CHAN_CONN_FIX_A2MP	4
 
 /* ----- L2CAP socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f366f5f..d4324f6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -425,9 +425,9 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
 
 	BT_DBG("chan %p", chan);
 
+	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
+
 	chan->sec_level = BT_SECURITY_LOW;
-	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
-	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
 	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 	chan->fcs = L2CAP_FCS_CRC16;
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a99009e..2a0d50c 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -341,6 +341,13 @@ void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 		chan->omtu = L2CAP_DEFAULT_MTU;
 		break;
 
+	case L2CAP_CHAN_CONN_FIX_A2MP:
+		chan->scid = L2CAP_CID_A2MP;
+		chan->dcid = L2CAP_CID_A2MP;
+		chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+		chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+		break;
+
 	default:
 		/* Raw socket can send/recv signalling messages only */
 		chan->scid = L2CAP_CID_SIGNALING;
@@ -366,7 +373,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 {
 	struct sock *sk = chan->sk;
 	struct l2cap_conn *conn = chan->conn;
-	struct sock *parent = bt_sk(sk)->parent;
+	struct sock *parent;
 
 	__clear_chan_timer(chan);
 
@@ -383,6 +390,11 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		hci_conn_put(conn->hcon);
 	}
 
+	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+		goto clean;
+
+	parent = bt_sk(sk)->parent;
+
 	l2cap_state_change(chan, BT_CLOSED);
 	sock_set_flag(sk, SOCK_ZAPPED);
 
@@ -399,6 +411,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 			test_bit(CONF_INPUT_DONE, &chan->conf_state)))
 		return;
 
+clean:
 	skb_queue_purge(&chan->tx_q);
 
 	if (chan->mode == L2CAP_MODE_ERTM) {
@@ -1014,9 +1027,15 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
 	/* Kill channels */
 	list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
 		sk = chan->sk;
-		lock_sock(sk);
+
+		if (sk)
+			lock_sock(sk);
+
 		l2cap_chan_del(chan, err);
-		release_sock(sk);
+
+		if (sk)
+			release_sock(sk);
+
 		chan->ops->close(chan->data);
 	}
 
-- 
1.7.4.1

[RFCv3 28/34] Bluetooth: A2MP: Manage incoming connections

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Handle incoming A2MP connection by creating AMP manager and
processing A2MP messages.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +++
 net/bluetooth/a2mp.c         |   11 +++++++++++
 net/bluetooth/l2cap_core.c   |   10 ++++++++--
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index a748ab0..783f6d4 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
 
 struct amp_mgr {
@@ -117,5 +119,6 @@ struct a2mp_physlink_rsp {
 
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
+void a2mp_channel_create(struct l2cap_conn *conn, struct sk_buff *skb);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d4324f6..b40b079 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -507,3 +507,14 @@ struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 finished:
 	return mgr;
 }
+
+void a2mp_channel_create(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+	struct amp_mgr *mgr;
+
+	mgr = amp_mgr_create(conn);
+
+	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
+
+	l2cap_ertm_data_rcv(mgr->a2mp_chan, skb);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 2a0d50c..6e4d69a 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;
 
@@ -4256,8 +4257,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_channel_create(conn, skb);
+			goto done;
+		} else {
+			BT_DBG("unknown cid 0x%4.4x", cid);
+			goto drop;
+		}
 	}
 
 	sk = chan->sk;
-- 
1.7.4.1

[RFCv3 29/34] Bluetooth: physical link HCI interface to AMP

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Adds support for physical link create/acceppt/disconnect AMP HCI
commands. To be used by the upper layer.
Backport from CodeAurora & Atheros

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   24 ++++++++++++++++++++++
 net/bluetooth/hci_core.c    |   45 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 69 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index eed7a55..fcd8fa9 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -30,6 +30,8 @@
 #define HCI_MAX_EVENT_SIZE	260
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
+#define HCI_MAX_AMP_KEY_SIZE	32
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
@@ -511,6 +513,28 @@ struct hci_cp_io_capability_neg_reply {
 	__u8     reason;
 } __packed;
 
+#define HCI_OP_CREATE_PHY_LINK		0x0435
+struct hci_cp_create_phy_link {
+	__u8     handle;
+	__u8     key_len;
+	__u8     key_type;
+	__u8     key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_ACCEPT_PHY_LINK		0x0436
+struct hci_cp_accept_phy_link {
+	__u8	handle;
+	__u8	key_len;
+	__u8	key_type;
+	__u8	key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_DISC_PHY_LINK		0x0437
+struct hci_cp_disc_phy_link {
+	__u8     handle;
+	__u8     reason;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 29bd53d..95ece9d 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -332,6 +332,51 @@ static void hci_linkpol_req(struct hci_dev *hdev, unsigned long opt)
 	hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, 2, &policy);
 }
 
+/* AMP HCI interface */
+void hci_phylink_create_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_create_req);
+
+void hci_phylink_accept_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_accept_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_accept_req);
+
+void hci_phylink_disc_req(struct hci_dev *hdev, __u8 handle, __u8 reason)
+{
+	struct hci_cp_disc_phy_link cp;
+
+	BT_DBG("handle %d reason %d", handle, reason);
+
+	cp.handle = handle;
+	cp.reason = reason;
+	hci_send_cmd(hdev, HCI_OP_DISC_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_disc_req);
+
 /* Get HCI device by index.
  * Device is held on return. */
 struct hci_dev *hci_dev_get(int index)
-- 
1.7.4.1

[RFCv3 30/34] Bluetooth: Define AMP controller statuses

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

AMP status codes copied from Bluez patch sent by Peter Krystad
<pkrystad@codeaurora.org>.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |    9 +++++++++
 1 files changed, 9 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index fcd8fa9..3310efe 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -60,6 +60,15 @@
 
 #define HCI_BREDR_ID	0
 
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN			0x00
+#define AMP_CTRL_BLUETOOTH_ONLY			0x01
+#define AMP_CTRL_NO_CAPACITY			0x02
+#define AMP_CTRL_LOW_CAPACITY			0x03
+#define AMP_CTRL_MEDIUM_CAPACITY		0x04
+#define AMP_CTRL_HIGH_CAPACITY			0x05
+#define AMP_CTRL_FULL_CAPACITY			0x06
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_NO_RESET,
-- 
1.7.4.1

[RFCv3 31/34] Bluetooth: General HCI callback implementation

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add general HCI callback implementation. Can be used for executing
HCI commands from A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |   18 ++++++++++++
 net/bluetooth/hci_core.c         |   57 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 75 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 039d8b1..4ee2065 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -125,6 +125,17 @@ struct adv_entry {
 	u8 bdaddr_type;
 };
 
+struct hci_dev;
+
+struct cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct cb_cmd *cmd);
+	void (*destructor)(struct cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -231,6 +242,7 @@ struct hci_dev {
 	__u16			init_last_cmd;
 
 	struct list_head	mgmt_pending;
+	struct list_head	cb_list;
 
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
@@ -1029,4 +1041,10 @@ void hci_le_ltk_neg_reply(struct hci_conn *conn);
 int hci_do_inquiry(struct hci_dev *hdev, u8 length);
 int hci_cancel_inquiry(struct hci_dev *hdev);
 
+struct cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	void (*cb)(struct hci_dev *hdev, struct cb_cmd *cmd), void *opt,
+				void (*destructor)(struct cb_cmd *cmd));
+void hci_remove_cb(struct cb_cmd *cmd);
+
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 95ece9d..5aefb61 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1678,6 +1678,8 @@ int hci_register_dev(struct hci_dev *hdev)
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 
+	INIT_LIST_HEAD(&hdev->cb_list);
+
 	INIT_LIST_HEAD(&hdev->blacklist);
 
 	INIT_LIST_HEAD(&hdev->uuids);
@@ -2081,6 +2083,61 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+void hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+			void (*cb)(struct hci_dev *hdev, struct cb_cmd *cmd),
+			void *opt, void (*destructor)(struct cb_cmd *cmd))
+{
+	struct cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
+	if (!cmd)
+		return;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+
+	if (destructor)
+		cmd->destructor = destructor;
+
+	list_add(&cmd->list, &hdev->cb_list);
+}
+
+struct cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct cb_cmd *cmd;
+
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode)
+			return cmd;
+
+	return NULL;
+}
+
+void hci_remove_cb(struct cb_cmd *cmd)
+{
+	list_del(&cmd->list);
+
+	if (cmd->destructor) {
+		cmd->destructor(cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+			void (*cb)(struct hci_dev *hdev, struct cb_cmd *cmd),
+			void *opt, void (*destructor)(struct cb_cmd *cmd))
+{
+	if (cb)
+		hci_add_cb(hdev, opcode, cb, opt, destructor);
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.4.1

[RFCv3 32/34] Bluetooth: Process HCI callbacks in a workqueue

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/hci_core.c         |   42 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 44 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 4ee2065..b43784e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1046,5 +1046,7 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	void (*cb)(struct hci_dev *hdev, struct cb_cmd *cmd), void *opt,
 				void (*destructor)(struct cb_cmd *cmd));
 void hci_remove_cb(struct cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev * hdev, struct cb_cmd *cmd,
+					struct workqueue_struct *workqueue);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 5aefb61..35c0dc8 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2115,6 +2115,48 @@ struct cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct cb_cmd *cmd;
+};
+
+static void hci_cb_work(struct work_struct *w)
+{
+	struct cb_work *work = (struct cb_work *) w;
+	struct cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_dev_put(hdev);
+
+	hci_remove_cb(cmd);
+	kfree(w);
+}
+
+void hci_queue_cb(struct hci_dev * hdev, struct cb_cmd *cmd,
+					struct workqueue_struct *workqueue)
+{
+	struct cb_work *work;
+
+	BT_ERR("Queue cmd %p opt %p", cmd, cmd->opt);
+
+	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_work);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct cb_cmd *cmd)
 {
 	list_del(&cmd->list);
-- 
1.7.4.1

[RFCv3 33/34] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Adds Read Local AMP Info HCI command with callback to be executed
when receiving command complete event.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/Makefile    |    4 ++--
 net/bluetooth/a2mp.c      |    4 ++++
 net/bluetooth/amp.c       |   34 ++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c |   13 +++++++++++--
 4 files changed, 51 insertions(+), 4 deletions(-)
 create mode 100644 net/bluetooth/amp.c

diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index 88bc999..b38e5be 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,6 +10,6 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
 
-bluetooth-$(CONFIG_BT_HS)	+= a2mp.o
+bluetooth-$(CONFIG_BT_HS)	+= a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b40b079..4de97a5 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -19,6 +19,8 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -94,6 +96,8 @@ static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
 		cl[i].id = hdev->id;
 		cl[i].type = hdev->amp_type;
 		cl[i].status = hdev->amp_status;
+
+		amp_read_loc_info(hdev, mgr);
 	}
 }
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..3832c28b
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,34 @@
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <linux/workqueue.h>
+
+void amp_read_loc_info_complete(struct hci_dev *hdev, struct cb_cmd *cmd)
+{
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+}
+
+static void cb_destructor(struct cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	int err = 0;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+			amp_read_loc_info_complete, mgr, cb_destructor);
+
+	return err;
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index c43ba90..acbf4a9 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -44,6 +44,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 static int enable_le;
 
@@ -813,14 +814,15 @@ static void hci_cc_write_ca_timeout(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
-		struct sk_buff *skb)
+							struct sk_buff *skb)
 {
 	struct hci_rp_read_local_amp_info *rp = (void *) skb->data;
+	struct cb_cmd *cmd;
 
 	BT_DBG("%s status 0x%x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto send;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -834,6 +836,13 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+send:
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.4.1

[RFCv3 34/34] Bluetooth: AMP: Read Local Assoc support

[Emeltchenko Andrei]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Adds reading AMP Assoc in HCI callback

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    3 ++
 include/net/bluetooth/hci.h      |   15 ++++++++++
 include/net/bluetooth/hci_core.h |    8 +++++
 net/bluetooth/a2mp.c             |    7 +++-
 net/bluetooth/amp.c              |   57 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   45 ++++++++++++++++++++++++++++++
 6 files changed, 133 insertions(+), 2 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 783f6d4..61bfde3 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -121,4 +121,7 @@ void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 void a2mp_channel_create(struct l2cap_conn *conn, struct sk_buff *skb);
 
+
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 3310efe..7683d93 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -31,6 +31,7 @@
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
 #define HCI_MAX_AMP_KEY_SIZE	32
+#define HCI_MAX_AMP_ASSOC_SIZE	672
 
 /* HCI dev events */
 #define HCI_DEV_REG			1
@@ -835,6 +836,20 @@ struct hci_rp_read_local_amp_info {
 	__le32   be_flush_to;
 } __packed;
 
+#define HCI_OP_READ_LOCAL_AMP_ASSOC	0x140a
+struct hci_cp_read_local_amp_assoc {
+	__u8     handle;
+	__le16   len_so_far;
+	__le16   max_len;
+} __packed;
+
+struct hci_rp_read_local_amp_assoc {
+	__u8     status;
+	__u8     handle;
+	__le16   rem_len;
+	__u8     frag[0];
+} __packed;
+
 #define HCI_OP_LE_SET_EVENT_MASK	0x2001
 struct hci_cp_le_set_event_mask {
 	__u8     mask[8];
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b43784e..2cb7f50 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -136,6 +136,12 @@ struct cb_cmd {
 	void (*destructor)(struct cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -184,6 +190,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4de97a5..88dc8f2 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -20,6 +20,8 @@
 #include <net/bluetooth/a2mp.h>
 
 int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -55,8 +57,7 @@ static inline int __a2mp_send(struct amp_mgr *mgr, u8 *data, int len)
 	return chan->ops->send(chan, &msg, len, 0);
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-								void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct a2mp_cmd *cmd;
 
@@ -239,6 +240,8 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 									&rsp);
 	}
 
+	amp_read_loc_assoc(hdev, mgr);
+
 	if (hdev)
 		hci_dev_put(hdev);
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 3832c28b..7f64f5b 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -32,3 +32,60 @@ int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 
 	return err;
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s: handle %d", hdev->name, phy_handle);
+
+	cp.handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc_complete(struct hci_dev *hdev, struct cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s: cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+			amp_read_loc_assoc_complete, mgr, cb_destructor);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index acbf4a9..ee94880 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -48,6 +48,8 @@
 
 static int enable_le;
 
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
+
 /* Handle HCI Event packets */
 
 static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb)
@@ -845,6 +847,45 @@ send:
 	}
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+							struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	struct cb_cmd *cmd;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto send;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+send:
+	/* Run callback when all fragments received */
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 							struct sk_buff *skb)
 {
@@ -2191,6 +2232,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.4.1