Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger than it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index af594685112e64fe134c895da57cdc362b453f4b..d5476719fa4c5bad9610776db3f87b7c74b32c39 100644 (file)
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -21,6 +21,7 @@
 
 enum amp_mgr_state {
        READ_LOC_AMP_INFO,
+       READ_LOC_AMP_ASSOC,
 };
 
 struct amp_mgr {
@@ -134,5 +135,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644 (file)
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   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 __AMP_H
+#define __AMP_H
+
+void 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);
+
+#endif /* __AMP_H */

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 4be26abf6dad94713ce652ab543ca6615d347c3a..ccc08e57c10705f24eaa5701f81923c3308e651d 100644 (file)
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE      16
 #define HCI_AMP_LINK_KEY_SIZE  (2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE 672
+
 /* HCI dev events */
 #define HCI_DEV_REG                    1
 #define HCI_DEV_UNREG                  2

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index e7d454609881a30d929ec8aa613390df157c8b43..b1c7d0607244df3ffbd9769cff2e8375c18fc36c 100644 (file)
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH      10
 
+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;
@@ -177,6 +183,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/Makefile b/net/bluetooth/Makefile
index fa6d94a4602a8352a6f2aa9ed13ed18ff7e6c08a..dea6a287daca88b666eaee999043a3abf6dd853a 100644 (file)
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ 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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0e97b3b42ab113b4029efbe3695a5695f58c39e9..71400612843d9f9a05a68fb69d96586593bfea6a 100644 (file)
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -218,26 +219,37 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 {
        struct a2mp_amp_assoc_req *req = (void *) skb->data;
        struct hci_dev *hdev;
+       struct amp_mgr *tmp;
 
        if (le16_to_cpu(hdr->len) < sizeof(*req))
                return -EINVAL;
 
        BT_DBG("id %d", req->id);
 
+       /* Make sure that other request is not processed */
+       tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+
        hdev = hci_dev_get(req->id);
-       if (!hdev || hdev->amp_type == HCI_BREDR) {
+       if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
                struct a2mp_amp_assoc_rsp rsp;
                rsp.id = req->id;
-               rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+               if (tmp) {
+                       rsp.status = A2MP_STATUS_COLLISION_OCCURED;
+                       amp_mgr_put(tmp);
+               } else {
+                       rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+               }
 
                a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
                          &rsp);
-               goto clean;
+
+               goto done;
        }
 
-       /* Placeholder for HCI Read AMP Assoc */
+       amp_read_loc_assoc(hdev, mgr);
 
-clean:
+done:
        if (hdev)
                hci_dev_put(hdev);
 
@@ -624,3 +636,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
        a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
        amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+       struct amp_mgr *mgr;
+       struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+       struct a2mp_amp_assoc_rsp *rsp;
+       size_t len;
+
+       mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+       if (!mgr)
+               return;
+
+       BT_DBG("%s mgr %p", hdev->name, mgr);
+
+       len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+       rsp = kzalloc(len, GFP_KERNEL);
+       if (!rsp) {
+               amp_mgr_put(mgr);
+               return;
+       }
+
+       rsp->id = hdev->id;
+
+       if (status) {
+               rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+       } else {
+               rsp->status = A2MP_STATUS_SUCCESS;
+               memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+       }
+
+       a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+       amp_mgr_put(mgr);
+       kfree(rsp);
+}

diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644 (file)
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   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/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+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.phy_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(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);
+
+       mgr->state = READ_LOC_AMP_ASSOC;
+       hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index eb45774165f4ff4bcecec416841eca8b241ff585..a4240f7a142e058ee72281c36a3c29ea41e7df47 100644 (file)
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
        a2mp_send_getinfo_rsp(hdev);
 }
 
+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;
+       size_t rem_len, frag_len;
+
+       BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+       if (rp->status)
+               goto a2mp_rsp;
+
+       frag_len = skb->len - sizeof(*rp);
+       rem_len = __le16_to_cpu(rp->rem_len);
+
+       if (rem_len > frag_len) {
+               BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+               memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+               assoc->offset += frag_len;
+
+               /* Read other fragments */
+               amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+               return;
+       }
+
+       memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+       assoc->len = assoc->offset + rem_len;
+       assoc->offset = 0;
+
+a2mp_rsp:
+       /* Send A2MP Rsp when all fragments are received */
+       a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
                                          struct sk_buff *skb)
 {
@@ -2318,6 +2355,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                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;