llcp_core.c 34 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (C) 2011 Intel Corporation. All rights reserved.
  4. * Copyright (C) 2014 Marvell International Ltd.
  5. */
  6. #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
  7. #include <linux/init.h>
  8. #include <linux/kernel.h>
  9. #include <linux/list.h>
  10. #include <linux/nfc.h>
  11. #include "nfc.h"
  12. #include "llcp.h"
  13. static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
  14. static LIST_HEAD(llcp_devices);
  15. static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
  16. void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
  17. {
  18. write_lock(&l->lock);
  19. sk_add_node(sk, &l->head);
  20. write_unlock(&l->lock);
  21. }
  22. void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
  23. {
  24. write_lock(&l->lock);
  25. sk_del_node_init(sk);
  26. write_unlock(&l->lock);
  27. }
  28. void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
  29. {
  30. sock->remote_rw = LLCP_DEFAULT_RW;
  31. sock->remote_miu = LLCP_MAX_MIU + 1;
  32. }
  33. static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
  34. {
  35. struct nfc_llcp_local *local = sock->local;
  36. struct sk_buff *s, *tmp;
  37. pr_debug("%p\n", &sock->sk);
  38. skb_queue_purge(&sock->tx_queue);
  39. skb_queue_purge(&sock->tx_pending_queue);
  40. if (local == NULL)
  41. return;
  42. /* Search for local pending SKBs that are related to this socket */
  43. skb_queue_walk_safe(&local->tx_queue, s, tmp) {
  44. if (s->sk != &sock->sk)
  45. continue;
  46. skb_unlink(s, &local->tx_queue);
  47. kfree_skb(s);
  48. }
  49. }
  50. static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
  51. int err)
  52. {
  53. struct sock *sk;
  54. struct hlist_node *tmp;
  55. struct nfc_llcp_sock *llcp_sock;
  56. skb_queue_purge(&local->tx_queue);
  57. write_lock(&local->sockets.lock);
  58. sk_for_each_safe(sk, tmp, &local->sockets.head) {
  59. llcp_sock = nfc_llcp_sock(sk);
  60. bh_lock_sock(sk);
  61. nfc_llcp_socket_purge(llcp_sock);
  62. if (sk->sk_state == LLCP_CONNECTED)
  63. nfc_put_device(llcp_sock->dev);
  64. if (sk->sk_state == LLCP_LISTEN) {
  65. struct nfc_llcp_sock *lsk, *n;
  66. struct sock *accept_sk;
  67. list_for_each_entry_safe(lsk, n,
  68. &llcp_sock->accept_queue,
  69. accept_queue) {
  70. accept_sk = &lsk->sk;
  71. bh_lock_sock(accept_sk);
  72. nfc_llcp_accept_unlink(accept_sk);
  73. if (err)
  74. accept_sk->sk_err = err;
  75. accept_sk->sk_state = LLCP_CLOSED;
  76. accept_sk->sk_state_change(sk);
  77. bh_unlock_sock(accept_sk);
  78. }
  79. }
  80. if (err)
  81. sk->sk_err = err;
  82. sk->sk_state = LLCP_CLOSED;
  83. sk->sk_state_change(sk);
  84. bh_unlock_sock(sk);
  85. sk_del_node_init(sk);
  86. }
  87. write_unlock(&local->sockets.lock);
  88. /* If we still have a device, we keep the RAW sockets alive */
  89. if (device == true)
  90. return;
  91. write_lock(&local->raw_sockets.lock);
  92. sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
  93. llcp_sock = nfc_llcp_sock(sk);
  94. bh_lock_sock(sk);
  95. nfc_llcp_socket_purge(llcp_sock);
  96. if (err)
  97. sk->sk_err = err;
  98. sk->sk_state = LLCP_CLOSED;
  99. sk->sk_state_change(sk);
  100. bh_unlock_sock(sk);
  101. sk_del_node_init(sk);
  102. }
  103. write_unlock(&local->raw_sockets.lock);
  104. }
  105. struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
  106. {
  107. kref_get(&local->ref);
  108. return local;
  109. }
  110. static void local_cleanup(struct nfc_llcp_local *local)
  111. {
  112. nfc_llcp_socket_release(local, false, ENXIO);
  113. del_timer_sync(&local->link_timer);
  114. skb_queue_purge(&local->tx_queue);
  115. cancel_work_sync(&local->tx_work);
  116. cancel_work_sync(&local->rx_work);
  117. cancel_work_sync(&local->timeout_work);
  118. kfree_skb(local->rx_pending);
  119. del_timer_sync(&local->sdreq_timer);
  120. cancel_work_sync(&local->sdreq_timeout_work);
  121. nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
  122. }
  123. static void local_release(struct kref *ref)
  124. {
  125. struct nfc_llcp_local *local;
  126. local = container_of(ref, struct nfc_llcp_local, ref);
  127. list_del(&local->list);
  128. local_cleanup(local);
  129. kfree(local);
  130. }
  131. int nfc_llcp_local_put(struct nfc_llcp_local *local)
  132. {
  133. if (local == NULL)
  134. return 0;
  135. return kref_put(&local->ref, local_release);
  136. }
  137. static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
  138. u8 ssap, u8 dsap)
  139. {
  140. struct sock *sk;
  141. struct nfc_llcp_sock *llcp_sock, *tmp_sock;
  142. pr_debug("ssap dsap %d %d\n", ssap, dsap);
  143. if (ssap == 0 && dsap == 0)
  144. return NULL;
  145. read_lock(&local->sockets.lock);
  146. llcp_sock = NULL;
  147. sk_for_each(sk, &local->sockets.head) {
  148. tmp_sock = nfc_llcp_sock(sk);
  149. if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
  150. llcp_sock = tmp_sock;
  151. break;
  152. }
  153. }
  154. read_unlock(&local->sockets.lock);
  155. if (llcp_sock == NULL)
  156. return NULL;
  157. sock_hold(&llcp_sock->sk);
  158. return llcp_sock;
  159. }
  160. static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
  161. {
  162. sock_put(&sock->sk);
  163. }
  164. static void nfc_llcp_timeout_work(struct work_struct *work)
  165. {
  166. struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
  167. timeout_work);
  168. nfc_dep_link_down(local->dev);
  169. }
  170. static void nfc_llcp_symm_timer(struct timer_list *t)
  171. {
  172. struct nfc_llcp_local *local = from_timer(local, t, link_timer);
  173. pr_err("SYMM timeout\n");
  174. schedule_work(&local->timeout_work);
  175. }
  176. static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
  177. {
  178. unsigned long time;
  179. HLIST_HEAD(nl_sdres_list);
  180. struct hlist_node *n;
  181. struct nfc_llcp_sdp_tlv *sdp;
  182. struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
  183. sdreq_timeout_work);
  184. mutex_lock(&local->sdreq_lock);
  185. time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
  186. hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
  187. if (time_after(sdp->time, time))
  188. continue;
  189. sdp->sap = LLCP_SDP_UNBOUND;
  190. hlist_del(&sdp->node);
  191. hlist_add_head(&sdp->node, &nl_sdres_list);
  192. }
  193. if (!hlist_empty(&local->pending_sdreqs))
  194. mod_timer(&local->sdreq_timer,
  195. jiffies + msecs_to_jiffies(3 * local->remote_lto));
  196. mutex_unlock(&local->sdreq_lock);
  197. if (!hlist_empty(&nl_sdres_list))
  198. nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
  199. }
  200. static void nfc_llcp_sdreq_timer(struct timer_list *t)
  201. {
  202. struct nfc_llcp_local *local = from_timer(local, t, sdreq_timer);
  203. schedule_work(&local->sdreq_timeout_work);
  204. }
  205. struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
  206. {
  207. struct nfc_llcp_local *local;
  208. list_for_each_entry(local, &llcp_devices, list)
  209. if (local->dev == dev)
  210. return local;
  211. pr_debug("No device found\n");
  212. return NULL;
  213. }
  214. static char *wks[] = {
  215. NULL,
  216. NULL, /* SDP */
  217. "urn:nfc:sn:ip",
  218. "urn:nfc:sn:obex",
  219. "urn:nfc:sn:snep",
  220. };
  221. static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
  222. {
  223. int sap, num_wks;
  224. pr_debug("%s\n", service_name);
  225. if (service_name == NULL)
  226. return -EINVAL;
  227. num_wks = ARRAY_SIZE(wks);
  228. for (sap = 0; sap < num_wks; sap++) {
  229. if (wks[sap] == NULL)
  230. continue;
  231. if (strncmp(wks[sap], service_name, service_name_len) == 0)
  232. return sap;
  233. }
  234. return -EINVAL;
  235. }
  236. static
  237. struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
  238. u8 *sn, size_t sn_len)
  239. {
  240. struct sock *sk;
  241. struct nfc_llcp_sock *llcp_sock, *tmp_sock;
  242. pr_debug("sn %zd %p\n", sn_len, sn);
  243. if (sn == NULL || sn_len == 0)
  244. return NULL;
  245. read_lock(&local->sockets.lock);
  246. llcp_sock = NULL;
  247. sk_for_each(sk, &local->sockets.head) {
  248. tmp_sock = nfc_llcp_sock(sk);
  249. pr_debug("llcp sock %p\n", tmp_sock);
  250. if (tmp_sock->sk.sk_type == SOCK_STREAM &&
  251. tmp_sock->sk.sk_state != LLCP_LISTEN)
  252. continue;
  253. if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
  254. tmp_sock->sk.sk_state != LLCP_BOUND)
  255. continue;
  256. if (tmp_sock->service_name == NULL ||
  257. tmp_sock->service_name_len == 0)
  258. continue;
  259. if (tmp_sock->service_name_len != sn_len)
  260. continue;
  261. if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
  262. llcp_sock = tmp_sock;
  263. break;
  264. }
  265. }
  266. read_unlock(&local->sockets.lock);
  267. pr_debug("Found llcp sock %p\n", llcp_sock);
  268. return llcp_sock;
  269. }
  270. u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
  271. struct nfc_llcp_sock *sock)
  272. {
  273. mutex_lock(&local->sdp_lock);
  274. if (sock->service_name != NULL && sock->service_name_len > 0) {
  275. int ssap = nfc_llcp_wks_sap(sock->service_name,
  276. sock->service_name_len);
  277. if (ssap > 0) {
  278. pr_debug("WKS %d\n", ssap);
  279. /* This is a WKS, let's check if it's free */
  280. if (local->local_wks & BIT(ssap)) {
  281. mutex_unlock(&local->sdp_lock);
  282. return LLCP_SAP_MAX;
  283. }
  284. set_bit(ssap, &local->local_wks);
  285. mutex_unlock(&local->sdp_lock);
  286. return ssap;
  287. }
  288. /*
  289. * Check if there already is a non WKS socket bound
  290. * to this service name.
  291. */
  292. if (nfc_llcp_sock_from_sn(local, sock->service_name,
  293. sock->service_name_len) != NULL) {
  294. mutex_unlock(&local->sdp_lock);
  295. return LLCP_SAP_MAX;
  296. }
  297. mutex_unlock(&local->sdp_lock);
  298. return LLCP_SDP_UNBOUND;
  299. } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
  300. if (!test_bit(sock->ssap, &local->local_wks)) {
  301. set_bit(sock->ssap, &local->local_wks);
  302. mutex_unlock(&local->sdp_lock);
  303. return sock->ssap;
  304. }
  305. }
  306. mutex_unlock(&local->sdp_lock);
  307. return LLCP_SAP_MAX;
  308. }
  309. u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
  310. {
  311. u8 local_ssap;
  312. mutex_lock(&local->sdp_lock);
  313. local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
  314. if (local_ssap == LLCP_LOCAL_NUM_SAP) {
  315. mutex_unlock(&local->sdp_lock);
  316. return LLCP_SAP_MAX;
  317. }
  318. set_bit(local_ssap, &local->local_sap);
  319. mutex_unlock(&local->sdp_lock);
  320. return local_ssap + LLCP_LOCAL_SAP_OFFSET;
  321. }
  322. void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
  323. {
  324. u8 local_ssap;
  325. unsigned long *sdp;
  326. if (ssap < LLCP_WKS_NUM_SAP) {
  327. local_ssap = ssap;
  328. sdp = &local->local_wks;
  329. } else if (ssap < LLCP_LOCAL_NUM_SAP) {
  330. atomic_t *client_cnt;
  331. local_ssap = ssap - LLCP_WKS_NUM_SAP;
  332. sdp = &local->local_sdp;
  333. client_cnt = &local->local_sdp_cnt[local_ssap];
  334. pr_debug("%d clients\n", atomic_read(client_cnt));
  335. mutex_lock(&local->sdp_lock);
  336. if (atomic_dec_and_test(client_cnt)) {
  337. struct nfc_llcp_sock *l_sock;
  338. pr_debug("No more clients for SAP %d\n", ssap);
  339. clear_bit(local_ssap, sdp);
  340. /* Find the listening sock and set it back to UNBOUND */
  341. l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
  342. if (l_sock) {
  343. l_sock->ssap = LLCP_SDP_UNBOUND;
  344. nfc_llcp_sock_put(l_sock);
  345. }
  346. }
  347. mutex_unlock(&local->sdp_lock);
  348. return;
  349. } else if (ssap < LLCP_MAX_SAP) {
  350. local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
  351. sdp = &local->local_sap;
  352. } else {
  353. return;
  354. }
  355. mutex_lock(&local->sdp_lock);
  356. clear_bit(local_ssap, sdp);
  357. mutex_unlock(&local->sdp_lock);
  358. }
  359. static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
  360. {
  361. u8 ssap;
  362. mutex_lock(&local->sdp_lock);
  363. ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
  364. if (ssap == LLCP_SDP_NUM_SAP) {
  365. mutex_unlock(&local->sdp_lock);
  366. return LLCP_SAP_MAX;
  367. }
  368. pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
  369. set_bit(ssap, &local->local_sdp);
  370. mutex_unlock(&local->sdp_lock);
  371. return LLCP_WKS_NUM_SAP + ssap;
  372. }
  373. static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
  374. {
  375. u8 *gb_cur, version, version_length;
  376. u8 lto_length, wks_length, miux_length;
  377. u8 *version_tlv = NULL, *lto_tlv = NULL,
  378. *wks_tlv = NULL, *miux_tlv = NULL;
  379. __be16 wks = cpu_to_be16(local->local_wks);
  380. u8 gb_len = 0;
  381. int ret = 0;
  382. version = LLCP_VERSION_11;
  383. version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
  384. 1, &version_length);
  385. if (!version_tlv) {
  386. ret = -ENOMEM;
  387. goto out;
  388. }
  389. gb_len += version_length;
  390. lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
  391. if (!lto_tlv) {
  392. ret = -ENOMEM;
  393. goto out;
  394. }
  395. gb_len += lto_length;
  396. pr_debug("Local wks 0x%lx\n", local->local_wks);
  397. wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&wks, 2, &wks_length);
  398. if (!wks_tlv) {
  399. ret = -ENOMEM;
  400. goto out;
  401. }
  402. gb_len += wks_length;
  403. miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
  404. &miux_length);
  405. if (!miux_tlv) {
  406. ret = -ENOMEM;
  407. goto out;
  408. }
  409. gb_len += miux_length;
  410. gb_len += ARRAY_SIZE(llcp_magic);
  411. if (gb_len > NFC_MAX_GT_LEN) {
  412. ret = -EINVAL;
  413. goto out;
  414. }
  415. gb_cur = local->gb;
  416. memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
  417. gb_cur += ARRAY_SIZE(llcp_magic);
  418. memcpy(gb_cur, version_tlv, version_length);
  419. gb_cur += version_length;
  420. memcpy(gb_cur, lto_tlv, lto_length);
  421. gb_cur += lto_length;
  422. memcpy(gb_cur, wks_tlv, wks_length);
  423. gb_cur += wks_length;
  424. memcpy(gb_cur, miux_tlv, miux_length);
  425. gb_cur += miux_length;
  426. local->gb_len = gb_len;
  427. out:
  428. kfree(version_tlv);
  429. kfree(lto_tlv);
  430. kfree(wks_tlv);
  431. kfree(miux_tlv);
  432. return ret;
  433. }
  434. u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
  435. {
  436. struct nfc_llcp_local *local;
  437. local = nfc_llcp_find_local(dev);
  438. if (local == NULL) {
  439. *general_bytes_len = 0;
  440. return NULL;
  441. }
  442. nfc_llcp_build_gb(local);
  443. *general_bytes_len = local->gb_len;
  444. return local->gb;
  445. }
  446. int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
  447. {
  448. struct nfc_llcp_local *local;
  449. if (gb_len < 3 || gb_len > NFC_MAX_GT_LEN)
  450. return -EINVAL;
  451. local = nfc_llcp_find_local(dev);
  452. if (local == NULL) {
  453. pr_err("No LLCP device\n");
  454. return -ENODEV;
  455. }
  456. memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
  457. memcpy(local->remote_gb, gb, gb_len);
  458. local->remote_gb_len = gb_len;
  459. if (memcmp(local->remote_gb, llcp_magic, 3)) {
  460. pr_err("MAC does not support LLCP\n");
  461. return -EINVAL;
  462. }
  463. return nfc_llcp_parse_gb_tlv(local,
  464. &local->remote_gb[3],
  465. local->remote_gb_len - 3);
  466. }
  467. static u8 nfc_llcp_dsap(struct sk_buff *pdu)
  468. {
  469. return (pdu->data[0] & 0xfc) >> 2;
  470. }
  471. static u8 nfc_llcp_ptype(struct sk_buff *pdu)
  472. {
  473. return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
  474. }
  475. static u8 nfc_llcp_ssap(struct sk_buff *pdu)
  476. {
  477. return pdu->data[1] & 0x3f;
  478. }
  479. static u8 nfc_llcp_ns(struct sk_buff *pdu)
  480. {
  481. return pdu->data[2] >> 4;
  482. }
  483. static u8 nfc_llcp_nr(struct sk_buff *pdu)
  484. {
  485. return pdu->data[2] & 0xf;
  486. }
  487. static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
  488. {
  489. pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
  490. sock->send_n = (sock->send_n + 1) % 16;
  491. sock->recv_ack_n = (sock->recv_n - 1) % 16;
  492. }
  493. void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
  494. struct sk_buff *skb, u8 direction)
  495. {
  496. struct sk_buff *skb_copy = NULL, *nskb;
  497. struct sock *sk;
  498. u8 *data;
  499. read_lock(&local->raw_sockets.lock);
  500. sk_for_each(sk, &local->raw_sockets.head) {
  501. if (sk->sk_state != LLCP_BOUND)
  502. continue;
  503. if (skb_copy == NULL) {
  504. skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
  505. GFP_ATOMIC, true);
  506. if (skb_copy == NULL)
  507. continue;
  508. data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
  509. data[0] = local->dev ? local->dev->idx : 0xFF;
  510. data[1] = direction & 0x01;
  511. data[1] |= (RAW_PAYLOAD_LLCP << 1);
  512. }
  513. nskb = skb_clone(skb_copy, GFP_ATOMIC);
  514. if (!nskb)
  515. continue;
  516. if (sock_queue_rcv_skb(sk, nskb))
  517. kfree_skb(nskb);
  518. }
  519. read_unlock(&local->raw_sockets.lock);
  520. kfree_skb(skb_copy);
  521. }
  522. static void nfc_llcp_tx_work(struct work_struct *work)
  523. {
  524. struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
  525. tx_work);
  526. struct sk_buff *skb;
  527. struct sock *sk;
  528. struct nfc_llcp_sock *llcp_sock;
  529. skb = skb_dequeue(&local->tx_queue);
  530. if (skb != NULL) {
  531. sk = skb->sk;
  532. llcp_sock = nfc_llcp_sock(sk);
  533. if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
  534. kfree_skb(skb);
  535. nfc_llcp_send_symm(local->dev);
  536. } else if (llcp_sock && !llcp_sock->remote_ready) {
  537. skb_queue_head(&local->tx_queue, skb);
  538. nfc_llcp_send_symm(local->dev);
  539. } else {
  540. struct sk_buff *copy_skb = NULL;
  541. u8 ptype = nfc_llcp_ptype(skb);
  542. int ret;
  543. pr_debug("Sending pending skb\n");
  544. print_hex_dump_debug("LLCP Tx: ", DUMP_PREFIX_OFFSET,
  545. 16, 1, skb->data, skb->len, true);
  546. if (ptype == LLCP_PDU_DISC && sk != NULL &&
  547. sk->sk_state == LLCP_DISCONNECTING) {
  548. nfc_llcp_sock_unlink(&local->sockets, sk);
  549. sock_orphan(sk);
  550. sock_put(sk);
  551. }
  552. if (ptype == LLCP_PDU_I)
  553. copy_skb = skb_copy(skb, GFP_ATOMIC);
  554. __net_timestamp(skb);
  555. nfc_llcp_send_to_raw_sock(local, skb,
  556. NFC_DIRECTION_TX);
  557. ret = nfc_data_exchange(local->dev, local->target_idx,
  558. skb, nfc_llcp_recv, local);
  559. if (ret) {
  560. kfree_skb(copy_skb);
  561. goto out;
  562. }
  563. if (ptype == LLCP_PDU_I && copy_skb)
  564. skb_queue_tail(&llcp_sock->tx_pending_queue,
  565. copy_skb);
  566. }
  567. } else {
  568. nfc_llcp_send_symm(local->dev);
  569. }
  570. out:
  571. mod_timer(&local->link_timer,
  572. jiffies + msecs_to_jiffies(2 * local->remote_lto));
  573. }
  574. static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
  575. u8 ssap)
  576. {
  577. struct sock *sk;
  578. struct nfc_llcp_sock *llcp_sock;
  579. read_lock(&local->connecting_sockets.lock);
  580. sk_for_each(sk, &local->connecting_sockets.head) {
  581. llcp_sock = nfc_llcp_sock(sk);
  582. if (llcp_sock->ssap == ssap) {
  583. sock_hold(&llcp_sock->sk);
  584. goto out;
  585. }
  586. }
  587. llcp_sock = NULL;
  588. out:
  589. read_unlock(&local->connecting_sockets.lock);
  590. return llcp_sock;
  591. }
  592. static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
  593. u8 *sn, size_t sn_len)
  594. {
  595. struct nfc_llcp_sock *llcp_sock;
  596. llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
  597. if (llcp_sock == NULL)
  598. return NULL;
  599. sock_hold(&llcp_sock->sk);
  600. return llcp_sock;
  601. }
  602. static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
  603. {
  604. u8 *tlv = &skb->data[2], type, length;
  605. size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
  606. while (offset < tlv_array_len) {
  607. type = tlv[0];
  608. length = tlv[1];
  609. pr_debug("type 0x%x length %d\n", type, length);
  610. if (type == LLCP_TLV_SN) {
  611. *sn_len = length;
  612. return &tlv[2];
  613. }
  614. offset += length + 2;
  615. tlv += length + 2;
  616. }
  617. return NULL;
  618. }
  619. static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
  620. struct sk_buff *skb)
  621. {
  622. struct nfc_llcp_sock *llcp_sock;
  623. struct nfc_llcp_ui_cb *ui_cb;
  624. u8 dsap, ssap;
  625. dsap = nfc_llcp_dsap(skb);
  626. ssap = nfc_llcp_ssap(skb);
  627. ui_cb = nfc_llcp_ui_skb_cb(skb);
  628. ui_cb->dsap = dsap;
  629. ui_cb->ssap = ssap;
  630. pr_debug("%d %d\n", dsap, ssap);
  631. /* We're looking for a bound socket, not a client one */
  632. llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
  633. if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
  634. return;
  635. /* There is no sequence with UI frames */
  636. skb_pull(skb, LLCP_HEADER_SIZE);
  637. if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
  638. /*
  639. * UI frames will be freed from the socket layer, so we
  640. * need to keep them alive until someone receives them.
  641. */
  642. skb_get(skb);
  643. } else {
  644. pr_err("Receive queue is full\n");
  645. }
  646. nfc_llcp_sock_put(llcp_sock);
  647. }
  648. static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
  649. struct sk_buff *skb)
  650. {
  651. struct sock *new_sk, *parent;
  652. struct nfc_llcp_sock *sock, *new_sock;
  653. u8 dsap, ssap, reason;
  654. dsap = nfc_llcp_dsap(skb);
  655. ssap = nfc_llcp_ssap(skb);
  656. pr_debug("%d %d\n", dsap, ssap);
  657. if (dsap != LLCP_SAP_SDP) {
  658. sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
  659. if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
  660. reason = LLCP_DM_NOBOUND;
  661. goto fail;
  662. }
  663. } else {
  664. u8 *sn;
  665. size_t sn_len;
  666. sn = nfc_llcp_connect_sn(skb, &sn_len);
  667. if (sn == NULL) {
  668. reason = LLCP_DM_NOBOUND;
  669. goto fail;
  670. }
  671. pr_debug("Service name length %zu\n", sn_len);
  672. sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
  673. if (sock == NULL) {
  674. reason = LLCP_DM_NOBOUND;
  675. goto fail;
  676. }
  677. }
  678. lock_sock(&sock->sk);
  679. parent = &sock->sk;
  680. if (sk_acceptq_is_full(parent)) {
  681. reason = LLCP_DM_REJ;
  682. release_sock(&sock->sk);
  683. sock_put(&sock->sk);
  684. goto fail;
  685. }
  686. if (sock->ssap == LLCP_SDP_UNBOUND) {
  687. u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
  688. pr_debug("First client, reserving %d\n", ssap);
  689. if (ssap == LLCP_SAP_MAX) {
  690. reason = LLCP_DM_REJ;
  691. release_sock(&sock->sk);
  692. sock_put(&sock->sk);
  693. goto fail;
  694. }
  695. sock->ssap = ssap;
  696. }
  697. new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC, 0);
  698. if (new_sk == NULL) {
  699. reason = LLCP_DM_REJ;
  700. release_sock(&sock->sk);
  701. sock_put(&sock->sk);
  702. goto fail;
  703. }
  704. new_sock = nfc_llcp_sock(new_sk);
  705. new_sock->dev = local->dev;
  706. new_sock->local = nfc_llcp_local_get(local);
  707. new_sock->rw = sock->rw;
  708. new_sock->miux = sock->miux;
  709. new_sock->nfc_protocol = sock->nfc_protocol;
  710. new_sock->dsap = ssap;
  711. new_sock->target_idx = local->target_idx;
  712. new_sock->parent = parent;
  713. new_sock->ssap = sock->ssap;
  714. if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
  715. atomic_t *client_count;
  716. pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
  717. client_count =
  718. &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
  719. atomic_inc(client_count);
  720. new_sock->reserved_ssap = sock->ssap;
  721. }
  722. nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
  723. skb->len - LLCP_HEADER_SIZE);
  724. pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
  725. nfc_llcp_sock_link(&local->sockets, new_sk);
  726. nfc_llcp_accept_enqueue(&sock->sk, new_sk);
  727. nfc_get_device(local->dev->idx);
  728. new_sk->sk_state = LLCP_CONNECTED;
  729. /* Wake the listening processes */
  730. parent->sk_data_ready(parent);
  731. /* Send CC */
  732. nfc_llcp_send_cc(new_sock);
  733. release_sock(&sock->sk);
  734. sock_put(&sock->sk);
  735. return;
  736. fail:
  737. /* Send DM */
  738. nfc_llcp_send_dm(local, dsap, ssap, reason);
  739. }
  740. int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
  741. {
  742. int nr_frames = 0;
  743. struct nfc_llcp_local *local = sock->local;
  744. pr_debug("Remote ready %d tx queue len %d remote rw %d",
  745. sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
  746. sock->remote_rw);
  747. /* Try to queue some I frames for transmission */
  748. while (sock->remote_ready &&
  749. skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
  750. struct sk_buff *pdu;
  751. pdu = skb_dequeue(&sock->tx_queue);
  752. if (pdu == NULL)
  753. break;
  754. /* Update N(S)/N(R) */
  755. nfc_llcp_set_nrns(sock, pdu);
  756. skb_queue_tail(&local->tx_queue, pdu);
  757. nr_frames++;
  758. }
  759. return nr_frames;
  760. }
  761. static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
  762. struct sk_buff *skb)
  763. {
  764. struct nfc_llcp_sock *llcp_sock;
  765. struct sock *sk;
  766. u8 dsap, ssap, ptype, ns, nr;
  767. ptype = nfc_llcp_ptype(skb);
  768. dsap = nfc_llcp_dsap(skb);
  769. ssap = nfc_llcp_ssap(skb);
  770. ns = nfc_llcp_ns(skb);
  771. nr = nfc_llcp_nr(skb);
  772. pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
  773. llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
  774. if (llcp_sock == NULL) {
  775. nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
  776. return;
  777. }
  778. sk = &llcp_sock->sk;
  779. lock_sock(sk);
  780. if (sk->sk_state == LLCP_CLOSED) {
  781. release_sock(sk);
  782. nfc_llcp_sock_put(llcp_sock);
  783. }
  784. /* Pass the payload upstream */
  785. if (ptype == LLCP_PDU_I) {
  786. pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
  787. if (ns == llcp_sock->recv_n)
  788. llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
  789. else
  790. pr_err("Received out of sequence I PDU\n");
  791. skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
  792. if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
  793. /*
  794. * I frames will be freed from the socket layer, so we
  795. * need to keep them alive until someone receives them.
  796. */
  797. skb_get(skb);
  798. } else {
  799. pr_err("Receive queue is full\n");
  800. }
  801. }
  802. /* Remove skbs from the pending queue */
  803. if (llcp_sock->send_ack_n != nr) {
  804. struct sk_buff *s, *tmp;
  805. u8 n;
  806. llcp_sock->send_ack_n = nr;
  807. /* Remove and free all skbs until ns == nr */
  808. skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
  809. n = nfc_llcp_ns(s);
  810. skb_unlink(s, &llcp_sock->tx_pending_queue);
  811. kfree_skb(s);
  812. if (n == nr)
  813. break;
  814. }
  815. /* Re-queue the remaining skbs for transmission */
  816. skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
  817. s, tmp) {
  818. skb_unlink(s, &llcp_sock->tx_pending_queue);
  819. skb_queue_head(&local->tx_queue, s);
  820. }
  821. }
  822. if (ptype == LLCP_PDU_RR)
  823. llcp_sock->remote_ready = true;
  824. else if (ptype == LLCP_PDU_RNR)
  825. llcp_sock->remote_ready = false;
  826. if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
  827. nfc_llcp_send_rr(llcp_sock);
  828. release_sock(sk);
  829. nfc_llcp_sock_put(llcp_sock);
  830. }
  831. static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
  832. struct sk_buff *skb)
  833. {
  834. struct nfc_llcp_sock *llcp_sock;
  835. struct sock *sk;
  836. u8 dsap, ssap;
  837. dsap = nfc_llcp_dsap(skb);
  838. ssap = nfc_llcp_ssap(skb);
  839. if ((dsap == 0) && (ssap == 0)) {
  840. pr_debug("Connection termination");
  841. nfc_dep_link_down(local->dev);
  842. return;
  843. }
  844. llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
  845. if (llcp_sock == NULL) {
  846. nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
  847. return;
  848. }
  849. sk = &llcp_sock->sk;
  850. lock_sock(sk);
  851. nfc_llcp_socket_purge(llcp_sock);
  852. if (sk->sk_state == LLCP_CLOSED) {
  853. release_sock(sk);
  854. nfc_llcp_sock_put(llcp_sock);
  855. }
  856. if (sk->sk_state == LLCP_CONNECTED) {
  857. nfc_put_device(local->dev);
  858. sk->sk_state = LLCP_CLOSED;
  859. sk->sk_state_change(sk);
  860. }
  861. nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
  862. release_sock(sk);
  863. nfc_llcp_sock_put(llcp_sock);
  864. }
  865. static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
  866. {
  867. struct nfc_llcp_sock *llcp_sock;
  868. struct sock *sk;
  869. u8 dsap, ssap;
  870. dsap = nfc_llcp_dsap(skb);
  871. ssap = nfc_llcp_ssap(skb);
  872. llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
  873. if (llcp_sock == NULL) {
  874. pr_err("Invalid CC\n");
  875. nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
  876. return;
  877. }
  878. sk = &llcp_sock->sk;
  879. /* Unlink from connecting and link to the client array */
  880. nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
  881. nfc_llcp_sock_link(&local->sockets, sk);
  882. llcp_sock->dsap = ssap;
  883. nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
  884. skb->len - LLCP_HEADER_SIZE);
  885. sk->sk_state = LLCP_CONNECTED;
  886. sk->sk_state_change(sk);
  887. nfc_llcp_sock_put(llcp_sock);
  888. }
  889. static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
  890. {
  891. struct nfc_llcp_sock *llcp_sock;
  892. struct sock *sk;
  893. u8 dsap, ssap, reason;
  894. dsap = nfc_llcp_dsap(skb);
  895. ssap = nfc_llcp_ssap(skb);
  896. reason = skb->data[2];
  897. pr_debug("%d %d reason %d\n", ssap, dsap, reason);
  898. switch (reason) {
  899. case LLCP_DM_NOBOUND:
  900. case LLCP_DM_REJ:
  901. llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
  902. break;
  903. default:
  904. llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
  905. break;
  906. }
  907. if (llcp_sock == NULL) {
  908. pr_debug("Already closed\n");
  909. return;
  910. }
  911. sk = &llcp_sock->sk;
  912. sk->sk_err = ENXIO;
  913. sk->sk_state = LLCP_CLOSED;
  914. sk->sk_state_change(sk);
  915. nfc_llcp_sock_put(llcp_sock);
  916. }
  917. static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
  918. struct sk_buff *skb)
  919. {
  920. struct nfc_llcp_sock *llcp_sock;
  921. u8 dsap, ssap, *tlv, type, length, tid, sap;
  922. u16 tlv_len, offset;
  923. char *service_name;
  924. size_t service_name_len;
  925. struct nfc_llcp_sdp_tlv *sdp;
  926. HLIST_HEAD(llc_sdres_list);
  927. size_t sdres_tlvs_len;
  928. HLIST_HEAD(nl_sdres_list);
  929. dsap = nfc_llcp_dsap(skb);
  930. ssap = nfc_llcp_ssap(skb);
  931. pr_debug("%d %d\n", dsap, ssap);
  932. if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
  933. pr_err("Wrong SNL SAP\n");
  934. return;
  935. }
  936. tlv = &skb->data[LLCP_HEADER_SIZE];
  937. tlv_len = skb->len - LLCP_HEADER_SIZE;
  938. offset = 0;
  939. sdres_tlvs_len = 0;
  940. while (offset < tlv_len) {
  941. type = tlv[0];
  942. length = tlv[1];
  943. switch (type) {
  944. case LLCP_TLV_SDREQ:
  945. tid = tlv[2];
  946. service_name = (char *) &tlv[3];
  947. service_name_len = length - 1;
  948. pr_debug("Looking for %.16s\n", service_name);
  949. if (service_name_len == strlen("urn:nfc:sn:sdp") &&
  950. !strncmp(service_name, "urn:nfc:sn:sdp",
  951. service_name_len)) {
  952. sap = 1;
  953. goto add_snl;
  954. }
  955. llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
  956. service_name_len);
  957. if (!llcp_sock) {
  958. sap = 0;
  959. goto add_snl;
  960. }
  961. /*
  962. * We found a socket but its ssap has not been reserved
  963. * yet. We need to assign it for good and send a reply.
  964. * The ssap will be freed when the socket is closed.
  965. */
  966. if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
  967. atomic_t *client_count;
  968. sap = nfc_llcp_reserve_sdp_ssap(local);
  969. pr_debug("Reserving %d\n", sap);
  970. if (sap == LLCP_SAP_MAX) {
  971. sap = 0;
  972. goto add_snl;
  973. }
  974. client_count =
  975. &local->local_sdp_cnt[sap -
  976. LLCP_WKS_NUM_SAP];
  977. atomic_inc(client_count);
  978. llcp_sock->ssap = sap;
  979. llcp_sock->reserved_ssap = sap;
  980. } else {
  981. sap = llcp_sock->ssap;
  982. }
  983. pr_debug("%p %d\n", llcp_sock, sap);
  984. add_snl:
  985. sdp = nfc_llcp_build_sdres_tlv(tid, sap);
  986. if (sdp == NULL)
  987. goto exit;
  988. sdres_tlvs_len += sdp->tlv_len;
  989. hlist_add_head(&sdp->node, &llc_sdres_list);
  990. break;
  991. case LLCP_TLV_SDRES:
  992. mutex_lock(&local->sdreq_lock);
  993. pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
  994. hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
  995. if (sdp->tid != tlv[2])
  996. continue;
  997. sdp->sap = tlv[3];
  998. pr_debug("Found: uri=%s, sap=%d\n",
  999. sdp->uri, sdp->sap);
  1000. hlist_del(&sdp->node);
  1001. hlist_add_head(&sdp->node, &nl_sdres_list);
  1002. break;
  1003. }
  1004. mutex_unlock(&local->sdreq_lock);
  1005. break;
  1006. default:
  1007. pr_err("Invalid SNL tlv value 0x%x\n", type);
  1008. break;
  1009. }
  1010. offset += length + 2;
  1011. tlv += length + 2;
  1012. }
  1013. exit:
  1014. if (!hlist_empty(&nl_sdres_list))
  1015. nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
  1016. if (!hlist_empty(&llc_sdres_list))
  1017. nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
  1018. }
  1019. static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
  1020. {
  1021. u8 ptype;
  1022. u16 pdu_len;
  1023. struct sk_buff *new_skb;
  1024. if (skb->len <= LLCP_HEADER_SIZE) {
  1025. pr_err("Malformed AGF PDU\n");
  1026. return;
  1027. }
  1028. skb_pull(skb, LLCP_HEADER_SIZE);
  1029. while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
  1030. pdu_len = skb->data[0] << 8 | skb->data[1];
  1031. skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
  1032. if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
  1033. pr_err("Malformed AGF PDU\n");
  1034. return;
  1035. }
  1036. ptype = nfc_llcp_ptype(skb);
  1037. if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
  1038. goto next;
  1039. new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
  1040. if (new_skb == NULL) {
  1041. pr_err("Could not allocate PDU\n");
  1042. return;
  1043. }
  1044. skb_put_data(new_skb, skb->data, pdu_len);
  1045. nfc_llcp_rx_skb(local, new_skb);
  1046. kfree_skb(new_skb);
  1047. next:
  1048. skb_pull(skb, pdu_len);
  1049. }
  1050. }
  1051. static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
  1052. {
  1053. u8 dsap, ssap, ptype;
  1054. ptype = nfc_llcp_ptype(skb);
  1055. dsap = nfc_llcp_dsap(skb);
  1056. ssap = nfc_llcp_ssap(skb);
  1057. pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
  1058. if (ptype != LLCP_PDU_SYMM)
  1059. print_hex_dump_debug("LLCP Rx: ", DUMP_PREFIX_OFFSET, 16, 1,
  1060. skb->data, skb->len, true);
  1061. switch (ptype) {
  1062. case LLCP_PDU_SYMM:
  1063. pr_debug("SYMM\n");
  1064. break;
  1065. case LLCP_PDU_UI:
  1066. pr_debug("UI\n");
  1067. nfc_llcp_recv_ui(local, skb);
  1068. break;
  1069. case LLCP_PDU_CONNECT:
  1070. pr_debug("CONNECT\n");
  1071. nfc_llcp_recv_connect(local, skb);
  1072. break;
  1073. case LLCP_PDU_DISC:
  1074. pr_debug("DISC\n");
  1075. nfc_llcp_recv_disc(local, skb);
  1076. break;
  1077. case LLCP_PDU_CC:
  1078. pr_debug("CC\n");
  1079. nfc_llcp_recv_cc(local, skb);
  1080. break;
  1081. case LLCP_PDU_DM:
  1082. pr_debug("DM\n");
  1083. nfc_llcp_recv_dm(local, skb);
  1084. break;
  1085. case LLCP_PDU_SNL:
  1086. pr_debug("SNL\n");
  1087. nfc_llcp_recv_snl(local, skb);
  1088. break;
  1089. case LLCP_PDU_I:
  1090. case LLCP_PDU_RR:
  1091. case LLCP_PDU_RNR:
  1092. pr_debug("I frame\n");
  1093. nfc_llcp_recv_hdlc(local, skb);
  1094. break;
  1095. case LLCP_PDU_AGF:
  1096. pr_debug("AGF frame\n");
  1097. nfc_llcp_recv_agf(local, skb);
  1098. break;
  1099. }
  1100. }
  1101. static void nfc_llcp_rx_work(struct work_struct *work)
  1102. {
  1103. struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
  1104. rx_work);
  1105. struct sk_buff *skb;
  1106. skb = local->rx_pending;
  1107. if (skb == NULL) {
  1108. pr_debug("No pending SKB\n");
  1109. return;
  1110. }
  1111. __net_timestamp(skb);
  1112. nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_RX);
  1113. nfc_llcp_rx_skb(local, skb);
  1114. schedule_work(&local->tx_work);
  1115. kfree_skb(local->rx_pending);
  1116. local->rx_pending = NULL;
  1117. }
  1118. static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
  1119. {
  1120. local->rx_pending = skb;
  1121. del_timer(&local->link_timer);
  1122. schedule_work(&local->rx_work);
  1123. }
  1124. void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
  1125. {
  1126. struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
  1127. pr_debug("Received an LLCP PDU\n");
  1128. if (err < 0) {
  1129. pr_err("err %d\n", err);
  1130. return;
  1131. }
  1132. __nfc_llcp_recv(local, skb);
  1133. }
  1134. int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
  1135. {
  1136. struct nfc_llcp_local *local;
  1137. local = nfc_llcp_find_local(dev);
  1138. if (local == NULL) {
  1139. kfree_skb(skb);
  1140. return -ENODEV;
  1141. }
  1142. __nfc_llcp_recv(local, skb);
  1143. return 0;
  1144. }
  1145. void nfc_llcp_mac_is_down(struct nfc_dev *dev)
  1146. {
  1147. struct nfc_llcp_local *local;
  1148. local = nfc_llcp_find_local(dev);
  1149. if (local == NULL)
  1150. return;
  1151. local->remote_miu = LLCP_DEFAULT_MIU;
  1152. local->remote_lto = LLCP_DEFAULT_LTO;
  1153. /* Close and purge all existing sockets */
  1154. nfc_llcp_socket_release(local, true, 0);
  1155. }
  1156. void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
  1157. u8 comm_mode, u8 rf_mode)
  1158. {
  1159. struct nfc_llcp_local *local;
  1160. pr_debug("rf mode %d\n", rf_mode);
  1161. local = nfc_llcp_find_local(dev);
  1162. if (local == NULL)
  1163. return;
  1164. local->target_idx = target_idx;
  1165. local->comm_mode = comm_mode;
  1166. local->rf_mode = rf_mode;
  1167. if (rf_mode == NFC_RF_INITIATOR) {
  1168. pr_debug("Queueing Tx work\n");
  1169. schedule_work(&local->tx_work);
  1170. } else {
  1171. mod_timer(&local->link_timer,
  1172. jiffies + msecs_to_jiffies(local->remote_lto));
  1173. }
  1174. }
  1175. int nfc_llcp_register_device(struct nfc_dev *ndev)
  1176. {
  1177. struct nfc_llcp_local *local;
  1178. local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
  1179. if (local == NULL)
  1180. return -ENOMEM;
  1181. local->dev = ndev;
  1182. INIT_LIST_HEAD(&local->list);
  1183. kref_init(&local->ref);
  1184. mutex_init(&local->sdp_lock);
  1185. timer_setup(&local->link_timer, nfc_llcp_symm_timer, 0);
  1186. skb_queue_head_init(&local->tx_queue);
  1187. INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
  1188. local->rx_pending = NULL;
  1189. INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
  1190. INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
  1191. rwlock_init(&local->sockets.lock);
  1192. rwlock_init(&local->connecting_sockets.lock);
  1193. rwlock_init(&local->raw_sockets.lock);
  1194. local->lto = 150; /* 1500 ms */
  1195. local->rw = LLCP_MAX_RW;
  1196. local->miux = cpu_to_be16(LLCP_MAX_MIUX);
  1197. local->local_wks = 0x1; /* LLC Link Management */
  1198. nfc_llcp_build_gb(local);
  1199. local->remote_miu = LLCP_DEFAULT_MIU;
  1200. local->remote_lto = LLCP_DEFAULT_LTO;
  1201. mutex_init(&local->sdreq_lock);
  1202. INIT_HLIST_HEAD(&local->pending_sdreqs);
  1203. timer_setup(&local->sdreq_timer, nfc_llcp_sdreq_timer, 0);
  1204. INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
  1205. list_add(&local->list, &llcp_devices);
  1206. return 0;
  1207. }
  1208. void nfc_llcp_unregister_device(struct nfc_dev *dev)
  1209. {
  1210. struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
  1211. if (local == NULL) {
  1212. pr_debug("No such device\n");
  1213. return;
  1214. }
  1215. local_cleanup(local);
  1216. nfc_llcp_local_put(local);
  1217. }
  1218. int __init nfc_llcp_init(void)
  1219. {
  1220. return nfc_llcp_sock_init();
  1221. }
  1222. void nfc_llcp_exit(void)
  1223. {
  1224. nfc_llcp_sock_exit();
  1225. }