sock.c 21 KB

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