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 | |
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, list: &l->head); |
29 | write_unlock(&l->lock); |
30 | } |
31 | |
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 | |
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 | |
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(list: &sock->tx_queue); |
51 | skb_queue_purge(list: &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(skb: s, list: &local->tx_queue); |
62 | kfree_skb(skb: s); |
63 | } |
64 | } |
65 | |
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(list: &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(sock: llcp_sock); |
83 | |
84 | if (sk->sk_state == LLCP_CONNECTED) |
85 | nfc_put_device(dev: 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(sk: 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(sock: 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 | |
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(idx: local->dev->idx)) |
153 | return NULL; |
154 | |
155 | kref_get(kref: &local->ref); |
156 | |
157 | return local; |
158 | } |
159 | |
160 | static void local_cleanup(struct nfc_llcp_local *local) |
161 | { |
162 | nfc_llcp_socket_release(local, device: false, ENXIO); |
163 | del_timer_sync(timer: &local->link_timer); |
164 | skb_queue_purge(list: &local->tx_queue); |
165 | cancel_work_sync(work: &local->tx_work); |
166 | cancel_work_sync(work: &local->rx_work); |
167 | cancel_work_sync(work: &local->timeout_work); |
168 | kfree_skb(skb: local->rx_pending); |
169 | local->rx_pending = NULL; |
170 | del_timer_sync(timer: &local->sdreq_timer); |
171 | cancel_work_sync(work: &local->sdreq_timeout_work); |
172 | nfc_llcp_free_sdp_tlv_list(sdp_head: &local->pending_sdreqs); |
173 | } |
174 | |
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(objp: local); |
183 | } |
184 | |
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(kref: &local->ref, release: local_release); |
196 | nfc_put_device(dev); |
197 | |
198 | return ret; |
199 | } |
200 | |
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(sk: &llcp_sock->sk); |
222 | break; |
223 | } |
224 | } |
225 | |
226 | read_unlock(&local->sockets.lock); |
227 | |
228 | return llcp_sock; |
229 | } |
230 | |
231 | static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) |
232 | { |
233 | sock_put(sk: &sock->sk); |
234 | } |
235 | |
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(dev: local->dev); |
242 | } |
243 | |
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(work: &local->timeout_work); |
251 | } |
252 | |
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(m: 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(n: &sdp->node); |
273 | |
274 | hlist_add_head(n: &sdp->node, h: &nl_sdres_list); |
275 | } |
276 | |
277 | if (!hlist_empty(h: &local->pending_sdreqs)) |
278 | mod_timer(timer: &local->sdreq_timer, |
279 | expires: jiffies + msecs_to_jiffies(m: 3 * local->remote_lto)); |
280 | |
281 | mutex_unlock(lock: &local->sdreq_lock); |
282 | |
283 | if (!hlist_empty(h: &nl_sdres_list)) |
284 | nfc_genl_llc_send_sdres(dev: local->dev, sdres_list: &nl_sdres_list); |
285 | } |
286 | |
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(work: &local->sdreq_timeout_work); |
292 | } |
293 | |
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(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(lock: &llcp_devices_lock); |
306 | |
307 | return res; |
308 | } |
309 | |
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(lock: &llcp_devices_lock); |
315 | list_for_each_entry_safe(local, tmp, &llcp_devices, list) |
316 | if (local->dev == dev) { |
317 | list_del(entry: &local->list); |
318 | spin_unlock(lock: &llcp_devices_lock); |
319 | return local; |
320 | } |
321 | spin_unlock(lock: &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 | |
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 |
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(p: sn, q: tmp_sock->service_name, size: sn_len) == 0) { |
396 | llcp_sock = tmp_sock; |
397 | if (needref) |
398 | sock_hold(sk: &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 | |
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(service_name: sock->service_name, |
417 | service_name_len: 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(lock: &local->sdp_lock); |
425 | |
426 | return LLCP_SAP_MAX; |
427 | } |
428 | |
429 | set_bit(nr: ssap, addr: &local->local_wks); |
430 | mutex_unlock(lock: &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, sn: sock->service_name, |
440 | sn_len: sock->service_name_len, |
441 | needref: false) != NULL) { |
442 | mutex_unlock(lock: &local->sdp_lock); |
443 | |
444 | return LLCP_SAP_MAX; |
445 | } |
446 | |
447 | mutex_unlock(lock: &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(nr: sock->ssap, addr: &local->local_wks); |
454 | mutex_unlock(lock: &local->sdp_lock); |
455 | |
456 | return sock->ssap; |
457 | } |
458 | } |
459 | |
460 | mutex_unlock(lock: &local->sdp_lock); |
461 | |
462 | return LLCP_SAP_MAX; |
463 | } |
464 | |
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(addr: &local->local_sap, LLCP_LOCAL_NUM_SAP); |
472 | if (local_ssap == LLCP_LOCAL_NUM_SAP) { |
473 | mutex_unlock(lock: &local->sdp_lock); |
474 | return LLCP_SAP_MAX; |
475 | } |
476 | |
477 | set_bit(nr: local_ssap, addr: &local->local_sap); |
478 | |
479 | mutex_unlock(lock: &local->sdp_lock); |
480 | |
481 | return local_ssap + LLCP_LOCAL_SAP_OFFSET; |
482 | } |
483 | |
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(v: 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(nr: local_ssap, addr: 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(sock: l_sock); |
515 | } |
516 | } |
517 | |
518 | mutex_unlock(lock: &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(nr: local_ssap, addr: sdp); |
531 | |
532 | mutex_unlock(lock: &local->sdp_lock); |
533 | } |
534 | |
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(addr: &local->local_sdp, LLCP_SDP_NUM_SAP); |
542 | if (ssap == LLCP_SDP_NUM_SAP) { |
543 | mutex_unlock(lock: &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(nr: ssap, addr: &local->local_sdp); |
551 | |
552 | mutex_unlock(lock: &local->sdp_lock); |
553 | |
554 | return LLCP_WKS_NUM_SAP + ssap; |
555 | } |
556 | |
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, value: &version, |
569 | value_length: 1, tlv_length: &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, value: &local->lto, value_length: 1, tlv_length: <o_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, value: (u8 *)&wks, value_length: 2, tlv_length: &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, value: (u8 *)&local->miux, value_length: 0, |
592 | tlv_length: &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(objp: version_tlv); |
627 | kfree(objp: lto_tlv); |
628 | kfree(objp: wks_tlv); |
629 | kfree(objp: miux_tlv); |
630 | |
631 | return ret; |
632 | } |
633 | |
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 | |
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(p: local->remote_gb, q: llcp_magic, size: 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 | tlv_array: &local->remote_gb[3], |
679 | tlv_array_len: local->remote_gb_len - 3); |
680 | out: |
681 | nfc_llcp_local_put(local); |
682 | return err; |
683 | } |
684 | |
685 | static u8 nfc_llcp_dsap(const struct sk_buff *pdu) |
686 | { |
687 | return (pdu->data[0] & 0xfc) >> 2; |
688 | } |
689 | |
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 | |
695 | static u8 nfc_llcp_ssap(const struct sk_buff *pdu) |
696 | { |
697 | return pdu->data[1] & 0x3f; |
698 | } |
699 | |
700 | static u8 nfc_llcp_ns(const struct sk_buff *pdu) |
701 | { |
702 | return pdu->data[2] >> 4; |
703 | } |
704 | |
705 | static u8 nfc_llcp_nr(const struct sk_buff *pdu) |
706 | { |
707 | return pdu->data[2] & 0xf; |
708 | } |
709 | |
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 | |
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, fclone: true); |
733 | |
734 | if (skb_copy == NULL) |
735 | continue; |
736 | |
737 | data = skb_push(skb: 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: skb_copy, GFP_ATOMIC); |
745 | if (!nskb) |
746 | continue; |
747 | |
748 | if (sock_queue_rcv_skb(sk, skb: nskb)) |
749 | kfree_skb(skb: nskb); |
750 | } |
751 | |
752 | read_unlock(&local->raw_sockets.lock); |
753 | |
754 | kfree_skb(skb: skb_copy); |
755 | } |
756 | |
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(list: &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(pdu: skb) == LLCP_PDU_I) { |
771 | kfree_skb(skb); |
772 | nfc_llcp_send_symm(dev: local->dev); |
773 | } else if (llcp_sock && !llcp_sock->remote_ready) { |
774 | skb_queue_head(list: &local->tx_queue, newsk: skb); |
775 | nfc_llcp_send_symm(dev: local->dev); |
776 | } else { |
777 | struct sk_buff *copy_skb = NULL; |
778 | u8 ptype = nfc_llcp_ptype(pdu: 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(dev: local->dev, target_idx: local->target_idx, |
794 | skb, cb: nfc_llcp_recv, cb_context: local); |
795 | |
796 | if (ret) { |
797 | kfree_skb(skb: copy_skb); |
798 | goto out; |
799 | } |
800 | |
801 | if (ptype == LLCP_PDU_I && copy_skb) |
802 | skb_queue_tail(list: &llcp_sock->tx_pending_queue, |
803 | newsk: copy_skb); |
804 | } |
805 | } else { |
806 | nfc_llcp_send_symm(dev: local->dev); |
807 | } |
808 | |
809 | out: |
810 | mod_timer(timer: &local->link_timer, |
811 | expires: jiffies + msecs_to_jiffies(m: 2 * local->remote_lto)); |
812 | } |
813 | |
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(sk: &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 | |
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, needref: true); |
843 | } |
844 | |
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 | |
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(pdu: skb); |
877 | ssap = nfc_llcp_ssap(pdu: 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, ssap: 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(sk: &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(sock: llcp_sock); |
903 | } |
904 | |
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(pdu: skb); |
913 | ssap = nfc_llcp_ssap(pdu: skb); |
914 | |
915 | pr_debug("%d %d\n" , dsap, ssap); |
916 | |
917 | if (dsap != LLCP_SAP_SDP) { |
918 | sock = nfc_llcp_sock_get(local, ssap: 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: &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(sk: &sock->sk); |
943 | |
944 | parent = &sock->sk; |
945 | |
946 | if (sk_acceptq_is_full(sk: parent)) { |
947 | reason = LLCP_DM_REJ; |
948 | release_sock(sk: &sock->sk); |
949 | sock_put(sk: &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(sk: &sock->sk); |
961 | sock_put(sk: &sock->sk); |
962 | goto fail; |
963 | } |
964 | |
965 | sock->ssap = ssap; |
966 | } |
967 | |
968 | new_sk = nfc_llcp_sock_alloc(NULL, type: parent->sk_type, GFP_ATOMIC, kern: 0); |
969 | if (new_sk == NULL) { |
970 | reason = LLCP_DM_REJ; |
971 | release_sock(sk: &sock->sk); |
972 | sock_put(sk: &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(sk: &new_sock->sk); |
982 | release_sock(sk: &sock->sk); |
983 | sock_put(sk: &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(v: client_count); |
1004 | new_sock->reserved_ssap = sock->ssap; |
1005 | } |
1006 | |
1007 | nfc_llcp_parse_connection_tlv(sock: new_sock, tlv_array: &skb->data[LLCP_HEADER_SIZE], |
1008 | tlv_array_len: 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(l: &local->sockets, sk: new_sk); |
1013 | |
1014 | nfc_llcp_accept_enqueue(parent: &sock->sk, sk: new_sk); |
1015 | |
1016 | nfc_get_device(idx: 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(sock: new_sock); |
1025 | |
1026 | release_sock(sk: &sock->sk); |
1027 | sock_put(sk: &sock->sk); |
1028 | |
1029 | return; |
1030 | |
1031 | fail: |
1032 | /* Send DM */ |
1033 | nfc_llcp_send_dm(local, ssap: dsap, dsap: ssap, reason); |
1034 | } |
1035 | |
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(list_: &sock->tx_pending_queue) < sock->remote_rw) { |
1048 | struct sk_buff *pdu; |
1049 | |
1050 | pdu = skb_dequeue(list: &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(list: &local->tx_queue, newsk: pdu); |
1058 | nr_frames++; |
1059 | } |
1060 | |
1061 | return nr_frames; |
1062 | } |
1063 | |
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(pdu: skb); |
1072 | dsap = nfc_llcp_dsap(pdu: skb); |
1073 | ssap = nfc_llcp_ssap(pdu: skb); |
1074 | ns = nfc_llcp_ns(pdu: skb); |
1075 | nr = nfc_llcp_nr(pdu: 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, ssap: dsap, dsap: ssap); |
1080 | if (llcp_sock == NULL) { |
1081 | nfc_llcp_send_dm(local, ssap: dsap, 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(sock: 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(sk: &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(pdu: s); |
1123 | |
1124 | skb_unlink(skb: s, list: &llcp_sock->tx_pending_queue); |
1125 | kfree_skb(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(skb: s, list: &llcp_sock->tx_pending_queue); |
1135 | skb_queue_head(list: &local->tx_queue, newsk: 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(sock: llcp_sock) == 0 && ptype == LLCP_PDU_I) |
1145 | nfc_llcp_send_rr(sock: llcp_sock); |
1146 | |
1147 | release_sock(sk); |
1148 | nfc_llcp_sock_put(sock: llcp_sock); |
1149 | } |
1150 | |
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(pdu: skb); |
1159 | ssap = nfc_llcp_ssap(pdu: skb); |
1160 | |
1161 | if ((dsap == 0) && (ssap == 0)) { |
1162 | pr_debug("Connection termination" ); |
1163 | nfc_dep_link_down(dev: local->dev); |
1164 | return; |
1165 | } |
1166 | |
1167 | llcp_sock = nfc_llcp_sock_get(local, ssap: dsap, dsap: ssap); |
1168 | if (llcp_sock == NULL) { |
1169 | nfc_llcp_send_dm(local, ssap: dsap, dsap: ssap, LLCP_DM_NOCONN); |
1170 | return; |
1171 | } |
1172 | |
1173 | sk = &llcp_sock->sk; |
1174 | lock_sock(sk); |
1175 | |
1176 | nfc_llcp_socket_purge(sock: llcp_sock); |
1177 | |
1178 | if (sk->sk_state == LLCP_CLOSED) { |
1179 | release_sock(sk); |
1180 | nfc_llcp_sock_put(sock: llcp_sock); |
1181 | } |
1182 | |
1183 | if (sk->sk_state == LLCP_CONNECTED) { |
1184 | nfc_put_device(dev: local->dev); |
1185 | sk->sk_state = LLCP_CLOSED; |
1186 | sk->sk_state_change(sk); |
1187 | } |
1188 | |
1189 | nfc_llcp_send_dm(local, ssap: dsap, dsap: ssap, LLCP_DM_DISC); |
1190 | |
1191 | release_sock(sk); |
1192 | nfc_llcp_sock_put(sock: llcp_sock); |
1193 | } |
1194 | |
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(pdu: skb); |
1203 | ssap = nfc_llcp_ssap(pdu: skb); |
1204 | |
1205 | llcp_sock = nfc_llcp_connecting_sock_get(local, ssap: dsap); |
1206 | if (llcp_sock == NULL) { |
1207 | pr_err("Invalid CC\n" ); |
1208 | nfc_llcp_send_dm(local, ssap: dsap, 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(l: &local->connecting_sockets, sk); |
1217 | nfc_llcp_sock_link(l: &local->sockets, sk); |
1218 | llcp_sock->dsap = ssap; |
1219 | |
1220 | nfc_llcp_parse_connection_tlv(sock: llcp_sock, tlv_array: &skb->data[LLCP_HEADER_SIZE], |
1221 | tlv_array_len: skb->len - LLCP_HEADER_SIZE); |
1222 | |
1223 | sk->sk_state = LLCP_CONNECTED; |
1224 | sk->sk_state_change(sk); |
1225 | |
1226 | nfc_llcp_sock_put(sock: llcp_sock); |
1227 | } |
1228 | |
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(pdu: skb); |
1237 | ssap = nfc_llcp_ssap(pdu: 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, ssap: dsap); |
1246 | break; |
1247 | |
1248 | default: |
1249 | llcp_sock = nfc_llcp_sock_get(local, ssap: dsap, 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(sock: llcp_sock); |
1265 | } |
1266 | |
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(pdu: skb); |
1282 | ssap = nfc_llcp_ssap(pdu: 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, sn: service_name, |
1316 | sn_len: service_name_len, |
1317 | needref: 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(sock: 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(v: 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(sock: 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(n: &sdp->node, h: &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(n: &sdp->node); |
1380 | |
1381 | hlist_add_head(n: &sdp->node, h: &nl_sdres_list); |
1382 | |
1383 | break; |
1384 | } |
1385 | |
1386 | mutex_unlock(lock: &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(h: &nl_sdres_list)) |
1400 | nfc_genl_llc_send_sdres(dev: local->dev, sdres_list: &nl_sdres_list); |
1401 | |
1402 | if (!hlist_empty(h: &llc_sdres_list)) |
1403 | nfc_llcp_send_snl_sdres(local, tlv_list: &llc_sdres_list, tlvs_len: sdres_tlvs_len); |
1404 | } |
1405 | |
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(pdu: skb); |
1430 | |
1431 | if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) |
1432 | goto next; |
1433 | |
1434 | new_skb = nfc_alloc_recv_skb(size: 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(skb: new_skb, data: skb->data, len: pdu_len); |
1441 | |
1442 | nfc_llcp_rx_skb(local, skb: new_skb); |
1443 | |
1444 | kfree_skb(skb: new_skb); |
1445 | next: |
1446 | skb_pull(skb, len: pdu_len); |
1447 | } |
1448 | } |
1449 | |
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(pdu: skb); |
1455 | dsap = nfc_llcp_dsap(pdu: skb); |
1456 | ssap = nfc_llcp_ssap(pdu: 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 | |
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(work: &local->tx_work); |
1532 | kfree_skb(skb: local->rx_pending); |
1533 | local->rx_pending = NULL; |
1534 | } |
1535 | |
1536 | static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb) |
1537 | { |
1538 | local->rx_pending = skb; |
1539 | del_timer(timer: &local->link_timer); |
1540 | schedule_work(work: &local->rx_work); |
1541 | } |
1542 | |
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 | |
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 | |
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, device: true, err: 0); |
1585 | |
1586 | nfc_llcp_local_put(local); |
1587 | } |
1588 | |
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(work: &local->tx_work); |
1608 | } else { |
1609 | mod_timer(timer: &local->link_timer, |
1610 | expires: jiffies + msecs_to_jiffies(m: local->remote_lto)); |
1611 | } |
1612 | |
1613 | nfc_llcp_local_put(local); |
1614 | } |
1615 | |
1616 | int nfc_llcp_register_device(struct nfc_dev *ndev) |
1617 | { |
1618 | struct nfc_llcp_local *local; |
1619 | |
1620 | local = kzalloc(size: 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(idx: ndev->idx); |
1629 | if (!local->dev) { |
1630 | kfree(objp: local); |
1631 | return -ENODEV; |
1632 | } |
1633 | |
1634 | INIT_LIST_HEAD(list: &local->list); |
1635 | kref_init(kref: &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(list: &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(lock: &llcp_devices_lock); |
1667 | list_add(new: &local->list, head: &llcp_devices); |
1668 | spin_unlock(lock: &llcp_devices_lock); |
1669 | |
1670 | return 0; |
1671 | } |
1672 | |
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 | |
1687 | int __init nfc_llcp_init(void) |
1688 | { |
1689 | return nfc_llcp_sock_init(); |
1690 | } |
1691 | |
1692 | void nfc_llcp_exit(void) |
1693 | { |
1694 | nfc_llcp_sock_exit(); |
1695 | } |
1696 | |