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