Skip to content

Commit

Permalink
---
Browse files Browse the repository at this point in the history
yaml
---
r: 314453
b: refs/heads/master
c: 8598d06
h: refs/heads/master
i:
  314451: 77acd26
v: v3
  • Loading branch information
Andrei Emeltchenko authored and Johan Hedberg committed Jun 5, 2012
1 parent 25e80dd commit 2fb4f68
Show file tree
Hide file tree
Showing 5 changed files with 104 additions and 1 deletion.
2 changes: 1 addition & 1 deletion [refs]
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
---
refs/heads/master: 21dbd2ce35f6d2b4aa5363be6c839cdb50644e11
refs/heads/master: 8598d064cbf22b2d84c7cd8a9fcb97138baffe3f
2 changes: 2 additions & 0 deletions trunk/include/net/bluetooth/a2mp.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
3 changes: 3 additions & 0 deletions trunk/include/net/bluetooth/hci.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
13 changes: 13 additions & 0 deletions trunk/include/net/bluetooth/hci_core.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
85 changes: 85 additions & 0 deletions trunk/net/bluetooth/a2mp.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
{
Expand Down Expand Up @@ -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:
Expand Down

0 comments on commit 2fb4f68

Please sign in to comment.