diff options
| author | Andrei Emeltchenko <andrei.emeltchenko@intel.com> | 2012-05-29 13:59:07 +0300 |
|---|---|---|
| committer | Johan Hedberg <johan.hedberg@intel.com> | 2012-06-05 06:34:12 +0300 |
| commit | 6b44d9b8d96b37f72ccd7335b32f386a67b7f1f4 (patch) | |
| tree | 5bbcf9598ae79f777e6af5fd1b2ab43fc8f34679 | |
| parent | e7af522e04bcf68caae6802722efc5c6e8fa63a7 (diff) | |
Bluetooth: A2MP: Process A2MP messages
Implement basic processing for AMP Manager Protocol (A2MP).
Example below shows processing unrecognized command.
...
> ACL data: handle 11 flags 0x02 dlen 12
A2MP: code 0x00 ident 3 len 0
< ACL data: handle 11 flags 0x00 dlen 14
A2MP: Command Reject: reason (0) - Command not recognized
...
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
| -rw-r--r-- | net/bluetooth/a2mp.c | 65 |
1 files changed, 65 insertions, 0 deletions
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index f7c710cacf44..0726c9fe005c 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -63,6 +63,70 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, kfree(cmd); } +/* Handle A2MP signalling */ +static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct a2mp_cmd *hdr = (void *) skb->data; + struct amp_mgr *mgr = chan->data; + int err = 0; + + amp_mgr_get(mgr); + + while (skb->len >= sizeof(*hdr)) { + struct a2mp_cmd *hdr = (void *) skb->data; + u16 len = le16_to_cpu(hdr->len); + + BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len); + + skb_pull(skb, sizeof(*hdr)); + + if (len > skb->len || !hdr->ident) { + err = -EINVAL; + break; + } + + mgr->ident = hdr->ident; + + switch (hdr->code) { + case A2MP_COMMAND_REJ: + case A2MP_DISCOVER_REQ: + case A2MP_CHANGE_NOTIFY: + case A2MP_GETINFO_REQ: + case A2MP_GETAMPASSOC_REQ: + case A2MP_CREATEPHYSLINK_REQ: + case A2MP_DISCONNPHYSLINK_REQ: + case A2MP_CHANGE_RSP: + case A2MP_DISCOVER_RSP: + case A2MP_GETINFO_RSP: + case A2MP_GETAMPASSOC_RSP: + case A2MP_CREATEPHYSLINK_RSP: + case A2MP_DISCONNPHYSLINK_RSP: + default: + BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); + err = -EINVAL; + break; + } + } + + if (err) { + struct a2mp_cmd_rej rej; + rej.reason = __constant_cpu_to_le16(0); + + BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); + + a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), + &rej); + } + + /* Always free skb and return success error code to prevent + from sending L2CAP Disconnect over A2MP channel */ + kfree_skb(skb); + + amp_mgr_put(mgr); + + return 0; +} + static void a2mp_chan_close_cb(struct l2cap_chan *chan) { l2cap_chan_destroy(chan); @@ -112,6 +176,7 @@ static void a2mp_chan_no_ready(struct l2cap_chan *chan) static struct l2cap_ops a2mp_chan_ops = { .name = "L2CAP A2MP channel", + .recv = a2mp_chan_recv_cb, .close = a2mp_chan_close_cb, .state_change = a2mp_chan_state_change_cb, .alloc_skb = a2mp_chan_alloc_skb_cb, |