a2mp.c 21.2 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
/*
   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
   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 <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
18
#include <net/bluetooth/a2mp.h>
19
#include <net/bluetooth/amp.h>
20

21 22 23 24
/* Global AMP Manager list */
LIST_HEAD(amp_mgr_list);
DEFINE_MUTEX(amp_mgr_list_lock);

25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44
/* A2MP build & send command helper functions */
static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
{
	struct a2mp_cmd *cmd;
	int plen;

	plen = sizeof(*cmd) + len;
	cmd = kzalloc(plen, GFP_KERNEL);
	if (!cmd)
		return NULL;

	cmd->code = code;
	cmd->ident = ident;
	cmd->len = cpu_to_le16(len);

	memcpy(cmd->data, data, len);

	return cmd;
}

45
void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
{
	struct l2cap_chan *chan = mgr->a2mp_chan;
	struct a2mp_cmd *cmd;
	u16 total_len = len + sizeof(*cmd);
	struct kvec iv;
	struct msghdr msg;

	cmd = __a2mp_build(code, ident, len, data);
	if (!cmd)
		return;

	iv.iov_base = cmd;
	iv.iov_len = total_len;

	memset(&msg, 0, sizeof(msg));

	msg.msg_iov = (struct iovec *) &iv;
	msg.msg_iovlen = 1;

	l2cap_chan_send(chan, &msg, total_len, 0);

	kfree(cmd);
}

70
u8 __next_ident(struct amp_mgr *mgr)
71 72 73 74 75 76 77
{
	if (++mgr->ident == 0)
		mgr->ident = 1;

	return mgr->ident;
}

78 79
static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
{
80
	cl->id = AMP_ID_BREDR;
81
	cl->type = AMP_TYPE_BREDR;
82
	cl->status = AMP_STATUS_BLUETOOTH_ONLY;
83 84 85 86 87 88 89 90 91 92 93 94
}

/* 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 */
95
		if (hdev->dev_type != HCI_AMP)
96 97 98 99 100 101 102 103 104 105 106 107
			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;
	}
}

108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
/* Processing A2MP messages */
static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
			    struct a2mp_cmd *hdr)
{
	struct a2mp_cmd_rej *rej = (void *) skb->data;

	if (le16_to_cpu(hdr->len) < sizeof(*rej))
		return -EINVAL;

	BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));

	skb_pull(skb, sizeof(*rej));

	return 0;
}

124 125 126 127 128 129 130 131
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;
132
	struct hci_dev *hdev;
133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155

	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);

156 157 158 159 160 161 162 163
	/* at minimum the BR/EDR needs to be listed */
	num_ctrl = 1;

	list_for_each_entry(hdev, &hci_dev_list, list) {
		if (hdev->dev_type == HCI_AMP)
			num_ctrl++;
	}

164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183
	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;
}

184 185 186 187 188 189 190
static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
			     struct a2mp_cmd *hdr)
{
	struct a2mp_discov_rsp *rsp = (void *) skb->data;
	u16 len = le16_to_cpu(hdr->len);
	struct a2mp_cl *cl;
	u16 ext_feat;
191
	bool found = false;
192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218

	if (len < sizeof(*rsp))
		return -EINVAL;

	len -= sizeof(*rsp);
	skb_pull(skb, sizeof(*rsp));

	ext_feat = le16_to_cpu(rsp->ext_feat);

	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->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));
	}

	cl = (void *) skb->data;
	while (len >= sizeof(*cl)) {
		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
		       cl->status);

219
		if (cl->id != AMP_ID_BREDR && cl->type == HCI_AMP) {
220 221
			struct a2mp_info_req req;

222
			found = true;
223 224 225 226 227 228 229 230 231
			req.id = cl->id;
			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
				  sizeof(req), &req);
		}

		len -= sizeof(*cl);
		cl = (void *) skb_pull(skb, sizeof(*cl));
	}

232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257
	/* Fall back to L2CAP init sequence */
	if (!found) {
		struct l2cap_conn *conn = mgr->l2cap_conn;
		struct l2cap_chan *chan;

		mutex_lock(&conn->chan_lock);

		list_for_each_entry(chan, &conn->chan_l, list) {

			BT_DBG("chan %p state %s", chan,
			       state_to_string(chan->state));

			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
				continue;

			l2cap_chan_lock(chan);

			if (chan->state == BT_CONNECT)
				l2cap_send_conn_req(chan);

			l2cap_chan_unlock(chan);
		}

		mutex_unlock(&conn->chan_lock);
	}

258 259 260
	return 0;
}

261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276
static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
			      struct a2mp_cmd *hdr)
{
	struct a2mp_cl *cl = (void *) skb->data;

	while (skb->len >= sizeof(*cl)) {
		BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
		       cl->status);
		cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
	}

	/* TODO send A2MP_CHANGE_RSP */

	return 0;
}

277 278 279 280 281 282 283 284 285 286 287 288
static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
			    struct a2mp_cmd *hdr)
{
	struct a2mp_info_req *req  = (void *) skb->data;
	struct hci_dev *hdev;

	if (le16_to_cpu(hdr->len) < sizeof(*req))
		return -EINVAL;

	BT_DBG("id %d", req->id);

	hdev = hci_dev_get(req->id);
289
	if (!hdev || hdev->dev_type != HCI_AMP) {
290 291 292 293 294 295 296
		struct a2mp_info_rsp rsp;

		rsp.id = req->id;
		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;

		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
			  &rsp);
297

298
		goto done;
299
	}
300

301
	set_bit(READ_LOC_AMP_INFO, &mgr->state);
302 303 304 305 306
	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);

done:
	if (hdev)
		hci_dev_put(hdev);
307 308 309 310 311

	skb_pull(skb, sizeof(*req));
	return 0;
}

312 313 314 315 316 317 318 319 320 321 322 323 324 325 326
static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
			    struct a2mp_cmd *hdr)
{
	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
	struct a2mp_amp_assoc_req req;
	struct amp_ctrl *ctrl;

	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
		return -EINVAL;

	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);

	if (rsp->status)
		return -EINVAL;

327
	ctrl = amp_ctrl_add(mgr, rsp->id);
328 329 330 331 332 333 334 335 336 337 338
	if (!ctrl)
		return -ENOMEM;

	req.id = rsp->id;
	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
		  &req);

	skb_pull(skb, sizeof(*rsp));
	return 0;
}

339 340 341 342 343
static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
				struct a2mp_cmd *hdr)
{
	struct a2mp_amp_assoc_req *req = (void *) skb->data;
	struct hci_dev *hdev;
344
	struct amp_mgr *tmp;
345 346 347 348 349 350

	if (le16_to_cpu(hdr->len) < sizeof(*req))
		return -EINVAL;

	BT_DBG("id %d", req->id);

351 352 353
	/* Make sure that other request is not processed */
	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);

354
	hdev = hci_dev_get(req->id);
355
	if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) {
356 357
		struct a2mp_amp_assoc_rsp rsp;
		rsp.id = req->id;
358 359 360 361 362 363 364

		if (tmp) {
			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
			amp_mgr_put(tmp);
		} else {
			rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
		}
365 366 367

		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
			  &rsp);
368 369

		goto done;
370 371
	}

372
	amp_read_loc_assoc(hdev, mgr);
373

374
done:
375 376 377 378 379 380 381
	if (hdev)
		hci_dev_put(hdev);

	skb_pull(skb, sizeof(*req));
	return 0;
}

382 383 384 385 386 387 388 389
static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
				struct a2mp_cmd *hdr)
{
	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
	u16 len = le16_to_cpu(hdr->len);
	struct hci_dev *hdev;
	struct amp_ctrl *ctrl;
	struct hci_conn *hcon;
390
	size_t assoc_len;
391 392 393 394

	if (len < sizeof(*rsp))
		return -EINVAL;

395 396 397 398
	assoc_len = len - sizeof(*rsp);

	BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
	       assoc_len);
399 400 401 402 403 404 405

	if (rsp->status)
		return -EINVAL;

	/* Save remote ASSOC data */
	ctrl = amp_ctrl_lookup(mgr, rsp->id);
	if (ctrl) {
406
		u8 *assoc;
407

408
		assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426
		if (!assoc) {
			amp_ctrl_put(ctrl);
			return -ENOMEM;
		}

		ctrl->assoc = assoc;
		ctrl->assoc_len = assoc_len;
		ctrl->assoc_rem_len = assoc_len;
		ctrl->assoc_len_so_far = 0;

		amp_ctrl_put(ctrl);
	}

	/* Create Phys Link */
	hdev = hci_dev_get(rsp->id);
	if (!hdev)
		return -EINVAL;

427
	hcon = phylink_add(hdev, mgr, rsp->id, true);
428 429 430 431 432
	if (!hcon)
		goto done;

	BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);

433
	mgr->bredr_chan->remote_amp_id = rsp->id;
434

435 436
	amp_create_phylink(hdev, mgr, hcon);

437 438 439 440 441 442
done:
	hci_dev_put(hdev);
	skb_pull(skb, len);
	return 0;
}

443 444 445 446 447 448 449
static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
				   struct a2mp_cmd *hdr)
{
	struct a2mp_physlink_req *req = (void *) skb->data;

	struct a2mp_physlink_rsp rsp;
	struct hci_dev *hdev;
450
	struct hci_conn *hcon;
451
	struct amp_ctrl *ctrl;
452 453 454 455 456 457 458 459 460 461

	if (le16_to_cpu(hdr->len) < sizeof(*req))
		return -EINVAL;

	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);

	rsp.local_id = req->remote_id;
	rsp.remote_id = req->local_id;

	hdev = hci_dev_get(req->remote_id);
462
	if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) {
463 464 465 466
		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
		goto send_rsp;
	}

467 468
	ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
	if (!ctrl) {
469
		ctrl = amp_ctrl_add(mgr, rsp.remote_id);
470 471 472 473 474 475 476 477 478
		if (ctrl) {
			amp_ctrl_get(ctrl);
		} else {
			rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
			goto send_rsp;
		}
	}

	if (ctrl) {
479 480
		size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
		u8 *assoc;
481

482
		assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
483 484 485 486 487 488 489 490 491 492 493 494 495
		if (!assoc) {
			amp_ctrl_put(ctrl);
			return -ENOMEM;
		}

		ctrl->assoc = assoc;
		ctrl->assoc_len = assoc_len;
		ctrl->assoc_rem_len = assoc_len;
		ctrl->assoc_len_so_far = 0;

		amp_ctrl_put(ctrl);
	}

496
	hcon = phylink_add(hdev, mgr, req->local_id, false);
497
	if (hcon) {
498
		amp_accept_phylink(hdev, mgr, hcon);
499 500 501 502
		rsp.status = A2MP_STATUS_SUCCESS;
	} else {
		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
	}
503 504 505 506 507

send_rsp:
	if (hdev)
		hci_dev_put(hdev);

508 509 510 511 512 513 514
	/* Reply error now and success after HCI Write Remote AMP Assoc
	   command complete with success status
	 */
	if (rsp.status != A2MP_STATUS_SUCCESS) {
		a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
			  sizeof(rsp), &rsp);
	} else {
515
		set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
516 517
		mgr->ident = hdr->ident;
	}
518 519 520 521 522

	skb_pull(skb, le16_to_cpu(hdr->len));
	return 0;
}

523 524 525 526 527 528
static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
				 struct a2mp_cmd *hdr)
{
	struct a2mp_physlink_req *req = (void *) skb->data;
	struct a2mp_physlink_rsp rsp;
	struct hci_dev *hdev;
529
	struct hci_conn *hcon;
530 531 532 533 534 535 536 537 538 539

	if (le16_to_cpu(hdr->len) < sizeof(*req))
		return -EINVAL;

	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);

	rsp.local_id = req->remote_id;
	rsp.remote_id = req->local_id;
	rsp.status = A2MP_STATUS_SUCCESS;

540
	hdev = hci_dev_get(req->remote_id);
541 542 543 544 545
	if (!hdev) {
		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
		goto send_rsp;
	}

546 547 548 549 550 551 552
	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
	if (!hcon) {
		BT_ERR("No phys link exist");
		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
		goto clean;
	}

553 554
	/* TODO Disconnect Phys Link here */

555
clean:
556 557 558 559 560 561 562 563 564
	hci_dev_put(hdev);

send_rsp:
	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);

	skb_pull(skb, sizeof(*req));
	return 0;
}

565 566 567
static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
			       struct a2mp_cmd *hdr)
{
568
	BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
569 570 571 572 573

	skb_pull(skb, le16_to_cpu(hdr->len));
	return 0;
}

574 575 576
/* Handle A2MP signalling */
static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
{
577
	struct a2mp_cmd *hdr;
578 579 580 581 582 583
	struct amp_mgr *mgr = chan->data;
	int err = 0;

	amp_mgr_get(mgr);

	while (skb->len >= sizeof(*hdr)) {
584 585 586 587
		u16 len;

		hdr = (void *) skb->data;
		len = le16_to_cpu(hdr->len);
588

589
		BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
590 591 592 593 594 595 596 597 598 599 600 601

		skb_pull(skb, sizeof(*hdr));

		if (len > skb->len || !hdr->ident) {
			err = -EINVAL;
			break;
		}

		mgr->ident = hdr->ident;

		switch (hdr->code) {
		case A2MP_COMMAND_REJ:
602 603 604
			a2mp_command_rej(mgr, skb, hdr);
			break;

605
		case A2MP_DISCOVER_REQ:
606 607 608
			err = a2mp_discover_req(mgr, skb, hdr);
			break;

609
		case A2MP_CHANGE_NOTIFY:
610 611 612
			err = a2mp_change_notify(mgr, skb, hdr);
			break;

613
		case A2MP_GETINFO_REQ:
614 615 616
			err = a2mp_getinfo_req(mgr, skb, hdr);
			break;

617
		case A2MP_GETAMPASSOC_REQ:
618 619 620
			err = a2mp_getampassoc_req(mgr, skb, hdr);
			break;

621
		case A2MP_CREATEPHYSLINK_REQ:
622 623 624
			err = a2mp_createphyslink_req(mgr, skb, hdr);
			break;

625
		case A2MP_DISCONNPHYSLINK_REQ:
626 627 628
			err = a2mp_discphyslink_req(mgr, skb, hdr);
			break;

629
		case A2MP_DISCOVER_RSP:
630 631 632
			err = a2mp_discover_rsp(mgr, skb, hdr);
			break;

633
		case A2MP_GETINFO_RSP:
634 635 636
			err = a2mp_getinfo_rsp(mgr, skb, hdr);
			break;

637
		case A2MP_GETAMPASSOC_RSP:
638 639 640 641
			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
			break;

		case A2MP_CHANGE_RSP:
642 643
		case A2MP_CREATEPHYSLINK_RSP:
		case A2MP_DISCONNPHYSLINK_RSP:
644 645 646
			err = a2mp_cmd_rsp(mgr, skb, hdr);
			break;

647 648 649 650 651 652 653 654 655
		default:
			BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
			err = -EINVAL;
			break;
		}
	}

	if (err) {
		struct a2mp_cmd_rej rej;
656

657
		rej.reason = __constant_cpu_to_le16(0);
658
		hdr = (void *) skb->data;
659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674

		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);

		a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
			  &rej);
	}

	/* Always free skb and return success error code to prevent
	   from sending L2CAP Disconnect over A2MP channel */
	kfree_skb(skb);

	amp_mgr_put(mgr);

	return 0;
}

675 676
static void a2mp_chan_close_cb(struct l2cap_chan *chan)
{
677
	l2cap_chan_put(chan);
678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704
}

static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
{
	struct amp_mgr *mgr = chan->data;

	if (!mgr)
		return;

	BT_DBG("chan %p state %s", chan, state_to_string(state));

	chan->state = state;

	switch (state) {
	case BT_CLOSED:
		if (mgr)
			amp_mgr_put(mgr);
		break;
	}
}

static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
					      unsigned long len, int nb)
{
	return bt_skb_alloc(len, GFP_KERNEL);
}

705 706
static struct l2cap_ops a2mp_chan_ops = {
	.name = "L2CAP A2MP channel",
707
	.recv = a2mp_chan_recv_cb,
708 709 710 711 712
	.close = a2mp_chan_close_cb,
	.state_change = a2mp_chan_state_change_cb,
	.alloc_skb = a2mp_chan_alloc_skb_cb,

	/* Not implemented for A2MP */
713 714 715
	.new_connection = l2cap_chan_no_new_connection,
	.teardown = l2cap_chan_no_teardown,
	.ready = l2cap_chan_no_ready,
716
	.defer = l2cap_chan_no_defer,
717 718
};

719
static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
720 721 722 723 724 725 726 727 728 729
{
	struct l2cap_chan *chan;
	int err;

	chan = l2cap_chan_create();
	if (!chan)
		return NULL;

	BT_DBG("chan %p", chan);

730
	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753
	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;

	chan->ops = &a2mp_chan_ops;

	l2cap_chan_set_defaults(chan);
	chan->remote_max_tx = chan->max_tx;
	chan->remote_tx_win = chan->tx_win;

	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;

	skb_queue_head_init(&chan->tx_q);

	chan->mode = L2CAP_MODE_ERTM;

	err = l2cap_ertm_init(chan);
	if (err < 0) {
		l2cap_chan_del(chan, 0);
		return NULL;
	}

	chan->conf_state = 0;

754 755 756 757
	if (locked)
		__l2cap_chan_add(conn, chan);
	else
		l2cap_chan_add(conn, chan);
758 759 760 761 762 763 764 765

	chan->remote_mps = chan->omtu;
	chan->mps = chan->omtu;

	chan->state = BT_CONNECTED;

	return chan;
}
766 767

/* AMP Manager functions */
768
struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
769
{
770
	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
771 772

	kref_get(&mgr->kref);
773 774

	return mgr;
775 776 777 778 779 780 781 782
}

static void amp_mgr_destroy(struct kref *kref)
{
	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);

	BT_DBG("mgr %p", mgr);

783 784 785 786
	mutex_lock(&amp_mgr_list_lock);
	list_del(&mgr->list);
	mutex_unlock(&amp_mgr_list_lock);

787
	amp_ctrl_list_flush(mgr);
788 789 790 791 792
	kfree(mgr);
}

int amp_mgr_put(struct amp_mgr *mgr)
{
793
	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
794 795 796 797

	return kref_put(&mgr->kref, &amp_mgr_destroy);
}

798
static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
799 800 801 802 803 804 805 806 807 808 809 810
{
	struct amp_mgr *mgr;
	struct l2cap_chan *chan;

	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
	if (!mgr)
		return NULL;

	BT_DBG("conn %p mgr %p", conn, mgr);

	mgr->l2cap_conn = conn;

811
	chan = a2mp_chan_open(conn, locked);
812 813 814 815 816 817 818 819 820 821 822 823
	if (!chan) {
		kfree(mgr);
		return NULL;
	}

	mgr->a2mp_chan = chan;
	chan->data = mgr;

	conn->hcon->amp_mgr = mgr;

	kref_init(&mgr->kref);

824 825 826 827
	/* Remote AMP ctrl list initialization */
	INIT_LIST_HEAD(&mgr->amp_ctrls);
	mutex_init(&mgr->amp_ctrls_lock);

828 829 830 831
	mutex_lock(&amp_mgr_list_lock);
	list_add(&mgr->list, &amp_mgr_list);
	mutex_unlock(&amp_mgr_list_lock);

832 833
	return mgr;
}
834 835 836 837 838 839

struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
				       struct sk_buff *skb)
{
	struct amp_mgr *mgr;

840
	mgr = amp_mgr_create(conn, false);
841 842 843 844 845 846 847 848 849
	if (!mgr) {
		BT_ERR("Could not create AMP manager");
		return NULL;
	}

	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);

	return mgr->a2mp_chan;
}
850 851 852 853 854 855 856

struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
{
	struct amp_mgr *mgr;

	mutex_lock(&amp_mgr_list_lock);
	list_for_each_entry(mgr, &amp_mgr_list, list) {
857
		if (test_and_clear_bit(state, &mgr->state)) {
858 859 860 861 862 863 864 865 866
			amp_mgr_get(mgr);
			mutex_unlock(&amp_mgr_list_lock);
			return mgr;
		}
	}
	mutex_unlock(&amp_mgr_list_lock);

	return NULL;
}
867 868 869 870 871 872 873 874 875 876 877 878 879 880 881

void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
{
	struct amp_mgr *mgr;
	struct a2mp_info_rsp rsp;

	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
	if (!mgr)
		return;

	BT_DBG("%s mgr %p", hdev->name, mgr);

	rsp.id = hdev->id;
	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;

882
	if (hdev->amp_type != AMP_TYPE_BREDR) {
883 884 885 886 887 888 889 890 891 892 893
		rsp.status = 0;
		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
	}

	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
	amp_mgr_put(mgr);
}
894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927

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);
}
928

929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955
void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
{
	struct amp_mgr *mgr;
	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
	struct a2mp_physlink_req *req;
	struct l2cap_chan *bredr_chan;
	size_t len;

	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
	if (!mgr)
		return;

	len = sizeof(*req) + loc_assoc->len;

	BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);

	req = kzalloc(len, GFP_KERNEL);
	if (!req) {
		amp_mgr_put(mgr);
		return;
	}

	bredr_chan = mgr->bredr_chan;
	if (!bredr_chan)
		goto clean;

	req->local_id = hdev->id;
956
	req->remote_id = bredr_chan->remote_amp_id;
957 958 959 960 961 962 963 964 965
	memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);

	a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);

clean:
	amp_mgr_put(mgr);
	kfree(req);
}

966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991
void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
{
	struct amp_mgr *mgr;
	struct a2mp_physlink_rsp rsp;
	struct hci_conn *hs_hcon;

	mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
	if (!mgr)
		return;

	hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
	if (!hs_hcon) {
		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
	} else {
		rsp.remote_id = hs_hcon->remote_id;
		rsp.status = A2MP_STATUS_SUCCESS;
	}

	BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
	       status);

	rsp.local_id = hdev->id;
	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
	amp_mgr_put(mgr);
}

992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011
void a2mp_discover_amp(struct l2cap_chan *chan)
{
	struct l2cap_conn *conn = chan->conn;
	struct amp_mgr *mgr = conn->hcon->amp_mgr;
	struct a2mp_discov_req req;

	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);

	if (!mgr) {
		mgr = amp_mgr_create(conn, true);
		if (!mgr)
			return;
	}

	mgr->bredr_chan = chan;

	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
	req.ext_feat = 0;
	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
}