rawsock.c 8.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (C) 2011 Instituto Nokia de Tecnologia
  4. *
  5. * Authors:
  6. * Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
  7. * Lauro Ramos Venancio <lauro.venancio@openbossa.org>
  8. */
  9. #define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
  10. #include <net/tcp_states.h>
  11. #include <linux/nfc.h>
  12. #include <linux/export.h>
  13. #include "nfc.h"
  14. static struct nfc_sock_list raw_sk_list = {
  15. .lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock)
  16. };
  17. static void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk)
  18. {
  19. write_lock(&l->lock);
  20. sk_add_node(sk, &l->head);
  21. write_unlock(&l->lock);
  22. }
  23. static void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk)
  24. {
  25. write_lock(&l->lock);
  26. sk_del_node_init(sk);
  27. write_unlock(&l->lock);
  28. }
  29. static void rawsock_write_queue_purge(struct sock *sk)
  30. {
  31. pr_debug("sk=%p\n", sk);
  32. spin_lock_bh(&sk->sk_write_queue.lock);
  33. __skb_queue_purge(&sk->sk_write_queue);
  34. nfc_rawsock(sk)->tx_work_scheduled = false;
  35. spin_unlock_bh(&sk->sk_write_queue.lock);
  36. }
  37. static void rawsock_report_error(struct sock *sk, int err)
  38. {
  39. pr_debug("sk=%p err=%d\n", sk, err);
  40. sk->sk_shutdown = SHUTDOWN_MASK;
  41. sk->sk_err = -err;
  42. sk->sk_error_report(sk);
  43. rawsock_write_queue_purge(sk);
  44. }
  45. static int rawsock_release(struct socket *sock)
  46. {
  47. struct sock *sk = sock->sk;
  48. pr_debug("sock=%p sk=%p\n", sock, sk);
  49. if (!sk)
  50. return 0;
  51. if (sock->type == SOCK_RAW)
  52. nfc_sock_unlink(&raw_sk_list, sk);
  53. sock_orphan(sk);
  54. sock_put(sk);
  55. return 0;
  56. }
  57. static int rawsock_connect(struct socket *sock, struct sockaddr *_addr,
  58. int len, int flags)
  59. {
  60. struct sock *sk = sock->sk;
  61. struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr;
  62. struct nfc_dev *dev;
  63. int rc = 0;
  64. pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags);
  65. if (!addr || len < sizeof(struct sockaddr_nfc) ||
  66. addr->sa_family != AF_NFC)
  67. return -EINVAL;
  68. pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n",
  69. addr->dev_idx, addr->target_idx, addr->nfc_protocol);
  70. lock_sock(sk);
  71. if (sock->state == SS_CONNECTED) {
  72. rc = -EISCONN;
  73. goto error;
  74. }
  75. dev = nfc_get_device(addr->dev_idx);
  76. if (!dev) {
  77. rc = -ENODEV;
  78. goto error;
  79. }
  80. if (addr->target_idx > dev->target_next_idx - 1 ||
  81. addr->target_idx < dev->target_next_idx - dev->n_targets) {
  82. rc = -EINVAL;
  83. goto put_dev;
  84. }
  85. rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol);
  86. if (rc)
  87. goto put_dev;
  88. nfc_rawsock(sk)->dev = dev;
  89. nfc_rawsock(sk)->target_idx = addr->target_idx;
  90. sock->state = SS_CONNECTED;
  91. sk->sk_state = TCP_ESTABLISHED;
  92. sk->sk_state_change(sk);
  93. release_sock(sk);
  94. return 0;
  95. put_dev:
  96. nfc_put_device(dev);
  97. error:
  98. release_sock(sk);
  99. return rc;
  100. }
  101. static int rawsock_add_header(struct sk_buff *skb)
  102. {
  103. *(u8 *)skb_push(skb, NFC_HEADER_SIZE) = 0;
  104. return 0;
  105. }
  106. static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
  107. int err)
  108. {
  109. struct sock *sk = (struct sock *) context;
  110. BUG_ON(in_irq());
  111. pr_debug("sk=%p err=%d\n", sk, err);
  112. if (err)
  113. goto error;
  114. err = rawsock_add_header(skb);
  115. if (err)
  116. goto error_skb;
  117. err = sock_queue_rcv_skb(sk, skb);
  118. if (err)
  119. goto error_skb;
  120. spin_lock_bh(&sk->sk_write_queue.lock);
  121. if (!skb_queue_empty(&sk->sk_write_queue))
  122. schedule_work(&nfc_rawsock(sk)->tx_work);
  123. else
  124. nfc_rawsock(sk)->tx_work_scheduled = false;
  125. spin_unlock_bh(&sk->sk_write_queue.lock);
  126. sock_put(sk);
  127. return;
  128. error_skb:
  129. kfree_skb(skb);
  130. error:
  131. rawsock_report_error(sk, err);
  132. sock_put(sk);
  133. }
  134. static void rawsock_tx_work(struct work_struct *work)
  135. {
  136. struct sock *sk = to_rawsock_sk(work);
  137. struct nfc_dev *dev = nfc_rawsock(sk)->dev;
  138. u32 target_idx = nfc_rawsock(sk)->target_idx;
  139. struct sk_buff *skb;
  140. int rc;
  141. pr_debug("sk=%p target_idx=%u\n", sk, target_idx);
  142. if (sk->sk_shutdown & SEND_SHUTDOWN) {
  143. rawsock_write_queue_purge(sk);
  144. return;
  145. }
  146. skb = skb_dequeue(&sk->sk_write_queue);
  147. sock_hold(sk);
  148. rc = nfc_data_exchange(dev, target_idx, skb,
  149. rawsock_data_exchange_complete, sk);
  150. if (rc) {
  151. rawsock_report_error(sk, rc);
  152. sock_put(sk);
  153. }
  154. }
  155. static int rawsock_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
  156. {
  157. struct sock *sk = sock->sk;
  158. struct nfc_dev *dev = nfc_rawsock(sk)->dev;
  159. struct sk_buff *skb;
  160. int rc;
  161. pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len);
  162. if (msg->msg_namelen)
  163. return -EOPNOTSUPP;
  164. if (sock->state != SS_CONNECTED)
  165. return -ENOTCONN;
  166. skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc);
  167. if (skb == NULL)
  168. return rc;
  169. rc = memcpy_from_msg(skb_put(skb, len), msg, len);
  170. if (rc < 0) {
  171. kfree_skb(skb);
  172. return rc;
  173. }
  174. spin_lock_bh(&sk->sk_write_queue.lock);
  175. __skb_queue_tail(&sk->sk_write_queue, skb);
  176. if (!nfc_rawsock(sk)->tx_work_scheduled) {
  177. schedule_work(&nfc_rawsock(sk)->tx_work);
  178. nfc_rawsock(sk)->tx_work_scheduled = true;
  179. }
  180. spin_unlock_bh(&sk->sk_write_queue.lock);
  181. return len;
  182. }
  183. static int rawsock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
  184. int flags)
  185. {
  186. int noblock = flags & MSG_DONTWAIT;
  187. struct sock *sk = sock->sk;
  188. struct sk_buff *skb;
  189. int copied;
  190. int rc;
  191. pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
  192. skb = skb_recv_datagram(sk, flags, noblock, &rc);
  193. if (!skb)
  194. return rc;
  195. copied = skb->len;
  196. if (len < copied) {
  197. msg->msg_flags |= MSG_TRUNC;
  198. copied = len;
  199. }
  200. rc = skb_copy_datagram_msg(skb, 0, msg, copied);
  201. skb_free_datagram(sk, skb);
  202. return rc ? : copied;
  203. }
  204. static const struct proto_ops rawsock_ops = {
  205. .family = PF_NFC,
  206. .owner = THIS_MODULE,
  207. .release = rawsock_release,
  208. .bind = sock_no_bind,
  209. .connect = rawsock_connect,
  210. .socketpair = sock_no_socketpair,
  211. .accept = sock_no_accept,
  212. .getname = sock_no_getname,
  213. .poll = datagram_poll,
  214. .ioctl = sock_no_ioctl,
  215. .listen = sock_no_listen,
  216. .shutdown = sock_no_shutdown,
  217. .sendmsg = rawsock_sendmsg,
  218. .recvmsg = rawsock_recvmsg,
  219. .mmap = sock_no_mmap,
  220. };
  221. static const struct proto_ops rawsock_raw_ops = {
  222. .family = PF_NFC,
  223. .owner = THIS_MODULE,
  224. .release = rawsock_release,
  225. .bind = sock_no_bind,
  226. .connect = sock_no_connect,
  227. .socketpair = sock_no_socketpair,
  228. .accept = sock_no_accept,
  229. .getname = sock_no_getname,
  230. .poll = datagram_poll,
  231. .ioctl = sock_no_ioctl,
  232. .listen = sock_no_listen,
  233. .shutdown = sock_no_shutdown,
  234. .sendmsg = sock_no_sendmsg,
  235. .recvmsg = rawsock_recvmsg,
  236. .mmap = sock_no_mmap,
  237. };
  238. static void rawsock_destruct(struct sock *sk)
  239. {
  240. pr_debug("sk=%p\n", sk);
  241. if (sk->sk_state == TCP_ESTABLISHED) {
  242. nfc_deactivate_target(nfc_rawsock(sk)->dev,
  243. nfc_rawsock(sk)->target_idx,
  244. NFC_TARGET_MODE_IDLE);
  245. nfc_put_device(nfc_rawsock(sk)->dev);
  246. }
  247. skb_queue_purge(&sk->sk_receive_queue);
  248. if (!sock_flag(sk, SOCK_DEAD)) {
  249. pr_err("Freeing alive NFC raw socket %p\n", sk);
  250. return;
  251. }
  252. }
  253. static int rawsock_create(struct net *net, struct socket *sock,
  254. const struct nfc_protocol *nfc_proto, int kern)
  255. {
  256. struct sock *sk;
  257. pr_debug("sock=%p\n", sock);
  258. if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
  259. return -ESOCKTNOSUPPORT;
  260. if (sock->type == SOCK_RAW) {
  261. if (!ns_capable(net->user_ns, CAP_NET_RAW))
  262. return -EPERM;
  263. sock->ops = &rawsock_raw_ops;
  264. } else {
  265. sock->ops = &rawsock_ops;
  266. }
  267. sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto, kern);
  268. if (!sk)
  269. return -ENOMEM;
  270. sock_init_data(sock, sk);
  271. sk->sk_protocol = nfc_proto->id;
  272. sk->sk_destruct = rawsock_destruct;
  273. sock->state = SS_UNCONNECTED;
  274. if (sock->type == SOCK_RAW)
  275. nfc_sock_link(&raw_sk_list, sk);
  276. else {
  277. INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
  278. nfc_rawsock(sk)->tx_work_scheduled = false;
  279. }
  280. return 0;
  281. }
  282. void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
  283. u8 payload_type, u8 direction)
  284. {
  285. struct sk_buff *skb_copy = NULL, *nskb;
  286. struct sock *sk;
  287. u8 *data;
  288. read_lock(&raw_sk_list.lock);
  289. sk_for_each(sk, &raw_sk_list.head) {
  290. if (!skb_copy) {
  291. skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
  292. GFP_ATOMIC, true);
  293. if (!skb_copy)
  294. continue;
  295. data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
  296. data[0] = dev ? dev->idx : 0xFF;
  297. data[1] = direction & 0x01;
  298. data[1] |= (payload_type << 1);
  299. }
  300. nskb = skb_clone(skb_copy, GFP_ATOMIC);
  301. if (!nskb)
  302. continue;
  303. if (sock_queue_rcv_skb(sk, nskb))
  304. kfree_skb(nskb);
  305. }
  306. read_unlock(&raw_sk_list.lock);
  307. kfree_skb(skb_copy);
  308. }
  309. EXPORT_SYMBOL(nfc_send_to_raw_sock);
  310. static struct proto rawsock_proto = {
  311. .name = "NFC_RAW",
  312. .owner = THIS_MODULE,
  313. .obj_size = sizeof(struct nfc_rawsock),
  314. };
  315. static const struct nfc_protocol rawsock_nfc_proto = {
  316. .id = NFC_SOCKPROTO_RAW,
  317. .proto = &rawsock_proto,
  318. .owner = THIS_MODULE,
  319. .create = rawsock_create
  320. };
  321. int __init rawsock_init(void)
  322. {
  323. int rc;
  324. rc = nfc_proto_register(&rawsock_nfc_proto);
  325. return rc;
  326. }
  327. void rawsock_exit(void)
  328. {
  329. nfc_proto_unregister(&rawsock_nfc_proto);
  330. }