Return-Path: From: Emeltchenko Andrei To: linux-bluetooth@vger.kernel.org Subject: [RFCv0 09/20] Bluetooth: A2MP: process A2MP messages Date: Wed, 2 Nov 2011 16:02:37 +0200 Message-Id: <1320242568-372-10-git-send-email-Andrei.Emeltchenko.news@gmail.com> In-Reply-To: <1320242568-372-1-git-send-email-Andrei.Emeltchenko.news@gmail.com> References: <1320242568-372-1-git-send-email-Andrei.Emeltchenko.news@gmail.com> Sender: linux-bluetooth-owner@vger.kernel.org List-ID: From: Andrei Emeltchenko Implement basic processing for AMP Manager Protocol (A2MP). Signed-off-by: Andrei Emeltchenko --- net/bluetooth/a2mp.c | 57 +++++++++++++++++++++++++++++++++++++++++++++++++- 1 files changed, 56 insertions(+), 1 deletions(-) diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index afb01e8..5071acc 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -71,6 +71,61 @@ static void a2mp_send(struct socket *a2mp_sock, u8 code, u8 ident, u16 len, kfree(cmd); } +/* Handle A2MP signalling */ +void a2mp_receive(struct sock *sk, struct sk_buff *skb) +{ + struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data; + struct amp_mgr *mgr; + int err = 0; + + mgr = get_amp_mgr_sk(sk); + if (!mgr) + return; + + while (skb->len >= sizeof(*hdr)) { + struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data; + u16 len = le16_to_cpu(hdr->len); + + BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len); + + skb_pull(skb, sizeof(*hdr)); + + if (len > skb->len || !hdr->ident) { + err = -EINVAL; + break; + } + + switch (hdr->code) { + case A2MP_COMMAND_REJ: + case A2MP_DISCOVER_REQ: + case A2MP_CHANGE_NOTIFY: + case A2MP_GETINFO_REQ: + case A2MP_GETAMPASSOC_REQ: + case A2MP_CREATEPHYSLINK_REQ: + case A2MP_DISCONNPHYSLINK_REQ: + case A2MP_CHANGE_RSP: + case A2MP_DISCOVER_RSP: + case A2MP_GETINFO_RSP: + case A2MP_GETAMPASSOC_RSP: + case A2MP_CREATEPHYSLINK_RSP: + case A2MP_DISCONNPHYSLINK_RSP: + default: + BT_ERR("Unknown A2MP signaling command 0x%2.2x", + hdr->code); + err = -EINVAL; + break; + } + } + + if (err && mgr) { + struct a2mp_cmd_rej rej; + rej.reason = cpu_to_le16(0); + + a2mp_send(mgr->a2mp_sock, A2MP_COMMAND_REJ, hdr->ident, + sizeof(rej), &rej); + } +} + static void data_ready_worker(struct work_struct *w) { struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w; @@ -81,7 +136,7 @@ static void data_ready_worker(struct work_struct *w) /* skb_dequeue() is thread-safe */ while ((skb = skb_dequeue(&sk->sk_receive_queue))) { - /* process a2mp here */ + a2mp_receive(sk, skb); kfree_skb(skb); } -- 1.7.4.1