Commit 6b44d9b8 authored by Andrei Emeltchenko's avatar Andrei Emeltchenko Committed by Johan Hedberg
Browse files

Bluetooth: A2MP: Process A2MP messages 



Implement basic processing for AMP Manager Protocol (A2MP).

Example below shows processing unrecognized command.
...
> ACL data: handle 11 flags 0x02 dlen 12
    A2MP: code 0x00 ident 3 len 0
< ACL data: handle 11 flags 0x00 dlen 14
    A2MP: Command Reject: reason (0) - Command not recognized
...

Signed-off-by: default avatarAndrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: default avatarGustavo Padovan <gustavo.padovan@collabora.co.uk>
parent e7af522e

net/bluetooth/a2mp.c

+65 −0
Original line number Diff line number Diff line
@@ -63,6 +63,70 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, 
	kfree(cmd);

}



/* Handle A2MP signalling */

static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)

{

	struct a2mp_cmd *hdr = (void *) skb->data;

	struct amp_mgr *mgr = chan->data;

	int err = 0;



	amp_mgr_get(mgr);



	while (skb->len >= sizeof(*hdr)) {

		struct a2mp_cmd *hdr = (void *) skb->data;

		u16 len = le16_to_cpu(hdr->len);



		BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);



		skb_pull(skb, sizeof(*hdr));



		if (len > skb->len || !hdr->ident) {

			err = -EINVAL;

			break;

		}



		mgr->ident = hdr->ident;



		switch (hdr->code) {

		case A2MP_COMMAND_REJ:

		case A2MP_DISCOVER_REQ:

		case A2MP_CHANGE_NOTIFY:

		case A2MP_GETINFO_REQ:

		case A2MP_GETAMPASSOC_REQ:

		case A2MP_CREATEPHYSLINK_REQ:

		case A2MP_DISCONNPHYSLINK_REQ:

		case A2MP_CHANGE_RSP:

		case A2MP_DISCOVER_RSP:

		case A2MP_GETINFO_RSP:

		case A2MP_GETAMPASSOC_RSP:

		case A2MP_CREATEPHYSLINK_RSP:

		case A2MP_DISCONNPHYSLINK_RSP:

		default:

			BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);

			err = -EINVAL;

			break;

		}

	}



	if (err) {

		struct a2mp_cmd_rej rej;

		rej.reason = __constant_cpu_to_le16(0);



		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);



		a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),

			  &rej);

	}



	/* Always free skb and return success error code to prevent

	   from sending L2CAP Disconnect over A2MP channel */

	kfree_skb(skb);



	amp_mgr_put(mgr);



	return 0;

}



static void a2mp_chan_close_cb(struct l2cap_chan *chan)

{

	l2cap_chan_destroy(chan);
@@ -112,6 +176,7 @@ static void a2mp_chan_no_ready(struct l2cap_chan *chan) 


static struct l2cap_ops a2mp_chan_ops = {

	.name = "L2CAP A2MP channel",

	.recv = a2mp_chan_recv_cb,

	.close = a2mp_chan_close_cb,

	.state_change = a2mp_chan_state_change_cb,

	.alloc_skb = a2mp_chan_alloc_skb_cb,