xref: /openbmc/linux/net/nfc/llcp_core.c (revision fb195df9)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Copyright (C) 2011  Intel Corporation. All rights reserved.
4  * Copyright (C) 2014 Marvell International Ltd.
5  */
6 
7 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
8 
9 #include <linux/init.h>
10 #include <linux/kernel.h>
11 #include <linux/list.h>
12 #include <linux/nfc.h>
13 
14 #include "nfc.h"
15 #include "llcp.h"
16 
17 static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
18 
19 static LIST_HEAD(llcp_devices);
20 /* Protects llcp_devices list */
21 static DEFINE_SPINLOCK(llcp_devices_lock);
22 
23 static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
24 
nfc_llcp_sock_link(struct llcp_sock_list * l,struct sock * sk)25 void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
26 {
27 	write_lock(&l->lock);
28 	sk_add_node(sk, &l->head);
29 	write_unlock(&l->lock);
30 }
31 
nfc_llcp_sock_unlink(struct llcp_sock_list * l,struct sock * sk)32 void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
33 {
34 	write_lock(&l->lock);
35 	sk_del_node_init(sk);
36 	write_unlock(&l->lock);
37 }
38 
nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock * sock)39 void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
40 {
41 	sock->remote_rw = LLCP_DEFAULT_RW;
42 	sock->remote_miu = LLCP_MAX_MIU + 1;
43 }
44 
nfc_llcp_socket_purge(struct nfc_llcp_sock * sock)45 static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
46 {
47 	struct nfc_llcp_local *local = sock->local;
48 	struct sk_buff *s, *tmp;
49 
50 	skb_queue_purge(&sock->tx_queue);
51 	skb_queue_purge(&sock->tx_pending_queue);
52 
53 	if (local == NULL)
54 		return;
55 
56 	/* Search for local pending SKBs that are related to this socket */
57 	skb_queue_walk_safe(&local->tx_queue, s, tmp) {
58 		if (s->sk != &sock->sk)
59 			continue;
60 
61 		skb_unlink(s, &local->tx_queue);
62 		kfree_skb(s);
63 	}
64 }
65 
nfc_llcp_socket_release(struct nfc_llcp_local * local,bool device,int err)66 static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
67 				    int err)
68 {
69 	struct sock *sk;
70 	struct hlist_node *tmp;
71 	struct nfc_llcp_sock *llcp_sock;
72 
73 	skb_queue_purge(&local->tx_queue);
74 
75 	write_lock(&local->sockets.lock);
76 
77 	sk_for_each_safe(sk, tmp, &local->sockets.head) {
78 		llcp_sock = nfc_llcp_sock(sk);
79 
80 		bh_lock_sock(sk);
81 
82 		nfc_llcp_socket_purge(llcp_sock);
83 
84 		if (sk->sk_state == LLCP_CONNECTED)
85 			nfc_put_device(llcp_sock->dev);
86 
87 		if (sk->sk_state == LLCP_LISTEN) {
88 			struct nfc_llcp_sock *lsk, *n;
89 			struct sock *accept_sk;
90 
91 			list_for_each_entry_safe(lsk, n,
92 						 &llcp_sock->accept_queue,
93 						 accept_queue) {
94 				accept_sk = &lsk->sk;
95 				bh_lock_sock(accept_sk);
96 
97 				nfc_llcp_accept_unlink(accept_sk);
98 
99 				if (err)
100 					accept_sk->sk_err = err;
101 				accept_sk->sk_state = LLCP_CLOSED;
102 				accept_sk->sk_state_change(sk);
103 
104 				bh_unlock_sock(accept_sk);
105 			}
106 		}
107 
108 		if (err)
109 			sk->sk_err = err;
110 		sk->sk_state = LLCP_CLOSED;
111 		sk->sk_state_change(sk);
112 
113 		bh_unlock_sock(sk);
114 
115 		sk_del_node_init(sk);
116 	}
117 
118 	write_unlock(&local->sockets.lock);
119 
120 	/* If we still have a device, we keep the RAW sockets alive */
121 	if (device == true)
122 		return;
123 
124 	write_lock(&local->raw_sockets.lock);
125 
126 	sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
127 		llcp_sock = nfc_llcp_sock(sk);
128 
129 		bh_lock_sock(sk);
130 
131 		nfc_llcp_socket_purge(llcp_sock);
132 
133 		if (err)
134 			sk->sk_err = err;
135 		sk->sk_state = LLCP_CLOSED;
136 		sk->sk_state_change(sk);
137 
138 		bh_unlock_sock(sk);
139 
140 		sk_del_node_init(sk);
141 	}
142 
143 	write_unlock(&local->raw_sockets.lock);
144 }
145 
nfc_llcp_local_get(struct nfc_llcp_local * local)146 static struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
147 {
148 	/* Since using nfc_llcp_local may result in usage of nfc_dev, whenever
149 	 * we hold a reference to local, we also need to hold a reference to
150 	 * the device to avoid UAF.
151 	 */
152 	if (!nfc_get_device(local->dev->idx))
153 		return NULL;
154 
155 	kref_get(&local->ref);
156 
157 	return local;
158 }
159 
local_cleanup(struct nfc_llcp_local * local)160 static void local_cleanup(struct nfc_llcp_local *local)
161 {
162 	nfc_llcp_socket_release(local, false, ENXIO);
163 	del_timer_sync(&local->link_timer);
164 	skb_queue_purge(&local->tx_queue);
165 	cancel_work_sync(&local->tx_work);
166 	cancel_work_sync(&local->rx_work);
167 	cancel_work_sync(&local->timeout_work);
168 	kfree_skb(local->rx_pending);
169 	local->rx_pending = NULL;
170 	del_timer_sync(&local->sdreq_timer);
171 	cancel_work_sync(&local->sdreq_timeout_work);
172 	nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
173 }
174 
local_release(struct kref * ref)175 static void local_release(struct kref *ref)
176 {
177 	struct nfc_llcp_local *local;
178 
179 	local = container_of(ref, struct nfc_llcp_local, ref);
180 
181 	local_cleanup(local);
182 	kfree(local);
183 }
184 
nfc_llcp_local_put(struct nfc_llcp_local * local)185 int nfc_llcp_local_put(struct nfc_llcp_local *local)
186 {
187 	struct nfc_dev *dev;
188 	int ret;
189 
190 	if (local == NULL)
191 		return 0;
192 
193 	dev = local->dev;
194 
195 	ret = kref_put(&local->ref, local_release);
196 	nfc_put_device(dev);
197 
198 	return ret;
199 }
200 
nfc_llcp_sock_get(struct nfc_llcp_local * local,u8 ssap,u8 dsap)201 static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
202 					       u8 ssap, u8 dsap)
203 {
204 	struct sock *sk;
205 	struct nfc_llcp_sock *llcp_sock, *tmp_sock;
206 
207 	pr_debug("ssap dsap %d %d\n", ssap, dsap);
208 
209 	if (ssap == 0 && dsap == 0)
210 		return NULL;
211 
212 	read_lock(&local->sockets.lock);
213 
214 	llcp_sock = NULL;
215 
216 	sk_for_each(sk, &local->sockets.head) {
217 		tmp_sock = nfc_llcp_sock(sk);
218 
219 		if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
220 			llcp_sock = tmp_sock;
221 			sock_hold(&llcp_sock->sk);
222 			break;
223 		}
224 	}
225 
226 	read_unlock(&local->sockets.lock);
227 
228 	return llcp_sock;
229 }
230 
nfc_llcp_sock_put(struct nfc_llcp_sock * sock)231 static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
232 {
233 	sock_put(&sock->sk);
234 }
235 
nfc_llcp_timeout_work(struct work_struct * work)236 static void nfc_llcp_timeout_work(struct work_struct *work)
237 {
238 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
239 						    timeout_work);
240 
241 	nfc_dep_link_down(local->dev);
242 }
243 
nfc_llcp_symm_timer(struct timer_list * t)244 static void nfc_llcp_symm_timer(struct timer_list *t)
245 {
246 	struct nfc_llcp_local *local = from_timer(local, t, link_timer);
247 
248 	pr_err("SYMM timeout\n");
249 
250 	schedule_work(&local->timeout_work);
251 }
252 
nfc_llcp_sdreq_timeout_work(struct work_struct * work)253 static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
254 {
255 	unsigned long time;
256 	HLIST_HEAD(nl_sdres_list);
257 	struct hlist_node *n;
258 	struct nfc_llcp_sdp_tlv *sdp;
259 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
260 						    sdreq_timeout_work);
261 
262 	mutex_lock(&local->sdreq_lock);
263 
264 	time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
265 
266 	hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
267 		if (time_after(sdp->time, time))
268 			continue;
269 
270 		sdp->sap = LLCP_SDP_UNBOUND;
271 
272 		hlist_del(&sdp->node);
273 
274 		hlist_add_head(&sdp->node, &nl_sdres_list);
275 	}
276 
277 	if (!hlist_empty(&local->pending_sdreqs))
278 		mod_timer(&local->sdreq_timer,
279 			  jiffies + msecs_to_jiffies(3 * local->remote_lto));
280 
281 	mutex_unlock(&local->sdreq_lock);
282 
283 	if (!hlist_empty(&nl_sdres_list))
284 		nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
285 }
286 
nfc_llcp_sdreq_timer(struct timer_list * t)287 static void nfc_llcp_sdreq_timer(struct timer_list *t)
288 {
289 	struct nfc_llcp_local *local = from_timer(local, t, sdreq_timer);
290 
291 	schedule_work(&local->sdreq_timeout_work);
292 }
293 
nfc_llcp_find_local(struct nfc_dev * dev)294 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
295 {
296 	struct nfc_llcp_local *local;
297 	struct nfc_llcp_local *res = NULL;
298 
299 	spin_lock(&llcp_devices_lock);
300 	list_for_each_entry(local, &llcp_devices, list)
301 		if (local->dev == dev) {
302 			res = nfc_llcp_local_get(local);
303 			break;
304 		}
305 	spin_unlock(&llcp_devices_lock);
306 
307 	return res;
308 }
309 
nfc_llcp_remove_local(struct nfc_dev * dev)310 static struct nfc_llcp_local *nfc_llcp_remove_local(struct nfc_dev *dev)
311 {
312 	struct nfc_llcp_local *local, *tmp;
313 
314 	spin_lock(&llcp_devices_lock);
315 	list_for_each_entry_safe(local, tmp, &llcp_devices, list)
316 		if (local->dev == dev) {
317 			list_del(&local->list);
318 			spin_unlock(&llcp_devices_lock);
319 			return local;
320 		}
321 	spin_unlock(&llcp_devices_lock);
322 
323 	pr_warn("Shutting down device not found\n");
324 
325 	return NULL;
326 }
327 
328 static char *wks[] = {
329 	NULL,
330 	NULL, /* SDP */
331 	"urn:nfc:sn:ip",
332 	"urn:nfc:sn:obex",
333 	"urn:nfc:sn:snep",
334 };
335 
nfc_llcp_wks_sap(const char * service_name,size_t service_name_len)336 static int nfc_llcp_wks_sap(const char *service_name, size_t service_name_len)
337 {
338 	int sap, num_wks;
339 
340 	pr_debug("%s\n", service_name);
341 
342 	if (service_name == NULL)
343 		return -EINVAL;
344 
345 	num_wks = ARRAY_SIZE(wks);
346 
347 	for (sap = 0; sap < num_wks; sap++) {
348 		if (wks[sap] == NULL)
349 			continue;
350 
351 		if (strncmp(wks[sap], service_name, service_name_len) == 0)
352 			return sap;
353 	}
354 
355 	return -EINVAL;
356 }
357 
358 static
nfc_llcp_sock_from_sn(struct nfc_llcp_local * local,const u8 * sn,size_t sn_len,bool needref)359 struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
360 					    const u8 *sn, size_t sn_len,
361 					    bool needref)
362 {
363 	struct sock *sk;
364 	struct nfc_llcp_sock *llcp_sock, *tmp_sock;
365 
366 	pr_debug("sn %zd %p\n", sn_len, sn);
367 
368 	if (sn == NULL || sn_len == 0)
369 		return NULL;
370 
371 	read_lock(&local->sockets.lock);
372 
373 	llcp_sock = NULL;
374 
375 	sk_for_each(sk, &local->sockets.head) {
376 		tmp_sock = nfc_llcp_sock(sk);
377 
378 		pr_debug("llcp sock %p\n", tmp_sock);
379 
380 		if (tmp_sock->sk.sk_type == SOCK_STREAM &&
381 		    tmp_sock->sk.sk_state != LLCP_LISTEN)
382 			continue;
383 
384 		if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
385 		    tmp_sock->sk.sk_state != LLCP_BOUND)
386 			continue;
387 
388 		if (tmp_sock->service_name == NULL ||
389 		    tmp_sock->service_name_len == 0)
390 			continue;
391 
392 		if (tmp_sock->service_name_len != sn_len)
393 			continue;
394 
395 		if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
396 			llcp_sock = tmp_sock;
397 			if (needref)
398 				sock_hold(&llcp_sock->sk);
399 			break;
400 		}
401 	}
402 
403 	read_unlock(&local->sockets.lock);
404 
405 	pr_debug("Found llcp sock %p\n", llcp_sock);
406 
407 	return llcp_sock;
408 }
409 
nfc_llcp_get_sdp_ssap(struct nfc_llcp_local * local,struct nfc_llcp_sock * sock)410 u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
411 			 struct nfc_llcp_sock *sock)
412 {
413 	mutex_lock(&local->sdp_lock);
414 
415 	if (sock->service_name != NULL && sock->service_name_len > 0) {
416 		int ssap = nfc_llcp_wks_sap(sock->service_name,
417 					    sock->service_name_len);
418 
419 		if (ssap > 0) {
420 			pr_debug("WKS %d\n", ssap);
421 
422 			/* This is a WKS, let's check if it's free */
423 			if (test_bit(ssap, &local->local_wks)) {
424 				mutex_unlock(&local->sdp_lock);
425 
426 				return LLCP_SAP_MAX;
427 			}
428 
429 			set_bit(ssap, &local->local_wks);
430 			mutex_unlock(&local->sdp_lock);
431 
432 			return ssap;
433 		}
434 
435 		/*
436 		 * Check if there already is a non WKS socket bound
437 		 * to this service name.
438 		 */
439 		if (nfc_llcp_sock_from_sn(local, sock->service_name,
440 					  sock->service_name_len,
441 					  false) != NULL) {
442 			mutex_unlock(&local->sdp_lock);
443 
444 			return LLCP_SAP_MAX;
445 		}
446 
447 		mutex_unlock(&local->sdp_lock);
448 
449 		return LLCP_SDP_UNBOUND;
450 
451 	} else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
452 		if (!test_bit(sock->ssap, &local->local_wks)) {
453 			set_bit(sock->ssap, &local->local_wks);
454 			mutex_unlock(&local->sdp_lock);
455 
456 			return sock->ssap;
457 		}
458 	}
459 
460 	mutex_unlock(&local->sdp_lock);
461 
462 	return LLCP_SAP_MAX;
463 }
464 
nfc_llcp_get_local_ssap(struct nfc_llcp_local * local)465 u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
466 {
467 	u8 local_ssap;
468 
469 	mutex_lock(&local->sdp_lock);
470 
471 	local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
472 	if (local_ssap == LLCP_LOCAL_NUM_SAP) {
473 		mutex_unlock(&local->sdp_lock);
474 		return LLCP_SAP_MAX;
475 	}
476 
477 	set_bit(local_ssap, &local->local_sap);
478 
479 	mutex_unlock(&local->sdp_lock);
480 
481 	return local_ssap + LLCP_LOCAL_SAP_OFFSET;
482 }
483 
nfc_llcp_put_ssap(struct nfc_llcp_local * local,u8 ssap)484 void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
485 {
486 	u8 local_ssap;
487 	unsigned long *sdp;
488 
489 	if (ssap < LLCP_WKS_NUM_SAP) {
490 		local_ssap = ssap;
491 		sdp = &local->local_wks;
492 	} else if (ssap < LLCP_LOCAL_NUM_SAP) {
493 		atomic_t *client_cnt;
494 
495 		local_ssap = ssap - LLCP_WKS_NUM_SAP;
496 		sdp = &local->local_sdp;
497 		client_cnt = &local->local_sdp_cnt[local_ssap];
498 
499 		pr_debug("%d clients\n", atomic_read(client_cnt));
500 
501 		mutex_lock(&local->sdp_lock);
502 
503 		if (atomic_dec_and_test(client_cnt)) {
504 			struct nfc_llcp_sock *l_sock;
505 
506 			pr_debug("No more clients for SAP %d\n", ssap);
507 
508 			clear_bit(local_ssap, sdp);
509 
510 			/* Find the listening sock and set it back to UNBOUND */
511 			l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
512 			if (l_sock) {
513 				l_sock->ssap = LLCP_SDP_UNBOUND;
514 				nfc_llcp_sock_put(l_sock);
515 			}
516 		}
517 
518 		mutex_unlock(&local->sdp_lock);
519 
520 		return;
521 	} else if (ssap < LLCP_MAX_SAP) {
522 		local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
523 		sdp = &local->local_sap;
524 	} else {
525 		return;
526 	}
527 
528 	mutex_lock(&local->sdp_lock);
529 
530 	clear_bit(local_ssap, sdp);
531 
532 	mutex_unlock(&local->sdp_lock);
533 }
534 
nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local * local)535 static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
536 {
537 	u8 ssap;
538 
539 	mutex_lock(&local->sdp_lock);
540 
541 	ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
542 	if (ssap == LLCP_SDP_NUM_SAP) {
543 		mutex_unlock(&local->sdp_lock);
544 
545 		return LLCP_SAP_MAX;
546 	}
547 
548 	pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
549 
550 	set_bit(ssap, &local->local_sdp);
551 
552 	mutex_unlock(&local->sdp_lock);
553 
554 	return LLCP_WKS_NUM_SAP + ssap;
555 }
556 
nfc_llcp_build_gb(struct nfc_llcp_local * local)557 static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
558 {
559 	u8 *gb_cur, version, version_length;
560 	u8 lto_length, wks_length, miux_length;
561 	const u8 *version_tlv = NULL, *lto_tlv = NULL,
562 	   *wks_tlv = NULL, *miux_tlv = NULL;
563 	__be16 wks = cpu_to_be16(local->local_wks);
564 	u8 gb_len = 0;
565 	int ret = 0;
566 
567 	version = LLCP_VERSION_11;
568 	version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
569 					 1, &version_length);
570 	if (!version_tlv) {
571 		ret = -ENOMEM;
572 		goto out;
573 	}
574 	gb_len += version_length;
575 
576 	lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
577 	if (!lto_tlv) {
578 		ret = -ENOMEM;
579 		goto out;
580 	}
581 	gb_len += lto_length;
582 
583 	pr_debug("Local wks 0x%lx\n", local->local_wks);
584 	wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&wks, 2, &wks_length);
585 	if (!wks_tlv) {
586 		ret = -ENOMEM;
587 		goto out;
588 	}
589 	gb_len += wks_length;
590 
591 	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
592 				      &miux_length);
593 	if (!miux_tlv) {
594 		ret = -ENOMEM;
595 		goto out;
596 	}
597 	gb_len += miux_length;
598 
599 	gb_len += ARRAY_SIZE(llcp_magic);
600 
601 	if (gb_len > NFC_MAX_GT_LEN) {
602 		ret = -EINVAL;
603 		goto out;
604 	}
605 
606 	gb_cur = local->gb;
607 
608 	memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
609 	gb_cur += ARRAY_SIZE(llcp_magic);
610 
611 	memcpy(gb_cur, version_tlv, version_length);
612 	gb_cur += version_length;
613 
614 	memcpy(gb_cur, lto_tlv, lto_length);
615 	gb_cur += lto_length;
616 
617 	memcpy(gb_cur, wks_tlv, wks_length);
618 	gb_cur += wks_length;
619 
620 	memcpy(gb_cur, miux_tlv, miux_length);
621 	gb_cur += miux_length;
622 
623 	local->gb_len = gb_len;
624 
625 out:
626 	kfree(version_tlv);
627 	kfree(lto_tlv);
628 	kfree(wks_tlv);
629 	kfree(miux_tlv);
630 
631 	return ret;
632 }
633 
nfc_llcp_general_bytes(struct nfc_dev * dev,size_t * general_bytes_len)634 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
635 {
636 	struct nfc_llcp_local *local;
637 
638 	local = nfc_llcp_find_local(dev);
639 	if (local == NULL) {
640 		*general_bytes_len = 0;
641 		return NULL;
642 	}
643 
644 	nfc_llcp_build_gb(local);
645 
646 	*general_bytes_len = local->gb_len;
647 
648 	nfc_llcp_local_put(local);
649 
650 	return local->gb;
651 }
652 
nfc_llcp_set_remote_gb(struct nfc_dev * dev,const u8 * gb,u8 gb_len)653 int nfc_llcp_set_remote_gb(struct nfc_dev *dev, const u8 *gb, u8 gb_len)
654 {
655 	struct nfc_llcp_local *local;
656 	int err;
657 
658 	if (gb_len < 3 || gb_len > NFC_MAX_GT_LEN)
659 		return -EINVAL;
660 
661 	local = nfc_llcp_find_local(dev);
662 	if (local == NULL) {
663 		pr_err("No LLCP device\n");
664 		return -ENODEV;
665 	}
666 
667 	memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
668 	memcpy(local->remote_gb, gb, gb_len);
669 	local->remote_gb_len = gb_len;
670 
671 	if (memcmp(local->remote_gb, llcp_magic, 3)) {
672 		pr_err("MAC does not support LLCP\n");
673 		err = -EINVAL;
674 		goto out;
675 	}
676 
677 	err = nfc_llcp_parse_gb_tlv(local,
678 				     &local->remote_gb[3],
679 				     local->remote_gb_len - 3);
680 out:
681 	nfc_llcp_local_put(local);
682 	return err;
683 }
684 
nfc_llcp_dsap(const struct sk_buff * pdu)685 static u8 nfc_llcp_dsap(const struct sk_buff *pdu)
686 {
687 	return (pdu->data[0] & 0xfc) >> 2;
688 }
689 
nfc_llcp_ptype(const struct sk_buff * pdu)690 static u8 nfc_llcp_ptype(const struct sk_buff *pdu)
691 {
692 	return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
693 }
694 
nfc_llcp_ssap(const struct sk_buff * pdu)695 static u8 nfc_llcp_ssap(const struct sk_buff *pdu)
696 {
697 	return pdu->data[1] & 0x3f;
698 }
699 
nfc_llcp_ns(const struct sk_buff * pdu)700 static u8 nfc_llcp_ns(const struct sk_buff *pdu)
701 {
702 	return pdu->data[2] >> 4;
703 }
704 
nfc_llcp_nr(const struct sk_buff * pdu)705 static u8 nfc_llcp_nr(const struct sk_buff *pdu)
706 {
707 	return pdu->data[2] & 0xf;
708 }
709 
nfc_llcp_set_nrns(struct nfc_llcp_sock * sock,struct sk_buff * pdu)710 static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
711 {
712 	pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
713 	sock->send_n = (sock->send_n + 1) % 16;
714 	sock->recv_ack_n = (sock->recv_n - 1) % 16;
715 }
716 
nfc_llcp_send_to_raw_sock(struct nfc_llcp_local * local,struct sk_buff * skb,u8 direction)717 void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
718 			       struct sk_buff *skb, u8 direction)
719 {
720 	struct sk_buff *skb_copy = NULL, *nskb;
721 	struct sock *sk;
722 	u8 *data;
723 
724 	read_lock(&local->raw_sockets.lock);
725 
726 	sk_for_each(sk, &local->raw_sockets.head) {
727 		if (sk->sk_state != LLCP_BOUND)
728 			continue;
729 
730 		if (skb_copy == NULL) {
731 			skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
732 						      GFP_ATOMIC, true);
733 
734 			if (skb_copy == NULL)
735 				continue;
736 
737 			data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
738 
739 			data[0] = local->dev ? local->dev->idx : 0xFF;
740 			data[1] = direction & 0x01;
741 			data[1] |= (RAW_PAYLOAD_LLCP << 1);
742 		}
743 
744 		nskb = skb_clone(skb_copy, GFP_ATOMIC);
745 		if (!nskb)
746 			continue;
747 
748 		if (sock_queue_rcv_skb(sk, nskb))
749 			kfree_skb(nskb);
750 	}
751 
752 	read_unlock(&local->raw_sockets.lock);
753 
754 	kfree_skb(skb_copy);
755 }
756 
nfc_llcp_tx_work(struct work_struct * work)757 static void nfc_llcp_tx_work(struct work_struct *work)
758 {
759 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
760 						    tx_work);
761 	struct sk_buff *skb;
762 	struct sock *sk;
763 	struct nfc_llcp_sock *llcp_sock;
764 
765 	skb = skb_dequeue(&local->tx_queue);
766 	if (skb != NULL) {
767 		sk = skb->sk;
768 		llcp_sock = nfc_llcp_sock(sk);
769 
770 		if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
771 			kfree_skb(skb);
772 			nfc_llcp_send_symm(local->dev);
773 		} else if (llcp_sock && !llcp_sock->remote_ready) {
774 			skb_queue_head(&local->tx_queue, skb);
775 			nfc_llcp_send_symm(local->dev);
776 		} else {
777 			struct sk_buff *copy_skb = NULL;
778 			u8 ptype = nfc_llcp_ptype(skb);
779 			int ret;
780 
781 			pr_debug("Sending pending skb\n");
782 			print_hex_dump_debug("LLCP Tx: ", DUMP_PREFIX_OFFSET,
783 					     16, 1, skb->data, skb->len, true);
784 
785 			if (ptype == LLCP_PDU_I)
786 				copy_skb = skb_copy(skb, GFP_ATOMIC);
787 
788 			__net_timestamp(skb);
789 
790 			nfc_llcp_send_to_raw_sock(local, skb,
791 						  NFC_DIRECTION_TX);
792 
793 			ret = nfc_data_exchange(local->dev, local->target_idx,
794 						skb, nfc_llcp_recv, local);
795 
796 			if (ret) {
797 				kfree_skb(copy_skb);
798 				goto out;
799 			}
800 
801 			if (ptype == LLCP_PDU_I && copy_skb)
802 				skb_queue_tail(&llcp_sock->tx_pending_queue,
803 					       copy_skb);
804 		}
805 	} else {
806 		nfc_llcp_send_symm(local->dev);
807 	}
808 
809 out:
810 	mod_timer(&local->link_timer,
811 		  jiffies + msecs_to_jiffies(2 * local->remote_lto));
812 }
813 
nfc_llcp_connecting_sock_get(struct nfc_llcp_local * local,u8 ssap)814 static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
815 							  u8 ssap)
816 {
817 	struct sock *sk;
818 	struct nfc_llcp_sock *llcp_sock;
819 
820 	read_lock(&local->connecting_sockets.lock);
821 
822 	sk_for_each(sk, &local->connecting_sockets.head) {
823 		llcp_sock = nfc_llcp_sock(sk);
824 
825 		if (llcp_sock->ssap == ssap) {
826 			sock_hold(&llcp_sock->sk);
827 			goto out;
828 		}
829 	}
830 
831 	llcp_sock = NULL;
832 
833 out:
834 	read_unlock(&local->connecting_sockets.lock);
835 
836 	return llcp_sock;
837 }
838 
nfc_llcp_sock_get_sn(struct nfc_llcp_local * local,const u8 * sn,size_t sn_len)839 static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
840 						  const u8 *sn, size_t sn_len)
841 {
842 	return nfc_llcp_sock_from_sn(local, sn, sn_len, true);
843 }
844 
nfc_llcp_connect_sn(const struct sk_buff * skb,size_t * sn_len)845 static const u8 *nfc_llcp_connect_sn(const struct sk_buff *skb, size_t *sn_len)
846 {
847 	u8 type, length;
848 	const u8 *tlv = &skb->data[2];
849 	size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
850 
851 	while (offset < tlv_array_len) {
852 		type = tlv[0];
853 		length = tlv[1];
854 
855 		pr_debug("type 0x%x length %d\n", type, length);
856 
857 		if (type == LLCP_TLV_SN) {
858 			*sn_len = length;
859 			return &tlv[2];
860 		}
861 
862 		offset += length + 2;
863 		tlv += length + 2;
864 	}
865 
866 	return NULL;
867 }
868 
nfc_llcp_recv_ui(struct nfc_llcp_local * local,struct sk_buff * skb)869 static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
870 			     struct sk_buff *skb)
871 {
872 	struct nfc_llcp_sock *llcp_sock;
873 	struct nfc_llcp_ui_cb *ui_cb;
874 	u8 dsap, ssap;
875 
876 	dsap = nfc_llcp_dsap(skb);
877 	ssap = nfc_llcp_ssap(skb);
878 
879 	ui_cb = nfc_llcp_ui_skb_cb(skb);
880 	ui_cb->dsap = dsap;
881 	ui_cb->ssap = ssap;
882 
883 	pr_debug("%d %d\n", dsap, ssap);
884 
885 	/* We're looking for a bound socket, not a client one */
886 	llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
887 	if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
888 		return;
889 
890 	/* There is no sequence with UI frames */
891 	skb_pull(skb, LLCP_HEADER_SIZE);
892 	if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
893 		/*
894 		 * UI frames will be freed from the socket layer, so we
895 		 * need to keep them alive until someone receives them.
896 		 */
897 		skb_get(skb);
898 	} else {
899 		pr_err("Receive queue is full\n");
900 	}
901 
902 	nfc_llcp_sock_put(llcp_sock);
903 }
904 
nfc_llcp_recv_connect(struct nfc_llcp_local * local,const struct sk_buff * skb)905 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
906 				  const struct sk_buff *skb)
907 {
908 	struct sock *new_sk, *parent;
909 	struct nfc_llcp_sock *sock, *new_sock;
910 	u8 dsap, ssap, reason;
911 
912 	dsap = nfc_llcp_dsap(skb);
913 	ssap = nfc_llcp_ssap(skb);
914 
915 	pr_debug("%d %d\n", dsap, ssap);
916 
917 	if (dsap != LLCP_SAP_SDP) {
918 		sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
919 		if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
920 			reason = LLCP_DM_NOBOUND;
921 			goto fail;
922 		}
923 	} else {
924 		const u8 *sn;
925 		size_t sn_len;
926 
927 		sn = nfc_llcp_connect_sn(skb, &sn_len);
928 		if (sn == NULL) {
929 			reason = LLCP_DM_NOBOUND;
930 			goto fail;
931 		}
932 
933 		pr_debug("Service name length %zu\n", sn_len);
934 
935 		sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
936 		if (sock == NULL) {
937 			reason = LLCP_DM_NOBOUND;
938 			goto fail;
939 		}
940 	}
941 
942 	lock_sock(&sock->sk);
943 
944 	parent = &sock->sk;
945 
946 	if (sk_acceptq_is_full(parent)) {
947 		reason = LLCP_DM_REJ;
948 		release_sock(&sock->sk);
949 		sock_put(&sock->sk);
950 		goto fail;
951 	}
952 
953 	if (sock->ssap == LLCP_SDP_UNBOUND) {
954 		u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
955 
956 		pr_debug("First client, reserving %d\n", ssap);
957 
958 		if (ssap == LLCP_SAP_MAX) {
959 			reason = LLCP_DM_REJ;
960 			release_sock(&sock->sk);
961 			sock_put(&sock->sk);
962 			goto fail;
963 		}
964 
965 		sock->ssap = ssap;
966 	}
967 
968 	new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC, 0);
969 	if (new_sk == NULL) {
970 		reason = LLCP_DM_REJ;
971 		release_sock(&sock->sk);
972 		sock_put(&sock->sk);
973 		goto fail;
974 	}
975 
976 	new_sock = nfc_llcp_sock(new_sk);
977 
978 	new_sock->local = nfc_llcp_local_get(local);
979 	if (!new_sock->local) {
980 		reason = LLCP_DM_REJ;
981 		sock_put(&new_sock->sk);
982 		release_sock(&sock->sk);
983 		sock_put(&sock->sk);
984 		goto fail;
985 	}
986 
987 	new_sock->dev = local->dev;
988 	new_sock->rw = sock->rw;
989 	new_sock->miux = sock->miux;
990 	new_sock->nfc_protocol = sock->nfc_protocol;
991 	new_sock->dsap = ssap;
992 	new_sock->target_idx = local->target_idx;
993 	new_sock->parent = parent;
994 	new_sock->ssap = sock->ssap;
995 	if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
996 		atomic_t *client_count;
997 
998 		pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
999 
1000 		client_count =
1001 			&local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
1002 
1003 		atomic_inc(client_count);
1004 		new_sock->reserved_ssap = sock->ssap;
1005 	}
1006 
1007 	nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
1008 				      skb->len - LLCP_HEADER_SIZE);
1009 
1010 	pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
1011 
1012 	nfc_llcp_sock_link(&local->sockets, new_sk);
1013 
1014 	nfc_llcp_accept_enqueue(&sock->sk, new_sk);
1015 
1016 	nfc_get_device(local->dev->idx);
1017 
1018 	new_sk->sk_state = LLCP_CONNECTED;
1019 
1020 	/* Wake the listening processes */
1021 	parent->sk_data_ready(parent);
1022 
1023 	/* Send CC */
1024 	nfc_llcp_send_cc(new_sock);
1025 
1026 	release_sock(&sock->sk);
1027 	sock_put(&sock->sk);
1028 
1029 	return;
1030 
1031 fail:
1032 	/* Send DM */
1033 	nfc_llcp_send_dm(local, dsap, ssap, reason);
1034 }
1035 
nfc_llcp_queue_i_frames(struct nfc_llcp_sock * sock)1036 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
1037 {
1038 	int nr_frames = 0;
1039 	struct nfc_llcp_local *local = sock->local;
1040 
1041 	pr_debug("Remote ready %d tx queue len %d remote rw %d",
1042 		 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
1043 		 sock->remote_rw);
1044 
1045 	/* Try to queue some I frames for transmission */
1046 	while (sock->remote_ready &&
1047 	       skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
1048 		struct sk_buff *pdu;
1049 
1050 		pdu = skb_dequeue(&sock->tx_queue);
1051 		if (pdu == NULL)
1052 			break;
1053 
1054 		/* Update N(S)/N(R) */
1055 		nfc_llcp_set_nrns(sock, pdu);
1056 
1057 		skb_queue_tail(&local->tx_queue, pdu);
1058 		nr_frames++;
1059 	}
1060 
1061 	return nr_frames;
1062 }
1063 
nfc_llcp_recv_hdlc(struct nfc_llcp_local * local,struct sk_buff * skb)1064 static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
1065 			       struct sk_buff *skb)
1066 {
1067 	struct nfc_llcp_sock *llcp_sock;
1068 	struct sock *sk;
1069 	u8 dsap, ssap, ptype, ns, nr;
1070 
1071 	ptype = nfc_llcp_ptype(skb);
1072 	dsap = nfc_llcp_dsap(skb);
1073 	ssap = nfc_llcp_ssap(skb);
1074 	ns = nfc_llcp_ns(skb);
1075 	nr = nfc_llcp_nr(skb);
1076 
1077 	pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
1078 
1079 	llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
1080 	if (llcp_sock == NULL) {
1081 		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
1082 		return;
1083 	}
1084 
1085 	sk = &llcp_sock->sk;
1086 	lock_sock(sk);
1087 	if (sk->sk_state == LLCP_CLOSED) {
1088 		release_sock(sk);
1089 		nfc_llcp_sock_put(llcp_sock);
1090 	}
1091 
1092 	/* Pass the payload upstream */
1093 	if (ptype == LLCP_PDU_I) {
1094 		pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
1095 
1096 		if (ns == llcp_sock->recv_n)
1097 			llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
1098 		else
1099 			pr_err("Received out of sequence I PDU\n");
1100 
1101 		skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
1102 		if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
1103 			/*
1104 			 * I frames will be freed from the socket layer, so we
1105 			 * need to keep them alive until someone receives them.
1106 			 */
1107 			skb_get(skb);
1108 		} else {
1109 			pr_err("Receive queue is full\n");
1110 		}
1111 	}
1112 
1113 	/* Remove skbs from the pending queue */
1114 	if (llcp_sock->send_ack_n != nr) {
1115 		struct sk_buff *s, *tmp;
1116 		u8 n;
1117 
1118 		llcp_sock->send_ack_n = nr;
1119 
1120 		/* Remove and free all skbs until ns == nr */
1121 		skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
1122 			n = nfc_llcp_ns(s);
1123 
1124 			skb_unlink(s, &llcp_sock->tx_pending_queue);
1125 			kfree_skb(s);
1126 
1127 			if (n == nr)
1128 				break;
1129 		}
1130 
1131 		/* Re-queue the remaining skbs for transmission */
1132 		skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
1133 					    s, tmp) {
1134 			skb_unlink(s, &llcp_sock->tx_pending_queue);
1135 			skb_queue_head(&local->tx_queue, s);
1136 		}
1137 	}
1138 
1139 	if (ptype == LLCP_PDU_RR)
1140 		llcp_sock->remote_ready = true;
1141 	else if (ptype == LLCP_PDU_RNR)
1142 		llcp_sock->remote_ready = false;
1143 
1144 	if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
1145 		nfc_llcp_send_rr(llcp_sock);
1146 
1147 	release_sock(sk);
1148 	nfc_llcp_sock_put(llcp_sock);
1149 }
1150 
nfc_llcp_recv_disc(struct nfc_llcp_local * local,const struct sk_buff * skb)1151 static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
1152 			       const struct sk_buff *skb)
1153 {
1154 	struct nfc_llcp_sock *llcp_sock;
1155 	struct sock *sk;
1156 	u8 dsap, ssap;
1157 
1158 	dsap = nfc_llcp_dsap(skb);
1159 	ssap = nfc_llcp_ssap(skb);
1160 
1161 	if ((dsap == 0) && (ssap == 0)) {
1162 		pr_debug("Connection termination");
1163 		nfc_dep_link_down(local->dev);
1164 		return;
1165 	}
1166 
1167 	llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
1168 	if (llcp_sock == NULL) {
1169 		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
1170 		return;
1171 	}
1172 
1173 	sk = &llcp_sock->sk;
1174 	lock_sock(sk);
1175 
1176 	nfc_llcp_socket_purge(llcp_sock);
1177 
1178 	if (sk->sk_state == LLCP_CLOSED) {
1179 		release_sock(sk);
1180 		nfc_llcp_sock_put(llcp_sock);
1181 	}
1182 
1183 	if (sk->sk_state == LLCP_CONNECTED) {
1184 		nfc_put_device(local->dev);
1185 		sk->sk_state = LLCP_CLOSED;
1186 		sk->sk_state_change(sk);
1187 	}
1188 
1189 	nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
1190 
1191 	release_sock(sk);
1192 	nfc_llcp_sock_put(llcp_sock);
1193 }
1194 
nfc_llcp_recv_cc(struct nfc_llcp_local * local,const struct sk_buff * skb)1195 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
1196 			     const struct sk_buff *skb)
1197 {
1198 	struct nfc_llcp_sock *llcp_sock;
1199 	struct sock *sk;
1200 	u8 dsap, ssap;
1201 
1202 	dsap = nfc_llcp_dsap(skb);
1203 	ssap = nfc_llcp_ssap(skb);
1204 
1205 	llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
1206 	if (llcp_sock == NULL) {
1207 		pr_err("Invalid CC\n");
1208 		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
1209 
1210 		return;
1211 	}
1212 
1213 	sk = &llcp_sock->sk;
1214 
1215 	/* Unlink from connecting and link to the client array */
1216 	nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
1217 	nfc_llcp_sock_link(&local->sockets, sk);
1218 	llcp_sock->dsap = ssap;
1219 
1220 	nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
1221 				      skb->len - LLCP_HEADER_SIZE);
1222 
1223 	sk->sk_state = LLCP_CONNECTED;
1224 	sk->sk_state_change(sk);
1225 
1226 	nfc_llcp_sock_put(llcp_sock);
1227 }
1228 
nfc_llcp_recv_dm(struct nfc_llcp_local * local,const struct sk_buff * skb)1229 static void nfc_llcp_recv_dm(struct nfc_llcp_local *local,
1230 			     const struct sk_buff *skb)
1231 {
1232 	struct nfc_llcp_sock *llcp_sock;
1233 	struct sock *sk;
1234 	u8 dsap, ssap, reason;
1235 
1236 	dsap = nfc_llcp_dsap(skb);
1237 	ssap = nfc_llcp_ssap(skb);
1238 	reason = skb->data[2];
1239 
1240 	pr_debug("%d %d reason %d\n", ssap, dsap, reason);
1241 
1242 	switch (reason) {
1243 	case LLCP_DM_NOBOUND:
1244 	case LLCP_DM_REJ:
1245 		llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
1246 		break;
1247 
1248 	default:
1249 		llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
1250 		break;
1251 	}
1252 
1253 	if (llcp_sock == NULL) {
1254 		pr_debug("Already closed\n");
1255 		return;
1256 	}
1257 
1258 	sk = &llcp_sock->sk;
1259 
1260 	sk->sk_err = ENXIO;
1261 	sk->sk_state = LLCP_CLOSED;
1262 	sk->sk_state_change(sk);
1263 
1264 	nfc_llcp_sock_put(llcp_sock);
1265 }
1266 
nfc_llcp_recv_snl(struct nfc_llcp_local * local,const struct sk_buff * skb)1267 static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
1268 			      const struct sk_buff *skb)
1269 {
1270 	struct nfc_llcp_sock *llcp_sock;
1271 	u8 dsap, ssap, type, length, tid, sap;
1272 	const u8 *tlv;
1273 	u16 tlv_len, offset;
1274 	const char *service_name;
1275 	size_t service_name_len;
1276 	struct nfc_llcp_sdp_tlv *sdp;
1277 	HLIST_HEAD(llc_sdres_list);
1278 	size_t sdres_tlvs_len;
1279 	HLIST_HEAD(nl_sdres_list);
1280 
1281 	dsap = nfc_llcp_dsap(skb);
1282 	ssap = nfc_llcp_ssap(skb);
1283 
1284 	pr_debug("%d %d\n", dsap, ssap);
1285 
1286 	if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
1287 		pr_err("Wrong SNL SAP\n");
1288 		return;
1289 	}
1290 
1291 	tlv = &skb->data[LLCP_HEADER_SIZE];
1292 	tlv_len = skb->len - LLCP_HEADER_SIZE;
1293 	offset = 0;
1294 	sdres_tlvs_len = 0;
1295 
1296 	while (offset < tlv_len) {
1297 		type = tlv[0];
1298 		length = tlv[1];
1299 
1300 		switch (type) {
1301 		case LLCP_TLV_SDREQ:
1302 			tid = tlv[2];
1303 			service_name = (char *) &tlv[3];
1304 			service_name_len = length - 1;
1305 
1306 			pr_debug("Looking for %.16s\n", service_name);
1307 
1308 			if (service_name_len == strlen("urn:nfc:sn:sdp") &&
1309 			    !strncmp(service_name, "urn:nfc:sn:sdp",
1310 				     service_name_len)) {
1311 				sap = 1;
1312 				goto add_snl;
1313 			}
1314 
1315 			llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
1316 							  service_name_len,
1317 							  true);
1318 			if (!llcp_sock) {
1319 				sap = 0;
1320 				goto add_snl;
1321 			}
1322 
1323 			/*
1324 			 * We found a socket but its ssap has not been reserved
1325 			 * yet. We need to assign it for good and send a reply.
1326 			 * The ssap will be freed when the socket is closed.
1327 			 */
1328 			if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
1329 				atomic_t *client_count;
1330 
1331 				sap = nfc_llcp_reserve_sdp_ssap(local);
1332 
1333 				pr_debug("Reserving %d\n", sap);
1334 
1335 				if (sap == LLCP_SAP_MAX) {
1336 					sap = 0;
1337 					nfc_llcp_sock_put(llcp_sock);
1338 					goto add_snl;
1339 				}
1340 
1341 				client_count =
1342 					&local->local_sdp_cnt[sap -
1343 							      LLCP_WKS_NUM_SAP];
1344 
1345 				atomic_inc(client_count);
1346 
1347 				llcp_sock->ssap = sap;
1348 				llcp_sock->reserved_ssap = sap;
1349 			} else {
1350 				sap = llcp_sock->ssap;
1351 			}
1352 
1353 			pr_debug("%p %d\n", llcp_sock, sap);
1354 
1355 			nfc_llcp_sock_put(llcp_sock);
1356 add_snl:
1357 			sdp = nfc_llcp_build_sdres_tlv(tid, sap);
1358 			if (sdp == NULL)
1359 				goto exit;
1360 
1361 			sdres_tlvs_len += sdp->tlv_len;
1362 			hlist_add_head(&sdp->node, &llc_sdres_list);
1363 			break;
1364 
1365 		case LLCP_TLV_SDRES:
1366 			mutex_lock(&local->sdreq_lock);
1367 
1368 			pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
1369 
1370 			hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
1371 				if (sdp->tid != tlv[2])
1372 					continue;
1373 
1374 				sdp->sap = tlv[3];
1375 
1376 				pr_debug("Found: uri=%s, sap=%d\n",
1377 					 sdp->uri, sdp->sap);
1378 
1379 				hlist_del(&sdp->node);
1380 
1381 				hlist_add_head(&sdp->node, &nl_sdres_list);
1382 
1383 				break;
1384 			}
1385 
1386 			mutex_unlock(&local->sdreq_lock);
1387 			break;
1388 
1389 		default:
1390 			pr_err("Invalid SNL tlv value 0x%x\n", type);
1391 			break;
1392 		}
1393 
1394 		offset += length + 2;
1395 		tlv += length + 2;
1396 	}
1397 
1398 exit:
1399 	if (!hlist_empty(&nl_sdres_list))
1400 		nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
1401 
1402 	if (!hlist_empty(&llc_sdres_list))
1403 		nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
1404 }
1405 
nfc_llcp_recv_agf(struct nfc_llcp_local * local,struct sk_buff * skb)1406 static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
1407 {
1408 	u8 ptype;
1409 	u16 pdu_len;
1410 	struct sk_buff *new_skb;
1411 
1412 	if (skb->len <= LLCP_HEADER_SIZE) {
1413 		pr_err("Malformed AGF PDU\n");
1414 		return;
1415 	}
1416 
1417 	skb_pull(skb, LLCP_HEADER_SIZE);
1418 
1419 	while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
1420 		pdu_len = skb->data[0] << 8 | skb->data[1];
1421 
1422 		skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
1423 
1424 		if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
1425 			pr_err("Malformed AGF PDU\n");
1426 			return;
1427 		}
1428 
1429 		ptype = nfc_llcp_ptype(skb);
1430 
1431 		if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
1432 			goto next;
1433 
1434 		new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
1435 		if (new_skb == NULL) {
1436 			pr_err("Could not allocate PDU\n");
1437 			return;
1438 		}
1439 
1440 		skb_put_data(new_skb, skb->data, pdu_len);
1441 
1442 		nfc_llcp_rx_skb(local, new_skb);
1443 
1444 		kfree_skb(new_skb);
1445 next:
1446 		skb_pull(skb, pdu_len);
1447 	}
1448 }
1449 
nfc_llcp_rx_skb(struct nfc_llcp_local * local,struct sk_buff * skb)1450 static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
1451 {
1452 	u8 dsap, ssap, ptype;
1453 
1454 	ptype = nfc_llcp_ptype(skb);
1455 	dsap = nfc_llcp_dsap(skb);
1456 	ssap = nfc_llcp_ssap(skb);
1457 
1458 	pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
1459 
1460 	if (ptype != LLCP_PDU_SYMM)
1461 		print_hex_dump_debug("LLCP Rx: ", DUMP_PREFIX_OFFSET, 16, 1,
1462 				     skb->data, skb->len, true);
1463 
1464 	switch (ptype) {
1465 	case LLCP_PDU_SYMM:
1466 		pr_debug("SYMM\n");
1467 		break;
1468 
1469 	case LLCP_PDU_UI:
1470 		pr_debug("UI\n");
1471 		nfc_llcp_recv_ui(local, skb);
1472 		break;
1473 
1474 	case LLCP_PDU_CONNECT:
1475 		pr_debug("CONNECT\n");
1476 		nfc_llcp_recv_connect(local, skb);
1477 		break;
1478 
1479 	case LLCP_PDU_DISC:
1480 		pr_debug("DISC\n");
1481 		nfc_llcp_recv_disc(local, skb);
1482 		break;
1483 
1484 	case LLCP_PDU_CC:
1485 		pr_debug("CC\n");
1486 		nfc_llcp_recv_cc(local, skb);
1487 		break;
1488 
1489 	case LLCP_PDU_DM:
1490 		pr_debug("DM\n");
1491 		nfc_llcp_recv_dm(local, skb);
1492 		break;
1493 
1494 	case LLCP_PDU_SNL:
1495 		pr_debug("SNL\n");
1496 		nfc_llcp_recv_snl(local, skb);
1497 		break;
1498 
1499 	case LLCP_PDU_I:
1500 	case LLCP_PDU_RR:
1501 	case LLCP_PDU_RNR:
1502 		pr_debug("I frame\n");
1503 		nfc_llcp_recv_hdlc(local, skb);
1504 		break;
1505 
1506 	case LLCP_PDU_AGF:
1507 		pr_debug("AGF frame\n");
1508 		nfc_llcp_recv_agf(local, skb);
1509 		break;
1510 	}
1511 }
1512 
nfc_llcp_rx_work(struct work_struct * work)1513 static void nfc_llcp_rx_work(struct work_struct *work)
1514 {
1515 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
1516 						    rx_work);
1517 	struct sk_buff *skb;
1518 
1519 	skb = local->rx_pending;
1520 	if (skb == NULL) {
1521 		pr_debug("No pending SKB\n");
1522 		return;
1523 	}
1524 
1525 	__net_timestamp(skb);
1526 
1527 	nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_RX);
1528 
1529 	nfc_llcp_rx_skb(local, skb);
1530 
1531 	schedule_work(&local->tx_work);
1532 	kfree_skb(local->rx_pending);
1533 	local->rx_pending = NULL;
1534 }
1535 
__nfc_llcp_recv(struct nfc_llcp_local * local,struct sk_buff * skb)1536 static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
1537 {
1538 	local->rx_pending = skb;
1539 	del_timer(&local->link_timer);
1540 	schedule_work(&local->rx_work);
1541 }
1542 
nfc_llcp_recv(void * data,struct sk_buff * skb,int err)1543 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
1544 {
1545 	struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
1546 
1547 	if (err < 0) {
1548 		pr_err("LLCP PDU receive err %d\n", err);
1549 		return;
1550 	}
1551 
1552 	__nfc_llcp_recv(local, skb);
1553 }
1554 
nfc_llcp_data_received(struct nfc_dev * dev,struct sk_buff * skb)1555 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
1556 {
1557 	struct nfc_llcp_local *local;
1558 
1559 	local = nfc_llcp_find_local(dev);
1560 	if (local == NULL) {
1561 		kfree_skb(skb);
1562 		return -ENODEV;
1563 	}
1564 
1565 	__nfc_llcp_recv(local, skb);
1566 
1567 	nfc_llcp_local_put(local);
1568 
1569 	return 0;
1570 }
1571 
nfc_llcp_mac_is_down(struct nfc_dev * dev)1572 void nfc_llcp_mac_is_down(struct nfc_dev *dev)
1573 {
1574 	struct nfc_llcp_local *local;
1575 
1576 	local = nfc_llcp_find_local(dev);
1577 	if (local == NULL)
1578 		return;
1579 
1580 	local->remote_miu = LLCP_DEFAULT_MIU;
1581 	local->remote_lto = LLCP_DEFAULT_LTO;
1582 
1583 	/* Close and purge all existing sockets */
1584 	nfc_llcp_socket_release(local, true, 0);
1585 
1586 	nfc_llcp_local_put(local);
1587 }
1588 
nfc_llcp_mac_is_up(struct nfc_dev * dev,u32 target_idx,u8 comm_mode,u8 rf_mode)1589 void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
1590 			u8 comm_mode, u8 rf_mode)
1591 {
1592 	struct nfc_llcp_local *local;
1593 
1594 	pr_debug("rf mode %d\n", rf_mode);
1595 
1596 	local = nfc_llcp_find_local(dev);
1597 	if (local == NULL)
1598 		return;
1599 
1600 	local->target_idx = target_idx;
1601 	local->comm_mode = comm_mode;
1602 	local->rf_mode = rf_mode;
1603 
1604 	if (rf_mode == NFC_RF_INITIATOR) {
1605 		pr_debug("Queueing Tx work\n");
1606 
1607 		schedule_work(&local->tx_work);
1608 	} else {
1609 		mod_timer(&local->link_timer,
1610 			  jiffies + msecs_to_jiffies(local->remote_lto));
1611 	}
1612 
1613 	nfc_llcp_local_put(local);
1614 }
1615 
nfc_llcp_register_device(struct nfc_dev * ndev)1616 int nfc_llcp_register_device(struct nfc_dev *ndev)
1617 {
1618 	struct nfc_llcp_local *local;
1619 
1620 	local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
1621 	if (local == NULL)
1622 		return -ENOMEM;
1623 
1624 	/* As we are going to initialize local's refcount, we need to get the
1625 	 * nfc_dev to avoid UAF, otherwise there is no point in continuing.
1626 	 * See nfc_llcp_local_get().
1627 	 */
1628 	local->dev = nfc_get_device(ndev->idx);
1629 	if (!local->dev) {
1630 		kfree(local);
1631 		return -ENODEV;
1632 	}
1633 
1634 	INIT_LIST_HEAD(&local->list);
1635 	kref_init(&local->ref);
1636 	mutex_init(&local->sdp_lock);
1637 	timer_setup(&local->link_timer, nfc_llcp_symm_timer, 0);
1638 
1639 	skb_queue_head_init(&local->tx_queue);
1640 	INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
1641 
1642 	local->rx_pending = NULL;
1643 	INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
1644 
1645 	INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
1646 
1647 	rwlock_init(&local->sockets.lock);
1648 	rwlock_init(&local->connecting_sockets.lock);
1649 	rwlock_init(&local->raw_sockets.lock);
1650 
1651 	local->lto = 150; /* 1500 ms */
1652 	local->rw = LLCP_MAX_RW;
1653 	local->miux = cpu_to_be16(LLCP_MAX_MIUX);
1654 	local->local_wks = 0x1; /* LLC Link Management */
1655 
1656 	nfc_llcp_build_gb(local);
1657 
1658 	local->remote_miu = LLCP_DEFAULT_MIU;
1659 	local->remote_lto = LLCP_DEFAULT_LTO;
1660 
1661 	mutex_init(&local->sdreq_lock);
1662 	INIT_HLIST_HEAD(&local->pending_sdreqs);
1663 	timer_setup(&local->sdreq_timer, nfc_llcp_sdreq_timer, 0);
1664 	INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
1665 
1666 	spin_lock(&llcp_devices_lock);
1667 	list_add(&local->list, &llcp_devices);
1668 	spin_unlock(&llcp_devices_lock);
1669 
1670 	return 0;
1671 }
1672 
nfc_llcp_unregister_device(struct nfc_dev * dev)1673 void nfc_llcp_unregister_device(struct nfc_dev *dev)
1674 {
1675 	struct nfc_llcp_local *local = nfc_llcp_remove_local(dev);
1676 
1677 	if (local == NULL) {
1678 		pr_debug("No such device\n");
1679 		return;
1680 	}
1681 
1682 	local_cleanup(local);
1683 
1684 	nfc_llcp_local_put(local);
1685 }
1686 
nfc_llcp_init(void)1687 int __init nfc_llcp_init(void)
1688 {
1689 	return nfc_llcp_sock_init();
1690 }
1691 
nfc_llcp_exit(void)1692 void nfc_llcp_exit(void)
1693 {
1694 	nfc_llcp_sock_exit();
1695 }
1696