cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

a2mp.c (22300B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
      4   Copyright (c) 2011,2012 Intel Corp.
      5
      6*/
      7
      8#include <net/bluetooth/bluetooth.h>
      9#include <net/bluetooth/hci_core.h>
     10#include <net/bluetooth/l2cap.h>
     11
     12#include "hci_request.h"
     13#include "a2mp.h"
     14#include "amp.h"
     15
     16#define A2MP_FEAT_EXT	0x8000
     17
     18/* Global AMP Manager list */
     19static LIST_HEAD(amp_mgr_list);
     20static DEFINE_MUTEX(amp_mgr_list_lock);
     21
     22/* A2MP build & send command helper functions */
     23static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
     24{
     25	struct a2mp_cmd *cmd;
     26	int plen;
     27
     28	plen = sizeof(*cmd) + len;
     29	cmd = kzalloc(plen, GFP_KERNEL);
     30	if (!cmd)
     31		return NULL;
     32
     33	cmd->code = code;
     34	cmd->ident = ident;
     35	cmd->len = cpu_to_le16(len);
     36
     37	memcpy(cmd->data, data, len);
     38
     39	return cmd;
     40}
     41
     42static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
     43{
     44	struct l2cap_chan *chan = mgr->a2mp_chan;
     45	struct a2mp_cmd *cmd;
     46	u16 total_len = len + sizeof(*cmd);
     47	struct kvec iv;
     48	struct msghdr msg;
     49
     50	cmd = __a2mp_build(code, ident, len, data);
     51	if (!cmd)
     52		return;
     53
     54	iv.iov_base = cmd;
     55	iv.iov_len = total_len;
     56
     57	memset(&msg, 0, sizeof(msg));
     58
     59	iov_iter_kvec(&msg.msg_iter, WRITE, &iv, 1, total_len);
     60
     61	l2cap_chan_send(chan, &msg, total_len);
     62
     63	kfree(cmd);
     64}
     65
     66static u8 __next_ident(struct amp_mgr *mgr)
     67{
     68	if (++mgr->ident == 0)
     69		mgr->ident = 1;
     70
     71	return mgr->ident;
     72}
     73
     74static struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
     75{
     76	struct amp_mgr *mgr;
     77
     78	mutex_lock(&amp_mgr_list_lock);
     79	list_for_each_entry(mgr, &amp_mgr_list, list) {
     80		if (test_and_clear_bit(state, &mgr->state)) {
     81			amp_mgr_get(mgr);
     82			mutex_unlock(&amp_mgr_list_lock);
     83			return mgr;
     84		}
     85	}
     86	mutex_unlock(&amp_mgr_list_lock);
     87
     88	return NULL;
     89}
     90
     91/* hci_dev_list shall be locked */
     92static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl)
     93{
     94	struct hci_dev *hdev;
     95	int i = 1;
     96
     97	cl[0].id = AMP_ID_BREDR;
     98	cl[0].type = AMP_TYPE_BREDR;
     99	cl[0].status = AMP_STATUS_BLUETOOTH_ONLY;
    100
    101	list_for_each_entry(hdev, &hci_dev_list, list) {
    102		if (hdev->dev_type == HCI_AMP) {
    103			cl[i].id = hdev->id;
    104			cl[i].type = hdev->amp_type;
    105			if (test_bit(HCI_UP, &hdev->flags))
    106				cl[i].status = hdev->amp_status;
    107			else
    108				cl[i].status = AMP_STATUS_POWERED_DOWN;
    109			i++;
    110		}
    111	}
    112}
    113
    114/* Processing A2MP messages */
    115static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
    116			    struct a2mp_cmd *hdr)
    117{
    118	struct a2mp_cmd_rej *rej = (void *) skb->data;
    119
    120	if (le16_to_cpu(hdr->len) < sizeof(*rej))
    121		return -EINVAL;
    122
    123	BT_DBG("ident %u reason %d", hdr->ident, le16_to_cpu(rej->reason));
    124
    125	skb_pull(skb, sizeof(*rej));
    126
    127	return 0;
    128}
    129
    130static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
    131			     struct a2mp_cmd *hdr)
    132{
    133	struct a2mp_discov_req *req = (void *) skb->data;
    134	u16 len = le16_to_cpu(hdr->len);
    135	struct a2mp_discov_rsp *rsp;
    136	u16 ext_feat;
    137	u8 num_ctrl;
    138	struct hci_dev *hdev;
    139
    140	if (len < sizeof(*req))
    141		return -EINVAL;
    142
    143	skb_pull(skb, sizeof(*req));
    144
    145	ext_feat = le16_to_cpu(req->ext_feat);
    146
    147	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
    148
    149	/* check that packet is not broken for now */
    150	while (ext_feat & A2MP_FEAT_EXT) {
    151		if (len < sizeof(ext_feat))
    152			return -EINVAL;
    153
    154		ext_feat = get_unaligned_le16(skb->data);
    155		BT_DBG("efm 0x%4.4x", ext_feat);
    156		len -= sizeof(ext_feat);
    157		skb_pull(skb, sizeof(ext_feat));
    158	}
    159
    160	read_lock(&hci_dev_list_lock);
    161
    162	/* at minimum the BR/EDR needs to be listed */
    163	num_ctrl = 1;
    164
    165	list_for_each_entry(hdev, &hci_dev_list, list) {
    166		if (hdev->dev_type == HCI_AMP)
    167			num_ctrl++;
    168	}
    169
    170	len = struct_size(rsp, cl, num_ctrl);
    171	rsp = kmalloc(len, GFP_ATOMIC);
    172	if (!rsp) {
    173		read_unlock(&hci_dev_list_lock);
    174		return -ENOMEM;
    175	}
    176
    177	rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
    178	rsp->ext_feat = 0;
    179
    180	__a2mp_add_cl(mgr, rsp->cl);
    181
    182	read_unlock(&hci_dev_list_lock);
    183
    184	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
    185
    186	kfree(rsp);
    187	return 0;
    188}
    189
    190static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
    191			     struct a2mp_cmd *hdr)
    192{
    193	struct a2mp_discov_rsp *rsp = (void *) skb->data;
    194	u16 len = le16_to_cpu(hdr->len);
    195	struct a2mp_cl *cl;
    196	u16 ext_feat;
    197	bool found = false;
    198
    199	if (len < sizeof(*rsp))
    200		return -EINVAL;
    201
    202	len -= sizeof(*rsp);
    203	skb_pull(skb, sizeof(*rsp));
    204
    205	ext_feat = le16_to_cpu(rsp->ext_feat);
    206
    207	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
    208
    209	/* check that packet is not broken for now */
    210	while (ext_feat & A2MP_FEAT_EXT) {
    211		if (len < sizeof(ext_feat))
    212			return -EINVAL;
    213
    214		ext_feat = get_unaligned_le16(skb->data);
    215		BT_DBG("efm 0x%4.4x", ext_feat);
    216		len -= sizeof(ext_feat);
    217		skb_pull(skb, sizeof(ext_feat));
    218	}
    219
    220	cl = (void *) skb->data;
    221	while (len >= sizeof(*cl)) {
    222		BT_DBG("Remote AMP id %u type %u status %u", cl->id, cl->type,
    223		       cl->status);
    224
    225		if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) {
    226			struct a2mp_info_req req;
    227
    228			found = true;
    229
    230			memset(&req, 0, sizeof(req));
    231
    232			req.id = cl->id;
    233			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
    234				  sizeof(req), &req);
    235		}
    236
    237		len -= sizeof(*cl);
    238		cl = skb_pull(skb, sizeof(*cl));
    239	}
    240
    241	/* Fall back to L2CAP init sequence */
    242	if (!found) {
    243		struct l2cap_conn *conn = mgr->l2cap_conn;
    244		struct l2cap_chan *chan;
    245
    246		mutex_lock(&conn->chan_lock);
    247
    248		list_for_each_entry(chan, &conn->chan_l, list) {
    249
    250			BT_DBG("chan %p state %s", chan,
    251			       state_to_string(chan->state));
    252
    253			if (chan->scid == L2CAP_CID_A2MP)
    254				continue;
    255
    256			l2cap_chan_lock(chan);
    257
    258			if (chan->state == BT_CONNECT)
    259				l2cap_send_conn_req(chan);
    260
    261			l2cap_chan_unlock(chan);
    262		}
    263
    264		mutex_unlock(&conn->chan_lock);
    265	}
    266
    267	return 0;
    268}
    269
    270static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
    271			      struct a2mp_cmd *hdr)
    272{
    273	struct a2mp_cl *cl = (void *) skb->data;
    274
    275	while (skb->len >= sizeof(*cl)) {
    276		BT_DBG("Controller id %u type %u status %u", cl->id, cl->type,
    277		       cl->status);
    278		cl = skb_pull(skb, sizeof(*cl));
    279	}
    280
    281	/* TODO send A2MP_CHANGE_RSP */
    282
    283	return 0;
    284}
    285
    286static void read_local_amp_info_complete(struct hci_dev *hdev, u8 status,
    287					 u16 opcode)
    288{
    289	BT_DBG("%s status 0x%2.2x", hdev->name, status);
    290
    291	a2mp_send_getinfo_rsp(hdev);
    292}
    293
    294static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
    295			    struct a2mp_cmd *hdr)
    296{
    297	struct a2mp_info_req *req  = (void *) skb->data;
    298	struct hci_dev *hdev;
    299	struct hci_request hreq;
    300	int err = 0;
    301
    302	if (le16_to_cpu(hdr->len) < sizeof(*req))
    303		return -EINVAL;
    304
    305	BT_DBG("id %u", req->id);
    306
    307	hdev = hci_dev_get(req->id);
    308	if (!hdev || hdev->dev_type != HCI_AMP) {
    309		struct a2mp_info_rsp rsp;
    310
    311		memset(&rsp, 0, sizeof(rsp));
    312
    313		rsp.id = req->id;
    314		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
    315
    316		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
    317			  &rsp);
    318
    319		goto done;
    320	}
    321
    322	set_bit(READ_LOC_AMP_INFO, &mgr->state);
    323	hci_req_init(&hreq, hdev);
    324	hci_req_add(&hreq, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
    325	err = hci_req_run(&hreq, read_local_amp_info_complete);
    326	if (err < 0)
    327		a2mp_send_getinfo_rsp(hdev);
    328
    329done:
    330	if (hdev)
    331		hci_dev_put(hdev);
    332
    333	skb_pull(skb, sizeof(*req));
    334	return 0;
    335}
    336
    337static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
    338			    struct a2mp_cmd *hdr)
    339{
    340	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
    341	struct a2mp_amp_assoc_req req;
    342	struct amp_ctrl *ctrl;
    343
    344	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
    345		return -EINVAL;
    346
    347	BT_DBG("id %u status 0x%2.2x", rsp->id, rsp->status);
    348
    349	if (rsp->status)
    350		return -EINVAL;
    351
    352	ctrl = amp_ctrl_add(mgr, rsp->id);
    353	if (!ctrl)
    354		return -ENOMEM;
    355
    356	memset(&req, 0, sizeof(req));
    357
    358	req.id = rsp->id;
    359	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
    360		  &req);
    361
    362	skb_pull(skb, sizeof(*rsp));
    363	return 0;
    364}
    365
    366static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
    367				struct a2mp_cmd *hdr)
    368{
    369	struct a2mp_amp_assoc_req *req = (void *) skb->data;
    370	struct hci_dev *hdev;
    371	struct amp_mgr *tmp;
    372
    373	if (le16_to_cpu(hdr->len) < sizeof(*req))
    374		return -EINVAL;
    375
    376	BT_DBG("id %u", req->id);
    377
    378	/* Make sure that other request is not processed */
    379	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
    380
    381	hdev = hci_dev_get(req->id);
    382	if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) {
    383		struct a2mp_amp_assoc_rsp rsp;
    384
    385		memset(&rsp, 0, sizeof(rsp));
    386		rsp.id = req->id;
    387
    388		if (tmp) {
    389			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
    390			amp_mgr_put(tmp);
    391		} else {
    392			rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
    393		}
    394
    395		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
    396			  &rsp);
    397
    398		goto done;
    399	}
    400
    401	amp_read_loc_assoc(hdev, mgr);
    402
    403done:
    404	if (hdev)
    405		hci_dev_put(hdev);
    406
    407	skb_pull(skb, sizeof(*req));
    408	return 0;
    409}
    410
    411static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
    412				struct a2mp_cmd *hdr)
    413{
    414	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
    415	u16 len = le16_to_cpu(hdr->len);
    416	struct hci_dev *hdev;
    417	struct amp_ctrl *ctrl;
    418	struct hci_conn *hcon;
    419	size_t assoc_len;
    420
    421	if (len < sizeof(*rsp))
    422		return -EINVAL;
    423
    424	assoc_len = len - sizeof(*rsp);
    425
    426	BT_DBG("id %u status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
    427	       assoc_len);
    428
    429	if (rsp->status)
    430		return -EINVAL;
    431
    432	/* Save remote ASSOC data */
    433	ctrl = amp_ctrl_lookup(mgr, rsp->id);
    434	if (ctrl) {
    435		u8 *assoc;
    436
    437		assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
    438		if (!assoc) {
    439			amp_ctrl_put(ctrl);
    440			return -ENOMEM;
    441		}
    442
    443		ctrl->assoc = assoc;
    444		ctrl->assoc_len = assoc_len;
    445		ctrl->assoc_rem_len = assoc_len;
    446		ctrl->assoc_len_so_far = 0;
    447
    448		amp_ctrl_put(ctrl);
    449	}
    450
    451	/* Create Phys Link */
    452	hdev = hci_dev_get(rsp->id);
    453	if (!hdev)
    454		return -EINVAL;
    455
    456	hcon = phylink_add(hdev, mgr, rsp->id, true);
    457	if (!hcon)
    458		goto done;
    459
    460	BT_DBG("Created hcon %p: loc:%u -> rem:%u", hcon, hdev->id, rsp->id);
    461
    462	mgr->bredr_chan->remote_amp_id = rsp->id;
    463
    464	amp_create_phylink(hdev, mgr, hcon);
    465
    466done:
    467	hci_dev_put(hdev);
    468	skb_pull(skb, len);
    469	return 0;
    470}
    471
    472static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
    473				   struct a2mp_cmd *hdr)
    474{
    475	struct a2mp_physlink_req *req = (void *) skb->data;
    476	struct a2mp_physlink_rsp rsp;
    477	struct hci_dev *hdev;
    478	struct hci_conn *hcon;
    479	struct amp_ctrl *ctrl;
    480
    481	if (le16_to_cpu(hdr->len) < sizeof(*req))
    482		return -EINVAL;
    483
    484	BT_DBG("local_id %u, remote_id %u", req->local_id, req->remote_id);
    485
    486	memset(&rsp, 0, sizeof(rsp));
    487
    488	rsp.local_id = req->remote_id;
    489	rsp.remote_id = req->local_id;
    490
    491	hdev = hci_dev_get(req->remote_id);
    492	if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) {
    493		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
    494		goto send_rsp;
    495	}
    496
    497	ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
    498	if (!ctrl) {
    499		ctrl = amp_ctrl_add(mgr, rsp.remote_id);
    500		if (ctrl) {
    501			amp_ctrl_get(ctrl);
    502		} else {
    503			rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
    504			goto send_rsp;
    505		}
    506	}
    507
    508	if (ctrl) {
    509		size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
    510		u8 *assoc;
    511
    512		assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
    513		if (!assoc) {
    514			amp_ctrl_put(ctrl);
    515			hci_dev_put(hdev);
    516			return -ENOMEM;
    517		}
    518
    519		ctrl->assoc = assoc;
    520		ctrl->assoc_len = assoc_len;
    521		ctrl->assoc_rem_len = assoc_len;
    522		ctrl->assoc_len_so_far = 0;
    523
    524		amp_ctrl_put(ctrl);
    525	}
    526
    527	hcon = phylink_add(hdev, mgr, req->local_id, false);
    528	if (hcon) {
    529		amp_accept_phylink(hdev, mgr, hcon);
    530		rsp.status = A2MP_STATUS_SUCCESS;
    531	} else {
    532		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
    533	}
    534
    535send_rsp:
    536	if (hdev)
    537		hci_dev_put(hdev);
    538
    539	/* Reply error now and success after HCI Write Remote AMP Assoc
    540	   command complete with success status
    541	 */
    542	if (rsp.status != A2MP_STATUS_SUCCESS) {
    543		a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
    544			  sizeof(rsp), &rsp);
    545	} else {
    546		set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
    547		mgr->ident = hdr->ident;
    548	}
    549
    550	skb_pull(skb, le16_to_cpu(hdr->len));
    551	return 0;
    552}
    553
    554static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
    555				 struct a2mp_cmd *hdr)
    556{
    557	struct a2mp_physlink_req *req = (void *) skb->data;
    558	struct a2mp_physlink_rsp rsp;
    559	struct hci_dev *hdev;
    560	struct hci_conn *hcon;
    561
    562	if (le16_to_cpu(hdr->len) < sizeof(*req))
    563		return -EINVAL;
    564
    565	BT_DBG("local_id %u remote_id %u", req->local_id, req->remote_id);
    566
    567	memset(&rsp, 0, sizeof(rsp));
    568
    569	rsp.local_id = req->remote_id;
    570	rsp.remote_id = req->local_id;
    571	rsp.status = A2MP_STATUS_SUCCESS;
    572
    573	hdev = hci_dev_get(req->remote_id);
    574	if (!hdev) {
    575		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
    576		goto send_rsp;
    577	}
    578
    579	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
    580				       &mgr->l2cap_conn->hcon->dst);
    581	if (!hcon) {
    582		bt_dev_err(hdev, "no phys link exist");
    583		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
    584		goto clean;
    585	}
    586
    587	/* TODO Disconnect Phys Link here */
    588
    589clean:
    590	hci_dev_put(hdev);
    591
    592send_rsp:
    593	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
    594
    595	skb_pull(skb, sizeof(*req));
    596	return 0;
    597}
    598
    599static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
    600			       struct a2mp_cmd *hdr)
    601{
    602	BT_DBG("ident %u code 0x%2.2x", hdr->ident, hdr->code);
    603
    604	skb_pull(skb, le16_to_cpu(hdr->len));
    605	return 0;
    606}
    607
    608/* Handle A2MP signalling */
    609static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
    610{
    611	struct a2mp_cmd *hdr;
    612	struct amp_mgr *mgr = chan->data;
    613	int err = 0;
    614
    615	amp_mgr_get(mgr);
    616
    617	while (skb->len >= sizeof(*hdr)) {
    618		u16 len;
    619
    620		hdr = (void *) skb->data;
    621		len = le16_to_cpu(hdr->len);
    622
    623		BT_DBG("code 0x%2.2x id %u len %u", hdr->code, hdr->ident, len);
    624
    625		skb_pull(skb, sizeof(*hdr));
    626
    627		if (len > skb->len || !hdr->ident) {
    628			err = -EINVAL;
    629			break;
    630		}
    631
    632		mgr->ident = hdr->ident;
    633
    634		switch (hdr->code) {
    635		case A2MP_COMMAND_REJ:
    636			a2mp_command_rej(mgr, skb, hdr);
    637			break;
    638
    639		case A2MP_DISCOVER_REQ:
    640			err = a2mp_discover_req(mgr, skb, hdr);
    641			break;
    642
    643		case A2MP_CHANGE_NOTIFY:
    644			err = a2mp_change_notify(mgr, skb, hdr);
    645			break;
    646
    647		case A2MP_GETINFO_REQ:
    648			err = a2mp_getinfo_req(mgr, skb, hdr);
    649			break;
    650
    651		case A2MP_GETAMPASSOC_REQ:
    652			err = a2mp_getampassoc_req(mgr, skb, hdr);
    653			break;
    654
    655		case A2MP_CREATEPHYSLINK_REQ:
    656			err = a2mp_createphyslink_req(mgr, skb, hdr);
    657			break;
    658
    659		case A2MP_DISCONNPHYSLINK_REQ:
    660			err = a2mp_discphyslink_req(mgr, skb, hdr);
    661			break;
    662
    663		case A2MP_DISCOVER_RSP:
    664			err = a2mp_discover_rsp(mgr, skb, hdr);
    665			break;
    666
    667		case A2MP_GETINFO_RSP:
    668			err = a2mp_getinfo_rsp(mgr, skb, hdr);
    669			break;
    670
    671		case A2MP_GETAMPASSOC_RSP:
    672			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
    673			break;
    674
    675		case A2MP_CHANGE_RSP:
    676		case A2MP_CREATEPHYSLINK_RSP:
    677		case A2MP_DISCONNPHYSLINK_RSP:
    678			err = a2mp_cmd_rsp(mgr, skb, hdr);
    679			break;
    680
    681		default:
    682			BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
    683			err = -EINVAL;
    684			break;
    685		}
    686	}
    687
    688	if (err) {
    689		struct a2mp_cmd_rej rej;
    690
    691		memset(&rej, 0, sizeof(rej));
    692
    693		rej.reason = cpu_to_le16(0);
    694		hdr = (void *) skb->data;
    695
    696		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
    697
    698		a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
    699			  &rej);
    700	}
    701
    702	/* Always free skb and return success error code to prevent
    703	   from sending L2CAP Disconnect over A2MP channel */
    704	kfree_skb(skb);
    705
    706	amp_mgr_put(mgr);
    707
    708	return 0;
    709}
    710
    711static void a2mp_chan_close_cb(struct l2cap_chan *chan)
    712{
    713	l2cap_chan_put(chan);
    714}
    715
    716static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state,
    717				      int err)
    718{
    719	struct amp_mgr *mgr = chan->data;
    720
    721	if (!mgr)
    722		return;
    723
    724	BT_DBG("chan %p state %s", chan, state_to_string(state));
    725
    726	chan->state = state;
    727
    728	switch (state) {
    729	case BT_CLOSED:
    730		if (mgr)
    731			amp_mgr_put(mgr);
    732		break;
    733	}
    734}
    735
    736static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
    737					      unsigned long hdr_len,
    738					      unsigned long len, int nb)
    739{
    740	struct sk_buff *skb;
    741
    742	skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL);
    743	if (!skb)
    744		return ERR_PTR(-ENOMEM);
    745
    746	return skb;
    747}
    748
    749static const struct l2cap_ops a2mp_chan_ops = {
    750	.name = "L2CAP A2MP channel",
    751	.recv = a2mp_chan_recv_cb,
    752	.close = a2mp_chan_close_cb,
    753	.state_change = a2mp_chan_state_change_cb,
    754	.alloc_skb = a2mp_chan_alloc_skb_cb,
    755
    756	/* Not implemented for A2MP */
    757	.new_connection = l2cap_chan_no_new_connection,
    758	.teardown = l2cap_chan_no_teardown,
    759	.ready = l2cap_chan_no_ready,
    760	.defer = l2cap_chan_no_defer,
    761	.resume = l2cap_chan_no_resume,
    762	.set_shutdown = l2cap_chan_no_set_shutdown,
    763	.get_sndtimeo = l2cap_chan_no_get_sndtimeo,
    764};
    765
    766static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
    767{
    768	struct l2cap_chan *chan;
    769	int err;
    770
    771	chan = l2cap_chan_create();
    772	if (!chan)
    773		return NULL;
    774
    775	BT_DBG("chan %p", chan);
    776
    777	chan->chan_type = L2CAP_CHAN_FIXED;
    778	chan->scid = L2CAP_CID_A2MP;
    779	chan->dcid = L2CAP_CID_A2MP;
    780	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
    781	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
    782	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
    783
    784	chan->ops = &a2mp_chan_ops;
    785
    786	l2cap_chan_set_defaults(chan);
    787	chan->remote_max_tx = chan->max_tx;
    788	chan->remote_tx_win = chan->tx_win;
    789
    790	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
    791	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
    792
    793	skb_queue_head_init(&chan->tx_q);
    794
    795	chan->mode = L2CAP_MODE_ERTM;
    796
    797	err = l2cap_ertm_init(chan);
    798	if (err < 0) {
    799		l2cap_chan_del(chan, 0);
    800		return NULL;
    801	}
    802
    803	chan->conf_state = 0;
    804
    805	if (locked)
    806		__l2cap_chan_add(conn, chan);
    807	else
    808		l2cap_chan_add(conn, chan);
    809
    810	chan->remote_mps = chan->omtu;
    811	chan->mps = chan->omtu;
    812
    813	chan->state = BT_CONNECTED;
    814
    815	return chan;
    816}
    817
    818/* AMP Manager functions */
    819struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
    820{
    821	BT_DBG("mgr %p orig refcnt %d", mgr, kref_read(&mgr->kref));
    822
    823	kref_get(&mgr->kref);
    824
    825	return mgr;
    826}
    827
    828static void amp_mgr_destroy(struct kref *kref)
    829{
    830	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
    831
    832	BT_DBG("mgr %p", mgr);
    833
    834	mutex_lock(&amp_mgr_list_lock);
    835	list_del(&mgr->list);
    836	mutex_unlock(&amp_mgr_list_lock);
    837
    838	amp_ctrl_list_flush(mgr);
    839	kfree(mgr);
    840}
    841
    842int amp_mgr_put(struct amp_mgr *mgr)
    843{
    844	BT_DBG("mgr %p orig refcnt %d", mgr, kref_read(&mgr->kref));
    845
    846	return kref_put(&mgr->kref, &amp_mgr_destroy);
    847}
    848
    849static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
    850{
    851	struct amp_mgr *mgr;
    852	struct l2cap_chan *chan;
    853
    854	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
    855	if (!mgr)
    856		return NULL;
    857
    858	BT_DBG("conn %p mgr %p", conn, mgr);
    859
    860	mgr->l2cap_conn = conn;
    861
    862	chan = a2mp_chan_open(conn, locked);
    863	if (!chan) {
    864		kfree(mgr);
    865		return NULL;
    866	}
    867
    868	mgr->a2mp_chan = chan;
    869	chan->data = mgr;
    870
    871	conn->hcon->amp_mgr = mgr;
    872
    873	kref_init(&mgr->kref);
    874
    875	/* Remote AMP ctrl list initialization */
    876	INIT_LIST_HEAD(&mgr->amp_ctrls);
    877	mutex_init(&mgr->amp_ctrls_lock);
    878
    879	mutex_lock(&amp_mgr_list_lock);
    880	list_add(&mgr->list, &amp_mgr_list);
    881	mutex_unlock(&amp_mgr_list_lock);
    882
    883	return mgr;
    884}
    885
    886struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
    887				       struct sk_buff *skb)
    888{
    889	struct amp_mgr *mgr;
    890
    891	if (conn->hcon->type != ACL_LINK)
    892		return NULL;
    893
    894	mgr = amp_mgr_create(conn, false);
    895	if (!mgr) {
    896		BT_ERR("Could not create AMP manager");
    897		return NULL;
    898	}
    899
    900	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
    901
    902	return mgr->a2mp_chan;
    903}
    904
    905void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
    906{
    907	struct amp_mgr *mgr;
    908	struct a2mp_info_rsp rsp;
    909
    910	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
    911	if (!mgr)
    912		return;
    913
    914	BT_DBG("%s mgr %p", hdev->name, mgr);
    915
    916	memset(&rsp, 0, sizeof(rsp));
    917
    918	rsp.id = hdev->id;
    919	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
    920
    921	if (hdev->amp_type != AMP_TYPE_BREDR) {
    922		rsp.status = 0;
    923		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
    924		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
    925		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
    926		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
    927		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
    928	}
    929
    930	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
    931	amp_mgr_put(mgr);
    932}
    933
    934void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
    935{
    936	struct amp_mgr *mgr;
    937	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
    938	struct a2mp_amp_assoc_rsp *rsp;
    939	size_t len;
    940
    941	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
    942	if (!mgr)
    943		return;
    944
    945	BT_DBG("%s mgr %p", hdev->name, mgr);
    946
    947	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
    948	rsp = kzalloc(len, GFP_KERNEL);
    949	if (!rsp) {
    950		amp_mgr_put(mgr);
    951		return;
    952	}
    953
    954	rsp->id = hdev->id;
    955
    956	if (status) {
    957		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
    958	} else {
    959		rsp->status = A2MP_STATUS_SUCCESS;
    960		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
    961	}
    962
    963	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
    964	amp_mgr_put(mgr);
    965	kfree(rsp);
    966}
    967
    968void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
    969{
    970	struct amp_mgr *mgr;
    971	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
    972	struct a2mp_physlink_req *req;
    973	struct l2cap_chan *bredr_chan;
    974	size_t len;
    975
    976	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
    977	if (!mgr)
    978		return;
    979
    980	len = sizeof(*req) + loc_assoc->len;
    981
    982	BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
    983
    984	req = kzalloc(len, GFP_KERNEL);
    985	if (!req) {
    986		amp_mgr_put(mgr);
    987		return;
    988	}
    989
    990	bredr_chan = mgr->bredr_chan;
    991	if (!bredr_chan)
    992		goto clean;
    993
    994	req->local_id = hdev->id;
    995	req->remote_id = bredr_chan->remote_amp_id;
    996	memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
    997
    998	a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
    999
   1000clean:
   1001	amp_mgr_put(mgr);
   1002	kfree(req);
   1003}
   1004
   1005void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
   1006{
   1007	struct amp_mgr *mgr;
   1008	struct a2mp_physlink_rsp rsp;
   1009	struct hci_conn *hs_hcon;
   1010
   1011	mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
   1012	if (!mgr)
   1013		return;
   1014
   1015	memset(&rsp, 0, sizeof(rsp));
   1016
   1017	hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
   1018	if (!hs_hcon) {
   1019		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
   1020	} else {
   1021		rsp.remote_id = hs_hcon->remote_id;
   1022		rsp.status = A2MP_STATUS_SUCCESS;
   1023	}
   1024
   1025	BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
   1026	       status);
   1027
   1028	rsp.local_id = hdev->id;
   1029	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
   1030	amp_mgr_put(mgr);
   1031}
   1032
   1033void a2mp_discover_amp(struct l2cap_chan *chan)
   1034{
   1035	struct l2cap_conn *conn = chan->conn;
   1036	struct amp_mgr *mgr = conn->hcon->amp_mgr;
   1037	struct a2mp_discov_req req;
   1038
   1039	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
   1040
   1041	if (!mgr) {
   1042		mgr = amp_mgr_create(conn, true);
   1043		if (!mgr)
   1044			return;
   1045	}
   1046
   1047	mgr->bredr_chan = chan;
   1048
   1049	memset(&req, 0, sizeof(req));
   1050
   1051	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
   1052	req.ext_feat = 0;
   1053	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
   1054}