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