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