a2mp.c 22 KB

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