Bluetooth: A2MP: Process A2MP Discover Request

Adds helper functions to count HCI devs and 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 22
    A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
      Controller list:
        id 0 type 0 (BR-EDR) status 0x01 (Bluetooth only)
        id 1 type 1 (802.11 AMP) status 0x01 (Bluetooth only)
...

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 391acd7a67d4a70a08009047d4945e765193a268..96f9cc2cf59b4b9d7d69fd09ae48c7bc686a3d56 100644 (file)
--- 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 l2cap_conn       *l2cap_conn;
        struct l2cap_chan       *a2mp_chan;

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index de09a26e4223936d82a82beb5c48a31fb6138fbc..66af2c6193d5ae6b9d35398ce34b50b01bd8aa1e 100644 (file)
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -59,6 +59,9 @@
 #define HCI_BREDR      0x00
 #define HCI_AMP                0x01
 
+/* First BR/EDR Controller shall have ID = 0 */
+#define HCI_BREDR_ID   0
+
 /* HCI device quirks */
 enum {
        HCI_QUIRK_RESET_ON_CLOSE,

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6e64b76e30aa92dc556376af291073d8b5a0426b..20fd57367ddcd90f4b3d7027d45af275a860cb41 100644 (file)
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -641,6 +641,19 @@ static inline void hci_set_drvdata(struct hci_dev *hdev, void *data)
        dev_set_drvdata(&hdev->dev, data);
 }
 
+/* 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 188b4212007478245ec88a3833201c90a090ac80..1cc920a62b0fb271abed84e99ff95aff86a074c2 100644 (file)
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,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 int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
                            struct a2mp_cmd *hdr)
@@ -79,6 +109,58 @@ static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
        return 0;
 }
 
+static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
+                            struct a2mp_cmd *hdr)
+{
+       struct a2mp_discov_req *req = (void *) skb->data;
+       u16 len = le16_to_cpu(hdr->len);
+       struct a2mp_discov_rsp *rsp;
+       u16 ext_feat;
+       u8 num_ctrl;
+
+       if (len < sizeof(*req))
+               return -EINVAL;
+
+       skb_pull(skb, sizeof(*req));
+
+       ext_feat = le16_to_cpu(req->ext_feat);
+
+       BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
+
+       /* check that packet is not broken for now */
+       while (ext_feat & A2MP_FEAT_EXT) {
+               if (len < sizeof(ext_feat))
+                       return -EINVAL;
+
+               ext_feat = get_unaligned_le16(skb->data);
+               BT_DBG("efm 0x%4.4x", ext_feat);
+               len -= sizeof(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 = __constant_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 */
 static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -109,6 +191,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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: