diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h index af594685112..d5476719fa4 100644 --- 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 index 00000000000..e8616751bc8 --- /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 4be26abf6da..ccc08e57c10 100644 --- 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 e7d45460988..b1c7d060724 100644 --- 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 fa6d94a4602..dea6a287dac 100644 --- 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 0e97b3b42ab..71400612843 100644 --- a/net/bluetooth/a2mp.c +++ b/net/bluetooth/a2mp.c @@ -16,6 +16,7 @@ #include #include #include +#include /* 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 index 00000000000..2d4e79ee06a --- /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 +#include +#include +#include +#include + +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 eb45774165f..a4240f7a142 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -31,6 +31,7 @@ #include #include #include +#include /* 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;