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 | |
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(wq_head: sk_sleep(sk), wq_entry: &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(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(wq_head: sk_sleep(sk), wq_entry: &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 | |
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(idx: 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(p: llcp_addr.service_name, |
108 | size: 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, sock: 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(l: &local->sockets, s: 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(objp: llcp_sock->service_name); |
134 | llcp_sock->service_name = NULL; |
135 | |
136 | sock_llcp_put_local: |
137 | nfc_llcp_local_put(local: 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 | |
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(idx: 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(l: &local->raw_sockets, s: 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 | |
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 | |
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 | if (copy_from_sockptr(dst: &opt, src: optval, size: sizeof(u32))) { |
256 | err = -EFAULT; |
257 | break; |
258 | } |
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 | if (copy_from_sockptr(dst: &opt, src: optval, size: sizeof(u32))) { |
278 | err = -EFAULT; |
279 | break; |
280 | } |
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 | |
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 | |
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(entry: &llcp_sock->accept_queue); |
388 | sk_acceptq_removed(sk: llcp_sock->parent); |
389 | llcp_sock->parent = NULL; |
390 | |
391 | sock_put(sk); |
392 | } |
393 | |
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(new: &llcp_sock->accept_queue, |
403 | head: &llcp_sock_parent->accept_queue); |
404 | llcp_sock->parent = parent; |
405 | sk_acceptq_added(sk: parent); |
406 | } |
407 | |
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(entry: &lsk->accept_queue); |
429 | sock_put(sk); |
430 | |
431 | if (newsock) |
432 | sock_graft(sk, parent: newsock); |
433 | |
434 | release_sock(sk); |
435 | |
436 | pr_debug("Returning sk state %d\n" , sk->sk_state); |
437 | |
438 | sk_acceptq_removed(sk: parent); |
439 | |
440 | return sk; |
441 | } |
442 | |
443 | release_sock(sk); |
444 | } |
445 | |
446 | return NULL; |
447 | } |
448 | |
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, noblock: flags & O_NONBLOCK); |
467 | |
468 | /* Wait for an incoming connection. */ |
469 | add_wait_queue_exclusive(wq_head: sk_sleep(sk), wq_entry: &wait); |
470 | while (!(new_sk = nfc_llcp_accept_dequeue(parent: 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(timeout: timeo); |
485 | lock_sock_nested(sk, SINGLE_DEPTH_NESTING); |
486 | } |
487 | __set_current_state(TASK_RUNNING); |
488 | remove_wait_queue(wq_head: sk_sleep(sk), wq_entry: &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 | |
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 | |
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 | |
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(filp: file, sock, p: wait); |
564 | |
565 | if (sk->sk_state == LLCP_LISTEN) |
566 | return llcp_accept_poll(parent: sk); |
567 | |
568 | if (sk->sk_err || !skb_queue_empty_lockless(list: &sk->sk_error_queue)) |
569 | mask |= EPOLLERR | |
570 | (sock_flag(sk, flag: SOCK_SELECT_ERR_QUEUE) ? EPOLLPRI : 0); |
571 | |
572 | if (!skb_queue_empty_lockless(list: &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 | |
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(sock: 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(sk: accept_sk); |
626 | |
627 | nfc_llcp_send_disconnect(sock: lsk); |
628 | nfc_llcp_accept_unlink(sk: accept_sk); |
629 | |
630 | release_sock(sk: accept_sk); |
631 | } |
632 | } |
633 | |
634 | if (sock->type == SOCK_RAW) |
635 | nfc_llcp_sock_unlink(l: &local->raw_sockets, s: sk); |
636 | else |
637 | nfc_llcp_sock_unlink(l: &local->sockets, s: sk); |
638 | |
639 | if (llcp_sock->reserved_ssap < LLCP_SAP_MAX) |
640 | nfc_llcp_put_ssap(local: llcp_sock->local, ssap: 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 | |
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(idx: 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->dev); |
696 | if (dev->dep_link_up == false) { |
697 | ret = -ENOLINK; |
698 | device_unlock(dev: &dev->dev); |
699 | goto sock_llcp_put_local; |
700 | } |
701 | device_unlock(dev: &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(p: addr->service_name, |
728 | size: 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(l: &local->connecting_sockets, s: sk); |
736 | |
737 | ret = nfc_llcp_send_connect(sock: llcp_sock); |
738 | if (ret) |
739 | goto sock_unlink; |
740 | |
741 | sk->sk_state = LLCP_CONNECTING; |
742 | |
743 | ret = sock_wait_state(sk, state: LLCP_CONNECTED, |
744 | timeo: sock_sndtimeo(sk, noblock: 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(l: &local->connecting_sockets, s: sk); |
754 | kfree(objp: llcp_sock->service_name); |
755 | llcp_sock->service_name = NULL; |
756 | |
757 | sock_llcp_release: |
758 | nfc_llcp_put_ssap(local, ssap: 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 | |
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 | DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, addr, |
800 | msg->msg_name); |
801 | |
802 | if (msg->msg_namelen < sizeof(*addr)) { |
803 | release_sock(sk); |
804 | return -EINVAL; |
805 | } |
806 | |
807 | release_sock(sk); |
808 | |
809 | return nfc_llcp_send_ui_frame(sock: llcp_sock, ssap: addr->dsap, dsap: addr->ssap, |
810 | msg, len); |
811 | } |
812 | |
813 | if (sk->sk_state != LLCP_CONNECTED) { |
814 | release_sock(sk); |
815 | return -ENOTCONN; |
816 | } |
817 | |
818 | release_sock(sk); |
819 | |
820 | return nfc_llcp_send_i_frame(sock: llcp_sock, msg, len); |
821 | } |
822 | |
823 | static int llcp_sock_recvmsg(struct socket *sock, struct msghdr *msg, |
824 | size_t len, int flags) |
825 | { |
826 | struct sock *sk = sock->sk; |
827 | unsigned int copied, rlen; |
828 | struct sk_buff *skb, *cskb; |
829 | int err = 0; |
830 | |
831 | pr_debug("%p %zu\n" , sk, len); |
832 | |
833 | lock_sock(sk); |
834 | |
835 | if (sk->sk_state == LLCP_CLOSED && |
836 | skb_queue_empty(list: &sk->sk_receive_queue)) { |
837 | release_sock(sk); |
838 | return 0; |
839 | } |
840 | |
841 | release_sock(sk); |
842 | |
843 | if (flags & (MSG_OOB)) |
844 | return -EOPNOTSUPP; |
845 | |
846 | skb = skb_recv_datagram(sk, flags, err: &err); |
847 | if (!skb) { |
848 | pr_err("Recv datagram failed state %d %d %d" , |
849 | sk->sk_state, err, sock_error(sk)); |
850 | |
851 | if (sk->sk_shutdown & RCV_SHUTDOWN) |
852 | return 0; |
853 | |
854 | return err; |
855 | } |
856 | |
857 | rlen = skb->len; /* real length of skb */ |
858 | copied = min_t(unsigned int, rlen, len); |
859 | |
860 | cskb = skb; |
861 | if (skb_copy_datagram_msg(from: cskb, offset: 0, msg, size: copied)) { |
862 | if (!(flags & MSG_PEEK)) |
863 | skb_queue_head(list: &sk->sk_receive_queue, newsk: skb); |
864 | return -EFAULT; |
865 | } |
866 | |
867 | sock_recv_timestamp(msg, sk, skb); |
868 | |
869 | if (sk->sk_type == SOCK_DGRAM && msg->msg_name) { |
870 | struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb); |
871 | DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, sockaddr, |
872 | msg->msg_name); |
873 | |
874 | msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp); |
875 | |
876 | pr_debug("Datagram socket %d %d\n" , ui_cb->dsap, ui_cb->ssap); |
877 | |
878 | memset(sockaddr, 0, sizeof(*sockaddr)); |
879 | sockaddr->sa_family = AF_NFC; |
880 | sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP; |
881 | sockaddr->dsap = ui_cb->dsap; |
882 | sockaddr->ssap = ui_cb->ssap; |
883 | } |
884 | |
885 | /* Mark read part of skb as used */ |
886 | if (!(flags & MSG_PEEK)) { |
887 | |
888 | /* SOCK_STREAM: re-queue skb if it contains unreceived data */ |
889 | if (sk->sk_type == SOCK_STREAM || |
890 | sk->sk_type == SOCK_DGRAM || |
891 | sk->sk_type == SOCK_RAW) { |
892 | skb_pull(skb, len: copied); |
893 | if (skb->len) { |
894 | skb_queue_head(list: &sk->sk_receive_queue, newsk: skb); |
895 | goto done; |
896 | } |
897 | } |
898 | |
899 | kfree_skb(skb); |
900 | } |
901 | |
902 | /* XXX Queue backlogged skbs */ |
903 | |
904 | done: |
905 | /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */ |
906 | if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC)) |
907 | copied = rlen; |
908 | |
909 | return copied; |
910 | } |
911 | |
912 | static const struct proto_ops llcp_sock_ops = { |
913 | .family = PF_NFC, |
914 | .owner = THIS_MODULE, |
915 | .bind = llcp_sock_bind, |
916 | .connect = llcp_sock_connect, |
917 | .release = llcp_sock_release, |
918 | .socketpair = sock_no_socketpair, |
919 | .accept = llcp_sock_accept, |
920 | .getname = llcp_sock_getname, |
921 | .poll = llcp_sock_poll, |
922 | .ioctl = sock_no_ioctl, |
923 | .listen = llcp_sock_listen, |
924 | .shutdown = sock_no_shutdown, |
925 | .setsockopt = nfc_llcp_setsockopt, |
926 | .getsockopt = nfc_llcp_getsockopt, |
927 | .sendmsg = llcp_sock_sendmsg, |
928 | .recvmsg = llcp_sock_recvmsg, |
929 | .mmap = sock_no_mmap, |
930 | }; |
931 | |
932 | static const struct proto_ops llcp_rawsock_ops = { |
933 | .family = PF_NFC, |
934 | .owner = THIS_MODULE, |
935 | .bind = llcp_raw_sock_bind, |
936 | .connect = sock_no_connect, |
937 | .release = llcp_sock_release, |
938 | .socketpair = sock_no_socketpair, |
939 | .accept = sock_no_accept, |
940 | .getname = llcp_sock_getname, |
941 | .poll = llcp_sock_poll, |
942 | .ioctl = sock_no_ioctl, |
943 | .listen = sock_no_listen, |
944 | .shutdown = sock_no_shutdown, |
945 | .sendmsg = sock_no_sendmsg, |
946 | .recvmsg = llcp_sock_recvmsg, |
947 | .mmap = sock_no_mmap, |
948 | }; |
949 | |
950 | static void llcp_sock_destruct(struct sock *sk) |
951 | { |
952 | struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); |
953 | |
954 | pr_debug("%p\n" , sk); |
955 | |
956 | if (sk->sk_state == LLCP_CONNECTED) |
957 | nfc_put_device(dev: llcp_sock->dev); |
958 | |
959 | skb_queue_purge(list: &sk->sk_receive_queue); |
960 | |
961 | nfc_llcp_sock_free(sock: llcp_sock); |
962 | |
963 | if (!sock_flag(sk, flag: SOCK_DEAD)) { |
964 | pr_err("Freeing alive NFC LLCP socket %p\n" , sk); |
965 | return; |
966 | } |
967 | } |
968 | |
969 | struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp, int kern) |
970 | { |
971 | struct sock *sk; |
972 | struct nfc_llcp_sock *llcp_sock; |
973 | |
974 | sk = sk_alloc(net: &init_net, PF_NFC, priority: gfp, prot: &llcp_sock_proto, kern); |
975 | if (!sk) |
976 | return NULL; |
977 | |
978 | llcp_sock = nfc_llcp_sock(sk); |
979 | |
980 | sock_init_data(sock, sk); |
981 | sk->sk_state = LLCP_CLOSED; |
982 | sk->sk_protocol = NFC_SOCKPROTO_LLCP; |
983 | sk->sk_type = type; |
984 | sk->sk_destruct = llcp_sock_destruct; |
985 | |
986 | llcp_sock->ssap = 0; |
987 | llcp_sock->dsap = LLCP_SAP_SDP; |
988 | llcp_sock->rw = LLCP_MAX_RW + 1; |
989 | llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1); |
990 | llcp_sock->send_n = llcp_sock->send_ack_n = 0; |
991 | llcp_sock->recv_n = llcp_sock->recv_ack_n = 0; |
992 | llcp_sock->remote_ready = 1; |
993 | llcp_sock->reserved_ssap = LLCP_SAP_MAX; |
994 | nfc_llcp_socket_remote_param_init(sock: llcp_sock); |
995 | skb_queue_head_init(list: &llcp_sock->tx_queue); |
996 | skb_queue_head_init(list: &llcp_sock->tx_pending_queue); |
997 | INIT_LIST_HEAD(list: &llcp_sock->accept_queue); |
998 | |
999 | if (sock != NULL) |
1000 | sock->state = SS_UNCONNECTED; |
1001 | |
1002 | return sk; |
1003 | } |
1004 | |
1005 | void nfc_llcp_sock_free(struct nfc_llcp_sock *sock) |
1006 | { |
1007 | kfree(objp: sock->service_name); |
1008 | |
1009 | skb_queue_purge(list: &sock->tx_queue); |
1010 | skb_queue_purge(list: &sock->tx_pending_queue); |
1011 | |
1012 | list_del_init(entry: &sock->accept_queue); |
1013 | |
1014 | sock->parent = NULL; |
1015 | |
1016 | nfc_llcp_local_put(local: sock->local); |
1017 | } |
1018 | |
1019 | static int llcp_sock_create(struct net *net, struct socket *sock, |
1020 | const struct nfc_protocol *nfc_proto, int kern) |
1021 | { |
1022 | struct sock *sk; |
1023 | |
1024 | pr_debug("%p\n" , sock); |
1025 | |
1026 | if (sock->type != SOCK_STREAM && |
1027 | sock->type != SOCK_DGRAM && |
1028 | sock->type != SOCK_RAW) |
1029 | return -ESOCKTNOSUPPORT; |
1030 | |
1031 | if (sock->type == SOCK_RAW) { |
1032 | if (!capable(CAP_NET_RAW)) |
1033 | return -EPERM; |
1034 | sock->ops = &llcp_rawsock_ops; |
1035 | } else { |
1036 | sock->ops = &llcp_sock_ops; |
1037 | } |
1038 | |
1039 | sk = nfc_llcp_sock_alloc(sock, type: sock->type, GFP_ATOMIC, kern); |
1040 | if (sk == NULL) |
1041 | return -ENOMEM; |
1042 | |
1043 | return 0; |
1044 | } |
1045 | |
1046 | static const struct nfc_protocol llcp_nfc_proto = { |
1047 | .id = NFC_SOCKPROTO_LLCP, |
1048 | .proto = &llcp_sock_proto, |
1049 | .owner = THIS_MODULE, |
1050 | .create = llcp_sock_create |
1051 | }; |
1052 | |
1053 | int __init nfc_llcp_sock_init(void) |
1054 | { |
1055 | return nfc_proto_register(nfc_proto: &llcp_nfc_proto); |
1056 | } |
1057 | |
1058 | void nfc_llcp_sock_exit(void) |
1059 | { |
1060 | nfc_proto_unregister(nfc_proto: &llcp_nfc_proto); |
1061 | } |
1062 | |