llcp.c 30 KB

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