llcp_sock.c 23 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (C) 2011 Intel Corporation. All rights reserved.
  4. */
  5. #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
  6. #include <linux/init.h>
  7. #include <linux/kernel.h>
  8. #include <linux/module.h>
  9. #include <linux/nfc.h>
  10. #include <linux/sched/signal.h>
  11. #include "nfc.h"
  12. #include "llcp.h"
  13. static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
  14. {
  15. DECLARE_WAITQUEUE(wait, current);
  16. int err = 0;
  17. pr_debug("sk %p", sk);
  18. add_wait_queue(sk_sleep(sk), &wait);
  19. set_current_state(TASK_INTERRUPTIBLE);
  20. while (sk->sk_state != state) {
  21. if (!timeo) {
  22. err = -EINPROGRESS;
  23. break;
  24. }
  25. if (signal_pending(current)) {
  26. err = sock_intr_errno(timeo);
  27. break;
  28. }
  29. release_sock(sk);
  30. timeo = schedule_timeout(timeo);
  31. lock_sock(sk);
  32. set_current_state(TASK_INTERRUPTIBLE);
  33. err = sock_error(sk);
  34. if (err)
  35. break;
  36. }
  37. __set_current_state(TASK_RUNNING);
  38. remove_wait_queue(sk_sleep(sk), &wait);
  39. return err;
  40. }
  41. static struct proto llcp_sock_proto = {
  42. .name = "NFC_LLCP",
  43. .owner = THIS_MODULE,
  44. .obj_size = sizeof(struct nfc_llcp_sock),
  45. };
  46. static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
  47. {
  48. struct sock *sk = sock->sk;
  49. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  50. struct nfc_llcp_local *local;
  51. struct nfc_dev *dev;
  52. struct sockaddr_nfc_llcp llcp_addr;
  53. int len, ret = 0;
  54. if (!addr || alen < offsetofend(struct sockaddr, sa_family) ||
  55. addr->sa_family != AF_NFC)
  56. return -EINVAL;
  57. pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
  58. memset(&llcp_addr, 0, sizeof(llcp_addr));
  59. len = min_t(unsigned int, sizeof(llcp_addr), alen);
  60. memcpy(&llcp_addr, addr, len);
  61. /* This is going to be a listening socket, dsap must be 0 */
  62. if (llcp_addr.dsap != 0)
  63. return -EINVAL;
  64. lock_sock(sk);
  65. if (sk->sk_state != LLCP_CLOSED) {
  66. ret = -EBADFD;
  67. goto error;
  68. }
  69. dev = nfc_get_device(llcp_addr.dev_idx);
  70. if (dev == NULL) {
  71. ret = -ENODEV;
  72. goto error;
  73. }
  74. local = nfc_llcp_find_local(dev);
  75. if (local == NULL) {
  76. ret = -ENODEV;
  77. goto put_dev;
  78. }
  79. llcp_sock->dev = dev;
  80. llcp_sock->local = nfc_llcp_local_get(local);
  81. llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
  82. llcp_sock->service_name_len = min_t(unsigned int,
  83. llcp_addr.service_name_len,
  84. NFC_LLCP_MAX_SERVICE_NAME);
  85. llcp_sock->service_name = kmemdup(llcp_addr.service_name,
  86. llcp_sock->service_name_len,
  87. GFP_KERNEL);
  88. if (!llcp_sock->service_name) {
  89. nfc_llcp_local_put(llcp_sock->local);
  90. llcp_sock->local = NULL;
  91. llcp_sock->dev = NULL;
  92. ret = -ENOMEM;
  93. goto put_dev;
  94. }
  95. llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
  96. if (llcp_sock->ssap == LLCP_SAP_MAX) {
  97. nfc_llcp_local_put(llcp_sock->local);
  98. llcp_sock->local = NULL;
  99. kfree(llcp_sock->service_name);
  100. llcp_sock->service_name = NULL;
  101. llcp_sock->dev = NULL;
  102. ret = -EADDRINUSE;
  103. goto put_dev;
  104. }
  105. llcp_sock->reserved_ssap = llcp_sock->ssap;
  106. nfc_llcp_sock_link(&local->sockets, sk);
  107. pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
  108. sk->sk_state = LLCP_BOUND;
  109. put_dev:
  110. nfc_put_device(dev);
  111. error:
  112. release_sock(sk);
  113. return ret;
  114. }
  115. static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
  116. int alen)
  117. {
  118. struct sock *sk = sock->sk;
  119. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  120. struct nfc_llcp_local *local;
  121. struct nfc_dev *dev;
  122. struct sockaddr_nfc_llcp llcp_addr;
  123. int len, ret = 0;
  124. if (!addr || alen < offsetofend(struct sockaddr, sa_family) ||
  125. addr->sa_family != AF_NFC)
  126. return -EINVAL;
  127. pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
  128. memset(&llcp_addr, 0, sizeof(llcp_addr));
  129. len = min_t(unsigned int, sizeof(llcp_addr), alen);
  130. memcpy(&llcp_addr, addr, len);
  131. lock_sock(sk);
  132. if (sk->sk_state != LLCP_CLOSED) {
  133. ret = -EBADFD;
  134. goto error;
  135. }
  136. dev = nfc_get_device(llcp_addr.dev_idx);
  137. if (dev == NULL) {
  138. ret = -ENODEV;
  139. goto error;
  140. }
  141. local = nfc_llcp_find_local(dev);
  142. if (local == NULL) {
  143. ret = -ENODEV;
  144. goto put_dev;
  145. }
  146. llcp_sock->dev = dev;
  147. llcp_sock->local = nfc_llcp_local_get(local);
  148. llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
  149. nfc_llcp_sock_link(&local->raw_sockets, sk);
  150. sk->sk_state = LLCP_BOUND;
  151. put_dev:
  152. nfc_put_device(dev);
  153. error:
  154. release_sock(sk);
  155. return ret;
  156. }
  157. static int llcp_sock_listen(struct socket *sock, int backlog)
  158. {
  159. struct sock *sk = sock->sk;
  160. int ret = 0;
  161. pr_debug("sk %p backlog %d\n", sk, backlog);
  162. lock_sock(sk);
  163. if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
  164. sk->sk_state != LLCP_BOUND) {
  165. ret = -EBADFD;
  166. goto error;
  167. }
  168. sk->sk_max_ack_backlog = backlog;
  169. sk->sk_ack_backlog = 0;
  170. pr_debug("Socket listening\n");
  171. sk->sk_state = LLCP_LISTEN;
  172. error:
  173. release_sock(sk);
  174. return ret;
  175. }
  176. static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
  177. sockptr_t optval, unsigned int optlen)
  178. {
  179. struct sock *sk = sock->sk;
  180. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  181. u32 opt;
  182. int err = 0;
  183. pr_debug("%p optname %d\n", sk, optname);
  184. if (level != SOL_NFC)
  185. return -ENOPROTOOPT;
  186. lock_sock(sk);
  187. switch (optname) {
  188. case NFC_LLCP_RW:
  189. if (sk->sk_state == LLCP_CONNECTED ||
  190. sk->sk_state == LLCP_BOUND ||
  191. sk->sk_state == LLCP_LISTEN) {
  192. err = -EINVAL;
  193. break;
  194. }
  195. if (copy_from_sockptr(&opt, optval, sizeof(u32))) {
  196. err = -EFAULT;
  197. break;
  198. }
  199. if (opt > LLCP_MAX_RW) {
  200. err = -EINVAL;
  201. break;
  202. }
  203. llcp_sock->rw = (u8) opt;
  204. break;
  205. case NFC_LLCP_MIUX:
  206. if (sk->sk_state == LLCP_CONNECTED ||
  207. sk->sk_state == LLCP_BOUND ||
  208. sk->sk_state == LLCP_LISTEN) {
  209. err = -EINVAL;
  210. break;
  211. }
  212. if (copy_from_sockptr(&opt, optval, sizeof(u32))) {
  213. err = -EFAULT;
  214. break;
  215. }
  216. if (opt > LLCP_MAX_MIUX) {
  217. err = -EINVAL;
  218. break;
  219. }
  220. llcp_sock->miux = cpu_to_be16((u16) opt);
  221. break;
  222. default:
  223. err = -ENOPROTOOPT;
  224. break;
  225. }
  226. release_sock(sk);
  227. pr_debug("%p rw %d miux %d\n", llcp_sock,
  228. llcp_sock->rw, llcp_sock->miux);
  229. return err;
  230. }
  231. static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
  232. char __user *optval, int __user *optlen)
  233. {
  234. struct nfc_llcp_local *local;
  235. struct sock *sk = sock->sk;
  236. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  237. int len, err = 0;
  238. u16 miux, remote_miu;
  239. u8 rw;
  240. pr_debug("%p optname %d\n", sk, optname);
  241. if (level != SOL_NFC)
  242. return -ENOPROTOOPT;
  243. if (get_user(len, optlen))
  244. return -EFAULT;
  245. local = llcp_sock->local;
  246. if (!local)
  247. return -ENODEV;
  248. len = min_t(u32, len, sizeof(u32));
  249. lock_sock(sk);
  250. switch (optname) {
  251. case NFC_LLCP_RW:
  252. rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
  253. if (put_user(rw, (u32 __user *) optval))
  254. err = -EFAULT;
  255. break;
  256. case NFC_LLCP_MIUX:
  257. miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
  258. be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
  259. if (put_user(miux, (u32 __user *) optval))
  260. err = -EFAULT;
  261. break;
  262. case NFC_LLCP_REMOTE_MIU:
  263. remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
  264. local->remote_miu : llcp_sock->remote_miu;
  265. if (put_user(remote_miu, (u32 __user *) optval))
  266. err = -EFAULT;
  267. break;
  268. case NFC_LLCP_REMOTE_LTO:
  269. if (put_user(local->remote_lto / 10, (u32 __user *) optval))
  270. err = -EFAULT;
  271. break;
  272. case NFC_LLCP_REMOTE_RW:
  273. if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
  274. err = -EFAULT;
  275. break;
  276. default:
  277. err = -ENOPROTOOPT;
  278. break;
  279. }
  280. release_sock(sk);
  281. if (put_user(len, optlen))
  282. return -EFAULT;
  283. return err;
  284. }
  285. void nfc_llcp_accept_unlink(struct sock *sk)
  286. {
  287. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  288. pr_debug("state %d\n", sk->sk_state);
  289. list_del_init(&llcp_sock->accept_queue);
  290. sk_acceptq_removed(llcp_sock->parent);
  291. llcp_sock->parent = NULL;
  292. sock_put(sk);
  293. }
  294. void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
  295. {
  296. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  297. struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
  298. /* Lock will be free from unlink */
  299. sock_hold(sk);
  300. list_add_tail(&llcp_sock->accept_queue,
  301. &llcp_sock_parent->accept_queue);
  302. llcp_sock->parent = parent;
  303. sk_acceptq_added(parent);
  304. }
  305. struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
  306. struct socket *newsock)
  307. {
  308. struct nfc_llcp_sock *lsk, *n, *llcp_parent;
  309. struct sock *sk;
  310. llcp_parent = nfc_llcp_sock(parent);
  311. list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
  312. accept_queue) {
  313. sk = &lsk->sk;
  314. lock_sock(sk);
  315. if (sk->sk_state == LLCP_CLOSED) {
  316. release_sock(sk);
  317. nfc_llcp_accept_unlink(sk);
  318. continue;
  319. }
  320. if (sk->sk_state == LLCP_CONNECTED || !newsock) {
  321. list_del_init(&lsk->accept_queue);
  322. sock_put(sk);
  323. if (newsock)
  324. sock_graft(sk, newsock);
  325. release_sock(sk);
  326. pr_debug("Returning sk state %d\n", sk->sk_state);
  327. sk_acceptq_removed(parent);
  328. return sk;
  329. }
  330. release_sock(sk);
  331. }
  332. return NULL;
  333. }
  334. static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
  335. int flags, bool kern)
  336. {
  337. DECLARE_WAITQUEUE(wait, current);
  338. struct sock *sk = sock->sk, *new_sk;
  339. long timeo;
  340. int ret = 0;
  341. pr_debug("parent %p\n", sk);
  342. lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
  343. if (sk->sk_state != LLCP_LISTEN) {
  344. ret = -EBADFD;
  345. goto error;
  346. }
  347. timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
  348. /* Wait for an incoming connection. */
  349. add_wait_queue_exclusive(sk_sleep(sk), &wait);
  350. while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
  351. set_current_state(TASK_INTERRUPTIBLE);
  352. if (!timeo) {
  353. ret = -EAGAIN;
  354. break;
  355. }
  356. if (signal_pending(current)) {
  357. ret = sock_intr_errno(timeo);
  358. break;
  359. }
  360. release_sock(sk);
  361. timeo = schedule_timeout(timeo);
  362. lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
  363. }
  364. __set_current_state(TASK_RUNNING);
  365. remove_wait_queue(sk_sleep(sk), &wait);
  366. if (ret)
  367. goto error;
  368. newsock->state = SS_CONNECTED;
  369. pr_debug("new socket %p\n", new_sk);
  370. error:
  371. release_sock(sk);
  372. return ret;
  373. }
  374. static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr,
  375. int peer)
  376. {
  377. struct sock *sk = sock->sk;
  378. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  379. DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr);
  380. if (llcp_sock == NULL || llcp_sock->dev == NULL)
  381. return -EBADFD;
  382. pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx,
  383. llcp_sock->dsap, llcp_sock->ssap);
  384. memset(llcp_addr, 0, sizeof(*llcp_addr));
  385. lock_sock(sk);
  386. if (!llcp_sock->dev) {
  387. release_sock(sk);
  388. return -EBADFD;
  389. }
  390. llcp_addr->sa_family = AF_NFC;
  391. llcp_addr->dev_idx = llcp_sock->dev->idx;
  392. llcp_addr->target_idx = llcp_sock->target_idx;
  393. llcp_addr->nfc_protocol = llcp_sock->nfc_protocol;
  394. llcp_addr->dsap = llcp_sock->dsap;
  395. llcp_addr->ssap = llcp_sock->ssap;
  396. llcp_addr->service_name_len = llcp_sock->service_name_len;
  397. memcpy(llcp_addr->service_name, llcp_sock->service_name,
  398. llcp_addr->service_name_len);
  399. release_sock(sk);
  400. return sizeof(struct sockaddr_nfc_llcp);
  401. }
  402. static inline __poll_t llcp_accept_poll(struct sock *parent)
  403. {
  404. struct nfc_llcp_sock *llcp_sock, *parent_sock;
  405. struct sock *sk;
  406. parent_sock = nfc_llcp_sock(parent);
  407. list_for_each_entry(llcp_sock, &parent_sock->accept_queue,
  408. accept_queue) {
  409. sk = &llcp_sock->sk;
  410. if (sk->sk_state == LLCP_CONNECTED)
  411. return EPOLLIN | EPOLLRDNORM;
  412. }
  413. return 0;
  414. }
  415. static __poll_t llcp_sock_poll(struct file *file, struct socket *sock,
  416. poll_table *wait)
  417. {
  418. struct sock *sk = sock->sk;
  419. __poll_t mask = 0;
  420. pr_debug("%p\n", sk);
  421. sock_poll_wait(file, sock, wait);
  422. if (sk->sk_state == LLCP_LISTEN)
  423. return llcp_accept_poll(sk);
  424. if (sk->sk_err || !skb_queue_empty_lockless(&sk->sk_error_queue))
  425. mask |= EPOLLERR |
  426. (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? EPOLLPRI : 0);
  427. if (!skb_queue_empty_lockless(&sk->sk_receive_queue))
  428. mask |= EPOLLIN | EPOLLRDNORM;
  429. if (sk->sk_state == LLCP_CLOSED)
  430. mask |= EPOLLHUP;
  431. if (sk->sk_shutdown & RCV_SHUTDOWN)
  432. mask |= EPOLLRDHUP | EPOLLIN | EPOLLRDNORM;
  433. if (sk->sk_shutdown == SHUTDOWN_MASK)
  434. mask |= EPOLLHUP;
  435. if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED)
  436. mask |= EPOLLOUT | EPOLLWRNORM | EPOLLWRBAND;
  437. else
  438. sk_set_bit(SOCKWQ_ASYNC_NOSPACE, sk);
  439. pr_debug("mask 0x%x\n", mask);
  440. return mask;
  441. }
  442. static int llcp_sock_release(struct socket *sock)
  443. {
  444. struct sock *sk = sock->sk;
  445. struct nfc_llcp_local *local;
  446. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  447. int err = 0;
  448. if (!sk)
  449. return 0;
  450. pr_debug("%p\n", sk);
  451. local = llcp_sock->local;
  452. if (local == NULL) {
  453. err = -ENODEV;
  454. goto out;
  455. }
  456. lock_sock(sk);
  457. /* Send a DISC */
  458. if (sk->sk_state == LLCP_CONNECTED)
  459. nfc_llcp_send_disconnect(llcp_sock);
  460. if (sk->sk_state == LLCP_LISTEN) {
  461. struct nfc_llcp_sock *lsk, *n;
  462. struct sock *accept_sk;
  463. list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
  464. accept_queue) {
  465. accept_sk = &lsk->sk;
  466. lock_sock(accept_sk);
  467. nfc_llcp_send_disconnect(lsk);
  468. nfc_llcp_accept_unlink(accept_sk);
  469. release_sock(accept_sk);
  470. }
  471. }
  472. if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
  473. nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
  474. release_sock(sk);
  475. /* Keep this sock alive and therefore do not remove it from the sockets
  476. * list until the DISC PDU has been actually sent. Otherwise we would
  477. * reply with DM PDUs before sending the DISC one.
  478. */
  479. if (sk->sk_state == LLCP_DISCONNECTING)
  480. return err;
  481. if (sock->type == SOCK_RAW)
  482. nfc_llcp_sock_unlink(&local->raw_sockets, sk);
  483. else
  484. nfc_llcp_sock_unlink(&local->sockets, sk);
  485. out:
  486. sock_orphan(sk);
  487. sock_put(sk);
  488. return err;
  489. }
  490. static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
  491. int len, int flags)
  492. {
  493. struct sock *sk = sock->sk;
  494. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  495. struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
  496. struct nfc_dev *dev;
  497. struct nfc_llcp_local *local;
  498. int ret = 0;
  499. pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
  500. if (!addr || len < sizeof(*addr) || addr->sa_family != AF_NFC)
  501. return -EINVAL;
  502. if (addr->service_name_len == 0 && addr->dsap == 0)
  503. return -EINVAL;
  504. pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
  505. addr->target_idx, addr->nfc_protocol);
  506. lock_sock(sk);
  507. if (sk->sk_state == LLCP_CONNECTED) {
  508. ret = -EISCONN;
  509. goto error;
  510. }
  511. if (sk->sk_state == LLCP_CONNECTING) {
  512. ret = -EINPROGRESS;
  513. goto error;
  514. }
  515. dev = nfc_get_device(addr->dev_idx);
  516. if (dev == NULL) {
  517. ret = -ENODEV;
  518. goto error;
  519. }
  520. local = nfc_llcp_find_local(dev);
  521. if (local == NULL) {
  522. ret = -ENODEV;
  523. goto put_dev;
  524. }
  525. device_lock(&dev->dev);
  526. if (dev->dep_link_up == false) {
  527. ret = -ENOLINK;
  528. device_unlock(&dev->dev);
  529. goto put_dev;
  530. }
  531. device_unlock(&dev->dev);
  532. if (local->rf_mode == NFC_RF_INITIATOR &&
  533. addr->target_idx != local->target_idx) {
  534. ret = -ENOLINK;
  535. goto put_dev;
  536. }
  537. llcp_sock->dev = dev;
  538. llcp_sock->local = nfc_llcp_local_get(local);
  539. llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
  540. if (llcp_sock->ssap == LLCP_SAP_MAX) {
  541. nfc_llcp_local_put(llcp_sock->local);
  542. llcp_sock->local = NULL;
  543. ret = -ENOMEM;
  544. goto put_dev;
  545. }
  546. llcp_sock->reserved_ssap = llcp_sock->ssap;
  547. if (addr->service_name_len == 0)
  548. llcp_sock->dsap = addr->dsap;
  549. else
  550. llcp_sock->dsap = LLCP_SAP_SDP;
  551. llcp_sock->nfc_protocol = addr->nfc_protocol;
  552. llcp_sock->service_name_len = min_t(unsigned int,
  553. addr->service_name_len,
  554. NFC_LLCP_MAX_SERVICE_NAME);
  555. llcp_sock->service_name = kmemdup(addr->service_name,
  556. llcp_sock->service_name_len,
  557. GFP_KERNEL);
  558. if (!llcp_sock->service_name) {
  559. ret = -ENOMEM;
  560. goto sock_llcp_release;
  561. }
  562. nfc_llcp_sock_link(&local->connecting_sockets, sk);
  563. ret = nfc_llcp_send_connect(llcp_sock);
  564. if (ret)
  565. goto sock_unlink;
  566. sk->sk_state = LLCP_CONNECTING;
  567. ret = sock_wait_state(sk, LLCP_CONNECTED,
  568. sock_sndtimeo(sk, flags & O_NONBLOCK));
  569. if (ret && ret != -EINPROGRESS)
  570. goto sock_unlink;
  571. release_sock(sk);
  572. return ret;
  573. sock_unlink:
  574. nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
  575. kfree(llcp_sock->service_name);
  576. llcp_sock->service_name = NULL;
  577. sock_llcp_release:
  578. nfc_llcp_put_ssap(local, llcp_sock->ssap);
  579. nfc_llcp_local_put(llcp_sock->local);
  580. llcp_sock->local = NULL;
  581. put_dev:
  582. nfc_put_device(dev);
  583. error:
  584. release_sock(sk);
  585. return ret;
  586. }
  587. static int llcp_sock_sendmsg(struct socket *sock, struct msghdr *msg,
  588. size_t len)
  589. {
  590. struct sock *sk = sock->sk;
  591. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  592. int ret;
  593. pr_debug("sock %p sk %p", sock, sk);
  594. ret = sock_error(sk);
  595. if (ret)
  596. return ret;
  597. if (msg->msg_flags & MSG_OOB)
  598. return -EOPNOTSUPP;
  599. lock_sock(sk);
  600. if (!llcp_sock->local) {
  601. release_sock(sk);
  602. return -ENODEV;
  603. }
  604. if (sk->sk_type == SOCK_DGRAM) {
  605. DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, addr,
  606. msg->msg_name);
  607. if (msg->msg_namelen < sizeof(*addr)) {
  608. release_sock(sk);
  609. return -EINVAL;
  610. }
  611. release_sock(sk);
  612. return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
  613. msg, len);
  614. }
  615. if (sk->sk_state != LLCP_CONNECTED) {
  616. release_sock(sk);
  617. return -ENOTCONN;
  618. }
  619. release_sock(sk);
  620. return nfc_llcp_send_i_frame(llcp_sock, msg, len);
  621. }
  622. static int llcp_sock_recvmsg(struct socket *sock, struct msghdr *msg,
  623. size_t len, int flags)
  624. {
  625. int noblock = flags & MSG_DONTWAIT;
  626. struct sock *sk = sock->sk;
  627. unsigned int copied, rlen;
  628. struct sk_buff *skb, *cskb;
  629. int err = 0;
  630. pr_debug("%p %zu\n", sk, len);
  631. lock_sock(sk);
  632. if (sk->sk_state == LLCP_CLOSED &&
  633. skb_queue_empty(&sk->sk_receive_queue)) {
  634. release_sock(sk);
  635. return 0;
  636. }
  637. release_sock(sk);
  638. if (flags & (MSG_OOB))
  639. return -EOPNOTSUPP;
  640. skb = skb_recv_datagram(sk, flags, noblock, &err);
  641. if (!skb) {
  642. pr_err("Recv datagram failed state %d %d %d",
  643. sk->sk_state, err, sock_error(sk));
  644. if (sk->sk_shutdown & RCV_SHUTDOWN)
  645. return 0;
  646. return err;
  647. }
  648. rlen = skb->len; /* real length of skb */
  649. copied = min_t(unsigned int, rlen, len);
  650. cskb = skb;
  651. if (skb_copy_datagram_msg(cskb, 0, msg, copied)) {
  652. if (!(flags & MSG_PEEK))
  653. skb_queue_head(&sk->sk_receive_queue, skb);
  654. return -EFAULT;
  655. }
  656. sock_recv_timestamp(msg, sk, skb);
  657. if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
  658. struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
  659. DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, sockaddr,
  660. msg->msg_name);
  661. msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
  662. pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
  663. memset(sockaddr, 0, sizeof(*sockaddr));
  664. sockaddr->sa_family = AF_NFC;
  665. sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
  666. sockaddr->dsap = ui_cb->dsap;
  667. sockaddr->ssap = ui_cb->ssap;
  668. }
  669. /* Mark read part of skb as used */
  670. if (!(flags & MSG_PEEK)) {
  671. /* SOCK_STREAM: re-queue skb if it contains unreceived data */
  672. if (sk->sk_type == SOCK_STREAM ||
  673. sk->sk_type == SOCK_DGRAM ||
  674. sk->sk_type == SOCK_RAW) {
  675. skb_pull(skb, copied);
  676. if (skb->len) {
  677. skb_queue_head(&sk->sk_receive_queue, skb);
  678. goto done;
  679. }
  680. }
  681. kfree_skb(skb);
  682. }
  683. /* XXX Queue backlogged skbs */
  684. done:
  685. /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
  686. if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
  687. copied = rlen;
  688. return copied;
  689. }
  690. static const struct proto_ops llcp_sock_ops = {
  691. .family = PF_NFC,
  692. .owner = THIS_MODULE,
  693. .bind = llcp_sock_bind,
  694. .connect = llcp_sock_connect,
  695. .release = llcp_sock_release,
  696. .socketpair = sock_no_socketpair,
  697. .accept = llcp_sock_accept,
  698. .getname = llcp_sock_getname,
  699. .poll = llcp_sock_poll,
  700. .ioctl = sock_no_ioctl,
  701. .listen = llcp_sock_listen,
  702. .shutdown = sock_no_shutdown,
  703. .setsockopt = nfc_llcp_setsockopt,
  704. .getsockopt = nfc_llcp_getsockopt,
  705. .sendmsg = llcp_sock_sendmsg,
  706. .recvmsg = llcp_sock_recvmsg,
  707. .mmap = sock_no_mmap,
  708. };
  709. static const struct proto_ops llcp_rawsock_ops = {
  710. .family = PF_NFC,
  711. .owner = THIS_MODULE,
  712. .bind = llcp_raw_sock_bind,
  713. .connect = sock_no_connect,
  714. .release = llcp_sock_release,
  715. .socketpair = sock_no_socketpair,
  716. .accept = sock_no_accept,
  717. .getname = llcp_sock_getname,
  718. .poll = llcp_sock_poll,
  719. .ioctl = sock_no_ioctl,
  720. .listen = sock_no_listen,
  721. .shutdown = sock_no_shutdown,
  722. .sendmsg = sock_no_sendmsg,
  723. .recvmsg = llcp_sock_recvmsg,
  724. .mmap = sock_no_mmap,
  725. };
  726. static void llcp_sock_destruct(struct sock *sk)
  727. {
  728. struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  729. pr_debug("%p\n", sk);
  730. if (sk->sk_state == LLCP_CONNECTED)
  731. nfc_put_device(llcp_sock->dev);
  732. skb_queue_purge(&sk->sk_receive_queue);
  733. nfc_llcp_sock_free(llcp_sock);
  734. if (!sock_flag(sk, SOCK_DEAD)) {
  735. pr_err("Freeing alive NFC LLCP socket %p\n", sk);
  736. return;
  737. }
  738. }
  739. struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp, int kern)
  740. {
  741. struct sock *sk;
  742. struct nfc_llcp_sock *llcp_sock;
  743. sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto, kern);
  744. if (!sk)
  745. return NULL;
  746. llcp_sock = nfc_llcp_sock(sk);
  747. sock_init_data(sock, sk);
  748. sk->sk_state = LLCP_CLOSED;
  749. sk->sk_protocol = NFC_SOCKPROTO_LLCP;
  750. sk->sk_type = type;
  751. sk->sk_destruct = llcp_sock_destruct;
  752. llcp_sock->ssap = 0;
  753. llcp_sock->dsap = LLCP_SAP_SDP;
  754. llcp_sock->rw = LLCP_MAX_RW + 1;
  755. llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
  756. llcp_sock->send_n = llcp_sock->send_ack_n = 0;
  757. llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
  758. llcp_sock->remote_ready = 1;
  759. llcp_sock->reserved_ssap = LLCP_SAP_MAX;
  760. nfc_llcp_socket_remote_param_init(llcp_sock);
  761. skb_queue_head_init(&llcp_sock->tx_queue);
  762. skb_queue_head_init(&llcp_sock->tx_pending_queue);
  763. INIT_LIST_HEAD(&llcp_sock->accept_queue);
  764. if (sock != NULL)
  765. sock->state = SS_UNCONNECTED;
  766. return sk;
  767. }
  768. void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
  769. {
  770. kfree(sock->service_name);
  771. skb_queue_purge(&sock->tx_queue);
  772. skb_queue_purge(&sock->tx_pending_queue);
  773. list_del_init(&sock->accept_queue);
  774. sock->parent = NULL;
  775. nfc_llcp_local_put(sock->local);
  776. }
  777. static int llcp_sock_create(struct net *net, struct socket *sock,
  778. const struct nfc_protocol *nfc_proto, int kern)
  779. {
  780. struct sock *sk;
  781. pr_debug("%p\n", sock);
  782. if (sock->type != SOCK_STREAM &&
  783. sock->type != SOCK_DGRAM &&
  784. sock->type != SOCK_RAW)
  785. return -ESOCKTNOSUPPORT;
  786. if (sock->type == SOCK_RAW) {
  787. if (!capable(CAP_NET_RAW))
  788. return -EPERM;
  789. sock->ops = &llcp_rawsock_ops;
  790. } else {
  791. sock->ops = &llcp_sock_ops;
  792. }
  793. sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC, kern);
  794. if (sk == NULL)
  795. return -ENOMEM;
  796. return 0;
  797. }
  798. static const struct nfc_protocol llcp_nfc_proto = {
  799. .id = NFC_SOCKPROTO_LLCP,
  800. .proto = &llcp_sock_proto,
  801. .owner = THIS_MODULE,
  802. .create = llcp_sock_create
  803. };
  804. int __init nfc_llcp_sock_init(void)
  805. {
  806. return nfc_proto_register(&llcp_nfc_proto);
  807. }
  808. void nfc_llcp_sock_exit(void)
  809. {
  810. nfc_proto_unregister(&llcp_nfc_proto);
  811. }