Bluetooth: A2MP: Process A2MP Discover Request
authorAndrei Emeltchenko <andrei.emeltchenko@intel.com>
Tue, 29 May 2012 10:59:09 +0000 (13:59 +0300)
committerJohan Hedberg <johan.hedberg@intel.com>
Tue, 5 Jun 2012 03:34:12 +0000 (06:34 +0300)
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>
include/net/bluetooth/a2mp.h
include/net/bluetooth/hci.h
include/net/bluetooth/hci_core.h
net/bluetooth/a2mp.c

index 391acd7a67d4a70a08009047d4945e765193a268..96f9cc2cf59b4b9d7d69fd09ae48c7bc686a3d56 100644 (file)
@@ -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;
index de09a26e4223936d82a82beb5c48a31fb6138fbc..66af2c6193d5ae6b9d35398ce34b50b01bd8aa1e 100644 (file)
@@ -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,
index 6e64b76e30aa92dc556376af291073d8b5a0426b..20fd57367ddcd90f4b3d7027d45af275a860cb41 100644 (file)
@@ -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);
 
index 188b4212007478245ec88a3833201c90a090ac80..1cc920a62b0fb271abed84e99ff95aff86a074c2 100644 (file)
@@ -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: