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